rbpf-slam原理

基于rao-blackwellized PF-slam>>>将slam问题分解为先定位再mapping。
激光slam：激光里程计测量值即机器人的位姿估计值，sensor测量值即环境障碍物地表位标信息；
基于RBPF的slam算法：联合概率密度

RBPF算法实现过程：
1.采样阶段；对k时刻粒子集，PF根据激光odo运动模型计算提议分布，由此分布得k+1时刻粒子集，每个粒子得位姿表示了robot得一种运动位姿估计；
2.权重计算阶段；初始各粒子权值1/n，迭代更新后，权值大小为目标分布和提议分布的比值：

3.重采样阶段；根据重要性权重对粒子集重采样，舍低留高。一般的重要性采样中，在几次迭代后，大部分粒子权重变的很小，造成大量计算浪费在低权重粒子上，估计准确度变差，例子退化现象。

4.地图更新阶段；粒子根据激光雷达sensor的观测数据和激光odo的当前状态轨迹更新map中的每个特征