M9-T4 — Monte Carlo Localization in Practice

Learning objectives

After this lesson, you should be able to:

• Describe the MCL loop with a range scan: motion sample, beam-model weights, resample.
• Explain the practical devices: ray-casting tables, the likelihood power, and the effect of beam independence.
• Implement recovery from kidnapping by injecting random particles in proportion to a drop in the average likelihood.
• Interpret the success rates against the number of particles.

The intuition

The room of the left panel has a divider, a box and plain walls. The robot knows the map but not where it is in it: 400 particles, each a hypothesis of position and heading, are scattered over the free space. The robot has a range scanner with 8 beams. For each particle the filter asks: if the robot were here, with this heading, what would the 8 beams read? It answers by casting 8 rays from the particle's pose into the map, compares the predicted ranges with the real ones through the beam model, and multiplies the likelihoods to get the weight.

A single scan is already informative in this asymmetric room: the run of the program below has 86% of the weight within 0.7 m of the truth at step 5, and an error of 4 cm at step 10. The number of particles still matters. With 30 particles, 2 of 12 runs localised (usually no particle began near the truth); with 100, 4; with 400, 9. The runs that failed lost the true hypothesis early, which no later reading can bring back.

The right panel shows the other failure, the kidnapped robot. At step 40 the robot is picked up and put down 4.7 m away. The particles, all in the old place, now disagree with every reading: the mean likelihood drops from 0.32 to 0.01. The filter notices this drop and starts replacing a fraction of its particles by random ones; one of them lands near the new place, is rewarded by the readings and takes over. The error is back to 6 cm by step 70. Without injection, only 2 of 12 runs recovered; with it, 11 of 12.

The average likelihood of a scan is a free health monitor: when it falls sharply and stays low, the filter’s belief no longer fits the world. Use it to add particles where the belief has no mass.

Definitions

Ray casting

Computing the expected range along a ray from a pose to the first wall of the map: the function $\hat z = \mathrm{cast}(x, y, \theta+\beta_k)$ for beam $k$.

Beam model

The mixture of Module 5: hit (Gaussian around the expected range), short, max, and random readings. It gives $p(z_k\mid\hat z_k)$ per beam.

Scan likelihood

$p(\mathbf{z}\mid x)=\prod_k p(z_k\mid\hat z_k(x))$: the product over the beams, assuming independence.

Likelihood power $\gamma$

The tempered weight $p(\mathbf{z}\mid x)^\gamma$ with $\gamma<1$ (here 0.2): compensates for the correlation between beams and for model error.

Augmented MCL

Keeps a fast and a slow average of the mean likelihood, $w_\mathrm{fast}$ and $w_\mathrm{slow}$, and injects random particles with probability $\max(0,\,1 - w_\mathrm{fast}/w_\mathrm{slow})$.

Derivation: why the likelihood power, and why injection works

Beams are not independent

Treating the 8 beams as independent multiplies 8 sharp likelihoods. In reality the beams share errors: the same wall, the same unmodelled object, the same small error in the map or in the particle's pose affect several at once. The product then overstates the information: if each beam's log-likelihood has a random part with variance $\sigma_\ell^2$, the true variance of the sum is up to $K^2\sigma_\ell^2$ for fully correlated beams, rather than $K\sigma_\ell^2$.

1. $$p(\mathbf{z}\mid x)^\gamma=\exp\bigl(\gamma\sum_k\log p(z_k\mid\hat z_k)\bigr)$$

   The tempered likelihood scales the log-likelihood by $\gamma$ and thus flattens differences between particles.

2. $$\gamma\approx\frac{K_\mathrm{eff}}{K}$$

   A heuristic: if the beams carry the information of $K_\mathrm{eff}$ independent ones, scale the log-likelihood by that fraction. Here 8 beams in a room count for about 2 independent ones, $\gamma=0.2$.

