Project: the kidnapped robot

No idea where it starts. Work it out, then drive somewhere on purpose.

U7.7LocalisationUniversity50 min

Do this lesson in the simulator

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

Guide: Particle filters explained

The robot wakes up somewhere on the mat. It is not told where. It has to work that out, and then drive to the green corner on purpose.

This is the standard hard case in the literature, and it is a fair test of everything in the module: a belief that starts as "anywhere", measurements that cut it down, resampling that keeps the good guesses without killing the diversity, and a controller that then acts on the answer.

Two phases

Phase one: find out where you are. Stand still, weighing and resampling until the cloud agrees with itself. One reading of the far wall leaves a whole stripe of the mat possible: it finds y and says nothing about x. Driving forward and reading the same wall again only finds y again. What cuts the stripe down is a reading from a different direction: the pair of readings is far more informative than either alone.

One sensor, one axis. The depth sensor measures the wall it is pointing at and nothing else, so a cloud weighed against it can only ever pin down one coordinate. Facing the far wall square on, the reading is 200 - y. Turn ninety degrees to the left, finish the turn, and the same arithmetic reads x instead. Two clouds, one after the other, and the robot knows both. Neither formula is right part way through the turn, which is the trap in this project (see below).

Facing the far wall the reading finds y; facing the left wall it finds x; together they fix the robotfar wallleft wallreads 80after the turn, reads 150facing the farwall: y = 121.0facing the leftwall: x = 150.6
The robot woke up at (150, 120). The first cloud, weighed facing the far wall, is the dotted stripe: the right y and every x. The second, weighed after the turn facing the left wall, is the dashed line: the right x and nothing about y. Where they cross is the answer.

Phase two: go somewhere. Now the estimate is in mat coordinates, so the target can be too. Steer towards it with the inverse kinematics from U2, and keep the filter running all the way, so that the estimate improves rather than decaying into dead reckoning. Driving straight towards or away from a wall keeps the robot square on to it, so the readings stay usable; only the turns need to be skipped.

The order of the project: find y, turn without weighing, find x, then drive with the filter running1. Face the far wallweigh with 200 - yuntil the spread is small2. Turn left 90 degreesskip every readinguntil the gyro says done3. Face the left wallweigh with xuntil the spread is small4. Drive to the cornerkeep weighing, butskip readings in turnsy foundx foundturn overEach stage repeats every tick until its last line is true.
The project in four stages: 1 to 3 are phase one, 4 is phase two. The red one is the trap: nothing is weighed while the robot turns, because neither 200 - y nor x is true part way round.

What to watch on the chart

  • The spread collapsing during phase one. If it does not, the robot needs to move before it can know anything.
  • The spread staying small during phase two. If it climbs, the measurements have stopped being informative and you are dead reckoning again.
  • A sudden jump in the estimate. Either the cloud had two clusters and has just made its mind up, or a reading the model cannot explain, such as one taken during a turn, has dragged it somewhere. Whether it is right is the interesting question.

The structure

from bugbot import *
import math, random
connect()
random.seed(1)

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

def weigh(measured):
    # standing still, square on to the far wall: the reading is 200 - y
    global weights
    w = [math.exp(-(((200 - y) - measured) ** 2) / (2 * SIGMA * SIGMA)) + 1e-12 for y in particles]
    total = sum(w)
    weights = [v / total for v in w]

def resample(jitter=0.8):
    global particles, weights
    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, jitter))
    particles, weights = fresh, [1.0 / N] * N

for i in range(20):
    weigh(distance())
    resample()
    mean = sum(w * y for w, y in zip(weights, particles))      # once, not once per particle
    plot("spread", math.sqrt(sum(w * (y - mean) ** 2 for w, y in zip(weights, particles))))
    # 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)

print("I think I am at y =", round(sum(w * y for w, y in zip(weights, particles)), 1))

Run this in the simulator

The robot is at y = 120, and the cloud says 121.0. The blue stripe on the mat is the answer so far: the right y, and every x still possible.

Readings while turning

Phase one is done standing still and square on to the far wall, which is exactly when 200 - y is true. The turn to the left wall is where a filter goes wrong. Part way round, the sensor looks at a wall on a slant and reads long: 111 cm at 45 degrees and 152 cm at 60 degrees, from this spot 80 cm from the far wall and 150 cm from the left one. Neither 200 - y nor x explains those readings, and a filter that weighs its cloud against them anyway moves the cloud to wherever they seem to point.

