卡尔曼滤波对gps轨迹数据清洗_卡尔曼滤波：从入门到精通

最早接触卡尔曼滤波是在卫星导航课中，GPS 和IMU 结合时常会用到卡尔曼滤波。但学完了也只明白了数学推导，不过是“会做题的机器”。最近在学习SLAM 时想要重新好好温习一下卡尔曼滤波，虽然现在SLAM 的主流趋势是利用图优化，但卡尔曼滤波仍然为我们提供了一个很好的参考。

导论

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

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

本文虽然是小白教程，但还是需要各位至少知道高斯分布，一点点线性代数，还有状态向量这样的名词。

简述

考虑一个SLAM 问题，它由一个运动方程：

和一个观测方程组成：

就把它当作一个线性系统吧（非线性系统请看下一讲扩展卡尔曼滤波），并且为了简化推导，忽略路标的下标j，并把路标y 并入到状态向量一起优化，那么运动方程就可以写为：

其中，

为t 时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息；
为运动测量值，如加速度，转向等等；
为状态转换方程，将t-1 时刻的状态转换至t 时刻的状态；
是控制输入矩阵，将运动测量值

的作用映射到状态向量上；
是预测的高斯噪声，其均值为0，协方差矩阵为

。

这一步在卡尔曼滤波中也称为预测 (predict)。

类似地，测量方程可以写为：

其中，

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

。

而卡尔曼滤波就是预测 - 测量之间不断循环迭代。当然，对于某些情况，如GPS + IMU，由于IMU 测量频率远比GPS 高，在只有IMU 测量值时，只执行运动更新，在有GPS 测量值时再进行测量更新。

一个小例子

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

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

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

。

假设t 和t-1 时刻之间的时间差为

。根据物理知识，有：

写成矩阵形式就有

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

上式就写为

表示t-1 时刻卡尔曼滤波的状态估计；

则表示中t-1 到t 时刻，预测更新所得的预测值。

再利用运动模型对状态向量进行更新后，还要继续更新状态向量的协方差矩阵P，公式为：

假设

为t 时刻下状态向量的真值（自然是永远未知的），由之前的现形运动方程（式(3)）给出，将式(3) 与式(9) 相减可得：

而

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

，上式就可以简化为

推导完毕。

可以看到，经过预测更新，协方差矩阵P 变大了。这是因为状态转换并不完美，而且运动测量值含有噪声，具有较大的不确定性。

预测更新实际上相当于“加法”：将当前状态转换到下一时刻（并增加一定不确定性），再把外界的干扰（运动测量值）叠加上去（又增加了一点不确定性）。

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

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

其中，

为卡尔曼增益。

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

推导

一维case

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

为了与前面的通用的推导区别开来，在这个一维的例子中我们使用了新的符号。不过熟悉高斯概率分布的话应该可以马上看出来，

为这个高斯分布的均值，

为方差，而r 为小车的可能位置，

为某个可能位置 (r) 的概率分布。

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

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

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

将上式化简一下：

其中，

为

和

的加权平均，

则是

和

的调和平均的二分一：

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

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

那么，怎么由上面两个简单的一维的式子得到前一节

和

呢？一步一步来。

转换矩阵H 的引入

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

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

此时，就该转换矩阵

闪亮登场了。由于

，c 为光速。所以此时

（测量方程为

，可以回去参考一下式(4)）。

预测值就要写为：

而测量值保持不变：

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

和

的公式（式(27) 和式(28)），可得：

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

由于

（这里转换矩阵H 不随时间变化而变化，所以把下标t 略去），并记

，则上式可以写为：

类似的有，

两边乘以

有：

推广至高维

到了这一步，这个一维情况下卡尔曼滤波的测量更新步骤就已经彻底讲完了。剩下的就是将这个一维例子推广至高维空间中。其实大家仔细观察一下就会得到答案。

就是

，是测量更新后所得的状态向量；
就是

，是t-1 时刻到t 时刻的小车的预测更新（或叫运动更新）后的状态向量；
是

，是测量值；
在高维空间中为

，为测量更新后，状态向量的协方差矩阵；
在高维空间中就成了协方差矩阵

，是预测更新后状态向量的协方差矩阵；
是

，是测量值的协方差矩阵；
H 就是
了，高维空间中的转换矩阵。
而最重要的，卡尔曼增益
，在高维空间中就可以写为

。看着很复杂，但仔细对照的话就是把前面相迎的数据替换带入即可。

最后，根据式(33)

就可得：

根据

就可得：

小结一下

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

讨论

至此，相信你已经明白了卡尔曼滤波的推导过程。而具体的问题就取决于你的建模了。如在上面的小车的例子中，各个参数如下：

假设该时刻下运动测量值为加速度

，为均值为0标准差为

的高斯分布（标准差可以为经验值或仪器精度。

，

对于测量值，同样可以假设

，那么

多问个为什么

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

但推完卡尔曼滤波，我还有几个个为什么。知其然更要知其所以然。下面是我对于自己的疑惑学习、思考得到的解答，而且碍于表达能力，不敢说百分百正确。

首先是对于预测更新。前面也说到了，预测更新相当于“加法”。这相对好理解一些。在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时刻下，小车运动后在其他位置上的概率分布也可以用全概率公式表达出来。当然，最后的计算结果还需要进行归一化处理。

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

至此，关于卡尔曼滤波的个人浅见就到此为止了。精通还需要不断地实践，但希望读完本文，能让你对卡尔曼滤波有全面的了解。

有帮助的话，帮忙点个赞呗。

一些参考文献：

Ramsey Faragher 的 understanding the basis of the kalman filter.

维基百科 Kalman filter

高翔的《视觉SLAM十四讲》