Python UnscentedKalmanFilter.update方法代码示例

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def test_saver_UKF():
    def fx(x, dt):
        F = np.array([[1, dt, 0, 0],
                      [0, 1, 0, 0],
                      [0, 0, 1, dt],
                      [0, 0, 0, 1]], dtype=float)

        return np.dot(F, x)

    def hx(x):
        return np.array([x[0], x[2]])

    dt = 0.1
    points = MerweScaledSigmaPoints(4, .1, 2., -1)
    kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)

    z_std = 0.1
    kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
    kf.x = np.array([-1., 1., -1., 1])
    kf.P *= 1.

    zs = [[i, i] for i in range(40)]
    s = Saver(kf, skip_private=False, skip_callable=False, ignore=['z_mean'])
    for z in zs:
        kf.predict()
        kf.update(z)
        #print(kf.x, kf.log_likelihood, kf.P.diagonal())
        s.save()
    s.to_array()

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def run_localization(
    cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
    sigma_bearing, ellipse_step=1, step=10):

    plt.figure()
    points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
                                    subtract=residual_x)
    ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx,
              dt=dt, points=points, x_mean_fn=state_mean,
              z_mean_fn=z_mean, residual_x=residual_x,
              residual_z=residual_h)

    ukf.x = np.array([2, 6, .3])
    ukf.P = np.diag([.1, .1, .05])
    ukf.R = np.diag([sigma_range**2,
                     sigma_bearing**2]*len(landmarks))
    ukf.Q = np.eye(3)*0.0001

    sim_pos = ukf.x.copy()

    # plot landmarks
    if len(landmarks) > 0:
        plt.scatter(landmarks[:, 0], landmarks[:, 1],
                    marker='s', s=60)

    track = []
    for i, u in enumerate(cmds):
        sim_pos = move(sim_pos, u, dt/step, wheelbase)
        track.append(sim_pos)

        if i % step == 0:
            ukf.predict(fx_args=u)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
                     facecolor='k', alpha=0.3)

            x, y = sim_pos[0], sim_pos[1]
            z = []
            for lmark in landmarks:
                dx, dy = lmark[0] - x, lmark[1] - y
                d = sqrt(dx**2 + dy**2) + randn()*sigma_range
                bearing = atan2(lmark[1] - y, lmark[0] - x)
                a = (normalize_angle(bearing - sim_pos[2] +
                     randn()*sigma_bearing))
                z.extend([d, a])
            ukf.update(z, hx_args=(landmarks,))

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
                     facecolor='g', alpha=0.8)
    track = np.array(track)
    plt.plot(track[:, 0], track[:,1], color='k', lw=2)
    plt.axis('equal')
    plt.title("UKF Robot localization")
    plt.show()
    return ukf

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def estimateUKF(camPoses):
    points = MerweScaledSigmaPoints(3,.1,2.,0.)
    filter = UKF(3,3,0,h, f, points, sqrt_fn=None, x_mean_fn=state_mean, z_mean_fn=state_mean, residual_x=residual, residual_z=residual)
    filter.P = np.diag([0.04,0.04,0.003])
    filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2])
    filter.R = np.diag([100,100,0.25])
    Uposes = [[],[]]
    for i in range(len(speed)):
        x = filter.x
        Uposes[0].append(x[0])
        Uposes[1].append(x[1])
        filter.predict(fx_args=[speed[i],angle[i]*0.01745])
        filter.update(z = [camPoses[0][i],camPoses[1][i],camPoses[2][i]])
    return Uposes

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def test_radar():
    def fx(x, dt):
        A = np.eye(3) + dt * np.array ([[0, 1, 0],
                                        [0, 0, 0],
                                        [0, 0, 0]])
        return A.dot(x)

    def hx(x):
        return np.sqrt (x[0]**2 + x[2]**2)

    dt = 0.05

    sp = JulierSigmaPoints(n=3, kappa=0.)
    kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)

    kf.Q *= 0.01
    kf.R = 10
    kf.x = np.array([0., 90., 1100.])
    kf.P *= 100.
    radar = RadarSim(dt)

    t = np.arange(0,20+dt, dt)

    n = len(t)

    xs = np.zeros((n,3))

    random.seed(200)
    rs = []
    #xs = []
    for i in range(len(t)):
        r = radar.get_range()
        #r = GetRadar(dt)
        kf.predict()
        kf.update(z=[r])

        xs[i,:] = kf.x
        rs.append(r)

    if DO_PLOT:
        print(xs[:,0].shape)

        plt.figure()
        plt.subplot(311)
        plt.plot(t, xs[:,0])
        plt.subplot(312)
        plt.plot(t, xs[:,1])
        plt.subplot(313)

        plt.plot(t, xs[:,2])

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def test_rts():
    def fx(x, dt):
        A = np.eye(3) + dt * np.array ([[0, 1, 0],
                                        [0, 0, 0],
                                        [0, 0, 0]])
        f = np.dot(A, x)
        return f

    def hx(x):
        return np.sqrt (x[0]**2 + x[2]**2)

    dt = 0.05

    sp = JulierSigmaPoints(n=3, kappa=1.)
    kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)

    kf.Q *= 0.01
    kf.R = 10
    kf.x = np.array([0., 90., 1100.])
    kf.P *= 100.
    radar = RadarSim(dt)

    t = np.arange(0,20+dt, dt)

    n = len(t)

    xs = np.zeros((n,3))

    random.seed(200)
    rs = []
    #xs = []
    for i in range(len(t)):
        r = radar.get_range()
        #r = GetRadar(dt)
        kf.predict()
        kf.update(z=[r])

        xs[i,:] = kf.x
        rs.append(r)


    kf.x = np.array([0., 90., 1100.])
    kf.P = np.eye(3)*100
    M, P = kf.batch_filter(rs)
    assert np.array_equal(M, xs), "Batch filter generated different output"

    Qs = [kf.Q]*len(t)
    M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def test_1d():

    def fx(x, dt):
        F = np.array([[1., dt],
                      [0,  1]])

        return np.dot(F, x)

    def hx(x):
        return np.array([[x[0]]])



    ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)

    ckf.x = np.array([[1.], [2.]])
    ckf.P = np.array([[1, 1.1],
                      [1.1, 3]])

    ckf.R = np.eye(1) * .05
    ckf.Q = np.array([[0., 0], [0., .001]])

    dt = 0.1
    points = MerweScaledSigmaPoints(2, .1, 2., -1)
    kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)


    kf.x = np.array([1, 2])
    kf.P = np.array([[1, 1.1],
                     [1.1, 3]])
    kf.R *= 0.05
    kf.Q = np.array([[0., 0], [0., .001]])


    for i in range(50):
        z = np.array([[i+randn()*0.1]])
        #xx, pp, Sx = predict(f, x, P, Q)
        #x, P = update(h, z, xx, pp, R)
        ckf.predict()
        ckf.update(z)
        kf.predict()
        kf.update(z[0])
        assert abs(ckf.x[0] -kf.x[0]) < 1e-10
        assert abs(ckf.x[1] -kf.x[1]) < 1e-10


    plt.show()

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def test_1d():
    def fx(x, dt):
        F = np.array([[1., dt],
                      [0,  1]])

        return np.dot(F, x)

    def hx(x):
        return x[0:1]

    ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)

    ckf.x = np.array([[1.], [2.]])
    ckf.P = np.array([[1, 1.1],
                      [1.1, 3]])

    ckf.R = np.eye(1) * .05
    ckf.Q = np.array([[0., 0], [0., .001]])

    dt = 0.1
    points = MerweScaledSigmaPoints(2, .1, 2., -1)
    kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)

    kf.x = np.array([1, 2])
    kf.P = np.array([[1, 1.1],
                     [1.1, 3]])
    kf.R *= 0.05
    kf.Q = np.array([[0., 0], [0., .001]])

    s = Saver(kf)
    for i in range(50):
        z = np.array([[i+randn()*0.1]])
        ckf.predict()
        ckf.update(z)
        kf.predict()
        kf.update(z[0])
        assert abs(ckf.x[0] - kf.x[0]) < 1e-10
        assert abs(ckf.x[1] - kf.x[1]) < 1e-10
        s.save()

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

    s.to_array()

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def filter(measurements):

    dt = 0.1

    # x = [x, x', x'' y, y', y'']
    x = np.array([measurements[0][0], 0., 0., measurements[0][1], 0., 0.])

    G = np.array([[0.19*(dt**2)],
                  [dt],
                  [1.],
                  [0.19*(dt**2)],
                  [dt],
                  [1.]])
     
    Q = G*G.T*0.1**2

    # Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
    sigmas = MerweScaledSigmaPoints(n=6, alpha=1., beta=2., kappa=-3.)
    
    bot_filter = UKF(dim_x=6, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
    bot_filter.x = np.array([measurements[0][0], 0., 0, measurements[0][1], 0., 0.])
    #bot_filter.F = F
    bot_filter.H = np.array([[1., 0., 0., 1., 0., 0.]])
    #bot_filter.Q = Q
    bot_filter.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=1, var=0.0002)
    bot_filter.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=1, var=0.0002)
    bot_filter.P *= 500
    bot_filter.R = np.diag([0.0001, 0.0001])

    observable_meas = measurements[0:len(measurements)-60]

    pos, cov = [], []
    for z in observable_meas:
        pos.append(bot_filter.x)
        cov.append(bot_filter.P)
        
        bot_filter.predict()
        bot_filter.update(z)

    for i in range(0,60):
        bot_filter.predict()
        pos.append(bot_filter.x)
        
    return pos

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def test_linear_1d():
    """ should work like a linear KF if problem is linear """


    def fx(x, dt):
        F = np.array([[1., dt],
                      [0,  1]], dtype=float)

        return np.dot(F, x)

    def hx(x):
        return np.array([x[0]])


    dt = 0.1
    points = MerweScaledSigmaPoints(2, .1, 2., -1)
    kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)


    kf.x = np.array([1, 2])
    kf.P = np.array([[1, 1.1],
                     [1.1, 3]])
    kf.R *= 0.05
    kf.Q = np.array([[0., 0], [0., .001]])

    z = np.array([2.])
    kf.predict()
    kf.update(z)

    zs = []
    for i in range(50):
        z = np.array([i+randn()*0.1])
        zs.append(z)

        kf.predict()
        kf.update(z)
        print('K', kf.K.T)
        print('x', kf.x)

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def linear_filter(measurements):
    
    dt = 1.0

    # x = [x, x', y, y']
    x = np.array([measurements[0][0], 0., measurements[0][1], 0.])

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

    # Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
    sigmas = MerweScaledSigmaPoints(n=4, alpha=0.3, beta=2., kappa=-3.)
    
    bot_filter = UKF(dim_x=4, dim_z=2, fx=f_linear, hx=h_linear, dt=dt, points=sigmas)
    bot_filter.x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
    #bot_filter.F = F
    bot_filter.H = np.asarray(H)
    #bot_filter.Q = Q
    bot_filter.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.1)
    bot_filter.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.1)
    bot_filter.P *= 10
    bot_filter.R = np.diag([0.0001, 0.0001])

    observable_meas = measurements[0:len(measurements)-60]

    pos, cov = [], []
    for z in observable_meas:
        pos.append(bot_filter.x)
        cov.append(bot_filter.P)
        
        bot_filter.predict()
        bot_filter.update(z)

    for i in range(0,60):
        bot_filter.predict()
        pos.append(bot_filter.x)
        
    return pos

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
    radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, kappa=0.)
    radarUKF.Q *= Q_discrete_white_noise(3, 1, .01)
    radarUKF.R *= 10
    radarUKF.x = np.array([0., 90., 1100.])
    radarUKF.P *= 100.

    t = np.arange(0, 20+dt, dt)
    n = len(t)
    xs = []
    rs = []
    for i in range(n):
        r = GetRadar(dt)
        rs.append(r)

        radarUKF.update(r, hx, fx)

        xs.append(radarUKF.x)

    xs = np.asarray(xs)

    plt.subplot(311)
    plt.plot(t, xs[:, 0])
    plt.title('distance')

    plt.subplot(312)
    plt.plot(t, xs[:, 1])
    plt.title('velocity')

    plt.subplot(313)
    plt.plot(t, xs[:, 2])

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
uxs = []
zs = []
txs = []
radius = 100
delta = 2*np.pi/360*10
for i in range(5):
    # 真实位置
    target_pos_x = 300+math.cos(i*delta)*radius + random.randn()*0.0001
    target_pos_y = 300+math.sin(i*delta)*radius + random.randn()*0.0001
    txs.append((target_pos_x, target_pos_y))
    
    # 测量位置
    zr = np.sqrt(target_pos_x**2 + target_pos_y**2) + random.randn()*5
    za = np.arctan2(target_pos_y,target_pos_x) + random.randn()*1
    zs.append([zr,za])
    
    ukf.predict()
    ukf.update(zs[-1])
    uxs.append(ukf.x.copy())
    