The readings taken during the turn, and where each would put the robot if it were read as x0 deg79 cm30 deg88 cm45 deg111 cm60 deg152 cm90 deg: reads 150, which is xrobot050100150200x, cmeach reading read as x:without skipping, the cloud ends at 92.2skipping them: 150.6
From the robot's spot, only the last reading, square on to the left wall, is x. The earlier ones, marked red on the line below, point at 79, 88, 111 and 152, and a cloud weighed against them settles near 92 before the true reading arrives, too far away for any guess to be near 150.

This cell starts the x cloud before the turn, and weighs it every tick unless the robot is turning.

from bugbot import *
import math, random
connect()
random.seed(1)

# change this and press Run
SKIP_WHILE_TURNING = True

DT, N, SIGMA = 0.1, 400, 3.5
FOUND_Y = 121    # what phase one found, only for drawing
particles = [random.uniform(0, 200) for i in range(N)]   # guesses at x

def weigh_and_resample(measured, jitter=0.8):
    global particles
    # facing the left wall square on, the reading is x
    w = [math.exp(-((x - measured) ** 2) / (2 * SIGMA * SIGMA)) + 1e-12 for x in particles]
    total = sum(w)
    step, r, c, i, fresh = 1.0 / N, random.uniform(0, 1.0 / N), w[0] / total, 0, []
    for m in range(N):
        u = r + m * step
        while u > c and i < N - 1:
            i += 1
            c += w[i] / total
        fresh.append(particles[i] + random.gauss(0, jitter))
    particles = fresh

drive(0, 0, -30)          # turn left, towards the left wall
turning = True
for tick in range(60):
    h = odometry()[2]
    if turning and 180 < h < 278:   # stop a little early: it coasts
        stop()
        turning = False
    if not (turning and SKIP_WHILE_TURNING):
        weigh_and_resample(distance())
    plot("x estimate", sum(particles) / N)
    # the truth, for the chart only: position() counts from the
    # start, which was x = 150 on the mat
    plot("robot x", 150 + position()[0])
    draw("particles", [(x, FOUND_Y) for x in particles], "blue")
    wait(DT)
stop()
mean = sum(particles) / N
spread = (sum((x - mean) ** 2 for x in particles) / N) ** 0.5
print("found x", round(mean, 1), "spread", round(spread, 1))
print("the robot is at x", round(150 + position()[0], 1))

Run this in the simulator

With the readings skipped, the cloud sits untouched over the whole mat until the turn ends at 2.4 s, and the first reading of the left wall pulls it straight onto the robot: it finds x = 150.6 with a spread of 1.8 cm, and the robot is at 149.9.

Set SKIP_WHILE_TURNING = False and run it again. Now the cloud is weighed first against the far wall (it is still facing it when the cell starts), then against the slanted readings, and by 1.5 s it has collapsed around x = 92. When the robot does face the left wall and reads 150, no guess is anywhere near 150, every weight is the same tiny floor, and resampling has nothing to choose between. It ends at x = 92.2 with a spread of 5.4 cm, 58 cm from the robot and no idea that it is wrong.

The rule for the project: weigh only while the robot is square on to the wall your formula is about. Stop, turn, wait for the turn to finish, then start weighing again. The gyro heading from odometry() is good enough to know when the turn is over: here it said 267.3 degrees when the robot was really at 268.7, and it drifts by 2 to 3 degrees over a longer run, which the sensor's cone forgives. The stop at 278 rather than 270 is because the robot keeps turning for a moment after stop().

Task: the kidnapped robot

Work out where the robot woke up, print it as found:, and then drive into the green corner. position() is not allowed anywhere.

from bugbot import *
import math, random
connect()

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

Challenges

  1. Shuffle 20 cm sideways during phase one, still facing the far wall, and keep weighing. Does the cloud collapse any faster, or learn anything about x? Why not?
  2. Inject 2 percent random particles every tick, and half way through the run, pick the robot up in your head: set the true position somewhere else by driving it there with the filter off. Does it recover?
  3. Track heading as well as position in each particle. What breaks, and what does it cost?

What comes next

Localisation assumed the map was known. U8 asks where the map comes from, which is the other half of the problem, and the point at which the two together get the name SLAM.