Prev
Unit 12 · Randomness and Estimation
Nonlinear Estimation: Extended and Unscented Kalman Filters
70m100 XP
🎲Randomness and Estimation

Nonlinear Estimation: Extended and Unscented Kalman Filters

The extended Kalman filter, the unscented Kalman filter, consistency, estimating what you cannot measure, the Riccati equation

Jezero Crater, 18 February 2021
NASA's Perseverance rover is descending to Mars under its parachute. Its target, Jezero Crater, was chosen because it once held a lake and may preserve signs of ancient life — and it is also dangerous: cliffs, fields of dunes, boulders and steep slopes cover much of the area the lander could reach. Earlier missions had to be sent to the flattest, safest plains available, because they could not know precisely where on Mars they were coming down. Perseverance carries a new system, Terrain-Relative Navigation. A camera photographs the ground during descent; the lander's computer matches features in the images to a map of the site stored on board, and its navigation filter fuses those landmark matches with its inertial measurements to estimate where it is. Knowing that, it picks the safest reachable spot from a hazard map and steers towards it. Every measurement in that loop is a nonlinear function of the state: a camera projects three-dimensional terrain onto a flat image through the lander's attitude. Perseverance touched down about 5 m from the spot it had chosen, on safe ground in a landing area that was otherwise full of hazards.
The Kalman filter of lesson 12-3 is optimal for linear models with Gaussian noise. Real ones are rarely linear: a radar measures range x2+y2\sqrt{x^2 + y^2} and bearing arctan⁡(x/y)\arctan(x/y); a GNSS receiver measures pseudoranges ∥rsat−r∥+c δt\|\mathbf r_{\text{sat}} - \mathbf r\| + c\,\delta t; a camera measures a perspective projection; attitude lives on rotations, not in a vector space. So we need xk+1=f(xk)+wk\mathbf x_{k+1} = \mathbf f(\mathbf x_k) + \mathbf w_k and zk=h(xk)+vk\mathbf z_k = \mathbf h(\mathbf x_k) + \mathbf v_k — and the question of lesson 12-2 returns: how do you push a mean and covariance through a nonlinear function?
💡The extended Kalman filter
Schmidt's answer at NASA Ames was to linearise about the current estimate at every step: Predict: x^−=f(x^)\hat{\mathbf x}^- = \mathbf f(\hat{\mathbf x}), P−=FPFT+Q\quad P^- = F P F^{\mathsf T} + Q, with F=∂f/∂xF = \partial\mathbf f/\partial\mathbf x at x^\hat{\mathbf x}. Update: H=∂h/∂xH = \partial\mathbf h/\partial\mathbf x at x^−\hat{\mathbf x}^-, then the gain and covariance update of lesson 12-3, with innovation z−h(x^−)\mathbf z - \mathbf h(\hat{\mathbf x}^-). The state is propagated through the exact nonlinear model; only the covariance uses the Jacobians — lesson 12-2's JPJTJPJ^{\mathsf T}. For a range measurement, HH has the unit line-of-sight vector in it; for GNSS, the rows of HH are the same line-of-sight vectors that formed the geometry matrix of lesson 5-4. Jacobians come from differentiation by hand, automatic differentiation (lesson 11-2) or, with care, finite differences (lesson 8-1).
Derivation
The radar's measurement Jacobian
The radar measures range and bearing of a vehicle at east–north position (e,n)(e, n):
ρ=e2+n2,b=atan2⁡(e,n).\rho = \sqrt{e^2 + n^2}, \qquad b = \operatorname{atan2}(e, n).
Differentiate. For the range, ∂ρ/∂e=e/ρ\partial\rho/\partial e = e/\rho and ∂ρ/∂n=n/ρ\partial\rho/\partial n = n/\rho: the unit line-of-sight vector, as for GNSS. For the bearing, using dduarctan⁡u=11+u2\frac{d}{du}\arctan u = \frac{1}{1 + u^2} with u=e/nu = e/n:
∂b∂e=nρ2,∂b∂n=−eρ2.\frac{\partial b}{\partial e} = \frac{n}{\rho^2}, \qquad \frac{\partial b}{\partial n} = -\frac{e}{\rho^2}.
So H=[e/ρn/ρn/ρ2−e/ρ2]H = \begin{bmatrix} e/\rho & n/\rho \\ n/\rho^2 & -e/\rho^2 \end{bmatrix}, evaluated at the predicted position. Notice the 1/ρ21/\rho^2 in the bearing row: close to the radar a small position error turns into a large bearing change, and a Jacobian evaluated at a wrong position is badly wrong there — the setting of the example below.
📐When linearisation misleads
A vehicle drives past a radar at 10 m/s, passing 60 m from it; range noise 5 m, bearing noise 1°. Over 100 simulated runs, measuring errors after the first 10 s:
  • good first guess (σ₀ = 30 m): EKF and UKF are identical — rms error 3.02 m, mean normalised estimation error squared (NEES) 2.09 against the ideal 2;
  • poor first guess (σ₀ = 200 m): the EKF's rms error is 7.31 m with NEES 8.74 — its squared errors are more than four times what its own covariance predicts — while the UKF gives 3.25 m and 2.34.
