本文整理汇总了Python中filterpy.kalman.UnscentedKalmanFilter.rts_smoother方法的典型用法代码示例。如果您正苦于以下问题：Python UnscentedKalmanFilter.rts_smoother方法的具体用法？Python UnscentedKalmanFilter.rts_smoother怎么用？Python UnscentedKalmanFilter.rts_smoother使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类filterpy.kalman.UnscentedKalmanFilter的用法示例。
在下文中一共展示了UnscentedKalmanFilter.rts_smoother方法的6个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: test_linear_2d
# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import rts_smoother [as 别名]
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)示例2: test_rts
# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import rts_smoother [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)示例3: test_linear_2d
# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import rts_smoother [as 别名]
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)示例4: test_fixed_lag
# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import rts_smoother [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')示例5: test_rts
# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import rts_smoother [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')示例6: two_radar_constalt
# 需要导入模块: from filterpy.kalman import UnscentedKalmanFilter [as 别名]
# 或者: from filterpy.kalman.UnscentedKalmanFilter import rts_smoother [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()