经典重温：卡尔曼滤波器介绍与理论分析

Love The Way You Lie 2022-08-28 02:44 156阅读 0赞

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

干货第一时间送达

![66358bc9e58012a2cb523ece6ee34dcd.png][]

作者丨 lcl

来源丨我爱计算机视觉

最近业余在研究物体追踪，看到传统的方法用到了卡尔曼滤波（Kalman Filter）+匈牙利算法做轨迹匹配，因而开始研究这两种算法是如何实现的。这里简单总结一下卡尔曼滤波算法探索的过程。

##     ##

**▊****卡尔曼滤波的背景**

卡尔曼滤波常用于动态多变化系统中的状态估计，是一种通用性强的自回归滤波器。它的由来和NASA登月有关。其发明者鲁道夫.E.卡尔曼在一次访问NASA的时候，发现阿波罗计划中一个难点是轨道预测问题，因而提出了一种滤波器，可以帮助高效预测轨迹，辅助导航。NASA最终使用了这个滤波器，然后成功实现人类第一次登月计划。卡尔曼滤波器由此得名。

卡尔曼滤波器可以用来估计不确定信息，并给出状态量下一时刻的情况。即便在有噪声干扰的情况下，也可以较好的预测下一状态的情况，并找出多变量间不易察觉的相关性。因而卡尔曼滤波器可以很好适应不断变化的系统，并且**内存占用量低，推理速度快，比较适合资源受限制的场景**。

**▊****卡尔曼滤波介绍**

先来看一个简单的问题。考虑一辆车在道路上行驶，他的状态和运行速度，位置信息相关，用向量表示为：

![0f5c6aa49e4c6d5ecbdc763dc870bbcc.png][]

其中 p 变量对应位置 v 变量对应速度，都为随机变量且符合正态分布，每个分布都有一个均值（正太分布的中心）和对应的方差 .（衡量变量的不确定性）。

那么卡尔曼滤波是怎么工作的呢？我们还是回到刚才的例子。上面例子中的这两个变量看起来没有明显的对应关系，也就是说，给定速度，没法准确定出位置。

但是如果我们知道位置和速度的初始信息，其实是有办法将两个变量联系起来的。假设车辆一直保持匀速行驶，通过下面的公式其实就可以（要注意的是，换成其他问题，我们**不一定**能意识到这个公式的存在）

![97dc198182dee69e5d927a36ac5df349.png][]

基于上面的公式，我们可以直观的定义当前时刻和上一时刻之间的对应关系；假定系统中所有状态量为![e6b61b5c70866dd944bedbc267f2d6bd.png][],对应的有

![fccb013cf296aaeb2935acd74cc0119f.png][]

这里我们称![972779dcf43b31fb8fabc6e4db1a4f44.png][]为预测矩阵，也会用![ebc7fb53c6302129375ea65926ecd088.png][]表示，预测矩阵的目的是刻画从上一状态到当前状态对应的关系。

这也就是卡尔曼滤波的目标。我们希望从不确定的系统中，尽可能的发掘出确定的信息。那么另一个问题是，如何知道有多少确定（不确定）的信息可以发掘呢？我们需要一个指标来衡量系统的不确定性。

回到上个例子中，位置和速度的关系其实是不确定的，但是却是有相关性的，只是不能直观的看出来。那么如何衡量这种相关性呢？其中一个办法就是使用协方差矩阵，来衡量两个变量的相关程度。给定![ceb964dfcc549de71d16808c43df8ff7.png][],对应的协方差矩阵p为

![29339b96dab43ad763fd592ae1bda5d1.png][]

考虑到预测矩阵的影响，给定上一时刻的![a2afc4adaf1b279cb67a25a38e115be8.png][],对应可以更新为

![7cb1831b1d672f34d0caa29a4084ad5f.png][]

由此便解决了理想情况下卡尔曼滤波如何做预测的问题。但是实际情况下，系统中可能还是存在干扰或者噪声因素。比如车辆行驶过程中，遇到雨天路滑则需要减速慢行，这种情况下道路状况会作为外部因素，干扰系统行为和决策。因为针对变化量![f725698a2323f2abbed7177fe3a8a155.png][],会额外追加一个控制变量来建模外部因素。

假设行车遇到雨天需要减速，我们考虑了运动时刻的车辆加速度a为外部影响,那么有，

![a13b963f6c37759a8b40e5b8fdae65c7.png][]

对比之前计算状态的关系式，可以发现新引入的状态对系统产生了影响，因而记为

![2d5246c480f5aeb7efdb7bdf0c24ea09.png][]

![d5e5d9df2e4dd1a1665a882780532959.png][]

同时我们使用![74321e1bfee79ceef55a1e8d3c419f05.png][]表示，引入的外部因素对于协方差的影响，因而有

