Level 3 · Robotics: Reaching and Sensing

Level 2 ran an arm forwards: given the joint angles, where is the hand? A robot that has to pick an apple, plug in a cable or touch up a weld needs the opposite question: the hand must go there, so which angles? That is inverse kinematics, and for a two-link arm in a plane it has an answer in closed form: one triangle, the law of cosines and atan2. You will find the elbow's bend, both mirror-image ways to reach a point, which of them the joint stops allow and which is the smallest move, how to reach from a parked robot, how to aim a tool as well as place it (wrist first), and what happens when the hand has to follow a straight line, including the poses where the two solutions meet.

The second half turns to sensing. A laser or sonar range reading is a ray from the robot that stopped at something, and a map lets the robot predict that stop: in a bare room, through an occupancy grid, against wall segments, at round pillars, inside a sonar's cone. Run backwards, readings tell the robot where it is: two bearings fix a position (triangulation), three ranges fix it too (trilateration). Then the honest part: readings are noisy, biased and sometimes wild, errors grow with distance, and the best a robot can do is ask which pose explains its scan.

The paths meet often here. The bias, spread and median of a sensor are the statistics of Statistics Level 2; the calibration line is the least-squares line of Statistics Level 3, and the same lesson's Bayes' rule is how Level 4 of this path turns "which pose explains the scan" into probabilities. The cross product that told left from right in Level 2 solves every ray-and-wall question below. And Level 4 asks how fast the hand moves when the joints do, which matters most near the special poses at the end of the first half.

The elbow from the law of cosines

Take Level 2's two-link arm: shoulder at the origin, links l_1 and l_2, relative joint angles t_1 (from +x) and t_2 (from the first link's direction). The shoulder only swings the whole arm round; how far the hand is from the shoulder depends on the elbow alone. Square and add the forward kinematics and the shoulder angle drops out:

(the cross terms are 2l_1l_2(\cos t_1\cos(t_1+t_2) + \sin t_1\sin(t_1+t_2)) = 2l_1l_2\cos t_2). This is the law of cosines for the triangle shoulder, elbow, hand, written with the bend t_2 instead of the triangle's inner angle \pi - t_2, which is why the sign is +. Solved for the bend:

An orchard robot's picking arm has links of 0.9 m and 0.6 m, and an apple hangs at (1.0, 0.7):

import math
l1, l2 = 0.9, 0.6                 # link lengths in metres
x, y = 1.0, 0.7                   # the apple, in the shoulder's frame

c = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
bend = math.acos(c)               # radians, between 0 and pi
print("distance", round(math.hypot(x, y), 4), " cos t2", round(c, 4))
print("bend", round(bend, 4), "rad, about", round(math.degrees(bend)), "degrees")

for t1 in (0.0, 1.0, -2.0):       # any shoulder angle: the hand stays at the same distance
    hx = l1 * math.cos(t1) + l2 * math.cos(t1 + bend)
    hy = l1 * math.sin(t1) + l2 * math.sin(t1 + bend)
    print("t1", t1, "-> hand distance", round(math.hypot(hx, hy), 4))

distance 1.2207  cos t2 0.2963
bend 1.27 rad, about 73 degrees
t1 0.0 -> hand distance 1.2207
t1 1.0 -> hand distance 1.2207
t1 -2.0 -> hand distance 1.2207

math.acos answers in radians between 0 and \pi: 0 is a straight arm, \pi folded back on itself. The last three lines check the claim: with that bend the hand is at the apple's distance whatever the shoulder does. Finding the shoulder angle that points it at the apple is the next section.

Two things can go wrong before acos. A target outside the ring of Level 2 has no triangle at all, and its "cosine" falls outside [-1, 1]; so test reachability first, on the distance, with a small margin for rounding. And a target exactly on an edge of the ring can produce a cosine a hair beyond 1, which acos refuses:

import math
l1, l2 = 0.9, 0.6
for x, y in [(1.0, 0.7), (1.3, 0.75), (0.2, 0.1), (0.0, 1.5)]:
    r = math.hypot(x, y)
    c = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
    reachable = abs(l1 - l2) - 1e-9 <= r <= l1 + l2 + 1e-9
    print((x, y), "r", round(r, 4), " cos", round(c, 4), " reachable", reachable)

l1, l2 = 1.2, 0.3                 # another arm; (0.9, 1.2) is exactly 1.5 away: full reach
c = (0.9 ** 2 + 1.2 ** 2 - l1 ** 2 - l2 ** 2) / (2 * l1 * l2)
print(repr(c))
try:
    math.acos(c)
except ValueError:
    print("acos refuses it: ValueError")
print("clamped:", math.acos(max(-1.0, min(1.0, c))))

(1.0, 0.7) r 1.2207  cos 0.2963  reachable True
(1.3, 0.75) r 1.5008  cos 1.0023  reachable False
(0.2, 0.1) r 0.2236  cos -1.037  reachable False
(0.0, 1.5) r 1.5  cos 1.0  reachable True
1.0000000000000002
acos refuses it: ValueError
clamped: 0.0

So the order is: distance test with a margin (refuse what is really out of reach), then clamp the cosine into [-1, 1] (forgive the rounding dust), then acos. Clamping without the distance test would quietly turn an unreachable apple into a straight arm.

Common mistakes: acos before the reachability test. Feeding r instead of r^2 into the formula. Reporting the triangle's inner angle \pi - t_2 as the bend. A strict < at the edges, or no margin at all: in floating point 1.17 + 0.13 is 1.2999999999999998.

Practice: How Far Must the Elbow Bend?

The shoulder, and two elbows

The bend fixes the distance; the shoulder fixes the direction. Freeze the shoulder at t_1 = 0: the hand then sits at (l_1 + l_2\cos t_2,\ l_2\sin t_2), in the direction

Turning the shoulder by t_1 turns that point by t_1, so to face the target's direction \operatorname{atan2}(y, x):

acos gave the bend's size; its sign is free. +b and -b give two arms that are mirror images across the line from the shoulder to the target. This path names them by the sign of t_2: elbow-up is t_2 < 0 (walking from the shoulder to the target, the elbow is on your left), elbow-down is t_2 > 0 (on your right), and when both exist, elbow-up is listed first.

import math
l1, l2 = 0.9, 0.6
x, y = 1.0, 0.7
b = math.acos((x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2))

for name, t2 in [("elbow-up", -b), ("elbow-down", b)]:
    t1 = math.atan2(y, x) - math.atan2(l2 * math.sin(t2), l1 + l2 * math.cos(t2))
    ex, ey = l1 * math.cos(t1), l1 * math.sin(t1)
    hx, hy = ex + l2 * math.cos(t1 + t2), ey + l2 * math.sin(t1 + t2)
    print(name, " t1", round(t1, 4), " t2", round(t2, 4),
          " elbow", (round(ex, 3), round(ey, 3)), " hand", (round(hx, 4), round(hy, 4)))
    plot([0, ex, hx], [0, ey, hy], label=name, xlabel="x (m)", ylabel="y (m)")

