Level 6 · Robotics: Estimation and Autonomy

Level 4's histogram filter kept a probability for every cell of a corridor. A real pose is three continuous numbers, and a grid fine enough in x, y and \theta has millions of cells. This level tracks a pose in two cheaper ways. A particle filter keeps a few hundred weighted guesses and lets the readings decide which survive. An extended Kalman filter (EKF) keeps one Gaussian: a mean pose and a 3 by 3 covariance, bent through the robot's curved motion and its range-and-bearing sensor. Then come two questions every localizer meets: which landmark did the sensor just see, and how should a whole trajectory be corrected when the robot recognises a place it has been before?

The second topic is autonomy: planning in open space by growing a random tree, straightening its path, steering by attraction and repulsion, following a path with pure pursuit and a PID loop on the heading, organising a task with state machines and behaviour trees, and checking that a two-finger grasp will not slip.

Much of this rests on other paths, and the lesson teaches only the robotics twist. The Kalman filter is taught on the Control path (Level 6); the EKF is that filter with Jacobians where its matrices were. A particle's new weight is Bayes' rule from Statistics (Level 3), one guess at a time. The Mahalanobis distance is the z-score of Statistics (Level 2) in two dimensions, the pose graph is solved with the normal equations of Machine Learning (Level 5), a potential field is gradient descent from Machine Learning (Level 4), and the heading loop is the PID of Control (Level 4). numpy appears for the small matrices of the EKF and the pose graph; everything else is plain Python.

A cloud of guesses

A particle is one complete guess of the pose (x, y, theta). A particle filter keeps many of them, often hundreds, and where they crowd together is where the robot probably is. Unlike Level 4's cells, particles sit anywhere, and only where there is probability to describe.

When the odometry reports a motion, every particle makes that motion too, each with its own error, because the real wheels slipped by an amount nobody knows. A running filter draws those errors at random (rng.gauss(0, s)); the problems hand you the draws as lists, so every run gives the same numbers. The motion model must be stated exactly. Here it is turn, then drive along the new heading, the turn-then-drive step of Level 1. The EKF later in this lesson uses the other common model, one Euler step that drives along the old heading. Read which one a problem specifies.

import math

def wrap(a):
    a = a % (2 * math.pi)
    return a - 2 * math.pi if a > math.pi else a

cloud = [(0.0, 0.0, 0.0)] * 5                      # five guesses, all sure of the start
turn_noise = [0.03, -0.05, 0.0, 0.08, -0.02]        # this step's draws, one per particle
dist_noise = [0.10, -0.04, 0.02, -0.12, 0.05]
for (x, y, th), et, ed in zip(cloud, turn_noise, dist_noise):
    th = th + 0.5 + et                              # turn first ...
    d = 2.0 + ed                                    # ... then drive along the NEW heading
    print(round(x + d * math.cos(th), 3), round(y + d * math.sin(th), 3), round(wrap(th), 3))

1.812 1.062 0.53
1.765 0.853 0.45
1.773 0.968 0.5
1.573 1.03 0.58
1.818 0.947 0.48

Five guesses that agreed on the start now disagree by up to a quarter of a metre: the fourth turned furthest (0.58 rad) and drove least (1.88 m). Repeat the step and the disagreement grows:

import math, random
rng = random.Random(5)
cloud = [(0.0, 0.0, 0.0)] * 300
for step in range(6):                               # six times: turn 0.25 rad, drive 1 m
    moved = []
    for x, y, th in cloud:
        th += 0.25 + rng.gauss(0, 0.05)
        d = 1.0 + rng.gauss(0, 0.05)
        moved.append((x + d * math.cos(th), y + d * math.sin(th), th))
    cloud = moved
    if step in (1, 3, 5):
        xs, ys = [p[0] for p in cloud], [p[1] for p in cloud]
        mx, my = sum(xs) / len(xs), sum(ys) / len(ys)
        sx = math.sqrt(sum((v - mx) ** 2 for v in xs) / len(xs))
        sy = math.sqrt(sum((v - my) ** 2 for v in ys) / len(ys))
        print(f"after {step + 1} steps: mean ({mx:.2f}, {my:.2f}), spread {sx:.3f} x {sy:.3f} m")
        plot(xs, ys, kind="scatter", label=f"{step + 1} steps", xlabel="x (m)", ylabel="y (m)")

after 2 steps: mean (1.84, 0.72), spread 0.078 x 0.104 m
after 4 steps: mean (3.11, 2.23), spread 0.180 x 0.201 m
after 6 steps: mean (3.49, 4.17), spread 0.366 x 0.244 m

Press Run to see three clouds along a curve. After six steps the robot heads roughly up the page, yet the cloud is widest in x, across the direction of travel: a small heading error swings every later metre sideways, and those errors add up. Moving alone never makes a filter surer, just as in Level 4.

Common mistakes: driving along the old heading when the model turns first. Giving every particle the same noise draw (the cloud moves but never spreads). Forgetting to wrap the headings.

Practice: Every Particle Takes the Step, Give or Take

Weighing the guesses against a reading

