淦ORB-SLAM2源码 11--卡尔曼滤波

卡尔曼滤波这个在看高博的《视觉SLAM十四讲》就已经接触到了，但是当时并不理解，只是囫囵吞枣的记了一下过程：

1. 先是预测，根据上一个状态预测当前状态
2. 更新卡尔曼增益
3. 最后计算后验概率分布

最近又详细的了解了一下具体的过程和原理，虽然现在SLAM 的主流趋势是利用图优化，但卡尔曼滤波仍然为我们提供了一个很好的参考，也算是为解决问题提供一个方法吧。

卡尔曼滤波（KF）

卡尔曼滤波本质上是一个数据融合算法，将具有同样测量目的、来自不同传感器、(可能) 具有不同单位 (unit) 的数据融合在一起，得到一个更精确的目的测量值。

卡尔曼滤波的局限性在于其只能拟合线性高斯系统。但其最大的优点在于计算量小，能够利用前一时刻的状态（和可能的测量值）来得到当前时刻下的状态的最优估计。

简述

在SLAM系统中，运动方程表示为：

观测方程表示为：

假设这里是一个线性系统（当然不是）并且为了简化推导，忽略路标的下标j，并把路标y 并入到状态向量一起优化，那么运动方程就可以写为：

其中，

Xt为t 时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息；
​Ut为运动测量值，如加速度，转向等等；
​Ft 为状态转换方程，将t-1 时刻的状态转换至t 时刻的状态；
Bt 是控制输入矩阵，将运动测量值​ Ut的作用映射到状态向量上；
Wt是预测的高斯噪声，其均值为0，协方差矩阵为​ Qt 。

这一步就是前面说的，在卡尔曼滤波中被称为预测 (predict)。
测量方程也可以同样的表示为：

其中，

Zt 为传感器的测量值；
Ht为转换矩阵，它将状态向量映射到测量值所在的空间中；
Vt为测量的高斯噪声，其均值为0，协方差矩阵为Rt。

而卡尔曼滤波就是预测 - 测量之间不断循环迭代.

例子

这里引用别人举的一个例子，我也是看完这个例子和推到，才有了恍然大悟的感觉。
例子和公式推导都参考自: 卡尔曼滤波：从入门到精通.

用一个在解释卡尔曼滤波时最常用的一维例子：小车追踪。如下图所示

状态向量为小车的位置和速度：

而司机要是踩了刹车或者油门，小车就会具有一个加速度

假设t 和t-1 时刻之间的时间差为三角形△t​ 。根据物理知识，有

写成矩阵形式就有

跟之前的运动方程对比，就知道

上式就写为

Xt-1 表示t-1 时刻卡尔曼滤波的状态估计； Xt|t-1 ​ 则表示中t-1 到t 时刻，预测更新所得的预测值。
而

考虑到状态向量和噪声是不相关的，则

上式就可以简化为

推导完毕。
可以看到，经过预测更新，协方差矩阵P 变大了。这是因为状态转换并不完美，而且运动测量值含有噪声，具有较大的不确定性。
预测更新实际上相当于“加法”：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。

上面即为卡尔曼滤波中预测这一步。这一步相对比较直观，推导也较测量更新简单，就只在这里详细给出了。

如果得到了测量值，那么我们就可以对状态向量进行测量更新了，对应的公式为

其中，

这里就是上面说的卡尔曼增益。

从这里就可以看到，测量更新显然比预测更新复杂，难点也集中在这里。下面就给出测量更性的详细推导。

推导

一维case

从t-1 时刻起，小车运动后，经过前面所述的预测更新后，我们就得到了t 时刻的小车位置的估计，由于在卡尔曼滤波中，我们使用高斯概率分布来表示小车的位置，因此这个预测的位置可以写为：

为了与前面的通用的推导区别开来，在这个一维的例子中我们使用了新的符号。不过熟悉高斯概率分布的话应该可以马上看出来， 是μ1为这个高斯分布的均值，​ 是σ1 为方差，而r 为小车的可能位置， y1 为某个可能位置 ® 的概率分布。

假设在t 时刻，我们通过某测距仪测得小车距离原点的距离r，由于测量包含噪声（且在面前我们假设了其为高斯噪声），因此该测量值也可以利用高斯概率分布来表示：

除了下标外，其余的字母的含义都和上面的式子一样

如上图琐事，现在在t 时刻，我们有了两个关于小车位置的估计。而我们所能得到的关于小测位置的最佳估计就是将预测更新和测量更新所得的数据融合起来，得到一个新的估计。而这个融合，就是一个简单的“乘法”，并利用了一个性质：两个高斯分布的乘积仍然是高斯分布。

将上式化简一下：

其中：


最右边的式子是为了后面的计算而准备的。

本质上，这（高斯分布相乘）就是卡尔曼滤波中测量更新的全部了。

一维的例子看完了，那怎么通过这个得到前面的X和P呢？

转换矩阵H 的引入

在刚刚的一维情况的小例子中，我们其实做了一个隐式的假设，即有预测更新得到的位置的概率分布和测距仪所得的测量值具有相同的单位 (unit)，如米 (m)。

但实际情况往往不是这样的，比如，测距仪给出的可能不是距离，而是信号的飞行时间（由仪器至小车的光的传播时间），单位为秒 (s)。这样的话，我们就无法直接如上面一般直接将两个高斯分布相乘了。
此时，就该转换矩阵 H 闪亮登场了。由于:

