卡尔曼滤波器在车辆跟踪预测的应用（上）

状态预测

卡尔曼滤波首先考虑的问题是目标状态，这也叫系统状态。以小车在高速公路上行驶为例，小车的状态可以用位置和速度来表示。用数学符号表示即,其中表示位置，表示速度。

小车在行驶，既可以做匀速直线运动，也可以做加速运动。假定表示小车的加速度，那么小车进行匀速运动时,。时刻，小车的位移和速度可以分别表示为：

因为当前时刻的位置和速度分别是上一时刻的线性组合，因此可以用矩阵的形式来描述上述两个公式， 即:

令,,那么上面公式就转换为：

划重点，此公式也就是卡尔曼滤波器的第一个公式，叫状态预测公式。 就是系统状态转移矩阵，表示上一个时刻状态怎么变换到下一个时刻的状态，是控制矩阵，表示控制量是怎么作用于当前状态。就是我们的预测值。细心的可以发现，这里的带有帽子，没有写错，这说明了这里的是系统状态的估计值，这个值是通过上一个时刻的状态（卡尔曼滤波修正后的状态值）计算估计出来的。既然是估计值，那么就存在噪声，我们用协方差矩阵表示当前估计值和真实值相比，带来多大的不确定性，那么就有卡尔曼滤波的第二个公式,也就是不确定性在各个时刻的传递公式：

表示协方差矩阵,是状态转移矩阵，考虑在估计下一时刻的状态，本身存在的噪声，这个噪声用来表示。这样下一时刻的协方差矩阵的估计值，就得到了。综上，卡尔曼滤波器的状态预测两个公式就说明清楚了。

观测矩阵

　　小车在行驶，那么这个观察矩阵是什么东西。这个跟实际的需求有关，可以是位置，可以是速度，也可以是速度和位置等。在这个例子中以小车的位置为观察量，用数学符号表示。因为预测值和观测值之间是线性关系（卡尔曼滤波器处理线性问题），那么转换关系就可以用矩阵表示，叫观测矩阵。需要考虑的是估计值和观测值的维度不总是相同，那么观测矩阵是根据实际情况构造的。

　　小车行驶例子,估计值，,那么,,据此，得到卡尔曼滤波的第三公式：

。

表示测量噪声，因为测量值不是１００％准确，即实际的测量值跟你测出来的测量值还是存在误差，那么用表示这个误差，专业术语就是观测噪声的协方差矩阵。小车例子观测值是一个标量，那么就只代表方差了。

状态更新

　　有了小车状态的估计值和测量值，就可以用卡尔曼滤波器来修正测量值了。

      

    

     其中是测量值和估计值之间的残差，是卡尔曼系数，用来衡量预测状态协方差和观测状态协方差的大小进而相信预测模型多一点还是观察模型多一点。相信预测模型多一点，则残差的权重小一点，反之，残差的权重大一点。

噪声协方差矩阵更新：

　卡尔曼滤波器的最后一个公式就是对最佳估计值的噪声分布的更新了：

　

以上，卡尔曼滤波器的所有的公式和参数就解释完啦，下集将会给出自己的实战例子。