Python UnscentedKalmanFilter.R方法代码示例

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

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

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

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import R [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 R [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 R [as 别名]
def h_cv(x):
    return np.array([x[0], x[2]])

def e(x):
    res = []
    for n in range(x.shape[0]):
        res.append(np.sqrt(x[n][0]**2+x[n][2]**2))
    return res

dt = 1.0
random.seed(1234)

ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, kappa=0)
ukf.x = np.array([100., 0., 0., 0.])
ukf.R = np.diag([25, 25])
ukf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ukf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04)

ckf = CubatureKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt)
ckf.x = np.array([100., 0., 0., 0.])
ckf.R = np.diag([25, 25])
ckf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ckf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04)

uxs = []
pxs = []
zs = []
txs = []
cxs = []
radius = 100

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import R [as 别名]
                  [0,  1, 0, 0],
                  [0,  0, 1, dt],
                  [0,  0, 0, 1]])
    return np.dot(F, x)

def h_cv(x):
    r = np.sqrt(x[0]**2 + x[2]**2)
    angle = np.arctan2(x[2],x[0])
    return np.array([r, angle])

dt = 1.0
random.seed(1234)

ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, kappa=0)
ukf.x = np.array([400., 0., 300., 0.])
ukf.R = np.diag([25, 1])
ukf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.02)
ukf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.02)

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))
    
    # 测量位置

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import R [as 别名]

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)

v = cmds[-1][0]

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