Python kalman.KalmanFilter类代码示例

class KFMotionModel(object):
    def __init__(self,bb):
        self.kf = KalmanFilter(dim_x=7, dim_z=4)
        self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
        self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

        self.kf.R[2:,2:] *= 10.
        self.kf.P[4:,4:] *= 1000.
        self.kf.P *= 10.
        self.kf.Q[-1,-1] *= 0.01
        self.kf.Q[4:,4:] *= 0.01

        self.kf.x[:4,0] = np.array(bb2z(bb))
        self.history = []
        self.predicted = False

    def update(self,bb):
        self.history = []
        bb = np.array(bb2z(bb))
        bb = np.expand_dims(bb, axis=1)
        self.kf.update(bb)
        self.predicted = False
        

    def predict(self):
        if not self.predicted:
            if((self.kf.x[6]+self.kf.x[2])<=0):
                self.kf.x[6] *= 0.0
            self.kf.predict()
            self.history.append(z2bb(self.kf.x))
            self.predicted=True
        return self.history[-1]

    def get_state(self):
        return z2bb(self.kf.x)

 def create_kalman_filter(self, det):
     """(x, y, s(area), r(aspect ratio), x', y', s')
     """
     model = KalmanFilter(dim_x=7, dim_z=4)
     model.F = np.array([
         [1, 0, 0, 0, 1, 0, 0],
         [0, 1, 0, 0, 0, 1, 0],
         [0, 0, 1, 0, 0, 0, 1],
         [0, 0, 0, 1, 0, 0, 0],
         [0, 0, 0, 0, 1, 0, 0],
         [0, 0, 0, 0, 0, 1, 0],
         [0, 0, 0, 0, 0, 0, 1],
     ], 'float32')
     model.H = np.array([
         [1, 0, 0, 0, 0, 0, 0],
         [0, 1, 0, 0, 0, 0, 0],
         [0, 0, 1, 0, 0, 0, 0],
         [0, 0, 0, 1, 0, 0, 0],
     ], 'float32')
     model.R[2:,2:] *= 10.
     model.P[4:,4:] *= 1000.  # high uncertainty of initial volocity
     model.P *= 10.
     model.Q[-1,-1] *= 0.01
     model.Q[4:,4:] *= 0.01
     model.x[:4] = np.array(xywh_to_xysr(*det), 'float32').reshape(4, 1)
     return model

 def _KF_init(self): # para: Center of box used for prediction
     KF = KalmanFilter(4,2)
     # KF.x = location + [0,0,0,0]
     # KF.F = np.array([
     #     [1,0,0,0,1,0,0,0],
     #     [0,1,0,0,0,1,0,0],
     #     [0,0,1,0,0,0,1,0],
     #     [0,0,0,1,0,0,0,1],
     #     [0,0,0,0,1,0,0,0],
     #     [0,0,0,0,0,1,0,0],
     #     [0,0,0,0,0,0,1,0],
     #     [0,0,0,0,0,0,0,1]])
     # KF.H = np.array([
     #     [1,0,0,0,0,0,0,0],
     #     [0,1,0,0,0,0,0,0],
     #     [0,0,1,0,0,0,0,0],
     #     [0,0,0,1,0,0,0,0]])
     KF.x = self.KF_center + [0,0] # can be improved for accuracy e.g. from which edge
     KF.F = np.array([
         [1,0,1,0],
         [0,1,0,1],
         [0,0,1,0],
         [0,0,0,1]])
     KF.H = np.array([
         [1,0,0,0],
         [0,1,0,0]])
     KF.P *= 100
     KF.R *= 100
     # KF.Q *= 2
     # KF.predict()
     return KF

    def __init__(self, dt, ID, position_x, position_y):

        self.p_x = KalmanFilter(dim_x=3, dim_z=1)
        self.p_y = KalmanFilter(dim_x=3, dim_z=1)
        self.p_x.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
        self.p_y.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])

        self.p_x.H = np.array([[1, 0., 0.]])
        self.p_y.H = np.array([[1, 0., 0.]])


        self.R_x_std = 0.01  # update to the correct value
        self.Q_x_std = 7      # update to the correct value
        self.R_y_std = 0.01  # update to the correct value
        self.Q_y_std = 7      # update to the correct value

        self.p_y.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_y_std ** 2)
        self.p_x.Q = Q_discrete_white_noise(dim=3, dt=dt, var=self.Q_x_std ** 2)

        self.p_x.R *= self.R_x_std ** 2
        self.dt = dt
        self.ID = ID
        self.p_x.x = np.array([[position_x], [0.], [0.]])
        self.p_y.x = np.array([[position_y], [0.], [0.]])
        self.p_x.P *= 100.  # can very likely be set to 100.
        self.p_y.P *= 100.  # can very likely be set to 100.

        self.time_since_last_update = 0.0

        self.p_y.R *= self.R_y_std ** 2

