SLAM后端优化中卡尔曼滤波的直观通俗解释
点击上方“小白学视觉”,选择加"星标"或“置顶”
重磅干货,第一时间送达
本文由知乎 郑纯然 授权转载
原文链接:
https://zhuanlan.zhihu.com/p/146593826
先讨论滤波器的概念,滤波的意思是,让机器人在某个正确位置上对应的概率越高越好。也就是可以理解为:把错误位置上的概率滤低,把正确位置处的概率滤高。
假设一个机器人小R在如下场景中出现,他刚开始不知道自己在哪(小R还没看到他眼前的门),因此他在这个场景中任何位置的概率是相等的。如果此时纵坐标为机器人小R在对应位置处的概率,横坐标表示各个位置,应该是一条均匀分布的直线。
突然,机器人看到了眼前这个门,这里假设机器人提前知道一共有三个门,因此小R现在知道自己可能在任意一个门前,即三个门分别对应着一个正态分布。此时的概率波形可以理解为先验概率。
小R继续向前走到第二个门前,他通过自己身上安装的里程计发现自己走了d个单位。
根据之前的概率分布,小R可以预测到,自己的位置应该向右平移了d个单位。那么可以将之前的概率分布向右平移d个单位,得到此时通过传感器得到的概率分布。此时的概率波形可以理解为似然概率。
小R突然发现自己看到了第二扇门前,仅根据当前的观测,小R知道自己在三个门前的概率相同,又可以得到之前的三个正态分布。根据传感器预测得到的分布和根据先验信息得到的分布得:
两个波形信号可以做个卷积融合得到:
这样小R在第二扇门处(正确位置)的概率就变大了,在其他位置处的概率就变小了,进而达到了滤波的目的。
以上即是普通滤波器的直观解释,同样地,可以类比到卡尔曼滤波上。
我们假设机器人的状态xk 为 即位置和速度。根据运动学公式可得:
向量化可得:
这样便引出了预测模型:
由式(1)可知,新的最优状态估计是根据上一时刻最优估计预测得到的,并加上已知外部控制量的修正(比如控制油门加速)。
由式(2)可知,新的不确定性由上一时刻不确定性预测得到,并加上外部环境的干扰。这时我们对系统的变化有了模糊的估计,更新的状态(均值)和不确定性(协方差)分别如式(1)和(2),预测的过程相当于之前的波形向右平移d个单位的过程。
此时结合传感器数据会如何?再来谈谈测量模型,利用传感器我们可以猜到系统当前的状态,但由于传感器存在噪声,传感器读数(均值)和噪声(方差)为 这里有一点需要注意:传感器读数的单位和比例可能与式(1)中追踪的状态的单位和比例不一样。我们将用矩阵Ck 来为传感器建模,可将预测模型的均值和方差改为
这时我们已经得到了两个高斯分布,类似于之前机器人小R在当前时刻得到的两个分布,将两个高斯分布融合的过程网上有很多推导,这里不多赘述,直接利用结果。
先计算卡尔曼增益K,如式(3)。
然后计算后验概率的分布:
式中Zk 表示实际的观测, 表示预测,观测和预测的差异乘上卡尔曼增益 K,再加上原来的预测,得到融合后的结果。此过程类似于之前两个波形卷积。
得到的新的最优估计可以放到下一时刻不断迭代。
以上就是经典卡尔曼滤波器的五个公式,给出了线性高斯系统的最优无偏估计。我们可以用这些公式对任何线性系统建立精确的模型,对于非线性系统来说,我们使用扩展卡尔曼滤波,区别在于EKF多了一个把预测和测量部分进行线性化的过程。
此时再看这个高赞无公式推导的回答来回顾全局,一切豁然开朗:无公式直白解释卡尔曼滤波
假设你有两个传感器,测的是同一个信号。可是它们每次的读数都不太一样,怎么办?
取平均。
再假设你知道其中贵的那个传感器应该准一些,便宜的那个应该差一些。那有比取平均更好的办法吗?
加权平均。(乘卡尔曼增益 K)
怎么加权?假设两个传感器的误差都符合正态分布,假设你知道这两个正态分布的方差,用这两个方差值,(此处省略若干数学公式),你可以得到一个“最优”的权重。
接下来,重点来了:假设你只有一个传感器,但是你还有一个数学模型(指的是上文中的预测模型)。模型可以帮你算出一个值,但也不是那么准。怎么办?
把模型算出来的值,和传感器测出的值,(就像两个传感器那样),取加权平均。
OK,最后一点说明:你的模型其实只是一个步长的,也就是说,知道x(k),我可以求x(k+1)。问题是x(k)是多少呢?答案:x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。
于是迭代也有了。
这就是卡尔曼滤波。
(无公式)
利用滤波方法进行传感器融合方面的经典论文推荐:
[1] Bloesch M, Omari S, Hutter M, et al. Robust visual inertial odometry using a direct EKF-based approach[C]//2015 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2015: 298-304.
[2] Qiu X, Zhang H, Fu W, et al. Monocular Visual-Inertial Odometry with an Unbiased Linear System Model and Robust Feature Tracking Front-End[J]. Sensors, 2019, 19(8): 1941.
[3] Xiong X, Chen W, Liu Z, et al. DS-VIO: Robust and Efficient Stereo Visual Inertial Odometry based on Dual Stage EKF[J]. arXiv preprint arXiv:1905.00684, 2019.
[4] Bloesch M, Burri M, Omari S, et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback[J]. The International Journal of Robotics Research, 2017, 36(10): 1053-1072.
解决方法是看完文章后,顺手点下文末右下角的“在看” ,系统会认为我们的文章合你口味,以后发文章就会第一时间推送到你面前的,比心~
下载1:OpenCV-Contrib扩展模块中文版教程
在「小白学视觉」公众号后台回复:扩展模块中文教程,即可下载全网第一份OpenCV扩展模块教程中文版,涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。
下载2:Python视觉实战项目52讲
在「小白学视觉」公众号后台回复:Python视觉实战项目,即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目,助力快速学校计算机视觉。
下载3:OpenCV实战项目20讲
在「小白学视觉」公众号后台回复:OpenCV实战项目20讲,即可下载含有20个基于OpenCV实现20个实战项目,实现OpenCV学习进阶。
交流群
欢迎加入公众号读者群一起和同行交流,目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群(以后会逐渐细分),请扫描下面微信号加群,备注:”昵称+学校/公司+研究方向“,例如:”张三 + 上海交大 + 视觉SLAM“。请按照格式备注,否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告,否则会请出群,谢谢理解~
SLAM后端优化中卡尔曼滤波的直观通俗解释相关推荐
- SLAM后端优化之-核函数
1.核函数作用:保证每条边的误差不会大的没边,掩盖掉其他的边 在SLAM后端优化中,BA优化了所有的相机姿态和所有路标点,使用的最小化误差项作的二范数平方和作为目标函数:当我们的误差来源特别大的时候: ...
- 激光SLAM后端优化总结之卡尔曼滤波
激光SLAM后端优化总结之卡尔曼滤波 一.贝叶斯滤波 1.1 状态估计模型 1.2 公式推导 二.卡尔曼滤波(KF) 三.扩展卡尔曼滤波(EKF) 一.贝叶斯滤波 1.1 状态估计模型 1.2 公式推 ...
- 激光SLAM后端优化——雅克比矩阵推导
激光SLAM后端优化--雅克比矩阵推导 Jacobi Matrix Jacobi Matrix In the EKF system, the maintained state quantities i ...
- 在 VSLAM 的后端优化中的重投影误差的雅可比计算详细推导
对于相机位姿的变换可以通过旋转矩阵或者四元数进行表示,对于旋转矩阵的定义满足: R{∣R∣=1RRT=IR \begin{cases} |R| = 1 \\ RR^T = I\\ \end{cases ...
- BA(后端)优化与卡尔曼滤波的一些区别
今天这篇文章会写得深入一些,主要知识来自于业内多位大佬的实际落地与自身的思考. 主要涉及机器人状态估计与机器视觉,多传感器结合时,BA优化与卡尔曼滤波的差异及如何选择. 机器人状态估计中,大家应该总体 ...
- SVM中支持向量的通俗解释
SVM中的支持向量,最开始接触的时候没想明白为什么HingeLoss为0的点不考虑,明明这些才是足够优秀的点,后来想清楚,正是因为人家足够优秀,所以没有拿来划分的意义,要的就是那些离分界线最近的点.支 ...
- 拒绝“套话”,Android中基本概念的通俗解释
前言 我在学习Android开发的过程中,会看到网上有人问类似这样的问题:什么是SDK?so文件是干什么的? 于是下面回答洋洋洒洒一大片,仔细一看,发现是维基,百度出来的标准答案,很全面,很正确.我时 ...
- 后端优化1(SLAM十四讲ch10)-BA
概述 视觉里程计可以给出一个短时间内的轨迹和地图,但由于不可避免的误差累积,如果时间长了这个地图是不准确的.所以我们希望构建一个尺度.规模更大的优化问题,以考虑长时间内的最优轨迹和地图.实际当中考虑到 ...
- 视觉SLAM⑨后端Ⅰ(KF、EKF、非线性优化)
目录 9.0 本章内容 9.1 概述 9.1.1 状态估计的概率解释 9.1.2 线性系统和KF 9.1.3 非线性系统和EKF 9.1.4 EKF的讨论 9.2 BA与图优化 9.2.1 投影模型和 ...
最新文章
- windows cmd color颜色设置
- 两个表循环的复杂度分析 征集
- 在aspx页面向iframe中post参数
- iis php网站500错误原因_因为曾经错误安装过PHP5.2而导致IIS7无法正常工作,显示500错误提示,大家帮忙看看!...
- 文件共享服务器地址格式,文件共享服务器配置
- dns 320 linux,linux 下 dns服务器(三 配置篇)
- app android切图工具,2018最强手机APP切图规范指南和切片要求
- .net core EPPlus npoi_在.NET中隐藏带有只读Web路径的Web shell
- 没有与这些操作数【】匹配的运算符_Maxima软件-7运算符(翻译)
- Windows查询端口的进程
- Linux LVM的PV操作
- 网易云音乐java_网易云音乐被下架?掌握这几步,你也能做出网易云!
- Excel如何根据出生日期计算年龄
- Oracle基本介绍与基本使用
- 359860-27-8,Biotin-PEG3-Amine增加了与生物素化合物共轭的分子的水溶性
- java发送图片邮件_使用javamail发送包含图片的html格式邮件详解
- poll,ppoll
- ikbc机械键盘打字出现重复_再也不怕半夜打字影响家人了——ikbc c104机械键盘评测...
- 【EMC基础篇①】噪声是什么
- Docker-Dockerfile学习