Extended Kalman Filter (EKF) — linearization, Jacobians

3.5.24 · Physics › Guidance, Navigation & Control (GNC)

EKF ki zaroorat KYU padti hai?

Standard KF KYA assume karta hai. Classic Kalman Filter ek Gaussian state estimate ko linear dynamics ke through propagate karta hai: $x_{k} = F x_{k - 1} + w, z_{k} = H x_{k} + v$ Ek Gaussian jab linear map se guzarta hai toh Gaussian hi rehta hai — isliye mean + covariance se sab kuch describe ho jaata hai.

GNC mein ye KYU fail hota hai. Navigation mein asli equations kuch aise dikhti hain: $x_{k} = f (x_{k - 1}, u_{k - 1}) + w, z_{k} = h (x_{k}) + v$ jahaan $f, h$ nonlinear hain. Example: ek radar range measure karta hai $r = x^{2} + y^{2}$ — ye square-root curved hai. Ek Gaussian ko curve se guzaaro toh wo non-Gaussian aur skewed nikalta hai. KF ka mean/covariance math ab valid nahi rehta.

EKF isko KAISE bachata hai. Current best estimate $\hat{x}$ ke around, hum curve ko uski tangent se approximate karte hain (Taylor first order tak). Ek chhote region mein curve ≈ line hoti hai, toh locally KF machinery phir se valid ho jaati hai.

Linearization first principles se derive karna

Ye sahi move KYU hai. KF ko constant matrices $F$ aur $H$ chahiye. Jacobian hi ek nonlinear map ka ek point pe sabse accha constant-matrix (linear) approximation hai — iske rows gradients hain, isliye ye exactly capture karta hai "har input mein ek unit change se har output kitna change hota hai."

Hum do EKF Jacobians define karte hain:

