The Kalman filter: optimal state estimation from noisy measurements

You want to know the true state of a system, say the position and velocity of a drone, but you cannot measure it directly or cleanly. You have two imperfect sources. First, a motion model that predicts how the state evolves, which drifts because the world is messier than the model. Second, measurements (GPS, a range sensor) that are noisy and often incomplete.

Neither source alone is good enough: the model drifts without correction, and the raw measurements jitter. The Kalman filter is the recipe for fusing them into an estimate that is better than either. Its core move is to track not just a best guess but an explicit measure of how uncertain that guess is, and to let that uncertainty decide how much to trust each new measurement.

The classic Kalman filter assumes both models are linear with Gaussian noise. The state $x$ (say position and velocity) evolves as

$x_k = F\,x_{k-1} + B\,u_k + w,\qquad w \sim \mathcal{N}(0, Q)$

Here $F$ is the transition matrix (how the state carries forward), $B\,u_k$ is any known control input, and $Q$ is the process noise covariance, how much the model itself is untrustworthy.

Measurements relate to the state through

$z_k = H\,x_k + v,\qquad v \sim \mathcal{N}(0, R)$

$H$ maps state to what the sensor sees (a GPS sees position, not velocity), and $R$ is the measurement noise covariance. Choosing $F$, $H$, $Q$, and $R$ is the modelling work; the rest of the filter is mechanical.

