Prev
Unit 12 · Randomness and Estimation
Recursive Estimation and the Kalman Filter
70m100 XP
Next
🎲Randomness and Estimation

Recursive Estimation and the Kalman Filter

Recursive least squares, predict and update, the Kalman gain, a tracking filter and inertial–GNSS fusion

NASA Ames, autumn 1960
Stanley Schmidt leads a group at NASA's Ames Research Center studying how a spacecraft could navigate to the Moon. A navigation computer on board will receive noisy measurements — angles to stars and landmarks, ranges — and must combine them with predictions from the equations of motion to estimate position and velocity, continually, in real time, on a computer with very little memory. That year Rudolf Kálmán, a Hungarian-born engineer at the Research Institute for Advanced Studies in Baltimore, has published A New Approach to Linear Filtering and Prediction Problems. It describes an estimator that is recursive — it keeps only the current estimate and its uncertainty, updating both with each new measurement — and optimal for linear systems with Gaussian noise. Kálmán visits Ames to present it. Schmidt's group recognises immediately that it is what they need, and works out how to apply it to the nonlinear equations of spaceflight by linearising about the current estimate — the extended Kalman filter. The filter flew to the Moon in the Apollo guidance computer. Today it runs in every GNSS receiver, every inertial navigation system, every aircraft autopilot and every phone that knows which way it is facing.
Start with the simplest case: estimating a constant from noisy readings zk=x+vkz_k = x + v_k with noise variance RR. The batch answer is the mean (Unit 5's least squares). Recursively: keep the current estimate x^\hat x and its variance PP, and for each new reading
K=PP+R,x^←x^+K(z−x^),P←(1−K)P.K = \dfrac{P}{P + R}, \qquad \hat x \leftarrow \hat x + K(z - \hat x), \qquad P \leftarrow (1 - K)P.
The gain KK weights the new reading by how uncertain the estimate is relative to the measurement. Starting from a vague prior, the first reading gets K=1K = 1 (believe it), the tenth K=0.1K = 0.1, the two-hundredth K=0.005K = 0.005 — and after 200 readings the recursive estimate equals the batch mean exactly, with standard deviation σ/200\sigma/\sqrt{200}. Nothing but the current estimate and its variance was ever stored.
Derivation
Where K=P/(P+R)K = P/(P + R) comes from
Combining two estimates. We hold an estimate x^\hat x with error variance PP and receive a reading zz with independent error variance RR. Any blend that is right on average has the form x^new=(1−K)x^+Kz\hat x_{\text{new}} = (1 - K)\hat x + Kz. Its error is (1−K)(1 - K) times the old error plus KK times the reading's error; independent variances add (lesson 8-5):
Pnew(K)=(1−K)2P+K2R.P_{\text{new}}(K) = (1 - K)^2P + K^2R.
Choose KK to make it as small as possible:
dPnewdK=−2(1−K)P+2KR=0⟹K=PP+R,Pnew=PRP+R=(1−K)P.\frac{dP_{\text{new}}}{dK} = -2(1 - K)P + 2KR = 0 \quad\Longrightarrow\quad K = \frac{P}{P + R}, \qquad P_{\text{new}} = \frac{PR}{P + R} = (1 - K)P.
The new variance is smaller than both PP and RR: combining two honest estimates always helps.
Recursion equals the batch mean. With a vague prior (P→∞P \to \infty) the first gain is 1 and the estimate becomes the first reading, with P=RP = R. If after kk readings P=R/kP = R/k, the next gain is R/kR/k+R=1k+1\frac{R/k}{R/k + R} = \frac{1}{k + 1} and the new variance R/(k+1)R/(k + 1). So
x^k+1=x^k+zk+1−x^kk+1,\hat x_{k+1} = \hat x_k + \frac{z_{k+1} - \hat x_k}{k + 1},
which is exactly how a running mean updates. After 200 readings: the mean of all 200, with variance R/200R/200 — the batch answer, from one number and one variance.
💡The Kalman filter
For a linear system with state xk\mathbf x_k, model xk+1=Fxk+wk\mathbf x_{k+1} = F\mathbf x_k + \mathbf w_k (process noise covariance QQ) and measurements zk=Hxk+vk\mathbf z_k = H\mathbf x_k + \mathbf v_k (noise covariance RR), alternate two steps: Predict: x^−=Fx^\hat{\mathbf x}^- = F\hat{\mathbf x}, P−=FPFT+Q\quad P^- = FPF^{\mathsf T} + Q — the model moves the estimate forward and uncertainty grows. Update: K=P−HT(HP−HT+R)−1K = P^-H^{\mathsf T}(HP^-H^{\mathsf T} + R)^{-1}, x^=x^−+K(z−Hx^−)\quad \hat{\mathbf x} = \hat{\mathbf x}^- + K(\mathbf z - H\hat{\mathbf x}^-), P=(I−KH)P−\quad P = (I - KH)P^- — the measurement corrects it and uncertainty shrinks. z−Hx^−\mathbf z - H\hat{\mathbf x}^- is the innovation: what the measurement says that the prediction did not. The gain is a matrix version of P/(P+R)P/(P + R): it trusts the measurement where the prediction is uncertain and the model where the measurement is noisy. For linear Gaussian problems the result is the best possible estimate — the minimum-variance one — and PP is its actual covariance.
Derivation
The predict and update equations, derived
Predict. The error of the prediction is e−=Fe+w\mathbf e^- = F\mathbf e + \mathbf w: the old error carried through the model plus new process noise, independent of it. Lesson 5-1's rule for a linear map, plus independence, gives P−=FPFT+QP^- = FPF^{\mathsf T} + Q.
Update. Blend as in the scalar case, x^=x^−+K(z−Hx^−)\hat{\mathbf x} = \hat{\mathbf x}^- + K(\mathbf z - H\hat{\mathbf x}^-). Since z=Hx+v\mathbf z = H\mathbf x + \mathbf v, the new error is
e=(I−KH) e−+Kv⟹P=(I−KH)P−(I−KH)T+KRKT,\mathbf e = (I - KH)\,\mathbf e^- + K\mathbf v \qquad\Longrightarrow\qquad P = (I - KH)P^-(I - KH)^{\mathsf T} + KRK^{\mathsf T},
valid for any gain KK (the 'Joseph form'). Choose KK to minimise the total error variance, the trace of PP. Expanding and differentiating with respect to KK — the matrix version of the scalar calculation above, using ∂∂Ktr⁡(KAKT)=2KA\frac{\partial}{\partial K}\operatorname{tr}(KAK^{\mathsf T}) = 2KA for symmetric AA — gives
−2P−HT+2K(HP−HT+R)=0⟹K=P−HT(HP−HT+R)−1.-2P^-H^{\mathsf T} + 2K\big(HP^-H^{\mathsf T} + R\big) = 0 \qquad\Longrightarrow\qquad K = P^-H^{\mathsf T}\big(HP^-H^{\mathsf T} + R\big)^{-1}.
With S=HP−HT+RS = HP^-H^{\mathsf T} + R, this optimal gain satisfies KSKT=P−HTKTKSK^{\mathsf T} = P^-H^{\mathsf T}K^{\mathsf T}; substituting into the expanded Joseph form cancels two terms and leaves P=(I−KH)P−P = (I - KH)P^-. For one state and one direct measurement (F=H=1F = H = 1, Q=0Q = 0) everything reduces to the scalar formulas above.
📐Tracking a vehicle
A vehicle moves along a road with random accelerations; a position fix arrives every second with 3 m noise. A two-state filter (position and velocity) with a constant-velocity model:
  • raw fixes: rms error 2.79 m;
  • filtered position: rms error 1.58 m — and a velocity estimate (0.46 m/s rms) that no single fix provides;
  • steady-state gain K=[0.32, 0.06]K = [0.32,\ 0.06]: each fix moves the position estimate by 32% of the innovation.
The mean normalised innovation squared is 0.92, close to the value 1 expected when the filter's assumed QQ and RR match reality — the standard check that a filter is consistent, honest about its own uncertainty.
At present, a nonspecialist might well regard the Wiener-Kolmogorov theory of filtering and prediction as 'classical' — in short, a field where the techniques are well established and only minor improvements and generalizations can be expected.
– Rudolf E. Kálmán, opening of A New Approach to Linear Filtering and Prediction Problems (1960) — the paper that proved it wrong
📐Through a tunnel
Inertial–GNSS fusion in one dimension: an accelerometer at 10 Hz drives the prediction; a GNSS fix (2 m noise) corrects it once per second — except for 30 seconds in a tunnel. Before the tunnel the filter's position error is 0.47 m (filter σ 0.73 m). Coasting on the accelerometer alone, the error at the tunnel exit is 1.0 m while the filter, honestly, reports σ = 2.9 m. Ten seconds after the first fix it is back to 1.3 m with σ 0.77 m. Neither sensor alone could do this: GNSS has gaps, and an accelerometer integrated twice drifts without bound. Together they are better than either — which is the whole point of estimation.
🛰From Apollo to your phone
Apollo's guidance computer ran a Kalman filter on a machine with about 4 kilobytes of erasable memory, processing star sightings and ground-tracking updates. Today: GNSS receivers estimate position, velocity and clock with Kalman filters; inertial navigation systems are 'aided' by GNSS through error-state filters exactly like the tunnel example; spacecraft attitude is estimated with multiplicative extended Kalman filters on quaternions; and the unscented Kalman filter of lesson 12-4 replaces linearisation with sigma points.
💡Where you will use this
GNSS Navigation: the receiver's position–velocity–time filter, RTK ambiguity resolution and precise orbit determination. Spacecraft Attitude Control: attitude determination from star trackers and gyros. Orbital Mechanics: sequential orbit determination from tracking data. Optimal Guidance and Control: the estimator half of every linear–quadratic–Gaussian controller. Structures and Geotechnics: structural health monitoring and back-analysis of field measurements.
📚Further reading
  • Rudolf E. Kálmán, 'A new approach to linear filtering and prediction problems', Journal of Basic Engineering 82(1), 35–45 (1960) — the original paper.
  • Dan Simon, Optimal State Estimation: Kalman, H-infinity, and Nonlinear Approaches (Wiley, 2006) — the derivations of this lesson and the next, with many worked examples.
  • Mohinder Grewal and Angus Andrews, 'Applications of Kalman filtering in aerospace 1960 to the present', IEEE Control Systems Magazine 30(3), 69–78 (2010) — Schmidt's group, Apollo and after.
↓But there is a problem
The Kalman filter is the best possible estimator — for linear models with Gaussian noise. Almost nothing a navigation system measures is linear: a range is a square root, a bearing an arctangent, a camera image a perspective projection, an attitude a rotation. Schmidt's group linearised, and that took Apollo to the Moon. Sixty years later a rover hanging under its parachute over Mars had to work out where it was from photographs of the ground — and decide, on its own, where it was safe to land.