elbow-up  t1 1.0994  t2 -1.27  elbow (0.409, 0.802)  hand (1.0, 0.7)
elbow-down  t1 0.122  t2 1.27  elbow (0.893, 0.11)  hand (1.0, 0.7)

Press Run: two arms, one elbow above the line to the apple and one below, both hands on (1.0, 0.7). Use atan2, never atan(y / x): only atan2 knows which quadrant the target is in.

Two more details make a solver complete. First, the shoulder angle is a difference of two angles and can leave (-\pi, \pi], so wrap it with Level 2's wrap. Behind and below the shoulder, elbow-down needs it:

import math

def wrap(a):                       # into (-pi, pi]
    a = math.fmod(a, 2 * math.pi)
    if a <= -math.pi:
        a += 2 * math.pi
    elif a > math.pi:
        a -= 2 * math.pi
    return a

l1, l2 = 0.9, 0.6
x, y = -1.1, -0.3
b = math.acos((x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2))
t1 = math.atan2(y, x) - math.atan2(l2 * math.sin(b), l1 + l2 * math.cos(b))
print("elbow-down t1:", round(t1, 4), "-> wrapped", round(wrap(t1), 4))

elbow-down t1: -3.425 -> wrapped 2.8582

Second, the edges of the ring. At full reach the only way is the straight arm, t_2 = 0; folded back, t_2 = \pi. There is one solution there, not two equal copies. Decide this on the distance (within 10^{-9} of l_1 + l_2 or of |l_1 - l_2|), not by testing whether acos came out exactly 0: a cosine one rounding step below 1 gives 1.5e-08, and you would report two arms that are the same arm.

Inverse kinematics is hard and forward kinematics is easy, so check every answer forwards: put the angles into Level 2's chain walk and measure how far the hand lands from the target. A tolerance on that distance (a circle around the target, math.hypot) is the right test; a tolerance on x and y separately accepts a square.

import math

def hand(lengths, q):              # Level 2: turn, then walk each link
    x = y = heading = 0.0
    for length, angle in zip(lengths, q):
        heading += angle
        x += length * math.cos(heading)
        y += length * math.sin(heading)
    return x, y

target = (1.0, 0.7)
for q in [(1.0994, -1.27), (0.122, 1.27), (1.0994, 1.27), (0.1, 1.27)]:
    hx, hy = hand([0.9, 0.6], q)
    print(q, "misses by", round(math.hypot(hx - target[0], hy - target[1]), 4))

(1.0994, -1.27) misses by 0.0
(0.122, 1.27) misses by 0.0
(1.0994, 1.27) misses by 1.1461
(0.1, 1.27) misses by 0.0269

The first two are the solutions above, rounded to a few decimals: they miss by less than a tenth of a millimetre. The third mixes the shoulder of one solution with the elbow of the other, and is far off; the fourth is a near miss that a tolerance of 1 cm would reject.

Common mistakes: atan(y / x). Adding the offset instead of subtracting it. Returning two copies at full stretch. Forgetting to wrap t_1. Checking a solution by comparing angles with another solver's angles instead of checking where the hand lands.

Practice: Elbow Up or Elbow Down, Trust, Then Check the Hand

Choosing among solutions: stops and the smallest move

Two solutions are a choice, and a real arm has reasons to prefer one. The first is joint stops: cables and hoses run through the joints, so each joint turns only between two limits. The stops do not change the geometry, only rule solutions out: solve as if there were none, then keep the solutions inside them. Angles come out of trigonometry with rounding dust, so a stop gets the same 10^{-9} of forgiveness as the ring's edges.

The second is effort: when the arm is already somewhere, the best solution is often the one its joints reach with the least turning. A joint can turn either way round, so the turn from a to b is the wrapped difference \text{wrap}(b - a), and a natural cost is the sum of the joints' turns:

Here the arm is at (c_1, c_2) = (-3.0, 1.2), reaching behind itself, and its elbow may only bend one way:

import math

def wrap(a):
    a = math.fmod(a, 2 * math.pi)
    if a <= -math.pi:
        a += 2 * math.pi
    elif a > math.pi:
        a -= 2 * math.pi
    return a

def solutions(l1, l2, x, y):       # inside the ring only: elbow-up, then elbow-down
    b = math.acos((x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2))
    return [(wrap(math.atan2(y, x) - math.atan2(l2 * math.sin(t2), l1 + l2 * math.cos(t2))), t2)
            for t2 in (-b, b)]

sols = solutions(0.9, 0.6, -1.1, -0.3)
stops = ((-math.pi, math.pi), (0.0, 2.6))          # the elbow bends counter-clockwise only
for t1, t2 in sols:
    ok = all(lo - 1e-9 <= q <= hi + 1e-9 for q, (lo, hi) in zip((t1, t2), stops))
    print((round(t1, 4), round(t2, 4)), "inside the stops:", ok)

c1, c2 = -3.0, 1.2
for t1, t2 in sols:
    raw = abs(t1 - c1) + abs(t2 - c2)
    short = abs(wrap(t1 - c1)) + abs(wrap(t2 - c2))
    print((round(t1, 4), round(t2, 4)), " raw cost", round(raw, 4), " wrapped cost", round(short, 4))

(-2.3257, -1.4501) inside the stops: False
(2.8582, 1.4501) inside the stops: True
(-2.3257, -1.4501)  raw cost 3.3245  wrapped cost 3.3245
(2.8582, 1.4501)  raw cost 6.1083  wrapped cost 0.6751

Elbow-down's shoulder angle, 2.858, looks almost a full turn from -3.0, but the short way round, past \pm\pi, it is only 0.425 away. With raw differences the arm would choose elbow-up and swing much further than it needs to. When two costs are equal, a rule must decide (on this path usually: the first in the list, elbow-up), and "equal" means within a small tolerance, because two costs that are equal on paper can differ by 4e-16 in floating point. When no solution survives the stops, say so (None) rather than return an empty choice.

Common mistakes: checking the stops before wrapping t_1. Testing only one joint. Raw differences in the cost. Letting floating-point noise break a tie that the rules decide.

Practice: Solutions the Stops Allow, The Smallest Move to the Next Bolt

Reaching from a robot

An arm on a mobile robot knows nothing about the map. Its shoulder is bolted to the robot at a mount point (m_x, m_y) in the robot's frame (x forward, y to the left), and its first joint angle is measured from the robot's forward direction. A target given in world coordinates must first be moved into the robot's frame, exactly as in Level 1: shift by the robot's position, then rotate by -\theta. Then measure it from the shoulder by subtracting the mount, and solve the ordinary two-link arm.

A cleaning robot parked at (3.0, 1.0), facing \theta = 2.0, wants to wipe a spot at (2.4, 2.1); its shoulder sits 0.25 m ahead of its centre:

import math
X, Y, theta = 3.0, 1.0, 2.0          # the robot's pose in the world
mx, my = 0.25, 0.0                   # the shoulder, in the robot's frame
tx, ty = 2.4, 2.1                    # the spot, in the world

dx, dy = tx - X, ty - Y              # shift...
c, s = math.cos(theta), math.sin(theta)
xr = c * dx + s * dy                 # ...then rotate by -theta
yr = -s * dx + c * dy
print("in the robot's frame:", round(xr, 4), round(yr, 4))
x, y = xr - mx, yr - my              # measured from the shoulder
print("from the shoulder:  ", round(x, 4), round(y, 4), " distance", round(math.hypot(x, y), 4))

l1, l2 = 0.6, 0.5
b = math.acos((x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2))
t1 = math.atan2(y, x) - math.atan2(l2 * math.sin(-b), l1 + l2 * math.cos(-b))
print("elbow-up:", round(t1, 4), round(-b, 4), " first link points at world heading", round(theta + t1, 4))

in the robot's frame: 1.2499 0.0878
from the shoulder:   0.9999 0.0878  distance 1.0038
elbow-up: 0.4699 -0.8466  first link points at world heading 2.4699

The spot is ahead and a little to the left of the robot, and the arm's angles are relative to the robot: add \theta to t_1 to get the first link's world direction. Rotating by +\theta instead of -\theta is the classic slip; it puts the spot behind the robot.

Common mistakes: rotating before shifting. Rotating by +\theta. Subtracting the mount in world coordinates (it is a robot-frame offset). Measuring t_1 from the world's +x.

Practice: Reaching From a Parked Robot

Orientation too: wrist first

A brush, a torch or a screwdriver must touch the right spot and point the right way. Add a third link, the tool, behind a wrist joint: now there are three joint angles and three requirements, the hand's position (x, y) and its orientation \varphi (the direction the last link points, from +x). The orientation untangles everything. If the last link of length l_3 must point along \varphi and end at the hand, the wrist can only be at

one tool length behind the hand. The first two links must put their end exactly there: an ordinary two-link problem. Whatever direction they leave the second link pointing in (t_1 + t_2), the wrist turns the rest of the way:

A glue robot must touch (1.1, 0.2) with its 0.25 m nozzle pointing straight down, \varphi = -\pi/2:

import math

def wrap(a):
    a = math.fmod(a, 2 * math.pi)
    if a <= -math.pi:
        a += 2 * math.pi
    elif a > math.pi:
        a -= 2 * math.pi
    return a

l1, l2, l3 = 0.9, 0.6, 0.25
x, y, phi = 1.1, 0.2, -math.pi / 2
wx, wy = x - l3 * math.cos(phi), y - l3 * math.sin(phi)
print("wrist at", round(wx, 4), round(wy, 4))

b = math.acos((wx * wx + wy * wy - l1 * l1 - l2 * l2) / (2 * l1 * l2))
for t2 in (-b, b):
    t1 = wrap(math.atan2(wy, wx) - math.atan2(l2 * math.sin(t2), l1 + l2 * math.cos(t2)))
    t3 = wrap(phi - t1 - t2)
    pts, heading, px, py = [(0.0, 0.0)], 0.0, 0.0, 0.0          # check forwards
    for length, angle in zip((l1, l2, l3), (t1, t2, t3)):
        heading += angle
        px, py = px + length * math.cos(heading), py + length * math.sin(heading)
        pts.append((px, py))
    print([round(t, 4) for t in (t1, t2, t3)], " hand", (round(px, 4), round(py, 4)), " pointing", round(heading, 4))
    plot([p[0] for p in pts], [p[1] for p in pts], xlabel="x (m)", ylabel="y (m)")

wrist at 1.1 0.45
[0.9027, -1.3443, -1.1291]  hand (1.1, 0.2)  pointing -1.5708
[-0.126, 1.3443, -2.7891]  hand (1.1, 0.2)  pointing -1.5708

Both solutions end at the spot pointing along -\pi/2; the wrist takes up whatever the first two joints leave. Reachability is now a question about the wrist point: a hand position inside the ring can still be impossible with a given orientation, because pointing the tool back towards the shoulder pushes the wrist outwards.

Common mistakes: adding the tool length instead of subtracting it. Testing reachability on the hand instead of the wrist. Using \varphi as t_3 (the angles are relative). Forgetting to wrap t_3.

Practice: Hold the Brush Level: Wrist First

A straight line for the hand

A pen, a glue nozzle or a cutting blade must often move along a straight line. A joint controller follows lists of angles, not lines, so the planner samples the line and solves the inverse kinematics at each sample. With n samples from S to G, sample i is

so that p_0 = S and p_{n-1} = G (with i / n the line would stop short). Keep one branch along the line: mixing elbow-up and elbow-down from sample to sample would make the arm jump between mirror poses. A sample out of reach gets a placeholder such as None in its place, so that entry i still belongs to sample i.

Between two samples each motor simply turns at a steady rate from one angle to the next, the short way. Halfway, joint k is at a_k + \frac{1}{2}\text{wrap}(b_k - a_k). A straight line in joint space is a curve in the plane, and forward kinematics shows how far the hand strays. The distance from a point Q to the line through S with direction d = G - S is the cross product's size over the direction's length, |d \times (Q - S)| / |d|.

import math

def wrap(a):
    a = math.fmod(a, 2 * math.pi)
    if a <= -math.pi:
        a += 2 * math.pi
    elif a > math.pi:
        a -= 2 * math.pi
    return a

def elbow_up(l1, l2, x, y):
    b = math.acos((x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2))
    return (wrap(math.atan2(y, x) - math.atan2(-l2 * math.sin(b), l1 + l2 * math.cos(b))), -b)

def hand(l1, l2, t1, t2):
    return (l1 * math.cos(t1) + l2 * math.cos(t1 + t2), l1 * math.sin(t1) + l2 * math.sin(t1 + t2))

l1, l2 = 0.9, 0.6
S, G = (1.3, -0.4), (0.3, 1.0)
dx, dy = G[0] - S[0], G[1] - S[1]
for n in (2, 3, 5, 9):
    qs = [elbow_up(l1, l2, S[0] + i / (n - 1) * dx, S[1] + i / (n - 1) * dy) for i in range(n)]
    worst = 0.0
    for a, b in zip(qs, qs[1:]):
        mid = [a[k] + wrap(b[k] - a[k]) / 2 for k in (0, 1)]
        hx, hy = hand(l1, l2, *mid)
        worst = max(worst, abs(dx * (hy - S[1]) - dy * (hx - S[0])) / math.hypot(dx, dy))
    print(n, "samples: largest stray halfway", round(worst, 4), "m")
    if n == 3:
        print("   angles", [(round(t1, 3), round(t2, 3)) for t1, t2 in qs])

xs, ys = [], []                                     # the pen between 3 samples, finely
qs = [elbow_up(l1, l2, S[0] + i / 2 * dx, S[1] + i / 2 * dy) for i in range(3)]
for a, b in zip(qs, qs[1:]):
    for j in range(21):
        h = hand(l1, l2, *[a[k] + j / 20 * wrap(b[k] - a[k]) for k in (0, 1)])
        xs.append(h[0])
        ys.append(h[1])