3. $$N_\mathrm{eff}\ \text{falls more slowly with}\ \gamma<1$$

   Flatter weights keep the particles diverse and avoid the collapse of the previous lesson, at the price of a slightly broader posterior.

Injection

The average unnormalised weight $\bar w$ of the particles after a scan estimates $p(\mathbf{z}_t\mid\mathbf{z}_{1:t-1})$, the probability the filter assigned to the scan it just saw. In a healthy filter it fluctuates around a stable value; after a kidnapping it drops by an order of magnitude and stays there.

1. $$\bar w_t=\tfrac1N\sum_i p(\mathbf{z}_t\mid x^{[i]}_t)^\gamma;\approx;\text{the data likelihood of the scan under the belief}$$

   The importance-weight average is an unbiased estimator of the marginal likelihood, a measure of how well the belief explains the data.

2. $$w_\mathrm{fast}\ \text{tracks recent scans},\quad w_\mathrm{slow}\ \text{the long-term level}$$

   Two exponential averages with rates 0.5 and 0.05. In steady state they agree.

3. $$p_\mathrm{rand}=\max\bigl(0,\,1-w_\mathrm{fast}/w_\mathrm{slow}\bigr)$$

   After a drop $w_\mathrm{fast}$ falls first, so the ratio shrinks and the injection probability rises towards 1. Once the filter recovers it is 0 again. In the example the likelihood went from 0.32 to 0.01 and back to 0.34.

Cost

Casting rays is the expensive part: the program of this lesson computes the expected ranges once on a grid of positions (0.25 m) and headings (15°) and looks them up, and likewise tabulates the beam model in 0.1 m bins. Production systems do the same (the “likelihood field” is a cheaper alternative that avoids ray casting).

MCL = particle filter + map. The likelihood power keeps it from becoming overconfident, and a monitor of the average likelihood lets it recover from being lost.

A worked example

