Skip to main content
Open In Colab Nav2, the navigation stack of ROS 2, localizes a robot in a map with AMCL, adaptive Monte Carlo localization. The map comes from an earlier run. You drive the robot once with a SLAM package such as slam_toolbox and save the occupancy grid it builds. From then on AMCL estimates the pose in that saved grid and never changes it. AMCL is the particle filter of the previous section. It predicts with the odometry motion model, weights with a LIDAR likelihood and resamples. It adds two things to the basic filter. KLD sampling changes the number of particles as the belief spreads or tightens (Fox, 2003). Random particles, injected when the measurements stop fitting, let the filter recover after the robot is carried somewhere else (Thrun, Burgard and Fox, 2005, Table 8.3). Its source code, in the nav2_amcl package, also departs from the textbook in how it combines the beams of a scan. The parts appear here in the order the filter uses them: the map and the likelihood of one beam, the way AMCL combines the beams of a scan, the adaptive number of particles, and the recovery mechanism. A table at the end maps Nav2’s parameters to the symbols.

​
The map and the likelihood field

The map is an occupancy grid with 5 cm cells. AMCL’s default sensor model is the likelihood field (Thrun, Burgard and Fox, 2005, Section 6.4 and Table 6.3), not the beam model. The beam model casts a ray through the map for every beam of every particle. The likelihood field does no ray casting. It projects the endpoint of each beam into the map and looks up how far that point lies from the nearest occupied cell. These distances are computed once, when the map is loaded, and stored in a second grid. Nav2 stops the computation at laser_likelihood_max_dist, 2 m by default, so every cell farther away reads 2 m. Take a particle at st=(x,y,θ)s_t = (x, y, \theta) and beam kk of the scan, with measured range ztkz_t^k and bearing ϕk\phi_k. With the LIDAR at the center of the robot, the endpoint of the beam is xk=x+ztkcos⁡(θ+ϕk),yk=y+ztksin⁡(θ+ϕk).x_k = x + z_t^k \cos(\theta + \phi_k), \qquad y_k = y + z_t^k \sin(\theta + \phi_k). If dkd_k is the distance from (xk,yk)(x_k, y_k) to the nearest occupied cell, the likelihood of the beam is p(ztk∣st,m)=zhitexp⁡(−dk22σhit2)+zrandzmax⁡.p(z_t^k \mid s_t, m) = z_{\text{hit}} \exp\left(-\frac{d_k^2}{2 \sigma_{\text{hit}}^2}\right) + \frac{z_{\text{rand}}}{z_{\max}}. The first term is large when the endpoint falls on a wall of the map. The second is a floor for readings the map cannot explain. AMCL names the mixture weights zhitz_{\text{hit}} and zrandz_{\text{rand}}, where the beam model section writes αhit\alpha_{\text{hit}} and αrand\alpha_{\text{rand}}. The textbook Gaussian has the factor 1/2πσhit1/\sqrt{2\pi}\sigma_{\text{hit}} in front, and AMCL leaves it out. A beam that returns zmax⁡z_{\max} has no endpoint and is skipped. The code below builds a map of four rooms around a corridor, its distance grid, and a simulated LIDAR with the parameters of the TurtleBot3’s LDS-01: 360 beams, a 3.5 m range and 1 cm range noise. The simulator finds the true ranges by stepping along each beam through the grid. The filter never calls it.
Top, a floor plan of four rooms around a corridor with furniture, a robot in the corridor and its scan endpoints lying on the corridor walls. Bottom, a color map of the distance from each cell to the nearest wall, dark on walls and brightest in the middle of the rooms. Top: the occupancy grid, with walls and furniture in black, and the endpoints of one simulated scan from the robot in the corridor. Bottom: the distance from each cell to the nearest occupied cell, computed once from the grid. A beam’s likelihood is read from this grid at the beam’s endpoint. The endpoints lie on the walls because the scan came from the true pose. For a particle at a wrong pose the same ranges and bearings put the endpoints in open space, where the distance grid reads large values and the likelihood falls to the floor zrand/zmax⁡z_{\text{rand}}/z_{\max}.

