Fusing measurements

Two sensors

The robot now has a real sensor, sensor, with two sources: a wide camera (σ = 2 cm) and a narrow one (σ = 5 mm). Both take a new reading 10 times per second. From here on, world.balls() is switched off: the sensor is your only way to see the ball.

m = sensor.next("red", "wide")     # wait for the next reading
m.position, m.t                     # where it saw the ball, and when
m = sensor.read("red", "narrow")   # the latest narrow reading (no waiting)
sensor.sigma("narrow")             # 0.005

Averaging all readings equally would let the sloppy camera drag the answer around. You want to trust each reading in proportion to how precise it is.

Belief = estimate + uncertainty

Keep a belief: an estimate $x$ and a covariance $P$ (for one number, $P = \sigma^2$). A measurement $z$ comes with its own covariance $R$. The Kalman update blends them:

$$K = P\,(P + R)^{-1}, \qquad x' = x + K\,(z - x), \qquad P' = (I - K)\,P$$

$K$, the Kalman gain, is between 0 and 1 (a matrix in general). In one dimension:

$$K = \frac{P}{P + R}, \qquad \frac{1}{P'} = \frac{1}{P} + \frac{1}{R}$$

• A vague belief ($P \gg R$): $K \approx 1$, so jump to the measurement.
• A sloppy measurement ($R \gg P$): $K \approx 0$, so barely move.
• Equal $R$ for every reading, starting vague: the updates add up to Step 1's plain average.

Precision (1/variance) adds up with every reading, so $P$ only ever shrinks. One narrow reading is worth $(2/0.5)^2 = 16$ wide ones.

Your task

1. Implement kf_update(x, P, z, R) and return (x, P). x and z are vectors, P and R are matrices; a 1D problem is a 1-vector with a 1×1 matrix. It must work in any dimension.
2. Start from a vague belief, fuse 20 readings from each source of the red ball, and store the result in a global fused (a 3-vector).
3. Plot $\sqrt{P}$ as the readings come in.