Each particle carries a weight, how much the filter believes it. A reading z updates the weights by Bayes' rule, exactly as Level 4's sense update did cell by cell. For each particle, predict what the sensor would read if that particle were the truth (Level 3's measurement model): \hat z_i. With Gaussian sensor noise of standard deviation \sigma, the likelihood of the actual reading is

The factor \frac{1}{\sigma\sqrt{2\pi}} is the same for every particle and cancels when you normalise. The old weight stays in the product: a particle that explained earlier readings well keeps that advantage.

After a few readings a handful of particles may hold nearly all the weight. The effective sample size counts how many still matter:

which is n when all weights are equal and 1 when one particle has everything. A robot measures 3.6 m to a charging post at a known spot:

import math
post = (6.0, 2.0)                                   # a charging post at a known spot
particles = [(1.0, 2.0, 0.1), (2.0, 4.5, -0.3), (3.5, 0.5, 1.2), (2.5, 2.0, 0.0), (4.0, 4.0, 2.0)]
weights = [0.1, 0.2, 0.2, 0.3, 0.2]
z, sigma = 3.6, 0.4                                 # the range reading and its noise

raw = []
for (x, y, th), w in zip(particles, weights):
    d = math.hypot(post[0] - x, post[1] - y)        # the range this guess expects
    L = math.exp(-(z - d) ** 2 / (2 * sigma ** 2))  # no 1/(sigma*sqrt(2*pi)): it cancels
    raw.append(w * L)
    print(f"expects {d:.3f} m  likelihood {L:.4f}")
total = sum(raw)
new = [r / total for r in raw]
print("weights", [round(w, 4) for w in new])
print("N_eff", round(1 / sum(w * w for w in new), 3), "of", len(new))
print("equal weights:", round(1 / sum(w * w for w in [0.2] * 5), 6), "  one takes all:", 1 / sum(w * w for w in [1, 0, 0, 0, 0]))

expects 5.000 m  likelihood 0.0022
expects 4.717 m  likelihood 0.0203
expects 2.915 m  likelihood 0.2312
expects 3.500 m  likelihood 0.9692
expects 2.828 m  likelihood 0.1556
weights [0.0006, 0.0109, 0.1242, 0.7808, 0.0836]
N_eff 1.582 of 5
equal weights: 5.0   one takes all: 1.0

The fourth guess expects 3.5 m, a quarter of a standard deviation from the reading, and it ends with 78% of the weight; it also started with the largest weight. Five particles, but only about one and a half that count. The headings played no part: a range does not depend on which way the robot faces. (A bearing reading would.)

Common mistakes: dropping the old weights (right only when they are all equal). Computing N_\text{eff} before normalising. 2 * sigma where the formula has 2 * sigma ** 2. Using the squared distance as the expected range.

Practice: Which Guesses Agree With the Beacon?

Resampling: a comb through the weights

Particles with tiny weights cost as much to move and weigh as good ones, and tell the filter nothing. Resampling replaces the set by n new particles, each a copy of an old one chosen with probability proportional to its weight; afterwards every weight is 1/n. Heavy particles are copied several times and light ones vanish.

Low-variance resampling (also called systematic) does this with one random number. Lay the weights end to end along a line from 0 to W = \sum_i w_i: with running sums C_i = w_0 + \dots + w_i, particle i owns the stretch [C_{i-1}, C_i), which includes its start but not its end, and a particle of weight 0 owns nothing. Then lay a comb of n teeth along the line, W/n apart, shifted by one offset u with 0 \le u < 1:

Tooth p_k picks the particle whose stretch holds it: the first i with C_i > p_k. bisect_right from the bisect module finds exactly that index in a sorted list:

from bisect import bisect_right
from itertools import accumulate

def comb(weights, u):
    n, W = len(weights), sum(weights)
    C = list(accumulate(weights))                   # running sums C_0, C_1, ...
    return [bisect_right(C, (u + k) * W / n) for k in range(n)]   # first i with C_i > tooth

w = [0.05, 0.3, 0.0, 0.25, 0.1, 0.3]
print("running sums", [round(c, 2) for c in accumulate(w)])
print("teeth       ", [round((0.42 + k) / 6, 3) for k in range(6)])
print("picks       ", comb(w, 0.42))
print("teeth from 0", comb([2, 1, 0, 1], 0.0))

running sums [0.05, 0.35, 0.35, 0.6, 0.7, 1.0]
teeth        [0.07, 0.237, 0.403, 0.57, 0.737, 0.903]
picks        [1, 1, 3, 3, 5, 5]
teeth from 0 [0, 0, 1, 3]

Particles 0 and 4 fall between teeth and vanish; 1, 3 and 5 are each copied twice. Particle 2 has weight 0, so its stretch is empty (the running sum stays at 0.35) and no tooth can land in it. In the last line the weights add up to 4, so the teeth are at 0, 1, 2 and 3; the tooth at exactly 2 is the end of particle 0's stretch [0, 2), so it goes to particle 1. A tooth on a boundary belongs to the next stretch.

Because the teeth come in increasing order, you can also walk one index forward with its running sum instead of searching: start at i = 0, c = w[0], and for each tooth step on while c <= p (and i is not yet the last index). That is O(n) overall. Floating-point sums can leave the last tooth a hair beyond the last running sum; the problems say what to pick then.

Why "low variance"? Compare the comb with n independent draws:

import random
from bisect import bisect_right
from itertools import accumulate

w = [0.05, 0.3, 0.0, 0.25, 0.1, 0.3]
C = list(accumulate(w))
rng = random.Random(8)
comb_counts, dice_counts = {}, {}
for trial in range(2000):
    u = rng.random()
    teeth = sum(1 for k in range(6) if bisect_right(C, (u + k) / 6) == 1)
    dice = rng.choices(range(6), weights=w, k=6).count(1)
    comb_counts[teeth] = comb_counts.get(teeth, 0) + 1
    dice_counts[dice] = dice_counts.get(dice, 0) + 1
print("copies of particle 1, comb:          ", dict(sorted(comb_counts.items())))
print("copies of particle 1, 6 independent: ", dict(sorted(dice_counts.items())))

copies of particle 1, comb:           {1: 405, 2: 1595}
copies of particle 1, 6 independent:  {0: 228, 1: 616, 2: 616, 3: 385, 4: 131, 5: 20, 6: 4}

Particle 1 deserves 6 \times 0.3 = 1.8 copies. The comb always gives it 1 or 2. Six independent draws give anything from 0 to 6, and in about one run in nine they throw a good particle away by bad luck.

Common mistakes: bisect_left, or c < p in the walk: a tooth on a boundary then picks the particle that ends there. Forgetting to scale the teeth by W when the weights do not add up to 1. Drawing a new random number for every tooth. Picking a particle of weight 0.

Practice: A Comb Through the Weights

The whole filter, and one pose from the cloud

One cycle of a particle filter is move, weigh, estimate, and resample if needed. Resampling too often loses variety (copies of copies), so a common rule resamples only when N_\text{eff} drops below n/2. Until then the weights carry over from cycle to cycle and keep accumulating evidence. The estimate is the weighted mean, taken before resampling.

A camera dolly on a straight rail; a laser measures its distance from the bumper at x = 0, so the expected reading of a particle at x_i is x_i itself:

import math, random
from bisect import bisect_right
from itertools import accumulate

rng = random.Random(21)
n, sigma = 8, 0.3
xs = [float(k) for k in range(n)]                   # guesses spread along the rail
ws = [1 / n] * n
true = 2.0
for step in range(6):
    true += 0.5                                     # the dolly moves 0.5 m ...
    xs = [x + 0.5 + rng.gauss(0, 0.1) for x in xs] # ... and so does every guess, give or take
    z = true + rng.gauss(0, sigma)                  # the laser's reading
    ws = [w * math.exp(-(z - x) ** 2 / (2 * sigma ** 2)) for x, w in zip(xs, ws)]
    total = sum(ws)
    ws = [w / total for w in ws]
    est = sum(w * x for w, x in zip(ws, xs))       # estimate before resampling
    n_eff = 1 / sum(w * w for w in ws)
    line = f"step {step}: true {true:.2f}  estimate {est:.3f}  N_eff {n_eff:.2f}"
    if n_eff < n / 2:                               # resample only when the cloud has thinned out
        C, u = list(accumulate(ws)), rng.random()
        xs = [xs[bisect_right(C, (u + k) / n)] for k in range(n)]
        ws = [1 / n] * n
        line += "  resampled"
    print(line)

step 0: true 2.50  estimate 2.348  N_eff 1.22  resampled
step 1: true 3.00  estimate 3.053  N_eff 6.46
step 2: true 3.50  estimate 3.529  N_eff 6.05
step 3: true 4.00  estimate 4.014  N_eff 5.54
step 4: true 4.50  estimate 4.600  N_eff 5.09
step 5: true 5.00  estimate 5.002  N_eff 3.38  resampled

The first reading rules out most of the rail at once (N_\text{eff} 1.22), so the filter resamples. For the next four cycles the weights carry over and N_\text{eff} sinks, 6.46, 6.05, 5.54, 5.09, as the evidence builds up; at step 5 it drops below 4 and the filter resamples again.

A 2D filter must also report a heading, and headings cannot be averaged like numbers: 3.0 and -3.05 both face nearly along -x, but their average faces +x. Average them as arrows instead: add up w_i(\cos\theta_i, \sin\theta_i) and take the direction of the sum,

import math
headings = [3.0, -3.05, 2.9, -2.95]                 # all nearly facing -x
weights = [0.3, 0.2, 0.3, 0.2]
plain = sum(w * t for w, t in zip(weights, headings)) / sum(weights)
c = sum(w * math.cos(t) for w, t in zip(weights, headings))
s = sum(w * math.sin(t) for w, t in zip(weights, headings))
print("plain average", round(plain, 4), "  circular mean", round(math.atan2(s, c), 4))
print("length of the summed arrow", round(math.hypot(c, s), 4), "of", sum(weights))
c2, s2 = math.cos(0.4) + math.cos(0.4 + math.pi), math.sin(0.4) + math.sin(0.4 + math.pi)
print(f"two opposite arrows: length {math.hypot(c2, s2):.1e}")

plain average 0.57   circular mean 3.083
length of the summed arrow 0.9855 of 1.0
two opposite arrows: length 2.5e-16

The plain average, 0.57 rad, points the wrong way entirely. The summed arrow's length (0.9855 out of a possible 1.0) says the headings agree closely. Two opposite arrows cancel to rounding dust, and atan2 of dust is a meaningless angle: test the length against a small tolerance times the total weight before calling atan2. And atan2 can return exactly -\pi, which a problem may want reported as \pi. The positions are ordinary weighted means.

Common mistakes: resetting the weights to 1/n every cycle instead of only after resampling. Recording the estimate after resampling. <= where the rule says strictly below n/2. Averaging angles as numbers.

Practice: One Answer From the Whole Cloud, The Hospital Runner in the Long Corridor

The Kalman filter, in numpy

The Kalman filter describes the belief by a Gaussian: a mean \mu and a covariance matrix P (variances on the diagonal, how the errors go together off it). It is taught step by step in the Control lesson, from one number to a position-and-velocity state; read it first if it is new. In matrix form, a prediction moves the belief with the model, \mu \leftarrow F\mu and P \leftarrow FPF^\top + Q, and a reading z = H\mu + \text{noise of covariance } R corrects it:

\nu is the innovation (what the reading says that the prediction did not), S its covariance and K the gain. With a pose of three numbers these are small matrices, and numpy writes each line as it reads. The few pieces needed:

import numpy as np
A = np.array([[1.0, 2.0], [3.0, 4.0]])
B = np.array([[0.5, 0.0], [1.0, 2.0]])
print("A * B  ", (A * B).tolist())                  # entry by entry
print("A @ B  ", (A @ B).tolist())                  # the matrix product
print("A.T    ", A.T.tolist())
v = np.array([1.0, -1.0])
print("A @ v  ", (A @ v).tolist(), "  v @ A @ v", float(v @ A @ v))
print("solve  ", np.round(np.linalg.solve(A, v), 6).tolist(), " inv(A) @ v", np.round(np.linalg.inv(A) @ v, 6).tolist())
print(np.eye(2).tolist(), np.diag([0.3, 0.1]).tolist())

A * B   [[0.5, 0.0], [3.0, 8.0]]
A @ B   [[2.5, 4.0], [5.5, 8.0]]
A.T     [[1.0, 3.0], [2.0, 4.0]]
A @ v   [-1.0, -1.0]   v @ A @ v 0.0
solve   [-3.0, 2.0]  inv(A) @ v [-3.0, 2.0]
[[1.0, 0.0], [0.0, 1.0]] [[0.3, 0.0], [0.0, 0.1]]

* multiplies entry by entry; @ is the matrix product. .T is the transpose, np.eye(n) the identity, np.diag a diagonal matrix. A 1D array works as a row or a column as needed, so v @ A @ v is the number v^\top A v. np.linalg.solve(A, v) finds x with Ax = v, the same as inv(A) @ v without forming the inverse. .tolist() and float(...) turn results back into plain Python. Build arrays from floats (np.array(P, dtype=float)): an integer array silently truncates a float stored into it.

A parked robot believes it is at (4, 1.5) and gets a radio fix of both coordinates:

import numpy as np
mu = np.array([4.0, 1.5])                           # the position estimate (m)
P = np.array([[0.30, 0.10], [0.10, 0.20]])          # its covariance
z = np.array([4.5, 1.2])                            # a radio fix of both coordinates
R = np.diag([0.25, 0.25])                           # the fix's covariance
H = np.eye(2)                                       # the fix measures x and y directly

nu = z - H @ mu                                     # the innovation
S = H @ P @ H.T + R                                 # its covariance
K = P @ H.T @ np.linalg.inv(S)                      # the gain
mu = mu + K @ nu
P = (np.eye(2) - K @ H) @ P
print("mean", np.round(mu, 4).tolist())
print("P   ", np.round(P, 4).tolist())

mean [4.2316, 1.4263]
P    [[0.1316, 0.0263], [0.0263, 0.1053]]

The estimate moves part of the way to the fix in each coordinate, and both variances shrink. Everything else about this filter is in the Control lesson; the next two sections change only where F and H come from.

Common mistakes: * where @ is meant. Forgetting .T. Integer arrays. Returning numpy arrays where plain lists are asked for (.tolist()).

Practice: A Kitchen Scale That Grows More Sure, Reading a Lift's Speed From a Rangefinder, A Cyclist in a Tunnel: Wheel Count or GPS?

The EKF predicts: move the mean, linearise the doubt

A robot's motion is not linear. The unicycle model of Level 2, as one Euler step of \Delta t seconds with speed v and turn rate \omega, moves the position along the old heading and turns afterwards:

The \cos\theta and \sin\theta mean there is no matrix F. The extended Kalman filter moves the mean through the true model and the covariance through the model's best straight-line approximation at the current mean: its Jacobian G, the matrix of partial derivatives of (x', y', \theta') with respect to (x, y, \theta), as in Level 4. Only the heading enters non-linearly, so