The room of the figure (10 × 8 m, a divider and a box), a robot driving a route of 84 steps with commands of speed and turn rate, noisy motion (2 cm and 0.03 rad per step on the real robot, 10 cm and 0.15 rad in the filter's motion sampler), and 8 beams of range 6 m with 10 cm noise and 10% bad readings.

1. One run, 400 particles, global. The initial error is 3.9 m with 4% of the weight near the truth. At step 5, 86% of the weight is within 0.7 m (error 63 cm); at step 10, 100% (4 cm). From then on the error stays within about 12 cm and the effective sample size oscillates between 215 and 400 (resampled whenever it falls below 200).
2. Particles. Of 12 runs with different seeds, the number with an error below 0.5 m at step 70 is 2 for 30 particles, 4 for 100 and 9 for 400.
3. Kidnapping. At step 40 the robot is moved to (8.5, 1.5) heading west, 4.7 m from where it was. Without injection, 2 of 12 runs recover (when a few particles happened to be near the new place). With augmented MCL, 11 of 12 recover by the end. In the printed run the error is 0.05 m before, 4.66 m just after, 3.2 m ten steps later, 0.72 m at step 60 and 0.01 m at step 84, while the average likelihood goes 0.32, 0.010, 0.015, 0.052, 0.34, 0.53.

import math
from robotprob import (Rng, ParticleFilter, beam_likelihood, sample_motion_velocity, range_bearing, wrap_angle, mean,
                       normalize_weights, resample_systematic)

# the map: a room with a partial divider and a box, as line segments
SEGS = [((0, 0), (10, 0)), ((10, 0), (10, 8)), ((10, 8), (0, 8)), ((0, 8), (0, 0)),
        ((5, 0), (5, 4)),                                   # a divider
        ((7, 5), (8.5, 5)), ((8.5, 5), (8.5, 6.5)), ((8.5, 6.5), (7, 6.5)), ((7, 6.5), (7, 5))]   # a box
ZMAX, NB = 6.0, 8
BEAMS = [2 * math.pi * k / NB for k in range(NB)]

def cast(x, y, th):                                         # the distance to the nearest wall along a ray
    dx, dy, best = math.cos(th), math.sin(th), ZMAX
    for (x1, y1), (x2, y2) in SEGS:
        ex, ey = x2 - x1, y2 - y1
        den = dx * ey - dy * ex
        if abs(den) < 1e-12: continue
        t = ((x1 - x) * ey - (y1 - y) * ex) / den
        u = ((x1 - x) * dy - (y1 - y) * dx) / den
        if t > 0.0 and 0.0 <= u <= 1.0 and t < best: best = t
    return best

# a lookup table of expected ranges: 0.25 m cells, 24 headings, computed once (as real MCL implementations do)
CELL, NH = 0.25, 24
TABLE = {}
def expected(x, y, th):
    i, j, k = int(x / CELL), int(y / CELL), int(round((th % (2 * math.pi)) / (2 * math.pi) * NH)) % NH
    key = (i, j, k)
    if key not in TABLE:
        cx, cy, ch = (i + 0.5) * CELL, (j + 0.5) * CELL, k * 2 * math.pi / NH
        TABLE[key] = [cast(cx, cy, ch + b) for b in BEAMS]
    return TABLE[key]

BIN = 0.1
LOGLIK = {}                                                 # the sensor model as a table: log p(reading | expected range), in 0.1 m bins
def beam_loglik(zi, ei):
    key = (int(zi / BIN), int(ei / BIN))
    if key not in LOGLIK:
        LOGLIK[key] = math.log(beam_likelihood(min((key[0] + 0.5) * BIN, ZMAX), (key[1] + 0.5) * BIN, ZMAX, 0.2, 1.0, 0.7, 0.1, 0.1, 0.1) + 1e-9)
    return LOGLIK[key]

def scan_likelihood(pose, z, power):
    e = expected(pose[0], pose[1], pose[2])
    return math.exp(power * sum([beam_loglik(zi, ei) for zi, ei in zip(z, e)]))

def inside(p):
    return 0.05 < p[0] < 9.95 and 0.05 < p[1] < 7.95

def free(p):                                                # not inside the box
    return not (6.9 < p[0] < 8.6 and 4.9 < p[1] < 6.6)

def make_commands(rng):                                     # drive round the divider and the box; the commands the robot is given
    pose = [1.5, 1.5, math.pi / 2]; cmds = []
    for wp in [(1.5, 7.0), (9.0, 7.4), (9.0, 1.5), (6.5, 1.5), (6.5, 3.2)]:
        for _ in range(80):
            ang = wrap_angle(math.atan2(wp[1] - pose[1], wp[0] - pose[0]) - pose[2])
            if math.hypot(wp[0] - pose[0], wp[1] - pose[1]) < 0.35: break
            w = max(-0.8, min(0.8, 2.5 * ang)); v = 0.3 if abs(ang) < 0.5 else 0.05
            cmds.append((v, w))
            pose = sample_motion_velocity(Rng(1), pose, v, w, 1.0, 0.0, 0.0)
    return cmds

def run(seed, n, steps=70, kidnap=None, inject=False, power=0.2):
    rng = Rng(seed)
    cmds = make_commands(rng)
    truth = [1.5, 1.5, math.pi / 2]
    def sample_pose():
        while True:
            p = [rng.uniform(0.2, 9.8), rng.uniform(0.2, 7.8), rng.uniform(-math.pi, math.pi)]
            if free(p): return p
    pf = ParticleFilter([sample_pose() for _ in range(n)], rng)
    w_slow = w_fast = None
    log = []
    for t in range(min(steps, len(cmds))):
        v, w = cmds[t]
        if kidnap and t == kidnap[0]:
            truth = list(kidnap[1])                         # the robot is carried to another place
        truth = sample_motion_velocity(rng, truth, v, w, 1.0, 0.02, 0.03)
        if not inside(truth) or not free(truth): truth = [min(max(truth[0], 0.1), 9.9), min(max(truth[1], 0.1), 7.9), truth[2]]
        e = expected(truth[0], truth[1], truth[2])
        z = [min(ZMAX, max(0.0, ei + rng.gauss(0.0, 0.1))) if rng.random() > 0.1 else (ZMAX if rng.random() < 0.5 else rng.uniform(0, ZMAX)) for ei in e]
        pf.predict(lambda p, r: sample_motion_velocity(r, p, v, w, 1.0, 0.1, 0.15))
        raw = [scan_likelihood(p, z, power) for p in pf.particles]
        avg = sum(raw) / n
        pf.weights = normalize_weights([wi * r for wi, r in zip(pf.weights, raw)])
        did = pf.resample(0.5)
        if inject and avg > 0:                              # augmented MCL: compare a fast and a slow average of the likelihood
            w_slow = avg if w_slow is None else w_slow + 0.05 * (avg - w_slow)
            w_fast = avg if w_fast is None else w_fast + 0.5 * (avg - w_fast)
            p_rand = max(0.0, 1.0 - w_fast / w_slow)
            if did:
                pf.particles = [sample_pose() if rng.random() < p_rand else p for p in pf.particles]
        mx = sum(w_ * p[0] for p, w_ in zip(pf.particles, pf.weights)); my = sum(w_ * p[1] for p, w_ in zip(pf.particles, pf.weights))
        near = sum(w_ for p, w_ in zip(pf.particles, pf.weights) if math.hypot(p[0] - truth[0], p[1] - truth[1]) < 0.7)
        log.append((t + 1, truth, math.hypot(mx - truth[0], my - truth[1]), near, pf.n_eff(), avg))
    return log

n_steps = len(make_commands(Rng(1)))
print('commands in the route:', n_steps)
lg = run(2, 400)
print('step  truth (x, y)   position error  mass within 0.7 m   N_eff')
for t, tr, err, near, ne, avg in lg:
    if t in (1, 5, 10, 20, 30, 45, 60, 70):
        print(str(t).rjust(4), (round(tr[0], 2), round(tr[1], 2)), str(round(err, 2)).rjust(12), str(round(near, 2)).rjust(14), str(round(ne)).rjust(10))
print('global localisation: runs (of 12) with error < 0.5 m at step 70')
for n in (30, 100, 400):
    ok = sum(1 for s in range(1, 13) if run(s, n)[-1][2] < 0.5)
    print(' particles', str(n).rjust(4), ' localised', str(ok).rjust(3))

# kidnapping: at step 40 the robot is carried to (8.5, 1.5) heading west and goes on driving
print('kidnapped at step 40, 400 particles, 12 runs: runs with error < 0.5 m at the end (step 84)')
for inject in (False, True):
    ok = 0
    for s in range(1, 13):
        lg2 = run(s, 400, steps=84, kidnap=(40, (8.5, 1.5, math.pi)), inject=inject)
        if lg2[-1][2] < 0.5: ok += 1
    print(' random particles injected:', str(inject).ljust(5), ' recovered', str(ok).rjust(3))
lg2 = run(3, 400, steps=84, kidnap=(40, (8.5, 1.5, math.pi)), inject=True)
print('one run with injection: step, error, mean likelihood')
for t, tr, err, near, ne, avg in lg2:
    if t in (38, 41, 45, 50, 60, 70, 84):
        print(' ', str(t).rjust(3), str(round(err, 2)).rjust(6), str(round(avg, 6)).rjust(12))

commands in the route: 84
step  truth (x, y)   position error  mass within 0.7 m   N_eff
   1 (1.49, 1.79)         3.89           0.04        400
   5 (1.43, 2.93)         0.63           0.86        400
  10 (1.4, 4.42)         0.04            1.0        215
  20 (1.31, 6.82)         0.07            1.0        207
  30 (4.19, 7.65)          0.1            1.0        400
  45 (7.94, 7.9)         0.12           0.97        400
  60 (9.94, 4.66)         0.08            1.0        400
  70 (9.62, 2.6)         0.08            1.0        400
global localisation: runs (of 12) with error < 0.5 m at step 70
 particles   30  localised   2
 particles  100  localised   4
 particles  400  localised   9
kidnapped at step 40, 400 particles, 12 runs: runs with error < 0.5 m at the end (step 84)
 random particles injected: False  recovered   2
 random particles injected: True   recovered  11
one run with injection: step, error, mean likelihood
   38   0.05     0.323591
   41   4.66     0.010016
   45   3.72     0.015653
   50   3.23     0.015174
   60   0.72     0.052348
   70   0.06     0.338394
   84   0.01     0.525205

In robotics

MCL in practice

Monte Carlo localisation is the default localiser in many mobile-robot stacks (the ROS navigation amcl node is an adaptive MCL). They differ from this sketch in details: laser likelihood fields instead of ray casting, KLD-sampling to vary the number of particles between a few hundred and several thousand, and odometry motion models with four noise parameters (Module 5). The Navigation course describes the stack in its localisation lessons.

Starting and restarting

• Global localisation: spread particles over the free space (as here). Convergence takes a few scans in a distinctive map and many in a symmetric one.
• Tracking: start from a pose estimate with a small spread.
• Recovery: augmented MCL as above, or a global re-localisation trigger when the likelihood stays low.

Diagnostics to log

Log the number of particles, N_eff, the spread of the cloud, the average likelihood and the fraction of beams classified as hits. A localisation failure always shows in one of these before it shows in the robot's behaviour.

Limits

• The map must be right. Moved furniture, a closed door or glass produce readings the model calls “random”; the filter survives a few but loses the pose in a rearranged hall.
• Long featureless corridors leave the position along the corridor unobservable and the cloud smears along it (module 7: observability).
• The cost is linear in the number of particles and beams, and the number of particles needed grows quickly with the dimension of the state, which is why MCL handles a 3-D pose but not a full map.

Common misconceptions

Key takeaways

1. MCL weights each pose particle by the likelihood of the scan under the particle’s expected ranges, using the beam model: ray cast from the pose, compare, multiply.
2. Tabulate the expected ranges and the beam model; the cost then is a few lookups per particle and beam.
3. Temper the likelihood (a power below 1; here 0.2) because beams are correlated and the model approximate.
4. In the room, 400 particles localised in 9 of 12 runs, against 4 with 100 and 2 with 30: the runs that failed never had a particle near the truth.
5. Random particles injected in proportion to 1 − w_fast/w_slow rescued 11 of 12 kidnapped runs, against 2 of 12 without.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, chapter 8 (mobile robot localisation: Markov and Monte Carlo), including the augmented MCL algorithm of section 8.3.5 and the beam model of section 6.3. Paid book.
• D. Fox, “Adapting the Sample Size in Particle Filters Through KLD-Sampling”, International Journal of Robotics Research 22 (12), 2003. Paid journal article.
• C. Stachniss, “Particle Filter and Monte Carlo Localization” (video lecture) , includes the augmented MCL. Free to watch.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Module 9 has covered the particle filter for a pose. Module 10 turns to the map itself: occupancy grids, where each cell holds a probability that it is occupied and the binary Bayes filter of Module 3 does the work.

Related content

Nothing below is required to finish this lesson.

• Before
  M9-T3: Resampling, Degeneracy and Impoverishment

  Compute the effective sample size, implement low-variance resampling, and explain particle deprivation.

• Lab
  M9-L1: Monte Carlo Localization

  Rover is placed in a known room and must find itself from range readings to four beacons. Implement the weights and the resampler, then make the filter survive a kidnapping.

Other courses on this site treat neighbouring topics from their own angle:

• SLAM M3-T8: Particle Filters and FastSLAM
• SLAM M3-L4: Explore Particle-Filter Localization