Extended Kalman Filter (EKF) — linearization, Jacobians: Exercises

Notation refresher (every symbol used below, defined once)

Before any problem, here is every letter you will meet, in plain words:

Definition The alphabet of the EKF

$x$ — the state vector: the list of numbers we are trying to estimate (position, velocity, angle...). Written as a column.
$\hat{x}$ — the estimate (a "hat" means "our best guess", not the truth).
$\hat{x}_{k}^{-}$ — the prediction at step $k$ before we look at the new measurement (the minus = "prior").
$u$ — the control input: the commands we send to the system (thrust, steering, motor torque). It is known, not estimated. In $f (\hat{x}_{k - 1}, u_{k - 1})$ it enters as a fixed number, so it does not appear in the state Jacobian $F = \partial f / \partial x$ . If there is no active command (free-falling pendulum, coasting target), simply $u = 0$ — which is the case in every exercise below.
$f (\cdot)$ — the motion model: how the state moves forward one step. May be curved (nonlinear).
$h (\cdot)$ — the measurement model: what our sensor should read given a state. May be curved.
$F = \frac{\partial f}{\partial x}$ , $H = \frac{\partial h}{\partial x}$ — the Jacobians: matrices of slopes (partial derivatives). Row $i$ , column $j$ = "how fast does output $i$ change when input $j$ nudges". This is the flat tangent replacing the curve.
$P$ — the covariance matrix: how uncertain / spread out the estimate is.
$Q, R$ — process- and measurement-noise covariance (how much we distrust the model / the sensor).
$z$ — the actual sensor reading. $y = z - h (\hat{x}^{-})$ — the innovation (surprise = reading minus prediction).
$S$ — innovation covariance; $K$ — Kalman gain (how much to trust the surprise).
$Δ t$ — the time between steps.

Keep this open in a side pane; the solutions call each symbol by name.

Level 1 — Recognition

L1.1 — Read the Jacobian's shape

A state has $n = 3$ numbers; a sensor returns $m = 2$ numbers via $h (x)$ . What are the dimensions of $H$ ? And which entry answers "how does the 2nd sensor output change when the 1st state changes"?

Recall Solution

WHAT $H$ is: a table with one row per output and one column per input. Rows = outputs ( $m = 2$ ), columns = inputs ( $n = 3$ ). $H is 2 \times 3.$ The requested entry: "2nd output, 1st input" = row 2, column 1 = $H_{21} = \frac{\partial h _{2}}{\partial x _{1}}$ . WHY: the Jacobian's job is exactly to store every "output-per-input" slope. Reading its shape = reading (number of sensor channels) × (number of state variables).

L1.2 — Where does the mean go?

In the EKF predict step, does the best estimate $\hat{x}_{k - 1}$ get pushed through the nonlinear $f$ or through the matrix $F$ ?

Recall Solution

Through the true nonlinear $f$ : $\hat{x}_{k}^{-} = f (\hat{x}_{k - 1}, u_{k - 1})$ . WHY: the single best guess of tomorrow's state is just the real motion law applied to today's best guess. The Jacobian $F$ is only used to move the spread (covariance): $P_{k}^{-} = F P_{k - 1} F^{⊤} + Q$ . Confusing these two is the classic error — mean uses the curve, uncertainty uses its tangent.

Level 2 — Application

L2.1 — Range Jacobian at a new point

Radar at the origin, target state $x = [x, y]^{⊤}$ , range $r = x^{2} + y^{2}$ . Compute $\frac{\partial r}{\partial x}, \frac{\partial r}{\partial y}$ and evaluate at $(x, y) = (0, 4)$ .

Figure — Extended Kalman Filter (EKF) — linearization, Jacobians

Figure s01 — the teal line is the line-of-sight from the radar at the origin to the target at $(0, 4)$ . The plum arrow shows a sideways ( $+ x$ ) nudge that barely changes the length (slope $0$ ); the orange arrow shows an outward ( $+ y$ ) nudge that lengthens the line one-for-one (slope $1$ ).

Recall Solution

