点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者丨东林钟声@知乎（已授权）

来源丨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

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~