13 — A complete Kalman update, with numbers¶
Before: noise models. After: you can explain each intermediate value and distinguish a prediction from a corrected estimate.
For linear dynamics and measurements with known, independent, zero-mean Gaussian process/measurement noise, the filter computes the exact Gaussian posterior. More generally, the linear estimator has a minimum mean-square property under its second-moment assumptions; that does not make a wrong sensor model optimal for a real robot.
Prediction: x⁻ = F x + B u P⁻ = F P Fᵀ + Q
Innovation: r = z − H x⁻ S = H P⁻ Hᵀ + R
Gain: K = P⁻ Hᵀ S⁻¹
State update: x⁺ = x⁻ + K r
Covariance: A = I − K H P⁺ = A P⁻ Aᵀ + K R Kᵀ
The last line is the Joseph form. Compute the gain using a linear solve, not a materialized inverse. Superscripts − and + mean before and after the current measurement, not negative and positive values.
Worked scalar case: prior position 4 m with variance 1 m²; a same-time sensor reports 6 m with variance 3 m². With H=1, the innovation is 2 m, S=4 m², K=1/4, corrected position is 4.5 m, and variance is 0.75 m². The normalized innovation squared is 2²/4=1. The estimate moves a quarter of the way toward the noisier sensor.
Now add time: if velocity is 2 m/s and the observation was captured 0.5 seconds earlier, first compare it with the state at that capture time. Otherwise a 1 m motion displacement masquerades as sensor error. The replay lab implements this case with a full event history.
A complete loop also validates units, timestamps, finite values, frame agreement, and measurement dimensions; handles dropouts; and records accepted/rejected innovations. Missing data means predict without correction. It does not mean insert a zero-valued measurement.
Try: replace R=3 with R=1. Answer: K=0.5, corrected position 5, corrected variance 0.5. Then remove the measurement entirely: the predicted mean and covariance remain unchanged by a correction step.
Pass: reproduce all intermediate numbers by hand, then identify the corresponding lines in predict, update, and estimate in the lab. Explain why velocity/position cross-covariance lets a position measurement adjust velocity.
Navigation: Learning path · Executable CPU lab.