Step 1 — differentiate the square root. Why chain rule? $r$ is a curve; we need its slope in each direction. $\frac{\partial r}{\partial x} = \frac{1}{2} (x^{2} + y^{2})^{- 1/2} \cdot 2 x = \frac{x}{r}, \frac{\partial r}{\partial y} = \frac{y}{r} .$ Step 2 — plug in $(0, 4)$ : $r = 0 + 16 = 4$ . $\frac{\partial r}{\partial x} = \frac{0}{4} = 0, \frac{\partial r}{\partial y} = \frac{4}{4} = 1.$ WHAT IT LOOKS LIKE (see figure s01): the target sits straight up the $y$ -axis. Sliding it sideways (in $x$ , plum arrow) barely changes the distance to origin — the range curve is flat in that direction, so the slope is $0$ . Sliding it outward along $y$ (orange arrow) changes range one-for-one, so the slope is $1$ . These are exactly $cos$ and $sin$ of the line-of-sight angle ( $9 0^{\circ}$ here).

L2.2 — Constant-velocity $F$

State $[p, v]^{⊤}$ , dynamics $p_{k} = p_{k - 1} + v_{k - 1} Δ t, v_{k} = v_{k - 1}$ , with $Δ t = 0.5$ . Write $F$ numerically.

Recall Solution

Step 1 — partials. $\partial p_{k} / \partial p = 1$ , $\partial p_{k} / \partial v = Δ t$ ; $\partial v_{k} / \partial p = 0$ , $\partial v_{k} / \partial v = 1$ . $F = [10 Δ t 1] = [10 0.5 1] .$ WHY it's constant: $f$ here is already a straight-line (linear) map, so its slope doesn't depend on where you are — the tangent equals the function itself. The EKF collapses to the plain Kalman Filter (linear).

Level 3 — Analysis

L3.1 — Full range/bearing Jacobian, and its determinant

For $h = [r, θ]^{⊤} = [x^{2} + y^{2}, atan2 (y, x)]^{⊤}$ , assemble $H$ symbolically, then compute $det H$ at $(3, 4)$ .

Recall Solution

Step 1 — range row (from L2.1): $[x / r, y / r]$ . Step 2 — bearing row. Why $\frac{d}{d u} arctan u = \frac{1}{1 + u ^{2}}$ ? Bearing is an angle; its rate of change with the coordinates is what we need. $\frac{\partial θ}{\partial x} = \frac{- y}{x ^{2} + y ^{2}} = \frac{- y}{r ^{2}}, \frac{\partial θ}{\partial y} = \frac{x}{r ^{2}} .$ Step 3 — stack: $H = [x / r - y / r^{2} y / r x / r^{2}] .$ Step 4 — at $(3, 4)$ , $r = 5$ : $H = [0.6 - 0.16 0.8 0.12], det H = (0.6) (0.12) - (0.8) (- 0.16) = 0.072 + 0.128 = 0.2.$ Interpretation: the general determinant is $\frac{x}{r} \cdot \frac{x}{r ^{2}} - \frac{y}{r} \cdot \frac{- y}{r ^{2}} = \frac{x ^{2} + y ^{2}}{r ^{3}} = \frac{r ^{2}}{r ^{3}} = \frac{1}{r} = \frac{1}{5} = 0.2.$ It equals $1/ r$ — the mapping from $(x, y)$ to $(r, θ)$ stretches area by $1/ r$ , so the linearization is well-conditioned everywhere except at the origin, where it blows up.

L3.2 — Why the bearing Jacobian dies at the origin

Using $H$ above, describe what happens as $(x, y) \to (0, 0)$ , and why physically.

Recall Solution

As $r \to 0$ : the range row entries $x / r, y / r$ stay bounded (they are $cos, sin$ of the angle), but the bearing row entries $- y / r^{2}, x / r^{2}$ blow up like $1/ r$ . And $det H = 1/ r \to \infty$ . WHY physically: right on top of the sensor, an infinitesimal sideways move swings the bearing by a huge angle — direction is undefined at a point. The tangent-line approximation (Taylor Series & Linearization) requires a smooth function locally; here $θ$ is singular, so the EKF is invalid at the origin. This is a degenerate case you must guard against.

Level 4 — Synthesis

L4.1 — Pendulum: linearize and read the stiffness

