Python UnscentedKalmanFilter.P方法代码示例

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import P [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 P [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 P [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 P [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 P [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 P [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 P [as 别名]
#m = array([[5., 10], [10, 5]])


sigma_r = .3
sigma_h =  .1#radians(.5)#np.radians(1)
#sigma_steer =  radians(10)
dt = 0.1
wheelbase = 0.5

points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0, subtract=residual_x)
#points = JulierSigmaPoints(n=3,  kappa=3)
ukf= UKF(dim_x=3, dim_z=2*len(m), 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 = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_r**2, sigma_h**2]* len(m))
ukf.Q =np.eye(3)*.00001


u = array([1.1, 0.])

xp = ukf.x.copy()


plt.cla()
plt.scatter(m[:, 0], m[:, 1])

cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)]
cmds.extend([cmds[-1]]*50)

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

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

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

    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)


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

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

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

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import P [as 别名]
track_offset = 30230   #tracking log is ~29998 ms behind left gopro
track_data_log_offset = track_offset + stereo_offset

left_reader = lr.LogReader(l_logname,l_first_data_time_ms)
right_reader = lr.LogReader(r_logname,r_first_data_time_ms)

track = tr.TrackingReader(track_fname,right_reader,track_data_log_offset,F,30,1,vid_fname=vid_fname)

pos_init = left_reader.get_ekf_loc_1d(START_TIME)
vel_init = left_reader.get_ekf_vel(START_TIME)
att_init = np.deg2rad(left_reader.get_ekf_att(START_TIME))
att_vel_init = np.zeros(3)
state_init = np.hstack((pos_init,vel_init,att_init,att_vel_init))

ukf = UKF(dim_x=12, dim_z=15, dt=1.0/30, fx=fx, hx=hx)
ukf.P = np.diag([5,5,2, 2,2,2, .017,.017,.017, .1,.1,.1])
ukf.x = state_init
ukf.Q = np.diag([.5,.5,.5, .5,.5,.5, .1,.1,.1, .1,.1,.1])

T = np.array([16.878, -7.1368, 0])		#Translation vector joining two inertial frames
time = np.arange(START_TIME,END_TIME,dt)
print time.shape,time[0],time[-1]
d = np.linspace(20,40,time.shape[0])
zs = np.zeros((time.shape[0],15))
Rs = np.zeros((time.shape[0],15,15))

means = np.zeros((time.shape[0],12))
covs = np.zeros((time.shape[0],12,12))
cam_locs = np.zeros((time.shape[0],3))
ekf_locs = np.zeros((time.shape[0],3))

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import P [as 别名]
    return x
def residual(a, b):
    y = a - b
    y[2] = normalize_angle(y[2])
    return y
def f(x,dt,u):
        """Estimate the non-linear state of the system"""
        #print ((u[0]/self.L)*math.tan(u[1]))
        return np.array([x[0]+u[0]*math.cos(x[2]),
                         x[1]+u[0]*math.sin(x[2]),
                         x[2]+((u[0]/1)*math.tan(u[1]))])
def h(z):
    return z               
points = MerweScaledSigmaPoints(3,.001,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,1])
filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2])
filter.R = np.diag([100,100,0.25])
robot = vehicle.Vehicle(1,50)           #create a robot
robot.setOdometry(True)                 #configure its odometer
robot.setOdometryVariance([0.2,1])
speed,angle = [],[]
for a in xrange(4):                     #create a retangular path
    for i in xrange(400):
        angle.append(0)
    for i in xrange(9):
        angle.append(10)

for i in xrange(len(angle)):        #set the speed to a constant along the path
    speed.append(1)

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import P [as 别名]
    """ takes a state variable and returns the measurement that would
    correspond to that state.
    """
    px = landmark[0]
    py = landmark[1]
    dist = np.sqrt((px - x[0])**2 + (py - x[1])**2)

    Hx = array([dist, atan2(py - x[1], px - x[0]) - x[2]])
    return Hx

points = MerweScaledSigmaPoints(n=3, alpha=1.e-3, beta=2, kappa=0)
ukf= UKF(dim_x=3, dim_z=2, 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 = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .2])
ukf.R = np.diag([sigma_r**2, sigma_h**2])
ukf.Q = np.zeros((3,3))


u = array([1.1, .01])

xp = ukf.x.copy()

plt.figure()
plt.scatter(m[:, 0], m[:, 1])