​
Prediction with odometry

AMCL moves every particle by a sample from the odometry motion model, described in the particle filter section. The relative motion between two odometry poses is split into a turn δrot1\delta_{\text{rot1}}, a straight drive δtrans\delta_{\text{trans}} and a second turn δrot2\delta_{\text{rot2}}, and each particle draws its own noisy version of the three. The noise parameters α1\alpha_1 to α4\alpha_4 are 0.2 by default in Nav2. AMCL does not update on every scan. It waits until the odometry reports that the robot has moved update_min_d, 0.25 m, or turned update_min_a, 0.2 rad. The simulated robot below therefore drives in steps of 0.25 m and turns in place in steps of 0.2 rad, and each step is one filter update. Its odometry has a 3% error in distance and about 0.6° of noise in each turn.

​
Combining the beams of a scan

The particle filter needs one weight per particle for the whole scan. With beams that are independent given the pose and the map, that weight is the product of the beam likelihoods, as in the beam model: wt[k]=∏jp(ztj∣st[k],m).w_t^{[k]} = \prod_{j} p(z_t^j \mid s_t^{[k]}, m). AMCL computes something else. It uses at most max_beams beams, 60 by default, evenly spaced through the scan. The product is in its source, commented out. In its place the code adds the cubes of the beam likelihoods to 1: wt[k]=1+∑jp(ztj∣st[k],m)3.w_t^{[k]} = 1 + \sum_{j} p(z_t^j \mid s_t^{[k]}, m)^3. A comment in the source calls this an ad hoc weighting scheme. The code below computes both weights for 2000 particles scattered around the true pose, with a spread of 30 cm in position and 6° in heading, and reports the effective sample size 1/∑k(w[k])21 / \sum_k (w^{[k]})^2 of the normalized weights. It counts how many particles carry the weight in practice.
Two scatter plots of particle weight against distance from the true position on log axes. For the product of beams the weights fall over 28 orders of magnitude within one meter; for AMCL's sum of cubes they fall only by a factor of about ten. Normalized weight of each particle, divided by the uniform weight 1/20001/2000, against its distance from the true position. The dotted line is a uniform weight. Left: the product of the beam likelihoods. Right: AMCL’s sum of cubes. With the product, the weights span 28 orders of magnitude and the effective sample size is 86 of 2000. With AMCL’s sum of cubes the largest weight is about 10 times the smallest and the effective sample size is 1620. Both weights fall with the distance from the true position, but the product picks out a few particles within about 20 cm, while AMCL’s weight still leaves particles 50 cm away with half the weight of the best ones. The product is the correct weight only if the beams are independent given the pose and the map. They are not. Neighboring beams hit the same wall, and an error in the map or a small error in a particle’s heading moves many endpoints together. Multiplying 46 beam likelihoods then counts the same evidence many times over, and the filter becomes overconfident: one scan can remove nearly every particle. The sum of cubes is not the likelihood of any measurement model. It keeps many particles alive, at the price of a weaker preference for the right pose. The beam model section assumes independent beams. AMCL’s code drops that assumption and puts a heuristic in its place.

​
The number of particles

A particle filter needs many particles when the belief is spread over the whole map and few when it is concentrated around one pose. KLD sampling (Fox, 2003) chooses the number while it resamples. It divides the state space into bins, 50 cm by 50 cm by 10° in AMCL, and draws particles one at a time. After each draw it counts the number kk of bins that hold at least one particle and stops once the number of particles exceeds n(k)=k−12ε(1−29(k−1)+29(k−1) z)3.n(k) = \frac{k-1}{2\varepsilon} \left( 1 - \frac{2}{9(k-1)} + \sqrt{\frac{2}{9(k-1)}}\, z \right)^3 . Fox derives the bound for a particle set whose histogram over the bins should differ from the true posterior by at most ε\varepsilon in Kullback-Leibler divergence, with probability 1−δ1 - \delta. The constant zz is then the upper 1−δ1 - \delta quantile of the standard normal distribution. The more bins the particles occupy, the more particles the bound requires. AMCL clips n(k)n(k) to the range [min_particles, max_particles], 500 to 2000 by default. In Nav2, ε\varepsilon is pf_err, 0.05 by default, and zz is pf_z, 0.99 by default. The code uses pf_z directly as the quantile zz, and the Nav2 documentation notes that 2.33 is the 99% quantile. The default of 0.99 therefore corresponds to δ≈0.16\delta \approx 0.16, not 0.01. The resampler below follows AMCL. Each draw is a resampled particle or, with probability prandp_{\text{rand}}, a random pose in free space. The second case is the recovery mechanism of the last part.