Continuous pendulum $\ddot{θ} = - \frac{g}{L} sin θ$ , state $[θ, ω]^{⊤}$ , $f = [ω, - \frac{g}{L} sin θ]^{⊤}$ . Take $g = 9.8, L = 1$ . Compute $F$ at $θ = 0$ and at $θ = π /2$ , and interpret.

Figure s02 — the teal curve is the restoring acceleration $- \frac{g}{L} sin θ$ . The orange dashed tangent at $θ = 0$ is steep (slope $- 9.8$ = full stiffness); the plum dashed tangent at $θ = π /2$ is flat (slope $0$ = no stiffness). The tangent's slope IS the bottom-left Jacobian entry.

Recall Solution

Step 1 — general Jacobian. $F = [\partial \dot{θ} / \partial θ \partial \overset{ω}{˙} / \partial θ \partial \dot{θ} / \partial ω \partial \overset{ω}{˙} / \partial ω] = [0 - \frac{g}{L} cos θ 10] .$ Why $cos θ$ ? $\frac{d}{d θ} (- \frac{g}{L} sin θ) = - \frac{g}{L} cos θ$ . Step 2 — at $θ = 0$ : $cos 0 = 1$ , so $F = [0 - 9.8 10] .$ The bottom-left $- 9.8$ is the restoring stiffness. Full strength ⇒ this is the ordinary small-angle oscillator. Step 3 — at $θ = π /2$ : $cos (π /2) = 0$ , so $F = [0010] .$ The stiffness has vanished. Horizontal pendulum: gravity gives no restoring twist at that instant. WHAT IT LOOKS LIKE (see figure s02): the restoring term $- \frac{g}{L} sin θ$ is a curve; its slope (the stiffness entry) is steepest at $θ = 0$ (orange tangent) and flat at $θ = \pm π /2$ (plum tangent). The Jacobian genuinely changes with the state — that is the whole reason we re-linearize every step.

L4.2 — One full predict step for the pendulum

Using forward Euler over $Δ t = 0.1$ (so the discrete map is $x_{k} = x_{k - 1} + Δ t f (x_{k - 1})$ ), and starting from $\hat{x}_{k - 1} = [θ, ω]^{⊤} = [0.2, 0]^{⊤}$ , $P_{k - 1} = I_{2}$ , $Q = 0$ , propagate the mean and the covariance.

Recall Solution

Mean (nonlinear $f$ ): $\hat{x}_{k}^{-} = [θ + Δ t ω ω + Δ t (- \frac{g}{L} sin θ)] = [0.2 + 0.1 \cdot 0 0 + 0.1 \cdot (- 9.8 sin 0.2)] .$ $sin 0.2 = 0.19867$ , so the $ω$ entry $= 0.1 \cdot (- 9.8 \cdot 0.19867) = - 0.19470$ . $\hat{x}_{k}^{-} = [0.2 - 0.19470] .$ Covariance — first, WHY the discrete Jacobian is $F_{d} = I + Δ t F$ . The covariance must be transported by the slope of the discrete one-step map $x_{k} = g_{d} (x_{k - 1}) = x_{k - 1} + Δ t f (x_{k - 1})$ . Take its Jacobian term-by-term: the derivative of $x_{k - 1}$ with respect to itself is the identity $I$ ; the derivative of $Δ t f (x_{k - 1})$ is $Δ t$ times $f$ 's own Jacobian $F$ . Adding them: $F_{d} = \frac{\partial g _{d}}{\partial x} = from x_{k - 1} I + Δ t from Δ t f F .$ That is not a memorized formula — it is just "differentiate the Euler step". (This is the first-order Taylor Series & Linearization of the flow over one small $Δ t$ .) Now build $F_{d}$ using $F$ from L4.1 at $θ = 0.2$ : $cos 0.2 = 0.98007$ , so $- \frac{g}{L} cos θ = - 9.60464$ . $F = [0 - 9.60464 10], F_{d} = I + 0.1 F = [1 - 0.960464 0.1 1] .$ With $P_{k - 1} = I$ , $Q = 0$ : $P_{k}^{-} = F_{d} I F_{d}^{⊤} = F_{d} F_{d}^{⊤} .$ Compute each entry carefully (row $i$ of $F_{d}$ dotted with row $j$ of $F_{d}$ ):