plot(xs, ys, label="pen, 3 samples", xlabel="x (m)", ylabel="y (m)")
plot([S[0], G[0]], [S[1], G[1]], label="the line")

2 samples: largest stray halfway 0.3849 m
3 samples: largest stray halfway 0.1108 m
   angles [(0.051, -0.89), (1.055, -1.99), (1.89, -1.645)]
5 samples: largest stray halfway 0.0308 m
9 samples: largest stray halfway 0.0081 m

Press Run: with three samples the pen bulges away from the line between them; halving the gap between samples cuts the stray by a factor of three to four. The samples themselves are always exact. Without the wrap in the halfway angle, a shoulder that crosses \pm\pi between two samples would swing nearly a full turn the long way round.

Common mistakes: i / n instead of i / (n - 1). Dropping unreachable samples, which shifts every later entry. Switching branch between samples. Averaging two angles without wrapping their difference. Measuring the stray to the nearest sample instead of to the line.

Practice: Ruling a Straight Line, Straight in the Plan, Curved in the Air

Singular poses: where the two elbows meet

Elbow-up and elbow-down are two separate families of solutions. Follow a path on one branch and the joints move smoothly; jump to the other branch and the elbow must swing from -b to +b, a turn of 2b, while the hand is supposed to stay put. On a real robot that is a flip: lift the tool, swing, start again.

The two branches meet in one kind of pose only: the arm stretched (t_2 = 0, on the outer edge of the ring) or folded (t_2 = \pi, on the inner edge). There the two solutions are the same solution, so an arm passing through such a pose can leave it on either branch without any jump. These poses are called singular. Watch the branches close in as the hand moves out along a ray to the edge of the ring:

import math
l1, l2 = 0.9, 0.6
for r in (1.0, 1.3, 1.45, 1.49, 1.499, 1.5):
    c = max(-1.0, min(1.0, (r * r - l1 * l1 - l2 * l2) / (2 * l1 * l2)))
    b = math.acos(c)
    print("r", r, " elbow-up t2", round(-b, 4) + 0.0, " elbow-down t2", round(b, 4), " gap between branches", round(2 * b, 4))

r 1.0  elbow-up t2 -1.7289  elbow-down t2 1.7289  gap between branches 3.4577
r 1.3  elbow-up t2 -1.0685  elbow-down t2 1.0685  gap between branches 2.1369
r 1.45  elbow-up t2 -0.5288  elbow-down t2 0.5288  gap between branches 1.0575
r 1.49  elbow-up t2 -0.2359  elbow-down t2 0.2359  gap between branches 0.4717
r 1.499  elbow-up t2 -0.0745  elbow-down t2 0.0745  gap between branches 0.1491
r 1.5  elbow-up t2 0.0  elbow-down t2 0.0  gap between branches 0.0

The gap between the branches closes to zero at the edge, and it closes fast: the last millimetre turns the elbow by 0.07 rad on each branch. Near a singular pose a tiny hand motion needs a large joint motion, and in a stretched arm the hand cannot move outwards at all; Level 4 measures this with the Jacobian.

A planner that follows a path with joint stops is therefore a small state machine: remember the current branch and whether the previous sample was singular. At each sample, keep the current branch if its solution is allowed; otherwise switch to the other branch, and that switch is a flip, unless the arm has just passed through a singular pose, where it is free. If no solution at a sample is allowed, the path cannot be followed.

Common mistakes: choosing each sample's branch afresh (that is not following a path). Counting the free switch after a singular pose as a flip. Deciding "singular" by testing an acos result against exactly 0 or \pi instead of by the distance.

Practice: Where the Elbow Must Flip

A range reading is a ray

A laser range finder sends a beam and times the echo; a sonar does the same with sound. Either way the robot gets a range: how far the beam went before it hit something. On a scanner the beam has an angle a measured from the robot's heading (counter-clockwise positive), so in the world it leaves the robot's position P along

and the point at distance d along it is P + d\,u. That is Level 1's "drive d along a heading", and also a complex number: complex(x, y) + d * cmath.exp(1j * (theta + a)), the turning arrow of the Control path.

To predict a reading from a map, find the smallest d \ge 0 at which the beam meets something. Every sensor has a maximum range; beyond it there is no echo, and the sensor reports the maximum itself. A reading equal to the maximum range means "nothing seen", not "a wall at exactly that distance", so it must not be drawn as a point.

A wall that is a straight line y = c is easy: the beam's y is y_0 + d\sin\alpha (with \alpha = \theta + a), so it reaches the wall at d = (c - y_0)/\sin\alpha, which is only a forward hit when it comes out positive. Only the wall on the side the beam moves towards can be hit, so the sign of \sin\alpha picks it, and a zero sine means the beam never reaches that wall. A robot scans a corridor 2.4 m wide whose end wall is the line x = 9:

import math
x, y, theta = 1.0, 0.8, 0.3            # the robot's pose
max_range = 6.0
xs, ys = [], []
for k in range(7):
    a = -1.2 + 0.4 * k                  # beam angle from the heading
    alpha = theta + a
    c, s = math.cos(alpha), math.sin(alpha)
    d = math.inf
    if s > 0:
        d = min(d, (2.4 - y) / s)       # the left wall, y = 2.4
    if s < 0:
        d = min(d, (0.0 - y) / s)       # the right wall, y = 0
    if c > 0:
        d = min(d, (9.0 - x) / c)       # the end wall, x = 9
    d = min(d, max_range)
    if d < max_range:
        xs.append(x + d * c)
        ys.append(y + d * s)
    print("beam", round(a, 1), " range", round(d, 3), "" if d < max_range else " (no echo)")
plot(xs, ys, kind="scatter", xlabel="x (m)", ylabel="y (m)")

beam -1.2  range 1.021 
beam -0.8  range 1.669 
beam -0.4  range 6.0  (no echo)
beam 0.0  range 5.414 
beam 0.4  range 2.484 
beam 0.8  range 1.795 
beam 1.2  range 1.604 

Press Run: the echoes lie on the two corridor walls. The beam at -0.4 runs almost along the corridor and would meet the right-hand wall only after 8 m, so it reports 6.0 and draws nothing.

Common mistakes: forgetting to add the heading to the beam angle. Dividing by a sine or cosine of 0 (test the sign first). Taking the largest distance instead of the smallest. Plotting max-range readings as obstacles.

Practice: The Vacuum's Laser in a Bare Room, Filming a Reflector on a Bicycle Wheel

Marching a beam through a grid

Many maps have no wall equations at all. An occupancy grid cuts the floor into square cells res metres wide and stores grid[row][col]: 1 for occupied, 0 for free. Printed plans put row 0 at the top, but the world's y grows upwards. With the map's bottom-left corner at the origin, the point (p_x, p_y) is in

