Extended Kalman Filter (EKF) — linearization, Jacobians

---

WHY do we even need the EKF?

WHAT the standard KF assumes. The classic Kalman Filter propagates a Gaussian state estimate through linear dynamics: $\mathbf{x}_k = F\,\mathbf{x}_{k-1} + w,\qquad \mathbf{z}_k = H\,\mathbf{x}_k + v$ A Gaussian pushed through a linear map stays Gaussian — so the mean + covariance are enough to describe everything.

WHY that fails in GNC. In navigation the true equations look like $\mathbf{x}_k = f(\mathbf{x}_{k-1},\mathbf{u}_{k-1}) + w,\qquad \mathbf{z}_k = h(\mathbf{x}_k) + v$ where $f,h$ are nonlinear. Example: a radar measures range $r=\sqrt{x^2+y^2}$ — that square-root is curved. Push a Gaussian through a curve and it comes out non-Gaussian & skewed. The KF's mean/covariance math no longer applies.

HOW the EKF rescues it. Around the current best estimate $\hat{\mathbf{x}}$, we approximate the curve by its tangent (Taylor to first order). Over a small region a curve ≈ a line, so locally the KF machinery is valid again.

---

Deriving the linearization from first principles

Why this is the right move. The KF needs the constant matrices $F$ and $H$. The Jacobian is the best constant-matrix (linear) approximation to a nonlinear map at a point — its rows are the gradients, so it captures exactly "how much does each output change per unit change in each input."

We define the two EKF Jacobians:

H_k=\left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k}^-}$$ Notice **WHERE** each is evaluated: $F$ at the *previous* estimate (before propagation), $H$ at the *predicted* estimate $\hat{\mathbf{x}}_k^-$ (after propagation, before update). Using the wrong evaluation point is the #1 EKF bug. --- ## The full EKF algorithm The **mean** goes through the *true* nonlinear function; only the **covariance** uses the Jacobians. This is subtle and important. > [!formula] Predict (time update) > $$\hat{\mathbf{x}}_k^- = f(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k-1})\qquad\text{(nonlinear!)}$$ > $$P_k^- = F_k\,P_{k-1}\,F_k^\top + Q$$ > *Why nonlinear for the mean but Jacobian for $P$?* The best guess of the next state is the actual function of the best current guess. But covariance describes spread, and spread transforms by the linear sensitivity — hence $F P F^\top$. > [!formula] Update (measurement update) > $$\text{innovation:}\quad \mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_k^-)\quad\text{(nonlinear!)}$$ > $$S_k = H_k P_k^- H_k^\top + R$$ > $$K_k = P_k^- H_k^\top S_k^{-1}\qquad(\text{Kalman gain})$$ > $$\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + K_k\,\mathbf{y}_k$$ > $$P_k = (I - K_k H_k)\,P_k^-$$ **HOW the gain decides.** $K$ balances trust: large measurement noise $R$ ⇒ small $K$ ⇒ trust the prediction; small $R$ ⇒ large $K$ ⇒ trust the sensor. Same logic as linear KF, only $H$ is now a *local* Jacobian. ![[3.5.24-Extended-Kalman-Filter-(EKF)-—-linearization,-Jacobians.png]] --- ## Worked Example 1 — Range/bearing radar Jacobian A 2D target has state $\mathbf{x}=[x,\ y]^\top$. Radar at origin measures **range** and **bearing**: $$h(\mathbf{x})=\begin{bmatrix} r\\ \theta\end{bmatrix}=\begin{bmatrix}\sqrt{x^2+y^2}\\ \operatorname{atan2}(y,x)\end{bmatrix}$$ **Step 1 — partials of range.** *Why?* Need $\partial r/\partial x$ and $\partial r/\partial y$. $$\frac{\partial r}{\partial x}=\frac{x}{\sqrt{x^2+y^2}}=\frac{x}{r},\qquad \frac{\partial r}{\partial y}=\frac{y}{r}$$ *Why this step:* chain rule on $\sqrt{x^2+y^2}$ gives $\frac{1}{2}(x^2+y^2)^{-1/2}\cdot 2x$. **Step 2 — partials of bearing.** *Why?* $\theta=\arctan(y/x)$, use $\frac{d}{du}\arctan u=\frac{1}{1+u^2}$. $$\frac{\partial\theta}{\partial x}=\frac{-y}{x^2+y^2}=\frac{-y}{r^2},\qquad \frac{\partial\theta}{\partial y}=\frac{x}{x^2+y^2}=\frac{x}{r^2}$$ **Step 3 — assemble the Jacobian.** *Why:* stack rows = outputs, columns = inputs. $$H=\begin{bmatrix} x/r & y/r\\[1mm] -y/r^2 & x/r^2\end{bmatrix}$$ **Step 4 — evaluate at a point**, say $(x,y)=(3,4)$ so $r=5$: $$H=\begin{bmatrix} 0.6 & 0.8\\ -0.16 & 0.12\end{bmatrix}$$ *Interpretation:* moving in $+x$ increases range by 0.6 units per unit — matches $\cos$ of the target angle. --- ## Worked Example 2 — Constant-velocity motion (linear $f$, still handy) State $\mathbf{x}=[p,\ v]^\top$, dynamics $p_k=p_{k-1}+v_{k-1}\,\Delta t,\ v_k=v_{k-1}$. $$f=\begin{bmatrix} p+v\Delta t\\ v\end{bmatrix}\Rightarrow F=\frac{\partial f}{\partial \mathbf{x}}=\begin{bmatrix}1 & \Delta t\\ 0 & 1\end{bmatrix}$$ *Why this matters:* here $f$ is already linear so $F$ is constant — the EKF collapses to the ordinary KF. **Steel-check:** if your Jacobian has no state-dependence, you never needed an EKF. --- ## Worked Example 3 — Pendulum (nonlinear dynamics) $\theta$-dynamics $\ddot\theta=-\frac{g}{L}\sin\theta$. State $\mathbf{x}=[\theta,\ \omega]^\top$, continuous $f=[\omega,\ -\frac{g}{L}\sin\theta]^\top$. $$F=\begin{bmatrix}\dfrac{\partial \dot\theta}{\partial\theta} & \dfrac{\partial\dot\theta}{\partial\omega}\\[2mm] \dfrac{\partial\dot\omega}{\partial\theta} & \dfrac{\partial\dot\omega}{\partial\omega}\end{bmatrix} =\begin{bmatrix}0 & 1\\ -\dfrac{g}{L}\cos\theta & 0\end{bmatrix}$$ *Why $\cos\theta$?* Derivative of $-\frac{g}{L}\sin\theta$ w.r.t. $\theta$. **Notice** the Jacobian depends on the current angle — that's the whole point: near $\theta=0$ it looks like the linear small-angle oscillator; near $\theta=\pi/2$ the restoring stiffness vanishes. --- > [!mistake] Steel-manned common errors > **1. "Push the covariance through $f$ nonlinearly too."** > *Why it feels right:* if the mean uses $f$, symmetry suggests $P$ should too. *Fix:* covariance is a *second-moment* object; it transforms by the linear sensitivity, $P^- = F P F^\top + Q$. There is no meaningful "$f(P)$." > > **2. Evaluating $H$ at the old estimate $\hat{\mathbf{x}}_{k-1}$ instead of the predicted $\hat{\mathbf{x}}_k^-$.** > *Why it feels right:* you computed $F$ at the previous step, so reuse the point. *Fix:* the measurement compares against the *predicted* state, so linearize $h$ **at** $\hat{\mathbf{x}}_k^-$. > > **3. Forgetting the innovation uses nonlinear $h$.** Writing $\mathbf{y}=\mathbf{z}-H\hat{\mathbf{x}}^-$. *Fix:* $\mathbf{y}=\mathbf{z}-h(\hat{\mathbf{x}}^-)$. $H$ is only for $S$ and $K$. > > **4. Bearing wrap-around.** Innovation in an angle like $359^\circ-1^\circ$ gives $358^\circ$, but the true error is $-2^\circ$. *Fix:* wrap angle residuals to $(-\pi,\pi]$. > > **5. Divergence when the initial guess is far off.** *Why it feels right:* "it's a filter, it'll converge." *Fix:* the tangent-line is only valid *near* $\hat{\mathbf{x}}$; a bad linearization point ⇒ garbage $F,H$ ⇒ divergence. Seed a good $\hat{\mathbf{x}}_0$ or use UKF. --- > [!recall] Forecast-then-Verify (predict before revealing) > Before reading each answer, say your guess aloud. > - If $f$ is already linear, what does $F$ become? ::: A constant matrix; the EKF becomes the ordinary linear KF. > - Does the **mean** get pushed through $f$ or through $F$? ::: Through the true nonlinear $f$; only the covariance uses $F$. > - Where do you evaluate $H_k$? ::: At the predicted state $\hat{\mathbf{x}}_k^-$. > [!recall]- Feynman: explain it to a 12-year-old > Imagine you're walking on a curvy hill in the dark and you want to guess where your next step lands. The curve is complicated, so you feel the ground right under your feet and pretend the hill is a flat ramp with that same slope for one step. You step, then feel again, and make a new flat ramp. The "slope you feel" is the **Jacobian** — just how steeply things change right where you're standing. The EKF walks the curvy world one flat little ramp at a time. > [!mnemonic] Remember the flow > **"Predict Nonlinear, Spread by Slope; Innovate Nonlinear, Gain by Slope."** > (Means and residuals use $f,h$; covariances $P,S,K$ use the Jacobians $F,H$.) --- ## #flashcards/physics What is the Jacobian in the EKF? ::: The matrix of first partial derivatives $\partial g_i/\partial x_j$; the best local linear approximation of a nonlinear function at a point. Why can't the standard KF handle nonlinear $f,h$? ::: A Gaussian through a nonlinear map becomes non-Gaussian, so mean+covariance no longer fully describe the state. Which quantity is propagated through the true nonlinear $f$, and which through $F$? ::: The mean through $f$; the covariance through $F$ (as $FPF^\top+Q$). Where is $F_k$ evaluated? ::: At $(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k-1})$, the previous estimate before prediction. Where is $H_k$ evaluated? ::: At the predicted state $\hat{\mathbf{x}}_k^-$. Write the EKF innovation. ::: $\mathbf{y}_k=\mathbf{z}_k-h(\hat{\mathbf{x}}_k^-)$ using the nonlinear $h$. Jacobian of range $r=\sqrt{x^2+y^2}$ w.r.t. $(x,y)$? ::: $[\,x/r,\ y/r\,]$. Jacobian of bearing $\theta=\text{atan2}(y,x)$ w.r.t. $(x,y)$? ::: $[\,-y/r^2,\ x/r^2\,]$. Kalman gain formula in EKF? ::: $K_k=P_k^- H_k^\top S_k^{-1}$ with $S_k=H_kP_k^-H_k^\top+R$. Pendulum dynamics $\dot\omega=-\frac{g}{L}\sin\theta$: what is $\partial\dot\omega/\partial\theta$? ::: $-\frac{g}{L}\cos\theta$. Main failure mode of the EKF and its cause? ::: Divergence; caused by a poor linearization point (bad initial estimate or strong nonlinearity). Effect of large $R$ on the gain? ::: Small $K$ — trust the model prediction over the noisy measurement. --- ## Connections - [[Kalman Filter (linear)]] — the EKF's parent; identical structure with constant $F,H$. - [[Unscented Kalman Filter (UKF)]] — avoids Jacobians by propagating sigma points; better for strong nonlinearity. - [[Taylor Series & Linearization]] — the mathematical engine behind the Jacobian. - [[Jacobian Matrix & Multivariable Calculus]] — general partial-derivative machinery. - [[Covariance Propagation]] — why $P\to FPF^\top+Q$. - [[State Estimation in GNC]] — where EKF sits in navigation stacks (INS/GPS fusion, attitude estimation). - [[atan2 & Angle Wrapping]] — needed to fix bearing-residual bugs. ## 🖼️ Concept Map ```mermaid flowchart TD KF[Linear Kalman Filter] -->|assumes linear f,h| GAUSS[Gaussian stays Gaussian] NL[Nonlinear world orbits, radar] -->|breaks| KF NL -->|f,h nonlinear| SKEW[Push Gaussian thru curve gives non-Gaussian] SKEW -->|motivates| EKF[Extended Kalman Filter] EKF -->|linearize about x-hat| TAYLOR[First-order Taylor expansion] TAYLOR -->|slope is| JAC[Jacobian matrix] JAC -->|best linear approx| FK[F_k at prev estimate] JAC -->|best linear approx| HK[H_k at predicted estimate] FK -->|feeds| KFEQ[Ordinary KF equations] HK -->|feeds| KFEQ KFEQ -->|locally valid| EKF ``` ## 🔊 Hinglish (regional understanding) > [!intuition]- Hinglish mein samjho > Dekho, normal Kalman Filter tabhi perfect kaam karta hai jab tumhara system **linear** ho — matlab motion aur measurement dono straight-line equations ho. Lekin real GNC mein cheezein curved hoti hain: radar range mein $\sqrt{x^2+y^2}$ aata hai, pendulum mein $\sin\theta$ aata hai. In curves ke through jab tum ek Gaussian (bell curve) ko push karte ho, to woh tedha-medha ho jaata hai aur KF ka maths fail ho jaata hai. > > EKF ka jugaad simple hai: **har step pe curve ko uske tangent line se replace kar do**, current best estimate $\hat{x}$ ke around. Us tangent ka slope hi **Jacobian** hai — bas partial derivatives ka matrix, jo batata hai ki har output kitna change hota hai har input ke change se. Chain rule laga ke nikaalo, jaise range ka Jacobian $[x/r,\ y/r]$ nikalta hai. Ek baar tangent mil gaya, to purana linear KF chala do. > > Sabse important cheez jo log bhool jaate hain: **mean ko toh original nonlinear $f$ aur $h$ se hi bhejo**, sirf **covariance** $P$ ke liye Jacobian $F,H$ use karo ($P^-=FPF^\top+Q$). Aur $H$ ko hamesha **predicted state** $\hat{x}^-$ pe evaluate karo, purane estimate pe nahi. Mnemonic yaad rakho: "Predict Nonlinear, Spread by Slope; Innovate Nonlinear, Gain by Slope." > > Yeh matter kyun karta hai? Kyunki satellite navigation, INS/GPS fusion, drone attitude estimation — sab jag ![[audio/3.5.24-Extended-Kalman-Filter-(EKF)-—-linearization,-Jacobians.mp3]]