uxs = np.asarray(uxs)
txs = np.asarray(txs)
zs = np.asarray(zs)
plt.plot(uxs[:,0], uxs[:,2])
plt.plot(txs[:,0], txs[:,1])
#plt.plot(zs[:,.0], zs[:,1],'ro')
plt.legend(('Filter','True'), loc=4)
plt.show()
plt.plot(zs[:,0])
plt.plot(zs[:,1])
plt.show()

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
    z = []
    for lmark in m:
        d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r
        bearing = atan2(lmark[1] - xp[1], lmark[0] - xp[0])
        a = normalize_angle(bearing - xp[2] + randn()*sigma_h)
        z.extend([d, a])

        #if cindex % 20 == 0:
        #    plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])], [lmark[1], lmark[1]-d*sin(a+xp[2])], color='r')

        if cindex  == 1197:
            plt.plot([lmark[0], lmark[0] - d2*cos(a2+xp[2])],
                     [lmark[1], lmark[1] - d2*sin(a2+xp[2])], color='r')
            plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])],
                     [lmark[1], lmark[1] - d*sin(a+xp[2])], color='k')

    ukf.update(np.array(z), hx_args=(m,))

    if cindex % 20 == 0:
        plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=std,
                                 facecolor='g', alpha=0.5)
    cindex += 1


