> ## Documentation Index > Fetch the complete documentation index at: https://aegean.ai/llms.txt > Use this file to discover all available pages before exploring further. # Adaptive Monte Carlo Localization > The particle filter that Nav2 runs to localize a robot. [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/pantelis/eng-ai-agents/blob/main/notebooks/rse/amcl/index.ipynb) [Nav2](https://docs.nav2.org), 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](https://aegean.ai/aiml-common/lectures/rse/particle-filter/index.md) 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. ```python theme={null} %config InlineBackend.figure_formats = ["svg"] import matplotlib.pyplot as plt import numpy as np from scipy.ndimage import distance_transform_edt plt.rcParams.update({"figure.figsize": (6.5, 3.5), "font.size": 9, "axes.titlesize": 10, "legend.fontsize": 8, "svg.fonttype": "path", "axes.grid": False}) BLUE, VERMILLION, GREEN, PINK, GOLD, GRAY = "#0072B2", "#D55E00", "#009E73", "#cb77a6", "#c28600", "#7f7f7f" ``` ## 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](https://aegean.ai/aiml-common/lectures/sensor-models/beam/index.md). 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 $s_t = (x, y, \theta)$ and beam $k$ of the scan, with measured range $z_t^k$ and bearing $\phi_k$. With the LIDAR at the center of the robot, the endpoint of the beam is $$ x_k = x + z_t^k \cos(\theta + \phi_k), \qquad y_k = y + z_t^k \sin(\theta + \phi_k). $$ If $d_k$ is the distance from $(x_k, y_k)$ to the nearest occupied cell, the likelihood of the beam is $$ 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 $z_{\text{hit}}$ and $z_{\text{rand}}$, where the beam model section writes $\alpha_{\text{hit}}$ and $\alpha_{\text{rand}}$. The textbook Gaussian has the factor $1/\sqrt{2\pi}\sigma_{\text{hit}}$ in front, and AMCL leaves it out. A beam that returns $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. ```python theme={null} RES = 0.05 # m per cell W, H = 12.0, 8.0 nx, ny = int(W / RES), int(H / RES) occ = np.zeros((ny, nx), bool) # occ[j, i] is the cell at x = i * RES, y = j * RES def wall(x0, y0, x1, y1, t=0.1): """Mark a 10 cm thick wall from (x0, y0) to (x1, y1) as occupied.""" i0, i1 = int(min(x0, x1) / RES), int(np.ceil((max(x0, x1) + t) / RES)) j0, j1 = int(min(y0, y1) / RES), int(np.ceil((max(y0, y1) + t) / RES)) occ[j0:j1, i0:i1] = True def box(x0, y0, x1, y1): occ[int(y0 / RES):int(y1 / RES), int(x0 / RES):int(x1 / RES)] = True for w in [(0, 0, 12, 0), (0, 7.9, 12, 7.9), (0, 0, 0, 8), (11.9, 0, 11.9, 8), # outer walls (0, 3.0, 2.0, 3.0), (3.0, 3.0, 7.0, 3.0), (8.0, 3.0, 12, 3.0), # corridor, lower side, with doors (0, 4.6, 4.5, 4.6), (5.5, 4.6, 9.0, 4.6), (10.0, 4.6, 12, 4.6), # corridor, upper side (6.0, 0, 6.0, 3.0), (4.0, 4.6, 4.0, 8.0), (8.0, 4.6, 8.0, 8.0)]: # walls between rooms wall(*w) for b in [(8.5, 1.0, 10.0, 1.6), (1.0, 6.0, 1.4, 6.4), (9.0, 6.6, 9.3, 7.4), (5.0, 7.2, 6.2, 7.9), (2.0, 0.9, 3.2, 1.5)]: box(*b) # furniture MAX_OCC_DIST = 2.0 # laser_likelihood_max_dist dist = np.minimum(distance_transform_edt(~occ) * RES, MAX_OCC_DIST) # distance to the nearest occupied cell free_cells = np.argwhere(dist > 0.25) # where the robot can be: at least 25 cm from any obstacle # The simulated LIDAR (an LDS-01) Z_MAX, N_BEAMS, SIGMA_RANGE = 3.5, 360, 0.01 def true_ranges(pose, angles, step=0.02): """Range to the first occupied cell along each beam, or Z_MAX.""" t = np.arange(0, Z_MAX, step) i = np.clip(((pose[0] + np.cos(angles)[:, None] * t) / RES).astype(int), 0, nx - 1) j = np.clip(((pose[1] + np.sin(angles)[:, None] * t) / RES).astype(int), 0, ny - 1) hit = occ[j, i] return np.where(hit.any(axis=1), t[hit.argmax(axis=1)], Z_MAX) def lidar_scan(pose, rng): """One scan from the pose: bearings in the robot frame and noisy ranges.""" bearings = np.arange(N_BEAMS) * 2 * np.pi / N_BEAMS r = true_ranges(pose, pose[2] + bearings) z = np.clip(r + rng.normal(0, SIGMA_RANGE, N_BEAMS), 0, Z_MAX) z[r >= Z_MAX] = Z_MAX return bearings, z ``` ![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.](https://mintcdn.com/aegeanaiinc/49cW2M54qfwz3vnn/aiml-common/lectures/rse/amcl/images/cell_3_output_1.svg?fit=max\&auto=format\&n=49cW2M54qfwz3vnn\&q=85\&s=2d20e30a9cc9ec9e4ec630b77b75c555) *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 $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](https://aegean.ai/aiml-common/lectures/rse/particle-filter/index.md). The relative motion between two odometry poses is split into a turn $\delta_{\text{rot1}}$, a straight drive $\delta_{\text{trans}}$ and a second turn $\delta_{\text{rot2}}$, and each particle draws its own noisy version of the three. The noise parameters $\alpha_1$ to $\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. ```python theme={null} ALPHA = (0.2, 0.2, 0.2, 0.2) # alpha1 to alpha4 def wrap(a): return (a + np.pi) % (2 * np.pi) - np.pi def relative_motion(a, b): """Odometry motion from pose a to pose b as (rot1, trans, rot2).""" dx, dy = b[0] - a[0], b[1] - a[1] trans = np.hypot(dx, dy) rot1 = wrap(np.arctan2(dy, dx) - a[2]) if trans > 1e-6 else 0.0 return rot1, trans, wrap(b[2] - a[2] - rot1) def sample_motion_model_odometry(S, odom_prev, odom_now, rng): """Move each particle (row of S) by its own noisy copy of the odometry motion.""" rot1, trans, rot2 = relative_motion(odom_prev, odom_now) a1, a2, a3, a4 = ALPHA n = len(S) r1 = rot1 - rng.normal(0, np.sqrt(a1 * rot1**2 + a2 * trans**2), n) tr = trans - rng.normal(0, np.sqrt(a3 * trans**2 + a4 * (rot1**2 + rot2**2)), n) r2 = rot2 - rng.normal(0, np.sqrt(a1 * rot2**2 + a2 * trans**2), n) return np.column_stack([S[:, 0] + tr * np.cos(S[:, 2] + r1), S[:, 1] + tr * np.sin(S[:, 2] + r1), S[:, 2] + r1 + r2]) def drive(waypoints, step=0.25, turn_step=0.2): """True poses along the waypoints: straight legs in steps of `step`, turns in place in steps of `turn_step`.""" poses, heading = [], np.arctan2(*(waypoints[1] - waypoints[0])[::-1]) for a, b in zip(waypoints[:-1], waypoints[1:]): turn = wrap(np.arctan2(*(b - a)[::-1]) - heading) for s in range(1, int(np.ceil(abs(turn) / turn_step)) + 1): poses.append([a[0], a[1], heading + turn * min(1, s * turn_step / abs(turn))]) heading += turn n = max(1, int(round(np.linalg.norm(b - a) / step))) for s in range(1 if poses else 0, n + 1): p = a + (b - a) * s / n poses.append([p[0], p[1], heading]) return np.array(poses) def wheel_odometry(truth, rng): """Odometry poses: the true motion with 3% distance error and turn noise. A jump of more than 1 m is not seen.""" odom = [truth[0].copy()] for a, b in zip(truth[:-1], truth[1:]): rot1, trans, rot2 = relative_motion(a, b) if trans > 1.0: # the robot was carried: the wheels did not turn rot1 = trans = rot2 = 0.0 trans *= 1 + rng.normal(0, 0.03) rot1 += rng.normal(0, 0.01) rot2 += rng.normal(0, 0.01) o = odom[-1] h = o[2] + rot1 odom.append(np.array([o[0] + trans * np.cos(h), o[1] + trans * np.sin(h), h + rot2])) return np.array(odom) WAYPOINTS = np.array([[1.0, 3.75], [5.0, 3.75], [5.0, 6.5], [6.5, 6.5], [6.5, 3.75], [10.8, 3.75], [10.8, 2.0], [7.0, 2.0], [7.0, 0.7], [11.0, 0.7]]) print(f"{len(drive(WAYPOINTS))} filter updates along the path") ``` ```output theme={null} 169 filter updates along the path ``` ## 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: $$ 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: $$ 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 / \sum_k (w^{[k]})^2$ of the normalized weights. It counts how many particles carry the weight in practice. ```python theme={null} Z_HIT, Z_RAND, SIGMA_HIT, MAX_BEAMS = 0.5, 0.5, 0.2, 60 # Nav2 defaults def beam_likelihoods(S, bearings, ranges): """Likelihood field: one row per particle, one column per beam used.""" step = max(1, (len(ranges) - 1) // (MAX_BEAMS - 1)) b, z = bearings[::step], ranges[::step] b, z = b[z < Z_MAX], z[z < Z_MAX] # max-range beams have no endpoint xk = S[:, 0:1] + z * np.cos(S[:, 2:3] + b) yk = S[:, 1:2] + z * np.sin(S[:, 2:3] + b) i, j = (xk / RES).astype(int), (yk / RES).astype(int) inside = (i >= 0) & (i < nx) & (j >= 0) & (j < ny) d = np.full(xk.shape, MAX_OCC_DIST) # an endpoint off the map counts as far from everything d[inside] = dist[j[inside], i[inside]] return Z_HIT * np.exp(-d**2 / (2 * SIGMA_HIT**2)) + Z_RAND / Z_MAX def weight_product(p): """Product over beams, scaled so that the largest weight is 1 (computed in logs).""" log_w = np.log(p).sum(axis=1) return np.exp(log_w - log_w.max()) def weight_amcl(p): """AMCL's combination: 1 plus the sum of the cubed beam likelihoods.""" return 1.0 + (p**3).sum(axis=1) def effective_sample_size(w): w = w / w.sum() return 1 / np.sum(w**2) rng = np.random.default_rng(1) s_true = np.array([5.0, 3.75, 0.0]) bearings, ranges = lidar_scan(s_true, rng) S_demo = s_true + rng.normal(0, [0.3, 0.3, 0.1], (2000, 3)) p_demo = beam_likelihoods(S_demo, bearings, ranges) offset = np.linalg.norm(S_demo[:, :2] - s_true[:2], axis=1) print(f"{p_demo.shape[1]} beams used") for name, w in (("product", weight_product(p_demo)), ("AMCL", weight_amcl(p_demo))): wn = w / w.sum() print(f"{name:8s} effective sample size {effective_sample_size(w):7.1f} of 2000, " f"largest weight {wn.max():.4f}, largest/smallest {w.max() / w.min():.3g}") ``` ```output theme={null} 46 beams used product effective sample size 86.0 of 2000, largest weight 0.0262, largest/smallest 2.14e+28 AMCL effective sample size 1620.2 of 2000, largest weight 0.0010, largest/smallest 10.3 ``` ![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.](https://mintcdn.com/aegeanaiinc/49cW2M54qfwz3vnn/aiml-common/lectures/rse/amcl/images/cell_6_output_1.svg?fit=max\&auto=format\&n=49cW2M54qfwz3vnn\&q=85\&s=7fc387068a6dd30f8c42324b89cb50de) *Normalized weight of each particle, divided by the uniform weight $1/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](https://aegean.ai/aiml-common/lectures/sensor-models/beam/index.md) 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 $k$ of bins that hold at least one particle and stops once the number of particles exceeds $$ 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 - \delta$. The constant $z$ is then the upper $1 - \delta$ quantile of the standard normal distribution. The more bins the particles occupy, the more particles the bound requires. AMCL clips $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 $z$ is `pf_z`, 0.99 by default. The code uses `pf_z` directly as the quantile $z$, and the Nav2 documentation notes that 2.33 is the 99% quantile. The default of 0.99 therefore corresponds to $\delta \approx 0.16$, not 0.01. The resampler below follows AMCL. Each draw is a resampled particle or, with probability $p_{\text{rand}}$, a random pose in free space. The second case is the recovery mechanism of the last part. ```python theme={null} BIN = np.array([0.5, 0.5, np.deg2rad(10)]) # KD-tree bin size in x, y, theta PF_ERR, PF_Z, MIN_PARTICLES, MAX_PARTICLES = 0.05, 0.99, 500, 2000 def kld_bound(k, err=PF_ERR, z=PF_Z): """Number of particles needed when k bins are occupied, clipped to [MIN_PARTICLES, MAX_PARTICLES].""" k = np.asarray(k, float) c = 2 / (9 * np.maximum(k - 1, 1)) n = np.ceil((k - 1) / (2 * err) * (1 - c + np.sqrt(c) * z) ** 3) n = np.where(k <= 1, MAX_PARTICLES, n) return np.clip(n, MIN_PARTICLES, MAX_PARTICLES).astype(int) def random_poses(n, rng): """Uniform poses in free space, as AMCL draws them for global localization and recovery.""" c = free_cells[rng.integers(len(free_cells), size=n)] return np.column_stack([(c[:, 1] + 0.5) * RES, (c[:, 0] + 0.5) * RES, rng.uniform(-np.pi, np.pi, n)]) def resample_kld(S, w, p_rand, rng): """Draw particles one at a time until there are more than kld_bound(k) of them.""" draws = S[rng.choice(len(S), size=MAX_PARTICLES, p=w / w.sum())] is_random = rng.random(MAX_PARTICLES) < p_rand draws[is_random] = random_poses(is_random.sum(), rng) keys = np.floor(draws[:, :3] / BIN).astype(int) # raw heading, as Nav2 bins it _, first = np.unique(keys, axis=0, return_index=True) new_bin = np.zeros(MAX_PARTICLES, int) new_bin[first] = 1 k = np.cumsum(new_bin) # occupied bins after each draw done = np.nonzero(np.arange(1, MAX_PARTICLES + 1) > kld_bound(k))[0] n = done[0] + 1 if len(done) else MAX_PARTICLES return draws[:n], k[n - 1] NEIGHBORS = np.array([(a, b, c) for a in (-1, 0, 1) for b in (-1, 0, 1) for c in (-1, 0, 1)]) def cluster_estimate(S): """Mean pose of the heaviest cluster, as Nav2 reports it. Occupied bins that touch, including diagonally, form one cluster. After resampling every particle has the same weight, so the heaviest cluster is the one with the most particles.""" keys = np.floor(S[:, :3] / BIN).astype(int) # raw heading, as Nav2 bins it bins, of_particle = np.unique(keys, axis=0, return_inverse=True) index = {tuple(b): i for i, b in enumerate(bins)} label = np.full(len(bins), -1) for start in range(len(bins)): if label[start] >= 0: continue label[start], stack = start, [start] while stack: for nb in bins[stack.pop()] + NEIGHBORS: j = index.get(tuple(nb)) if j is not None and label[j] < 0: label[j] = start stack.append(j) cluster = label[of_particle.ravel()] P = S[cluster == np.bincount(cluster).argmax()] return np.array([P[:, 0].mean(), P[:, 1].mean(), np.arctan2(np.sin(P[:, 2]).mean(), np.cos(P[:, 2]).mean())]) for k in (10, 30, 50, 100, 300): print(f"k = {k:3d} occupied bins: {kld_bound(k)} particles") ``` ```output theme={null} k = 10 occupied bins: 500 particles k = 30 occupied bins: 500 particles k = 50 occupied bins: 588 particles k = 100 occupied bins: 1129 particles k = 300 occupied bins: 2000 particles ``` ## 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 $w_{\text{slow}}$ and $w_{\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. ```python theme={null} def amcl(truth, rng, initial=None, alpha_slow=0.0, alpha_fast=0.0, reset_averages=True, keep=()): """Run AMCL along the true poses. Returns a per-update log and the particle sets at the updates in `keep`.""" odom = wheel_odometry(truth, rng) S = random_poses(MAX_PARTICLES, rng) if initial is None else initial w_slow = w_fast = 0.0 log, kept = [], {} for t in range(len(truth)): if t > 0: S = sample_motion_model_odometry(S, odom[t - 1], odom[t], rng) bearings, ranges = lidar_scan(truth[t], rng) w = weight_amcl(beam_likelihoods(S, bearings, ranges)) w_avg = w.mean() / len(S) # AMCL multiplies each score by the prior weight 1/N, then averages w_slow = w_avg if w_slow == 0 else w_slow + alpha_slow * (w_avg - w_slow) w_fast = w_avg if w_fast == 0 else w_fast + alpha_fast * (w_avg - w_fast) p_rand = max(0.0, 1 - w_fast / w_slow) if alpha_slow > 0 else 0.0 if t in keep: kept[t] = S.copy() n = len(S) S, bins = resample_kld(S, w, p_rand, rng) estimate = cluster_estimate(S) log.append((np.linalg.norm(estimate[:2] - truth[t, :2]), n, w_avg, w_slow, w_fast, p_rand, bins)) if p_rand > 0 and reset_averages: # AMCL zeroes both averages after it injects random particles w_slow = w_fast = 0.0 keys = ("error", "particles", "w_avg", "w_slow", "w_fast", "p_rand", "bins") return {key: np.array(col) for key, col in zip(keys, zip(*log))}, kept truth_global = drive(WAYPOINTS) SNAPSHOTS = (0, 6, 24) log_global, kept_global = amcl(truth_global, np.random.default_rng(0), keep=SNAPSHOTS) for t in (0, 5, 10, 15, 20, 25, 30, 60): print(f"update {t:3d}: error {log_global['error'][t]:5.2f} m, {log_global['particles'][t]:4d} particles, " f"{log_global['bins'][t]:4d} occupied bins after resampling") ``` ```output theme={null} update 0: error 7.90 m, 2000 particles, 1151 occupied bins after resampling update 5: error 7.19 m, 2000 particles, 844 occupied bins after resampling update 10: error 4.48 m, 2000 particles, 565 occupied bins after resampling update 15: error 3.37 m, 2000 particles, 397 occupied bins after resampling update 20: error 2.28 m, 2000 particles, 263 occupied bins after resampling update 25: error 1.90 m, 2000 particles, 265 occupied bins after resampling update 30: error 0.06 m, 2000 particles, 285 occupied bins after resampling update 60: error 0.05 m, 501 particles, 40 occupied bins after resampling ``` ![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.](https://mintcdn.com/aegeanaiinc/49cW2M54qfwz3vnn/aiml-common/lectures/rse/amcl/images/cell_9_output_1.svg?fit=max\&auto=format\&n=49cW2M54qfwz3vnn\&q=85\&s=6d375be7b25374b3ee6c2196b05e512a) *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 $w_{\text{avg}}$ of the particles, $$ 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 \le \alpha_{\text{slow}} \ll \alpha_{\text{fast}}$. Each resampled particle is replaced by a random pose with probability $$ 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 $p_{\text{rand}}$ is 0. When the fit gets suddenly worse, $w_{\text{fast}}$ drops faster than $w_{\text{slow}}$ and random poses enter the set. In Table 8.3, $w_{\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/N$. AMCL's $w_{\text{avg}}$ is therefore the mean score divided by the number of particles $N$. Because KLD sampling changes $N$, $w_{\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. ```python theme={null} KIDNAP_AT = 60 before = drive(WAYPOINTS)[:KIDNAP_AT] after = drive(np.array([[1.0, 1.9], [4.5, 1.9], [4.5, 0.6], [1.0, 0.6], [1.0, 1.9], [4.5, 1.9]])) truth_kidnap = np.vstack([before, after]) print(f"kidnapped from ({before[-1, 0]:.1f}, {before[-1, 1]:.1f}) to ({after[0, 0]:.1f}, {after[0, 1]:.1f}) m " f"at update {KIDNAP_AT}; {len(truth_kidnap)} updates in all") runs = {} for name, settings in (("recovery off (Nav2 default)", {}), ("alpha_slow 0.001, alpha_fast 0.1", {"alpha_slow": 0.001, "alpha_fast": 0.1}), ("same, without the reset (Table 8.3)", {"alpha_slow": 0.001, "alpha_fast": 0.1, "reset_averages": False})): rng = np.random.default_rng(2) start = truth_kidnap[0] + rng.normal(0, [0.05, 0.05, 0.03], (MAX_PARTICLES, 3)) runs[name], _ = amcl(truth_kidnap, rng, initial=start, **settings) r = runs[name] late = slice(len(truth_kidnap) - 20, None) print(f"{name:38s} error before {r['error'][KIDNAP_AT - 10:KIDNAP_AT].mean():.2f} m, " f"last 20 updates {r['error'][late].mean():.2f} m, largest p_rand {r['p_rand'].max():.2f}, " f"updates with p_rand > 0: {(r['p_rand'] > 0).sum()}") r = runs["same, without the reset (Table 8.3)"] print(f"at update {KIDNAP_AT - 1}: w_avg {r['w_avg'][KIDNAP_AT - 1]:.4f}, w_slow {r['w_slow'][KIDNAP_AT - 1]:.4f}, " f"w_fast {r['w_fast'][KIDNAP_AT - 1]:.4f}") # The same suggested values, started with other random seeds. recovered = 0 for seed in range(2, 12): rng = np.random.default_rng(seed) start = truth_kidnap[0] + rng.normal(0, [0.05, 0.05, 0.03], (MAX_PARTICLES, 3)) r, _ = amcl(truth_kidnap, rng, initial=start, alpha_slow=0.001, alpha_fast=0.1) recovered += r["error"][-20:].mean() < 0.3 print(f"suggested values: relocalized in {recovered} of 10 runs with seeds 2 to 11") ``` ```output theme={null} kidnapped from (6.5, 6.0) to (1.0, 1.9) m at update 60; 145 updates in all ``` ```output theme={null} recovery off (Nav2 default) error before 0.01 m, last 20 updates 3.02 m, largest p_rand 0.00, updates with p_rand > 0: 0 ``` ```output theme={null} alpha_slow 0.001, alpha_fast 0.1 error before 0.01 m, last 20 updates 0.04 m, largest p_rand 0.01, updates with p_rand > 0: 2 ``` ```output theme={null} same, without the reset (Table 8.3) error before 0.01 m, last 20 updates 4.31 m, largest p_rand 0.70, updates with p_rand > 0: 73 at update 59: w_avg 0.0199, w_slow 0.0078, w_fast 0.0194 ``` ```output theme={null} suggested values: relocalized in 2 of 10 runs with seeds 2 to 11 ``` ![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.](https://mintcdn.com/aegeanaiinc/49cW2M54qfwz3vnn/aiml-common/lectures/rse/amcl/images/cell_11_output_1.svg?fit=max\&auto=format\&n=49cW2M54qfwz3vnn\&q=85\&s=6500cf71f3975682dac1043f429969cd) *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 $p_{\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, $p_{\text{rand}}$ climbs to 0.70 and the filter never settles. The middle panel shows how the averages move. At the kidnapping $w_{\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 $w_{\text{avg}}$ is only 0.007. The count then falls to about 500 and $w_{\text{avg}}$ rises to about 0.02. With $\alpha_{\text{slow}} = 0.001$, $w_{\text{slow}}$ covers only 6% of that rise in 60 updates, so it is still near 0.008 at the kidnapping. $p_{\text{rand}}$ stays 0 until $w_{\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, $w_{\text{slow}}$ stays near 0.008 while $w_{\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. ## Nav2 parameters and their symbols | Nav2 parameter | Default | Symbol | Role | | - | - | - | - | | `laser_model_type` | `likelihood_field` | | sensor model: `beam`, `likelihood_field` or `likelihood_field_prob` | | `z_hit`, `z_rand` | 0.5, 0.5 | $z_{\text{hit}}, z_{\text{rand}}$ | mixture weights of the likelihood field | | `z_short`, `z_max`, `lambda_short` | 0.05, 0.05, 0.1 | $z_{\text{short}}, z_{\max}, \lambda_{\text{short}}$ | used by the beam model only | | `sigma_hit` | 0.2 m | $\sigma_{\text{hit}}$ | width of the hit term | | `laser_likelihood_max_dist` | 2.0 m | | cap of the distance grid | | `max_beams` | 60 | | beams used per scan | | `alpha1` to `alpha4` | 0.2 | $\alpha_1, \ldots, \alpha_4$ | odometry motion noise (`alpha5` for an omnidirectional base) | | `update_min_d`, `update_min_a` | 0.25 m, 0.2 rad | | motion between filter updates | | `min_particles`, `max_particles` | 500, 2000 | | limits of the KLD bound | | `pf_err`, `pf_z` | 0.05, 0.99 | $\varepsilon$, $z$ | KLD error bound and normal quantile | | `recovery_alpha_slow`, `recovery_alpha_fast` | 0.0, 0.0 | $\alpha_{\text{slow}}, \alpha_{\text{fast}}$ | random-pose recovery, off by default | | `resample_interval` | 1 | | filter updates between resamplings | 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 * Fox, D. (2003). [Adapting the sample size in particle filters through KLD-sampling](https://rse-lab.cs.washington.edu/projects/mcl/abstracts/adaptive-ijrr-2003.abstract.html). International Journal of Robotics Research, pp. 985 to 1003. * Nav2 documentation. [AMCL configuration guide](https://github.com/ros-navigation/docs.nav2.org/blob/rolling/docs/configuration_and_development/configuration_guide/others/configuring_amcl.md). * Nav2 source. [`nav2_amcl` package](https://github.com/ros-navigation/navigation2/tree/main/nav2_amcl). * Thrun, S., Burgard, W., and Fox, D. (2005). Probabilistic Robotics. MIT Press. *** Found an error on this page? [Report an issue](https://github.com/aegean-ai/eaia-feedback/issues/new?template=page-issue.yml\&page=/aiml-common/lectures/rse/amcl). This documentation is built and hosted on [Mintlify](https://mintlify.com), a developer documentation platform.