卡尔曼滤波是非常经典的预测追踪算法，能够在系统存在噪声和干扰的情况下进行系统状态的最优估计，广泛使用在导航、制导、控制相关的领域。
简介
- SLAM 过程可以由运动方程和观测方程来描述，考虑一个简单的场景，在离散的时间 t=0…N 的时间内，有一个机器人“小萝卜”，他在这些时间上不断控制自己做着各种各样的动作，每个对应时刻有自己的状态变量 [x_0, …, x_N]，在过程中也会产生某种观测变量 [y_0, …,y_N]
- 在实际数据产生过程中，必定会带有误差，控制系统会有控制误差，观测会产生测量误差，如果误差符合某种分布，那么状态和观测值也是服从某种概率分布的随机变量。
- 因此，我们关心的问题就变成：当我们拥有某些运动数据和观测数据之时，如何确定状态量 x,y 的分布。
问题描述
- 考虑在 k 时刻下，“小萝卜” 的状态随机变量用 x_k 表示，这个时刻控制变量用 u_k 表示，控制噪声用 w_k 表示，此时观测到的数据记作 z_k，观测噪声为 v_k，于是可以写出运动与观测方程：
- 我们希望用过去0到k中的数据来估计现在的状态分布：
- 当我们只关注 x_k 与 z_k 时，根据贝叶斯法则：
$$ begin{array}{c} Pleft(boldsymbol{x}{k},z_k mid boldsymbol{x}{0}, boldsymbol{u}{1: k}, boldsymbol{z}{1: k-1}right) =Pleft(boldsymbol{x}{k} mid boldsymbol{x}{0}, boldsymbol{u}{1: k}, boldsymbol{z}{1: k}right)·P(z_k) = Pleft(boldsymbol{z}{k} mid boldsymbol{x}{k}right) Pleft(boldsymbol{x}{k} mid boldsymbol{x}{0}, boldsymbol{u}{1: k}, boldsymbol{z}{1: k-1}right) end{array} $$
- 由于 P(z_k) 是常量，因此有：
- 右边第一项为似然，第二项为先验。
- 还有一个假设是 x_k 是基于过去所有的状态估计得到的，那么 x_k 至少会受到 x_{k-1} 的影响，那么以 boldsymbol{x}_{k-1} 时刻为条件概率展开:
滤波器模型
- 我们假设这个状态变化具有马尔科夫性，也就是 k 时刻的状态仅与 {k-1} 时刻状态有关，上述积分式的右边第一项：
- 右边第二项：
线性高斯系统
- 我们从形式最简单的线性高斯系统开始，最后得到卡尔曼滤波器。明确了起点和终点之后，我们再来考虑中间的路线。线性高斯系统是指，运动方程和观测方程可以由线性方程来描述：
- 假设所有状态和噪声均满足高斯分布，噪声零均值：
- 利用马尔科夫性，假设我们知道了在 k-1 时刻的后验（在 k-1 时刻看来) 状态估计 hat{boldsymbol{x}}{k-1} 及其协方差 hat{boldsymbol{P}}{k-1} , 现在要根据 k 时刻的输人和观 测数据, 确定 boldsymbol{x}_{k} 的后验分布。
- 为区分推导中的先验和后验, 我们在记号上做一点区别：以上帽 子 hat{boldsymbol{x}}{k} 表示后验, 以下帽子 check{boldsymbol{x}}{k} 表示先验分布。
卡尔曼滤波
预测
- 卡尔曼滤波器的第一步为预测, 通过运动方程确定 boldsymbol{x}_{k} 的先验分布。这一步是线性的，而高斯分布的线性变换仍是高斯分布。
- 而且协方差矩阵具有性质：
- 所以有：
- 它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声）推断当前时刻的状态分布。这个分布也就是
先验。
- 显然这一步状态的不确定度要变大，因为系统中添加了噪声。
观测
- 由观测方程，我们可以计算在某个状态下应该产生怎样的观测数据，可以认为这就是
似然：
- 设得到的关于 x_k 的后验概率为：
- 根据上文的贝叶斯公式，这个
似然与先验乘积与后验相差一个常数倍
- 这里我们稍微用点讨巧的方法。既然我们已经知道等式两侧都是高斯分布，那就只需比较指数部分，无须理会高斯分布前面的因子部分。指数部分很像是一个二次型的配方，我们来推导一下。首先把指数部分展开，有：
- 为了求左侧的 hat{boldsymbol{x}}{k} 和 hat{boldsymbol{P}}{k} , 我们把两边展开, 并比较 boldsymbol{x}_{k} 的二次和一次系数。对于二次系数, 有：
- 该式给出了协方差的计算过程。为了便于后面列写式子, 定义一个中间变量:
- 左右各乘 hat{boldsymbol{P}}_{k} , 有：
- 于是有：
- 比较一次项系数：
$$ 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)两个步骤：
- 预测：
- 更新：先计算 boldsymbol{K} , 它又称为卡尔曼增益：
- 计算后验概率的分布：
实现代码
- 简单写了导弹打飞机的卡尔曼滤波控制过程：
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() |
|---|
- 实现效果：
参考资料
- https://zhuanlan.zhihu.com/p/444977764
- https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
- 视觉SLAM十四讲从理论到实践
