Monte Carlo localisation

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

U7.6LocalisationUniversity35 min

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: SLAM explained: simultaneous localisation and mapping

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()
random.seed(1)

DT, N, SIGMA = 0.1, 300, 3.5
START_Y = 50   # where the robot starts on the mat: only for the chart
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)
    # the truth, for the chart only: position() counts from where
    # the robot started, so add the start to put it on the mat
    plot("robot y", START_Y + position()[1])
    # each guess is only a y, so draw it as a stripe across the mat
    draw("particles", [((i + 0.5) * 200 / N, y) for i, y in enumerate(particles)], "blue")
    wait(DT)
stop()
print("the filter says the robot is at y =", round(mean, 1))
print("the robot is at y =", round(START_Y + position()[1], 1))

Run this in the simulator

The spread line tells most of the story. The cloud starts over the whole mat, but the first reading is weighed before anything is plotted, so the first point on the chart is already 3.7 cm: one reading of the far wall is enough to find y. For the next 3 seconds the spread stays at about 2.5 cm and the estimate follows the robot to within 3 cm, all the way from y = 50 to y = 80. The blue stripe on the mat moves up with the robot.

Then, at about 3.4 s, the estimate jumps ahead of the robot and stays 7 to 12 cm ahead to the end, while the spread stays at 2 cm. At the end the filter says y = 110.1 and the robot is at 102.1. Nothing is wrong with the filter. The robot started at x = 70, right in front of tag 11, and the tag's card stands on the mat about 9 cm in front of the far wall. Once the robot is within about a metre of it, the depth sensor meets the card before the wall and reads 9 cm short, and a filter that only knows 200 - y believes it. The spread says it is sure to 2 cm and the error is 8. A sensor model that does not match the world gives a confident wrong answer, and the only way to see it is to compare with the truth, which a simulator gives you and a real robot usually does not.

Notice the START_Y. position() is measured from where the robot started, not from the corner of the mat, so to compare it with a cloud that lives on the mat the chart adds the starting y. Forget it and the filter looks 50 cm out.

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.

Only while it faces the wall

The sensor model here is 200 - y, which is only true while the robot faces the far wall square on (U7.4). This run works because the robot drives straight: by the end it has wandered 1.5 degrees off, and the sensor's narrow cone reads the same as square on for the first 10 degrees or so. Make the robot turn and the readings go long, 111 cm at 45 degrees from a spot 80 cm from the wall, and a filter that keeps weighing moves its whole cloud to the wrong place with full confidence. If the robot has to turn, skip the readings until it has stopped turning and faces a wall your model describes. U7.7 shows what happens when you do not.

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.
• It jumps when the robot turns. The sensor model assumes the robot is square on to the wall. Skip readings while it turns.
• It is sure and wrong, with no warning. Something the sensor sees is missing from the model, like the tag card above. The spread cannot tell you this; only a check against a known position can.

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(). Drive straight, and expect the tag card to pull your answer a few centimetres ahead once the robot is near it.

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?
4. Part way along, stop, turn 30 degrees left and straight back, and keep weighing all the while. Where does the estimate go, and does it come back? Then skip the readings until the robot faces the far wall again.