for i in range(200):
    xp = move(xp, u, dt/10., wheelbase) # simulate robot
    plt.plot(xp[0], xp[1], ',', color='g')

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import P [as 别名]
def two_radar_constvel():
    dt = 5

    def hx(x):
        r1, b1 = hx.R1.reading_of((x[0], x[2]))
        r2, b2 = hx.R2.reading_of((x[0], x[2]))

        return array([r1, b1, r2, b2])
        pass

    def fx(x, dt):
        x_est = x.copy()
        x_est[0] += x[1] * dt
        x_est[2] += x[3] * dt
        return x_est

    f = UKF(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
    aircraft = ACSim((100, 100), (0.1 * dt, 0.02 * dt), 0.002)

    range_std = 0.2
    bearing_std = radians(0.5)

    R1 = RadarStation((0, 0), range_std, bearing_std)
    R2 = RadarStation((200, 0), range_std, bearing_std)

    hx.R1 = R1
    hx.R2 = R2

    f.x = array([100, 0.1, 100, 0.02])
    f.R = np.diag([range_std ** 2, bearing_std ** 2, range_std ** 2, bearing_std ** 2])
    q = Q_discrete_white_noise(2, var=0.002, dt=dt)
    # q = np.array([[0,0],[0,0.0002]])
    f.Q[0:2, 0:2] = q
    f.Q[2:4, 2:4] = q
    f.P = np.diag([0.1, 0.01, 0.1, 0.01])

    track = []
    zs = []

    for i in range(int(300 / dt)):

        pos = aircraft.update()

        r1, b1 = R1.noisy_reading(pos)
        r2, b2 = R2.noisy_reading(pos)

        z = np.array([r1, b1, r2, b2])
        zs.append(z)
        track.append(pos.copy())

    zs = asarray(zs)

    xs, Ps, Pxz = f.batch_filter(zs)
    ms, _, _ = f.rts_smoother2(xs, Ps, Pxz)

    track = asarray(track)
    time = np.arange(0, len(xs) * dt, dt)

    plt.figure()
    plt.subplot(411)
    plt.plot(time, track[:, 0])
    plt.plot(time, xs[:, 0])
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("x position (m)")

    plt.subplot(412)
    plt.plot(time, track[:, 1])
    plt.plot(time, xs[:, 2])
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("y position (m)")

    plt.subplot(413)
    plt.plot(time, xs[:, 1])
    plt.plot(time, ms[:, 1])
    plt.legend(loc=4)
    plt.ylim([0, 0.2])
    plt.xlabel("time (sec)")
    plt.ylabel("x velocity (m/s)")

    plt.subplot(414)
    plt.plot(time, xs[:, 3])
    plt.plot(time, ms[:, 3])
    plt.ylabel("y velocity (m/s)")
    plt.legend(loc=4)
    plt.xlabel("time (sec)")

    plt.show()

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import P [as 别名]
def two_radar_constalt():
    dt = 0.05

    def hx(x):
        r1, b1 = hx.R1.reading_of((x[0], x[2]))
        r2, b2 = hx.R2.reading_of((x[0], x[2]))

        return array([r1, b1, r2, b2])
        pass

    def fx(x, dt):
        x_est = x.copy()
        x_est[0] += x[1] * dt
        return x_est

    vx = 100 / 1000  # meters/sec
    vz = 0.0

    f = UKF(dim_x=3, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
    aircraft = ACSim((0, 1), (vx * dt, vz * dt), 0.00)

    range_std = 1 / 1000.0
    bearing_std = 1 / 1000000.0

    R1 = RadarStation((0, 0), range_std, bearing_std)
    R2 = RadarStation((60, 0), range_std, bearing_std)

    hx.R1 = R1
    hx.R2 = R2

    f.x = array([aircraft.pos[0], vx, aircraft.pos[1]])
    f.R = np.diag([range_std ** 2, bearing_std ** 2, range_std ** 2, bearing_std ** 2])
    q = Q_discrete_white_noise(2, var=0.0002, dt=dt)
    # q = np.array([[0,0],[0,0.0002]])
    f.Q[0:2, 0:2] = q
    f.Q[2, 2] = 0.0002
    f.P = np.diag([0.1, 0.01, 0.1]) * 0.1

    track = []
    zs = []

    for i in range(int(500 / dt)):
        pos = aircraft.update()

        r1, b1 = R1.noisy_reading(pos)
        r2, b2 = R2.noisy_reading(pos)

        z = np.array([r1, b1, r2, b2])
        zs.append(z)
        track.append(pos.copy())

    zs = asarray(zs)

    xs, Ps = f.batch_filter(zs)
    ms, _, _ = f.rts_smoother(xs, Ps)

    track = asarray(track)
    time = np.arange(0, len(xs) * dt, dt)

    plt.figure()
    plt.subplot(311)
    plt.plot(time, track[:, 0])
    plt.plot(time, xs[:, 0])
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("x position (m)")

    plt.subplot(312)
    plt.plot(time, xs[:, 1] * 1000, label="UKF")
    plt.plot(time, ms[:, 1] * 1000, label="RTS")
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("velocity (m/s)")

    plt.subplot(313)
    plt.plot(time, xs[:, 2] * 1000, label="UKF")
    plt.plot(time, ms[:, 2] * 1000, label="RTS")
    plt.legend(loc=4)
    plt.xlabel("time (sec)")
    plt.ylabel("altitude (m)")
    plt.ylim([900, 1100])

    for z in zs[:10]:
        p = R1.z_to_x(z[0], z[1])
        # plt.scatter(p[0], p[1], marker='+', c='k')

        p = R2.z_to_x(z[2], z[3])
        # plt.scatter(p[0], p[1], marker='+', c='b')

    plt.show()