根据概率论知识，SLAM工作可以划分为定位和建图两部分，一般是通过观测值$z_{1:t}$和里程计信息$u_{1:t-1}$进行定位得到$x_{1:t}$，然后根据位姿 $x_{1:t}$ 以及观测信息进行建图得到m。

$$P(x_{1:t}\ ,m\ | z_{1:t}\ , u_{1:t-1}\ ) = P(m\ |x_{1:t}\ , z_{1:t}) \cdot P(x_{1:t}\ | z_{1:t}\ ,\ u_{1:t-1}\ )$$

gmapping其实就是优化版的RBPF，先看RBPF的一般过程：

1. 时间t的粒子群 $X^ \left( i \right)_t$ 是 $X^ \left( i \right)_{t-1}$ 从提议分布中采样获得的，提议分布往常是里程计运动模型 $P(x_t\ |x_{t-1}\ , u_{t-1}\ )$
2. 粒子权重 $w^ \left( i \right)$ 是目标分布和提议分布的比，用来描述提议分布和目标分布的差别，越接近1说明提议分布越好
3. 重采样：只有部分粒子用于估计目标分布，重采样之后，所有粒子权重相同。 如果能直接从目标分布采样，所有粒子权重都相同，实际上只用1个粒子就行，或者说没有采样的概念了。因此，提议分布越差，粒子群的权重差别越大， $N_{eff}$ (粒子归一化权重的平方和的倒数)就越大。
4. 根据轨迹各个位姿 $X^ \left( i \right)_{1:t}$ 和各个观测 $Z^ \left( i \right)_{1:t}$ 估计地图

• 目标分布： 真实分布，如果考虑机器人的运动，那么机器人在t时刻位置的真实分布就是目标分布，但是这是不能得到的，只是理论上的分布，我们需要只能通过其他分布来得到该分布

• 提议分布: 使用提议分布来得到目标分布的真实情况。一般的，使用里程计模型来作为提议分布。但是改进的提议分布发现激光具有单峰的特性，方差更小，更适合作为提议分布，因此考虑激光和里程计一起作为改进的提议分布

gmappping的优化

优化提议分布

不仅考虑机器人的运动，而且考虑了最近的观测，使得提议分布更接近目标分布，从而减少了所需的粒子数。之前的提议分布是 $P(x_t\ |x_{t-1}\ , u_{t-1}\ )$，优化之后是 $P(x{_t}\ |x_{t-1}\ , \ u_{t-1}\ , \ m_{t-1}\ , \ z_t)$

如果只用运动模型做提议分布，粒子权重计算比较简单: $w^ \left( i \right)_t \propto w^ \left( i \right)_{t-1}\ \cdot p(z_t\ | m^ \left( i \right)_{t-1}\ , x^ \left( i \right)_t)$。 但是里程计提供位姿的不确定度比激光大得多，导致置信区间太大，似然又太小，各个粒子的权重差别很大，实际上有意义的只有一小部分，结果就需要很多粒子。

相比之下，雷达观测的置信区间小，似然大，所需粒子就少了很多。所以可以把观测加入到提议分布优化之后，结果粒子权重公式为公式 13

因为观测似然函数的不确定形态，提议分布没有闭式解，所以方法是用采样对提议分布做估计。

减少重采样

RBPF中，粒子要覆盖里程计状态的全部空间，但只有一部分粒子是接近目标分布的，粒子权重的差别大，所以需要频繁重采样来丢弃权重小的粒子，保留权重大的粒子，同时又造成了另一个弊端：粒子退化。

选择性重采样，当 $N_{eff}$ 小于粒子数一半时，说明粒子分布和真实分布差距很大，某些粒子离真实值近，某些远，执行重采样。

GMapping依靠粒子的多样性， 回环时消除累积的误差。起始位置需要选择特征较为丰富的地方， 这样能在回环时提高正确粒子权重。

1. 使用scan-matcher估计似然函数的置信区间
2. 对置信区间采样，对采样点进行评价
3. 对每个粒子计算，计算K个粒子的期望方差，计算时考虑了里程计运动信息，公式15和16

算法复杂度

主要影响因素是计算提议分布，更新地图，计算权重，检测是否需要重采样，重采样。 结合源码，最大的影响参数是particles, 其次是resampleThreshold, map_update_interval, throttle_scans, lskip

$N_{eff}$

1. 机器人经过未知区域时，$N_{eff}$ 下降很慢
2. 机器人经过已知区域时，每个粒子都在各自的地图中保持定位，权重都相差不大
3. 当closing loop时， $N_{eff}$ 会显著下降，有些粒子能保持定位，有些则丢失了定位，权重相差很大，此时执行重采样

缺陷

从论文和源码上看，gmapping算法有以下缺陷:

1. scan matcher 计算的是观测似然函数的置信区间，但函数可能是多峰的，比如closing loop的时候，这就造成了问题，粒子群的权重会有很大波动， $N_{eff}$ 会显著下降，当低于resample_Threshold时，就会执行重采样，然后恢复 $N_{eff}$ 最大值． $N_{eff}$ 之所以大幅下降，就是因为观测似然出现多峰，使得提议分布与目标分布差别太大，导致粒子权重减小．因此不适用于多闭环的环境

2. 遇到长走廊或很空旷的环境，雷达数据没有什么特征，比如测距都是最大值，造成位姿在走廊方向的严重不确定，此时里程计精度就很重要了，有利于粒子群的收敛。如果里程计精度不好，增加粒子数也可以解决这个问题。

3. drawFromMotion函数是一个十分粗糙的运动模型，只是简单的矢量加减运算。相比于《概率机器人》中提到的速度模型和里程计模型，有很多方面都没有考虑，精度上可能有折扣。

4. 随着场景增大所需的粒子也增加，内存和计算量都会增加，实际中建的地图不到几千平米时就会发生错误

5. 重采样的粒子索引选取用的是轮盘赌算法，有些论文提出了一些更好的算法

6. 机器人的轨迹估计与真实的轨迹有一定差异，轨迹出现较大畸变的地方导致了＂假墙＂．这是粒子多样性降低造成的，需要增加粒子数
   

7. 源码中，用户设置的miniScore参数过大时(超过170)，scan match失败，转而使用里程计进行位姿估计．但这样就更不准了，而且gmapping算法里没有提示

综合来看，Gmapping适合的环境有以下特点: 小场景、closing loop比较少、没有长走廊或很空旷的环境、雷达精度高、机器人主机配置较高。此时相比Cartographer，Gmapping不需要太多的粒子并且没有回环检测，因此计算量小于Cartographer，精度没有差太多。

参考：
各种激光slam: 开源代码的比较
2D激光SLAM算法优劣对比