with G evaluated at the mean before the step, and Q the doubt the motion itself adds each step.

import math
import numpy as np

mu = np.array([0.0, 0.0, 0.0])
P = np.diag([0.01, 0.01, 0.0025])                   # heading known to 0.05 rad
Q = np.diag([0.0004, 0.0004, 0.0001])
v, w, dt = 0.8, 0.2, 0.5
for k in range(8):
    th = mu[2]
    G = np.array([[1.0, 0.0, -v * dt * math.sin(th)],    # built at the mean BEFORE the step
                  [0.0, 1.0, v * dt * math.cos(th)],
                  [0.0, 0.0, 1.0]])
    mu = mu + np.array([v * dt * math.cos(th), v * dt * math.sin(th), w * dt])   # old heading
    P = G @ P @ G.T + Q
    if k % 2 == 1:
        sd = np.sqrt(np.diag(P))
        print(f"step {k + 1}: mean {np.round(mu, 3).tolist()}  sd {np.round(sd, 3).tolist()}  cov(y, theta) {P[1, 2]:.5f}")

step 2: mean [0.798, 0.04, 0.2]  sd [0.104, 0.111, 0.052]  cov(y, theta) 0.00203
step 4: mean [1.572, 0.238, 0.4]  sd [0.108, 0.134, 0.054]  cov(y, theta) 0.00416
step 6: mean [2.292, 0.585, 0.6]  sd [0.116, 0.162, 0.056]  cov(y, theta) 0.00628
step 8: mean [2.928, 1.069, 0.8]  sd [0.128, 0.191, 0.057]  cov(y, theta) 0.00829

The rover drives mostly along x. Its x doubt grows slowly, but its y doubt climbs from 0.11 m to 0.19 m: a heading error of e radians puts the rover about e \cdot v\,\Delta t off to the side each step. The growing covariance between y and \theta records that link (a heading too far left goes with a position too far left), and it is what will later let a heading measurement correct the position.

The straight-line approximation has limits. Give a rover a large heading doubt and a long drive, and compare with sampling:

import math, random
import numpy as np
rng = random.Random(2)
v_dt, s_th = 3.0, 0.3                               # a long drive with a big heading doubt
ends = []
for _ in range(20000):
    th = rng.gauss(0, s_th)                         # a start heading drawn from the belief
    ends.append((v_dt * math.cos(th), v_dt * math.sin(th)))
xs, ys = [e[0] for e in ends], [e[1] for e in ends]
mx, my = sum(xs) / len(xs), sum(ys) / len(ys)
print("sampled mean", round(mx, 3), round(my, 3), "  sd", round(np.std(xs), 3), round(np.std(ys), 3))
G = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, v_dt], [0.0, 0.0, 1.0]])
P = G @ np.diag([0.0, 0.0, s_th ** 2]) @ G.T
print("EKF mean     ", v_dt, 0.0, "  sd", round(math.sqrt(P[0, 0]), 3), round(math.sqrt(P[1, 1]), 3))

