The Kalman filter and EKF

What is a belief, if it is always a bell?

Part 16 built the Bayes filter without assuming anything about the shape of the belief. You keep a distribution over the state, you push it through the motion model, and you multiply it by the likelihood of the measurement. For a robot on a line that belief might be a row of bins; for a pose in a building it might be a cloud of particles. The update is exact in principle and expensive in practice, because a general distribution has no finite description. You approximate it with a grid, or sample it, and pay for the resolution.

The Kalman filter is what happens when you refuse to approximate. Suppose the belief at every step is Gaussian: a mean x and a covariance P. Suppose the motion model is linear in the state and corrupted by Gaussian noise, and the measurement is likewise a linear function plus Gaussian noise. Then the prediction is Gaussian, the product of two Gaussians is Gaussian, and the belief at every future step is Gaussian too. The whole distribution is carried by a vector and a matrix, and the update is finite, closed-form, and optimal — not approximately optimal, but the exact conditional mean and covariance.

That is a startling amount of structure to get for free, and it is bought with two strong assumptions: linearity and Gaussian noise. The first is the one that breaks. A range-and-bearing sensor does not report a linear function of position; it reports a distance and an angle, and the angle is $\arctan$ of a ratio. A robot that rotates has a motion model full of sines and cosines. The extended Kalman filter keeps the Gaussian belief and the two-step recursion but linearises the models at the current estimate, turning the curve into its tangent line. It works beautifully while the belief is small relative to the curvature, and fails when the belief is broad enough that the tangent is a poor stand-in for the curve.

So the question of this part is concrete. If the belief is a bell, what exactly are the predict and update steps, where do the matrices Q and R come from, how do we choose them, and how badly does the tangent-line trick hurt when the world curves? We answer with a tracker you can tune on the left and an extended filter on the right, and we measure the linearisation error rather than hand-waving about it.

Three sentences that buy you the whole algorithm

Write the state at time k as a vector x_k. For a moving object on a line the natural state is position and velocity, $x=(p,v)^\top$, because a model that predicts position from position alone cannot know that the object was already moving. Given the state and a control input u_k, the motion model says how the state evolves: multiply by a matrix F, add the effect of the control through a matrix B, and add noise. The measurement model says what the sensor reports: multiply the state by an observation matrix H and add noise.

Every symbol earns its place. F is the deterministic dynamics: for the constant-velocity model with time step $\Delta t$ it is $\begin{pmatrix}1 & \Delta t\\ 0 & 1\end{pmatrix}$, which says position advances by velocity times time and velocity is unchanged. B lets an external command push the state, for instance an acceleration that adds $\tfrac12\Delta t^2$ to position and $\Delta t$ to velocity. H is the sensor's view of the state: if the sensor measures position only, $H=\begin{pmatrix}1 & 0\end{pmatrix}$ and the velocity is never seen directly, only inferred from how position changes.

The two noise covariances are the honest part of the model. Q is the process noise: everything the dynamics failed to predict — gusts, friction changes, a driver who did not hold a constant velocity. R is the measurement noise: sensor error, quantisation, the fact that two readings of the same true position differ. Neither is a nuisance parameter to be swept under the rug. They are the numbers that decide how much the filter believes the model against the sensors, and the next two sections show exactly how.

One more object is needed: the belief itself. Before any data, the state is unknown; we summarise that ignorance as a Gaussian prior with mean x_0 and covariance P_0. A large diagonal in P_0 means "I have almost no idea where this thing is". The filter's job is to shrink that matrix with evidence. Because every operation it performs — a linear map, an addition of independent Gaussians, a product of Gaussians — preserves Gaussianity, the belief stays a bell described by a mean and a covariance forever.

Five lines, and the gain that decides everything

The prediction step pushes the belief through the motion model. A linear map sends a mean through F and a covariance through $F(\cdot)F^\top$, and adding independent Gaussian noise adds its covariance. That gives the prior for the new time step, written with a minus superscript.

Prediction always widens the belief: the covariance is pushed through F and then inflated by Q. Uncertainty can never decrease without data. The update step is where information arrives. The sensor predicts a measurement Hx^-; the actual reading z differs by the innovation z-Hx^-; and the Kalman gain K decides how much of that surprise to fold into the state.

Read the gain as a ratio of uncertainties. The matrix $H P^- H^\top$ is the uncertainty the model has about the measurement it expects; R is the uncertainty the sensor adds. Their sum $S=HP^-H^\top+R$ is the total innovation covariance. When R is tiny next to the model's own uncertainty, K is close to a pseudo-inverse of H: the filter snaps the estimate onto the measurement and ignores the prediction. When R is large, the inverse shrinks, K goes to zero, and the filter barely moves from x^-. The update is a weighted average of two noisy opinions, weighted by how much each is trusted — and the weights are inverse variances. That is the entire content of the algorithm.

Notice that the covariance update never looks at the data. P=(I-KH)P^- is a function of P^-, H and R alone. The uncertainty about how well you know the state evolves deterministically, whether the measurements agree with the prediction or not. This is why you can pre-compute the gain schedule for a fixed measurement pattern before a mission, and why the filter is so cheap: no sampling, no integrals, just a handful of matrix products per step.