class KalmanBoxTracker(object):
  """
  This class represents the internel state of individual tracked objects observed as bbox.
  """
  count = 0
  def __init__(self,bbox):
    """
    Initialises a tracker using initial bounding box.
    """
    #define constant velocity model
    self.kf = KalmanFilter(dim_x=7, dim_z=4)
    self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0],  [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]])
    self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]])

    self.kf.R[2:,2:] *= 10.
    self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
    self.kf.P *= 10.
    self.kf.Q[-1,-1] *= 0.01
    self.kf.Q[4:,4:] *= 0.01

    self.kf.x[:4] = convert_bbox_to_z(bbox)
    self.time_since_update = 0
    self.id = KalmanBoxTracker.count
    KalmanBoxTracker.count += 1
    self.history = []
    self.hits = 0
    self.hit_streak = 0
    self.age = 0

  def update(self,bbox):
    """
    Updates the state vector with observed bbox.
    """
    self.time_since_update = 0
    self.history = []
    self.hits += 1
    self.hit_streak += 1
    self.kf.update(convert_bbox_to_z(bbox))

  def predict(self):
    """
    Advances the state vector and returns the predicted bounding box estimate.
    """
    if((self.kf.x[6]+self.kf.x[2])<=0):
      self.kf.x[6] *= 0.0
    self.kf.predict()
    self.age += 1
    if(self.time_since_update>0):
      self.hit_streak = 0
    self.time_since_update += 1
    self.history.append(convert_x_to_bbox(self.kf.x))
    return self.history[-1]

  def get_state(self):
    """
    Returns the current bounding box estimate.
    """
    return convert_x_to_bbox(self.kf.x)

def make_cv_filter(dt, noise_factor):
    cvfilter = KalmanFilter(dim_x = 2, dim_z=1)
    cvfilter.x = array([0., 0.])
    cvfilter.P *= 3
    cvfilter.R *= noise_factor**2
    cvfilter.F = array([[1, dt],
                        [0,  1]], dtype=float)
    cvfilter.H = array([[1, 0]], dtype=float)
    cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
    return cvfilter

def test_rts():
    fk = KalmanFilter(dim_x=2, dim_z=1)

    fk.x = np.array([-1., 1.])    # initial state (location and velocity)

    fk.F = np.array([[1.,1.],
                     [0.,1.]])      # state transition matrix

    fk.H = np.array([[1.,0.]])      # Measurement function
    fk.P = .01                     # covariance matrix
    fk.R = 5                       # state uncertainty
    fk.Q = 0.001                   # process uncertainty


    zs = [t + random.randn()*4 for t in range(40)]

    mu, cov, _, _ = fk.batch_filter (zs)
    mus = [x[0] for x in mu]

    M, P, _, _ = fk.rts_smoother(mu, cov)

    if DO_PLOT:
        p1, = plt.plot(zs,'cyan', alpha=0.5)
        p2, = plt.plot(M[:,0],c='b')
        p3, = plt.plot(mus,c='r')
        p4, = plt.plot([0, len(zs)], [0, len(zs)], 'g') # perfect result
        plt.legend([p1, p2, p3, p4],
                   ["measurement", "RKS", "KF output", "ideal"], loc=4)

        plt.show()

def make_ca_filter(dt, noise_factor):
    cafilter = KalmanFilter(dim_x=3, dim_z=1)
    cafilter.x = array([0., 0., 0.])
    cafilter.P *= 3
    cafilter.R *= noise_factor**2
    cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02)
    cafilter.F = array([[1, dt, 0.5*dt*dt],
                        [0, 1,         dt],
                        [0, 0,          1]], dtype=float)
    cafilter.H = array([[1, 0, 0]], dtype=float)
    return cafilter

 def ZeroOrderKF(R, Q, P=20):
     """ Create zero order Kalman filter.
     Specify R and Q as floats."""
     kf = KalmanFilter(dim_x=1, dim_z=1)
     kf.x = np.array([0.])
     kf.R *= R
     kf.Q *= Q
     kf.P *= P
     kf.F = np.eye(1)
     kf.H = np.eye(1)
     return kf

def kf_circle():
    from filterpy.kalman import KalmanFilter
    from math import radians
    import math
    def hx(x):
        radius = x[0]
        angle = x[1]
        x = cos(radians(angle)) * radius
        y = sin(radians(angle)) * radius
        return np.array([x, y])

    def fx(x, dt):
        return np.array([x[0], x[1]+x[2], x[2]])


    def hx_inv(x, y):
        angle = math.atan2(y,x)
        radius = math.sqrt(x*x + y*y)
        return np.array([radius, angle])


    std_noise = .1


    kf = KalmanFilter(dim_x=3, dim_z=2)
    kf.x = np.array([50., 0., 0.])

    F = np.array([[1., 0, 0.],
                  [0., 1., 1.,],
                  [0., 0., 1.,]])

    kf.F = F
    kf.P*= 100
    kf.H = np.array([[1,0,0],
                     [0,1,0]])

    kf.R = np.eye(2)*(std_noise**2)
    #kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)



    zs = []
    kfxs = []
    for t in range (0,2000):
        a = t / 30 + 90
        x = cos(radians(a)) * 50.+ randn() * std_noise
        y = sin(radians(a)) * 50. + randn() * std_noise

        z = hx_inv(x,y)
        zs.append(z)

        kf.predict()
        kf.update(z)

        # save data
        kfxs.append(kf.x)

    zs = np.asarray(zs)
    kfxs = np.asarray(kfxs)

 def FirstOrderKF(R, Q, dt):
     """ Create first order Kalman filter.
     Specify R and Q as floats."""
     kf = KalmanFilter(dim_x=2, dim_z=1)
     kf.x = np.zeros(2)
     kf.P *= np.array([[100, 0], [0, 1]])
     kf.R *= R
     kf.Q = Q_discrete_white_noise(2, dt, Q)
     kf.F = np.array([[1., dt],
     [0., 1]])
     kf.H = np.array([[1., 0]])
     return kf

