卡尔曼滤波学习心得（1）方程推导

前言

最近由于项目需求需要用到Kalman滤波，另外也想在明年大学毕业之前做出一架自己的四旋翼来，也算是个人的爱好，其中不免也要用到KF，故将一些学习心得写在这里，一来方便日后查阅，二来希望能够与大家互相交流共同进步。最近读到一篇文章，推导过程比较容易理解，故整理出来与大家分享。文中难免有许多错误，望能够指出，及时与我联系，我会第一时间更正。QQ：694439986

言归正传

1.随机状态模型

这两个式子想必大家都很熟悉，状态方程和测量方程。式中，Xk是n*1维的状态向量，Zk 是m*1维的量测向量；其中系统的过程噪声与测量噪声阵均为零均值的高斯白噪声，而且他们互不相关，所以他们满足：

上式也是卡尔曼滤波的基本假设。

2.方程推导

先从状态方程出发

估计误差（k-1）=真实状态（k-1）-状态估计（k-1)

（1）

这里注意一点，通常用“~"的上表表示误差量，“^"的上表表示估计量。

根据式（1）我们可以得到状态估计均方误差阵Pk-1

(2)

现在，根据状态方程，我们可以对k时刻的状态Xk做出预测，由于系统状态方程的零均值白噪声对预测不会有任何贡献，所以状态一步预测：

（3）

式中，下角标k/k-1表示根据k-1时刻对k时刻做出的预测。

则一步预测误差:

(4)

将状态方程和式（3）代入(4)得：

（5）

由于

因此可得状态一步预测均方误差阵P（k/k-1):

（6）

然后看测量方程

同理通过状态一步预测和系统的量测方程可对k 时刻的量测作一步预测：

（7）

这个对测量的预测与真实测量值之间存在误差，记测量一步预测误差为：

（8）

将测量方程和式（7）一起带入（8）可得

（9）

记量测一步预测均方误差阵为：

（10）

状态一步预测与量测一步预测之间的协方差阵为：

（11）

由式（9）可以看出，量测一步预测误差同样包含状态一步预测X（k/k-1)的信息，所以综合考虑状态方程和预测方程，利用Z（k/k-1) 修正 X(k/k-1) 得到准确的估计值。

所以令

（12）

式（7）代入式（12），并考虑状态一步预测方程，可得：

（13）

接下来求解待定的系数矩阵K

记k时刻状态估计误差为：

（14）

将式（13）代入（14）得：

（15）

上述k 时刻状态估计的均方误差为：

（16）

至此，我们只需要使k 时刻状态估计的均方误差最小，即式（16）最小，便可得到最优估计，我们使用求导的方法找到使式（16）最小的K值，即可完成最优估计。这里，求导通过矩阵的迹来实现。

式（16）可展开为：

（17）

两边同时求迹：

（18）

由迹的性质：可得：

（19）

令上式右端等于零，可解得：

（20）

（21）

至此我们就可以得到Kalman滤波的5个基本公式：