sampled mean 2.869 0.006   sd 0.18 0.858
EKF mean      3.0 0.0   sd 0.0 0.9

The true end points lie on an arc (a banana, as it is called): their mean is pulled back to 2.87 m and they spread 0.18 m along x, where the EKF's ellipse claims no spread at all. A particle filter would carry the banana; the EKF is accurate when each step is short and the heading doubt small, which is why it predicts in small steps.

Common mistakes: building G from the heading after the turn. Forgetting G.T, or adding Q once at the end instead of every step. Not wrapping the final heading.

Practice: How the Doubt Grows While the Rover Drives

The EKF corrects: range and bearing to a landmark

A common robot sensor reports a known landmark (l_x, l_y) as a range and a bearing, the angle from the robot's heading, counter-clockwise positive (Level 1). From the mean pose it expects

and the measurement Jacobian H, two rows for the two readings and three columns for x, y, \theta, is

The signs are worth a sentence each. Moving the robot towards the landmark shortens the range, hence the minus signs in the first row. Turning the robot left makes every landmark appear further right: -1. A Jacobian derived by hand deserves Level 4's check by central differences:

import math
x, y, th = 2.0, 1.0, 0.3                            # the pose estimate
lx, ly = 6.0, 4.0                                   # a known landmark

def expect(x, y, th):
    dx, dy = lx - x, ly - y
    return math.hypot(dx, dy), math.atan2(dy, dx) - th

dx, dy = lx - x, ly - y
q = dx * dx + dy * dy
H = [[-dx / math.sqrt(q), -dy / math.sqrt(q), 0.0],
     [dy / q, -dx / q, -1.0]]
print("expected", [round(v, 4) for v in expect(x, y, th)])
print("H       ", [[round(v, 4) for v in row] for row in H])
h = 1e-5
cols = []
for i in range(3):
    up, down = [x, y, th], [x, y, th]
    up[i] += h
    down[i] -= h
    cols.append([(a - b) / (2 * h) for a, b in zip(expect(*up), expect(*down))])
print("nudged  ", [[round(cols[i][r], 4) for i in range(3)] for r in range(2)])

expected [5.0, 0.3435]
H        [[-0.8, -0.6, 0.0], [0.12, -0.16, -1.0]]
nudged   [[-0.8, -0.6, 0.0], [0.12, -0.16, -1.0]]

The update is then the Kalman update of the last section with this H, and one twist: the bearing part of the innovation, b - \hat b, must be wrapped into (-\pi, \pi]. Two bearings either side of \pm\pi are close, not a full turn apart. After the update, wrap the mean's heading as well. A boat with a buoy almost straight behind it measures only the bearing, so H is a single row and S a single number:

import math
import numpy as np

def wrap(a):
    a = a % (2 * math.pi)
    return a - 2 * math.pi if a > math.pi else a

mu = np.array([0.0, 0.0, 0.05])
P = np.diag([0.04, 0.04, 0.01])
lx, ly = -5.0, 0.1                                  # a buoy almost straight behind
b, rb = -3.10, 0.0004                               # measured bearing and its variance

dx, dy = lx - mu[0], ly - mu[1]
q = dx * dx + dy * dy
b_hat = math.atan2(dy, dx) - mu[2]
h = np.array([dy / q, -dx / q, -1.0])               # one reading: H is a single row
print("expected", round(b_hat, 4), " raw difference", round(b - b_hat, 4), " wrapped", round(wrap(b - b_hat), 4))
nu = wrap(b - b_hat)
S = h @ P @ h + rb                                  # a 1 by 1 "matrix": a number
K = P @ h / S
mu = mu + K * nu
mu[2] = wrap(mu[2])
P = P - np.outer(K, h) @ P                          # (I - K H) P
print("mean", np.round(mu, 4).tolist())
print("sd  ", np.round(np.sqrt(np.diag(P)), 4).tolist())

expected 3.0716  raw difference -6.1716  wrapped 0.1116
mean [0.0015, 0.0744, -0.043]
sd   [0.2, 0.1862, 0.0408]

Unwrapped, the innovation would be -6.17 rad, nearly a whole turn, and the filter would wreck its estimate. Wrapped, it is 0.11 rad: the buoy appears a little further left than expected. The filter explains that by turning its heading estimate right (0.05 to -0.043) and sliding the boat a little towards +y, each in proportion to its doubt. The x doubt barely changes: a bearing to a point straight behind says almost nothing about how far along x the boat is. With both readings, H has two rows, S is 2 by 2, and K = PH^\top S^{-1} needs np.linalg.inv(S) (or a solve).

Common mistakes: forgetting to wrap the bearing innovation. +1 instead of -1 for \partial\hat b/\partial\theta. \Delta x = x - l_x in one place and l_x - x in another. K * nu with a 2-vector innovation, where K @ nu is meant. Not wrapping the new heading.

Practice: A Range and a Bearing to the Lighthouse

Which landmark was it? The Mahalanobis distance

A sensor that sees several identical posts does not say which one it saw. The robot must associate each reading with a landmark, and the nearest landmark in metres is the wrong rule when the uncertainties differ. The Control lesson gated a one-number innovation by dividing it by its standard deviation \sqrt{S}, the z-score of Statistics. In two dimensions the same idea is the squared Mahalanobis distance,

a miss \mathbf{d} measured in standard deviations along each direction, where S adds up every source of doubt: the map's covariance for that landmark and the sensor's. A lidar places a lamp post at (3, 2.4); the map knows post 0 precisely but post 1 only roughly along x:

import numpy as np
p = np.array([3.0, 2.4])                            # where the lidar puts the post (world frame)
posts = [np.array([3.0, 2.0]), np.array([2.4, 2.4])]
covs = [np.diag([0.01, 0.01]), np.diag([0.25, 0.01])]   # post 1 is known only roughly along x
R = np.diag([0.02, 0.02])
for j, (m, cov) in enumerate(zip(posts, covs)):
    d = p - m
    S = cov + R
    d2 = float(d @ np.linalg.solve(S, d))           # d^T S^-1 d, without forming the inverse
    print(f"post {j}: {np.linalg.norm(d):.2f} m away, squared Mahalanobis {d2:.2f}")

post 0: 0.40 m away, squared Mahalanobis 5.33
post 1: 0.60 m away, squared Mahalanobis 1.33

Post 0 is nearer (0.40 m), but along a direction where everything is precise; post 1 is 0.60 m off along the direction where the map is vague. Measured in standard deviations, post 1 is the match.

A reading far from every landmark (a reflection, a post not on the map) should match nothing. The gate is a threshold on d^2: a match must have d^2 at most the gate. For a correct match with Gaussian errors in two dimensions, d^2 exceeds g with probability e^{-g/2}, which gives the usual gates. Sensor covariances are often given in the robot's frame, and then must be turned into the world frame first, R_w = \text{Rot}(\theta)\,R\,\text{Rot}(\theta)^\top:

import math
import numpy as np
for g in (5.99, 9.21, 13.8):
    print(f"gate {g}: a true match fails it with probability {math.exp(-g / 2):.4f}")
th = math.pi / 4                                    # the robot faces north-east
rot = np.array([[math.cos(th), -math.sin(th)], [math.sin(th), math.cos(th)]])
R_robot = np.diag([0.25, 0.01])                     # unsure along the beam, sharp across it
print("in the world:", np.round(rot @ R_robot @ rot.T, 4).tolist())

