Tracking a moving ball

Estimating what you can't measure

A conveyor now slides the red ball towards the robot at a few cm/s. The sensor (σ = 1 cm, 10 readings per second) only reports position, but to catch the ball you'll need its velocity too. So estimate both, as one state:

Predict, then update

Between readings, the ball keeps moving. A motion model says how the state evolves over $\Delta t$; for constant velocity, $x \leftarrow x + v_x \Delta t$:

Prediction moves the belief and grows it. $Q$, the process noise, says how much you distrust "constant velocity". Here $Q = \operatorname{diag}(0, 0, q\Delta t, q\Delta t)$ lets the velocity wander by about $\sqrt{q\,\Delta t}$ per step.

Then update with the reading. The sensor sees only $Hs$, the position, with $H = [\,I_2 \;\; 0\,]$. That's Step 2's update, generalised:

The magic is in $P$'s off-diagonal terms: prediction couples position and velocity, so every position reading also corrects the velocity.

Tuning Q

A conveyor is very steady, so trust the model: $q \approx 10^{-5}$ works well. A big $q$ makes the filter chase every noisy reading; watch the estimated line in the Plots tab get jumpy.

Your task

1. Fill in cv_model(dt) (F and Q) and kf_predict(x, P, F, Q), which returns (x, P).
2. Implement track(zs, dt): $N \times 2$ readings in, $N \times 4$ states out (one per reading). The grader runs it on a fixed 60-reading track and wants the position error to be under half that of the raw readings.
3. Track the live ball with sensor.next("red") for 3 s, then store its velocity (m/s) in velocity_estimate.