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语言编程方法,在学习其它的语 ...
随机推荐
- Paddle Release Note
Paddle Release Note 重要更新 飞桨paddle框架2.0.0版本有如下重要更新: 编程范式:默认开启动态图模式进行模型开发和训练,通过动转静的方式进行模型部署和训练加速.如果需要使 ...
- MinkowskiNonlinearities非线性
MinkowskiNonlinearities非线性 MinkowskiReLU class MinkowskiEngine.MinkowskiReLU(*args, **kwargs) __init ...
- 浅谈Gson和fastjson使用中的坑
相信大家在代码编写中都用过Gson和fastjson吧,用来进行 Java对象和json字符串之间的转换. 本篇文章就主要介绍博主在工作中使用这两款工具时遇到的坑和对应的解决办法. 觉得有用的可以点个 ...
- 剑指 Offer 07. 重建二叉树
链接:https://leetcode-cn.com/problems/zhong-jian-er-cha-shu-lcof/ 标签:树.递归 题目 输入某二叉树的前序遍历和中序遍历的结果,请重建该二 ...
- UiPath中恢复依赖项失败的解决方法
目录 序言 正文 什么是依赖包? 如何查看项目使用了哪些版本的依赖包? 一.项目内查看 二.查看项目的 JSON 文件 问题根源 解决方法 一.「等」字诀 二.切换网络环境(根治) 三.手动复制依赖包 ...
- 深入理解Java中的反射机制和使用原理!详细解析invoke方法的执行和使用
反射的概念 反射: Refelection,反射是Java的特征之一,允许运行中的Java程序获取自身信息,并可以操作类或者对象的内部属性 通过反射,可以在运行时获得程序或者程序中的每一个类型的成员活 ...
- 题解 P3232 [HNOI2013]游走
洛谷P3232[NOI2013]游走 题目描述 给定一个 n 个点 m 条边的无向连通图,顶点从 1 编号到 n,边从 1 编号到 m. 小 Z 在该图上进行随机游走,初始时小 Z 在 1 号顶点,每 ...
- Golang学习(用代码来学习) - 第三篇
type Books struct { title string author string subject string id int } /** 结构体的学习 */ func struct_tes ...
- [源码解析] 深度学习分布式训练框架 horovod (7) --- DistributedOptimizer
[源码解析] 深度学习分布式训练框架 horovod (7) --- DistributedOptimizer 目录 [源码解析] 深度学习分布式训练框架 horovod (7) --- Distri ...
- java变量及常量
变量 本质:就是代表一个"可操作的存储空间",空间位置是确定的,但是里面放置什么值不确定.我们可通过变量名来访问"对应的存储空间",从而操纵这个"存储 ...