无人驾驶高精度定位技术(2)-卡尔曼滤波

本节我们介绍机器人定位中技术中的卡尔曼滤波(Kalman Filter)，卡尔曼滤波作为连续状态空间问题的一种解决方案，已经成功运用在火星登陆和自动导弹制导等领域。本质上卡尔曼滤波(Kalman Filter)是一置信度为正态分布的贝叶斯(Bayes Filter)滤波器，它的置信度可以表示为一个均值向量和协方差矩阵的形式，均值向量表示可能的状态，协方差矩阵表示该状态的不确定度。

前提假设

卡尔曼滤波需要满足以下三个前提假设，才能保证在任意一个时刻t的置信度bel(x(t))都是正态分布：

1) 运动模型必须是一个线性高斯模型。也就是说t+1时刻的状态必须是t时刻的状态的线性函数，附加一个高斯随机噪声。

其中A是一个矩阵，Δ是运动向量，ξ是高斯随机噪声

2) 传感器模型也是一个线性高斯模型。

其中是一个矩阵，ζ表示传感器测量的高斯噪声。

3) 初始置信度bel(x0)是一个正态分布。

一维状态空间卡尔曼滤波

先从最简单的一维卡尔曼滤波说起，假设一辆小车以δ的速度和一定的随机噪声ϵ沿着一条直线前进，则它的运动方程：

运动转移模型为：

ϕ^2是对运动不确定度的度量，是对运动噪声ϵ的一种反应。

传感器模型为：

ψ^2是对传感器测量噪声不确定度的度量。

假设t时刻的置信度为：

则t+1时刻的预测置信度为:

置信度更新后：

其中：

可以看到传感器模型更新后的正态分布的均值为传感器测量的均值与旧的运动预测的均值的加权平均，权重为各自的不确定度。

一维卡尔曼滤波算法

其中变量k被成为卡尔曼增益，它可以用来衡量传感器测量值的重要程度。预测置信度越低，卡尔曼增益就越小，新的测量值在最终的结果中被采纳的比例就越大，反之，传感器测量值在最终结果中被采纳的比例就越小。

多维卡尔曼滤波算法

卡尔曼滤波的适用性

针对于线性高斯系统，卡尔曼滤波高效、简单、易行，然而遗憾的是在实际问题中，这样的理想的场景非常少。于是又有了扩展卡尔曼滤波，扩展卡尔曼滤波的思想是非线性系统在均值的一个小的邻域内可以近似认为是线性的，从而可以得到一个近似的高斯置信度。