Python KalmanFilter.H方法代码示例

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
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()

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def createLegKF(x, y):
    # kalman_filter = KalmanFilter(dim_x=4, dim_z=2)
    kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
    dt = 0.1
    KF_F = np.array([[1.0, dt, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, dt], [0, 0, 0, 1.0]])
    KF_q = 0.7  # 0.3
    KF_Q = np.vstack(
        (
            np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q), np.zeros((2, 2)))),
            np.hstack((np.zeros((2, 2)), Q_discrete_white_noise(2, dt=0.1, var=KF_q))),
        )
    )
    KF_pd = 25.0
    KF_pv = 10.0
    KF_P = np.diag([KF_pd, KF_pv, KF_pd, KF_pv])
    KF_rd = 0.05
    KF_rv = 0.2  # 0.5
    # KF_R = np.diag([KF_rd,KF_rd])
    KF_R = np.diag([KF_rd, KF_rd, KF_rv, KF_rv])
    # KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
    KF_H = np.array([[1.0, 0, 0, 0], [0, 0, 1.0, 0], [0, 1.0, 0, 0], [0, 0, 0, 1.0]])

    kalman_filter.x = np.array([x, 0, y, 0])
    kalman_filter.F = KF_F
    kalman_filter.H = KF_H
    kalman_filter.Q = KF_Q
    kalman_filter.B = 0
    kalman_filter.R = KF_R
    kalman_filter.P = KF_P

    return kalman_filter

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def tracker1():
    #
    #
    # Design 2D filter
    #
    #
    
    # 1. Choose state vars - x
    
    # 2. Design state trans. Function - F
    tracker = KalmanFilter(dim_x=4, dim_z=2)
    dt = 1.
    # time step 1 second
    tracker.F = np.array([[1, dt, 0, 0],
                            [0, 1, 0, 0],
                            [0, 0, 1, dt],
                            [0, 0, 0, 1]])
                            
    # 3. Design Process Noise Mat - Q
    v = 0.05
    q = Q_discrete_white_noise(dim=2, dt=dt, var=v)
    tracker.Q = block_diag(q, q)

    # 4. B
    # 5. Design measurement function
    tracker.H = np.array([[1/0.3048, 0, 0, 0],
                          [0, 0, 1/0.3048, 0]])
                          
    # 6. Design Meas. Noise Mat - R
    tracker.R = np.array([[5, 0],[0, 5]])
    
    # Init conditions
    tracker.x = np.array([[0, 0, 0, 0]]).T
    tracker.P = np.eye(4) * 500.
    return tracker

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
 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

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def ball_filter6(dt,R=1., Q = 0.1):
    f1 = KalmanFilter(dim=6)
    g = 10

    f1.F = np.mat ([[1., dt, dt**2,  0,       0,  0],
                    [0,  1., dt,     0,       0,  0],
                    [0,  0,  1.,     0,       0,  0],
                    [0,  0,  0.,    1., dt, -0.5*dt*dt*g],
                    [0,  0,  0,      0, 1.,      -g*dt],
                    [0,  0,  0,      0, 0.,      1.]])

    f1.H = np.mat([[1,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,1,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0]])


    f1.R = np.mat(np.eye(6)) * R

    f1.Q = np.zeros((6,6))
    f1.Q[2,2] = Q
    f1.Q[5,5] = Q
    f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T
    f1.P = np.eye(6) * 50.
    f1.B = 0.
    f1.u = 0

    return f1

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def ball_filter4(dt,R=1., Q = 0.1):
    f1 = KalmanFilter(dim=4)
    g = 10

    f1.F = np.mat ([[1., dt,  0, 0,],
                    [0,  1.,  0, 0],
                    [0,  0,  1., dt],
                    [0,  0,  0.,  1.]])

    f1.H = np.mat([[1,0,0,0],
                   [0,0,0,0],
                   [0,0,1,0],
                   [0,0,0,0]])



    f1.B = np.mat([[0,0,0,0],
                   [0,0,0,0],
                   [0,0,1.,0],
                   [0,0,0,1.]])

    f1.u = np.mat([[0],
                   [0],
                   [-0.5*g*dt**2],
                   [-g*dt]])

    f1.R = np.mat(np.eye(4)) * R

    f1.Q = np.zeros((4,4))
    f1.Q[1,1] = Q
    f1.Q[3,3] = Q
    f1.x = np.mat([0, 0 , 0, 0]).T
    f1.P = np.eye(4) * 50.
    return f1

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def template():
    kf = KalmanFilter(dim_x=2, dim_z=1)
    
    # x0 
    kf.x = np.array([[.0], [.0]])
        
    # P - change over time
    kf.P = np.diag([500., 49.])
    
    # F - state transition matrix
    dt = .1
    kf.F = np.array([[1, dt], [0, 1]])
    
    ## now can predict
    ## дисперсия растет и становится видна корреляция
    #plot_covariance_ellipse(kf.x, kf.P, edgecolor='r')
    #kf.predict()
    #plot_covariance_ellipse(kf.x, kf.P, edgecolor='k', ls='dashed')
    #show()
    
    # Control information
    kf.B = 0
    
    # !!Attention!! Q! Process noise
    kf.Q = Q_discrete_white_noise( dim=2, dt=dt, var=2.35)
    
    # H - measurement function
    kf.H = np.array([[1., 0.]])
    
    # R - measure noise matrix
    kf.R = np.array([[5.]])

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
 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

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def test_noisy_1d():
    f = KalmanFilter(dim_x=2, dim_z=1)

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

    f.F = np.array([[1.0, 1.0], [0.0, 1.0]])  # state transition matrix

    f.H = np.array([[1.0, 0.0]])  # Measurement function
    f.P *= 1000.0  # covariance matrix
    f.R = 5  # state uncertainty
    f.Q = 0.0001  # process uncertainty

    fsq = SquareRootKalmanFilter(dim_x=2, dim_z=1)

    fsq.x = np.array([[2.0], [0.0]])  # initial state (location and velocity)

    fsq.F = np.array([[1.0, 1.0], [0.0, 1.0]])  # state transition matrix

    fsq.H = np.array([[1.0, 0.0]])  # Measurement function
    fsq.P = np.eye(2) * 1000.0  # covariance matrix
    fsq.R = 5  # state uncertainty
    fsq.Q = 0.0001  # process uncertainty

    measurements = []
    results = []

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

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

        fsq.update(z)
        fsq.predict()

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

        # save data
        results.append(f.x[0, 0])
        measurements.append(z)

    p = f.P - fsq.P
    print(f.P)
    print(fsq.P)

    for i in range(f.P.shape[0]):
        assert abs(f.P[i, i] - fsq.P[i, i]) < 0.01

    # now do a batch run with the stored z values so we can test that
    # it is working the same as the recursive implementation.
    # give slightly different P so result is slightly different
    f.x = np.array([[2.0, 0]]).T
    f.P = np.eye(2) * 100.0
    m, c, _, _ = f.batch_filter(zs, update_first=False)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
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)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.):
    dt = 0.1

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

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


    random.seed(SEED)
    xs = []
    zs = []
    nom = []
    for i in range(1, 100):
        m0 = i + randn()*wheel_sigma
        m1 = i + randn()*gps_sigma
        if gps_sigma >1e40:
            m1 = -1e40

        z = array([[m0], [m1]])

        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('fusion std: {:.3f}'.format (np.std(res)))

    if DO_PLOT:

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

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

    print(kf2.Q)
    print(kf2.K)
    return std_dev

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def test_1d_0P():
    global inf
    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)

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

    f.H = np.array([[1., 0.]])    # Measurement function
    f.R = np.array([[5.]])                 # state uncertainty
    f.Q = np.eye(2)*0.0001                 # process uncertainty
    f.P = np.diag([20., 20.])

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

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


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

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

        inf.predict()
        inf.update(z)

        try:
            print(t, inf.P)
        except:
            pass

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

    #assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'

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

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
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)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def dog_tracking_filter(R,Q=0,cov=1.):
    f = KalmanFilter (dim_x=2, dim_z=1)
    f.x = np.matrix([[0], [0]])    # initial state (location and velocity)
    f.F = np.matrix([[1,1],[0,1]]) # state transition matrix
    f.H = np.matrix([[1,0]])       # Measurement function
    f.R = R                        # measurement uncertainty
    f.P *= cov                     # covariance matrix
    f.Q = Q
    return f

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import H [as 别名]
def test_noisy_1d():
    f = KalmanFilter(dim_x=2, dim_z=1)

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

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

    f.H = np.array([[1., 0.]])    # Measurement function
    f.P *= 1000.                  # covariance matrix
    f.R = 5                       # state uncertainty
    f.Q = 0.0001                  # process uncertainty

    measurements = []
    results = []

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

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

        # save data
        results.append(f.x[0, 0])
        measurements.append(z)

        # test mahalanobis
        a = np.zeros(f.y.shape)
        maha = scipy_mahalanobis(a, f.y, f.SI)
        assert f.mahalanobis == approx(maha)


    # now do a batch run with the stored z values so we can test that
    # it is working the same as the recursive implementation.
    # give slightly different P so result is slightly different
    f.x = np.array([[2., 0]]).T
    f.P = np.eye(2) * 100.
    s = Saver(f)
    m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s)
    s.to_array()
    assert len(s.x) == len(zs)
    assert len(s.x) == len(s)

    # plot data
    if DO_PLOT:
        p1, = plt.plot(measurements, 'r', alpha=0.5)
        p2, = plt.plot(results, 'b')
        p4, = plt.plot(m[:, 0], 'm')
        p3, = plt.plot([0, 100], [0, 100], 'g')  # perfect result
        plt.legend([p1, p2, p3, p4],
                   ["noisy measurement", "KF output", "ideal", "batch"], loc=4)
        plt.show()