Project: the kidnapped robot
No idea where it starts. Work it out, then drive somewhere on purpose.
Do this lesson in the simulatorThe 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, or shuffle a little, weighing and resampling until the cloud agrees with itself. Standing still is the harder version, because one reading leaves a whole stripe of the mat possible. A small movement, and a second reading from the new place, cuts that stripe down fast: 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, the reading is 200 - y. Turn ninety degrees to the left and the same arithmetic reads x instead. Two clouds, one after the other, and the robot knows both.
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.
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. The cloud had two clusters and has just made its mind up. Whether it made it up correctly is the interesting question.
The structure
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)]
weights = [1.0 / N] * N
def weigh(measured):
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))))
wait(DT)
print("I think I am at y =", round(sum(w * y for w, y in zip(weights, particles)), 1))
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
- Shuffle 20 cm sideways during phase one and use the pair of readings. How much faster does the cloud collapse?
- 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?
- 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.