转自 http://blog.csdn.net/qq_30159351/article/details/53408740
这是SLAM最传统的基础,是SLAM最原始的方法,虽然现在使用较少,但是还是有必要了解。
这是一个贝叶斯滤波器,估计线性高斯模型,是对线性模型和高斯分布的优化方法。
首先,回顾一下高斯分布: 高斯分布的一些性质: 如果原变量为高斯分布,则边缘化和条件概率仍然满足高斯分布。 边缘分布和条件分布的模型:
卡尔曼滤波器假设x(paths), z(observations)都为线性高斯的: 主要参数: A是在没有命令的情况下,由于环境因素造成的机器人的位置移动。 B是命令对机器人位置的改变 C是地图和observations的对应关系,即两者的联系,描述。 最后两个为噪声,是由于测量中的误差造成的。协方差分布为R, Q。
因为之前已经假设了x,z都是高斯分布的 运动模型: 观测模型: 由此就可以使用第三节的贝叶斯滤波器公式:
2,3是prediction过程,4-7是correction过程。 其实卡尔曼滤波就是在估计和测量中找到一个平衡。 K为卡尔曼增益,就是通过这个变量来调节估计和预测的平衡。 卡尔曼滤波是在假设高斯和线性动作和观测模型下进行的,但是现实中并不是这样的。
引入非线性模型: 在线性高斯模型中: 在非线性高斯模型中: 通过局部线性来解决非线性的问题。
它相当于对一个非线性函数做了切平面。
用图表示为: 由此运动模型和观测模型修改为:
上一节主要讲解了EKF的基本原理,这一次主要关注如何将EKF算法应用在SLAM上。
现在的问题就是解决下面这个概率分布的估计问题: 阴影部分为未知 这里我们需要确定均值和方差到底是什么?
假设在2D平面内,状态表示为下面这个式子: 所以有均值和方差为: 这么Map有n个landmark,所以是3+2n维的高斯分布 简写为:
机器人往前走一步,通过motion model可以估计机器人所在的位置,此时机器人位置的不确定性会增加(红色区域为机器人可能在的位置)。均值和方差绿色部分将需要更新。
例如之前提到的速度运动模型,可以有下面的预测 相对应3+2n的维度上有: 然后对于Gt有,因为移动还只是更新了位置,所以只需要更新均值的前三个变量x,y , 角度即可,方差更新左上角的3*3矩阵。 Gxt的推导: - Measurement prediction and measurement
此时根据观测模型估计看到的landmark 然后根据传感器去观测真实的landmark 即: **至此EKF的2,3步的prediction过程就完成了 总结一下:(速度运动模型的EKF的prediction过程分为下面这5步)**
计算h(x)和z之间的差距,即预测的和实际测量中的误差。
然后更新整个估计矩阵: 可以看出,如果你的landmark非常多的时候,更新是非常耗时的。
例如: 以激光为例,可以这样表示observations uj为landmark的位置,ut为机器人的位置。
第一个变量是机器人位置和landmark的距离, 第二个变量是距离的平方, 第三个变量是landmark的表示。
然后我们就需要计算Jacobian矩阵: low指只考虑一个landmark的情况: 所以有: 对应到高维,即只是更新了对应landmark的H H和h求出来了,这样校正过程就完成了。
总结:(用激光的校正过程如下)