算法流程

• 初始化

首先定义两个粒子集，开始都是空集，一个用于重采样缓冲的set_a，一个是最终需要的set_b，对每份粒子集合创建kdtree。算法的目的是用粒子集合来表示置信度。

先在AMCL里给定一个初始位姿值和协方差矩阵，粒子数为M。一开始，每一个粒子的权重值都是1/M，位姿的分布是一个高斯分布，每个粒子的初始位姿记做(x,y,theta)，其均值是AMCL的初始位姿，方差是从协方差矩阵获得的。将每个粒子插入到kdtree中，对粒子滤波器的粒子集进行聚类。

• 运动模型

概率模型是 $p(x_t\ | \ x_{t-1},\ u_t)$， 使用里程计模型进行估计。对每一个粒子更新位姿。位姿是从运动模型中采样获得的，机器人从t-1时刻运动到t时刻，获得相对运动参数rot1,trans,rot2

最终，对于每个粒子的位姿， x加上相对平移量在x轴的分量(乘以cos), y加上相对平移量在y轴的分量， theta加上两次旋转的参数。这些分量除了跟rot1,trans,rot2有关，还跟AMCL中的四个运动噪声参数alpha1~alpha4有关

• 测量模型

概率模型是 $p(z_t\ | \ x_t)$，使用似然域模型，用于更新每一个粒子的权重。

似然域测量模型认为测量的结果包含一个高斯分布噪声和随机噪声。激光测量距离（x,y）变换到map坐标系。找到距离激光点最近的障碍，查表获得它们的最近距离dist。这个dist也是高斯噪声的均值，所以dist越大，打到障碍物的可能性越小，则粒子不可信，权重越小。

$\omega = \prod_{i=1}^N \left\{ z_{hit}*prob(dist_i, \sigma_{hit})+\frac{z_{rand}}{z_{max}} \right\}\ \qquad \qquad$ N为一帧激光的点数

似然域测量模型比较理想，因此在场景有些变化，和动态人存在干扰的情况，估计的pose精度会下降。放大测量方差或许会有效果，最好要是能引入类似非线性优化中的核函数（损失函数）或许不错。

• 失效恢复的准备

把每个粒子添加到临时粒子集中，也就是说set_a内的粒子的权重是不同的，更新粒子集的平均权重。根据alpha_slow和alpha_fast计算短期和长期的似然平均

• 重采样

从缓冲的粒子集中替换所有粒子，抽取粒子的概率取决于粒子的权重，重采样之后，粒子的数量不变，权重较小的粒子将被过滤掉，权重较大的粒子将被复制

上面的算法还有两个问题： 1. 粒子重采样的退化问题；2. 固定的粒子群导致无用的计算，影响效率。
每个粒子集合中包含的粒子数量很大，粒子的权重即使在多次重采样之后还是很小，导致位置上相邻的粒子的权重差值不大，从而最后输出的预测位姿不准确。

因此AMCL加入了新的结构体 Cluster，表示相邻粒子的集合，具有权重与位姿等属性，与粒子相似。而其位姿是通过该 Cluster中包含的粒子的位姿加权平均得到。开始时 Cluster的数量与粒子数量相等，即每一个 Cluster中只有一个粒子。当粒子在地图上产生聚合后，Cluster的数量就会下降，此时选取权重最大的 Cluster 作为输出结果。

使用KD树存储粒子信息，并且进行聚类分析。树的叶子节点是每一个粒子，而非叶子节点则保存的是该节点的分叉判断的标准。生成树之后，进行粒子聚类，循环遍历每一个叶子节点，在叶子节点的key值的 ±1 范围内的粒子都归为一类。也就是手，每个粒子周围27个粒子若有相连的，就将它们全部连起来，形成聚类。

处理每一个粒子，先生成一个(0,1)的随机数rand，如果 rand< w_diff = (1 - w_fast / w_slow)，那么当前粒子的位姿是随机值。

把上面找出的所有粒子添加到set_b，set_b的粒子个数是M，多数粒子是原来set_a中权重比较大的粒子，而且可能重复。让set_b中的粒子权重全是1 / M

AMCL定位不准的3个依据:

1. 协方差矩阵的对角线元素小于阈值
2. 当前粒子群的个数大于阈值
3. 收敛标志 pf->converged 为false

AMCL用到的算法:

• sample_motion_model_odometry
• likelihood_field_range_finder_model
• Augmented_MCL
• KLD_Sampling_MCL