$(1, 1) = 1^{2} + 0. 1^{2} = 1.01$
$(2, 2) = (- 0.960464)^{2} + 1^{2} = 1.922491$
$(1, 2) = (2, 1) = 1 \cdot (- 0.960464) + 0.1 \cdot 1 = - 0.860464$

$P_{k}^{-} = [1.01 - 0.860464 - 0.860464 1.922491] .$ WHY the split: the mean rode the true curve $f$ ; the uncertainty $P$ rode the tangent $F_{d}$ via $F_{d} P F_{d}^{⊤}$ . Covariance is a second-moment object and has no meaningful " $f (P)$ ".

Level 5 — Mastery

L5.1 — Full range/bearing measurement update with angle wrap

2D target, predicted state $\hat{x}_{k}^{-} = [3, 4]^{⊤}$ (so $r = 5$ ), $P_{k}^{-} = I_{2}$ , $R = diag (0.25, 0.01)$ (range var $0.25$ , bearing var $0.01 rad^{2}$ ). The radar's raw reported bearing is $θ_{z} = 2 π + atan2 (4, 3) - 0.05 = 6.233185 + 0.927295 - 2 π (unwrapped)$ — see Step 2 for exactly what the sensor sent. The reported range is $z_{r} = 5.5$ . Compute the correctly-wrapped angle innovation, then the innovation $y$ , $S$ , $K$ , and the posterior state.

Recall Solution

Step 1 — predicted measurement. $h (\hat{x}^{-}) = [5, atan2 (4, 3)]^{⊤}$ , and $atan2 (4, 3) = 0.927295$ rad. So the predicted bearing is $θ_{pred} = 0.927295$ . Step 2 — what the sensor actually sent, numerically. The true bearing error is a small $- 0.05$ rad. But an angle sensor reports values modulo $2 π$ , and this one happened to wrap its reading up near $2 π$ : it reported $θ_{z} = (0.927295 - 0.05) + 2 π (wrapped high) = 0.877295 + 6.283185 = 7.160480 rad .$ If you naively subtract, the raw residual is $θ_{z} - θ_{pred} = 7.160480 - 0.927295 = 6.233185 = 2 π - 0.05.$ That looks like a huge $+ 6.23$ rad error — but a $6.23$ rad turn is almost a full circle, i.e. essentially the same direction. The real error is tiny. Step 3 — wrap the bearing innovation into $(- π, π]$ . Why? Two angles that differ by a full turn point the same way; the meaningful residual must live in $(- π, π]$ . Subtract $2 π$ : $6.233185 - 2 π = - 0.05$ rad. See atan2 & Angle Wrapping. $y = [z_{r} - r_{pred} wrap (θ_{z} - θ_{pred})] = [5.5 - 5 - 0.05] = [0.5 - 0.05] .$ Step 4 — Jacobian at $(3, 4)$ (from L3.1): $H = [0.6 - 0.16 0.8 0.12]$ . Step 5 — innovation covariance $S = H P^{-} H^{⊤} + R$ . With $P^{-} = I$ , $H P^{-} H^{⊤} = H H^{⊤}$ : $H H^{⊤} = [0. 6^{2} + 0. 8^{2} (- 0.16) (0.6) + (0.12) (0.8) 0.6 (- 0.16) + 0.8 (0.12) 0.1 6^{2} + 0.1 2^{2}] = [1.0 0 0 0.04] .$ WHY the cross terms vanish (do not skip this): the off-diagonal $H H_{12}^{⊤}$ is the dot product of $H$ 's two rows — the range gradient $[0.6, 0.8]$ and the bearing gradient $[- 0.16, 0.12]$ . Their dot product is $0.6 (- 0.16) + 0.8 (0.12) = - 0.096 + 0.096 = 0$ : the two rows are orthogonal. This is not luck — range increases radially while bearing increases tangentially, and radial and tangential directions are always perpendicular. So with $P^{-} = I$ the mapped innovation covariance is exactly diagonal at this operating point. In general (any $P^{-} \neq = I$ , or different $x, y$ ) it will not be diagonal. $S = [1.0 + 0.25 0 0 0.04 + 0.01] = [1.25 0 0 0.05] .$ Step 6 — invert $S$ and form the gain $K = P^{-} H^{⊤} S^{- 1} = H^{⊤} S^{- 1}$ (since $P^{-} = I$ ). Here $S$ is diagonal, so $S^{- 1}$ just reciprocates the diagonal: $H^{⊤} = [0.6 0.8 - 0.16 0.12], S^{- 1} = [1/1.25 0 0 1/0.05] = [0.8 0 020] .$ $K = H^{⊤} S^{- 1} = [0.6 \cdot 0.8 0.8 \cdot 0.8 (- 0.16) \cdot 20 0.12 \cdot 20] = [0.48 0.64 - 3.2 2.4] .$ Step 7 — posterior state $\hat{x}_{k} = \hat{x}_{k}^{-} + K y$ : $K y = [0.48 \cdot 0.5 + (- 3.2) (- 0.05) 0.64 \cdot 0.5 + 2.4 \cdot (- 0.05)] = [0.24 + 0.16 0.32 - 0.12] = [0.4 0.2] .$ $\hat{x}_{k} = [34] + [0.4 0.2] = [3.4 4.2] .$ Step 8 — posterior covariance $P_{k} = (I - K H) P_{k}^{-}$ (completes the update): $K H = [0.48 0.64 - 3.2 2.4] [0.6 - 0.16 0.8 0.12] = [0.48 (0.6) + (- 3.2) (- 0.16) 0.64 (0.6) + 2.4 (- 0.16) 0.48 (0.8) + (- 3.2) (0.12) 0.64 (0.8) + 2.4 (0.12)] = [0.8 0 0 0.8] .$ $P_{k} = (I - K H) I = [1 - 0.8 0 0 1 - 0.8] = [0.2 0 0 0.2] .$ The uncertainty shrank from $1.0$ to $0.2$ on each axis — the measurement genuinely sharpened our estimate. Interpretation: the range surprise ( $+ 0.5$ ) pushes the estimate outward; the (small, correctly-wrapped) bearing surprise nudges it slightly. Had we not wrapped, the bearing innovation $\approx 6.23$ would have catastrophically thrown the estimate — this is why wrapping is mandatory, not cosmetic.

