Python kalman.UnscentedKalmanFilter类代码示例

 def __init__(self, trueTrajectory, dt, route, Q=np.eye(2), R=np.eye(2)):
     
     #from filterpy.kalman import KalmanFilter as KF
     from filterpy.kalman import UnscentedKalmanFilter as UKF  
     from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints        
     
     n_state = len(Q)
     n_meas = len(R)
     sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=0.)
     ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_v, hx=h_kal,
               dt=dt, points=sigmas)
     ukf.Q = Q
     ukf.R = R
     self.ukf = ukf
     self.isFirst = True
     self.route = route

def test_sigma_points_1D():
    """ tests passing 1D data into sigma_points"""

    kappa = 0.
    ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)

    points = ukf.weights(1, 0.)
    assert len(points) == 3

    mean = 5
    cov = 9

    Xi = ukf.sigma_points (mean, cov, kappa)
    xm, ucov = unscented_transform(Xi, ukf.W, ukf.W, 0)

    # sum of weights*sigma points should be the original mean
    m = 0.0
    for x,w in zip(Xi, ukf.W):
        m += x*w

    assert abs(m-mean) < 1.e-12
    assert abs(xm[0] - mean) < 1.e-12
    assert abs(ucov[0,0]-cov) < 1.e-12

    assert Xi.shape == (3,1)
    assert len(ukf.W) == 3

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

 def __init__(self, trueTrajectory, dt=.1, noise = 0.):
     from filterpy.kalman import UnscentedKalmanFilter as UKF  
     from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints
     self.dt = dt
     sigmas = SigmaPoints(3, alpha=.1, beta=2., kappa=0.)
     self.KF = UKF(dim_x=3, dim_z=2, fx=f_kal_a, hx=h_kal,
               dt=dt, points=sigmas)
     self.KF.Q = np.diag([1., 0.5, 0.2])
     self.KF.R = np.diag([2., 1.12])*noise + np.diag([.05, .05])
     self.first = True

def test_linear_2d():
    """ should work like a linear KF if problem is linear """


    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 = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)


    kf.x = np.array([-1., 1., -1., 1])
    kf.P*=0.0001
    #kf.R *=0
    #kf.Q

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



    Ms, Ps = kf.batch_filter(zs)
    smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)

def test_sigma_plot():
    """ Test to make sure sigma's correctly mirror the shape and orientation
    of the covariance array."""

    x = np.array([[1, 2]])
    P = np.array([[2, 1.2],
                  [1.2, 2]])
    kappa = .1

    # if kappa is larger, than points shoudld be closer together
    sp0 = UKF.weights(2, kappa)
    sp1 = UKF.weights(2, kappa*1000)

    Xi0 = UKF.sigma_points (x, P, kappa)
    Xi1 = UKF.sigma_points (x, P, kappa*1000)

    assert max(Xi1[:,0]) > max(Xi0[:,0])
    assert max(Xi1[:,1]) > max(Xi0[:,1])

    if DO_PLOT:
        plt.figure()
        for i in range(Xi0.shape[0]):
            plt.scatter((Xi0[i,0]-x[0, 0])*sp0[i] + x[0, 0],
                        (Xi0[i,1]-x[0, 1])*sp0[i] + x[0, 1],
                         color='blue')

        for i in range(Xi1.shape[0]):
            plt.scatter((Xi1[i, 0]-x[0, 0]) * sp1[i] + x[0,0],
                        (Xi1[i, 1]-x[0, 1]) * sp1[i] + x[0,1],
                         color='green')

        stats.plot_covariance_ellipse([1, 2], P)

def test_batch_missing_data():
    """ batch filter should accept missing data with None in the measurements """


    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 = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)


    kf.x = np.array([-1., 1., -1., 1])
    kf.P*=0.0001

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

    zs[2] = None
    Rs = [1]*len(zs)
    Rs[2] = None
    Ms, Ps = kf.batch_filter(zs)

def sensor_fusion_kf():
    global hx, fx
    # create unscented Kalman filter with large initial uncertainty
    points = JulierSigmaPoints(2, kappa=2)
    kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, points=points)
    kf.x = np.array([100, 100.])
    kf.P *= 40
    return kf

 def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
     n_state = len(Q)
     n_meas = len(R)
     sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=1.)
     ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal, hx=h_kal,
               dt=dt, points=sigmas)
     ukf.Q = Q
     ukf.R = R
     self.ukf = ukf
     self.isFirst = True

 def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
     n_state = len(Q)
     n_meas = len(R)
     sigmas = SigmaPoints(n_state, alpha=.5, beta=2., kappa=0.)
     ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_accel, hx=h_kal_accel,
               dt=dt, points=sigmas, x_mean_fn = state_mean, residual_x=res_x,
               residual_z=res_x)
     ukf.Q = Q
     ukf.R = R
     self.ukf = ukf
     self.isFirst = True

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

def show_sigma_transform():
    fig = plt.figure()
    ax=fig.gca()

    x = np.array([0, 5])
    P = np.array([[4, -2.2], [-2.2, 3]])

    plot_covariance_ellipse(x, P, facecolor='b', variance=9, alpha=0.5)
    S = UKF.sigma_points(x=x, P=P, kappa=0)
    plt.scatter(S[:,0], S[:,1], c='k', s=80)

    x = np.array([15, 5])
    P = np.array([[3, 1.2],[1.2, 6]])
    plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.5)


    ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6))
    ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6))
    ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6))
    ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6))
    ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6))

    ax.axes.get_xaxis().set_visible(False)
    ax.axes.get_yaxis().set_visible(False)

    #plt.axis('equal')
    plt.show()

def test_linear_2d():
    """ should work like a linear KF if problem is linear """
    
    
    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
    kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, kappa=0)
    
    
    kf.x = np.array([-1., 1., -1., 1])
    kf.P*=0.0001
    #kf.R *=0
    #kf.Q 
    
    zs = []
    for i in range(20):
        z = np.array([i+randn()*0.1, i+randn()*0.1])
        zs.append(z)

        
    
    Ms, Ps = kf.batch_filter(zs)
    smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
    
    if DO_PLOT:
        zs = np.asarray(zs)
        
        #plt.plot(zs[:,0])
        plt.plot(Ms[:,0])
        plt.plot(smooth_x[:,0], smooth_x[:,2])
        
        print(smooth_x)

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

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