​
Global localization

The filter loop puts the parts together. Each update moves the particles with the odometry, weights them with the scan and resamples them with KLD sampling. It also keeps the two averages wsloww_{\text{slow}} and wfastw_{\text{fast}} that the recovery mechanism needs. Like AMCL, it reports the mean pose of the heaviest cluster. After resampling, AMCL groups the occupied bins of its KD-tree into clusters, where bins that touch, diagonally included, belong to the same cluster. It adds up the weights of the particles in each cluster and reports the mean pose of the cluster with the largest sum. Every resampled particle has the same weight, so this is the cluster with the most particles. The mean of all particles would fall between the modes of a spread-out belief. Nav2 can start without an initial pose. Its reinitialize_global_localization service spreads max_particles particles uniformly over the free space of the map, and the run below starts that way.
Three floor plans showing particles spread over the whole map at update 0, gathered into clusters at update 6, and grouped in a few clusters along the corridor at update 24, one of them at the robot. Below, a plot where the particle count stays at 2000 until update 34 and then drops to about 500, while the position error falls from about 8 m to about 1 m by update 28 and then drops to about 0.1 m. Global localization. Top three panels: the particles (orange) and the true pose (blue circle) at three updates. Bottom: the number of particles and the position error at every update. At the first update the particles cover the free space. After six updates they have collected into clusters in the corridor and in the rooms. The corridor looks the same from several places along it, so the particles gather in blobs along it, and many of the blobs face the same way. Their bins touch along the corridor, and AMCL’s clustering joins several blobs into one cluster whose mean lies between them. At update 24 the heaviest cluster holds 1426 particles spread from 3.9 to 12.4 m along the corridor, the particles at the true pose among them, and its mean lies 2 m ahead of the robot. For the first 28 updates the estimate is off: the position error starts near 8 m and falls slowly to 1.3 m. At update 29 the robot turns into the upper room, the scans rule out the other blobs, and the error drops to 0.1 m, where it stays. The particle count stays at its maximum of 2000 through update 34. Until then the particles occupy more than 183 bins, the number at which the bound reaches 2000. Once the other clusters die out, the count falls to about 500, the minimum, by update 40. It rises briefly, to at most about 900, after each turn in place, when the heading noise spreads the particles over more bins.

​
Recovery after the robot is moved

