EKF—SLAM推导

转自   http://blog.youkuaiyun.com/qq_30159351/article/details/53408740

这是SLAM最传统的基础，是SLAM最原始的方法，虽然现在使用较少，但是还是有必要了解。

What’s Kalman Filter

这是一个贝叶斯滤波器，估计线性高斯模型，是对线性模型和高斯分布的优化方法。

高斯分布

首先，回顾一下高斯分布： 
 
高斯分布的一些性质： 
如果原变量为高斯分布，则边缘化和条件概率仍然满足高斯分布。 
 
边缘分布和条件分布的模型： 

---

卡尔曼滤波器的主要参数

卡尔曼滤波器假设x（paths）, z（observations）都为线性高斯的： 
 
主要参数： 
 
A是在没有命令的情况下，由于环境因素造成的机器人的位置移动。 
B是命令对机器人位置的改变 
C是地图和observations的对应关系，即两者的联系，描述。 
最后两个为噪声，是由于测量中的误差造成的。协方差分布为R, Q。

线性motion model和observation model

因为之前已经假设了x,z都是高斯分布的 
运动模型： 
 
观测模型： 
 
由此就可以使用第三节的贝叶斯滤波器公式： 

---

卡尔曼滤波器算法

 
2，3是prediction过程，4-7是correction过程。 
其实卡尔曼滤波就是在估计和测量中找到一个平衡。 
K为卡尔曼增益，就是通过这个变量来调节估计和预测的平衡。 
 
卡尔曼滤波是在假设高斯和线性动作和观测模型下进行的，但是现实中并不是这样的。

---

What’s Extended Kalman Filter

引入非线性模型： 
 
在线性高斯模型中： 
 
在非线性高斯模型中: 
 
通过局部线性来解决非线性的问题。

---

复习Jacobian矩阵

 
它相当于对一个非线性函数做了切平面。 

---

修改预测和校正过程

 
用图表示为： 
 
 
由此运动模型和观测模型修改为： 
 

---

Extended Kalman Filter算法

上一节主要讲解了EKF的基本原理，这一次主要关注如何将EKF算法应用在SLAM上。

EKF-SLAM

现在的问题就是解决下面这个概率分布的估计问题： 
 
阴影部分为未知 
 
这里我们需要确定均值和方差到底是什么？

假设在2D平面内，状态表示为下面这个式子： 
 
所以有均值和方差为： 
这么Map有n个landmark，所以是3+2n维的高斯分布 
 
简写为： 

---

EKF-SLAM的过程

• State prediction

 
机器人往前走一步，通过motion model可以估计机器人所在的位置，此时机器人位置的不确定性会增加（红色区域为机器人可能在的位置）。均值和方差绿色部分将需要更新。

例如之前提到的速度运动模型，可以有下面的预测 
 
相对应3+2n的维度上有： 
 
然后对于Gt有，因为移动还只是更新了位置，所以只需要更新均值的前三个变量x,y , 角度即可，方差更新左上角的3*3矩阵。 
 
Gxt的推导： 
 
- Measurement prediction and measurement

此时根据观测模型估计看到的landmark 
 
然后根据传感器去观测真实的landmark 
 
即： 
 
**至此EKF的2，3步的prediction过程就完成了 
总结一下：（速度运动模型的EKF的prediction过程分为下面这5步）** 

• Data associate

计算h(x)和z之间的差距，即预测的和实际测量中的误差。 

• Update

然后更新整个估计矩阵： 
 
可以看出，如果你的landmark非常多的时候，更新是非常耗时的。

例如： 
以激光为例，可以这样表示observations 
 
uj为landmark的位置，ut为机器人的位置。

 
第一个变量是机器人位置和landmark的距离， 
第二个变量是距离的平方， 
第三个变量是landmark的表示。

然后我们就需要计算Jacobian矩阵： 
 
low指只考虑一个landmark的情况： 
所以有： 
 
 
对应到高维，即只是更新了对应landmark的H 
 
H和h求出来了，这样校正过程就完成了。

总结：（用激光的校正过程如下）