3.5.24 · D3 · HinglishGuidance, Navigation & Control (GNC)
Worked examples — Extended Kalman Filter (EKF) — linearization, Jacobians
3.5.24 · D3· Physics › Guidance, Navigation & Control (GNC) › Extended Kalman Filter (EKF) — linearization, Jacobians
Scenario matrix
Jo bhi EKF tumhare saamne throw kar sakta hai, woh in cells mein se kisi ek mein aata hai. Neeche har worked example un cells ke saath tagged hai jo woh cover karta hai.
| # | Case class | Kya tricky hai | Example |
|---|---|---|---|
| A | Standard interior point (Q-I) | plain chain rule | Ex 1 |
| B | Doosre quadrants / sign flips (Q-II, III, IV) | ke andar ke signs | 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) | ko sahi point par evaluate karo, gain, update | Ex 6 |
| G | Angle-wrap innovation trap | residual | Ex 7 |
| H | Real-world word problem | banao, Jacobian, units interpret karo | Ex 8 |
| I | Exam-style twist: covariance shrink | zaroor shrink hona chahiye | Ex 9 |
Radar measurement model jo poore note mein use hoga (parent se):
H=\begin{bmatrix} x/r & y/r\\[1mm] -y/r^2 & x/r^2\end{bmatrix}$$ Yahan $r$ **origin se target tak ki distance** hai (arrow ki length), $\theta$ **bearing** hai (woh angle jo arrow $+x$ axis ke saath banata hai, counter-clockwise measure kiya gaya). Har radar example ke liye neeche wali picture mind mein rakhna. ![[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)$. Range/bearing measurement Jacobian $H$ compute karo aur padhkar batao ki ek chhoti $+x$ step range ke saath kya karti hai. **Forecast:** Compute karne se pehle — target upar-aur-daayein hai. $+x$ mein ek tiny step tumhe *thoda* baahir move karti hai. Kya tumhara expect hai ki $\partial r/\partial x$ $0$ ke kareeb hoga, $0.6$ ke, ya $1$ ke? Ek number bolo. 1. **$r$ compute karo.** *Kyun yeh step?* $H$ ki har entry $r$ ya $r^2$ se divide hoti hai, isliye pehle yeh nikalo. $$r=\sqrt{3^2+4^2}=\sqrt{25}=5$$ 2. **Range partials.** *Kyun?* $H$ ki Row 1 hai $[x/r,\ y/r]$ — $\sqrt{x^2+y^2}$ par chain rule. $$\frac{\partial r}{\partial x}=\frac{3}{5}=0.6,\qquad \frac{\partial r}{\partial y}=\frac{4}{5}=0.8$$ 3. **Bearing partials.** *Kyun?* Row 2 hai $[-y/r^2,\ x/r^2]$ — $\arctan(y/x)$ ka derivative. $$\frac{\partial\theta}{\partial x}=\frac{-4}{25}=-0.16,\qquad \frac{\partial\theta}{\partial y}=\frac{3}{25}=0.12$$ 4. **Assemble karo.** $$H=\begin{bmatrix} 0.6 & 0.8\\ -0.16 & 0.12\end{bmatrix}$$ **Verify:** $x/r=\cos\theta$ aur $y/r=\sin\theta$. Target angle ka $\cos\theta=0.6,\ \sin\theta=0.8$ hai — ek 3-4-5 triangle, isliye $0.6$ bilkul sahi hai. $\partial r/\partial x$ ki units: length per length = dimensionless ✓. $\partial\theta/\partial x$ ki units: radians per length $=1/5$ ✓. --- ## Example 2 — Charon quadrants (Cell B) > [!example] Statement > Same radar. Chaar targets $(3,4)$, $(-3,4)$, $(-3,-4)$, $(3,-4)$ ke liye — sab ke liye $r=5$ — $H$ ka sign pattern likho aur confirm karo ki $\theta$ sahi quadrant mein land karta hai. **Forecast:** $H$ ki Row 1 hai $[\cos\theta,\ \sin\theta]$. Kis quadrant mein row 1 ki DONO entries negative hain? Scroll karne se pehle guess karo. 1. **Formula reuse karo, bas signs plug karo.** *Kyun?* $H$ sirf $x,y$ par depend karta hai; $r=5$ fixed hai. *Magnitudes* hamesha $0.6,0.8,0.16,0.12$ hi rahenge, sirf signs change hote hain. | 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. **`atan2` se bearing values.** *Kyun `atan2` na ki `arctan`?* Plain $\arctan(y/x)$ $(3,4)$ aur $(-3,-4)$ mein fark nahi bata sakta — dono ka ratio $4/3$ same hai. [[atan2 & Angle Wrapping|`atan2(y,x)`]] *dono* arguments ke signs use karta hai sahi quadrant pick karne ke liye. $$\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]$ sirf Q-III mein negative-negative hai, jo table se match karta hai. $\cos(126.87^\circ)=-0.6$ ✓. Charon bearings axes ke baare mein symmetric hain jaise expect tha. ![[deepdives/dd-physics-3.5.24-d3-s02.png]] > [!mistake] Q-III trap > $(-3,-4)$ par `arctan(y/x)` use karne se $+53.13^\circ$ return hota hai (Q-I ki taraf point karta hai!) kyunki ratio ke andar dono minus signs cancel ho jaate hain. Jacobian $H$ numerically sahi bhi rahega, lekin tumhara predicted bearing $h(\hat{\mathbf x})$ $180^\circ$ off hoga, jo innovation ko poison kar dega. $h$ hamesha `atan2` se banao. --- ## Example 3 — Degenerate input: target at the origin (Cell C) > [!example] Statement > Jab target radar ke paas aata hai, $r\to 0$, toh $H$ ka kya hota hai? Kya EKF measurement update wahan use ho sakta hai? **Forecast:** $H$ ki ek entry doosri se zyada tezi se blow up hoti hai. Kaun sa row explode hota hai — range ya bearing? Guess karo. 1. **$r$ ki powers dekho.** *Kyun?* Powers count karke singularity diagnose karo ki har entry kaise scale hoti hai. $$\text{range row}\sim \frac{1}{r}\ \text{(entries }x/r,y/r),\qquad \text{bearing row}\sim\frac{1}{r^2}.$$ 2. **$r\to0$ lo.** *Kyun?* Yeh limiting/degenerate case hai. Jab $r\to0$ toh dono rows diverge karte hain, aur bearing row *zyada tezi se* diverge karta hai ($1/r^2$ ki tarah). $$\lim_{r\to0}H=\text{undefined (unbounded)}.$$ 3. **Physical reason.** *Yeh sense kyun karta hai?* Radar *par* khade hoke, ek infinitesimal sideways nudge bearing ko bade angle se swing karta hai — sensitivity genuinely infinite hai. Bearing origin par undefined hai; `atan2(0,0)` ka koi answer nahi. **Verify:** Numerically, $(0.01,0)$ par: $r=0.01$, bearing sensitivity $x/r^2=0.01/0.0001=100$ rad per unit — already enormous, aur yeh $1/r^2$ ki tarah badhti hai. ✓ Unbounded confirm karta hai. > [!mistake] Singularity handle karna > $r=0$ ke paas bearing measurement par **bharosa mat karo** — ya toh bearing noise $R_{\theta\theta}$ ko infinity ki taraf inflate karo (taaki $K$ use ignore kare) ya us step ke liye bearing row ko bilkul drop kar do. Yeh ek real GNC failure mode hai jab koi tracked object directly overhead se guzarta hai. --- ## Example 4 — Limiting value: pendulum stiffness vanish ho jaati hai (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}$. $F$ ko $\theta=0$, $\theta=\pi/2$, aur $\theta=\pi$ par evaluate karo. Numbers ke liye $g/L=1$ lo. **Forecast:** Bottom-left entry $-\tfrac{g}{L}\cos\theta$ local "restoring stiffness" hai. Teen angles mein se kab yeh **vanish** hoti hai, matlab pendulum locally koi restoring pull feel nahi karta? 1. **Bottom, $\theta=0$.** *Kyun?* 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$.** *Kyun?* $\cos(\pi/2)=0$ — stiffness disappear ho jaati hai. $$F=\begin{bmatrix}0&1\\ 0 & 0\end{bmatrix}\quad\Rightarrow\quad \text{eigenvalues dono } 0,\ \text{koi restoring force nahi.}$$ 3. **Top, $\theta=\pi$ (inverted).** *Kyun?* $\cos\pi=-1$ sign flip karta hai — unstable. $$F=\begin{bmatrix}0&1\\ +1 & 0\end{bmatrix}\quad\Rightarrow\quad \text{eigenvalues } \pm 1,\ \text{ek badhta hai: divergence.}$$ **Verify:** $\begin{bmatrix}0&1\\ -k&0\end{bmatrix}$ ke eigenvalues $\pm\sqrt{-k}$ hain. $k=1$ ke liye: $\pm i$ (oscillate). $k=0$ ke liye: $0,0$ (drift). $k=-1$ ke liye: $\pm1$ (ek unstable) ✓. Isliye EKF neeche se swing karte pendulum ko khushi se track karta hai lekin inverted ek ko balance karne mein struggle karta hai — wahan linearization mein ek growing mode hai. ![[deepdives/dd-physics-3.5.24-d3-s03.png]] --- ## Example 5 — Linear collapse: EKF ordinary KF ban jaata hai (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$. $F$ compute karo aur dikkhao ki yeh state par depend nahi karta. **Forecast:** Agar $F$ mein koi $p$ ya $v$ nahi hai, toh kya tumhe abhi bhi *extended* filter ki zaroorat hai? 1. **Differentiate karo.** *Kyun?* $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. **State-dependence check karo.** *Kyun?* Yeh parent ka "steel-check" hai. $$\text{Koi }p\text{ ya }v\text{ nahi aata} \Rightarrow F \text{ constant hai.}$$ **Verify:** Constant $F$ ka matlab hai Taylor remainder $\mathcal{O}(\|\mathbf x-\hat{\mathbf x}\|^2)$ *exactly zero* hai (ek linear map apni tangent khud hi hoti hai). Isliye $\hat{\mathbf x}^-=F\hat{\mathbf x}$ aur $f(\hat{\mathbf x})$ perfectly agree karte hain — EKF aur [[Kalman Filter (linear)|linear KF]] identical results dete hain. Agar tumhara Jacobian constant hai, tumhe EKF ki kabhi zaroorat hi nahi thi. ✓ --- ## Example 6 — Ek poora EKF cycle numbers ke saath (Cell F) > [!example] Statement > $\mathbf{x}=[x,y]^\top$ ka 1D range-only tracking, lekin maano hum sirf range $r=\sqrt{x^2+y^2}$ (scalar) measure karte hain. Predicted state $\hat{\mathbf x}^-=(3,4)$, isliye $\hat r^-=5$. Covariance $P^-=\begin{bmatrix}1&0\\0&1\end{bmatrix}$, measurement noise $R=1$. Ek radar $z=5.5$ return karta hai. Ek update step karo: $H$, $S$, $K$, updated state, updated $P$ compute karo. **Forecast:** Sensor keh raha hai target thoda *door* hai prediction se ($5.5>5$). Kya updated state outward (bade $x,y$) ya inward move karegi? Direction guess karo. 1. **PREDICTED state par $h$ ko linearize karo.** *Kyun yahan?* Measurement $\hat{\mathbf x}^-$ ke against compare hoti hai, isliye $H$ ko $(3,4)$ par evaluate karna hoga — #1 EKF bug hai puraane point ko use karna. $$H=\begin{bmatrix}x/r & y/r\end{bmatrix}=\begin{bmatrix}0.6 & 0.8\end{bmatrix}.$$ 2. **Innovation (nonlinear $h$).** *Kyun nonlinear?* Residual true model use karta hai, $H$ nahi. $$y=z-h(\hat{\mathbf x}^-)=5.5-5=0.5.$$ 3. **Innovation covariance $S$.** *Kyun?* $S=HP^-H^\top+R$ — predicted measurement kitni uncertain hai. $$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.** *Kyun?* $K=P^-H^\top S^{-1}$ ek range residual ko state corrections par map karta hai. $$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. **Mean update karo.** *Kyun?* $\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. **Covariance update karo.** *Kyun?* $P=(I-KH)P^-$ — knowledge badhi, isliye spread shrink hona chahiye. $$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:** Correction *outward* move hui ($3\to3.15$, $4\to4.20$) jaise forecast tha, kyunki measured range predicted se zyada thi. Naya range $\sqrt{3.15^2+4.20^2}=5.25$, prediction $5$ aur measurement $5.5$ ke beech — bilkul wahi compromise jo Kalman blend dena chahiye ✓. $P$ ka trace $2$ se $1.5$ gira, isliye [[Covariance Propagation|uncertainty shrink hui]] ✓. --- ## Example 7 — Angle-wrap innovation trap (Cell G) > [!example] Statement > Predicted bearing $\hat\theta^-=1^\circ$, radar $z_\theta=359^\circ$ return karta hai. *Sahi* innovation compute karo. **Forecast:** Naive subtraction $358^\circ$ deta hai. Lekin target sirf ek choti si door hai. True angular error kya hai, aur uska sign kya hoga? 1. **Naive residual.** *Kyun dikhao?* Bug expose karne ke liye. $$y_{\text{naive}}=359^\circ-1^\circ=358^\circ.$$ 2. **$(-180^\circ,180^\circ]$ mein wrap karo.** *Kyun?* Angles periodic hote hain; physically jo matter karta hai woh hai shortest signed rotation. Range mein aane tak $360^\circ$ add/subtract karo. $$y=358^\circ-360^\circ=-2^\circ.$$ **Verify:** $359^\circ$ same heading hai $-1^\circ$ ke. $\hat\theta^-=1^\circ$ se $-1^\circ$ tak ek step of $-2^\circ$ hai ✓ — tiny, na ki $358^\circ$. $358^\circ$ ko $\hat{\mathbf x}=\hat{\mathbf x}^-+Ky$ mein feed karna estimate ko violently aur galat tarike se jerk kar deta. Wrapping mandatory hai jab bhi measurement mein koi [[atan2 & Angle Wrapping|angle]] aaye. --- ## Example 8 — Real-world word problem: altimeter over a hill (Cell H) > [!example] Statement > Ek drone horizontally height $y$ par fly kar raha hai, seedha neeche ground point ke upar, aur uske paas downward laser rangefinder hai. Flat ground par reading sirf $y$ hoti hai, lekin drone ek sloped hill ke upar hai jiska surface height $s(x)=0.1x$ hai (slope $10\%$). Laser $h(x,y)=y-0.1x$ (sloped ground ke upar clearance) measure karta hai. $H$ banao aur har entry ke sign ko interpret karo. **Forecast:** Rising ground ke upar $+x$ direction mein fly karna — measured clearance badhti hai ya ghatti hai? $\partial h/\partial x$ ka sign predict karo. 1. **$x$ ke w.r.t. partial.** *Kyun?* Ground $x$ ke saath rise karta hai, isliye clearance change honi chahiye. $$\frac{\partial h}{\partial x}=\frac{\partial}{\partial x}(y-0.1x)=-0.1.$$ 2. **$y$ ke w.r.t. partial.** *Kyun?* Directly climb karne se clearance one-for-one badhti hai. $$\frac{\partial h}{\partial y}=1.$$ 3. **Assemble karo.** $$H=\begin{bmatrix}-0.1 & 1\end{bmatrix}.$$ **Verify:** $\partial h/\partial x=-0.1<0$ ka sign: rising ground ke upar aage badh ne se clearance *reduce* hoti hai — intuition se match karta hai (hill tumhare paas aa rahi hai) ✓. Units: clearance ek length hai, $x,y$ bhi lengths hain, isliye $\partial h/\partial y=1$ dimensionless-per... ek pure ratio $1$ hai ✓; $\partial h/\partial x=-0.1$ hill ka negative slope hai ✓. Note karo $h$ linear hai, isliye yeh $H$ constant hai — ek rare case jahan ek "real world" model ko bhi re-linearization ki zaroorat nahi. --- ## Example 9 — Exam twist: kya covariance hamesha shrink hoti hai? (Cell I) > [!example] Statement > Scalar state, scalar measurement, $H=1$. Predicted variance $P^-=4$, measurement noise $R$. Algebraically dikhao ki $P=(1-KH)P^-$ ke baad updated variance hamesha $\le P^-$ hoti hai, aur ise $R=4$ aur limiting cases $R\to0$ aur $R\to\infty$ ke liye compute karo. **Forecast:** Ek perfect sensor ($R=0$) state ko exactly pin kar dena chahiye. Tab updated variance kya hogi — $0$, $2$, ya $4$? 1. **Gain likho.** *Kyun?* $K=P^-H^\top S^{-1}$ with $S=H P^- H^\top+R=P^-+R$. $$K=\frac{P^-}{P^-+R}.$$ 2. **Updated variance.** *Kyun?* $P=(1-K)P^-$. $$P=\left(1-\frac{P^-}{P^-+R}\right)P^-=\frac{R}{P^-+R}\,P^-.$$ 3. **Bound karo.** *Kyun?* Factor $\frac{R}{P^-+R}\in[0,1]$ for $R\ge0$, isliye $P\le P^-$ hamesha. 4. **Cases.** *Kyun?* Dono limits cover karo. $$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$ se $P=2$ milta hai, exactly half (prediction aur sensor mein equal trust) ✓. Perfect sensor $R\to0 \Rightarrow P\to0$: state pin ho gayi, forecast se match ✓. Useless sensor $R\to\infty \Rightarrow P\to4$: koi information gain nahi, variance unchanged ✓. Measurement update uncertainty kabhi *increase* nahi kar sakta. --- > [!recall]- Quick self-test (guess karne ke baad reveal karo) > - Kis quadrant mein $H$ ki range-row ki dono entries negative hoti hain? ::: Quadrant III (dono $x<0$ aur $y<0$). > - Example 6 mein $H$ kahan evaluate karna chahiye? ::: Predicted state $\hat{\mathbf x}^-=(3,4)$ par, puraane estimate par nahi. > - $\hat\theta^-=1^\circ$, $z=359^\circ$ ke liye true innovation? ::: Wrapping ke baad $-2^\circ$. > - $P^-=4$, $R=4$, $H=1$ ke liye updated variance? ::: $2$. > - Kis pendulum angle par local stiffness vanish hoti hai? ::: $\theta=\pi/2$ (horizontal), kyunki $\cos(\pi/2)=0$. > [!mnemonic] Scenario checklist > **"Sign, Singular, Slope, Same, Shrink, Spin."** — har **sign**/quadrant check karo, **singular** origin dekhna, **slope** limits padho, yaad rakho linear collapse karta hai same puraane KF mein, covariance zaroor **shrink** honi chahiye, aur **spin** (angle) hamesha wrap karo. Yeh bhi dekho: [[Unscented Kalman Filter (UKF)]] large-residual / far-initial-guess cases handle karta hai jahan yeh linearizations break ho jaate hain, aur [[State Estimation in GNC]] bigger picture ke liye.