卡尔曼滤波是非常经典的预测追踪算法,能够在系统存在噪声和干扰的情况下进行系统状态的最优估计,广泛使用在导航、制导、控制相关的领域。
$$ \begin{array}{c} P\left(\boldsymbol{x}{k},z_k \mid \boldsymbol{x}{0}, \boldsymbol{u}{1: k}, \boldsymbol{z}{1: k-1}\right) =P\left(\boldsymbol{x}{k} \mid \boldsymbol{x}{0}, \boldsymbol{u}{1: k}, \boldsymbol{z}{1: k}\right)·P(z_k)\ = P\left(\boldsymbol{z}{k} \mid \boldsymbol{x}{k}\right) P\left(\boldsymbol{x}{k} \mid \boldsymbol{x}{0}, \boldsymbol{u}{1: k}, \boldsymbol{z}{1: k-1}\right) \end{array} $$
先验
。似然
:似然
与先验
乘积与后验
相差一个常数倍$$ \begin{aligned} -2 \hat{\boldsymbol{x}}{k}^{\mathrm{T}} \hat{\boldsymbol{P}}{k}^{-1} \boldsymbol{x}{k}&=-2 \boldsymbol{z}{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{C}{k} \boldsymbol{x}{k}-2 \check{\boldsymbol{x}}{k}^{\mathrm{T}} \check{\boldsymbol{P}}{k}^{-1} \boldsymbol{x}{k} \ \hat{\boldsymbol{P}}{k}^{-1} \hat{\boldsymbol{x}}{k}&=\boldsymbol{C}{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{z}{k}+\check{\boldsymbol{P}}{k}^{-1} \check{\boldsymbol{x}}{k} \ \hat{\boldsymbol{x}}{k} & =\hat{\boldsymbol{P}}{k} \boldsymbol{C}{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{z}{k}+\hat{\boldsymbol{P}}{k} \check{\boldsymbol{P}}{k}^{-1} \check{\boldsymbol{x}}{k} \ \hat{\boldsymbol{x}}{k}&=\boldsymbol{K} \boldsymbol{z}{k}+\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}{k}\right)\check{\boldsymbol{x}}{k} \ \hat{\boldsymbol{x}}{k}&=\check{\boldsymbol{x}}{k}+\boldsymbol{K}\left(\boldsymbol{z}{k}-\boldsymbol{C}{k} \check{\boldsymbol{x}}_{k}\right)\end{aligned} $$
预测
和更新
(Update)两个步骤:123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117 | import numpy as npimport matplotlib.pyplot as pltimport matplotlib.animation as animationimport cv2def target_simple(t): x_t = 100 y_t = 100 + t return x_t, y_tdef get_u(target_xy, cur_xy, length): d_v = np.mat(target_xy).T - cur_xy mode_d = (np.array(d_v) ** 2).sum() ** 0.5 u = d_v / mode_d * length return ud_t = 1# 初始数据 x y v_x v_ydata_init = np.mat([[0], [0], [0], [0]])# 初始协方差矩阵P_init = np.zeros([4, 4])# 状态转移矩阵A = np.mat([ [1, 0, d_t, 0], [0, 1, 0, d_t], [0, 0, 1, 0], [0, 0, 0, 1]])# 控制输出矩阵B = np.mat([ [0.5 * d_t * d_t, 0], [0, 0.5 * d_t * d_t], [d_t, 0], [0, d_t]])# 控制噪声协方差矩阵sigma_r = 0.0R = np.mat(np.eye(4) * (1 - sigma_r) + sigma_r) * 0.1# 观测状态转移矩阵C = np.eye(4)# 观测噪声协方差矩阵sigma_q = 0.0Q = np.mat(np.eye(4) * (1 - sigma_q) + sigma_q) * 0.1# 加速度大小u_l = 1data_list = [data_init]P_list = [P_init]target_pos_list = [target_simple(0)]# 生成地图画布fig, ax = plt.subplots(1, 1)plt.grid(ls='--')ax.set_xlim(-100, 500)ax.set_ylim(-100, 800)line_missile, = plt.plot(data_list[0][0],data_list[0][1], 'r-')line_fly, = plt.plot(target_pos_list[0][0],target_pos_list[0][1], 'b-')label_data = 200def fly_update(index): x_t, y_t = target_simple(index) target_pos_list.append(target_simple(index)) cur_u = get_u([x_t, y_t], data_list[index - 1][:2], u_l) x_pre = A * data_list[index - 1] + B * cur_u p_pre = A * P_list[index - 1] * A.T + R control_noise = np.mat(np.random.multivariate_normal([0, 0, 0, 0], R, 1).T) real_x = x_pre + control_noise view_noise = np.mat(np.random.multivariate_normal([0, 0, 0, 0], Q, 1).T) z_k = C * real_x + view_noise K = (p_pre * C) * ((C * p_pre * C.T + Q) ** -1) x_post = x_pre + K * (z_k - C * x_pre) P_post = (np.mat(np.eye(4))- K * C) * p_pre data_list.append(x_post) P_list.append(P_post) pos_data = np.array(data_list)[:, :2, 0] fly_data = np.array(target_pos_list)[:, :2] line_missile, = plt.plot(pos_data[:, 0],pos_data[:, 1], 'r-') line_fly, = plt.plot(fly_data[:, 0],fly_data[:, 1], 'b-') return line_missile, line_fly,def fly_init(): pos_data = np.array(data_list)[:, :2, 0] line_missile.set_data(pos_data[:, 0], pos_data[:, 1]) fly_data = np.array(target_pos_list)[:, :2] line_fly, = plt.plot(fly_data[:, 0],fly_data[:, 1], 'b-') return line_missile, line_fly,fly_anim = animation.FuncAnimation(fig=fig, func=fly_update, frames=np.arange(1, label_data), init_func=fly_init, interval=100, blit=True)fly_anim.save('vvd_missile.gif', writer='pillow', fps=10)plt.show() |
---|