Kalman Filter Sensor Fusion for Robot Localization

A robot's wheels slip, encoders drift, and sensors return noisy readings. No single source gives the true position. The Kalman Filter is the optimal linear estimator: it combines a motion model and a sensor model, weighted by their respective uncertainties, to produce the best possible estimate of state at every time step.

Predict (state $\hat{x}$, covariance $P$, motion model $A$, control $B$, input $u$):

$\hat{x}^- = A\hat{x} + Bu, \qquad P^- = A P A^\top + Q$

Update (measurement $z$, model $H$, Kalman gain $K$):

$K = P^- H^\top (H P^- H^\top + R)^{-1}$

$\hat{x} = \hat{x}^- + K(z - H\hat{x}^-), \qquad P = (I - KH)\,P^-$

The term $(z - H\hat{x}^-)$ is the innovation — the gap between what the sensor saw and what the model predicted.

1. Start with an initial estimate $\hat{x}_0$ and uncertainty $P_0$.
2. Predict: move the estimate forward using the motion command; $P$ grows (uncertainty increases).
3. Observe: receive a landmark measurement $z$.
4. Compute Kalman gain $K$: high $K$ means trust the sensor more; low $K$ means trust the model more.
5. Update: correct the estimate toward the measurement; $P$ shrinks.
6. Repeat every time step.

The Kalman filter was originally designed for the Apollo program to navigate to the Moon. Today it runs inside every GPS receiver (fusing satellite range measurements), every drone (fusing accelerometer + barometer), and every phone (fusing gyroscope + magnetometer).

1. What does it mean when the Kalman gain $K \approx 0$? And when $K \approx H^{-1}$?
2. The Kalman filter is "optimal" only under two assumptions. What are they?