The Kalman filter and the EKF

How do you keep a belief about a moving state?

The Bayes filter of Part 27 represented a belief as a histogram over a grid: it blurred the belief when the robot moved and multiplied it when a sensor spoke. That picture is exact for any motion and any sensor, but it scales terribly, because the number of grid cells grows exponentially with the number of state variables. A robot that tracks position and velocity in two dimensions already has four numbers to describe, and a histogram over four dimensions is a nightmare of bookkeeping.

The Kalman filter buys tractability with one restriction. If the belief is a Gaussian and the motion and measurement models are linear, then the belief stays Gaussian forever, and the entire distribution is summarised by a mean vector and a covariance matrix. Instead of a grid, the filter carries a single point around the state space together with a shape that says how uncertain it is about every direction. Predict with the motion model, correct with the measurement, repeat. That is the whole algorithm.

The state here is a vector: position and velocity in the plane, $x = [x,\; y,\; v_x,\; v_y]$. The position part is what the sensor sees; the velocity part is inferred, never measured directly. The covariance matrix tells the filter, and you, how confident it is about each, and about how they trade off: fast in one direction but unsure of a heading is a long, thin ellipse, not a blurry circle.

Predict, then fuse — the precision sum from Part 17

Write the two models the filter assumes. The state evolves by a linear map $A$ plus Gaussian noise with covariance $Q$: $x_k = A x_{k-1} + w$, $w \sim \mathcal{N}(0, Q)$. The sensor returns a linear projection $H$ of the state plus Gaussian noise with covariance $R$: $z_k = H x_k + v$, $v \sim \mathcal{N}(0, R)$. Both noises are zero-mean, and $Q$ and $R$ are the only places the filter's humility enters.

The predict step pushes the belief through the motion model. The mean is carried by the dynamics, and the covariance is carried by the dynamics and then inflated by the process noise:

Read the second expression as the filter admitting that it does not believe its own model. A perfect prediction would leave the covariance unchanged; $Q$ adds uncertainty because the true motion has accelerations, wobbles and turns that the constant matrix $A$ cannot know. This is the covariance inflation the whole page is about.

The update step folds in the measurement. Form the innovation — the part of the reading the prediction failed to explain — and its covariance, then the Kalman gain, and finally the corrected belief:

The gain decides how far to move toward the measurement, and it is a precision-weighted compromise. In one dimension with $H=1$ it reduces to $K = \sigma^2/(\sigma^2+r)$, the fraction of the total precision carried by the prior — which is precisely the scalar update of Part 17, where the posterior precision was the sum of the prior and observation precisions and the mean was the weight-average. Nothing has changed but the vocabulary: a reciprocal became a matrix inverse, a sum became a matrix sum, and the fusion is still exact. The same weighted-average algebra appears whenever independent Gaussian errors combine, as in the probability guide.

There is no approximation anywhere in these five equations. Given the linear-Gaussian model, they are the exact posterior. That is also their weakness: the moment either model curves, exactness is gone, and the extended Kalman filter in Step 5 is the honest repair.

The signature interaction: a live covariance ellipse

The demo runs the filter on a target whose true motion is a constant-velocity drift interrupted by small accelerations and one sustained turn. The noisy pink dots are position measurements with a fixed, true sensor noise of $0.55$; the dark path is the filter's estimate; the pale dashed line is the truth. The ellipse is the filter's $2\sigma$ covariance projected onto the $x$–$y$ block of the full $4\times4$ matrix — the filter's own picture of where it might be. Step forward one measurement at a time, or press Play to watch the belief converge from a wide initial ellipse to something tight that follows the target.

Watch the ellipse and the point tell different stories. Early on the ellipse is large and round, because almost nothing is known; after a few measurements it shrinks, and once the velocity is inferred it becomes an elongated shape that anticipates where the target is heading. That elongation is the covariance earning its keep: position is known well, velocity a little less so, and the two are correlated, so the uncertainty leans along the direction of travel rather than spreading evenly.

The bottom canvas is the honesty check. The normalised innovation squared, $\mathrm{NIS} = \nu^\top S^{-1}\nu$, is what statisticians call a consistency statistic: if the filter's model and covariances are right, it should behave like a $\chi^2$ variable with as many degrees of freedom as the measurement has numbers — two here — so its mean should hover near $2$, and most of the time it should sit under the $95\%$ threshold. When the NIS lives far above that line, the filter is not merely wrong; it is confidently wrong, and its covariance no longer describes its error.

Trust is a parameter you can get wrong

The sensor in the demo is fixed: its true noise never moves. The sliders change only what the filter believes about the sensor and about its own motion. That gap between belief and reality is where every practical Kalman failure lives, and turning the knobs is the fastest way to feel it.