The filter never commits to a single number. It carries a Gaussian belief about the state: a mean $\hat{x}$ (the current best estimate) and a covariance matrix $P$ (how uncertain that estimate is, and how the components' errors correlate).

Think of $P$ as an uncertainty ellipse around $\hat{x}$. A tight ellipse means confident; a wide one means unsure. Everything the Kalman filter does is move this ellipse and resize it:

• Predict slides the ellipse along the motion model and inflates it (we get less sure over time).
• Update pulls the ellipse toward a new measurement and shrinks it (a measurement buys certainty).

Because a Gaussian is fully described by its mean and covariance, tracking just $\hat{x}$ and $P$ is enough to represent the entire probability distribution over the state.

Between measurements, the filter runs the motion model on its belief. The mean moves forward and the covariance grows by the process noise:

$\bar{x}_k = F\,\hat{x}_{k-1} + B\,u_k$ $\bar{P}_k = F\,P_{k-1}\,F^\top + Q$

The bar denotes a prior (before seeing the new measurement). The first line is just the model applied to the last estimate. The second is the important one: transforming the old uncertainty through $F$ (the $F P F^\top$ term), then adding $Q$.

That added $Q$ is why prediction always makes you less certain: the ellipse inflates. Run predict many times with no measurements and $P$ blows up, which correctly says the estimate is now mostly a guess. This is dead reckoning, and it is exactly what the update step exists to rein in.

Predict grows the uncertainty; a measurement and the Kalman gain shrink it. Repeat.

A measurement $z_k$ arrives. The filter first asks: how surprised am I? It compares the actual measurement to what its prediction expected to see, $H\,\bar{x}_k$. The difference is the innovation (or measurement residual):

$y_k = z_k - H\,\bar{x}_k$

If $y_k$ is near zero, the measurement agrees with the prediction and little correction is needed. A large $y_k$ means a surprise worth acting on.

How much a given surprise should move the estimate depends on how uncertain both sides are. That combined uncertainty is the innovation covariance:

$S_k = H\,\bar{P}_k\,H^\top + R$

$S_k$ blends the predicted uncertainty (mapped into measurement space by $H$) with the sensor noise $R$. It is the denominator that keeps the correction properly scaled.

The Kalman gain is the heart of the algorithm. It decides how much of the innovation to actually apply:

$K_k = \bar{P}_k\,H^\top\,S_k^{-1}$

Read it as a ratio of confidences. The numerator carries the prediction's uncertainty $\bar{P}_k$; the denominator $S_k$ carries the total uncertainty including the sensor noise $R$. So:

• Prediction very uncertain ($\bar{P}_k$ large) or sensor very precise ($R$ small) → $K$ large → trust the measurement, move a lot.
• Prediction confident or sensor noisy ($R$ large) → $K$ small → mostly ignore the measurement, trust the model.

The gain is recomputed every step from the current uncertainties, so the filter automatically leans on whichever source is currently more trustworthy. No manual tuning of "how much to believe the sensor" is needed once $Q$ and $R$ are set.

With the gain in hand, the correction is a weighted step toward the measurement, and the uncertainty shrinks:

$\hat{x}_k = \bar{x}_k + K_k\,y_k$ $P_k = (I - K_k H)\,\bar{P}_k$

The mean moves from the prior $\bar{x}_k$ by the gain times the surprise. The covariance contracts: $(I - K_k H)$ is between 0 and 1 in the relevant directions, so a measurement always reduces uncertainty (or leaves it unchanged if the gain is zero).

That is the whole loop. Predict inflates $P$; update deflates it. In steady state the two balance, and $P$ settles to a value where each new measurement buys exactly as much certainty as the next prediction step loses. The estimate then tracks the true state as tightly as the noise allows.

The five equations are just matrix algebra. A single predict-plus-update step:

import numpy as np

def kalman_step(x, P, F, Q, H, R, z, B=None, u=None):
    # --- predict ---
    x = F @ x + (B @ u if u is not None else 0)   # mean forward
    P = F @ P @ F.T + Q                            # uncertainty grows
    # --- update ---
    y = z - H @ x                                  # innovation (residual)
    S = H @ P @ H.T + R                            # innovation covariance
    K = P @ H.T @ np.linalg.inv(S)                 # Kalman gain
    x = x + K @ y                                  # corrected mean
    P = (np.eye(len(x)) - K @ H) @ P               # uncertainty shrinks
    return x, P

Call it once per timestep with the latest measurement z. Everything the filter "knows" lives in x and P; there is no history to store. That recursive, constant-memory structure is a big part of why the Kalman filter runs happily on tiny embedded hardware.

For a system that really is linear with Gaussian noise, the Kalman filter is not just reasonable, it is optimal: among all estimators it minimizes the mean-squared error, and it exactly tracks the true Bayesian posterior over the state. It is also recursive: it needs only the previous estimate, not the full measurement history, so cost and memory stay constant per step.

The assumptions are the fine print, and they bite:

• Linearity. Real dynamics and sensors are often nonlinear (angles, ranges, perspective).
• Gaussian noise. Heavy tails or outliers (a GPS multipath spike) violate this and can yank the estimate.
• Known $Q$ and $R$. These are usually guessed and tuned; wrong values make the filter over- or under-trust its sensors.

When these hold, the filter is beautiful. When they do not, you reach for a variant.

Most real navigation is nonlinear, so two variants dominate:

The EKF replaces $F$ and $H$ with their Jacobians (local linear approximations) at each step, then runs the ordinary equations. The UKF skips linearization entirely: it samples a few carefully chosen points, transforms them through the real function, and reassembles a Gaussian. UKF is usually more accurate on sharp nonlinearities; EKF is lighter and often good enough. Both keep the exact same predict-update skeleton.

Strip away the matrices and one idea remains: keep a Gaussian belief about a hidden state; predict it forward while growing the uncertainty, correct it with each measurement while shrinking the uncertainty, and weight the two by their relative confidence. The Kalman gain is just that weighting made precise.

That framing is why the filter is everywhere: GPS and inertial (IMU) sensor fusion, robot localization and SLAM, target tracking, spacecraft navigation, even smoothing in finance and signal processing. Any time you must combine a model you trust somewhat with measurements you trust somewhat, this is the canonical tool. When you evaluate a variant, ask the two questions the math forces: is my system linear enough for these equations, and do I trust my noise covariances $Q$ and $R$. Those answers, not the branding, decide which filter you want.