之前记录着就忘记录了，现在补上，欢迎大家一起交流啊~
（二）gmapping原理解析
A、先验知识
- 条件概率公式：
上图中红色为A事件，蓝色为B时间，紫色为A、B事件同时发生的概率，P(A|B)表示B发生的条件下A发生的概率：
- 全概率公式：
如上图，对于样本空间A，由A1...A7个事件组成，且A1...A7事件两两互斥，即任意两个事件的交集为空集；则对于任意事件B有：
对于离散域和连续域的全概率公式如下：
举例：小明要去上班，有三条路，选择每条路的概率为P(L1)=0.5，P(L2)=0.3，P(L3)=0,2=0.2;每条路不会堵车的概率P(C1)=0.2，P(C2)=0.4，P(C3)=0.7，问小明上班不会堵车的概率?
解：不堵车的事件为C；样本空间为L1，L2，L3；题目的P(C1)=P(C|L1)，P(C2)=P(C|L2)，P(C3)=P(C|L3)
所以：P(C)=P(L1)*P(C|L1)+P(L2)=P(C|L2)+P(L3)*P(C|L3)=0.36
- 贝叶斯公式：
贝叶斯公式其实是由条件概率和全概率公式可以推导出来的，全概率公式是求B事件在A样本空间的概率，而贝叶斯就是已经知道A事件发生了，求样本空间中Ai事件发生的概率，如下图：
公式如下：
这里公式A、B和图中的A、B是相反的，画图画错了（....）
其中A事件在样本空间B中发生的概率可由全概率公式得到：
简单记忆：
B、gmapping系统架构
我们在gmapping原理及代码解析（一）对代码进行了简单的解析，现在来看看gmapping各个流程里做了什么：
（1）传感器数据输入
gmapping需要激光雷达和里程计数据，接收到传感器数据后进入回调函数，进入后面的算法处理
（2）里程计更新
里程计其实就是运动模型，根据t-1时刻预测机器人t时刻的位姿
（3）扫描匹配
扫描匹配是根据当前帧激光雷达数据求当前最佳位姿，如果扫描匹配算法失败，则后续的权重更新则采用默认的似然估计
（4）proposal分布
gmapping里面，是混合了运动模型和观测模型的proposal分布的，根据上一步扫描匹配获得的最佳坐标，围绕该坐标取若干位置样本（距离差值小于某阈值）计算均值与方差，使得当前粒子位置满足该均值方差的高斯分布
（5）计算权重
对各个粒子的权重进行更新，更新之后还需进行归一化操作。注意：重采样前更新过一次，重采样后又更新过一次
（6）重采样
使用Neff判断是否进行重采样，重采样频率越高，粒子退化越严重，即粒子多样性降低，导致建图精确度降低，有必要设定一个判定值改善粒子退化问题
（7）地图更新
在ros中进行地图更新。先得到最优的粒子（使用权重和 weightSum判断 ），得到机器人最优轨迹，地图膨胀更新
C、gmapping改进
- SLAM数学表达:
这里强调一下。gmapping是基于粒子滤波实现的SLAM算法。
- 粒子滤波缺点：
（1）当proposa分布较差时，我们采样的粒子不能很好的拟合后验分布，我们有两种解决方法：一是增加粒子的数目，但是粒子数增大会使得计算量增大，复杂度增大，耗内存；二是改善proposal分布，用较少的粒子数就可以准确表示后验（你也可以说是目标分布）。
（2）粒子退化问题。粒子退化是指正确的粒子被丢弃和粒子多样性降低，造成这样的情况主要有两个原因：一是当环境比较相似或者测量噪声较大的时候，计算权重时正确粒子的权重会减小而错误的粒子权重增大，而重采样步骤是根据权重来的，会导致正确的粒子被丢弃；二是频繁的重采样会加速粒子多样性减小的速率，重采样其实就是权重大的粒子多复制几个，这样迭代多次后，后面就只会剩下刚开始权重比较大粒子的副本了。
gmapping就针对上面的缺点做了改进：（1）改善proposal分布；（2）自适应重采样，减少重采样的次数。
（1）改善proposal分布
因为里程计模型便于计算，所以很多时候会使用里程计作为proposal分布。但是当激光雷达的信息比较可靠的时候，激光雷达的匹配会比里程计要准确的多，即上图表示的分布的方差会比里程计分布的方差小的多；考虑加入最近一帧激光雷达观测数据，改善proposal分布，将proposal分布限制到较小的有效区域，再采样
- 加入观测数据的proposal分布：
上图中的（1）式是由贝叶斯公式展开得到的。
proposal分布改变了，权重是后验分布和proposal分布的比值，权重的迭代公式自然也就改变了，我们接下来看看权重的计算公式是怎么改变的：
（1）proposal分布改善前，没有加入观测数据的权重计算：
（2）proposal改善后，考虑最近一帧观测数据的权重计算：
上图中的（12）-->（13）应用了全概率公式；
改善proposal分布的原理基本就结束了，接下来就是该怎么去计算/获得proposal分布了？
在gmapping的流程如下：
从上面的流程图可知，我们是在scan-match成功得到位姿x^后，在x^位姿附近领域类采样K个位姿来近似高斯分布，近似的这个高斯分布作为改善的proposal分布；要是匹配不成功就依旧用里程计模型来作为proposal分布。
用高斯分布来模拟proposal分布步骤如下：
（1）scan-match，根据最大似然估计的方法计算的得到局部极值
（2）我们认为scan-match得到的位姿离高斯分布的均值比较近，所以在该位姿领域内采集K个位姿
（3）对K个位姿进行打分，计算高斯分布的均值和方差
其中归一化因子：
所以改善后的proposal分布可以表示为：
根据这个proposal分布，权重的计算可以变为：
考虑在scan-match邻域内激光雷达观测模型占据主导地位，忽略掉由运动模型推导的位姿，我们可以简单记忆权重计算如下：
（2）自适应采样
设定一个值来表示目标分布和proposal分布的差异性：
权重是目标分布和建议分布的比值，我们可以这样理解这个Neff,如果我们直接从目标分布中采样来近似你粒子集，那么计算得到的权重将会相等，但是如果目标分布和建议分布的相似性越差，那么权重的方差也就会越大，所以这个Neff值可以表示粒子集是否接近目标分布的度量值。当NEeff的值越大，说明差异性很小，不进行重采样；当Neff的值很小时，说明差异性很大，需要进行重采样。这样就可以有效的减少了重采样的次数，减缓粒子多样性的缺失。
D、gmapping的粒子滤波实现
- 已知数据：
- Scan-match和采样：
- 计算高斯分布：
- 从高斯分布中采样和更新权重：
- 在已知位姿的情况下更新权重和粒子集：
- 自适应重采样：
- 这里的T=N/2,N为粒子数
参考：
（1）GMapping原理分析
（2）gmapping论文：G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid mapping with rao-blackwellized particle filters,” IEEE Transactions on Robotics, vol. 23, no. 1, pp. 34–46, 2007