AMCL 原理解读
AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位，A也可以理解为augmented，，源于MCL算法的一种增强，是机器人在二维移动过程中概率定位系统，采用粒子滤波器来跟踪已经知道的地图中机器人位姿，对于大范围的局部定位问题工作良好。对机器人的定位是非常重要的，因为若无法正确定位机器人当前位置，那么基于错误的起始点来进行后面规划的到达目的地的路径必定也是错误的。
一. 基础概念
AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。（说白了就是2D的概率定位系统，输入激光雷达数据、里程计数据，输出机器人在地图中的位姿。用的是AMCL--自适应蒙特卡洛定位方法，这个方法是在已知地图中使用粒子滤波方法得到位姿的。）
如下图所示，如果里程计没有误差，完美的情况下，我们可以直接使用里程计信息（上半图）推算出机器人（base_frame）相对里程计坐标系的位置。但现实情况，里程计存在漂移以及无法忽略的累计误差，所以AMCL采用下半图的方法，即先根据里程计信息初步定位base_frame，然后通过测量模型得到base_frame相对于map_frame（全局地图坐标系），也就知道了机器人在地图中的位姿。（注意，这里虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为里程计的漂移。）
里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。
- 里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
- amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。
补充知识：
坐标系变换
上述两种定位实现中，机器人坐标系一般使用机器人模型中的根坐标系(base_link 或 base_footprint)，里程计定位时，父级坐标系一般称之为 odom，如果通过传感器定位，父级参考系一般称之为 map。当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中"单继承"的原则的，所以，一般会将转换关系设置为: map -> odom -> base_link 或 base_footprint。
激光雷达自主定位：
PoseArray可显示粒子情况。需要注意的是，需要在fixed frame :map下进行初始位姿的发布。此时貌似就能够通过前后左右自主定位（但可能是由于初始粒子聚在一起定位过程十分缓慢）。
一些易混淆的概念：
1.粒子滤波和蒙特卡洛
蒙特卡洛：是一种思想或方法。举例：一个矩形里面有个不规则形状，怎么计算不规则形状的面积？不好算。但我们可以近似。拿一堆豆子，均匀的撒在矩形上，然后统计不规则形状里的豆子的个数和剩余地方的豆子个数。矩形面积知道的呀，所以就通过估计得到了不规则形状的面积。拿机器人定位来讲，它处在地图中的任何一个位置都有可能，这种情况我们怎么表达一个位置的置信度呢？我们也使用粒子，哪里的粒子多，就代表机器人在哪里的可能性高。
粒子滤波：粒子数代表某个东西的可能性高低。通过某种评价方法（评价这个东西的可能性），改变粒子的分布情况。比如在机器人定位中，某个粒子A，我觉得这个粒子在这个坐标（比如这个坐标就属于之前说的“这个东西”）的可能性很高，那么我给他打高分。下次重新安排所有的粒子的位置的时候，就在这个位置附近多安排一些。这样多来几轮，粒子就都集中到可能性高的位置去了。
2.重要性采样
就像转盘抽奖一样，原本分数高（我们给它打分）的粒子，它在转盘上对应的面积就大。原本有100个粒子，那下次我就转100次，转到什么就取个对应的粒子。这样多重复几次，仍然是100个粒子，但是分数高的粒子越来越多了，它们代表的东西（比如位姿）几乎是一样的。
3.机器人绑架
举例，机器人突然被抱走，放到了另外一个地方。类似这种情况。
4.自适应蒙特卡洛
自适应体现在：
1 解决了机器人绑架问题，它会在发现粒子们的平均分数突然降低了（意味着正确的粒子在某次迭代中被抛弃了）的时候，在全局再重新的撒一些粒子。
2 解决了粒子数固定的问题，因为有时候当机器人定位差不多得到了的时候，比如这些粒子都集中在一块了，还要维持这么多的粒子没必要，这个时候粒子数可以少一点了。
5.KLD采样（库尔贝克-莱布勒散度Kullback-Leibler Divergence）
就是为了控制上述粒子数冗余而设计的。比如在栅格地图中，看粒子占了多少栅格。占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些。占得少，说明粒子都已经集中了，那就将上限设低，采样到这个数就行了。
KLD采样是一种计算两个概率分布之间差异的方法。KLD采样背后的思想就是根据基于采样近似质量的统计界限来确定粒子数量。 更特殊的，在粒子滤波器的每次迭代过程中，KLD采样以概率1−σ来确定样本数量，真实的后验概率与基于采样的近似分布之间的差异小于ε。
6.粒子滤波和蒙特卡洛
蒙特卡洛：是一种思想或方法。举例：一个矩形里面有个不规则形状，怎么计算不规则形状的面积？不好算。但我们可以近似。拿一堆豆子，均匀的撒在矩形上，然后统计不规则形状里的豆子的个数和剩余地方的豆子个数。矩形面积知道的呀，所以就通过估计得到了不规则形状的面积。拿机器人定位来讲，它处在地图中的任何一个位置都有可能，这种情况我们怎么表达一个位置的置信度呢？我们也使用粒子，哪里的粒子多，就代表机器人在哪里的可能性高。
粒子滤波：粒子数代表某个东西的可能性高低。通过某种评价方法（评价这个东西的可能性），改变粒子的分布情况。比如在机器人定位中，某个粒子A，我觉得这个粒子在这个坐标（比如这个坐标就属于之前说的“这个东西”）的可能性很高，那么我给他打高分。下次重新安排所有的粒子的位置的时候，就在这个位置附近多安排一些。这样多来几轮，粒子就都集中到可能性高的位置去了。
7.测距仪的似然域(likelihood field）
主要思想是首先将传感器扫描到的终点z_t映射到地图的全局坐标空间。假定三种噪声和不确定性的来源
1.测量噪声。由测量过程引起的噪声使用高斯进行建模。在x-y空间，它涉及寻找地图上最近的障碍物。
2.测量失败，
3.无法解释的随机测量
将障碍物检测的似然描述成全局x-y坐标的函数，叫作似然域。
参考链接：https://zhuanlan.zhihu.com/p/70682953#
二.MCL 蒙特卡洛定位算法
首先介绍粒子滤波
粒子滤波（particle filter）是贝叶斯滤波的一种非参数体现，主要思想是用一系列从后验得到的随机状态采样表示后验，我的理解是相当于起初会有有限个不同的样本点，假定他们的位置是是服从其状态更新或预测概率的，计算每个样本在当前时刻的测量概率，根据每个点的测量概率，从中在挑选一定数量的样本点，通过展现这一群点的分布情况来描述其当前时刻的置信度，其算法伪代码如下：
其中
,状态预测部分简单的说是第一个for循环，而测量更新部分是第二个for循环，算法输入的部分包括前一个时刻的粒子集、当前的控制及测量，输出是当前时刻的粒子集。其推导过程书上有些，程序第四行
，这里
，而函数g相当于时机器人自身的控制函数。
AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位 ，源于MCL算法的一种升华，一种提高。那么为什么要从MCL上升至AMCL呢？首先应该了解MCL的算法原理 。
粒子滤波很粗浅的说就是一开始在地图空间很均匀的撒一把粒子，然后通过获取机器人的motion来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对。使用每个粒子所处位置模拟一个传感器信息跟观察到的传感器信息（一般是激光）作对比，从而赋给每个粒子一个概率。之后根据生成的概率来重新生成粒子，概率越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人的确切位置也就被推算出来了。
MCL VS EKF
MCL 的计算步骤
1.随机生成一堆粒子
粒子可以有位置、航向和/或任何其他需要估计的状态变量。每个都有一个权重（概率），指示它与系统的实际状态匹配的可能性。用相同的权重初始化每个。
2.预测粒子的下一个状态
根据预测真实系统的行为方式移动粒子。
3.更新
根据测量结果更新粒子的权重。与测量值非常匹配的粒子的权重高于与测量值不匹配的粒子。
4.重新取样
丢弃极不可能的粒子，并用更可能的粒子的副本替换它们。
5.计算估算
可选地，计算粒子集的加权平均值和协方差以获得状态估计。
粒子滤波的基本步骤为上面所述的5步，其本质是使用一组有限的加权随机样本（粒子）来近似表征任意状态的后验概率密度。粒子滤波的优势在于对复杂问题的求解上，比如高度的非线性、非高斯动态系统的状态递推估计或概率推理问题。
伪代码：
具体分析下这5步：
第一步：随机生成M个粒子（这些粒子都是有算计的方向和位置）
第二步：根据小车的运动参数(里程计，速度等)来估算每一个粒子基于xt-1 的 xt的预测位置。这里的预测与KF的算法相同。
第三步：根据传感器的测量数据，计算出每一个粒子的权重。（权重计算方式有很多，简单的理解可以认为是和真实传感器测量值之间的差别大小，比如说当前的一个例子的预测位置和测量目标之前的距离和 zt差别大， 越大权重越小，小的权重说明距离真实的位置）。随后更新每一个粒子的状态。
第四步：重采样，从所有粒子中根据新的权重值获取M个粒子。
之后不断重复上面的4步来实现定位的效果。
通过一个实例来感觉下：
a)图， 我们看到机器人初始在一个门的位置，但是这时候机器人并不知道它自己在哪，这时候我们初始化所有的粒子，并且所有粒子的权重都是一样的。
b)图，这时候机器人通过传感器得知面前是一个门，这时候可以看到和门比较近的粒子的权重都提高了。
c)图，进行重采样之后，机器人移动后，我们可以看到采样后的粒子都集中在原来靠近门的这些粒子点。就是图中黑色密集的位置。
继续看D)图，这时候机器人重新感知，得知面前还是一个门，这时候，原来采样的靠近门的粒子权重变大。从图中已经看到主要的采样点已经集中在第二个门的位置了，机器人现在就知道自己的位置是在第二个门了。
之后在e)图中，重采样，再次移动机器人，就可以知道机器人这次移动到的位置在中间了。
最后看下实例图，感受下粒子滤波的定位：
重采样的方式有很多，计算权重的方式也是。
参考：定位算法 -- MCL蒙特卡洛（粒子）滤波 - 知乎 (zhihu.com)
蒙特卡洛定位算法
蒙特卡洛定位适用于局部定位和全局定位两类问题，尽管它相对的年轻，但是已经成为定位领域中的主流算法，如下所示为蒙特卡洛定位算法：
通过把合适的概率运动和感知模型代入到粒子滤波算法中得到，使用M个粒子的集合 
\表示置信度
，初始置信度由先验分布随机产生的M个这样的粒子得到。算法第4行使用运动模型采样，以当前置信度为起点使用粒子，第5行使用测量模型以确定粒子的重要性权值（这两个模型介绍见后面）通过增加粒子总数M能提高定位的近似精度。
MCL以目前的形式解决了全局定位问题，但无法从机器人绑架或全局定位失败中恢复过来。当机器人位置被获取时，其他地方的不正确粒子会逐渐消失。在某种程度上，粒子只能“幸存”在一个单一的姿势附近，如果这个姿势恰好不正确，算法就无法恢复。
这个问题是非常重要的，实际上任何随机算法，就像蒙特卡洛算法，在重采样步骤中可能意外的丢弃所有正确位姿附近的粒子。当粒子数很小（M=50）。并且，当粒子扩散到整个比较大的体积（如全局定位过程）时，这个问题的重要性就能充分的显示。当然这个问题已经解决了，AMCL的提出正是对这个问题的一个解决方法。AMCL算法在机器人遭到绑架的时候，会随机的注入粒子（injection of random particles）其思想是增加随机粒子到粒子集合，从而在运动模型中产生一些随机状态。相对MCL算法，AMCL算法正是多了一个“A”” ,那么这个"A"是什么？“”A“”是adaptive 自适应的缩写，但是个人觉得将"A"理解为Augmented（增加的）更为合适。
amcl重实际的处理流程跟上面的伪代码不太一样。稍微解释下上面的伪代码。
函数输入机器人上一个状态，运动，测量和粒子数。利用运动采样模型生成M个服从给定分布的随机粒子X，利用测量模型为每个粒子评估产生测量zt的概率Pzt，并把X和Pzt组合到一起存放。
第8行开始筛选粒子，筛选的规则是，在上面产生的M个粒子中选出M个粒子，每次从中按概率Pzt去选择一个粒子。将产生的M的新粒子作为结果输出。
实际的流程不太一样，但是思想基本一致。流程如下：
1、获取机器人当前位姿，计算移动增量delta，判断机器人移动调节是否满足
2、根据上一个位姿Xt-1和移动增量delta，用运动采样模型产生（在原有粒子基础上按给定分布生成）若干数量（sample-count个）的粒子，也就是生成若干个当前状态的可能位姿。
3、将激光雷达数据从激光坐标系换算到baselink坐标系，使用激光传感器模型为每一个粒子计算在其状态下获得测量Zt的概率，并计算总概率totalweight。
4、上面已经获得了伪代码中的粒子和概率组合，就进行采样（可设定若干次数据更新采样一次）。采样后粒子数量不变，并重置所有粒子的概率为平均值。（1/sample-count）
5、对粒子进行聚类，计算每个cluster的位姿均值、概率（权重）均值、协方差等参数 （实际中此步骤和上一步是同时进行的）
6、获取平均概率最大的那一个聚类，认为是粒子滤波所得的最终机器人位姿。根据此位姿（机器人在MAP坐标系下的位置），修正ODOM坐标系下机器人的位姿（编码器累计值计算得到的机器人位姿）。完成一个PF滤波周期。
其实，代码中采用的重采样方法很简单，原理依然是，按照每个粒子的权重对粒子进行采样。在重采样过程中，我们还需要提起机器人绑架问题。由于粒子聚集后，万一机器人定位是错误的，或者机器人被搬动，粒子无法聚集到正确的范围内。所以还需要在定位效果不好的情况下全局随机的粒子，来模拟初始化时的情形。
稍后我看再看MCL和AMCL的区别，我们先看一下AMCL具体是怎么执行的：
三. AMCL 蒙特卡洛定位算法自适应变种
AMCL算法在机器人遭到绑架的时候，会随机注入粒子（injection of random particles），增加粒子的方法引起两个问题，一是每次算法迭代中应该增加多少粒子，二是从那种分布产生这些粒子。
解决第一个问题可通过监控传感器测量的概率来评估增加粒子，即式(1)
并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式
这个接近上式中的期望概率。
解决第二个问题可以根据均匀分布在位姿空间产生粒子，用当前观测值加权得到这些粒子。如下给出增加随机粒子的蒙特卡洛定位算法自适应变种（AMCL）：
与MCL相比，这个算法跟踪式(1)的似然值的短期与长期均值，整体框架与MCL相同，但在第八行中给出了经验测量似然，并在第10、11行维持短期和长期似然平均，算法要求
，参数
和
分别估计长期和短期平均的指数滤波器的衰减率。算法的关键在第13行，重采样过程中，随机采样以以下式 概率增加
否则重采样以MCL相同的方式进行，即根据式( 1.3 ) (1.3)(1.3)如果短期似然优于长期似然，则算法将判断不增加随机采样，否则的话则按两者之比的比例增加随机采样，以这种方式可抵消瞬时传感器噪声带来的定位误差。
（1）MCL算法和AMCL算法的区别：
AMCL算法增加了短期和长期的指数滤波器衰减率αslow，αfast，换句话说MCL中αslow，αfast为0，AMCL中的不为0。
（2）四个参数的含义
粒子X产生测量Zt的概率的含义是：在X的位姿下，能够产生实际测量Zt的概率，其实也就是反演模型产生的虚拟测量与实际测量的吻合程度。显然，定位越不正确，概率越小，定位越准确，概率越大。因此，可以监控全局平均概率，如果概率变小，说明定位越来越不准确了。
Alfa（slow）要远远小于Alfa（fast），Wavg是本次滤波得到的全局平均概率。显然Wfast会很显著地反应滤波效果，而Wslow则变化比较慢，更能反应滤波的长期效果。
如上图公式（称为滤波评价值），当滤波效果越差，Wfast就变得越来越小于Wslow，那么返回值就会越大。反之，滤波效果较好时候，或者比较稳定的时候，Wfast和Wslow比较接近，返回值就接近0。
（3）Xt代表M个粒子的集合，第5行利用运动模型从旧粒子采样获取新位姿，第6行它的重要性权重依据测量模型设置。
（4）AMCL中最重要的地方就是随机采样概率。（看图说话）,sample_motion_model()函数也就是粒子滤波中提到的g()函数，
（5）motion_model用的是《概率机器人》这本书第5章的sample_motion_model_velocity 也就是状态转移模型，amcl重使用的是下面的里程计模型
（6）各个参数的作用、
（7）measurement_model用的是《概率机器人》这本书的第6章landmark_model_known_correspondece
也就是测量模型。measurement_model()函数计算的就是
,基本符合了粒子滤波算法的形式，并且也展现了贝叶斯滤波的思想
AMCL算法中默认的采用测距仪的似然域模型，另外还给出了测距仪的波束波形（beam）和likelihood_field_prob
（8）算法各部分作用，以及算法的输入输出
借用《Probablistic Robotics》书中一个例子，AMCL算法的全局定位的一个过程：
白色小圆点代表：真实的机器人位置，红色圈圈代表离自己均值（请仔细对照图，分析过程，收获颇丰）
在进行第一次标记检测时，几乎所有粒子根据这个检测抽取，如图b所示。该步骤对应测量概率的短期平均小于测量概率的长期平均的情况。多次检测后，粒子紧紧环绕早真实的机器人周围，就像d所示，并且短期和长期测量似然平均都将增加。在这个定位阶段，机器人只是跟踪其位置，观察似然相当高，并且只偶尔增加小数量的随机粒子。
当将机器人放置在其他位置时，测量概率下降。在这个新的位置，第一次标记检测还没有触发任何附加粒子，因为平滑估计Wfast仍然很高（e），在新位置进行了几次标记检测后，Wfast比Wslow下降的快，并有更多的随机粒子被加进来（f、g）。最后机器人定位成功。
四. 具体模型分析
（1）里程计运动模型
在第一章MCL和AMCL算法中均涉及运动模型采样，本章将介绍所使用的里程计运动模型。里程通常可通过整合轮子的编码信息来得到，许多商业机器人在固定的时间间隔产生这样的积分位姿估计，即里程计运动模型通过距离测量来估计运动。
解决机器人定位问题主要是解决从机器人内部里程计使用的坐标到物理世界坐标之间的变换问题。里程计模型使用相对运动信息，由内部里程计测量，在时间间隔(t-1,t]内，机器人从位姿xt-1运动到,里程计反馈了从
的相对前进，这里的‘-’代表其是基于机器人内部坐标的，该坐标系与全局世界坐标的关系是未知的。在状态估计中利用\(\bar{x}_{t-1}\)和\(\bar{x_t}\)之间的相对差是真是位姿xt-1和xt之间差异的一个很好的估计器，因此运动信息ut由下式( 2.1 )给出：
为了提取相对距离，被转变成三个步骤：旋转、直线平移和另一个旋转，如下图所示：
这三个参数足以组成由历程及编码的相对运动的统计量。由于AMCL是基于粒子滤波的定位方法，因此希望有一个的采样算法，运动模型采样算法如下所示：
从算法2. 1中我们可以看到，四个α \alphaα参数分别对旋转和平移方向的撒点角度δ \deltaδ产生影响，使用采样算法可直接通过控制得到位姿估计xt的值。
（2）测距仪模型
与里程计运动模型相同，在第一章MCL和AMCL算法中均涉及了使用测量模型确定离子的重要性权值，测距仪是时下最流行的机器人传感器，因此使用测距仪作为测量模型的近似物理模型来测量附近物体的距离。
（2.1） 波束模型
波束模型采用四类测量误差，包括小的测量噪声、意外对象引起的误差、由于未检测到对象引起的误差和随机意外噪声。因此期望模型$$ P= \left(z_t|x_t,m\right)$$ 是四个密度的混合，每一种密度都与一个特定类型的误差有关，四种密度如下所示：
1.即使传感器正确测量了最近对象的距离，它返回的值也受到误差的影响，即测量噪声，通常由一个窄的均值为\(z^{k*}_t\)、标准偏差为\(\sigma_{hit}\)的高斯建模，\(p_{hit}\)表示高斯分布，如上图a)所示，实际上测距传感器的值局限于区间\([0;z_{max}]\)，这里\(z_max\)表示最大的传感器距离。
2.意外对象比如与机器人共享操作空间的人，处理这类对象的一种方法是将他们作为传感器噪声来处理，未建模对象会导致比\(z^{k*}_t\)更短的距离。该情况下距离测量用指数分布描述，如上图b)，其密度在\(z^{k*}_t\)范围内指数减少。
3.有时环境中的障碍会被完全忽略如声呐传感器遇到镜面反射、激光雷达检测到黑色吸光物体等，这便是传感器检测失败误差，其典型结果是返回传感器允许的最大值\(z_max\)，这是一个离散函数，如上图c)所示。
4.测距仪偶尔会产生完全无法解释的测量，例如超声在几面墙之间反弹，从而产生随机意外噪声，为了使之简单化，对于这样的测量，这里使用一个分布在完整传感器测量范围\([0;z_{max}]\)的均匀分布来建立模型。如上图d)所示。
上述4个参数 \(z_{hit}\),\(z_{short}\),\(z_{rand}\),通过加权平均混合，且有
注意此处的\(z_max\)只是一个参数而与前文所提最大传感器距离不同，所有4种密度线性组合后得到的典型密度如下图所示：
可以看到所有4种基本模型的基本特性在这个组合密度中仍然存在，故测距仪模型可由如下波束模型算法实现：
其中算法的输入包括一个完整的距离扫描\(z^k_t\)、机器人姿态\(x_t\)和地图m，在一个循环中将各传感器波束的似然相乘，第4行采用射线投射来特定的传感器测量计算无噪声距离，第5行计算了各个距离测量的似然，算法返回期望的概率$$ P= \left(z_t|x_t,m\right)$$ 。
基于波束的传感器模型具有两个主要缺点，一是缺乏光滑性，在有许多小障碍的混乱环境中分布不光滑，二是计算量过大，因为该模型需要对每一个传感器的波束都进行评估，故计算量过大需要很大内存。为了克服以上缺点，引入另一种似然域模型。
（2.2） 似然域模型
似然域模型不必计算相对于任何有意义的传感器物理生成模型的条件概率，因为该模型没有一个合理的物理解释。它的主要思想是首先将传感器扫描的终点\(z_t\)映射到地图的全局坐标空间，因此就必须知道机器人的局部坐标系在何处、传感器光束的源头和指向等信息。
机器人\(t\)时刻的位姿为\(x_t = {(x,y,\theta)}^T\)，传感器的安装位置相对于机器人的中心坐标\({(x_{k,sense},y_{k,sense})}^T\),激光光束相对于机器人的朝向角度为\(\theta_{k,sense}\),激光测量的终点相对传感器中心为\(z^k_t\)，激光扫描到的点投影到地图的全局坐标系坐标为,其坐标变化如下式( 3.1 ) 所示
这些坐标只有当传感器监测到障碍物才有意义，否则会输出最大值\(z_{max}\)，而在似然域模型中将会把最大距离的读数丢弃。
与波束模型类似，似然域模型有三种噪声和不确定性来源，分别是测量噪声、测量失败和无法解释的随机测量，其中测量噪声涉及寻找地图上最近的障碍物，其高斯噪声与测量目标到地图m mm上最近物体之间的欧氏距离有关，与基于波束模型的传感器模型相同，期望概率$$ P= \left(z_t|x_t,m\right)$$集成了所有三种分布\(z_{hit}p_{hit}+z_{rand}p_{rand}+z_{max}p_{max}\)，式中使用熟悉的混合权值\(z_{hit}\)、\(z_{rand}\)和\(z_{max}\) 。其算法如下所示：
与波束算法类似，该算法通过循环将各个$$ P= \left(z_t|x_t,m\right)$$的值相乘，并假定不同传感器波束的噪声相互独立，第4行判断传感器读数是否为最大距离读数，如果是则舍弃，否则进行5~8行运算——通过将一个正态分布和一个均匀分布混合得到似然结果，注意该算法第8行中的\(z_{max}\)并非随机测量带来的误差，而是传感器的最大测量距离\(z_{max}\)！（此处需要对比源码查看）故基于似然模型的算法只使用了\(z_{rand}\)和\(z_{hit}\)参数，要保证\(z_{rand}+z_{hit}=1\)。
以上，基于AMCL定位方法的算法和模型分析结束，从上述所有模型和算法设计到的参数来看，需要重点调试的是AMCL中撒点粒子数、有效粒子数、里程计采样算法中的四个α 参数、似然域模型中的误差参数\(z_{hit}\)和\(z_{hit}\)、波束模型的四个误差参数以及最大测量距离\(z_{max}\)。
重采样小技巧
在前面提供的伪代码中，都有一句draw i with probability的伪代码，也就是根据粒子的权重，去挑选粒子。实际如何做呢？
我们知道，所有粒子的权重归一化后求和为1。如果把所有粒子排列成一排，权重越大，占用位置越大，而他们占用的总位置求和是1。那么我们获取一个（0,1）的随机数，显然，这个随机数落在权重大的粒子上的可能性也更大啦。算法：
c[0] = 0.0;
for(i=0;i<sample_count;i++)
c[i+1] = c[i]+weight[i]; //用c来储存权重的累加值
foreach(p in all particles)
{
double r;
r = drand48();
for(i=0;i<sample_count;i++)
{
if((c[i] <= r) && (r < c[i+1])) //权重越大的粒子，区间就越大，随机数r落在其区间的可能性也越大
break;
}
//TODO: Select particle i
}
总结
粒子滤波的思想很简单，是基于穷举法的一种高效方法。若使用穷举法，那么就是对位姿空间中的每个可能去遍历，当然从计算量上来说这是不可能的。所以粒子滤波就是足够多的随机粒子（位姿假设）放到整个位姿空间中，然后用本文的方法去选出定位效果好的粒子，不停迭代，最终所有的粒子收敛到正确的位姿附近。
优点：能够得到机器人位姿的较优解（蒙特拉罗的），大大降低了穷举法的计算量，能够一定程度上解决机器人绑架问题
缺点：必须要机器人移动了一定距离才能执行一次滤波，机器人不运动时无法定位；粒子需要多次迭代才能收敛；计算量随着粒子数量的增加而直线上升。
