最早接触卡尔曼滤波是在卫星导航课中,GPS 和IMU 结合时常会用到卡尔曼滤波。但学完了也只明白了数学推导,不过是“会做题的机器”。最近在学习SLAM 时想要重新好好温习一下卡尔曼滤波,虽然现在SLAM 的主流趋势是利用图优化,但卡尔曼滤波仍然为我们提供了一个很好的参考。
导论
卡尔曼滤波本质上是一个数据融合算法,将具有同样测量目的、来自不同传感器、(可能) 具有不同单位 (unit) 的数据融合在一起,得到一个更精确的目的测量值。
卡尔曼滤波的局限性在于其只能拟合线性高斯系统。但其最大的优点在于计算量小,能够利用前一时刻的状态(和可能的测量值)来得到当前时刻下的状态的最优估计。
本文虽然是小白教程,但还是需要各位至少知道高斯分布,一点点线性代数,还有状态向量这样的名词。
简述
考虑一个SLAM 问题,它由一个运动方程:
和一个观测方程组成:
就把它当作一个线性系统吧(非线性系统请看下一讲扩展卡尔曼滤波),并且为了简化推导,忽略路标的下标j,并把路标y 并入到状态向量一起优化,那么运动方程就可以写为:
其中,
-
为t 时刻的状态向量,包括了相机位姿、路标坐标等信息,也可能有速度、朝向等信息;
-
为运动测量值,如加速度,转向等等;
-
为状态转换方程,将t-1 时刻的状态转换至t 时刻的状态;
-
是控制输入矩阵,将运动测量值的作用映射到状态向量上;
-
是预测的高斯噪声,其均值为0,协方差矩阵为。
这一步在卡尔曼滤波中也称为预测 (predict)。
类似地,测量方程可以写为:
其中,
-
为传感器的测量值;
-
为转换矩阵,它将状态向量映射到测量值所在的空间中;
-
为测量的高斯噪声,其均值为0,协方差矩阵为。
而卡尔曼滤波就是预测 - 测量之间不断循环迭代。当然,对于某些情况,如GPS + IMU,由于IMU 测量频率远比GPS 高,在只有IMU 测量值时,只执行运动更新,在有GPS 测量值时再进行测量更新。
一个小例子
用一个在解释卡尔曼滤波时最常用的一维例子:小车追踪。如下图所示:
状态向量为小车的位置和速度:
而司机要是踩了刹车或者油门,小车就会具有一个加速度,
假设t 和t-1 时刻之间的时间差为