Kalman Filters

Tracking a hidden number by blending what motion predicts with what the sensor says, trusting each exactly as much as its uncertainty earns.

A prior estimate of 2.0 with wide uncertainty and a sensor reading of 2.5 blend through a gain of 0.69 into a fused estimate of 2.34

Why Does This Exist?

A robot never sees its position. It sees wheel odometry that drifts and GPS fixes that jitter, and it must turn both into one best guess that updates as it moves. Fixed weights fail the moment conditions change: in a tunnel GPS deserves no vote, on open road it deserves a large one. The blend must breathe with the uncertainties.

The Kalman filter is that blend for the linear Gaussian world, and it is optimal there with the smallest mean squared error. Motion and sensing are linear with additive Gaussian noise, the belief stays Gaussian forever, and each step is two closed-form moves: predict forward through motion, then correct with the observation. When the world turns nonlinear or multimodal, this page stays the right prerequisite, because every successor such as particle filters is best understood as what breaks one Kalman assumption. The time structure underneath is a Markov chain.

The one idea you need first

A Gaussian stays Gaussian through linear maps and conditioning. If the state starts Gaussian and every transition and observation adds Gaussian noise linearly, the posterior after any number of steps is still Gaussian, fully described by a mean and a covariance. The filter tracks exactly those two.

Think of It Like This

A navigator blending dead reckoning with landmarks

A sailor dead-reckons from the last fix by speed and heading, but errors accumulate with every mile. Then a lighthouse bearing arrives with its own error.

The sailor does not pick one. On a foggy night with a sharp log, reckoning gets most of the vote. After a long blind run with a crisp noon sight, the sight dominates. The blend ratio follows the two uncertainties, and the fused fix is more confident than either source alone.

Where the analogy stops: the filter also grows confidence during blind motion and shrinks it at each sighting by exact arithmetic rather than judgement.

How It Actually Works

State $x_t$ evolves and observation $y_t$ arrives by:

x_t = F x_{t-1} + w_t, \qquad y_t = H x_t + v_t

Here $F$ is the motion matrix, $H$ maps state to sensor, $w_t$ has covariance $Q$ for motion noise, and $v_t$ has covariance $R$ for sensor noise. The belief is a mean $\hat{x}$ with covariance $P$ .

Predict by pushing through motion: $\hat{x}_{t \mid t-1} = F \hat{x}_{t-1}$ and $P_{t \mid t-1} = F P_{t-1} F^\top + Q$ . Uncertainty always grows here because motion adds $Q$ .

Update by weighting with the Kalman gain $K = P_{t \mid t-1} H^\top (H P_{t \mid t-1} H^\top + R)^{-1}$ , then $\hat{x}_t = \hat{x}_{t \mid t-1} + K (y_t - H \hat{x}_{t \mid t-1})$ and $P_t = (I - K H) P_{t \mid t-1}$ . In plain words, $K$ is prediction uncertainty over total uncertainty, so a precise sensor with small $R$ pulls hard toward the reading while a vague one barely nudges.

Worked example

Scalar tracking with static state, so $F = 1$ and $H = 1$ . Prior $\hat{x} = 2.0$ with $P = 1.0$ , motion noise $Q = 0.10$ , reading $y = 2.5$ with $R = 0.50$ .

Predict leaves the mean at $2.0$ and grows variance to $P = 1.0 + 0.10 = 1.10$ .

Gain divides prediction uncertainty by total uncertainty: $K = 1.10 / (1.10 + 0.50) = 1.10 / 1.60 = 0.6875$ . The sensor earns about a 69 percent vote because $0.50$ against $1.10$ is the sharper of the two.

Update moves 69 percent of the way from prior to reading: $\hat{x} = 2.0 + 0.6875 \times (2.5 - 2.0) = 2.0 + 0.34375 = 2.34375$ . Covariance falls to $P = (1 - 0.6875) \times 1.10 = 0.34375$ , below both $1.10$ and $0.50$ . Fusing beats either source alone, which is the point the diagram shows.

Watch Out For

Running a linear filter on a nonlinear world

A radar measuring range and bearing, or a pendulum near horizontal, breaks the flat-world assumption while $F$ and $H$ pretend it holds. The covariance then shrinks as if the linear model were true, so the filter grows confident around a wrong estimate and rejects the readings that could save it.

Symptom: normalized innovations drift far above 1 while reported $P$ stays small. Fix: use extended or unscented variants for mild nonlinearity, and move to particle filters when posteriors turn multimodal.

Letting the covariance go asymmetric or negative

The update $(I - K H) P$ subtracts nearly equal matrices in finite precision. After thousands of steps $P$ can gain tiny asymmetry or a negative eigenvalue, and the next gain turns garbage: uncertainty that should shrink explodes instead.

Symptom: divergence after long healthy runs, with asymmetry beyond $10^{-12}$ or negative diagonals. Fix: use the Joseph form, symmetrize each step with $(P + P^\top) / 2$ , and check the conditioning before inverting the innovation covariance.

The Quick Version

A Kalman filter tracks a Gaussian belief through linear motion and linear sensing, alternating predict and update, forming the core algorithmic engine for robot state estimation.
The Kalman gain weights the reading by prediction uncertainty over total uncertainty.
In the worked pass, prior 2.0 and reading 2.5 fuse to 2.34 with variance 0.34, better than either source.
Prediction always grows uncertainty, while update always shrinks it.
Nonlinear or non-Gaussian worlds need extended, unscented, or particle variants.
Guard the covariance update numerically, or long runs diverge.