卡尔曼滤波:从入门到精通-程序员宅基地

技术标签: 算法  协方差  机器学习  深度学习  人工智能  

点击上方“计算机视觉工坊”,选择“星标”

干货第一时间送达

作者丨David LEE@知乎(已授权)

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

编辑丨极市平台

导读

 

卡尔曼滤波器是一个“optimal recursive data processing algorithm(最优化自回归数据处理算法)”。对于解决很大部分的问题,他是最优,效率最高甚至是最有用的。本文是一篇关于卡尔曼滤波的基础入门教程,详细阐述了卡尔曼滤波的推导过程以及推广到高维的过程。 

最早接触卡尔曼滤波是在卫星导航课中,GPS 和IMU 结合时常会用到卡尔曼滤波。但学完了也只明白了数学推导,不过是“会做题的机器”。最近在学习SLAM 时想要重新好好温习一下卡尔曼滤波,虽然现在SLAM 的主流趋势是利用图优化,但卡尔曼滤波仍然为我们提供了一个很好的参考。

导论

卡尔曼滤波本质上是一个数据融合算法,将具有同样测量目的、来自不同传感器、(可能) 具有不同单位 (unit) 的数据_融合_在一起,得到一个更精确的目的测量值。

卡尔曼滤波的局限性在于其只能拟合线性高斯系统。但其最大的优点在于计算量小,能够利用前一时刻的状态(和可能的测量值)来得到当前时刻下的状态的最优估计。

本文虽然是小白教程,但还是需要各位至少知道高斯分布,一点点线性代数,还有状态向量这样的名词。

简述

考虑一个SLAM 问题,它由一个运动方程:

和一个观测方程组成:

就把它当作一个线性系统吧(非线性系统请看下一讲扩展卡尔曼滤波),并且为了简化推导,忽略路标的下标j,并把路标y 并入到状态向量一起优化,那么运动方程就可以写为:

其中,

  • 为t 时刻的状态向量,包括了相机位姿、路标坐标等信息,也可能有速度、朝向等信息;

  • 为运动测量值,如加速度,转向等等;

  • 为状态转换方程,将t-1 时刻的状态转换至t 时刻的状态;

  • 是控制输入矩阵,将运动测量值 的作用映射到状态向量上;

  • 是预测的高斯噪声,其均值为0,协方差矩阵为

这一步在卡尔曼滤波中也称为预测 (predict)。

类似地,测量方程可以写为:

其中,

  • 为传感器的测量值;

  • 为转换矩阵,它将状态向量映射到测量值所在的空间中;

  • 为测量的高斯噪声,其均值为0,协方差矩阵为

而卡尔曼滤波就是预测 - 测量之间不断循环迭代。当然,对于某些情况,如GPS + IMU,由于IMU 测量频率远比GPS 高,在只有IMU 测量值时,只执行运动更新,在有GPS 测量值时再进行测量更新。

一个小例子

用一个在解释卡尔曼滤波时最常用的一维例子:小车追踪。如下图所示:

状态向量为小车的位置和速度:

而司机要是踩了刹车或者油门,小车就会具有一个加速度,

假设t 和t-1 时刻之间的时间差为 。根据物理知识,有:

写成矩阵形式就有

跟之前的运动方程对比,就知道

上式就写为

表示t-1 时刻卡尔曼滤波的状态估计;  则表示中t-1 到t 时刻,预测更新所得的预测值。

再利用运动模型对状态向量进行更新后,还要继续更新状态向量的协方差矩阵P,公式为:

假设  为t 时刻下状态向量的真值(自然是永远未知的),由之前的现形运动方程(式(3))给出,将式(3) 与式(9) 相减可得:

考虑到状态向量和噪声是不相关的,则 ,上式就可以简化为

推导完毕。

可以看到,经过预测更新,协方差矩阵P 变大了。这是因为状态转换并不完美,而且运动测量值含有噪声,具有较大的不确定性。

