卡尔曼滤波算法原理

思想：

线性、递推、最优估计、最小均方误差

特点：

依赖于系统模型，跟踪机动目标时，滤波器易发散

系统模型遵循线性系统状态空间模型：

X(k) = A(k,k-1)X(k-1)+G(k)W(k)

Y(k) = H(k)X(k)+V(k)

其中，X：目标状态向量；Y：量测向量；A：状态转移矩阵；G：输入矩阵；H：观测矩阵；W：状态噪声；V：量测噪声；Q：W的协方差矩阵；R：V的协方差矩阵。

若干假设：

……

公式：

（1）第一步：根据k-1时刻的最优状态估计值先验预测k时刻的状态值

X(k|k-1)=AX(k-1|k-1)

k-1时刻的最优状态估计值X(k-1|k-1)由上一次估计获得。根据递推思想，最初应该对其有一个初始化值，这个值的初始化会影响收敛的速度。

 第二步：获得k时刻的量测值Y(k)

这个值有系统量测得到，不是算出来的。在目标跟踪中，是图像分析获得的结果。

（2）第三步：将上述两个量结合，后验估计k时刻的最优状态值。

X(k|k)=X(k|k-1)+K[Y(k)-HX(k|k-1)]

其中，K被称为卡尔曼滤波增益，因为它是（Y-HX）的系数。

（3）K(k)=P(k|k-1) H'(k) [H(k)P(k|k-1)H'(k)+R(k)]。

称Y(k)-HX(k|k-1)为残差(新息)，即d(k)=Y(k)-HX(k|k-1)。

称H(k)P(k|k-1)H'(k)+R(k)为残差协方差矩阵，即S(k)=H(k)P(k|k-1)H'(k)+R(k)。

于是引出P(k|k-1)，为先验估计误差的协方差

（4）P(k|k-1)=A(k|k-1)P(k-1|k-1)A'(k|k-1)+G(k-1)Q(k-1)G'(k-1)

于是引出P(k-1|k-1)，为后验估计误差的协方差

（5）P(k|k)=[I-K(k)H(k)]P(k|k-1)