gate 5.99: a true match fails it with probability 0.0500
gate 9.21: a true match fails it with probability 0.0100
gate 13.8: a true match fails it with probability 0.0010
in the world: [[0.13, 0.12], [0.12, 0.13]]

The beam's doubt now lies along the north-east diagonal, which the off-diagonal 0.12 expresses. Among the landmarks inside the gate, take the smallest d^2 (a stated tie-break, usually the lowest index); if none is inside, the reading stays unmatched.

An EKF localizer does the same in measurement space: for each landmark, the innovation \nu and S = HPH^\top + R from the last section give d^2 = \nu^\top S^{-1}\nu (with the bearing part of \nu wrapped first). The best landmark within the gate gets the update; then the next reading of the same step is judged against the corrected mean and covariance.

Common mistakes: matching by plain distance. Leaving out the map's covariance, or adding a robot-frame covariance without rotating it. Comparing the square root with a gate meant for d^2. A strict < where the rule says "at most". Judging all readings of a step against the belief from before its first update.

Practice: Which Trunk Did the Lidar See?, The Tank Gauge That Sometimes Hears the Wrong Echo, The Museum Guide Finds Its Way Between the Pillars

A pose graph: closing the loop

A filter keeps only the current pose. When a robot drives a long loop and recognises its starting point, the error accumulated all the way round should be corrected everywhere along the loop, and a filter has already forgotten those poses. A pose graph keeps them: the poses x_0, x_1, \dots are nodes, and every measurement of how two poses relate is an edge, a constraint (i, j, d, \text{var}): "x_j - x_i was measured as d, with variance var". Odometry gives constraints between neighbours; recognising a place gives a loop closure between poses far apart in time. This is the back end of most SLAM (simultaneous localization and mapping) systems.

The constraints disagree a little, so choose the poses that make the weighted squared misses smallest, holding x_0 = 0 (the constraints are all differences, so otherwise the whole chain could slide):

Picture each constraint as a spring of natural length d and stiffness 1/\text{var}. At the minimum every partial derivative is zero. The term of one constraint, with w = 1/\text{var}, has derivative 2w(x_j - x_i - d) by x_j and the negative of that by x_i. Collecting the terms gives a linear system Hx = b, the normal equations of Machine Learning, built by adding each constraint's share: w to H_{ii} and H_{jj}, -w to H_{ij} and H_{ji}, +wd to b_j and -wd to b_i. Pinning x_0 removes row 0 and column 0.

A robot drives from its dock to a shelf and back; the last step went down a slippery ramp:

import numpy as np
cons = [(0, 1, 2.1, 0.04), (1, 2, 1.9, 0.04), (2, 3, -2.2, 0.04), (3, 4, -2.0, 0.16),
        (0, 4, 0.0, 0.001)]                         # four odometry steps, then "back at the dock"
n = 5
H, b = np.zeros((n, n)), np.zeros(n)
for i, j, d, var in cons:
    w = 1 / var
    H[i, i] += w
    H[j, j] += w
    H[i, j] -= w
    H[j, i] -= w
    b[j] += w * d
    b[i] -= w * d
print(H[1:, 1:].tolist())
x = [0.0] + np.linalg.solve(H[1:, 1:], b[1:]).tolist()
odo = [0.0]
for i, j, d, var in cons[:4]:
    odo.append(odo[-1] + d)
print("odometry alone", [round(v, 3) for v in odo])
print("pose graph    ", [round(v, 3) for v in x])
print("steps now     ", [round(x[k + 1] - x[k], 3) for k in range(4)])

[[50.0, -25.0, 0.0, 0.0], [-25.0, 50.0, -25.0, 0.0], [0.0, -25.0, 31.25, -6.25], [0.0, 0.0, -6.25, 1006.25]]
odometry alone [0.0, 2.1, 4.0, 1.8, -0.2]
pose graph     [0.0, 2.128, 4.057, 1.885, -0.001]
steps now      [2.128, 1.928, -2.172, -1.886]

Odometry alone ends 0.2 m from the dock it actually returned to. The precise loop closure pulls pose 4 back to almost exactly 0, and the 0.2 m is shared out by trust: the ramp step, with four times the variance of the others, changes by 0.114 m, each of the other three by 0.028 m. Every intermediate pose moves too, which no filter could do after the fact.

Common mistakes: solving the full H (it is singular until a pose is pinned). Weighting by var instead of 1/\text{var}. Getting the signs in b backwards for a constraint written from the later pose to the earlier one. Merging two constraints between the same poses instead of adding both.

Practice: Back at the Lift Door: Closing the Loop

Planning by sampling: the RRT

Level 5 planned on grids, with the searches of the Algorithms path. A grid must be fine enough to fit through gaps and small enough to search, and in open terrain that is a poor trade. A rapidly-exploring random tree (RRT) plans in continuous space instead. The tree starts at the robot; each round takes a sample point and grows the tree a little towards it:

1. Nearest. Find the tree node closest to the sample. Scan with a strict < (or use min, which keeps the first), so a tie goes to the node added earliest.
2. Steer. Move from that node towards the sample by at most step: if the sample is within reach, the new point is the sample itself; otherwise the point step metres along the line to it.
3. Collide. Reject the branch if the segment from the node to the new point comes within a disc obstacle. The distance from a centre c to the segment from a to b is Level 2's: project, t = \frac{(c - a)\cdot(b - a)}{|b - a|^2}, clamp t to [0, 1], and measure from c to a + t(b - a).
4. Add the point with its parent, the nearest node. If it is close enough to the goal, stop: walk the parents back to the root and reverse.

Samples come from a random generator in practice; the problems give them as a list so that every run grows the same tree.

import math, random

def seg_dist(c, a, b):
    dx, dy = b[0] - a[0], b[1] - a[1]
    t = ((c[0] - a[0]) * dx + (c[1] - a[1]) * dy) / (dx * dx + dy * dy)
    t = max(0.0, min(1.0, t))                       # stay on the segment
    return math.hypot(c[0] - (a[0] + t * dx), c[1] - (a[1] + t * dy))

pond = (5.0, 4.0, 1.5)                              # a disc: centre and radius
start, goal, step = (1.0, 1.0), (9.0, 7.0), 0.8
nodes, parent = [start], [None]
rng = random.Random(4)
for k in range(500):
    s = (rng.uniform(0, 10), rng.uniform(0, 8)) if k % 10 else goal   # every tenth sample: the goal
    near = min(range(len(nodes)), key=lambda i: math.dist(nodes[i], s))   # first on a tie
    a, d = nodes[near], math.dist(nodes[near], s)
    new = s if d <= step else (a[0] + step * (s[0] - a[0]) / d, a[1] + step * (s[1] - a[1]) / d)
    if seg_dist(pond[:2], a, new) <= pond[2]:       # touching the pond counts as hitting it
        continue
    nodes.append(new)
    parent.append(near)
    if math.dist(new, goal) <= 0.3:
        break
path, i = [], len(nodes) - 1
while i is not None:                                # walk the parents back to the root
    path.append(nodes[i])
    i = parent[i]
path.reverse()
print(len(nodes), "nodes after", k + 1, "samples;", len(path), "waypoints")
print("path length", round(sum(math.dist(p, q) for p, q in zip(path, path[1:])), 3), " straight line", round(math.dist(start, goal), 3))
plot([p[0] for p in nodes], [p[1] for p in nodes], kind="scatter", label="tree")
plot([p[0] for p in path], [p[1] for p in path], label="path", xlabel="x (m)", ylabel="y (m)")

39 nodes after 44 samples; 16 waypoints
path length 12.0  straight line 10.0

Press Run to see the tree reach round the pond. Steering the occasional sample straight at the goal (a goal bias) makes the tree head that way sooner. The parent list is all the bookkeeping a tree needs: following it from any node leads back to the root, the same trick as a BFS's parent dictionary. The path avoids the pond but is longer than it needs to be; the next section straightens it.