and off the map when either floor falls outside its range. Use math.floor, not int: int rounds towards zero, so int(-0.2) is 0 and a point just left of the map would land in column 0.

import math
grid = [[0, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 0, 0],
        [0, 0, 0, 1, 0, 0],
        [0, 0, 0, 0, 0, 0]]
res = 0.5
rows, cols = len(grid), len(grid[0])

def cell(px, py):
    col, up = math.floor(px / res), math.floor(py / res)
    if not (0 <= col < cols and 0 <= up < rows):
        return None                                  # off the map
    return (rows - 1 - up, col)

for p in [(0.2, 0.3), (1.6, 1.2), (2.9, 1.99), (-0.1, 0.5), (3.0, 0.5)]:
    print(p, "->", cell(*p), "  int() would give column", int(p[0] / res))

(0.2, 0.3) -> (3, 0)   int() would give column 0
(1.6, 1.2) -> (1, 3)   int() would give column 3
(2.9, 1.99) -> (0, 5)   int() would give column 5
(-0.1, 0.5) -> None   int() would give column 0
(3.0, 0.5) -> None   int() would give column 6

To predict a laser reading on a grid, march: look up the cell under the points at distances step, 2 * step, ... along the beam, and stop at the first occupied cell, or when the beam leaves the map, or after the sensor's maximum number of steps. Compute each point as k * step from the start; adding step to a running total collects rounding errors.

import math
grid = [[0, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 0, 0],
        [0, 0, 0, 1, 0, 0],
        [0, 0, 0, 0, 0, 0]]
res, rows, cols = 0.5, 4, 6

def march(x, y, theta, step, n):
    for k in range(1, n + 1):
        px, py = x + k * step * math.cos(theta), y + k * step * math.sin(theta)
        col, up = math.floor(px / res), math.floor(py / res)
        if not (0 <= col < cols and 0 <= up < rows):
            return ("left the map after", round(k * step, 3))
        if grid[rows - 1 - up][col] == 1:
            return ("hit", (rows - 1 - up, col), "at", round(k * step, 3))
    return ("nothing within", round(n * step, 3))

print(march(0.3, 1.1, 0.05, 0.2, 20))
print(march(0.3, 1.1, 0.05, 0.6, 20))
print(march(0.3, 1.1, 0.05, 0.2, 5))

('hit', (1, 3), 'at', 1.4)
('left the map after', 3.0)
('nothing within', 1.0)

The middle line is the price of marching: with steps longer than a cell, the samples hop from column 2 to column 4 and the beam passes straight through a wall one cell thick. Steps must be shorter than a cell, and even then a beam can slip through the corner where two occupied cells touch diagonally. The reading is a multiple of the step, so it is only as precise as the step is short.