The state is jointly Gaussian throughout, so the filter is not just a plausible heuristic; it is the exact Bayes posterior under the linear-Gaussian assumptions. The mean it produces minimises the mean-squared error among all estimators, not merely among linear ones. It is also a least-squares solution to a weighted problem: the estimate is the smallest set of state changes that reconciles the motion model with the measurements, each residual weighted by its inverse covariance. That reading is the one that generalises to the extended filter, and to the smoother and the bundle adjuster.

A tracker you can lean on the model or the sensor

Here is a constant-velocity tracker following a target on a line. The true trajectory is drawn faintly, the position measurements are pink dots, and the filter's estimate is the heavy blue line with two shaded bands: $\pm1\sigma$ and $\pm2\sigma$ from the diagonal of P. The measurements are fixed; the sliders change only what the filter believes about the noise, not the data. Start the run and watch the band narrow as evidence accumulates, then pump the process-noise slider up and watch the estimate start chasing the dots.

The two sliders are the two dials that matter. Q is the assumed process noise, added to P^- every step: turn it up and the filter thinks the model is unreliable, so P^- grows fast, the gain rises, and each measurement pulls the estimate hard. The estimate becomes jumpy and tracks the noisy dots. Turn Q down and the filter trusts its dynamics; the gain falls, the line smooths out, and when the true trajectory actually changes direction the estimate lags behind. R is the assumed measurement noise: raise it and the filter treats each reading as untrustworthy, the gain drops, and the estimate coasts on the model. Lower it and the band contracts around the dots, sharp but twitchy.

The readout shows the gain at the current step. At the very first update it is close to one, because P_0 is large and the first measurement genuinely dominates. As the run proceeds the position gain falls toward a steady-state value determined entirely by the ratio of Q to R. There is no "correct" setting in the abstract. The right question is whether the assumed noises match the real ones; if they do, the filter is optimal, and if they do not, the bands lie about the actual error. A useful sanity check is to compare the innovation magnitudes to S: if measurements routinely land outside the $\pm3\sigma$ band, one of the two sliders is wrong.

Two details are worth noticing in the readout. First, the velocity gain K_2 is not zero even though the sensor never measures velocity: the filter infers velocity from the trend of the position readings, which is why the state contains velocity at all. Second, the bands are not confidence intervals for the truth in the frequentist sense; they are the filter's own posterior standard deviations under its assumptions. That is precisely the Bayesian object, and it is what makes this recursive filter the linear-Gaussian special case of Part 16's loop.

When the tangent line is not the curve

Most real sensors are not linear. A range-and-bearing sensor reports $r=\sqrt{(x_l-x_r)^2+(y_l-y_r)^2}$ and $\phi=\arctan\!\big((y_l-y_r)/(x_l-x_r)\big)$, both nonlinear functions of the landmark position and the robot pose. A rotating vehicle's motion model is full of sines and cosines. The extended Kalman filter keeps the Gaussian belief and the same predict-update skeleton, but replaces the true models with their first-order Taylor expansions at the current estimate. Matrices become Jacobians:

Here the robot moves along a known path in the plane and observes a single fixed landmark, so the state to estimate is the landmark's position, $x=(l_x,l_y)^\top$. The motion model of the landmark is the identity — it does not move — while the measurement model is the range and bearing from the current robot pose. The canvas below shows the path, the noisy bearing rays, the true landmark in pink, and the estimate in blue with its $1\sigma$ and $2\sigma$ covariance ellipses from $\mathrm{Prob.confEllipse}$.

The price of linearisation is the error between the true curved measurement prediction and the straight-line stand-in. The smaller the belief, the closer the two agree; the wider the angular extent the belief subtends at the robot, the worse the tangent fits. The second canvas plots that bearing error against the angular spread of the belief as the robot travels. Both rise and fall together, and both spike when the robot passes close to the landmark, where the bearing function bends hardest and a small position error swings the predicted angle the most.

The EKF is not the only remedy, and it is worth knowing what the alternatives buy. The unscented Kalman filter propagates a deterministic set of sigma points through the true nonlinear function and re-fits a Gaussian to the result, capturing curvature to second order without ever forming a Jacobian. Iterated EKFs re-linearise around the updated estimate instead of the predicted one. Full nonlinear least squares, the subject of the SLAM and bundle-adjustment machinery, abandons the recursion and optimises the whole trajectory at once. Each is a different answer to the same question: how much of the Gaussian shortcut can you keep once the world stops being linear?

The default filter of engineered systems

Every formula in one place

Where to go deeper

• R. E. Kalman, "A New Approach to Linear Filtering and Prediction Problems", Journal of Basic Engineering, 1960 — the original paper, shockingly readable.
• Sebastian Thrun, Wolfram Burgard and Dieter Fox, Probabilistic Robotics, chapters 3 and 7 — the Bayesian framing and the EKF, with the range-bearing example this part draws.
• Yaakov Bar-Shalom, X.-Rong Li and Thiagalingam Kirubarajan, Estimation with Applications to Tracking and Navigation — the reference for tuning Q and R and for the consistency checks.
• Simo Särkkä, Bayesian Filtering and Smoothing, chapters 4–5 — a clean derivation of the Kalman filter as the Gaussian special case, and the unscented extension.
• Greg Welch and Gary Bishop, "An Introduction to the Kalman Filter" — the short tutorial most people actually learned it from.