线性卡尔曼滤波详解
Posted Alex_996
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了线性卡尔曼滤波详解相关的知识，希望对你有一定的参考价值。
自动驾驶面临的第一个问题：从哪里来，到哪里去？要解决这个问题，自动驾驶汽车首先需要准确的知道自己在地图上的位置。理所当然的我们可以想到通过GPS来进行定位，但获取GPS信号需要跟卫星进行通信，这就导致它的更新频率比较低，每次获取的位置是不连续的。换一个思路，我们高中都学过物理，当知道了一个小车的起点、速度和加速度之后，就可以通过直线运动公式预测接下来的位置，再结合小车偏向的角度、角速度和角加速度，完全可以通过运动学模型预测小车在二维道路上接下来的位置。此时我们有了两种定位的方式：直接通过GPS观测和间接通过运动模型预测，那该选择哪一种呢？小孩子才做选择，成年人表示我都要，这也是本节要介绍的——卡尔曼滤波。
介绍
卡尔曼滤波的应用非常广泛，尤其是在导航当中，而它的广泛应用主要是因为我们实际生活的世界中存在着大量的不确定性，当我们去描述一个系统的时候，不确定性主要体现在三个方面：① 不存在完美的数学模型，② 系统的扰动往往不可控也很难建模，③ 测量传感器本身存在误差。
最小例子
让我们先从一个简单的例子开始。假如有一辆小车从原点出发，原点处有一个光学测距仪器，每一时刻测量光从原点传播到小车所需的时间，可以据此计算出小车与原点之间的距离。小车上还装了加速度传感器，为了简化任务，该传感器只会输出 x x x方向上的加速度值，每隔 Δ t \\Delta t Δt时间输出小车在 t t t时刻的瞬时加速度。我们的最终目标是希望能够尽可能准确的估计小车在每一时刻的速度 v t v_t vt和距离 d t d_t dt。
理想情况下，我们当让希望加速度计的测量是完美的，那么直接对加速度做积分就能得到速度，对速度再做一次积分就得到距离。但是在实际应用中，无论什么样的传感器，它的输出都是带有噪声的，直接使用带有噪声的数据做计算会导致累计误差越来越大。
简化一下任务，假如我们现在只想要小车与原点的距离，并且只考虑光学测距仪，再假如小车在某一个位置保持静止，那么为了得到更加准确的距离，我们可以多次测距取平均值，或者用不同的光学测距仪进行测距。我们用 z k z_k zk来表示测量的结果，下标 k k k就表示第 k k k次的测量结果。
如果我们采用同一仪器多次测量的方案，此时如果要估计真实结果，很自然的就可以想到取平均值，即 z ^ k = 1 k ( z 1 + z 2 + . . . + z k ) \\hatz_k = \\frac1k(z_1+z_2+...+z_k) z^k=k1(z1+z2+...+zk)，这里的 ^ \\hat ^表示这是 z k z_k zk的估计值，即期望。我们对 z ^ k \\hatz_k z^k的公式做一下变形： z ^ k = k − 1 k 1 k − 1 ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k = k − 1 k z ^ k − 1 + 1 k z k = z ^ k − 1 + 1 k ( z k − z ^ k − 1 ) \\hatz_k =\\frack-1k\\frac1k-1(z_1+z_2+...+z_k-1) + \\frac1kz_k =\\frack-1k\\hatz_k-1 + \\frac1kz_k =\\hatz_k-1+\\frac1k(z_k - \\hatz_k-1) z^k=kk−1k−11(z1+z2+...+zk−1)+k1zk=kk−1z^k−1+k1zk=z^k−1+k1(zk−z^k−1)，即 k k k时刻的估计值 z ^ k \\hatz_k z^k=上一次的估计值 z ^ k − 1 \\hatz_k-1 z^k−1+系数 1 k \\frac1k k1×（当前测量值 z k z_k zk-上一次估计值 z ^ k − 1 \\hatz_k-1 z^k−1）。
分析这个公式可以得出以下结论：
- 当 k → + ∞ k \\to +\\infty k→+∞时， 1 k → 0 \\frac1k \\to 0 k1→0，此时 z ^ k → z ^ k − 1 \\hatz_k \\to \\hatz_k-1 z^k→z^k−1，也就是说，随着测量次数的增加的，新的测量结果就变得不再重要了，大量测试数据估计的结果就已经比较准确了，所以再增加测量值就没有太大意义了。
- 当 k k k还比较小的时候， 1 k \\frac1k k1相对来说还比较大，此时测量的结果就非常有意义了，尤其是测量值和估计值差距比较大的时候。
- 第 k k k次的估计值 z ^ k \\hatz_k z^k与第 k − 1 k-1 k−1次的估计值 z ^ k − 1 \\hatz_k-1 z^k−1有关，即为递归形式，因此不需要追溯很久以前的数据，只需要此次的测量值和上一次的测量值就可以。
我们将上面的公式提炼一下，令系数为 K \\mathbfK K，则 z ^ k = z ^ k − 1 + K ( z k − z ^ k − 1 ) \\hatz_k = \\hatz_k-1 + \\mathbfK(z_k - \\hatz_k-1) z^k=z^k−1+K(zk−z^k−1)，这里的 K \\mathbfK K就是后面要推导的卡尔曼增益。为了初步理解卡尔曼增益，我们引入两个参数：
- 估计误差
e
e
s
t
i
以上是关于线性卡尔曼滤波详解的主要内容，如果未能解决你的问题，请参考以下文章