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