![ddd7bf57775e23627165f7f8bf2bef4b.png][]

这里总结一下，新引入的外部变量使得

**1)当前的最佳估计不再为系统自身的最佳估计，还考虑了外部因素影响**

**2)当前的不确定性不再为系统自身的不确定性，引入了外部的不确定性做参考**

**▊****卡尔曼滤波公式推导**

通过预测状态量和不确定性，我们可以有较大把握预测对某一时刻下的状态。到目前为止，我们很好地完成了对系统的建模，但是有一个问题没有考虑到，那就是系统的初始值怎么得到。

对于系统中数据的收集，我们一般基于传感器来实现。但是同一传感器在同一时刻，可能因为外部原因导致收集得到的信号不完全一样。因而系统输入初始值对应结果大概率有很多噪音。这里我们把收集数据的均值记为, 方差记为![ff18923236ac0bab12ef0c50ba6c4011.png][]

现在就有了新的问题。对于当前时刻，我们有了两组参考值，一组是预测值，一组是测量值。也许两组都对，也许其中只有一组对，也可能都不准（比如传感器测得不直接，偏差太大；预测值也偏差太大）。这种情况下，应该如何得到最终结果呢？

其中一个方案是，考虑直接融合两组结果。我们假设这两组数据都是正态分布，因而直接取pdf相乘就可以得到一个新的分布，也就是做一个联合概率分布的计算。这个联合概率分布本质上计算了前后状态转化对应分布和输入输出状态分布联合作用下的结果。

因而我们有下式

![012c2f13ab950640238c558839e8b0a2.png][]

这里要注意的是，两个高斯pdf的乘积不是一个pdf（概率加和明显不是1），但是正比于一个未知高斯分布，因而我们可以求得这个新分布的均值和方差，根据如下关系，

![f268b4eb93c785d768b90ad36e88c8d7.png][]

为了使其成立，我们需要保证：

![84db36b77c60ace86c72cc5b87f181be.png][]

联立二式并稍作变化，我们得到：

![c0449f567464bbf4dc24dc622a93c342.png][]

令

![97ed65a8d67dad3d2971484cd750d13b.png][]

为卡尔曼增益， 我们有

![c9b97ef0ec12201d3f93cd2a5e2fbf33.png][]

对应的矩阵形式为：

![60cdd3326646beee5b38dc66be1d373b.png][]

这里为了防止混淆，先停下来总结一下，

1)我们介绍了预测变量，下标为p，分布均值，方差![a72601f43730c566e9b4fcd03f2b1a56.png][]分别对应于![67a9523e70699261d8a0de77c8e143ea.png][]![2c9387c0cf999b4834be2193ca11749e.png][]

2)我们介绍了观测变量，下标为ob，分布均值记为![2dd3e244bbd68171803aee5ad79ee074.png][], 方差记为![a4bd89d00daf2ba801ddea7a0aa329e8.png][]

3)基于1，2，我们代入即可推导出完整的卡尔曼滤波公式

所以根据刚才列出的计算式分别代入，我们可以得到，

![f43d5f1acfc9604c86f118eb41fea2e5.png][]

进而可以求解得状态更新方程和卡尔曼增益求解公式:

![609ebcb3ff25784571399b4aeb925616.png][]

注意这里的卡尔曼增益更新为![c16d6380fe9075be4a859dff0a1fcffe.png][]

![7d3b7b022c02372bc53b309906a9af14.png][]

至此，便可以套公式更新卡尔曼滤波的参数了。

**▊****总结**

卡尔曼滤波是处理线性系统非常好用的工具，对于不确定性的建模取自于物理、机械模型，所以对于现实生活中的状态还是能很好把握的。对于非线性系统，拓展卡尔曼滤波(extended Kalman filter)也可以做到很好地处理。

本文通过现实场景建模引入问题，简单介绍了卡尔曼滤波算法，对于更加清楚的公式推导请见参考列出的几篇相关文章。

欢迎大家在下方留言或者将建议发到我的邮箱(lcltopismine3@163.com)，以便更好地交流心得。另外感谢参考文献所列诸作者。

## **参考文献:** ##

. 论智：图说卡尔曼滤波，一份通俗易懂的教程