预测更新实际上相当于“加法”:将当前状态转换到下一时刻(并增加一定不确定性),再把外界的干扰(运动测量值)叠加上去(又增加了一点不确定性)。

上面即为卡尔曼滤波中预测这一步。这一步相对比较直观,推导也较测量更新简单,就只在这里详细给出了。

如果得到了测量值,那么我们就可以对状态向量进行测量更新了,对应的公式为

其中,

为卡尔曼增益。

从这里就可以看到,测量更新显然比预测更新复杂,难点也集中在这里。下面就给出测量更性的详细推导。

推导

一维case

从t-1 时刻起,小车运动后,经过前面所述的预测更新后,我们就得到了t 时刻的小车位置的估计,由于在卡尔曼滤波中,我们使用高斯概率分布来表示小车的位置,因此这个预测的位置可以写为:

为了与前面的通用的推导区别开来,在这个一维的例子中我们使用了新的符号。不过熟悉高斯概率分布的话应该可以马上看出来,  为这个高斯分布的均值, 为方差,而r 为小车的可能位置,  为某个可能位置 (r) 的概率分布。

假设在t 时刻,我们通过某测距仪测得小车距离原点的距离r,由于测量包含噪声(且在面前我们假设了其为高斯噪声),因此该测量值也可以利用高斯概率分布来表示:

除了下标外,其余的字母的含义都和上面的式子一样。

如上图琐事,现在在t 时刻,我们有了两个关于小车位置的估计。而我们所能得到的关于小测位置的最佳估计就是将预测更新和测量更新所得的数据融合起来,得到一个新的估计。而这个融合,就是一个简单的“乘法”,并利用了一个性质:两个高斯分布的乘积仍然是高斯分布。

将上式化简一下:

其中,  和  的加权平均,  则是  和  的调和平均的二分一:

最右边的式子是为了后面的计算而准备的。

本质上,这(高斯分布相乘)就是卡尔曼滤波中测量更新的全部了。

那么, 怎么由上面两个简单的一维的式子得到前一节 呢?一步一步来。


转换矩阵H 的引入

在刚刚的一维情况的小例子中,我们其实做了一个隐式的假设,即有预测更新得到的位置的概率分布和测距仪所得的测量值具有相同的单位 (unit),如米 (m)。

但实际情况往往不是这样的,比如,测距仪给出的可能不是距离,而是信号的飞行时间(由仪器至小车的光的传播时间),单位为秒 (s)。这样的话,我们就无法直接如上面一般直接将两个高斯分布相乘了。

此时,就该转换矩阵  闪亮登场了。由于 ,c 为光速。所以此时  (测量方程为 ,可以回去参考一下式(4))。

预测值就要写为:

而测量值保持不变:

这样,两个高斯概率分布在转换矩阵H 的作用下又在同一个空间下了。根据前面  和  的公式 (式(27) 和式(28)),可得:

将上式两端都乘以c 则可得:

由于 (这里转换矩阵H 不随时间变化而变化,所以把下标t 略去),并记 ,则上式可以写为:

类似的有,

两边乘以 有:

推广至高维

到了这一步,这个一维情况下卡尔曼滤波的测量更新步骤就已经彻底讲完了。剩下的就是将这个一维例子推广至高维空间中。其实大家仔细观察一下就会得到答案。

  • 就是 ,是测量更新后所得的状态向量;

  • 就是 ,是t-1 时刻到t 时刻的小车的预测更新(或叫运动更新)后的状态向量;

  • ,是测量值;

  • 在高维空间中为 ,为测量更新后,状态向量的协方差矩阵;

  • 在高维空间中就成了协方差矩阵 ,是预测更新后状态向量的协方差矩阵;

  • ,是测量值的协方差矩阵;

  • H 就是 了,高维空间中的转换矩阵。

  • 而最重要的,卡尔曼增益 ,在高维空间中就可以写为 。看着很复杂,但仔细对照的话就是把前面相迎的数据替换带入即可。

最后,根据式(33)  就可得:

根据 就可得:

小结一下

