Python KalmanFilter.P方法代码示例

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

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import P [as 别名]
def makeLinearKF(A, B, C, P, F, x, R = None, dim_x = 4, dim_z = 4):

    kf = KalmanFilter(dim_x=dim_x, dim_z=dim_z)
    kf.x = x
    kf.P = P
    kf.F = F
    kf.H = C
    kf.R = R

    return kf

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import P [as 别名]
def rot_box_kalman_filter(initial_state, Q_std, R_std):
    """
    Tracks a 2D rectangular object (e.g. a bounding box) whose state includes
    position, centroid velocity, dimensions, and rotation angle.

    Parameters
    ----------
    initial_state : sequence of floats
        [x, vx, y, vy, w, h, phi]
    Q_std : float
        Standard deviation to use for process noise covariance matrix
    R_std : float
        Standard deviation to use for measurement noise covariance matrix

    Returns
    -------
    kf : filterpy.kalman.KalmanFilter instance
    """
    kf = KalmanFilter(dim_x=7, dim_z=5)
    dt = 1.0   # time step

    # state mean and covariance
    kf.x = np.array([initial_state]).T
    kf.P = np.eye(kf.dim_x) * 500.

    # no control inputs
    kf.u = 0.

    # state transition matrix
    kf.F = np.eye(kf.dim_x)
    kf.F[0, 1] = kf.F[2, 3] = dt

    # measurement matrix - maps from state space to observation space, so
    # shape is dim_z x dim_x.
    kf.H = np.zeros([kf.dim_z, kf.dim_x])

    # z = Hx. H has nonzero coefficients for the following components of kf.x:
    #   x            y            w            h           phi
    kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = kf.H[4, 6] = 1.0

    # measurement noise covariance
    kf.R = np.eye(kf.dim_z) * R_std**2

    # process noise covariance for x-vx or y-vy pairs
    q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)

    # diagonal process noise sub-matrix for width, height, and phi
    qq = Q_std**2*np.eye(3)

    # process noise covariance matrix for full state
    kf.Q = block_diag(q, q, qq)

    return kf

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

    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 *= 0.0001
    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)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import P [as 别名]
def init_tracker():
    tracker = KalmanFilter(dim_x=4, dim_z=2)
    dt = 1.0  # time step

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

    tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])

    tracker.R = np.eye(2) * 1000
    q = Q_discrete_white_noise(dim=2, dt=dt, var=1)
    tracker.Q = block_diag(q, q)
    tracker.x = np.array([[2000, 0, 700, 0]]).T
    tracker.P = np.eye(4) * 50.0
    return tracker

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import P [as 别名]
def test_procedure_form():

    dt = 1.
    std_z = 10.1

    x = np.array([[0.], [0.]])
    F = np.array([[1., dt], [0., 1.]])
    H = np.array([[1.,0.]])
    P = np.eye(2)
    R = np.eye(1)*std_z**2
    Q = Q_discrete_white_noise(2, dt, 5.1)

    kf = KalmanFilter(2, 1)
    kf.x = x.copy()
    kf.F = F.copy()
    kf.H = H.copy()
    kf.P = P.copy()
    kf.R = R.copy()
    kf.Q = Q.copy()


    measurements = []
    results = []

    xest = []
    ks = []
    pos = 0.
    for t in range (2000):
        z = pos + random.randn() * std_z
        pos += 100

        # perform kalman filtering
        x, P = predict(x, P, F, Q)
        kf.predict()
        assert norm(x - kf.x) < 1.e-12
        x, P, _, _, _, _ = update(x, P, z, R, H, True)
        kf.update(z)
        assert norm(x - kf.x) < 1.e-12

        # save data
        xest.append (x.copy())
        measurements.append(z)

    xest = np.asarray(xest)
    measurements = np.asarray(measurements)
    # plot data
    if DO_PLOT:
        plt.plot(xest[:, 0])
        plt.plot(xest[:, 1])
        plt.plot(measurements)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import P [as 别名]
def createPersonKF(x, y):
    kalman_filter = KalmanFilter(dim_x=6, dim_z=2)
    # kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
    dt = 0.1
    dt2 = 0.005
    # KF_F = np.array([[1., dt, 0 ,  0],
    # [0 , 1., 0 ,  0],
    # [0 , 0 , 1., dt],
    # [0 , 0 , 0 , 1.]])
    KF_Fca = np.array([[1.0, dt, dt2 / 2], [0, 1.0, dt], [0, 0, 1]])
    KF_F = np.vstack(
        (np.hstack((KF_Fca, np.zeros((3, 3)))), np.hstack((np.zeros((3, 3)), KF_Fca)))  # np.zeros((3,3)))),
    )  # , np.zeros((3,3)))) )))#,
    # np.hstack((np.zeros((3,3)),np.zeros((3,3)), KF_Fca))))
    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_Q = np.vstack(
        (
            np.hstack((Q_discrete_white_noise(3, dt=0.1, var=KF_q), np.zeros((3, 3)))),
            np.hstack((np.zeros((3, 3)), Q_discrete_white_noise(3, dt=0.1, var=KF_q))),
        )
    )
    KF_pd = 25.0
    KF_pv = 10.0
    KF_pa = 30.0
    KF_P = np.diag([KF_pd, KF_pv, KF_pa, KF_pd, KF_pv, KF_pa])
    KF_rd = 0.05
    KF_rv = 0.2  # 0.5
    KF_ra = 2  # 0.5
    KF_R = np.diag([KF_rd, KF_rd])
    # KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv])
    # KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv, KF_ra, KF_ra])
    # KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
    # KF_H = np.array([[1.,0,0,0],[0,0,1.,0],[0,1.,0,0],[0,0,0,1.]])
    KF_H = np.array([[1.0, 0, 0, 0, 0, 0], [0, 0, 0, 1.0, 0, 0]])

    # kalman_filter.x = np.array([x,0,y,0])
    kalman_filter.x = np.array([x, 0, 0, y, 0, 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