(https://zhuanlan.zhihu.com/p/39912633)

. 涅索斯衬衫：卡尔曼滤波（Kalman Filter）原理与公式推导

(https://zhuanlan.zhihu.com/p/48876718)

. Calculate the product of two Gaussian PDF's

(https://math.stackexchange.com/questions/114420/calculate-the-product-of-two-gaussian-pdfs)

. 详解ADAS之FCW前碰撞预警系统技术原理-公司新闻-深圳前海米乐视科技有限公司

(http://mileview.cn/gongsixinwen/22-43.html)

. 如何通俗并尽可能详细地解释卡尔曼滤波？

(https://www.zhihu.com/question/23971601)

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

**3D视觉精品课程推荐：**

[1.面向自动驾驶领域的多传感器数据融合技术][1.]

[2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)][2._3D]  
[3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进][3.]  
[4.国内首个面向工业级实战的点云处理课程][4.]  
[5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解][5._-_-IMU-GPS_SLAM]  
[6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦][6._-_SLAM_VINS-Fusion]  
[7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化][7._LOAM_3D_SLAM_]  
[8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)][8._SLAM_cartographer_LOAM _LIO-SAM]

[9.从零搭建一套结构光3D重建系统\[理论+源码+实践\]][9._3D]

[10.单目深度估计方法：算法梳理与代码实现][10.]

##  ##

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

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

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

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

![48ce41dba9074c2ee94cab669d6b299c.png][]

▲长按加微信群或投稿

![c82ea364224261a29f169e9c77d58cb8.png][]

▲长按关注公众号

##  ##

**3D视觉从入门到精通知识星球**：针对3D视觉领域的**视频课****程（[三维重建系列][Link 1][三维点云系列][Link 2][结构光系列][Link 3]、[手眼标定][Link 4]、[相机标定][Link 5]、****激光/视觉SLAM****、****自动****驾驶等****）、****知识点汇总、入门进阶学习路线、最新paper分享、疑问解答**五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

##  ##

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

![97a5991719c4e422bbe72cbc2d6d779e.png][]

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

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

[66358bc9e58012a2cb523ece6ee34dcd.png]: /images/20220828/267ba3d2e8ff40728850c4a3ba6c62ba.png
[0f5c6aa49e4c6d5ecbdc763dc870bbcc.png]: /images/20220828/2e8c993632a44f99ac03116b79074375.png
[97dc198182dee69e5d927a36ac5df349.png]: /images/20220828/bee21597715f4bf88e3aae4badd358c5.png
[e6b61b5c70866dd944bedbc267f2d6bd.png]: /images/20220828/658920b45d42484fbf72ebff7a0e9613.png
[fccb013cf296aaeb2935acd74cc0119f.png]: /images/20220828/934ce9ccde97490cbff37ec87e7ee7a3.png
[972779dcf43b31fb8fabc6e4db1a4f44.png]: https://img-blog.csdnimg.cn/img_convert/972779dcf43b31fb8fabc6e4db1a4f44.png
[ebc7fb53c6302129375ea65926ecd088.png]: /images/20220828/cee64ebab6f74d0e8f842acffe3b924a.png
[ceb964dfcc549de71d16808c43df8ff7.png]: /images/20220828/74e3d01fb4864121bf36f2d65020029f.png
[29339b96dab43ad763fd592ae1bda5d1.png]: /images/20220828/196fafed10ab49828ff65f5c88231615.png
[a2afc4adaf1b279cb67a25a38e115be8.png]: /images/20220828/5da43b4ece624d58b6b42037475a5a00.png
[7cb1831b1d672f34d0caa29a4084ad5f.png]: /images/20220828/a99d08a3f42a457db563195fc6cfefd8.png
[f725698a2323f2abbed7177fe3a8a155.png]: /images/20220828/5591f41bb796451fa6284e3ef0f56af1.png
[a13b963f6c37759a8b40e5b8fdae65c7.png]: /images/20220828/87487b3ffda44ad5b8ffb858bd2244f7.png
[2d5246c480f5aeb7efdb7bdf0c24ea09.png]: /images/20220828/f7e9ae3325f04d51851cb44955e76cd1.png
[d5e5d9df2e4dd1a1665a882780532959.png]: /images/20220828/e36a081b51884c0998fc1f0670c99094.png
[74321e1bfee79ceef55a1e8d3c419f05.png]: /images/20220828/f719ef51a73b406ca28463c7c856010d.png
[ddd7bf57775e23627165f7f8bf2bef4b.png]: /images/20220828/a8f7f4914e4d4851b146d0aeda335533.png
[ff18923236ac0bab12ef0c50ba6c4011.png]: /images/20220828/0ad9ffa9f1d54ee6aa072baf4916dd1d.png
[012c2f13ab950640238c558839e8b0a2.png]: /images/20220828/80d0843aa78b4734a218b3ef3fd5eeef.png
[f268b4eb93c785d768b90ad36e88c8d7.png]: /images/20220828/2e3006cf8e92402a946c8b38afe53beb.png
[84db36b77c60ace86c72cc5b87f181be.png]: /images/20220828/e8c3a97e414d4d73aeb4d5b2e7cecff3.png
[c0449f567464bbf4dc24dc622a93c342.png]: /images/20220828/f444dbabdfc14323a8a5f965221fb6f5.png
[97ed65a8d67dad3d2971484cd750d13b.png]: /images/20220828/4b169219c5a3407ebff460b684c00c72.png
[c9b97ef0ec12201d3f93cd2a5e2fbf33.png]: /images/20220828/12ecf7f1cd5a42c0b9437e6a00cb79db.png
[60cdd3326646beee5b38dc66be1d373b.png]: /images/20220828/61ef5d86150e496aae2bef860286a7ba.png
[a72601f43730c566e9b4fcd03f2b1a56.png]: https://img-blog.csdnimg.cn/img_convert/a72601f43730c566e9b4fcd03f2b1a56.png
[67a9523e70699261d8a0de77c8e143ea.png]: https://img-blog.csdnimg.cn/img_convert/67a9523e70699261d8a0de77c8e143ea.png
[2c9387c0cf999b4834be2193ca11749e.png]: /images/20220828/0f95568f9f99482c91108e1826ff2a20.png
[2dd3e244bbd68171803aee5ad79ee074.png]: https://img-blog.csdnimg.cn/img_convert/2dd3e244bbd68171803aee5ad79ee074.png
[a4bd89d00daf2ba801ddea7a0aa329e8.png]: /images/20220828/49a6ef21d3124edbb37d487f0ebbad03.png
[f43d5f1acfc9604c86f118eb41fea2e5.png]: /images/20220828/99d8d6a3a2264ab38708c7203a71649f.png
[609ebcb3ff25784571399b4aeb925616.png]: /images/20220828/dfbb9a6bb5464589b2d452241c617b47.png
[c16d6380fe9075be4a859dff0a1fcffe.png]: /images/20220828/131978189abc4bd09b62be702c390442.png
[7d3b7b022c02372bc53b309906a9af14.png]: /images/20220828/8bbeb32ce2d94612b1d0f76b158bb221.png
[1.]: https://blog.csdn.net/Yong_Qi2015/article/details/119284035
[2._3D]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247580753&idx=1&sn=174f87726a07cc67247c528de0e924ca&chksm=fbfd9765cc8a1e732c08cbaedaa78b35df43c1cb47ef94bad9ec1c942b9be63cf63a64e8c53f&scene=21#wechat_redirect
[3.]: https://blog.csdn.net/Yong_Qi2015/article/details/117828998
[4.]: https://blog.csdn.net/Yong_Qi2015/article/details/118865512
[5._-_-IMU-GPS_SLAM]: https://blog.csdn.net/Yong_Qi2015/article/details/119975746
[6._-_SLAM_VINS-Fusion]: https://blog.csdn.net/Yong_Qi2015/article/details/119047545
[7._LOAM_3D_SLAM_]: https://blog.csdn.net/qq_29462849/article/details/118448408
[8._SLAM_cartographer_LOAM _LIO-SAM]: https://blog.csdn.net/Yong_Qi2015/article/details/120328880
[9._3D]: https://blog.csdn.net/biaoshu2/article/details/106665145
[10.]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247582082&idx=2&sn=cc10291ddfdf6b05bdae71887c7c819c&chksm=fbfdaab6cc8a23a020a7359eae081f06c24572a12975bfd90396ac583a0081750f489db89b49&scene=21#wechat_redirect
[48ce41dba9074c2ee94cab669d6b299c.png]: /images/20220828/16ae567794f04f5fa574492a7cddc0d4.png
[c82ea364224261a29f169e9c77d58cb8.png]: /images/20220828/0ac7ef399ee04481a8cad2e4df9b9ebd.png
[Link 1]: https://blog.csdn.net/DDWDDZY/article/details/105608179
[Link 2]: https://blog.csdn.net/pikachu_777/article/details/82992991
[Link 3]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247546284&idx=2&sn=d067a98047192a04f21489aca338a5d6&chksm=fbfc1e98cc8b978e5fb6b2afd1a473c89e2861374542421b9972f87a6fdfcd47eb50c443042e&scene=21#wechat_redirect
[Link 4]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247554670&idx=2&sn=cf3323178d5d1960476208e6303a8479&chksm=fbfc3d5acc8bb44c184084dcf447a5981d297815d62bef699129338d99a0472c622f96ee71d8&scene=21#wechat_redirect
[Link 5]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247486422&idx=1&sn=381ab793959aed0eb6f01019dea522d4&chksm=fbff20e2cc88a9f456b80e23d2482fb319b08e89da071b39597bc51942a6aa7b756a6749856c&scene=21#wechat_redirect
[97a5991719c4e422bbe72cbc2d6d779e.png]: /images/20220828/8501a6b730a0404abc90add4dfcb8dc1.png