Common mistakes: floor(py / res) as the row (the map upside down). int instead of floor. Indexing the grid before checking the bounds (Python's negative indices hide the bug). A step longer than a cell.

Practice: Stepping a Beam Across the Floor Plan

Where a beam meets a wall

A building map is often a list of wall segments, each from A to B. The beam is P + t\,u with t \ge 0; the wall is A + s\,e with e = B - A and 0 \le s \le 1. They meet where

two equations in two unknowns. The 2-D cross product a \times b = a_x b_y - a_y b_x from Level 2 solves them in one line each: crossing both sides with e removes s (because e \times e = 0), crossing with u removes t. With w = A - P:

The beam hits the wall when t \ge 0 (forwards) and 0 \le s \le 1 (between the end points, both included). When u \times e = 0 the wall is parallel to the beam: no single crossing point, and on this path the beam grazes it. Among all the walls hit, the nearest decides the reading.

import math
x, y, theta = 0.5, 1.0, 0.0                  # facing +x
ux, uy = math.cos(theta), math.sin(theta)
walls = {"slanted": ((2, 3), (3, 1.5)),
         "along the beam": ((1, 1), (3, 1)),
         "behind": ((-1, 0), (-1, 3)),
         "end point on the beam": ((2.5, 1), (3.5, -1)),
         "far": ((4, 0), (4, 2))}
best = 10.0                                   # the maximum range
for name, ((ax, ay), (bx, by)) in walls.items():
    ex, ey = bx - ax, by - ay
    den = ux * ey - uy * ex                   # u x e
    if den == 0:
        print(name, ": parallel, skipped")
        continue
    wx, wy = ax - x, ay - y
    t = (wx * ey - wy * ex) / den             # (w x e) / (u x e)
    s = (wx * uy - wy * ux) / den             # (w x u) / (u x e)
    hit = t >= 0 and 0 <= s <= 1
    print(name, ": t =", round(t, 4), " s =", round(s, 4) + 0.0, " hit" if hit else " miss")
    if hit and t < best:
        best = t
print("reading:", best)

slanted : t = 2.8333  s = 1.3333  miss
along the beam : parallel, skipped
behind : t = -1.5  s = 0.3333  miss
end point on the beam : t = 2.0  s = 0.0  hit
far : t = 3.5  s = 0.5  hit
reading: 2.0

The slanted wall's line crosses the beam, but at s = 1.33, beyond its end: a miss. The wall behind gives a negative t. The fourth wall touches the beam with its end point (s = 0), which counts, and it is nearer than the far wall. Here the parallel test is exact because the numbers are exact; with computed directions, two parallel vectors can have a cross product of 1e-16, so a problem may ask you to compare |u \times e| with a small tolerance instead.

Common mistakes: accepting s outside [0, 1] (the wall's line, not the wall). Accepting negative t. Dividing by a zero cross product. Swapping the order inside a cross product, which flips its sign.

Practice: Where the Lidar Beam Stops in the Workshop

Round obstacles: a beam meets a circle

Tree trunks, pillars and table legs are circles: centre C, radius \rho. The beam's point P + t\,u is on the circle when |P + t\,u - C|^2 = \rho^2. Because u has length 1 this is a quadratic in t,

with roots t = -\beta \pm \sqrt{\beta^2 - \gamma}. A negative discriminant \beta^2 - \gamma means the beam misses; zero means it just touches (a tangent, which counts as a hit); otherwise the beam enters at the smaller root and leaves at the larger, and the reading is the entry. A robot standing outside the circle has \gamma > 0, so both roots have the same sign, and negative roots mean the circle is behind it.

A robot scans the pillars of a car park:

import math
x, y, theta = 0.0, 0.0, 0.0
pillars = [(3.0, 1.0, 0.4), (5.0, -1.5, 0.5), (2.0, -2.0, 0.3)]
max_range = 6.0
xs, ys = [], []
for k in range(21):
    a = -1.0 + 0.1 * k
    ux, uy = math.cos(theta + a), math.sin(theta + a)
    best = None
    for cx, cy, rho in pillars:
        px, py = x - cx, y - cy
        beta = ux * px + uy * py
        disc = beta * beta - (px * px + py * py - rho * rho)
        if disc < 0:
            continue                          # this beam misses this pillar
        t = -beta - math.sqrt(disc)           # the entry point
        if 0 <= t <= max_range and (best is None or t < best):
            best = t
    if best is not None:
        xs.append(x + best * ux)
        ys.append(y + best * uy)
        if k % 4 == 0:
            print("beam", round(a, 1), "stops at", (round(xs[-1], 3), round(ys[-1], 3)), "after", round(best, 3))
print(len(xs), "of 21 beams have an echo")
plot(xs, ys, kind="scatter", xlabel="x (m)", ylabel="y (m)")

beam -0.2 stops at (4.947, -1.003) after 5.048
beam 0.2 stops at (2.967, 0.601) after 3.027
7 of 21 beams have an echo

Press Run: each pillar shows up as a short arc of points, its near side only; the far side is in the pillar's own shadow. A beam aimed at a centre stops \rho short of it, at |PC| - \rho.

Common mistakes: the larger root (the far side). Skipping tangents with disc <= 0. Keeping the first pillar hit rather than the nearest. Forgetting the heading in the beam's direction.

Practice: A Scan Between the Orchard Trunks

A sonar hears a cone

A laser beam is thin. Sound spreads: a sonar's ping fills a cone of half-angle \psi around the direction it faces, and the echo reports the distance of the nearest thing anywhere in the cone, without saying where in the cone it was. A point is inside when the angle between the facing direction and the direction to the point, its bearing, is at most \psi either side. Directions are angles on a circle, so the difference must be wrapped before it is compared:

import math

def wrap(a):
    a = math.fmod(a, 2 * math.pi)
    if a <= -math.pi:
        a += 2 * math.pi
    elif a > math.pi:
        a -= 2 * math.pi
    return a

x, y, theta = 0.0, 0.0, -3.0          # the sonar faces left, a little below -x
half = 0.3
for px, py in [(-2.0, -0.3), (-3.0, 0.1), (-1.0, 1.0), (1.5, 0.0)]:
    direction = math.atan2(py - y, px - x)
    raw = direction - theta
    print((px, py), " raw difference", round(raw, 3), " wrapped", round(wrap(raw), 3),
          " inside" if abs(wrap(raw)) <= half else " outside", " distance", round(math.hypot(px - x, py - y), 3))

(-2.0, -0.3)  raw difference 0.007  wrapped 0.007  inside  distance 2.022
(-3.0, 0.1)  raw difference 6.108  wrapped -0.175  inside  distance 3.002
(-1.0, 1.0)  raw difference 5.356  wrapped -0.927  outside  distance 1.414
(1.5, 0.0)  raw difference 3.0  wrapped 3.0  outside  distance 1.5

The point at (-3.0, 0.1) has direction 3.108, which looks 6.1 rad away from a heading of -3.0; wrapped, it is only 0.175 rad off, inside the cone. The sonar's reading would be 2.022, from the first point: the nearest inside the cone, with the cap at the maximum range as for any range sensor. A wide cone is a blunt instrument: the robot learns that something is about 2 m away somewhere in a 34-degree wedge.

Common mistakes: comparing bearings without wrapping. Comparing the bearing itself with the half-angle instead of its difference from the heading. Using the full cone width as the half-angle. Taking the nearest point overall, inside the cone or not.

Practice: The Parking Sonar's Cone

Two bearings: triangulation

So far the robot knew its pose and predicted readings. Now run it backwards: from readings to a position. A camera that sees a landmark at a known place L measures its bearing b from the robot's heading \theta; with a compass for \theta, the landmark lies in the world direction \alpha = \theta + b from the robot. That does not place the robot, but walk backwards from the landmark along that direction and you trace every place it could be: the line L - t\,d with d = (\cos\alpha, \sin\alpha) and t \ge 0 the distance. A second landmark gives a second line, and the robot is where they cross. This is triangulation.

Two lines L_1 - t_1 d_1 and L_2 - t_2 d_2 cross where L_1 - t_1 d_1 = L_2 - t_2 d_2. Cross both sides with d_2 to remove t_2, which leaves t_1 = ((L_1 - L_2) \times d_2) / (d_1 \times d_2). When d_1 \times d_2, the sine of the angle between the two directions, is close to 0 the lines are parallel and give no single point.

import math
theta = 0.4                                   # the compass heading
masts = [(8.0, 6.0), (-2.0, 7.0)]
true = (2.0, 1.0)                             # used only to invent consistent bearings
bearings = [math.atan2(my - true[1], mx - true[0]) - theta for mx, my in masts]
print("bearings", [round(b, 4) for b in bearings])

(l1x, l1y), (l2x, l2y) = masts
a1, a2 = theta + bearings[0], theta + bearings[1]
d1 = (math.cos(a1), math.sin(a1))
d2 = (math.cos(a2), math.sin(a2))
den = d1[0] * d2[1] - d1[1] * d2[0]           # d1 x d2
wx, wy = l1x - l2x, l1y - l2y
t1 = (wx * d2[1] - wy * d2[0]) / den          # distance from the robot to mast 1
print("mast 1 is", round(t1, 4), "m away; the robot is at",
      (round(l1x - t1 * d1[0], 4), round(l1y - t1 * d1[1], 4)))

bearings [0.2947, 1.7588]
mast 1 is 7.8102 m away; the robot is at (2.0, 1.0)

The fix is good when the two directions are far from parallel. Standing nearly in line with both landmarks, the two lines cross at a shallow angle, and a small error in a bearing slides the crossing a long way along them. Surveyors choose landmarks that are well apart as seen from the robot.

Common mistakes: using the bearings as world directions (forgetting the heading). Walking from the landmark forwards, L + t\,d. Testing the cross product with == 0 when directions come from trigonometry. Degrees.

Practice: Two Spires From the Canal Boat

Three ranges: trilateration

Radio beacons, ultra-wideband tags and GPS satellites give ranges instead: distances r_i to known points B_i = (x_i, y_i). Each range puts the robot on a circle,

and finding the point from several circles is trilateration. The squares are awkward, but every circle has the same x^2 + y^2 in it, so subtracting circle 1 from circle 2 cancels them and leaves a straight line:

Circle 1 against circle 3 gives a second line, and two lines in two unknowns are a 2×2 linear system, solved by Cramer's rule: for a_{11}x + a_{12}y = c_1 and a_{21}x + a_{22}y = c_2, with \det = a_{11}a_{22} - a_{12}a_{21},

When the three beacons lie on one straight line the determinant is 0: the two lines are parallel, and the circles' two crossing points mirror each other across the beacons' line, so no single answer exists.

import math
beacons = [(1, 1), (11, 2), (4, 9)]

def solve(ranges):
    (x1, y1), (x2, y2), (x3, y3) = beacons
    r1, r2, r3 = ranges
    a11, a12 = 2 * (x2 - x1), 2 * (y2 - y1)
    a21, a22 = 2 * (x3 - x1), 2 * (y3 - y1)
    c1 = r1 ** 2 - r2 ** 2 + x2 ** 2 - x1 ** 2 + y2 ** 2 - y1 ** 2
    c2 = r1 ** 2 - r3 ** 2 + x3 ** 2 - x1 ** 2 + y3 ** 2 - y1 ** 2
    det = a11 * a22 - a12 * a21
    return ((c1 * a22 - a12 * c2) / det, (a11 * c2 - a21 * c1) / det)

true = (5, 4)
exact = [math.dist(true, b) for b in beacons]
print("exact ranges", [round(r, 4) for r in exact], "->", [round(v, 4) for v in solve(exact)])

noisy = [exact[0] + 0.1, exact[1] - 0.05, exact[2] + 0.08]
p = solve(noisy)
print("noisy ranges ->", [round(v, 4) for v in p])
print("|P - B|^2 - r^2 for each beacon:", [round(math.dist(p, b) ** 2 - r ** 2, 4) for b, r in zip(beacons, noisy)])

exact ranges [5.0, 6.3246, 5.099] -> [5.0, 4.0]
noisy ranges -> [5.084, 3.9802]
|P - B|^2 - r^2 for each beacon: [-0.4493, -0.4493, -0.4493]

With exact ranges the answer is the true position. With noisy ranges the three circles no longer share a point, yet the method still returns one: the point where |P - B_i|^2 - r_i^2 is the same for all three beacons, as the last line shows. That is a definition worth stating whenever readings are imperfect, because other reasonable answers exist (averaging the pairwise circle crossings, or the least-squares point) and they differ slightly.

Common mistakes: dropping the beacon terms x_2^2 - x_1^2 + y_2^2 - y_1^2 (they vanish only when beacon 1 sits at the origin and the others on the axes). Pairing a range with the wrong beacon. Dividing by a zero determinant.

Practice: Three Radio Beacons in the Mine

Noise: bias, spread and stray echoes

A new range finder is pointed at a board exactly 1.2 m away (measured with a tape) and logs a batch of readings. Two numbers describe it. Its bias is the mean error, \bar{r} - d_{\text{true}}: positive means it reads long, and a known bias can simply be subtracted. Its spread is the sample standard deviation \sqrt{\sum (r - \bar{r})^2 / (n - 1)} of Statistics Level 2: the random part, which no subtraction removes.

Range sensors also produce stray echoes: someone walks past, sound bounces off the floor. One wild reading drags the mean and inflates the spread, so first find a centre that strays cannot move, the median, drop the readings far from it, and only then compute the mean and the spread:

import math
readings = [1.23, 1.19, 1.25, 1.22, 0.31, 1.21, 1.24, 1.20, 1.26, 1.22]
true_d, tol = 1.2, 0.15

s = sorted(readings)
n = len(s)
median = s[n // 2] if n % 2 else (s[n // 2 - 1] + s[n // 2]) / 2
print("mean of all", round(sum(readings) / n, 4), "  median", round(median, 4))

strays = [i for i, r in enumerate(readings) if abs(r - median) > tol]
kept = [r for r in readings if abs(r - median) <= tol]
m = sum(kept) / len(kept)
sd = math.sqrt(sum((r - m) ** 2 for r in kept) / (len(kept) - 1))
print("strays at", strays, "  bias", round(m - true_d, 4), "m  spread", round(sd, 4), "m")

mean of all 1.133   median 1.22
strays at [4]   bias 0.0244 m  spread 0.023 m

The echo at 0.31 m pulled the mean of all readings down by about 9 cm; the median did not move. After dropping it, the sensor reads 2.4 cm long with a spread of 2.3 cm. A robot can subtract the 2.4 cm, but every reading will still wobble by a couple of centimetres. On a robot that streams readings forever, the mean and variance are kept up to date one reading at a time instead of from a stored list.

Common mistakes: the mean as the centre for spotting strays. Dividing by n where the sample standard deviation is asked for. Sorting the readings in place and then reporting positions in the sorted list. Confusing bias (fixable) with spread (not).

Practice: The Range Finder Meets the Tape Measure, Mean and Spread While the Data Arrives

A calibration line

Many sensors are off by more than a constant. A cheap infrared ranger may read 6 cm long at half a metre and 17 cm long at three metres: a bias that grows with distance. Measure it at several known distances and fit a line, \text{reading} = a + b \cdot \text{true}, with the least-squares line of Statistics Level 3: slope = covariance / variance, and the line passes through the two means. Inverting the line corrects every later reading: \text{true} = (\text{reading} - a) / b.

true = [0.5, 1.0, 1.5, 2.0, 2.5, 3.0]
read = [0.56, 1.08, 1.61, 2.12, 2.66, 3.17]

def fit(xs, ys):
    mx, my = sum(xs) / len(xs), sum(ys) / len(ys)
    b = sum((x - mx) * (y - my) for x, y in zip(xs, ys)) / sum((x - mx) ** 2 for x in xs)
    return my - b * mx, b

a, b = fit(true, read)
print("reading =", round(a, 4), "+", round(b, 4), "* true")
print("a reading of 1.90 means", round((1.90 - a) / b, 4), "m")

bad = read[:5] + [0.4]                         # one stray echo during calibration
a2, b2 = fit(true, bad)
print("with one stray echo: reading =", round(a2, 4), "+", round(b2, 4), "* true")

reading = 0.0367 + 1.0457 * true
a reading of 1.90 means 1.7819 m
with one stray echo: reading = 0.96 + 0.2543 * true

The sensor reads about 4.6 % long plus 3.7 cm. One stray echo at the far end, where a lever is longest, wrecks the whole line, slope included. Least squares trusts every point equally, so clean the data first (the median trick above) or use a robust fit that ranks instead of averaging.

Common mistakes: fitting true distance against reading and then inverting again. Correcting with (\text{reading} - a) \cdot b. Fitting through a stray echo.

Practice: Minutes per Kilometre, The Glitchy Sensor

Errors grow with range

A lidar point is a range r and a bearing \beta, turned into the world by x = x_0 + r\cos\alpha, y = y_0 + r\sin\alpha with \alpha = \varphi + \beta. Each reading has a small random error, \delta r and \delta\beta. To first order (for small errors, the slopes of x and y times the errors) the point moves by

The range error moves the point along the beam; the bearing error moves it across, by r\,\delta\beta: an angle times a radius is an arc, as in Level 1. That factor r is the whole story of far points: 0.005 rad is 5 mm at 1 m and 25 cm at 50 m.

When independent errors with mean 0 are mixed as a\,\delta_1 + b\,\delta_2, their variances add, each scaled by its coefficient squared: a^2\sigma_1^2 + b^2\sigma_2^2 (the cross term averages out). The covariance of two such mixes, a\,\delta_1 + b\,\delta_2 and c\,\delta_1 + d\,\delta_2, is ac\,\sigma_1^2 + bd\,\sigma_2^2: it says whether the errors in x and y tend to have the same sign. A simulation with a seeded generator agrees:

import math, random
phi, r, beta = 0.7, 8.0, 0.4               # robot heading; range and bearing of one point
sr, sb = 0.03, 0.005                        # their standard deviations (m, rad)
al = phi + beta
c, s = math.cos(al), math.sin(al)
var_x = c * c * sr ** 2 + (r * s) ** 2 * sb ** 2
var_y = s * s * sr ** 2 + (r * c) ** 2 * sb ** 2
cov = c * s * sr ** 2 - r * s * r * c * sb ** 2
print("first order:", round(math.sqrt(var_x), 4), round(math.sqrt(var_y), 4), round(cov, 6))

rng = random.Random(7)
dxs, dys = [], []
for _ in range(20000):
    rr, bb = r + rng.gauss(0, sr), beta + rng.gauss(0, sb)
    dxs.append(rr * math.cos(phi + bb) - r * c)
    dys.append(rr * math.sin(phi + bb) - r * s)
sx = math.sqrt(sum(v * v for v in dxs) / len(dxs))
sy = math.sqrt(sum(v * v for v in dys) / len(dys))
print("simulated:  ", round(sx, 4), round(sy, 4), round(sum(a * b for a, b in zip(dxs, dys)) / len(dxs), 6))
for dist in (1, 10, 50):
    print("at", dist, "m: along the beam", sr, "m, across", round(dist * sb, 3), "m")

first order: 0.0382 0.0323 -0.000283
simulated:   0.0381 0.0322 -0.000275
at 1 m: along the beam 0.03 m, across 0.005 m
at 10 m: along the beam 0.03 m, across 0.05 m
at 50 m: along the beam 0.03 m, across 0.25 m

The two agree to a tenth of a millimetre. Here the bearing error dominates already at 8 m (4 cm across against 3 cm along), and the negative covariance says the cloud of possible points is tilted: a point that lands too far right tends to land too low. This is why a robot trusts near points more than far ones, and why Level 4 weighs evidence by its spread.

Common mistakes: adding standard deviations instead of variances. Dropping the factor r on the bearing term. Using the bearing alone instead of heading plus bearing. A bearing error in degrees.

Practice: How Blurry Is a Far Lidar Point?

Which pose explains the scan?

Everything above comes together in one question that every mobile robot asks: given a map and a scan, where am I? Take a few candidate poses. For each, predict the scan it would see (ray casting against the map, capped at the maximum range), and compare with the scan actually measured. This is a measurement model: what the sensor should read if the robot were there.

How to compare matters. Adding up the errors lets one beam blocked by a person ruin a perfect match. Counting agreements (beams whose predicted and measured ranges are within a tolerance) costs that beam a single point:

import math

def cast(x, y, direction, walls, max_range):
    ux, uy = math.cos(direction), math.sin(direction)
    best = max_range
    for (ax, ay), (bx, by) in walls:
        ex, ey = bx - ax, by - ay
        den = ux * ey - uy * ex
        if den == 0:
            continue
        wx, wy = ax - x, ay - y
        t, s = (wx * ey - wy * ex) / den, (wx * uy - wy * ux) / den
        if t >= 0 and 0 <= s <= 1 and t < best:
            best = t
    return best

room = [((0, 0), (8, 0)), ((8, 0), (8, 5)), ((8, 5), (0, 5)), ((0, 5), (0, 0)), ((2, 3), (4, 3))]
angles = [k * math.pi / 4 for k in range(8)]
measured = [2.62, 3.65, 3.03, 4.22, 0.9, 2.85, 1.97, 2.81]     # beam 4: a person 0.9 m behind
candidates = [(5.4, 2.0, 0.0), (5.7, 2.3, 1.35), (5.4, 2.0, 0.1)]
scores = []
for x, y, th in candidates:
    pred = [cast(x, y, th + a, room, 6.0) for a in angles]
    score = sum(abs(p - m) <= 0.1 for p, m in zip(pred, measured))
    error = sum(abs(p - m) for p, m in zip(pred, measured))
    scores.append(score)
    print((x, y, th), " agreements", score, " total error", round(error, 2))

like = [0.9 ** k * 0.1 ** (len(angles) - k) for k in scores]     # P(this scan | candidate)
print("posterior", [round(v / sum(like), 4) for v in like])

(5.4, 2.0, 0.0)  agreements 7  total error 4.67
(5.7, 2.3, 1.35)  agreements 2  total error 2.91
(5.4, 2.0, 0.1)  agreements 3  total error 5.95
posterior [0.9998, 0.0, 0.0002]

Candidate 0 is the true pose: its predicted scan agrees on every beam but the blocked one. Candidate 1, a different spot facing another way, has the smaller total error, because the blocked beam costs the true pose 4.5 m of error all by itself; counted, it agrees on only two beams. Candidate 2 is the true spot turned by 0.1 rad: close, but the far beams swing off their walls.

Counts can become probabilities. Say each beam agrees with probability 0.9 when the candidate is the true pose, and disagrees with probability 0.1 (a person, a reflection). Beams are independent, so the chance of the whole pattern of agreements is a product, 0.9^k\,0.1^{n-k} for k agreements out of n. With every candidate equally likely beforehand, Bayes' rule (Statistics Level 3) says the probability of each candidate after the scan is that product divided by the sum over all candidates: the last line. One scan makes the true pose almost certain here. Level 4 of this path does the same over every cell of a map, with the robot moving between scans.

Common mistakes: forgetting the candidate's heading in each beam's direction. Summing errors instead of counting agreements. Breaking ties differently from what is asked. Predicting a hit beyond the maximum range.

Practice: Which Pose Explains the Scan?, Listening for Leaks

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.

• How Far Must the Elbow Bend?
• Elbow Up or Elbow Down
• Hold the Brush Level: Wrist First
• Stepping a Beam Across the Floor Plan
• Where the Lidar Beam Stops in the Workshop
• Three Radio Beacons in the Mine

Checklist

You can now:

• Find a two-link arm's elbow bend from the law of cosines, after testing reachability with a margin and before clamping the cosine.
• Find the shoulder angle with two atan2 calls, list elbow-up and elbow-down in order, and give the single solution at full stretch or folded.
• Check any inverse-kinematics answer by running forward kinematics and measuring the miss.
• Filter solutions by joint stops and pick the cheapest move with wrapped joint differences and a stated tie-break.
• Reach a world point from a robot's arm (shift, rotate by -\theta, subtract the mount), and place and aim a tool by solving for the wrist first.
• Sample a straight line for the hand, measure how far joint-space motion strays from it, and explain flips and singular poses.
• Predict a range reading: in a bare room, by marching through an occupancy grid, against wall segments with cross products, and at circles with a quadratic; and decide what lies inside a sonar's cone.
• Locate a robot from two bearings (triangulation) or three ranges (trilateration), and say when no single answer exists.
• Describe a sensor by its bias and spread, drop stray echoes with the median, fit a calibration line, and propagate range and bearing errors into x and y.
• Score candidate poses against a scan with a measurement model that counts agreements.