track = np.array(track)
plt.plot(track[:, 0], track[:,1], color='k')

plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()

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

    std_noise = .1


    f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, kappa=0)
    f.x = np.array([50., 90., 0])
    f.P *= 100
    f.R = np.eye(2)*(std_noise**2)
    f.Q = np.eye(3)*.001
    f.Q[0,0]=0
    f.Q[2,2]=0

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

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

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


    kf.R = f.R
    kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
    kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)

    measurements = []
    results = []

    zs = []
    kfxs = []
    for t in range (0,12000):
        a = t / 30 + 90
        x = cos(radians(a)) * 50.+ randn() * std_noise
        y = sin(radians(a)) * 50. + randn() * std_noise
        # create measurement = t plus white noise
        z = np.array([x,y])
        zs.append(z)

        f.predict()
        f.update(z)

        kf.predict()
        kf.update(z)

        # save data
        results.append (hx(f.x))
        kfxs.append(kf.x)
        #print(f.x)

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

    print(results)
    if DO_PLOT:
        plt.plot(zs[:,0], zs[:,1], c='r', label='z')
        plt.plot(results[:,0], results[:,1], c='k', label='UKF')
        plt.plot(kfxs[:,0], kfxs[:,3], c='g', label='KF')
        plt.legend(loc='best')
        plt.axis('equal')

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import update [as 别名]
def test_fixed_lag():
    def fx(x, dt):
        A = np.eye(3) + dt * np.array ([[0, 1, 0],
                                        [0, 0, 0],
                                        [0, 0, 0]])
        f = np.dot(A, x)
        return f

    def hx(x):
        return np.sqrt (x[0]**2 + x[2]**2)

    dt = 0.05

    kf = UKF(3, 1, dt, fx=fx, hx=hx, kappa=0.)

    kf.Q *= 0.01
    kf.R = 10
    kf.x = np.array([0., 90., 1100.])
    kf.P *= 1.
    radar = RadarSim(dt)

    t = np.arange(0,20+dt, dt)

    n = len(t)

    xs = np.zeros((n,3))

    random.seed(200)
    rs = []
    #xs = []

    M = []
    P = []
    N =10
    flxs = []
    for i in range(len(t)):
        r = radar.get_range()
        #r = GetRadar(dt)
        kf.predict()
        kf.update(z=[r])

        xs[i,:] = kf.x
        flxs.append(kf.x)
        rs.append(r)
        M.append(kf.x)
        P.append(kf.P)
        print(i)
        #print(i, np.asarray(flxs)[:,0])
        if i == 20 and len(M) >= N:
            try:
                M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:], Ps=np.asarray(P)[-N:])
                flxs[-N:] = M2
                #flxs[-N:] = [20]*N
            except:
                print('except', i)
            #P[-N:] = P2


    kf.x = np.array([0., 90., 1100.])
    kf.P = np.eye(3)*100
    M, P = kf.batch_filter(rs)

    Qs = [kf.Q]*len(t)
    M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)


    flxs = np.asarray(flxs)
    print(xs[:,0].shape)

    plt.figure()
    plt.subplot(311)
    plt.plot(t, xs[:,0])
    plt.plot(t, flxs[:,0], c='r')
    plt.plot(t, M2[:,0], c='g')
    plt.subplot(312)
    plt.plot(t, xs[:,1])
    plt.plot(t, flxs[:,1], c='r')
    plt.plot(t, M2[:,1], c='g')

    plt.subplot(313)
    plt.plot(t, xs[:,2])
    plt.plot(t, flxs[:,2], c='r')
    plt.plot(t, M2[:,2], c='g')