自动驾驶——卡尔曼滤波（KF&EKF）的学习笔记

1 致谢

感谢 Dr. Cyrill Stachniss的教学视频，向您表示敬意！
教程链接：https://www.youtube.com/watch?v=ELyQMGF_WIg

2 卡尔曼滤波的核心优点——“不需要标注”

卡尔曼滤波是一种基于假设和统计学方法的状态估计方法，不是一种学习型算法（没有任何假设，属于函数拟合方法），所以前期在验证阶段不需要大量的标注信息；

2 卡尔曼滤波在自动驾驶中的作用

2.1 多传感器的信号融合

《百度自动驾驶系列课程》里面讲到： “卡尔曼滤波可以用来进行GPS和IMU的信号之间的校准；”

2.2 帧间位置的插值（用来进行加速）

我请教赵老师关于卡尔曼滤波在人脸跟踪中的作用时，他说到：
“卡尔曼滤波可以跟踪、加速，不用每帧都检测；”

可以看到，卡尔曼滤波的插值作用可以用来在多目标跟踪中进行加速；

1.2 通过插值来进行加速

1.3 对目标框进行平滑

关于平滑的作用，张老师是这样讲的： “一般用在跟踪里边，比如某一帧
人脸没检测到，可以用Kalman滤波器预测下，也可以用来做平滑，比如框抖得很厉害，Kalman可以把高频抖动去掉”

2 引入卡尔曼滤波

小汽车是我认为KF最形象的例子了，
已知，
$状态= \begin{bmatrix} p_t\\ v_t \end{bmatrix}$

2.1 各个参数的简单介绍

这里只是引入的符号，数学严谨的符号还要参考《卡尔曼滤波组合导航原理》；
$B_t$ —— 控制矩阵；
$\boldsymbol{R}$ —— 系统噪声序列的方差阵（实际上就是“ $\boldsymbol{P}_{t}^-$的噪声”，这里的小"-"号表示这是单步估计的“不完美”的结果）；
$\boldsymbol{Q}$ —— 观测噪声方差阵（实际上就是“ $\boldsymbol{z}_t$的噪声”），（如果 $Q=0$，可以推导出“ $\mu = C^{-1} z_t$”，其直观含义就是：当观测噪声为0时，观测值是100%准确的，所以可以完全地相信观测值计算而来的状态变量，详细的推导过程可以在《SLAM-Course - 04 - Extended Kalman Filter (2013/14; Cyrill Stachniss)》视频里面观看）；

2.1.1 噪声矩阵的形象化理解

在KF中，需要设置的超参数有：“ $\boldsymbol{P}$矩阵的初值”、 $\boldsymbol{R}$矩阵和 $\boldsymbol{Q}$矩阵；
这三个矩阵分别代表了我们对三种不同噪声的高斯方差的估计，也就是“人的猜测”，如果猜得准，滤波的效果就会很好；而如果猜的不准，滤波效果则可能会不好；

2.2 卡尔曼滤波形象化理解——利用噪声矩阵进行加权滤波

卡尔曼滤波的本质是可以认为是利用了系统噪声和观测噪声来进行加权滤波的方法，
那么这里的加权是如何实现的呢？
加权计算融合了两者噪声的矩阵，通过增益方程进行计算：
$K_t = \bar{\Sigma}_t C^T_t \left ( C_t \bar{\Sigma}_t C^T_t + Q_t\right )^{-1}$
这里我们可以做一种形象化的变形：
$“K_t =\frac{\bar{\Sigma}_t C^T_t}{\left ( C_t \bar{\Sigma}_t C^T_t + Q_t\right )}”$
这样就比较好理解了，如果 $Q_t$很小，残差权值 $K_t$就会很大，表示更加相信观测值；

2.3 状态转移方程——递推估计

我们可以把状态转移方程视作一种递推估计，因为它在本质上其实与数列中“递推公式—— $a_{n+1} = f(a_n)$”的思想是相同的；
于是这种递推式的传感器信号，就可以融合在状态转移方程中，
例如：平面SLAM中的位移估计；
其实在我看来，状态方程是一种对系统“状态转移的先验递推”，融合我们已知的先验递推的知识以及控制量；

扫描二维码关注公众号，回复： 10921625 查看本文章

2.4 KF形象理解的备注

2.4.1 为什么“ $\boldsymbol{X}$”的估计噪声 $\boldsymbol{P}_{t}^-$要放在 $\boldsymbol{K}_{t}$的方程中呢？

这是因为 $\boldsymbol{P}_{t}^-$指的是单步估计带来的误差，在进行量测估计时，我们需要把这个误差“加回去”；

3 卡尔曼滤波的数学推导

3.1 变量符号的备注

用大写Sigma表示协方差；

4 卡尔曼滤波平滑的理解

Q和R矩阵可以用来控制平滑的程度；

4 EKF和KF的不同

4.1 EKF用来处理系统的状态转移变换是非线性的情况

首先，一个经常出现的情况是，系统的状态转移是非线性的；
那么，如果定义系统的“状态转移”呢？具体来说，我们指的是系统的状态转移方程，
也就是
${\bar{\mu}}_t=g\left ( u_t, {\mu}_{t-1}\right )$
于是，我们说的“系统的状态转移是非线性的”则指的是： $g\left ( u_t, {\mu}_{t-1}\right )$不再满足线性的形式—— $A_t \mu_{t-1}+ B_t u_t$的形式；
经过实验，我们可以发现，此时噪声的传递会出现较大的误差：

所以我们需要改变噪声的传递方式；

5 EKF算法的时间复杂度

Complexity: $O(k^{2.4}+ n^2)$

3 多传感器的融合

状态方程不需要进行修改；

5 Q和R的调参

可以通过真值来估计噪声的大小；

3 后记

我感觉卡尔曼滤波就是一种信号的插值算法；