卡尔曼滤波对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 时刻之间的时间差为​

。根据物理知识，有：