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

1 致谢

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

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

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

3 卡尔曼滤波的优势

3.1 不需要训练（基于贝叶斯理论）

如果是使用LSTM来综合时序信息的话，还需要标注信息以及训练；

3.2 速度快：0.1~0.3ms

赵老师在课程中说到：
“卡尔曼滤波的效率一般在0.3ms以下，如果是电脑比较好的情况下，可以达到0.1ms以下； ”

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

4.1 滤波操作——时序信息融合（滤波作用）

卡尔曼滤波有滤波的作用，实际上是一种时序信息的融合；

2.1 多传感器的信号融合

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

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

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

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

1.2 通过插值来进行加速

1.3 对目标框进行平滑

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

5 卡尔曼滤波的公式

小汽车是我认为KF最形象的例子了，
已知，
$$状态=\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]$$

5.1 各个参数的简单介绍

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

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

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

5.4 观测方程——全局估计

5.5 加权平均

5.5.2 加权平均的时候需要在残差上面加上噪声呢？

是不需要的；在KF中，我们对状态变量的描述是通过均值和方差两个统计参数来进行描述的，也就是通过状态变量的分布进行整体估计，于是噪声部分的估计则有方差来 “负责”，于是不需要在均值中进行噪声的相关操作，所以不用在均值变量$x_{k+1}$上面加上噪声；

5.6 卡尔曼滤波形象化理解

5.6.1 利用噪声矩阵进行线性插值

卡尔曼滤波的本质是可以认为是线性插值，
这里的插值系数矩阵$K$利用了系统噪声和观测噪声来进行加权滤波，
那么这里的加权是如何实现的呢？
加权计算融合了两者噪声的矩阵，通过增益方程进行计算：
$$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)}”$$
（实际上这里的$C_t$在推导过程中被消掉了，实际上的插值系数方程是这样的，
$$“K_t=\frac{{\overline{C_t\Sigma }}_t{C}_t^T}{\left(C_t{\bar{\Sigma }}_t{C}_t^T+Q_t\right)}”$$）
这样就比较好理解了$C_t{\bar{\Sigma }}_t{C}_t^T$和$Q_t$的值获取插值系统矩阵，
如果$Q_t$很小，残差权值$K_t$就会很大，表示更加相信观测值；

5.6.2 递推估计与观测估计的融合——“高斯图的形象理解”

可以参考极市平台的博文《一文图解卡尔曼滤波(Kalman Filter)》

5.6.2 为什么“$\mathbf{X}$”的估计噪声${\mathbf{P}}_t^{-}$要放在${\mathbf{K}}_t$的方程中呢？

这是因为${\mathbf{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)$

6 多传感器的融合

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

7 KF的实现

使用OpenCV中的KF的函数；

7.1 转移矩阵的写作——“看作是多元方程组”

转移矩阵看作是方程组就会比较好写了；

8 Q和R的调参

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