Common mistakes: checking only the new point, not the whole segment. Not clamping t (the infinite line may pass a disc that the segment never reaches). <= in the nearest scan, which keeps the last of equal nodes. Adding a rejected point to the tree. Returning the path to the goal instead of to the node that reached it.

Practice: One Branch Toward the Sample, A Tree That Grows Around the Boulders

Straightening a sampled path

A sampled path zigzags. Level 5 smoothed grid paths by line of sight; with a sampled path the usual tidy-up is shortcutting: pick two waypoints i < j, and if the straight segment between them is clear, drop every waypoint in between. Repeating with many pairs (often chosen at random) removes most of the wiggles.

When the pairs refer to the original indices, keep one flag per waypoint, "still on the path". A pair can be taken only while both its ends are still on the path; taking it clears the flags strictly between them. The clearance test is the RRT's, with the boundary stated: a segment exactly touching a disc is not clear.

import math

def clear(a, b, discs):
    dx, dy = b[0] - a[0], b[1] - a[1]
    for cx, cy, r in discs:
        t = max(0.0, min(1.0, ((cx - a[0]) * dx + (cy - a[1]) * dy) / (dx * dx + dy * dy)))
        if math.hypot(cx - a[0] - t * dx, cy - a[1] - t * dy) <= r:
            return False
    return True

def length(pts):
    return sum(math.dist(p, q) for p, q in zip(pts, pts[1:]))

path = [(0, 0), (1, 1.5), (2, 0.5), (3, 2), (4, 1), (5, 2.5), (6, 2)]
rocks = [(2.5, 1.8, 0.4)]
on = [True] * len(path)
for i, j in [(0, 3), (0, 2), (2, 5), (1, 4), (4, 6)]:
    ok = on[i] and on[j] and clear(path[i], path[j], rocks)
    if ok:
        for k in range(i + 1, j):
            on[k] = False
    print((i, j), "taken" if ok else "skipped", [k for k in range(len(path)) if on[k]])
kept = [path[k] for k in range(len(path)) if on[k]]
print("length", round(length(path), 3), "->", round(length(kept), 3))

(0, 3) skipped [0, 1, 2, 3, 4, 5, 6]
(0, 2) taken [0, 2, 3, 4, 5, 6]
(2, 5) taken [0, 2, 5, 6]
(1, 4) skipped [0, 2, 5, 6]
(4, 6) skipped [0, 2, 5, 6]
length 9.355 -> 6.785

(0, 3) would cut through the rock. (0, 2) and (2, 5) are clear and remove waypoints 1, 3 and 4. After that, (1, 4) and (4, 6) no longer apply: their ends are gone. The path shrinks from 9.36 m to 6.79 m.

Common mistakes: reading pair indices as positions in the shortened list. Taking a pair whose end was already removed. Treating a touching segment as clear. Testing only the end points.

Practice: Straightening the Rover's Wiggles

Potential fields: pulled to the goal, pushed by obstacles

A potential field steers without planning ahead: the goal pulls the robot and obstacles push it away, and it moves along the total force. The pull grows with the distance to the goal, k_{att}(\text{goal} - p). A round obstacle with centre o and radius r, at distance \rho from the robot's centre and d = \rho - r from its surface, pushes only inside an influence distance d_0:

The push is zero at the edge of its influence and grows without bound near the surface; \frac{p - o}{\rho} is the unit vector pointing away from the centre. The forces are the downhill direction of a potential, so following them is gradient descent, as in Machine Learning, with the same weakness: it stops wherever the slope is flat, and with obstacles that can be short of the goal, a local minimum. A delivery robot meets a crate:

import math

def force(p, goal, discs, k_att=0.8, k_rep=1.0, d0=1.5):
    fx, fy = k_att * (goal[0] - p[0]), k_att * (goal[1] - p[1])
    for ox, oy, r in discs:
        rho = math.hypot(p[0] - ox, p[1] - oy)
        d = rho - r                                 # distance to the surface, not the centre
        if d < d0:
            mag = k_rep * (1 / d - 1 / d0) / d ** 2
            fx += mag * (p[0] - ox) / rho           # away from the centre
            fy += mag * (p[1] - oy) / rho
    return fx, fy

goal, crate = (8.0, 0.0), [(4.0, 0.0, 0.8)]
for start in [(0.0, 0.0), (0.0, 0.4)]:
    p = start
    for k in range(600):
        fx, fy = force(p, goal, crate)
        f = math.hypot(fx, fy)
        if math.dist(p, goal) <= 0.05 or f < 0.01:
            break
        scale = min(0.02 * f, 0.1) / f              # step 0.02 F, at most 0.1 m long
        p = (p[0] + scale * fx, p[1] + scale * fy)
    print(start, "->", (round(p[0], 3), round(p[1], 3) + 0.0), "after", k, "steps; force", round(f, 4))

(0.0, 0.0) -> (2.667, 0.0) after 35 steps; force 0.0081
(0.0, 0.4) -> (7.956, 0.022) after 354 steps; force 0.0399

Started on the line through the crate's centre, every force points along that line, and the robot stops 0.53 m in front of the crate, where the push balances the pull. Started 0.4 m off the line, the push has a sideways part, and the robot slides round the crate to the goal. Capping the step's length (not each component) keeps the robot from leaping across an obstacle where the force is huge. The step factor matters too: near the surface the push changes quickly, and a large factor makes the robot overshoot back and forth instead of settling.

Common mistakes: measuring d to the centre instead of the surface. Pushing towards the obstacle. Forgetting to divide by \rho, which scales the push by the distance. Letting an obstacle beyond d_0 push (with a negative magnitude it pulls). Testing the stopping rules in another order than the one specified.

Practice: Pulled to the Charger, Pushed by the Bins, Stuck in Front of the Pillar

Pure pursuit: the arc through the carrot

To follow a path, a robot can repeatedly pick a point on it some distance ahead, the "carrot", and drive the circular arc that leaves in the direction it faces and passes through that point. This is pure pursuit. Only the carrot's position in the robot's frame (Level 1: x_r forward, y_r to the left) matters:

An arc that leaves the robot along its heading has its centre straight to the left, at (0, R) in the robot's frame. The carrot is on the circle when x_r^2 + (y_r - R)^2 = R^2, that is x_r^2 + y_r^2 = 2y_rR, so with d^2 = x_r^2 + y_r^2:

The curvature \kappa is positive for a left turn and 0 whenever the carrot lies on the robot's line of travel (y_r = 0): dead ahead, but also dead behind, where the formula would drive straight away from it (real followers pick a carrot ahead of the robot). Driving at speed v along the arc means turning at \omega = v\kappa, and Level 2's differential drive with wheels b apart turns that into wheel speeds v \mp \omega b/2.

import math
x, y, th = 3.0, 1.0, 2.0                            # a mower's pose
px, py = 1.0, 3.5                                   # the carrot
dx, dy = px - x, py - y
x_r = math.cos(th) * dx + math.sin(th) * dy         # ahead
y_r = -math.sin(th) * dx + math.cos(th) * dy        # to the left
kappa = 2 * y_r / (dx * dx + dy * dy)
v, b = 0.6, 0.45
omega = v * kappa
print("robot frame", round(x_r, 4), round(y_r, 4), " curvature", round(kappa, 4), " radius", round(1 / kappa, 4))
print("wheels", round(v - omega * b / 2, 4), round(v + omega * b / 2, 4))
R = 1 / kappa
cx, cy = x - R * math.sin(th), y + R * math.cos(th) # the arc's centre, R to the robot's left
print("carrot's distance from the centre", round(math.hypot(px - cx, py - cy), 4))

