Fuse noisy measurements.

Use motion to connect the clues

Suppose a cart is estimated to be at 1 metre, moving at 0.5 metres per second. After 0.05 seconds, a constant-velocity prediction places it at 1.025 metres. A fresh position reading can correct that prediction. A velocity reading can help correct both velocity and the position prediction it influenced.

The lab implements this idea with two state numbers: position and velocity. Unlike the previous lesson’s independent smoothers, this filter connects them through a motion model. All readings are current and arrive together in this experiment. Handling delayed or out-of-order readings would require additional bookkeeping.

Prediction comes before correction

The filter starts with a declared estimate and uncertainty. It does not initialize from true state. At time zero it uses the first measurements without advancing time. Every later step first advances position using estimated velocity and keeps velocity unchanged in the mean prediction.

The uncertainty calculation additionally allows an unknown acceleration during that interval. A larger assumed acceleration standard deviation allows more uncertainty about the prediction. This is a modelling choice, not a measurement of the actual acceleration.

Innovation means a disagreement

The innovation is the reading minus the prediction for that reading. If predicted position is 1 metre and the sensor reports 1.2 metres, the position innovation is 0.2 metres. It describes disagreement between two available quantities; it is not the true position error.

The Kalman gain determines how much of that disagreement changes the estimate. In a one-number example, prediction variance 4 and sensor variance 1 give gain 4/(4+1) = 0.8. The correction moves 80% of the way toward the reading. If the prediction is more trusted, the correction is smaller.

Variance measures squared spread, so position variance uses square metres. Its square root, the standard deviation, returns to metres. A normalized innovation divides the disagreement by its predicted standard deviation, making the ratio unitless. Large ratios can reveal a mismatch, but they do not tell us whether the sensor, model, timing or bias caused it.

Keep the cross-covariance

The covariance matrix has position variance, velocity variance and a shared cross-covariance. The cross term records how errors in the two estimates are related. An uncertain velocity makes future position uncertain; a position measurement can therefore provide information about velocity too.

This experiment corrects position first, then velocity using the already updated estimate and covariance. Under its independent sensor-error assumption, this is equivalent to a joint two-sensor correction. It would be wrong to reuse the original prediction for both updates and then casually average them, or to ignore correlated measurement errors in a real fusion problem.

The small matrix calculation

Let the state vector be $m=[x,v]^T$. With interval $d$, the constant-velocity transition is $F=\begin{bmatrix}1&d\\0&1\end{bmatrix}$. Prediction uses $m^-=Fm$ and $P^-=FPF^T+Q$. For independent acceleration with variance $q$ held during the interval, $G=[d^2/2,d]^T$ and $Q=qGG^T$.

A position measurement uses row $H=[1,0]$; velocity uses $H=[0,1]$. The innovation is $e=z-Hm^-$, its variance is $S=HP^-H^T+R$, and the gain is $K=P^-H^T/S$. Correct the mean with $m^+=m^-+Ke$.

The implementation updates covariance in Joseph form: $P^+=(I-KH)P^-(I-KH)^T+KRK^T$. This retains the cross terms and avoids relying on a subtraction of nearly equal covariance entries. Matrices are only 2×2 here; the public trace exposes the values so they can be checked independently.

A band is only as credible as its model

The plot shows two standard deviations on either side of estimated position. Do not call this a guaranteed 95% interval. Such probability interpretations need distributional and model assumptions that this teaching trace does not establish.

The sensors here use fixed seeded uniform noise. Their actual variances are amplitude squared divided by three. The trajectory is an authored sinusoid, not a sample from the independent random-acceleration model. A constant camera bias deliberately violates the assumed zero mean. Covariance can remain unchanged while that bias makes the estimate wrong.

The stationary-model fault disconnects velocity from position and leaves process noise only in velocity. Turning up acceleration uncertainty cannot repair a missing relationship. Compare innovations, true errors and band width rather than choosing the smoothest-looking curve.

Make a fusion audit

Capture the baseline, one changed trust assumption and a model or bias failure. Guided learners should explain each stage in everyday words. Implementation learners should reproduce a correction independently, verify covariance symmetry and nonnegative eigenvalues, and explain the units of the gains. Decision learners should identify the noise, bias and motion assumptions that would need validation before deployment.

Truth-based error scores are simulator diagnostics, never estimator inputs. Controls reuse the same underlying readings; these are development comparisons rather than fresh independent trials. Captures are temporary practice. Human review and learner pilots remain pending.