Python KalmanFilter.batch_filter方法代码示例

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

    f.x = np.array([2.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

    zs = [None, 1.0, 2.0]
    m, c, _, _ = f.batch_filter(zs, update_first=False)
    m, c, _, _ = f.batch_filter(zs, update_first=True)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import batch_filter [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 batch_filter [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 batch_filter [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 batch_filter [as 别名]
def one_run_test_fls():
    fls = FixedLagSmoother(dim_x=2, dim_z=1)

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

    fls.H = np.array([[1.,0.]])
    fls.P *= 200
    fls.R *= 5.
    fls.Q *= 0.001

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

    kf.x = np.array([0., .5])
    kf.F = np.array([[1.,1.],
                     [0.,1.]])
    kf.H = np.array([[1.,0.]])
    kf.P *= 2000
    kf.R *= 1.
    kf.Q *= 0.001

    N = 4 # size of lag

    nom =  np.array([t/2. for t in range (0,40)])
    zs = np.array([t + random.randn()*1.1 for t in nom])

    xs, x = fls.smooth_batch(zs, N)

    M, P, *_ = kf.batch_filter(zs)
    rts_x, *_ = kf.rts_smoother(M, P)

    xfl = xs[:,0].T[0]
    xkf = M[:,0].T[0]

    fl_res = abs(xfl-nom)
    kf_res = abs(xkf-nom)

    if DO_PLOT:
        plt.cla()
        plt.plot(zs,'o', alpha=0.5, marker='o', label='zs')
        plt.plot(x[:,0], label='FLS')
        plt.plot(xfl, label='FLS S')
        plt.plot(xkf, label='KF')
        plt.plot(rts_x[:,0], label='RTS')
        plt.legend(loc=4)
        plt.show()


        print(fl_res)
        print(kf_res)

        print('std fixed lag:', np.mean(fl_res[N:]))
        print('std kalman:', np.mean(kf_res[N:]))

    return np.mean(fl_res) <= np.mean(kf_res)

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

    f.x = np.array([2.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

    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])
        measurements.append(z)

    # 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)

    # 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 batch_filter [as 别名]
def one_run_test_fls():
    fls = FixedLagSmoother(dim_x=2, dim_z=1)

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

    fls.H = np.array([[1.,0.]])
    fls.P *= 200
    fls.R *= 5.
    fls.Q *= 0.001

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

    kf.x = np.array([0., .5])
    kf.F = np.array([[1.,1.],
                     [0.,1.]])
    kf.H = np.array([[1.,0.]])
    kf.P *= 2000
    kf.R *= 1.
    kf.Q *= 0.001

    N = 4 # size of lag

    nom =  np.array([t/2. for t in range (0,40)])
    zs = np.array([t + random.randn()*1.1 for t in nom])

    xs, x = fls.smooth_batch(zs, N)

    M,P,_,_ = kf.batch_filter(zs)
    rts_x,_,_ = kf.rts_smoother(M, P)

    xfl = xs[:,0].T[0]
    xkf = M[:,0].T[0]

    fl_res = abs(xfl-nom)
    kf_res = abs(xkf-nom)
    return np.mean(fl_res) <= np.mean(kf_res)

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

    f.x = np.array([2., 0])

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

    f.H = np.array([[1.,0.]])
    f.P = np.eye(2)*1000.
    f.R = np.eye(1)*5
    f.Q = Q_discrete_white_noise(2, 1., 0.0001)

    f.test_matrix_dimensions()

    x = np.array([2., 0])

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

    H = np.array([[1., 0.]])
    P = np.eye(2)*1000.
    R = np.eye(1)*5
    Q = Q_discrete_white_noise(2, 1., 0.0001)

    zs = [13., None, 1., 2.]*10
    m,c,_,_ = f.batch_filter(zs,update_first=False)

    n = len(zs)
    # test both list of matrices, and single matrix forms
    mp, cp, _, _ = batch_filter(x, P, zs, F, Q, [H]*n, R)

    for x1, x2 in zip(m, mp):
        assert np.allclose(x1, x2)

    for p1, p2 in zip(c, cp):
        assert np.allclose(p1, p2)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import batch_filter [as 别名]
f.predict()
f.x # apriori x, x has been updated
f.P # apriori P, has been updated
f.K # still all 0s

f.update(series[1]) # has to be a measurement from the right period!
f.x # a posteriori estimate of x
f.P # a posteriori estimate of P 
f.K # Now there is a K

# Try it as batch
f.x = np.array([18.0, 5, -12, -8])
# initial covariance has a nice identity default
f.P = np.diag([1, 1, 1, 1]) * 1.

z = f.batch_filter(series, update_first = True)
z[0]
print simdkalman_result 

# Now for statsmodels

class CustomStateSpace(sm.tsa.statespace.MLEModel):
    def __init__(self, endog,
                 initial_state = [18, 5, -12, 8],
                 initial_state_cov = np.eye(4),
                 initial_params = [1, 1, 1],
                 estimate_params = True):
        # Initialize state space model
        super(CustomStateSpace, self).__init__(endog, k_states = 4,
        # k_posdef is a parameter of the Representation class
                                              k_posdef = 2,

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

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

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

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

    fsq.H = np.array([[1.,0.]])  # Measurement function
    fsq.P = np.eye(2) * 1000.    # 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.e-12
        assert abs(f.x[1,0] - fsq.x[1,0]) < 1.e-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]]).T
    f.P = np.eye(2)*100.
    m,c,_,_ = f.batch_filter(zs,update_first=False)

    # 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"], 4)


        plt.show()

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import batch_filter [as 别名]
    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,C = fk.rts_smoother(mu, cov)



    # plot data
    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)

# 需要导入模块: from filterpy.kalman import KalmanFilter [as 别名]
# 或者: from filterpy.kalman.KalmanFilter import batch_filter [as 别名]
tracker.F = np.array([[1, dt, 0, 0],
                      [0,  1, 0, 0],
                      [0,  0, 1, dt],
                      [0,  0, 0, 1]])
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
tracker.Q = block_diag(q, q)
tracker.B = 0
tracker.H = np.array([[1.0, 0, 0, 0],
                      [0  , 0, 1, 0]])
tracker.R = np.array([[0.1, 0],
                      [0,   0.1]])
tracker.x = np.array([[x, y, 0, 0]]).T
tracker.P = np.eye(4)*10.

# Convert measurements to expected form
# Note: n x 4 x 1 np array, measurements must be a col vector
measurements = np.array([np.array([pos]).T for pos in positions])

mu, cov, _, _ = tracker.batch_filter(measurements)

f3 = plt.figure()
plt.scatter(xs, ys)
plt.plot(mu[:,0], mu[:,2])
plt.title('Comparison of Measurements and Kalman Filter')
plt.xlabel('x')
plt.ylabel('y')
f3.show()

# IMM

1