卡尔曼滤波原理_【无人驾驶】卡尔曼滤波的基本原理

卡尔曼滤波器被称作最优线性滤波器，是利用线性状态方程，对观测值进行最优估计的算法，由于观测数据中包括系统中各种误差的影响，因此最优估计也被看作是滤波过程。在无人驾驶领域当中，我们需要时刻监视车辆的状态并且尽可能估计车辆下一个时刻的状态，以便采取合理的决策，而卡尔曼滤波则正是这样的一种估计算法。

本文主要介绍的是线性卡尔曼滤波器，针对非线性系统使用的扩展卡尔曼滤波及无损卡尔曼滤波会在后面的文章中更新。

首先我们假设小车从原点出发，在x轴上沿正方向做速度为5m/s的匀速直线运动，用S表示小车的位置，t-1时刻时，小车的位置为5t-5，假设此值的方差为4。我们认为小车的运动是符合运动学的，小车t时刻的位置是t-1时刻的位置加5，即5t，假设该模型的不确定度为0，则t时刻我们预测的小车位置的方差为4。

现在原点上有一台测距仪，我们从测距仪上读出的位置是5t+3，假设该值的方差是2。

当我们要估计t时刻小车的位置时，实际上是有两个值，那么我们应该如何取舍，此时就需要引入卡尔曼系数，令Kt=4/(4+2)=0.67，所以t时刻的位置为：

St=5t-5+0.67*[(5t+3)-5t]=5t-3

因为测距仪的方差较小，我们更加相信测距仪的数据，所以5t-3为此时的最优估计值，0.67则相当于测量值与经验值的权重。

在进行t+1时刻位置预测前，首先需要求出t