Python UnscentedKalmanFilter.Q方法代码示例

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import Q [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 Q [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 Q [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 Q [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 Q [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 Q [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 Q [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 Q [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 Q [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]
cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)])

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

for i in range(1,time.shape[0]):
	t = time[i]

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

robot.sim_Path(speed,angle)             #run in a rectangular path

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

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

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import Q [as 别名]
platform is stationary, this is a very difficult problem because there are
an infinite number of solutions. The literature is filled with this example,
along with proposed solutions (usually, platform makes manuevers).

"""
dt = 0.1
y = 20
platform_pos = (0, 20)


sf = SUKF(2, 1, dt, alpha=1.0e-4, beta=2.0, kappa=1.0)
sf.Q = Q_discrete_white_noise(2, dt, 0.1)


f = UKF(2, 1, dt, kappa=0.0)
f.Q = Q_discrete_white_noise(2, dt, 0.1)


def fx(x, dt):
    """ state transition function"""

    # pos = pos + vel
    # vel = vel
    return array([x[0] + x[1], x[1]])


def hx(x):
    """ measurement function - convert position to bearing"""

    return math.atan2(platform_pos[1], x[0] - platform_pos[0])

# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import Q [as 别名]
    """
    px, py = landmark
    dist = sqrt((px - x[0])**2 + (py - x[1])**2)
    angle = atan2(py - x[1], px - x[0])
    return array([dist, normalize_angle(angle - x[2])])


points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0)
#points = JulierSigmaPoints(n=3,  kappa=3)
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 = None#np.eye(3)*.00000


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]
cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)])