卡尔曼滤波算法原理

1 公式

预测：
${\hat{X}}_k=A{\hat{X}}_{k-1}+B_k\overrightarrow{u_k}$

$P_k=A{P}_{k-1}{A}^T+Q$

修正：
$K_k^{\prime }=P_k{H}^T(H{P}_k{H}^T+R{)}^{-1}$

${\hat{X}}_k^{\prime }=\widehat{X_k}+K_k^{\prime }(z_k-H_k{\hat{X}}_k)$

$P_k^{\prime }=P_k-K_k^{\prime }H{P}_k$

2 建模

假设一辆汽车在直路上行驶，车内可以通过 GPS 定位获取自己的位置 p（$\pm 10m$)，也可以获取车速 v（$\pm 1m/h$），同时车里的人会随机加速或减速，也能获得加速度 a（$\pm 1m/s^2$），我们的目的是通过对这些测量值的滤波让位置 p 的误差控制在 $\pm 1m$ 内。（模型中的测量误差均成正太分布）

我们就用 kalman 的原理逐步推导，直观的了解其数学模型。首先，算法中最关键的其实是概率模型，也就是高斯分布（正太分布）：$N(x,\mu ,{\sigma }^2)=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$， kalman filter 最终的估计值也就是一个概率分布，只是给外面使用的是其中的均值。在这个模型当中，我们假设估计值 $\hat{X}=\left[\begin{array}{c}p\\ v\end{array}\right]$ （高斯分布的均值 $\mu$），对应的估计协方差为 $P=\left[\begin{array}{ccc}{\Sigma }_{pp}& & {\Sigma }_{pv}\\ {\Sigma }_{vp}& & {\Sigma }_{vv}\end{array}\right]$ （高斯分布的方差 ${\sigma }^2$）。

注意：$\sigma$ 是标准差，$\Sigma$ 是方差，$\Sigma ={\sigma }^2$。

预测

预测取决于实际的物理模型， 这里用到的就是运动学方程：

$p_k=p_{k-1}+\Delta t{v}_{k-1}$

$v_k=v_{k-1}$

即：

$\begin{array}{rl}{\hat{X}}_k& =\left[\begin{array}{ccc}1& & \Delta t\\ 0& & 1\end{array}\right]{\hat{X}}_{k-1}\\ & =A{\hat{X}}_{k-1}\end{array}$

所以这里的 $A$ 就是物理模型矩阵，用于预测出下一时刻的估计值。同样，协方差 $P$ 也要经过物理模型的转换，更新协方差的公式如下：

$Cov(x)=\Sigma$

$Cov(Ax)=A\Sigma A^T$

所以预测方程：

${\hat{X}}_k=A_k{\hat{X}}_{k-1}$

$P_k=A{P}_{k-1}{A}^T$

外部控制

上面的预测是在匀速的情况下，没有外部干扰，如果踩油门或刹车带来了加速减速，那么会引入加速度 $a$，同样可以根据运动学方程来描述：
$p_k=p_{k-1}+\Delta t{v}_{k-1}+\frac{1}{2}a\Delta t^2$

$v_k=v_{k-1}+a\Delta t$

即：

$\begin{array}{rl}{\hat{X}}_k& =A_k{\hat{X}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a\\ & =A_k\widehat{X_{k-1}}+B_k{u}_k\end{array}$

这里的 $B_k$ 就是外部控制矩阵，描述了外部控制量与估计值的物理关系，$u_k$ 就是外部控制量。同时，外部控制量带来的影响也是呈高斯分布，其协方差为 $Q$，需要叠加到已预测的协方差当中，所以最终的预测方程组为：
${\hat{X}}_k=A{\hat{X}}_{k-1}+B_k{u}_k$

$P_k=A{P}_{k-1}{A}^T+Q$

修正

完成预测之后，我们需要通过传感器的测量值对其进行校正，由于预测值和测量值都是一个估计，都存在自己的 $\mu$ 和 ${\sigma }^2$，修正这个环节就是融合两个估计值，得到新的 ${\mu }^{\prime }$ 和 ${\sigma }^{\prime 2}$，即滤波器的最优估计：${\hat{X}}_x^{\prime }$、$P_k^{\prime }$。如何融合两个高斯分布？方法很简单，直接把他们相乘，做归一化（概率和为1）。

先使用一维高斯分布来分析，假设两个分布：$N(x,{\mu }_0,{\sigma }_0^2)$ 和 $N(x,{\mu }_1,{\sigma }_1^2)$，使：

$N(x,{\mu }_0,{\sigma }_0^2)\cdot N(x,{\mu }_1,{\sigma }_1^2)=N(x,{\mu }^{\prime },{\sigma }^{\prime 2})$

将 $N(x,\mu ,{\sigma }^2)=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$ 代入上式且做归一化，得到：

${\mu }^{\prime }={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}$

${\sigma }^{\prime 2}={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}$

把上式相同的部分提取出来，得到：

$k=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}$

${\mu }^{\prime }={\mu }_0+k({\mu }_1-{\mu }_0)$

${\sigma }^{\prime 2}={\sigma }_0^2-k{\sigma }_0^2$

好，得到这个关系式基本就快结束了，我们再看需要融合的两个估计：

• 预测估计：$N(x,H{\hat{X}}_k,H{P}_k{H}^T)$，即 ${\mu }_0=H_k{\hat{X}}_k$ ， ${\sigma }_0^2=H{P}_k{H}^T$
• 测量估计：$N(x,z_k,R)$，即 ${\mu }_1=z_k$ ， ${\sigma }_1^2=R$

在上式中，你可能存在疑问，为什么预测估计中要加入 $H$ 转换矩阵？其实我们做融合的前提是两个估计处于同一尺度，例如一个估计是预测的路程，一个估计是测量的耗油量，它们在数值没有直接的关联，融合之后也不存在意义，所以要把路程换算成耗油量再去做融合才有意义，融合之后的值就是耗油量的最优估计，这里的转换就是 $H$，称之为测量转换矩阵，把预测值的尺度转换为测量值的尺度。你可能还会有疑问，我们建模中的预测和测量是同一尺度啊？是的，同一尺度给 $H$ 赋值为 1 就可以了。

我们把两个估计代入高斯融合的关系式，得到：

$K=\frac{H{P}_k{H}^T}{H{P}_k{H}^T+R}$

$H{\hat{X}}_k^{\prime }=H{\hat{X}}_k+K(z_k-H{\hat{X}}_k)$

$H{P}_k^{\prime }{H}^T=H{P}_k{H}^T-KH{P}_k{H}^T$

化简上面三个式子，等式两边同时左乘 $H^T$：
$H^{T}K=\frac{P_k{H}^T}{H{P}_k{H}^T+R}$

${\hat{X}}_k^{\prime }={\hat{X}}_k+H^{T}K(z_k-H{\hat{X}}_k)$

$P_k^{\prime }=P_k-H^{T}KH{P}_k$

使 $K_k^{\prime }=H^{T}K$， 所以最终的修正公式为：

$K_k^{\prime }=P_k{H}^T(H{P}_k{H}^T+R{)}^{-1}$

${\hat{X}}_k^{\prime }=\widehat{X_k}+K_k^{\prime }(z_k-H_k{\hat{X}}_k)$

$P_k^{\prime }=P_k-K_k^{\prime }H{P}_k$

大功告成！