H_k=\left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k}^-}$$ Dhyan do **KAHAN** evaluate hota hai: $F$ *previous* estimate pe (propagation se pehle), $H$ *predicted* estimate $\hat{\mathbf{x}}_k^-$ pe (propagation ke baad, update se pehle). Galat evaluation point use karna EKF ka #1 bug hai. --- ## Poora EKF algorithm **Mean** *true* nonlinear function se guzarta hai; sirf **covariance** Jacobians use karta hai. Ye subtle aur important hai. > [!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$$ > *Mean ke liye nonlinear kyun lekin $P$ ke liye Jacobian kyun?* Agli state ka best guess actual function ka best current guess ke through result hai. Lekin covariance spread describe karta hai, aur spread linear sensitivity se transform hoti hai — isliye $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^-$$ **Gain kaise decide karta hai.** $K$ trust ko balance karta hai: bada measurement noise $R$ ⇒ chhota $K$ ⇒ prediction pe trust karo; chhota $R$ ⇒ bada $K$ ⇒ sensor pe trust karo. Linear KF jaisi hi logic, bas $H$ ab ek *local* Jacobian hai. ![[3.5.24-Extended-Kalman-Filter-(EKF)-—-linearization,-Jacobians.png]] --- ## Worked Example 1 — Range/bearing radar Jacobian 2D target ka state $\mathbf{x}=[x,\ y]^\top$ hai. Origin pe Radar **range** aur **bearing** measure karta hai: $$h(\mathbf{x})=\begin{bmatrix} r\\ \theta\end{bmatrix}=\begin{bmatrix}\sqrt{x^2+y^2}\\ \operatorname{atan2}(y,x)\end{bmatrix}$$ **Step 1 — range ke partials.** *Kyun?* $\partial r/\partial x$ aur $\partial r/\partial y$ chahiye. $$\frac{\partial r}{\partial x}=\frac{x}{\sqrt{x^2+y^2}}=\frac{x}{r},\qquad \frac{\partial r}{\partial y}=\frac{y}{r}$$ *Ye step kyun:* $\sqrt{x^2+y^2}$ pe chain rule lagate hain to $\frac{1}{2}(x^2+y^2)^{-1/2}\cdot 2x$ milta hai. **Step 2 — bearing ke partials.** *Kyun?* $\theta=\arctan(y/x)$, use karo $\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 — Jacobian assemble karo.** *Kyun:* rows = outputs, columns = inputs stack karo. $$H=\begin{bmatrix} x/r & y/r\\[1mm] -y/r^2 & x/r^2\end{bmatrix}$$ **Step 4 — ek point pe evaluate karo**, maano $(x,y)=(3,4)$ toh $r=5$: $$H=\begin{bmatrix} 0.6 & 0.8\\ -0.16 & 0.12\end{bmatrix}$$ *Interpretation:* $+x$ direction mein move karne se range 0.6 units per unit badhti hai — target angle ke $\cos$ se match karta hai. --- ## Worked Example 2 — Constant-velocity motion (linear $f$, phir bhi useful) 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}$$ *Ye kyun important hai:* yahaan $f$ already linear hai isliye $F$ constant hai — EKF ordinary KF mein collapse ho jaata hai. **Steel-check:** agar tumhare Jacobian mein state-dependence nahi hai, toh tumhe kabhi EKF ki zaroorat nahi thi. --- ## 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}$$ *$\cos\theta$ kyun?* $-\frac{g}{L}\sin\theta$ ka $\theta$ ke w.r.t. derivative. **Notice karo** Jacobian current angle pe depend karta hai — yahi toh poora point hai: $\theta=0$ ke paas ye linear small-angle oscillator jaisa dikhta hai; $\theta=\pi/2$ ke paas restoring stiffness khatam ho jaati hai. --- > [!mistake] Steel-manned common errors > **1. "Covariance ko bhi $f$ se nonlinearly push karo."** > *Ye sahi kyun lagta hai:* agar mean $f$ use karta hai, toh symmetry suggest karti hai $P$ bhi use kare. *Fix:* covariance ek *second-moment* object hai; ye linear sensitivity se transform hoti hai, $P^- = F P F^\top + Q$. Koi meaningful "$f(P)$" exist nahi karta. > > **2. $H$ ko predicted $\hat{\mathbf{x}}_k^-$ ke bajaye purani estimate $\hat{\mathbf{x}}_{k-1}$ pe evaluate karna.** > *Ye sahi kyun lagta hai:* tumne $F$ previous step pe compute kiya tha, toh wahi point reuse karo. *Fix:* measurement *predicted* state ke against compare hota hai, isliye $h$ ko **$\hat{\mathbf{x}}_k^-$ pe** linearize karo. > > **3. Innovation mein nonlinear $h$ bhool jaana.** $\mathbf{y}=\mathbf{z}-H\hat{\mathbf{x}}^-$ likhna. *Fix:* $\mathbf{y}=\mathbf{z}-h(\hat{\mathbf{x}}^-)$. $H$ sirf $S$ aur $K$ ke liye hai. > > **4. Bearing wrap-around.** $359^\circ-1^\circ$ jaisi angle mein innovation $358^\circ$ deta hai, lekin true error $-2^\circ$ hai. *Fix:* angle residuals ko $(-\pi,\pi]$ mein wrap karo. > > **5. Initial guess zyada door hone pe divergence.** *Ye sahi kyun lagta hai:* "ye ek filter hai, converge ho jaayega." *Fix:* tangent-line sirf $\hat{\mathbf{x}}$ ke *paas* valid hai; bura linearization point ⇒ garbage $F,H$ ⇒ divergence. Ek accha $\hat{\mathbf{x}}_0$ seed karo ya UKF use karo. --- > [!recall] Forecast-then-Verify (answer dekhne se pehle guess karo) > Har answer padhne se pehle apna guess zyoor se bolo. > - Agar $f$ already linear ho, toh $F$ kya ban jaata hai? ::: Ek constant matrix; EKF ordinary linear KF ban jaata hai. > - **Mean** $f$ se guzarta hai ya $F$ se? ::: True nonlinear $f$ se; sirf covariance $F$ use karti hai. > - $H_k$ kahaan evaluate karte ho? ::: Predicted state $\hat{\mathbf{x}}_k^-$ pe. > [!recall]- Feynman: 12-saal ke bachche ko samjhao > Socho tum raat ke andhere mein ek curvy pahaadi pe chal rahe ho aur guess karna chahte ho agle kadam ke baad kahaan pahunchoge. Curve complicated hai, isliye tum apne pairo ke neeche ki zameen feel karte ho aur maan lete ho ki pahaadi ek flat ramp hai jiska same slope hai ek kadam ke liye. Kadam lete ho, phir dobara feel karte ho, aur naya flat ramp banate ho. "Jo slope tum feel karte ho" wahi **Jacobian** hai — bas itna ki cheezein theek jahaan tum khade ho kitni tezi se change ho rahi hain. EKF curvy duniya mein ek waqt mein ek chhota flat ramp le ke chalta hai. > [!mnemonic] Flow yaad rakho > **"Predict Nonlinear, Spread by Slope; Innovate Nonlinear, Gain by Slope."** > (Means aur residuals $f,h$ use karte hain; covariances $P,S,K$ Jacobians $F,H$ use karte hain.) --- ## #flashcards/physics What is the Jacobian in the EKF? ::: First partial derivatives ka matrix $\partial g_i/\partial x_j$; ek point pe nonlinear function ka best local linear approximation. Standard KF nonlinear $f,h$ kyun handle nahi kar sakta? ::: Nonlinear map se guzarne ke baad Gaussian non-Gaussian ho jaata hai, isliye mean+covariance state ko poori tarah describe nahi kar sakta. Kaun si quantity true nonlinear $f$ se propagate hoti hai, aur kaun si $F$ se? ::: Mean $f$ se; covariance $F$ se ($FPF^\top+Q$ ke roop mein). $F_k$ kahaan evaluate hota hai? ::: $(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k-1})$ pe, prediction se pehle ki previous estimate. $H_k$ kahaan evaluate hota hai? ::: Predicted state $\hat{\mathbf{x}}_k^-$ pe. EKF innovation likho. ::: $\mathbf{y}_k=\mathbf{z}_k-h(\hat{\mathbf{x}}_k^-)$ nonlinear $h$ use karke. Range $r=\sqrt{x^2+y^2}$ ka $(x,y)$ ke w.r.t. Jacobian? ::: $[\,x/r,\ y/r\,]$. Bearing $\theta=\text{atan2}(y,x)$ ka $(x,y)$ ke w.r.t. Jacobian? ::: $[\,-y/r^2,\ x/r^2\,]$. EKF mein Kalman gain formula? ::: $K_k=P_k^- H_k^\top S_k^{-1}$ jahaan $S_k=H_kP_k^-H_k^\top+R$. Pendulum dynamics $\dot\omega=-\frac{g}{L}\sin\theta$: $\partial\dot\omega/\partial\theta$ kya hai? ::: $-\frac{g}{L}\cos\theta$. EKF ka main failure mode aur uski wajah? ::: Divergence; bure linearization point ki wajah se (bura initial estimate ya strong nonlinearity). Bade $R$ ka gain pe effect? ::: Chhota $K$ — noisy measurement ke bajaye model prediction pe trust karo. --- ## Connections - [[Kalman Filter (linear)]] — EKF ka parent; identical structure with constant $F,H$. - [[Unscented Kalman Filter (UKF)]] — Jacobians se bachta hai sigma points propagate karke; strong nonlinearity ke liye better. - [[Taylor Series & Linearization]] — Jacobian ke peeche ka mathematical engine. - [[Jacobian Matrix & Multivariable Calculus]] — general partial-derivative machinery. - [[Covariance Propagation]] — kyun $P\to FPF^\top+Q$. - [[State Estimation in GNC]] — navigation stacks mein EKF kahaan fit hota hai (INS/GPS fusion, attitude estimation). - [[atan2 & Angle Wrapping]] — bearing-residual bugs fix karne ke liye zaroorat. ## 🖼️ 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 ```