A robot that is picked up and set down elsewhere, the kidnapped robot of Thrun, Burgard and Fox (2005, Section 7.2), leaves the particle filter with no particles near the new pose. Odometry does not see the move. Resampling only copies existing particles, so on its own the filter cannot find the robot again. Augmented MCL (Thrun, Burgard and Fox, 2005, Table 8.3) adds random poses during resampling. It tracks two exponential averages of the mean weight wavgw_{\text{avg}} of the particles, wslow←wslow+αslow(wavg−wslow),wfast←wfast+αfast(wavg−wfast),w_{\text{slow}} \leftarrow w_{\text{slow}} + \alpha_{\text{slow}} (w_{\text{avg}} - w_{\text{slow}}), \qquad w_{\text{fast}} \leftarrow w_{\text{fast}} + \alpha_{\text{fast}} (w_{\text{avg}} - w_{\text{fast}}), with 0≤αslow≪αfast0 \le \alpha_{\text{slow}} \ll \alpha_{\text{fast}}. Each resampled particle is replaced by a random pose with probability prand=max⁡(0,  1−wfastwslow).p_{\text{rand}} = \max\left(0,\; 1 - \frac{w_{\text{fast}}}{w_{\text{slow}}}\right). While the scans fit the particles, the two averages agree and prandp_{\text{rand}} is 0. When the fit gets suddenly worse, wfastw_{\text{fast}} drops faster than wsloww_{\text{slow}} and random poses enter the set. In Table 8.3, wavgw_{\text{avg}} is the mean of the measurement likelihoods. AMCL computes it from the particle weights before it normalizes them. Each of those weights is the particle’s previous weight times its scan score, and after resampling every previous weight is 1/N1/N. AMCL’s wavgw_{\text{avg}} is therefore the mean score divided by the number of particles NN. Because KLD sampling changes NN, wavgw_{\text{avg}} also falls when the particle set grows, even if the scores stay the same. Nav2 sets both recovery_alpha_slow and recovery_alpha_fast to 0, which turns the mechanism off. Its documentation suggests 0.001 and 0.1. AMCL also does one thing that Table 8.3 does not: after any update that injects random poses it sets both averages to zero, so that the next update restarts them from the current mean weight. The run below starts from a known pose, as after you set the initial pose in Nav2, and carries the robot from the upper middle room to the lower left room after 60 updates. It runs three times: with Nav2’s defaults, with the suggested values, and with the suggested values but without the reset, as in Table 8.3.
Three stacked plots against filter update with the kidnapping at update 60. Top: with recovery off the error stays near 3 m after the kidnapping, with the suggested values it falls back to near zero by update 95, and without the reset it stays at several meters. Middle: the mean weight drops sharply at the kidnapping while the slow average stays near 0.008. Bottom: the probability of a random pose stays near zero with the reset and rises to about 0.7 without it. Kidnapping at update 60 (dotted line). Top: position error for the three settings. Middle: the mean weight and its two averages, for the run without the reset. Bottom: the probability of a random pose for the three settings. With recovery off, the particles follow the odometry from the old pose and settle about 3 m from the truth. With the suggested values, random poses enter at only two updates, 72 and 74, and the largest prandp_{\text{rand}} is 0.008, about 15 of 2000 particles. In this run that is enough: one of the random poses lands near the robot, its cluster takes over, and the error stays under 0.3 m from update 95. Without the reset, prandp_{\text{rand}} climbs to 0.70 and the filter never settles. The middle panel shows how the averages move. At the kidnapping wavgw_{\text{avg}} drops by a factor of about 3, from 0.020 to 0.006. It keeps falling, to about 0.0022, because the lost particles spread over more bins and KLD sampling raises their number to 2000, which divides every weight by 4. The drop is clear, and still random poses enter 12 updates late. The run starts from 2000 particles in a tight cloud, where wavgw_{\text{avg}} is only 0.007. The count then falls to about 500 and wavgw_{\text{avg}} rises to about 0.02. With αslow=0.001\alpha_{\text{slow}} = 0.001, wsloww_{\text{slow}} covers only 6% of that rise in 60 updates, so it is still near 0.008 at the kidnapping. prandp_{\text{rand}} stays 0 until wfastw_{\text{fast}} falls below it. AMCL then zeroes both averages, they restart at the new, low level, and the injections stop after one more update. Without the reset, wsloww_{\text{slow}} stays near 0.008 while wfastw_{\text{fast}} follows the falling weight, and from update 85 more than half of the particles are random at every update. The few random poses that the reset allows make recovery a matter of luck. Started with the seeds 2 to 11, the suggested values relocalize the robot in 2 of the 10 runs; the run shown is one of the two. In practice, when AMCL is lost you give it a new initial pose, or you call reinitialize_global_localization and let the robot drive until the particles converge, as in the previous part. The defaults are from the nav2_amcl source. The likelihood_field_prob model adds beam skipping: when most particles disagree with a beam, the beam is ignored, which helps with objects that are not in the map.

​
References

Found an error on this page? Report an issue.