Skip to main content
Contents3 / 6 ▾
  1. ~Cover & contents
  2. IThe problem, before any algorithm
  3. IIThe Kalman filter: the exact case
  4. IIIThe EKF: pretend it is a straight line
  5. IVThe UKF: do not differentiate, sample
  6. VFactor graphs: keep everything, solve it all
  7. VIThe three compared, and unified
III

Chapter three The EKF: pretend it is a straight line

Reality curves. Rotations compose multiplicatively, a camera projects by dividing by depth, range to a beacon is a square root of a sum of squares. None of these is linear, so the Kalman filter's guarantee evaporates. The extended Kalman filter's answer is the most obvious one an engineer could give: if the function is curved, replace it with its tangent and carry on as though nothing happened.

3.1 · The first-order Taylor expansion

Expand \(f\) about the current estimate \(\hat{x}_{k-1}\) and throw away everything past the linear term:

\[ f(x) \;\approx\; f(\hat{x}_{k-1}) + F_k\,(x - \hat{x}_{k-1}), \qquad F_k = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k-1},\,u_k} \]
(3.1)

Likewise \(H_k = \partial h / \partial x\) evaluated at the predicted state \(\hat{x}_k^-\). Now substitute those Jacobians into the Kalman equations wherever \(F\) and \(H\) appeared. One subtlety matters enormously and is the source of most EKF bugs:

\[ \underbrace{\hat{x}_k^- = f(\hat{x}_{k-1}, u_k)}_{\text{use the true nonlinear } f}, \qquad \underbrace{P_k^- = F_k P_{k-1} F_k^\top + Q}_{\text{use the Jacobian}} \]
(3.2)

The mean travels through the real function; only the covariance is transported by the linear approximation. The update step follows the same pattern:

\[ y_k = z_k - h(\hat{x}_k^-), \qquad K_k = P_k^- H_k^\top \left( H_k P_k^- H_k^\top + R \right)^{-1} \]
(3.3)
true f(x) tangent used by the EKF x̂ (linearisation point) error f(x)
Figure 3.1 — At the estimate the tangent is perfect. Away from it the error grows with the curvature and with the width of your uncertainty. An EKF is accurate exactly in proportion to how confident and how correct it already was.

3.2 · What goes wrong

You linearise about a guess. If \(\hat{x}\) is off, the Jacobian is evaluated at the wrong point, the covariance update is wrong, the gain is wrong, and the next estimate is worse. The error compounds. This is divergence, and it is the classic EKF failure.

The covariance becomes a lie. Truncating the Taylor series discards real uncertainty, so \(P\) reports more confidence than is warranted. An overconfident filter down-weights good measurements, which makes it more wrong, which makes it more overconfident.

You must supply Jacobians. By hand, for a sixteen-dimensional state, with quaternions in it. They are tedious, error-prone, and silently wrong when you get a sign backwards.

And yet the EKF remains the workhorse of embedded state estimation, because it is cheap, deterministic, single-pass, and — when the sampling rate is high enough that the state barely moves between steps — the linearisation error is genuinely small.

9