SLAM笔记五

上一节主要讲解了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求出来了，这样校正过程就完成了。

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