通过这个一维情况的推导,希望能说明卡尔曼滤波就是在给定初始值的情况下,由预测和测量不断迭代、更新状态向量。而预测就是一个“加法”:状态转换和运动预测叠加;测量则是简单的高斯分布相乘,中间引入了一个转换矩阵将测量值和状态向量映射在同一个代数空间中。

讨论

至此,相信你已经明白了卡尔曼滤波的推导过程。而具体的问题就取决于你的建模了。如在上面的小车的例子中,各个参数如下:

假设该时刻下运动测量值为加速度 ,为均值为0标准差为 的高斯分布(标准差可以为经验值或仪器精度。

对于测量值,同样可以假设 ,那么

多问个为什么

如果只关心卡尔曼滤波的推导和结果,到这里就可以停止啦。

但推完卡尔曼滤波,我还有几个个为什么。知其然更要知其所以然。下面是我对于自己的疑惑学习、思考得到的解答,而且碍于表达能力,不敢说百分百正确。

首先是对于预测更新。前面也说到了,预测更新相当于“加法”。这相对好理解一些。在t-1 时刻我们有了对于小车位置的一个估计,根据对小车速度(状态向量之一)、小车的加速度(运动测量值)的建模,在辅以时间间隔,自然可以计算出小车在该时间间隔内的位移和速度增量,再将之叠加到原有的状态向量上即可。由于建模和测量的过程带有噪声,所以此时小车的位置估计的精度是下降的(方差增大)。

那么为什么测量更新就是乘法而非加法呢?

因为测量更新的依据是贝叶斯法则。在有了测量值之后,我们求小车位置的概率分布其实就是在求 。根据贝叶斯法则有:

后验概率。直接求后验概率比较困难(为什么?)。假设就在这个一维的小车例子中,当我们得到一个距离测量值z,那么小车的位置可能是距离原点的-z 或z 的两个点上。对于二维就可能是一个圆,三维则是一个球。此时要精确地知道小车的位置(消除歧义点),一则我们可以继续测量,二则需要额外的信息。这就使得求后验概率比较费时费力。

反观贝叶斯法则的右侧,此时我们已经有了先验概率 ,这是上一时刻的状态向量的概率分布,并且我们也有了 ,因为所得的测量值  表达的就是在当前位置下,我们能得到的测量值,亦即贝叶斯中的似然。在  为一个常数的情况下,最大化  就得到了最优的

既然测量更新是以贝叶斯公式为基础,那么反观预测更新,除了前面那个直观的“加法”解释之外,是不是也有一个概率上的解释呢?

连续的高斯分布所表示的小车位置的预测更新我没找到(不好意思),但就离散情况的话还是有的,就是全概率公式。以下图为例,假设t-1 时刻,小车的位置分布概率如图所示,到了t 时刻,预测小车向前运动了3米(3个格子),但由于模型的不确定性和噪音,我们不能保证小车精确地向前走了3米,根据概率分布,我们可以假设小车有80%的概率往前走了3米,10%的概率往前走了两米,而另有10%的概率往前走了4米。

那么,在t 时刻,若小车真的运动到了这个位置,其概率分布是怎样的呢?它既有可能是在距离该位置3米远的地方以0.8的概率运动到现在这个位置的,也有可能是以0.1 的概率从2或4米远的地方为初始位置运动到这的,根据全概率公式,可以表达为

类似地,t时刻下,小车运动后在其他位置上的概率分布也可以用全概率公式表达出来。当然,最后的计算结果还需要进行归一化处理

如果我们不断地减小每个方格的分辨率,并按照高斯分布给予每个方格一个概率值,并对小车运动也做如此的离散化处理,应该也是可以不断逼近连续的情况(个人猜想)。

至此,关于卡尔曼滤波的个人浅见就到此为止了。精通还需要不断地实践,但希望读完本文,能让你对卡尔曼滤波有全面的了解。

有帮助的话,帮忙点个赞呗。

一些参考文献:

Ramsey Faragher 的 understanding the basis of the kalman filter.

维基百科 Kalman filter

高翔的《视觉SLAM十四讲》

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

下载1

在「计算机视觉工坊」公众号后台回复:深度学习,即可下载深度学习算法、3D深度学习、深度学习框架、目标检测、GAN等相关内容近30本pdf书籍。

下载2

在「计算机视觉工坊」公众号后台回复:计算机视觉,即可下载计算机视觉相关17本pdf书籍,包含计算机视觉算法、Python视觉实战、Opencv3.0学习等。

下载3

在「计算机视觉工坊」公众号后台回复:SLAM,即可下载独家SLAM相关视频课程,包含视觉SLAM、激光SLAM精品课程。

重磅!计算机视觉工坊-学习交流群已成立

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

同时也可申请加入我们的细分方向交流群,目前主要有ORB-SLAM系列源码学习、3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、深度估计、学术交流、求职交流等微信群,请扫描下面微信号加群,备注:”研究方向+学校/公司+昵称“,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

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

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

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

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

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/qq_29462849/article/details/118160848

智能推荐

没有第三方控件用RadioGroup做轮播图--MainActivity 主页面和布局_mainactivity类怎么转成radiogroup类-程序员宅基地

文章浏览阅读405次。没有第三方控件用RadioGroup做轮播图--MainActivity 主页面和布局_mainactivity类怎么转成radiogroup类

Spark闭包与序列化_闭包序列化-程序员宅基地

文章浏览阅读1.9w次,点赞9次,收藏22次。本文原文出处: http://blog.csdn.net/bluishglc/article/details/50945032 严禁任何形式的转载,否则将委托CSDN官方维护权益!在Spark的官方文档再三强调那些将要作用到RDD上的操作,都会被分发到各个worker节点上去执行,我们都知道,这些操作实际上就是一些函数和涉及的变量组成的闭包,这里显然涉及到一个容易被忽视的问题:闭包的“序列化”。显然_闭包序列化

Unity ECS Manual [17]_unity ecs1.0-程序员宅基地

文章浏览阅读123次。Job dependencies  Unity根据系统读取和写入的ECS组件来分析每个系统的数据依赖性。如果一个在框架中较早更新的系统读取了后来的系统所写的数据,或者写了后来的系统所读的数据,那么第二个系统就会依赖第一个系统。为了防止竞争条件,作业调度器在运行一个系统的作业之前,要确保该系统所依赖的所有作业已经完成。  一个系统的Dependency属性是一个JobHandle,代表了该系统的ECS相关依赖关系。在OnUpdate()之前,Dependency属性反映了系统对先前作业的传入依赖关系。默认_unity ecs1.0

【干货合集】看完这16篇文章,不再做敏捷项目管理上的小白!_项目管理干货 site:csdn.net-程序员宅基地

文章浏览阅读1.1k次。原文链接:点击打开链接摘要: 流行技术大狂欢,5月29日即将召开的第二届研发效能嘉年华,带来了前沿技术理念及实践技术成果分享。本次峰会将有10位技术大咖进行干货分享,多角度,不同领域的带领大家了解高效研发。项目管理的主流模式有哪几种?传统项目管理通常采用的是瀑布式、部分迭代开发模式,要求在项目建设时,需求足够明确、文档足够规范,迭代过程中需求变更越多、越晚,对项目影响越大,会影响到项目的交付质量。..._项目管理干货 site:csdn.net

大数据(二十七):Sqoop常用命令和公用参数_大数据 公参-程序员宅基地

文章浏览阅读211次。一、常用命令列举 命令 类 说明 import ImportTool 将数据导入到集群 export ExportTool 将集群数据导出 codegen CodeGenTool ..._大数据 公参

基于MAC地址的安全配置与管理实战_如何防范mac地址泛滥-程序员宅基地

文章浏览阅读1.9k次,点赞2次,收藏10次。MAC地址是网络设备中不变的物理地址,基于MAC地址的接入控制成了最直接,甚至可能是大多数情况下最有效的控制手段。在二层交换网络中,是通过依靠保存在交换机中的MAC地址表来进行寻址的,这时如果控制交换机中存储的MAC地址表就可以控制一些非法设备的接入,让其他设备不能与他进行通信。在华为S系列交换机中,为了实现这个目的提供了多种基于MAC地址的安全手段,如限制接口学习MAC地址表项的数量,限制交..._如何防范mac地址泛滥

随便推点

cookie和session机制之间的区别与联系-程序员宅基地

文章浏览阅读1.1w次。具体来说cookie机制采用的是在客户端保持状态的方案。它是在用户端的会话状态的存贮机制,他需要用户打开客户端的cookie支持。cookie的作用就是为了解决HTTP协议无状态的缺陷所作的努力.而session机制采用的是一种在客户端与服务器之间保持状态的解决方案。同时我们也看到,由于采用服务器端保持状态的方案在客户端也需要保存一个标识,所以session机制可能需要借助于cookie机制来达到

微信小程序初识---地图开发_微信公小程序地图开发-程序员宅基地

文章浏览阅读2k次。微信小程序似乎比较火,很多公司除了自家app外都要给自家再弄个小程序,于是乎空余时间就试了试小程序,现记录下地图页面的开发。小程序官方文档也比较全,基本上面的东西都有,有前端开发经验的上手更简单。先上图 : 1.层级结构: 小程序类似于app,以页面为单位,每个页面包含4个文件: .js .json .wxml .wxss 为后缀的文件,官方文_微信公小程序地图开发

Anaconda详细安装及使用教程(带图文)-程序员宅基地

文章浏览阅读10w+次,点赞1.4k次,收藏7.9k次。Anacond的介绍Anaconda指的是一个开源的Python发行版本,其包含了conda、Python等180多个科学包及其依赖项。因为包含了大量的科学包,Anaconda的下载文件比较大(约531 MB),如果只需要某些包,或者需要节省带宽或存储空间,也可以使用Miniconda这个较小的发行版(仅包含conda和Python)。Conda是一个开源的包、环境管理器,可以用于..._anaconda

JDK的动态代理(附带完整代码demo)_jdk动态代理demo-程序员宅基地

文章浏览阅读1.5k次。JDK的代理,实际上是对Java接口(Interface)的代理,而且只能对接口进行代理,达到的效果是,当调用指定接口的指定方法时,会执行特定的操作。例如,HelloService是一个接口,public String sayHello(String msg)是该接口的其中一个方法,当代码调用该接口的sayHello方法时,会执行规定好的动作。这里会有一个问题,HelloService接口的实现类是从哪里来的呢?这里HelloService接口的实现类是由代理动态生成的实现类。所以想为一个接口生成代理,需要_jdk动态代理demo

设计模式—组合与聚合详解_聚合是菱形空心吗-程序员宅基地

文章浏览阅读2.2k次。在UML类图中,聚合是空心菱形,组合是实心菱形。简单来说:组合的关系就像一个学生和他的各个器官,手、脚、鼻子、眼睛等器官组合成了一个学生,这些器官离开了学生这个个体,也就失去了意义,无法单独生存,因此,组合关系的类具有相同的生命周期,它们的联系更加紧密。而聚合就像一个班级有许多学生构成,学生离开了班级,作为一个个体仍然能够存活。聚合案例public ClassRoom{ public..._聚合是菱形空心吗

【备战金三银四】2022史上最全Android 面试题_2022 最全android面试题合集-程序员宅基地

文章浏览阅读499次。小编经历过这么多年的摸爬滚打,面试过也被面试过。现总结与归纳Android开发相关面试题:初级面试题:1、Activity启动模式有哪些,分别有什么不同?2、Service启动模式有哪些,对应的生命周期?IntentService呢?3、ContentProvider的作用,是否支持多线程和多进程4、Broadcast的注册方式,对应的生命周期是什么,有序和无序那种可以中断广播?5、AsyncTask的作用,如何使用(包括有哪些方法,能说出同步异步,能说出不同Android版本下的区别加分)6_2022 最全android面试题合集