卡尔曼滤波算法详细推导

一、预备知识

1、协方差矩阵

X是一个n维列向量，u_i是x_i的期望，协方差矩阵为

可以看出

协方差矩阵都是对称矩阵且是半正定的

协方差矩阵的迹tr(P)是X的均方误差

2、用到的两个矩阵微分公式

公式一：

公式二：若B是对称矩阵，则下式成立

tr表示矩阵的迹，具体推导过程参考相关矩阵分析教程

二、系统模型与变量说明

1、系统离散型状态方程如下

由k-1时刻到k时刻，系统状态预测方程

系统状态观测方程

2、变量说明如下

初始状态以及每一时刻的噪声都认为是互相独立的，实际上，很多真实世界的动态系统都并不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。

三、卡尔曼滤波器

卡尔曼估计实际由两个过程组成：预测与校正，在预测阶段，滤波器使用上一状态的估计，做出对当前状态的预测。在校正阶段，滤波器利用对当前状态的观测值修正在预测阶段获得的预测值，以获得一个更接进真实值的新估计值。变量说明

2、卡尔曼滤波器计算过程

预测：

3、卡尔曼滤波算法详细推导

从协方差矩阵开始说起，真实值与预测值之间的误差为

预测误差协方差矩阵为

真实值与估计值之间的误差为：

卡尔曼估计误差协方差矩阵为

将ek代入得到

卡尔曼滤波本质是最小均方差估计，而均方差是Pk的迹，将上式展开并求迹

最优估计Kk使tr(Pk)最小，所以上式两边对Kk求导

套用第一节中提到的那两个矩阵微分公式，得到

令上式等于0，得到

所以预测误差协方差矩阵P'k可以由上一次算出的估计误差协方差矩阵Pk-1及状态转移矩阵A和过程激励噪声的协方差矩阵Q算得。

4、总结

总结卡尔曼滤波的更新过程为

这就是卡尔曼滤波器递推过程。

至于Pk的算法：