Close to the radar, with a wrong estimate, the Jacobian points in the wrong direction; the EKF applies a confident correction along it and believes the result. That is inconsistency, and its worst form, divergence, is the EKF's characteristic failure.
💡The unscented Kalman filter and beyond
The unscented Kalman filter (Julier and Uhlmann, 1997) replaces both linearisations by the unscented transform of lesson 12-2: 2n+12n + 1 sigma points drawn from (x^,P)(\hat{\mathbf x}, P) are pushed through f\mathbf f, and then through h\mathbf h; the predicted measurement, its covariance SS and the cross-covariance CxzC_{xz} come from weighted sums, and the gain is K=CxzS−1K = C_{xz}S^{-1}. No Jacobians, the same O(n3)O(n^3) cost, and better behaviour when the nonlinearity matters over the spread of the uncertainty. When the uncertainty is not even roughly Gaussian — a robot that could be in one of two corridors, a lander matching terrain that repeats — particle filters represent it with thousands of weighted samples: Monte Carlo (lesson 12-1) run as a filter.
Derivation
The same gain in another form, and the consistency test
K=CxzS−1K = C_{xz}S^{-1} is lesson 12-3's gain. For a linear measurement, the predicted measurement error is He−+vH\mathbf e^- + \mathbf v. Its covariance is S=HP−HT+RS = HP^-H^{\mathsf T} + R, and its cross-covariance with the state error is
Cxz=E[e−(He−+v)T]=P−HT,C_{xz} = E\big[\mathbf e^-(H\mathbf e^- + \mathbf v)^{\mathsf T}\big] = P^-H^{\mathsf T},
since v\mathbf v is independent of e−\mathbf e^-. So P−HTS−1=CxzS−1P^-H^{\mathsf T}S^{-1} = C_{xz}S^{-1}: the Kalman gain is 'how the state error co-varies with the measurement error, divided by how uncertain the measurement prediction is'. The UKF estimates CxzC_{xz} and SS from sigma points instead of from HH — no Jacobian needed.
Why NEES should average nn. If the filter is honest, its error e\mathbf e has covariance PP. Then, using aTb=tr⁡(baT)\mathbf a^{\mathsf T}\mathbf b = \operatorname{tr}(\mathbf b\mathbf a^{\mathsf T}),
E[eTP−1e]=E[tr⁡(P−1eeT)]=tr⁡(P−1E[eeT])=tr⁡(I)=n.E[\mathbf e^{\mathsf T}P^{-1}\mathbf e] = E\big[\operatorname{tr}(P^{-1}\mathbf e\mathbf e^{\mathsf T})\big] = \operatorname{tr}\big(P^{-1}E[\mathbf e\mathbf e^{\mathsf T}]\big) = \operatorname{tr}(I) = n.
For the 2-D position error that is 2. A mean NEES of 8.7 says the actual squared errors are more than four times what the filter's own PP claims: overconfidence.
💡Estimating what you cannot measure
A spacecraft's gyro measures angular rate plus a slowly varying bias; a star tracker measures attitude once a second. Put the bias in the state: θk+1=θk+(ωgyro−b) Δt\theta_{k+1} = \theta_k + (\omega_{\text{gyro}} - b)\,\Delta t, bk+1=bkb_{k+1} = b_k. The bias is never measured, but it is observable: a wrong bias makes the predicted attitude drift away from the star tracker, and the filter's cross-covariance between θ\theta and bb turns that drift into a bias correction. For a gyro with a 36°/h bias the estimate is 37.03 ± 1.54°/h after one minute. Over half an hour the gyro alone would drift 18.2°; the filter holds the attitude to 15.2 arcsec rms — better than any single 20-arcsec star-tracker fix. In three dimensions the same idea, with a small attitude error on top of a reference quaternion, is the multiplicative extended Kalman filter (Lefferts, Markley and Shuster, 1982), still the standard for spacecraft attitude determination.
The Riccati equation. For a linear time-invariant model the covariance recursion of lesson 12-3 does not depend on the measurements at all. It converges to a fixed point P∞P_\infty of
P=F(P−PHT(HPHT+R)−1HP)FT+Q,P = F\big(P - PH^{\mathsf T}(HPH^{\mathsf T} + R)^{-1}HP\big)F^{\mathsf T} + Q,
the discrete algebraic Riccati equation, solved directly by scipy.linalg.solve_discrete_are. For the tracking filter of lesson 12-3 the recursion settles after 78 steps and the Riccati equation gives the same gain, K=[0.3203, 0.0615]K = [0.3203,\ 0.0615], with predicted σ 2.059 m and 1.698 m after each update. A filter with that fixed gain needs no covariance arithmetic at run time — the classic α–β tracker of radar systems. Kálmán showed in 1960 that this equation is the dual of the one that gives the optimal linear–quadratic regulator: estimation and control are mirror images, and the courses on guidance and attitude control will meet the other half.
Derivation
Where the Riccati equation comes from
Follow the predicted covariance from one step to the next. The update gives P=(I−KH)P−P = (I - KH)P^- with K=P−HTS−1K = P^-H^{\mathsf T}S^{-1}, so
P=P−−P−HT(HP−HT+R)−1HP−.P = P^- - P^-H^{\mathsf T}\big(HP^-H^{\mathsf T} + R\big)^{-1}HP^-.
The next prediction is Pnext−=FPFT+QP^-_{\text{next}} = FPF^{\mathsf T} + Q. Substitute:
Pnext−=F(P−−P−HT(HP−HT+R)−1HP−)FT+Q.P^-_{\text{next}} = F\Big(P^- - P^-H^{\mathsf T}\big(HP^-H^{\mathsf T} + R\big)^{-1}HP^-\Big)F^{\mathsf T} + Q.
The measurements z\mathbf z appear nowhere: for a fixed model the covariances, and therefore the gains, can be computed before the filter ever runs. If this map settles to a fixed point, Pnext−=P−P^-_{\text{next}} = P^-, that fixed point solves the equation above — the discrete algebraic Riccati equation — and the gain built from it is constant.
The gyro-bias model. With the state (angle, bias), θk+1=θk+(ωgyro−bk)Δt\theta_{k+1} = \theta_k + (\omega_{\text{gyro}} - b_k)\Delta t and bk+1=bkb_{k+1} = b_k, the transition matrix is F=[1−Δt01]F = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix}. Its off-diagonal −Δt-\Delta t is how the filter's covariance learns that a wrong bias shows up as a drifting angle.
All exact science is dominated by the idea of approximation. When a man tells you that he knows the exact truth about anything, you are safe in inferring that he is an inexact man.
– Bertrand Russell, The Scientific Outlook (1931)
🛰Where these filters fly
Apollo: Schmidt's extended filter in the guidance computer. GPS receivers: an EKF on position, velocity and clock from pseudoranges. Aircraft and launchers: inertial navigation aided by GNSS through error-state filters. Spacecraft: multiplicative EKFs on quaternions for attitude from star trackers and gyros. Mars landers: terrain-relative navigation fusing images with inertial data. Your phone: a small filter fusing accelerometer, gyro and magnetometer to know which way it faces.
💡Where you will use this
GNSS Navigation: position–velocity–time filters, inertial integration, precise orbit determination. Spacecraft Attitude Control: attitude and gyro-bias estimation, and the Riccati equation again in LQR design. Orbital Mechanics: sequential orbit determination from tracking data. Optimal Guidance: the estimator in every guidance loop. Structures and Geotechnics: tracking the state of a structure or slope from sensors over time.
📚Further reading
  • Dan Simon, Optimal State Estimation (Wiley, 2006), chapters 13–15 — the extended and unscented Kalman filters and particle filters, side by side.
  • E. J. Lefferts, F. L. Markley and M. D. Shuster, 'Kalman filtering for spacecraft attitude estimation', Journal of Guidance, Control, and Dynamics 5(5), 417–429 (1982) — the multiplicative EKF still flown today.
  • F. Landis Markley and John Crassidis, Fundamentals of Spacecraft Attitude Determination and Control (Springer, 2014) — attitude and gyro-bias estimation in full.
  • Sebastian Thrun, Wolfram Burgard and Dieter Fox, Probabilistic Robotics (MIT Press, 2005) — particle filters and non-Gaussian estimation.
↓The end of this course
This is the end of the Numerical Analysis course — and, by design, the beginning of the others. You started with a single number, 0.1, that a computer cannot store exactly, and a Patriot battery that drifted a third of a second. From there: roots of equations nobody can rearrange; linear systems with millions of unknowns; least squares from Ceres to GNSS; eigenvalues that bring down bridges; interpolation, derivatives and integrals of data; time-stepping that respects or violates the physics, and the exact exponential of a linear system; fields in beams, soil and air; the best design under limits; and finally, honest computation with uncertainty. Every one of those methods reappears in the courses ahead — in the orbit propagators of Orbital Mechanics, the guidance laws of Optimal Guidance, the filters of GNSS Navigation and Attitude Control, the solvers of Structures and Geotechnics, the chemistry of Rocket Propulsion. When you meet them there, you will know what is inside, how it can fail, and how to check it. That is what this course was for.