所以此时:

预测值就要写为：

而测量值保持不变：

这样，两个高斯概率分布在转换矩阵H 的作用下又在同一个空间下了。根据前面的公式可得：

将上式两端都乘以c 则可得：

由于H=1/C，记

则上式可以写为：

同理：

两边乘以​ C^2 有：

到了这一步，这个一维情况下卡尔曼滤波的测量更新步骤就已经彻底讲完了。

推广至高维

将这个一维例子推广至高维空间中。其实大家仔细观察一下就会得到答案。

最后，根据

可得：

根据​
可得

小结一下

通过这个一维情况的推导，希望能说明卡尔曼滤波就是在给定初始值的情况下，由预测和测量不断迭代、更新状态向量。而预测就是一个“加法”：状态转换和运动预测叠加；测量则是简单的高斯分布相乘，中间引入了一个转换矩阵将测量值和状态向量映射在同一个代数空间中。

卡尔曼滤波的推导和结果，到这里就可以停止啦。

讨论

那么为什么测量更新就是乘法而非加法呢？

首先是对于预测更新。前面也说到了，预测更新相当于“加法”。这相对好理解一些。在t-1 时刻我们有了对于小车位置的一个估计，根据对小车速度（状态向量之一）、小车的加速度（运动测量值）的建模，在辅以时间间隔，自然可以计算出小车在该时间间隔内的位移和速度增量，再将之叠加到原有的状态向量上即可。由于建模和测量的过程带有噪声，所以此时小车的位置估计的精度是下降的（方差增大）。

这里就要涉及另一个知识点了在状态估计和机器学习中经常出现的贝叶斯法则。
在有了测量值之后，我们求小车位置的概率分布其实就是在求​

根据贝叶斯法则有：

上式得到的是后验概率。直接求后验概率比较困难（为什么？）。假设就在这个一维的小车例子中，当我们得到一个距离测量值z，那么小车的位置可能是距离原点的-z 或z 的两个点上。对于二维就可能是一个圆，三维则是一个球。此时要精确地知道小车的位置（消除歧义点），一则我们可以继续测量，二则需要额外的信息。这就使得求后验概率比较费时费力。

反观贝叶斯法则的右侧，此时我们已经有了先验概率 [公式] ​，这是上一时刻的状态向量的概率分布，并且我们也有了

因为所得的测量值 ：

达的就是在当前位置下，我们能得到的测量值，亦即贝叶斯中的似然。在分母为常数的情况下，最大化这个乘积就是最优的后验概率。

既然测量更新是以贝叶斯公式为基础，那么反观预测更新，除了前面那个直观的“加法”解释之外，是不是也有一个概率上的解释呢？

连续的高斯分布所表示的小车位置的预测更新我没找到（不好意思），但就离散情况的话还是有的，就是全概率公式。以下图为例，假设t-1 时刻，小车的位置分布概率如图所示，到了t 时刻，预测小车向前运动了3米（3个格子），但由于模型的不确定性和噪音，我们不能保证小车精确地向前走了3米，根据概率分布，我们可以假设小车有80%的概率往前走了3米，10%的概率往前走了两米，而另有10%的概率往前走了4米。

那么，在t 时刻，若小车真的运动到了这个位置，其概率分布是怎样的呢？它既有可能是在距离该位置3米远的地方以0.8的概率运动到现在这个位置的，也有可能是以0.1 的概率从2或4米远的地方为初始位置运动到这的，根据全概率公式，可以表达为

类似地，t时刻下，小车运动后在其他位置上的概率分布也可以用全概率公式表达出来。当然，最后的计算结果还需要进行归一化处理。

如果我们不断地减小每个方格的分辨率，并按照高斯分布给予每个方格一个概率值，并对小车运动也做如此的离散化处理，应该也是可以不断逼近连续的情况（个人猜想）。

扩展卡尔曼滤波（EKF）

扩展卡尔尼滤波其实就是做了一阶泰勒展开。

假设状态转移概率和测量概率分别由非线性函数g和h控制，而不再是一个线性变换：

这种情况下，由于线性变换的关系不在了，因此概率分布也不再是高斯分布。整个系统不再有闭式解，这是最让人头疼的。
而EKF 的主要思想就是线性化：通过一个在高斯函数的均值处与非线性函数g相切的线性函数来近似g。

线性化的主要优点就是效率，一旦对g和h进行了线性化，KEF和KF就是等效的。
EKF采用一阶泰勒展开的方式来进行线性化，其根据g的值和斜率构造一个函数g的线性近似函数：

线性点的选择依据的是在线性化点附近自变量最有可能的状态。对于高斯函数，最可能的状态自然就是后验的均值 ：

同理，将测量函数h线性化，有：

最后，整个EKF算法的流程如下：

1.运动更新：

2. 测量更新

EKF 能否成功应用取决于两个因素：

1. 被近似的函数的局部非线性化程度；
2. 概率分布自身的不确定度（协方差）。
   
   
   上两图就明确展示了非线性函数在近似点非线性程度越高、概率分布本身越不确定，所得到的近似结果就越差。此时，采用扩展卡尔曼滤波的效果往往很差，甚至会导致发散。