robot frame 3.1055 0.7782  curvature 0.1518  radius 6.5855
wheels 0.5795 0.6205
carrot's distance from the centre 6.5855

The carrot is 3.1 m ahead and 0.78 m to the left: a gentle left arc of radius 6.59 m, so the right wheel runs a little faster. The last line checks the geometry: the carrot lies exactly on the circle. Note that d^2 does not depend on the heading, so it can be computed from the world offsets.

Common mistakes: using the world offset \Delta y instead of y_r (right only when the robot faces +x). The sign of the rotation into the robot's frame. Dividing by d instead of d^2. Swapping the wheels: a left turn needs the right wheel faster.

Practice: Bend Toward the Carrot

Where is the carrot? A circle meets the path

The carrot is the first point of the path, from the robot's progress onwards, at the lookahead distance L from the robot. The path is a polyline: segment i is A + t\,D with D = B - A and 0 \le t \le 1. A point is at distance L from the robot R when |A + tD - R|^2 = L^2, a quadratic in t. With F = A - R,

A negative discriminant means the segment's line misses the circle. Otherwise try the smaller root first. A root counts if it lies on the segment and, on the segment where the progress is, not behind it. Walk the segments in order from there; if no root counts, aim at the path's last point.

import math
path = [(0, 0), (3, 0), (3, 3), (6, 3)]
robot, L, (k, s) = (2.5, 0.6), 1.5, (0, 0.7)        # progress: 70% along segment 0
carrot = path[-1]                                   # if nothing counts: aim at the end
for i in range(k, len(path) - 1):
    (ax, ay), (bx, by) = path[i], path[i + 1]
    Dx, Dy, Fx, Fy = bx - ax, by - ay, ax - robot[0], ay - robot[1]
    a, b, c = Dx * Dx + Dy * Dy, 2 * (Dx * Fx + Dy * Fy), Fx * Fx + Fy * Fy - L * L
    disc = b * b - 4 * a * c
    roots = [] if disc < 0 else [(-b - math.sqrt(disc)) / (2 * a), (-b + math.sqrt(disc)) / (2 * a)]
    print("segment", i, "roots", [round(t, 4) for t in roots])
    lo = s if i == k else 0.0                       # behind the progress only on segment k
    good = [t for t in roots if lo <= t <= 1]
    if good:
        carrot = (ax + good[0] * Dx, ay + good[0] * Dy)
        break
print("carrot", tuple(round(v, 4) for v in carrot))

segment 0 roots [0.3751, 1.2916]
segment 1 roots [-0.2714, 0.6714]
carrot (3.0, 2.0142)

On segment 0 one crossing is behind the progress (0.375 < 0.7) and the other is past the segment's end. Segment 1 has a root before its start and one at t = 0.6714: the carrot is on the corner's far side, and the robot will start turning before it reaches the corner. That early turn is what makes pure pursuit cut corners smoothly; a larger L cuts them more.

Common mistakes: applying the progress limit to later segments. Starting from segment 0 instead of the progress. t < 1, which misses a crossing at a segment's very end. Returning nothing when no crossing is left.

Practice: Where Is the Carrot Now?

Steering by the heading error

A simpler follower aims straight at the next waypoint and lets a PID controller from Control choose the turn rate. The robotics twist is the error: an angle, the direction to the waypoint minus the heading, wrapped into (-\pi, \pi] so the robot turns the short way. Wrap with %, as in Level 1: math.atan2(math.sin(a), math.cos(a)) is tempting, but it returns -\pi for a half turn where the convention wants +\pi, and a half-turn error is exactly what a robot facing away from its goal gets.

The rest is the usual loop, in a stated order: the integral adds e * dt; the derivative is (e - previous) / dt, or 0 on the first step when there is no previous error; the output is clamped to the steering's limit; the robot then moves one Euler step with the old heading and turns. A waypoint counts as reached within an acceptance radius, checked at every position, the start included. A small boat with heading 3.0, almost along -x, must reach a buoy ahead of it and a little to its left:

import math

def wrap(a):
    a = a % (2 * math.pi)
    return a - 2 * math.pi if a > math.pi else a

print("atan2 way:", math.atan2(math.sin(-math.pi), math.cos(-math.pi)), "  % way:", wrap(-math.pi))

def steer(kp, ki, kd, wrap_error, steps=400):
    x, y, th = 0.0, 0.0, 3.0
    buoy, v, dt = (-4.0, -1.0), 0.5, 0.1
    I, prev, xs, ys = 0.0, None, [x], [y]
    for n in range(steps):
        if math.dist(buoy, (x, y)) <= 0.3:          # arrived
            break
        e = math.atan2(buoy[1] - y, buoy[0] - x) - th
        if wrap_error:
            e = wrap(e)
        I += e * dt
        D = 0.0 if prev is None else (e - prev) / dt
        prev = e
        omega = max(-1.0, min(1.0, kp * e + ki * I + kd * D))   # the rudder's limit
        x, y = x + v * math.cos(th) * dt, y + v * math.sin(th) * dt
        th = wrap(th + omega * dt)
        xs.append(x)
        ys.append(y)
    plot(xs, ys, label="wrapped" if wrap_error else "not wrapped", xlabel="x (m)", ylabel="y (m)")
    return n, round(sum(math.dist(p, q) for p, q in zip(zip(xs, ys), list(zip(xs, ys))[1:])), 2)

print("error wrapped:    steps, metres", steer(1.5, 0.0, 0.2, True))
print("error not wrapped:steps, metres", steer(1.5, 0.0, 0.2, False))

atan2 way: -3.141592653589793   % way: 3.141592653589793
error wrapped:    steps, metres (77, 3.85)
error not wrapped:steps, metres (140, 7.0)

The direction to the buoy is -2.90 rad and the heading 3.0, so the raw difference is -5.90: "turn almost a full circle clockwise". Wrapped, it is +0.39: a slight left turn. With the wrapped error the boat arrives in 77 steps; without it, it swings round a whole loop and needs 140. Press Run to see both tracks. The other details matter as much in a simulation that must match a specification: the derivative's first step, the clamp, nothing reset when the target waypoint changes, and the position moved with the old heading.

Common mistakes: an unwrapped error. Wrapping with atan2(sin, cos) where -\pi must become \pi. Moving with the new heading. Resetting the integral or the previous error at each waypoint. Checking arrival only after moving.

Practice: The Campus Shuttle Steers by Its Error

Task logic as a finite-state machine

Above the controllers sits the logic of the task: search, approach, grasp, return. The simplest way to write it is a finite-state machine: the robot is always in exactly one state, and an event from its sensors may move it to another. Keep the transitions as data, a dictionary from (state, event) to the next state; then running the machine is one lookup per event, and dict.get with the current state as the default ignores every event the state does not care about. A robot lawn mower:

TABLE = {("mow", "rain"): "dock", ("mow", "battery_low"): "dock",
         ("dock", "docked"): "charge", ("charge", "full"): "mow",
         ("charge", "rain"): "charge", ("dock", "bumped"): "stuck"}

state, log = "mow", ["mow"]
for event in ["bumped", "rain", "rain", "docked", "full", "battery_low", "bumped", "full"]:
    state = TABLE.get((state, event), state)        # no row: ignore the event
    log.append(state)
print(log)

['mow', 'mow', 'dock', 'dock', 'charge', 'mow', 'dock', 'stuck', 'stuck']

A bump while mowing is ignored (the table has no row for it), the second shower changes nothing while docking, and a bump on the way to the dock leaves the mower "stuck", a state with no rows at all, so nothing gets it out but a person. Writing the table first and checking every (state, event) pair against it finds such gaps before the robot does. The log has one more entry than there are events: the start state comes first.