def test_against_kf():
    inv = np.linalg.inv

    dt = 1.0
    IM = np.eye(2)
    Q = np.array([[.25, 0.5], [0.5, 1]])

    F = np.array([[1, dt], [0, 1]])
    #QI = inv(Q)
    P = inv(IM)


    from filterpy.kalman import InformationFilter
    from filterpy.common import Q_discrete_white_noise

    #f = IF2(2, 1)
    r_std = .2
    R = np.array([[r_std*r_std]])
    RI = inv(R)

    '''f.F = F.copy()
    f.H = np.array([[1, 0.]])
    f.RI = RI.copy()
    f.Q = Q.copy()
    f.IM = IM.copy()'''


    kf = KalmanFilter(2, 1)
    kf.F = F.copy()
    kf.H = np.array([[1, 0.]])
    kf.R = R.copy()
    kf.Q = Q.copy()

    f0 = InformationFilter(2, 1)
    f0.F = F.copy()
    f0.H = np.array([[1, 0.]])
    f0.R_inv = RI.copy()
    f0.Q = Q.copy()



    #f.IM = np.zeros((2,2))


    for i in range(1, 50):
        z = i + (np.random.rand() * r_std)
        f0.predict()
        #f.predict()
        kf.predict()


        f0.update(z)
        #f.update(z)
        kf.update(z)

        print(f0.x.T, kf.x.T)
        assert np.allclose(f0.x, kf.x)

def test_univariate():
    f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
    f.x = np.array([[0]])
    f.P *= 50
    f.H = np.array([[1.]])
    f.F = np.array([[1.]])
    f.B = np.array([[1.]])
    f.Q = .02
    f.R *= .1

    for i in range(50):
        f.predict()
        f.update(i)

def single_measurement_test():
    dt = 0.1
    sigma = 2.

    kf2 = KalmanFilter(dim_x=2, dim_z=1)

    kf2.F = array ([[1., dt], [0., 1.]])
    kf2.H = array ([[1., 0.]])
    kf2.x = array ([[0.], [1.]])
    kf2.Q = array ([[dt**3/3, dt**2/2],
                    [dt**2/2, dt]]) * 0.02
    kf2.P *= 100
    kf2.R[0,0] = sigma**2

    random.seed(SEED)
    xs = []
    zs = []
    nom = []
    for i in range(1, 100):
        m0 = i + randn()*sigma
        z = array([[m0]])

        kf2.predict()
        kf2.update(z)

        xs.append(kf2.x.T[0])
        zs.append(z.T[0])
        nom.append(i)

    xs = asarray(xs)
    zs = asarray(zs)
    nom = asarray(nom)


    res = nom-xs[:,0]
    std_dev = np.std(res)
    print('std: {:.3f}'.format (std_dev))

    global DO_PLOT
    if DO_PLOT:

        plt.subplot(211)
        plt.plot(xs[:,0])
        #plt.plot(zs[:,0])


        plt.subplot(212)
        plt.plot(res)
        plt.show()

    return std_dev

def test_1d_0P():
    f = KalmanFilter (dim_x=2, dim_z=1)
    inf = InformationFilter (dim_x=2, dim_z=1)

    f.x = np.array([[2.],
                    [0.]])       # initial state (location and velocity)

    inf.x = f.x.copy()
    f.F = (np.array([[1.,1.],
                     [0.,1.]]))    # state transition matrix

    inf.F = f.F.copy()
    f.H = np.array([[1.,0.]])    # Measurement function
    inf.H = np.array([[1.,0.]])    # Measurement function
    f.R = 5.                 # state uncertainty
    inf.R_inv = 1./5                 # state uncertainty
    f.Q = 0.0001                 # process uncertainty
    inf.Q = 0.0001
    f.P *= 20
    inf.P_inv = 0
    #inf.P_inv = inv(f.P)

    m = []
    r = []
    r2 = []


    zs = []
    for t in range (100):
        # create measurement = t plus white noise
        z = t + random.randn()*20
        zs.append(z)

        # perform kalman filtering
        f.predict()
        f.update(z)

        inf.predict()
        inf.update(z)

        # save data
        r.append (f.x[0,0])
        r2.append (inf.x[0,0])
        m.append(z)

        #assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12

    if DO_PLOT:
        plt.plot(m)
        plt.plot(r)
        plt.plot(r2)