3.5.24 · D3Guidance, Navigation & Control (GNC)
Worked examples — Extended Kalman Filter (EKF) — linearization, Jacobians
The scenario matrix
Everything the EKF can throw at you falls into one of these cells. Each worked example below is tagged with the cell(s) it covers.
| # | Case class | What is tricky | Example |
|---|---|---|---|
| A | Standard interior point (Q-I) | plain chain rule | Ex 1 |
| B | Other quadrants / sign flips (Q-II, III, IV) | signs of inside | Ex 2 |
| C | Degenerate input: target at origin | division by zero, bearing undefined | Ex 3 |
| D | Limiting value: pendulum at | stiffness | Ex 4 |
| E | Linear-collapse sanity ( linear) | Jacobian constant, no EKF needed | Ex 5 |
| F | Full one-step filter (numbers) | evaluate at the right point, gain, update | Ex 6 |
| G | Angle-wrap innovation trap | residual | Ex 7 |
| H | Real-world word problem | build , Jacobian, interpret units | Ex 8 |
| I | Exam-style twist: covariance shrink | must shrink | Ex 9 |
Radar measurement model used throughout (from the parent):
H=\begin{bmatrix} x/r & y/r\\[1mm] -y/r^2 & x/r^2\end{bmatrix}$$ Here $r$ is the **distance from origin to target** (length of the arrow), $\theta$ is the **bearing** (the angle that arrow makes with the $+x$ axis, measured counter-clockwise). Keep the picture below in mind for every radar example. ![[deepdives/dd-physics-3.5.24-d3-s01.png]] --- ## Example 1 — Interior point, first quadrant (Cell A) > [!example] Statement > Target at $(x,y)=(3,4)$. Compute the range/bearing measurement Jacobian $H$ and read off what a small $+x$ step does to the range. **Forecast:** Before computing — the target is up-and-to-the-right. A tiny step in $+x$ moves you *partly* outward. Do you expect $\partial r/\partial x$ to be closer to $0$, to $0.6$, or to $1$? Say a number. 1. **Compute $r$.** *Why this step?* Every entry of $H$ divides by $r$ or $r^2$, so get it first. $$r=\sqrt{3^2+4^2}=\sqrt{25}=5$$ 2. **Range partials.** *Why?* Row 1 of $H$ is $[x/r,\ y/r]$ — the chain rule on $\sqrt{x^2+y^2}$. $$\frac{\partial r}{\partial x}=\frac{3}{5}=0.6,\qquad \frac{\partial r}{\partial y}=\frac{4}{5}=0.8$$ 3. **Bearing partials.** *Why?* Row 2 is $[-y/r^2,\ x/r^2]$ — derivative of $\arctan(y/x)$. $$\frac{\partial\theta}{\partial x}=\frac{-4}{25}=-0.16,\qquad \frac{\partial\theta}{\partial y}=\frac{3}{25}=0.12$$ 4. **Assemble.** $$H=\begin{bmatrix} 0.6 & 0.8\\ -0.16 & 0.12\end{bmatrix}$$ **Verify:** $x/r=\cos\theta$ and $y/r=\sin\theta$. The target angle has $\cos\theta=0.6,\ \sin\theta=0.8$ — a 3-4-5 triangle, so $0.6$ is exactly right. Units of $\partial r/\partial x$: length per length = dimensionless ✓. Units of $\partial\theta/\partial x$: radians per length $=1/5$ ✓. --- ## Example 2 — All four quadrants (Cell B) > [!example] Statement > Same radar. For the four targets $(3,4)$, $(-3,4)$, $(-3,-4)$, $(3,-4)$ — all with $r=5$ — write the sign pattern of $H$ and confirm $\theta$ lands in the correct quadrant. **Forecast:** Row 1 of $H$ is $[\cos\theta,\ \sin\theta]$. In which quadrant are BOTH entries of row 1 negative? Guess before scrolling. 1. **Reuse the formula, just plug signs.** *Why?* $H$ depends only on $x,y$; $r=5$ is fixed. The *magnitudes* are always $0.6,0.8,0.16,0.12$, only signs move. | Target | Quadrant | $x/r$ | $y/r$ | $-y/r^2$ | $x/r^2$ | |--------|----------|-------|-------|----------|---------| | $(3,4)$ | I | $+0.6$ | $+0.8$ | $-0.16$ | $+0.12$ | | $(-3,4)$ | II | $-0.6$ | $+0.8$ | $-0.16$ | $-0.12$ | | $(-3,-4)$ | III | $-0.6$ | $-0.8$ | $+0.16$ | $-0.12$ | | $(3,-4)$ | IV | $+0.6$ | $-0.8$ | $+0.16$ | $+0.12$ | 2. **Bearing values via `atan2`.** *Why `atan2` not `arctan`?* Plain $\arctan(y/x)$ can't tell $(3,4)$ from $(-3,-4)$ — both give the same ratio $4/3$. [[atan2 & Angle Wrapping|`atan2(y,x)`]] uses the signs of *both* arguments to pick the true quadrant. $$\theta_{\text I}=53.13^\circ,\quad \theta_{\text{II}}=126.87^\circ,\quad \theta_{\text{III}}=-126.87^\circ,\quad \theta_{\text{IV}}=-53.13^\circ$$ **Verify:** Row 1 = $[\cos\theta,\sin\theta]$ is negative-negative only in Q-III, matching the table. $\cos(126.87^\circ)=-0.6$ ✓. The four bearings are symmetric about the axes as expected. ![[deepdives/dd-physics-3.5.24-d3-s02.png]] > [!mistake] Q-III trap > Using `arctan(y/x)` on $(-3,-4)$ returns $+53.13^\circ$ (points at Q-I!) because the two minus signs cancel inside the ratio. The Jacobian $H$ would still be numerically correct, but your predicted bearing $h(\hat{\mathbf x})$ would be off by $180^\circ$, poisoning the innovation. Always build $h$ with `atan2`. --- ## Example 3 — Degenerate input: target at the origin (Cell C) > [!example] Statement > What happens to $H$ as the target approaches the radar, $r\to 0$? Is the EKF measurement update usable there? **Forecast:** One entry of $H$ blows up faster than another. Which row explodes — range or bearing? Guess. 1. **Look at the powers of $r$.** *Why?* Diagnose the singularity by counting how each entry scales. $$\text{range row}\sim \frac{1}{r}\ \text{(entries }x/r,y/r),\qquad \text{bearing row}\sim\frac{1}{r^2}.$$ 2. **Take $r\to0$.** *Why?* This is the limiting/degenerate case. As $r\to0$ both rows diverge, and the bearing row diverges *faster* (as $1/r^2$). $$\lim_{r\to0}H=\text{undefined (unbounded)}.$$ 3. **Physical reason.** *Why does this make sense?* Standing *on* the radar, an infinitesimal sideways nudge swings the bearing by a huge angle — the sensitivity is genuinely infinite. Bearing is undefined at the origin; `atan2(0,0)` has no answer. **Verify:** Numerically, at $(0.01,0)$: $r=0.01$, bearing sensitivity $x/r^2=0.01/0.0001=100$ rad per unit — already enormous, and it grows as $1/r^2$. ✓ Confirms unbounded. > [!mistake] Handling the singularity > Near $r=0$ do **not** trust the bearing measurement — either inflate the bearing noise $R_{\theta\theta}$ toward infinity (so $K$ ignores it) or drop the bearing row entirely for that step. This is a real GNC failure mode when a tracked object passes directly overhead. --- ## Example 4 — Limiting value: pendulum stiffness vanishes (Cell D) > [!example] Statement > Pendulum state $\mathbf{x}=[\theta,\omega]^\top$, $f=[\omega,\ -\tfrac{g}{L}\sin\theta]^\top$, Jacobian $F=\begin{bmatrix}0&1\\ -\tfrac{g}{L}\cos\theta & 0\end{bmatrix}$. Evaluate $F$ at $\theta=0$, $\theta=\pi/2$, and $\theta=\pi$. Take $g/L=1$ for numbers. **Forecast:** The bottom-left entry $-\tfrac{g}{L}\cos\theta$ is the local "restoring stiffness". At which of the three angles does it **vanish**, meaning the pendulum locally feels no restoring pull? 1. **Bottom, $\theta=0$.** *Why?* Small-angle equilibrium; $\cos0=1$. $$F=\begin{bmatrix}0&1\\ -1 & 0\end{bmatrix}\quad\Rightarrow\quad \text{harmonic oscillator, eigenvalues } \pm i.$$ 2. **Horizontal, $\theta=\pi/2$.** *Why?* $\cos(\pi/2)=0$ — stiffness disappears. $$F=\begin{bmatrix}0&1\\ 0 & 0\end{bmatrix}\quad\Rightarrow\quad \text{eigenvalues both } 0,\ \text{no restoring force.}$$ 3. **Top, $\theta=\pi$ (inverted).** *Why?* $\cos\pi=-1$ flips the sign — unstable. $$F=\begin{bmatrix}0&1\\ +1 & 0\end{bmatrix}\quad\Rightarrow\quad \text{eigenvalues } \pm 1,\ \text{one grows: divergence.}$$ **Verify:** Eigenvalues of $\begin{bmatrix}0&1\\ -k&0\end{bmatrix}$ are $\pm\sqrt{-k}$. For $k=1$: $\pm i$ (oscillate). For $k=0$: $0,0$ (drift). For $k=-1$: $\pm1$ (one unstable) ✓. This is why an EKF happily tracks a swinging pendulum near the bottom but struggles balancing an inverted one — the linearization there has a growing mode. ![[deepdives/dd-physics-3.5.24-d3-s03.png]] --- ## Example 5 — Linear collapse: EKF becomes ordinary KF (Cell E) > [!example] Statement > Constant-velocity model $p_k=p_{k-1}+v_{k-1}\Delta t,\ v_k=v_{k-1}$ with $\Delta t=0.5$. Compute $F$ and show it does not depend on the state. **Forecast:** If $F$ has no $p$ or $v$ inside it, do you still need an *extended* filter? 1. **Differentiate.** *Why?* $F_{ij}=\partial f_i/\partial x_j$. $$F=\begin{bmatrix}\partial p'/\partial p & \partial p'/\partial v\\ \partial v'/\partial p & \partial v'/\partial v\end{bmatrix}=\begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}=\begin{bmatrix}1 & 0.5\\ 0 & 1\end{bmatrix}.$$ 2. **Check state-dependence.** *Why?* This is the "steel-check" from the parent. $$\text{No }p\text{ or }v\text{ appears} \Rightarrow F \text{ is constant.}$$ **Verify:** A constant $F$ means the Taylor remainder $\mathcal{O}(\|\mathbf x-\hat{\mathbf x}\|^2)$ is *exactly zero* (a linear map equals its own tangent). So $\hat{\mathbf x}^-=F\hat{\mathbf x}$ and $f(\hat{\mathbf x})$ agree perfectly — the EKF and the [[Kalman Filter (linear)|linear KF]] give identical results. If your Jacobian is constant, you never needed the EKF. ✓ --- ## Example 6 — One full EKF cycle with numbers (Cell F) > [!example] Statement > 1D range-only tracking of $\mathbf{x}=[x,y]^\top$, but suppose we only measure range $r=\sqrt{x^2+y^2}$ (scalar). Predicted state $\hat{\mathbf x}^-=(3,4)$, so $\hat r^-=5$. Covariance $P^-=\begin{bmatrix}1&0\\0&1\end{bmatrix}$, measurement noise $R=1$. A radar returns $z=5.5$. Do one update step: compute $H$, $S$, $K$, updated state, updated $P$. **Forecast:** The sensor says the target is a bit *farther* than predicted ($5.5>5$). Will the updated state move outward (larger $x,y$) or inward? Guess the direction. 1. **Linearize $h$ at the PREDICTED state.** *Why here?* The measurement compares against $\hat{\mathbf x}^-$, so $H$ must be evaluated at $(3,4)$ — the #1 EKF bug is using the old point. $$H=\begin{bmatrix}x/r & y/r\end{bmatrix}=\begin{bmatrix}0.6 & 0.8\end{bmatrix}.$$ 2. **Innovation (nonlinear $h$).** *Why nonlinear?* The residual uses the true model, not $H$. $$y=z-h(\hat{\mathbf x}^-)=5.5-5=0.5.$$ 3. **Innovation covariance $S$.** *Why?* $S=HP^-H^\top+R$ — how uncertain the predicted measurement is. $$S=\begin{bmatrix}0.6&0.8\end{bmatrix}\begin{bmatrix}1&0\\0&1\end{bmatrix}\begin{bmatrix}0.6\\0.8\end{bmatrix}+1=(0.36+0.64)+1=2.$$ 4. **Kalman gain.** *Why?* $K=P^-H^\top S^{-1}$ maps a range residual back onto state corrections. $$K=\begin{bmatrix}1&0\\0&1\end{bmatrix}\begin{bmatrix}0.6\\0.8\end{bmatrix}\cdot\tfrac12=\begin{bmatrix}0.3\\0.4\end{bmatrix}.$$ 5. **Update the mean.** *Why?* $\hat{\mathbf x}=\hat{\mathbf x}^-+Ky$. $$\hat{\mathbf x}=\begin{bmatrix}3\\4\end{bmatrix}+\begin{bmatrix}0.3\\0.4\end{bmatrix}(0.5)=\begin{bmatrix}3.15\\4.20\end{bmatrix}.$$ 6. **Update covariance.** *Why?* $P=(I-KH)P^-$ — knowledge went up, so spread must shrink. $$KH=\begin{bmatrix}0.3\\0.4\end{bmatrix}\begin{bmatrix}0.6&0.8\end{bmatrix}=\begin{bmatrix}0.18&0.24\\0.24&0.32\end{bmatrix},\quad P=\begin{bmatrix}0.82&-0.24\\-0.24&0.68\end{bmatrix}.$$ **Verify:** The correction moved *outward* ($3\to3.15$, $4\to4.20$) as forecast, since the measured range exceeded the predicted. New range $\sqrt{3.15^2+4.20^2}=5.25$, between prediction $5$ and measurement $5.5$ — a compromise, exactly what a Kalman blend should give ✓. Trace of $P$ dropped from $2$ to $1.5$, so [[Covariance Propagation|uncertainty shrank]] ✓. --- ## Example 7 — Angle-wrap innovation trap (Cell G) > [!example] Statement > Predicted bearing $\hat\theta^-=1^\circ$, radar returns $z_\theta=359^\circ$. Compute the *correct* innovation. **Forecast:** Naive subtraction gives $358^\circ$. But the target is only a whisker away. What is the true angular error, and its sign? 1. **Naive residual.** *Why show it?* To expose the bug. $$y_{\text{naive}}=359^\circ-1^\circ=358^\circ.$$ 2. **Wrap to $(-180^\circ,180^\circ]$.** *Why?* Angles are periodic; the shortest signed rotation is what physically matters. Add/subtract $360^\circ$ until in range. $$y=358^\circ-360^\circ=-2^\circ.$$ **Verify:** $359^\circ$ is the same heading as $-1^\circ$. From $\hat\theta^-=1^\circ$ to $-1^\circ$ is a step of $-2^\circ$ ✓ — tiny, not $358^\circ$. Feeding $358^\circ$ into $\hat{\mathbf x}=\hat{\mathbf x}^-+Ky$ would violently and wrongly jerk the estimate. Wrapping is mandatory whenever an [[atan2 & Angle Wrapping|angle]] appears in the measurement. --- ## Example 8 — Real-world word problem: altimeter over a hill (Cell H) > [!example] Statement > A drone flying horizontally at height $y$ over ground point directly below has a downward laser rangefinder. Over flat ground the reading is just $y$, but the drone is over a sloped hill whose surface height is $s(x)=0.1x$ (slope $10\%$). The laser measures $h(x,y)=y-0.1x$ (clearance above the sloped ground). Build $H$ and interpret each entry's sign. **Forecast:** Flying *forward* ($+x$) over rising ground — does the measured clearance go up or down? Predict the sign of $\partial h/\partial x$. 1. **Partial w.r.t. $x$.** *Why?* Ground rises with $x$, so clearance should change. $$\frac{\partial h}{\partial x}=\frac{\partial}{\partial x}(y-0.1x)=-0.1.$$ 2. **Partial w.r.t. $y$.** *Why?* Climbing directly increases clearance one-for-one. $$\frac{\partial h}{\partial y}=1.$$ 3. **Assemble.** $$H=\begin{bmatrix}-0.1 & 1\end{bmatrix}.$$ **Verify:** Sign of $\partial h/\partial x=-0.1<0$: moving forward over rising ground *reduces* clearance — matches intuition (the hill comes up to meet you) ✓. Units: clearance is a length, $x,y$ are lengths, so $\partial h/\partial y=1$ is dimensionless-per... a pure ratio $1$ ✓; $\partial h/\partial x=-0.1$ is the negative slope of the hill ✓. Note $h$ is linear, so this $H$ is constant — a rare case where even a "real world" model needs no re-linearization. --- ## Example 9 — Exam twist: does the covariance always shrink? (Cell I) > [!example] Statement > Scalar state, scalar measurement, $H=1$. Predicted variance $P^-=4$, measurement noise $R$. Show algebraically that after $P=(1-KH)P^-$ the updated variance is always $\le P^-$, and compute it for $R=4$ and for the limiting cases $R\to0$ and $R\to\infty$. **Forecast:** A perfect sensor ($R=0$) should pin the state exactly. What updated variance do you expect then — $0$, $2$, or $4$? 1. **Write the gain.** *Why?* $K=P^-H^\top S^{-1}$ with $S=H P^- H^\top+R=P^-+R$. $$K=\frac{P^-}{P^-+R}.$$ 2. **Updated variance.** *Why?* $P=(1-K)P^-$. $$P=\left(1-\frac{P^-}{P^-+R}\right)P^-=\frac{R}{P^-+R}\,P^-.$$ 3. **Bound it.** *Why?* The factor $\frac{R}{P^-+R}\in[0,1]$ for $R\ge0$, so $P\le P^-$ always. 4. **Cases.** *Why?* Cover both limits. $$R=4:\ P=\frac{4}{4+4}\cdot4=2;\qquad R\to0:\ P\to0;\qquad R\to\infty:\ P\to P^-=4.$$ **Verify:** $R=4$ gives $P=2$, exactly halved (equal trust in prediction and sensor) ✓. Perfect sensor $R\to0 \Rightarrow P\to0$: the state is pinned, matching the forecast ✓. Useless sensor $R\to\infty \Rightarrow P\to4$: no information gained, variance unchanged ✓. The measurement update can never *increase* uncertainty. --- > [!recall]- Quick self-test (reveal after guessing) > - In which quadrant are both entries of the range-row of $H$ negative? ::: Quadrant III (both $x<0$ and $y<0$). > - Where must $H$ be evaluated in Example 6? ::: At the predicted state $\hat{\mathbf x}^-=(3,4)$, not the old estimate. > - True innovation for $\hat\theta^-=1^\circ$, $z=359^\circ$? ::: $-2^\circ$ after wrapping. > - Updated variance for $P^-=4$, $R=4$, $H=1$? ::: $2$. > - At which pendulum angle does the local stiffness vanish? ::: $\theta=\pi/2$ (horizontal), since $\cos(\pi/2)=0$. > [!mnemonic] The scenario checklist > **"Sign, Singular, Slope, Same, Shrink, Spin."** — check every **sign**/quadrant, watch the **singular** origin, read the **slope** limits, remember linear collapses to the **same** old KF, covariance must **shrink**, and always wrap the **spin** (angle). See also: [[Unscented Kalman Filter (UKF)]] handles the large-residual / far-initial-guess cases where these linearizations break, and [[State Estimation in GNC]] for the bigger picture.