Monte Carlo localisation

The three steps in a loop on a real mat, with tags as the measurement.

U7.6LocalisationUniversity35 min

Do this lesson in the simulator

The three steps in a loop, on a moving robot. This is MCL, and it is what ran on almost every research robot of the 2000s and most of the vacuum cleaners of the 2010s.

each tick:
    move every particle by the odometry, plus noise
    weigh every particle against the sensor reading
    if neff is low, resample
    report the weighted mean as the estimate

from bugbot import *
import math, random
connect()

DT, N, SIGMA = 0.1, 300, 3.5
particles = [random.uniform(0, 200) for i in range(N)]
weights = [1.0 / N] * N

forward(60)
for tick in range(50):
    v = flow()[1]
    particles = [y + v * DT + random.gauss(0, 0.4) for y in particles]

    measured = distance()
    w = [math.exp(-(((200 - y) - measured) ** 2) / (2 * SIGMA * SIGMA)) + 1e-12 for y in particles]
    total = sum(w)
    weights = [v2 / total for v2 in w]

    neff = 1.0 / sum(v2 * v2 for v2 in weights)
    if neff < N / 2:
        step, r, c, i, fresh = 1.0 / N, random.uniform(0, 1.0 / N), weights[0], 0, []
        for m in range(N):
            u = r + m * step
            while u > c and i < N - 1:
                i += 1
                c += weights[i]
            fresh.append(particles[i] + random.gauss(0, 0.8))
        particles, weights = fresh, [1.0 / N] * N

    mean = sum(v2 * y for v2, y in zip(weights, particles))
    spread = math.sqrt(sum(v2 * (y - mean) ** 2 for v2, y in zip(weights, particles)))
    plot("estimate", mean)
    plot("spread", spread)
    wait(DT)
stop()
print("the filter says the robot is at y =", round(mean, 1))

Run this in the simulator

The spread line tells the story. It starts enormous, because the cloud is everywhere. It collapses over the first second or two as readings rule places out. Then it stays small, wobbling slightly, as the filter tracks.

Mat coordinates, not travel

Note what the filter produces: a position on the mat, not a distance travelled. That is the point of localisation. Dead reckoning tells you how far you have come from wherever you started; localisation tells you where you are in a frame that other things are described in, which is what you need in order to go anywhere on purpose.

Using tags as well

Anything you can predict from a pose can be a measurement. If tag 11 is on the far wall at a known place, then for each particle you can predict how far away it should look, and weight by how well that matches what the camera saw. Two tags at different places are much better than one, because each rules out a different family of poses.

Combining several measurements is multiplication: the weight from the depth sensor times the weight from the tag. That is Bayes' rule, applied to particles, and it is why the filter gets sharper as you add sensors rather than getting confused.

When it goes wrong

  • The cloud collapses on the wrong answer. Not enough jitter, or a sigma that was too small, or a motion model that was too confident.
  • The cloud never collapses. The measurement is not informative enough. Move the robot: motion is what turns one ambiguous reading into two that disagree.
  • It tracks for a minute, then loses it. Particle deprivation. Inject a few random particles.

Task: Monte Carlo localisation

Run the full filter while driving at least 50 cm. Plot spread, and print my y:, the robot's position on the mat as your filter has it. No position().

from bugbot import *
import math, random
connect()

DT, N, SIGMA = 0.1, 300, 3.5
particles = [random.uniform(0, 200) for i in range(N)]

Challenges

  1. Start with the cloud in the wrong half of the mat only. Does it recover, and how?
  2. Add the tag on the far wall as a second measurement and compare how fast the spread collapses.
  3. Drop to 30 particles. At what point does it stop working?