Conditioning guard (general case). $S$ came out clean and diagonal only because $P^{-} = I$ and the rows of $H$ happened to be orthogonal here. In a real filter you must always check $S$ before inverting: if $det S$ is near zero (e.g. small range makes the bearing row huge, or two channels become nearly redundant), $S^{- 1}$ amplifies noise and $K$ explodes. Fix: solve $K S = P^{-} H^{⊤}$ by a stable linear solve (Cholesky) instead of forming $S^{- 1}$ explicitly, add a small jitter $ε I$ to $R$ to regularize, and monitor $det S$ / condition number each step. See Covariance Propagation.

L5.2 — When would you abandon the EKF entirely?

Give two concrete conditions from the exercises above where the EKF's core assumption breaks, and name the fix.

Recall Solution

Condition A — singular linearization (L3.2): near $r = 0$ the bearing Jacobian blows up ( $1/ r^{2}$ ) and Taylor's neglected $O (∥ \cdot ∥^{2})$ terms dominate. Fix: switch to the Unscented Kalman Filter (UKF) (uses sample points, no Jacobian) or skip the bearing channel. Condition B — large linearization error / bad initial guess: the tangent is only valid near $\hat{x}$ (parent-note mistake #5). A far-off seed ⇒ wrong $F, H$ ⇒ divergence. Fix: seed a good $\hat{x}_{0}$ , inflate $P_{0}$ , or use the UKF. Contrast with the plain Kalman Filter (linear), which never has this problem because its map is exactly linear. WHY these are the same root cause: both are failures of "curve ≈ line locally". When the curvature over your uncertainty region is not small, first-order Taylor Series & Linearization simply is not accurate enough.

Recall Feynman self-test (say it in one breath)

Explain to a 12-year-old why the EKF pushes the guess through the real curved rule but pushes the uncertainty through a flat straight-line copy of that rule ::: Because your single best guess should follow the real physics, but "how unsure you are" is a little cloud around the guess — and to see how a small cloud gets stretched, you only need the local steepness (slope) of the rule, which is the flat tangent = the Jacobian.