卡尔曼滤波最完整公式推导

点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达

作者丨东林钟声@知乎

来源丨https://zhuanlan.zhihu.com/p/341440139

编辑丨极市平台

导读

 

卡尔曼滤波能够很好地从带有噪声的数据过程中估计状态，而且卡尔曼滤波也是阿波罗登月中使用的突破性技术之一，本文回顾了卡尔曼滤波的技术细节，对公式推导做一个完整的整理。

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。上面一段话来自百度百科，其实最核心的意思就是卡尔曼滤波可以很好地从带有噪声的数据过程中估计状态。而且卡尔曼滤波也是阿波罗登月中使用的突破性技术之一，正好我国嫦娥五号采土归来，正好回顾一下卡尔曼滤波的技术细节，对公式推导做一个完整的整理。

1、定义

• 状态方程：

• 观测方程：

• 符号定义：

首先定义状态估计误差

理解卡尔曼公式推导的核心，是一定要理解这里的符号标记。 核心是在第k时刻，对k+1时刻的估计！所以后面都要记住，在第k时刻的时候，对k+1时刻的任何值，都只能是估计（预测未来值）。具体怎么估计的，如下：

同理，我们也可以得到观测估计误差

基于上面的公式，我们定义两个重要的误差协方差矩阵

• 最终目的

其实卡尔曼的最终目的，是得到一个基于误差能够不断修正的迭代式估计表达式，其具体形式应该如下：

这个式子非常直观，就是基于误差去修正，怎么最优的去修正？就是我们怎么来算这个W，也就是卡尔曼增益（Kalman Gain）。

2、推导

为了得到不停的基于误差修正的一种计算模式，可能要能够得到一些关键的递推形式，如我之前发表过关于递推最小二乘的推导。首先我们推导关于状态误差协方差矩阵：

这里我们得到了P(k|k) 到P(k+1|k) 的一个递推形式，注意这里P(k|k) 的标记形式是为了更方便后面的推导，为什么这么说，其实是为了得到

这样一种能够不停向前迭代的形式。带着这个思考继续下面的推导，就会明白卡尔曼的巧妙之处。

我们先总结下，这里得到了P的一个递推形式：

同理，也是可以得到观测误差协方差的一个递推形式：

会发现其实S(k+1) 和S(k) 没啥直接的关系，这就是为什么不把S(k+1) 写成像P那样 S(K+1|K) 的形式，因为没有必要，只有P是需要这样来写的。

这里再次思考，我们的目的是求一个最优的W，就是卡尔曼增益，具体怎么来最优？这里就用到了状态估计误差，就是说，我们的目的是让状态误差的平方和最小，这里可以使用P的迹。具体推导如下：

然后使用P的迹对W求导，令其为0得到最优的W：

得到：

然后再把W带回P(k+1|k+1)的式子中：

到这里，推导完毕，得到了W的最优表达式，同时，也解决了我们上面提出的问题，一个完整的递推链：

建立完毕。那么通过这一套完整的递推链，给定P的一个初始估计P(0|0) 就可以按照下面的链来进行状态估计：

到这里卡尔曼滤波的公式推导完成了，这个版本是我在看过很多其他资料，反复提炼之后，标记最友好，推导最友好的一个版本，理解卡尔曼滤波一定要首先理解符号的定义，特别是下标，以及怎么样构建递推链。在掌握之后可以自行通过上面的推导总结出，其他教程反复提到的黄金五条公式，其实这些在我看来不是重点，重点是理解其本质原理。

3、资料

其实要弄懂卡尔曼滤波还真不简单，需要一些基本矩阵、统计、自控原理以及矩阵求导的知识，特别是对迹求导，这里推荐Matrix CookBook，里面详细列举了各种常用的求导方式。

本文仅做学术分享，如有侵权，请联系删文。

下载1：OpenCV-Contrib扩展模块中文版教程

在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。

下载2：Python视觉实战项目52讲

在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。

下载3：OpenCV实战项目20讲

在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~