SLAM基础算法(1):卡尔曼滤波
对于一个正在运动中的小车来说,如何准确的知道它所处的位置?
理论家说:我可以通过牛顿公式来计算!
实践家说:给它装个GPS不就得了!
好吧,你们说的听上去都很有道理,但我们到底该相信谁?
现实情况是:
理论家没有考虑到现实存在的摩擦力、空气阻力、时间测量误差等因素,算出来的结果存在较大误差;
实践家没有考虑GPS的测量存在较大误差。
这样一说,感觉两位半斤八两,都有误差,感觉谁都不可信。不过,我们还是要解决问题的嘛,能不能让这两位合作一下呢,理论联系实际嘛,马克思他老人家说的准没错。
我们知道,误差其实是一种噪声,那么是不是可以用滤波的方法呢?
感觉可以,我们把他俩的结果加起来,再除二嘛,均值滤波谁不会呢!但是,但是传感器的测量值(不一定是GPS)有时候简直会上天哪,这样简单粗暴肯定不靠谱,那有没有更牛X的方法呢?
-------------------------------------------------------------------------------------------------严肃的分隔线----------------------------------------------------------------------------------------
在刚刚提出的例子里,小车的状态向量可以表示为:
这时候,理论家提出,他可以预测k时刻的小车状态,并很难受的承认,这个预测是有误差的,假设这个误差服从正态分布:
给出误差的协方差矩阵
假设小车是匀速行驶,使用基本的运动学公式来表示,有:
即:
其中:
称为预测矩阵。
现实中,小车肯定不是匀速的,因为各种阻力的存在,需要时不时的给小车一个向前的力,才能使它继续向前行驶:
假设某个时刻给了小车一个向前的力,产生了一个向前的加速度a,那么:
即:
其中:
称为控制矩阵。
对于施加给小车的加速度a,它也称为一个向量,叫做控制向量,常用来表示,即有:
到这里,理论家的事情也差不多了,我们为他补上最后一步。在系统会中,可能会有其他不可预知的外力可能会干扰预测结果,我们需要把这一部分误差也加进来,假设它也是一个服从正态分布的误差,我们已经知道:
则有:
可以看出,是有上一步得来的外部不确定性误差的总和。
这时,实践家也给出了他买的山寨GPS的参数,知道了这个GPS的误差范围,那么有如下关系:
其中, 为观测向量,
为传感器与现实单位换算的转换矩阵(这里因为GPS输出单位与现实一致,就是1:1的关系),
为GPS的正态分布协方差矩阵。
OK,到现在为止,理论和实践的要素都准备完成,是时候把他们整在一起了,我们先来看一幅图片:
图中紫色的小车正在行走,理论家计算出的位置为绿色曲线(正态分布),实践家测量出的位置为蓝色曲线(也是正态分布),那么我们如果把这两个正态分布融合起来,是不是得出一个更“瘦”的曲线呢?
如上图红色所示,我们得出了融合后的曲线,这个曲线的方差要比另外两个要小了,至于为啥高斯分布和高斯分布融合起来还是高斯分布,请参考论文《Products and Convolutions of Gaussian Probability Density Functions》有详细的推导和论证,这里就不多说啦。
现在,我们有了两个不同的数据集,分别是预测的数据和测量的数据,我们用 来归一到相同的单位,则有:
稍微加以变形,不难得出:
设:
那么上面的公式变为:
代入:
为了得到最后的结果,把上面三个式子化简,最终得到:
总结一下,我们把卡尔曼滤波的过程一般分为两大部分,预测更新和测量更新:
到这里,了解了卡尔曼滤波大概的推理和机制,说白了,卡尔曼滤波就是通过不断的迭代,将测量结果不断收敛的一种方法。可能到这里我们还是不太明白卡尔曼滤波有啥牛X的,那我们接下来具体计算一下。
以小车的运动为例,为了简化运算假设它是匀速行驶,我们每隔1秒获取一次GPS的数据并算出相对位移,数据如下:
此时,由上述条件容易得知:
1) 由于是匀速行驶,所以不需要控制量,可以忽略;
2) 1秒一次的更新间隔,预测矩阵F,这里是预测因子为1;
3) GPS输出的测量数据已转换为和现实一致,故H=1;
4) 假设小车初速为0,方差为1;
5) 就上面说的,GPS的误差是R=1米
可以看出,滤波后的结果相比测量的结果,是比较收敛的,接近于小车匀速运动的现实情况。
References:
[1]"A New Approach to Linear Filtering and Prediction Problems" by R.E.Kalman,1960
[2]"Kalman filter" in wikipedia
[3]"How a Kalman filter works,in pictures" by Bzarg's blog
[4]《Probabilistic Robotics》
SLAM基础算法(1):卡尔曼滤波的更多相关文章
- PHP基础算法
1.首先来画个菱形玩玩,很多人学C时在书上都画过,咱们用PHP画下,画了一半. 思路:多少行for一次,然后在里面空格和星号for一次. <?php for($i=0;$i<=3;$i++ ...
- 10个经典的C语言面试基础算法及代码
10个经典的C语言面试基础算法及代码作者:码农网 – 小峰 原文地址:http://www.codeceo.com/article/10-c-interview-algorithm.html 算法是一 ...
- Java基础算法集50题
最近因为要准备实习,还有一个蓝桥杯的编程比赛,所以准备加强一下算法这块,然后百度了一下java基础算法,看到的都是那50套题,那就花了差不多三个晚自习的时间吧,大体看了一遍,做了其中的27道题,有一些 ...
- 贝叶斯公式由浅入深大讲解—AI基础算法入门
1 贝叶斯方法 长久以来,人们对一件事情发生或不发生的概率,只有固定的0和1,即要么发生,要么不发生,从来不会去考虑某件事情发生的概率有多大,不发生的概率又是多大.而且概率虽然未知,但最起码是一个确定 ...
- 贝叶斯公式由浅入深大讲解—AI基础算法入门【转】
本文转载自:https://www.cnblogs.com/zhoulujun/p/8893393.html 1 贝叶斯方法 长久以来,人们对一件事情发生或不发生的概率,只有固定的0和1,即要么发生, ...
- java入门学习(3)—循环,选择,基础算法,API概念
1.顺序结构:也就是顺着程序的前后关系,依次执行.2.选择分支:利用if..else , / switch(){case [ 这个必须是常量]:}; / if..else if….. ….else.. ...
- Java - 冒泡排序的基础算法(尚学堂第七章数组)
/** * 冒泡排序的基础算法 */ import java.util.Arrays; public class TestBubbleSort1 { public static void main(S ...
- c/c++面试总结---c语言基础算法总结2
c/c++面试总结---c语言基础算法总结2 算法是程序设计的灵魂,好的程序一定是根据合适的算法编程完成的.所有面试过程中重点在考察应聘者基础算法的掌握程度. 上一篇讲解了5中基础的算法,需要在面试之 ...
- c/c++面试指导---c语言基础算法总结1
c语言基础算法总结 1 初学者学习任何一门编程语言都必须要明确,重点是学习编程方法和编程思路,不是学习语法规则,语法规则是为编程实现提供服务和支持.所以只要认真的掌握了c语言编程方法,在学习其它的语 ...
随机推荐
- 腾讯 angel 3.0:高效处理模型
腾讯 angel 3.0:高效处理模型 紧跟华为宣布新的 AI 框架开源的消息,腾讯又带来了全新的全栈机器学习平台 angel3.0.新版本功能特性覆盖了机器学习的各个阶段,包括:特征工程.模型训练. ...
- CVPR2020:端到端学习三维点云的局部多视图描述符
CVPR2020:端到端学习三维点云的局部多视图描述符 End-to-End Learning Local Multi-View Descriptors for 3D Point Clouds 论文地 ...
- selenium-python元素定位技巧(一)
在python-selenium元素定位中,有很多小技巧,在此记录总结 技巧一.尽量不要用可见的文本去定位 尽量不要用可见的文本去定位(特别是支持国际化的软件-比如禅道),因为一旦切换语言后,使用该方 ...
- 【NX二次开发】体消参,移除体参数UF_MODL_delete_body_parms()
例子: 源码: extern DllExport void ufusr(char *param, int *returnCode, int rlen) { UF_initialize(); tag_t ...
- sql优化问题
一.分析阶段 一 般来说,在系统分析阶段往往有太多需要关注的地方,系统各种功能性.可用性.可靠性.安全性需求往往吸引了我们大部分的注意力,但是,我们必须注意,性能 是很重要的非功能性需求,必须根据系统 ...
- npm i安装命令中的-g -D -S的区别
-g为全局安装 -D 对模块进行局部安装,模块写入到 devDependencies 对象 用于开发阶段,开发时用到的工具等 -S 局部安装,不同的是模块写入到 dependencies对象 用于生产 ...
- SVN分支的创建与使用
首先放出右键菜单点击Branch/tag... 所示位置输入想新建分支的路径 然后ok就完成了 之后就是切换分支上传代码了 点击Switch... 在里面就可以切换分支了To path
- 20204107 孙嘉临《Python程序设计》实验一报告
课程:<python程序设计> 班级:2041 姓名:孙嘉临 学号:20204107 实验教师:王志强 实验日期:2021年4月12日 必修/选修:公选课 ##一.实验内容 1.熟悉Pyt ...
- python随机漫步
- pip安装setuptools_rust报错
公司项目中有主备CDN存在,由于阿里云以及腾讯云的预热功能不支持自动(一般是云函数),所以就根据云厂商给的脚本稍作更改,手动传入数据来进行预热. 由于之前部署在centos7.7系统python2.7 ...