Turn R far below the truth and the filter believes the sensor is exquisite. It chases each noisy reading and forgets the smooth constant-velocity prediction, so the estimate becomes a jagged copy of the noise. That is the sense in which too small an R makes the filter ignore the motion model: the measurement is trusted so completely that the dynamics stop mattering. Turn Q far above the truth and the opposite happens — every prediction is treated as wildly unreliable, the ellipse balloons at every step, and the estimate jitters because it is constantly re-learning a state it should already know.

The extreme setting is the instructive one. Press Detune: Q is driven to almost nothing while R is left large, so the filter is certain its motion model is perfect and convinced the sensor is poor. Its covariance shrinks to a sliver, the Kalman gain goes to zero, and it stops listening to the measurements entirely — free-running on a straight line while the truth curves away. The estimate sails off, but the ellipse stays small. That is divergence, and it is worse than being lost: the filter is lost and reports high confidence. An error that grows while the ellipse stays tight is the signature, and the readout calls it out.

Bending the linear machinery around a curve

Switch the demo to Range-bearing. The sensor no longer reports $x$ and $y$; it reports a range $r$ and a bearing $\theta$ to a fixed landmark, and those two numbers are a nonlinear function of the state: $r = \sqrt{(x-x_L)^2 + (y-y_L)^2}$ and $\theta = \operatorname{atan2}(y-y_L,\, x-x_L)$. There is no matrix $H$ that produces $r$ and $\theta$ from $[x,y,v_x,v_y]$, so the exact update has no closed form.

The extended Kalman filter handles this by lying locally and honestly. At the current estimate it evaluates the Jacobian $H_k = \partial h/\partial x$, the matrix of first derivatives of the measurement model, using Stats.filter.jacobian — the same object the Jacobian and Hessian guide develops. It then replaces the true curved map with its tangent plane at the estimate and runs the ordinary Kalman update with that tangent in place of $H$. The prediction step can be linearised the same way when the motion model curves, but here the motion stays linear, so only the sensor bends.

Linearisation is a bargain with geometry, and the bill arrives when the belief is wide. The tangent drawn at the estimate is a straight stand-in for a curve that sweeps over the whole arc of the belief; when that arc is broad, the update pushes the estimate in a slightly wrong direction and the error accumulates. Press Wide bearing to set the assumed bearing σ to $0.5$: the estimate lags the truth along the tangential direction the ellipse has stretched into, and the position error climbs into the several-unit range while range itself still tracks. Push the slider the other way, toward a confident bearing, and the filter trusts the tangent so completely that a small linearisation error is amplified into a large position error — the NIS climbs into the hundreds as the filter grows certain in a direction it has mis-modelled. Only while the bearing uncertainty is small enough that the tangent nearly matches the curve is the EKF effectively exact, and locating that threshold is the single most important thing to know before trusting an EKF in a real system. The same question reappears in the optimisation guides as a conditioning problem.

The NIS canvas is just as useful here. An EKF whose linearisation is poor tends to be overconfident in exactly the directions it got wrong, so its NIS climbs even as the estimate looks plausible. Comparing the dark estimate path against the pale truth path, with the error and RMSE printed below, is the direct measurement of the linearisation error the ellipse is hiding.

One recursion, two worlds

The pattern is not particular to robots. Inertial navigation, satellite positioning, radar tracking and camera calibration all maintain a running Gaussian estimate and fold in noisy measurements with these same two steps. What changes is the size of the state and the shape of the models; what stays fixed is the picture — a point, an ellipse, and a rule for shrinking it with each honest measurement while inflation undoes some of that progress with each step of motion.

Further reading

The references below treat the filter as recursive Bayesian inference rather than a bag of matrix formulas. If you take away one thing, take away the picture of an ellipse that grows with motion, shrinks with information, and tells you how far to trust the point inside it.

• Sebastian Thrun, Wolfram Burgard, and Dieter Fox, Probabilistic Robotics, MIT Press, chapter 3 — Gaussian filters, the Kalman and extended Kalman filters, and the origin of the prediction/update split used here.
• Greg Welch and Gary Bishop, "An Introduction to the Kalman Filter", UNC-Chapel Hill TR 95-041 — the standard short derivation, with the discrete update written out step by step.
• Rudolf E. Kálmán, "A New Approach to Linear Filtering and Prediction Problems", Journal of Basic Engineering, 1960 — the original paper, worth a look for how little the core idea has changed.
• Christopher Bishop, Pattern Recognition and Machine Learning, §13.3 — the linear dynamical system as the probabilistic model the filter solves exactly.
• Simo Särkkä, Bayesian Filtering and Smoothing, Cambridge University Press — a modern, careful treatment of the EKF and its consistency, including the failure modes this part dramatises.

Cheat sheet