Common mistakes: a chain of if statements that raises an error, or jumps to a default state, on an unexpected event. Forgetting the start state in the log. Letting a final state react to events.

Practice: Fetch the Mug, One Event at a Time

Behaviour trees

State machines grow tangled as tasks grow: every new behaviour needs transitions to and from many states. A behaviour tree organises the same decisions as a tree that is ticked from the root many times a second. Every node answers a tick with "SUCCESS", "FAILURE" or "RUNNING" (still busy):

• a condition leaf checks something and answers success or failure; an action leaf does something and reports how it went;
• a Sequence ticks its children in order and stops at the first one that does not succeed, returning that status; if all succeed (or there are none), it succeeds. It means "do these in order";
• a Fallback ticks its children in order and stops at the first one that does not fail; if all fail (or there are none), it fails. It means "try these until one works";
• a Not swaps success and failure and leaves running alone.

When every tick starts again from the root (a memoryless tree), a condition that becomes true can interrupt an action that was running in another branch. A tree is a recursive structure, so its tick is a recursive function. Here each node is a small Python function that ticks its children; the problems store the tree as nested tuples, where one function tick(node) that branches on the node's kind does the same job.

def leaf(name, kind, world, ran):
    def tick():
        if kind == "action":
            ran.append(name)                        # record which actions were reached
        return world[name]
    return tick

def sequence(*kids):
    def tick():
        for kid in kids:
            status = kid()
            if status != "SUCCESS":
                return status                       # stop at the first FAILURE or RUNNING
        return "SUCCESS"
    return tick

def fallback(*kids):
    def tick():
        for kid in kids:
            status = kid()
            if status != "FAILURE":
                return status                       # stop at the first SUCCESS or RUNNING
        return "FAILURE"
    return tick

world, ran = {}, []
root = sequence(fallback(leaf("door_open", "cond", world, ran), leaf("open_door", "action", world, ran)),
                leaf("drive_through", "action", world, ran))
for door, opener in [("FAILURE", "RUNNING"), ("SUCCESS", "RUNNING"), ("FAILURE", "FAILURE")]:
    world.update(door_open=door, open_door=opener, drive_through="RUNNING")
    ran.clear()
    print(root(), ran)

RUNNING ['open_door']
RUNNING ['drive_through']
FAILURE ['open_door']

"Make sure the door is open, then drive through": the Fallback succeeds at once if the door is open and otherwise tries to open it. In the first tick the opener is still busy, so the Sequence stops there, running. In the second the door is open and the robot drives. In the third someone has shut the door: the next tick starts at the root, notices it, and the robot goes back to opening it instead of driving into it; the opener fails, so the whole tree fails, and drive_through is never reached.

Common mistakes: treating "RUNNING" as success in a Sequence or as failure in a Fallback. Recording conditions as actions. Remembering which child was running at the last tick when the tree is memoryless. An empty Sequence must succeed and an empty Fallback must fail.

Practice: The Night Guard's Behaviour Tree

Two fingers: grasps that do not slip

A gripper with two fingers squeezes an object along the line between its contact points. With friction coefficient \mu, a finger can push on a surface in any direction within the angle \arctan\mu of the surface's inward normal: its friction cone. The grasp holds without slipping when the squeeze line lies inside the cones at both contacts, an antipodal grasp. The margin, \arctan\mu minus the larger of the two angles, says how much slack it has.

For a flat object seen from above as a convex polygon, the inward normal of an edge with direction (d_x, d_y) depends on which way round the vertices are listed. The sign of the shoelace sum \sum_i (x_i y_{i+1} - x_{i+1} y_i) says: positive for counter-clockwise, where the inside is on the left of each edge, (-d_y, d_x); negative for clockwise, where it is on the right. The angle between two vectors is best taken as \operatorname{atan2}(|u \times v|, u \cdot v), which needs no lengths and stays accurate near 0, where acos of a rounded dot product loses digits.

import math
block = [(0, 0), (1, 3), (4, 3), (5, 0)]            # a trapezoid, listed clockwise
n = len(block)
area2 = sum(block[i][0] * block[(i + 1) % n][1] - block[(i + 1) % n][0] * block[i][1] for i in range(n))
sign = 1 if area2 > 0 else -1
print("shoelace", area2, "->", "counter-clockwise" if sign > 0 else "clockwise")

def contact(e, t):
    (ax, ay), (bx, by) = block[e], block[(e + 1) % n]
    dx, dy = bx - ax, by - ay
    L = math.hypot(dx, dy)
    return (ax + t * dx, ay + t * dy), (-sign * dy / L, sign * dx / L)   # point, inward normal

def angle(u, v):
    return math.atan2(abs(u[0] * v[1] - u[1] * v[0]), u[0] * v[0] + u[1] * v[1])

mu = 0.25
for (e1, t1), (e2, t2) in [((1, 0.5), (3, 0.5)), ((0, 0.5), (2, 0.5))]:
    (p1, n1), (p2, n2) = contact(e1, t1), contact(e2, t2)
    d = (p2[0] - p1[0], p2[1] - p1[1])              # finger 1 squeezes towards finger 2
    worst = max(angle(d, n1), angle((-d[0], -d[1]), n2))
    print(p1, p2, "worst angle", round(worst, 4), "cone", round(math.atan(mu), 4), "margin", round(math.atan(mu) - worst, 4))

shoelace -24 -> clockwise
(2.5, 3.0) (2.5, 0.0) worst angle 0.0 cone 0.245 margin 0.245
(0.5, 1.5) (4.5, 1.5) worst angle 0.3218 cone 0.245 margin -0.0768

The top and bottom edges are parallel, so squeezing between their midpoints lines up with both normals: margin 0.245 rad, the whole cone. The two slanted sides lean outwards by 0.32 rad each, more than the cone's 0.245, so fingers there would squeeze the block out of their grip like a wet bar of soap: a negative margin. With \mu = 0.4 (\arctan\mu = 0.38) the same pair would hold.

Common mistakes: outward normals from ignoring the polygon's orientation. Comparing angles with \mu instead of \arctan\mu. Checking the cone at one finger only. acos of a dot product that rounding has pushed past 1.

Practice: Two Fingers, One Pebble

Practice

These problems are required on this path at this level; the path adds one problem from each of the level's topics, picked for you and kept.

• Which Guesses Agree With the Beacon?
• A Comb Through the Weights
• A Range and a Bearing to the Lighthouse
• A Tree That Grows Around the Boulders
• Bend Toward the Carrot
• The Night Guard's Behaviour Tree

Checklist

You can now:

• Move a particle cloud with given noise draws, weigh it by a Gaussian likelihood, and judge it by its effective sample size.
• Resample with a low-variance comb, and run whole particle-filter cycles that resample only when needed.
• Average a cloud's headings on the circle, and know when no average exists.
• Write a Kalman filter in numpy with @, .T, np.eye and np.linalg.solve.
• Predict with an EKF through a unicycle step and its Jacobian, and correct it with a range-bearing reading, wrapping the bearing innovation.
• Associate readings with landmarks by Mahalanobis distance, with a gate and a rotated sensor covariance.
• Correct a whole trajectory with a 1D pose graph and a loop closure.
• Grow an RRT around disc obstacles, recover its path, and shorten it by shortcuts.
• Steer by potential fields, pure pursuit and a PID loop on a wrapped heading error.
• Write task logic as a finite-state machine or a behaviour tree, and test a two-finger grasp against friction cones.

Further reading: Wikipedia: Monte Carlo localization, Wikipedia: Extended Kalman filter and Wikipedia: Rapidly exploring random tree.