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

$H$ kya hai: ek table jismein har output ke liye ek row aur har input ke liye ek column hota hai. Rows = outputs ($m=2$), columns = inputs ($n=3$). $H\text{ is }2\times 3.$ Maanga hua entry: "2nd output, 1st input" = row 2, column 1 = $H_{21}=\frac{\partial h_2}{\partial x_1}$. KYU: Jacobian ka kaam exactly yahi hai ki har "output-per-input" slope store kare. Uski shape padhna = (sensor channels ki sankhya) × (state variables ki sankhya) padhna hai.

True nonlinear $f$ se: ${\hat{\mathbf{x}}}_k^{-}=f({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1})$. KYU: kal ki state ka single best guess bas aaj ke best guess pe real motion law lagane se milta hai. Jacobian $F$ sirf spread (covariance) ko move karne ke liye use hota hai: $P_k^{-}=F{P}_{k-1}{F}^{\top }+Q$. Inhe confuse karna classic error hai — mean curve se jaata hai, uncertainty uski tangent se.

Step 1 — square root differentiate karo. Chain rule kyun? $r$ ek curve hai; hume uska slope har direction mein chahiye. $\frac{\partial r}{\partial x}=\frac{1}{2}(x^2+y^2{)}^{-1/2}\cdot 2x=\frac{x}{r},\frac{\partial r}{\partial y}=\frac{y}{r}.$ Step 2 — $(0,4)$ plug in karo: $r=\sqrt{0+16}=4$. $\frac{\partial r}{\partial x}=\frac{0}{4}=0,\frac{\partial r}{\partial y}=\frac{4}{4}=1.$ KAISA DIKHTA HAI (figure s01 dekho): target seedha $y$-axis ke upar baitha hai. Ise sideways slide karo ($x$ mein, plum arrow) toh origin se distance barely change hota hai — range curve us direction mein flat hai, isliye slope $0$ hai. Ise $y$ ke saath outward slide karo (orange arrow) toh range one-for-one change hoti hai, isliye slope $1$ hai. Yeh exactly line-of-sight angle ke $\cos$ aur $\sin$ hain ($90^{\circ }$ yahan).

Step 1 — partials. $\partial p_k/\partial p=1$, $\partial p_k/\partial v=\Delta t$; $\partial v_k/\partial p=0$, $\partial v_k/\partial v=1$. $F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}1& 0.5\\ 0& 1\end{array}\right].$ KYU yeh constant hai: $f$ yahan already ek straight-line (linear) map hai, isliye uska slope depend nahi karta tum kahan ho — tangent function ke barabar hi hai. EKF plain Kalman Filter (linear) mein collapse ho jaata hai.

Step 1 — range row (L2.1 se): $[x/r,y/r]$. Step 2 — bearing row. $\frac{d}{du}\arctan u=\frac{1}{1+u^2}$ kyun? Bearing ek angle hai; coordinates ke saath uski change rate chahiye. $\frac{\partial \theta }{\partial x}=\frac{-y}{x^2+y^2}=\frac{-y}{r^2},\frac{\partial \theta }{\partial y}=\frac{x}{r^2}.$ Step 3 — stack karo: $H=\left[\begin{array}{cc}x/r& y/r\\ -y/r^2& x/r^2\end{array}\right].$ Step 4 — $(3,4)$ par, $r=5$: $H=\left[\begin{array}{cc}0.6& 0.8\\ -0.16& 0.12\end{array}\right],\det H=(0.6)(0.12)-(0.8)(-0.16)=0.072+0.128=\mathrm{0.2.}$ Interpretation: general determinant hai $\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}=\mathrm{0.2.}$ Yeh $1/r$ ke barabar hai — $(x,y)$ se $(r,\theta )$ tak ka mapping area ko $1/r$ se stretch karta hai, isliye linearization har jagah well-conditioned hai sivaaye origin ke, jahan yeh blow up karta hai.

Jaise $r\to 0$: range row entries $x/r,y/r$ bounded rehti hain (woh angle ke $\cos ,\sin$ hain), lekin bearing row entries $-y/r^2,x/r^2$ blow up ho jaati hain $1/r$ ki tarah. Aur $\det H=1/r\to \infty$. PHYSICALLY KYU: bilkul sensor ke upar, ek infinitesimal sideways move bearing ko ek huge angle se swing kar deta hai — ek point par direction undefined hai. Tangent-line approximation (Taylor Series & Linearization) locally smooth function require karta hai; yahan $\theta$ singular hai, isliye EKF origin par invalid hai. Yeh ek degenerate case hai jisse tum zaroor guard karo.

Step 1 — general Jacobian. $F=\left[\begin{array}{cc}\partial \dot{\theta }/\partial \theta & \partial \dot{\theta }/\partial \omega \\ \partial \dot{\omega }/\partial \theta & \partial \dot{\omega }/\partial \omega \end{array}\right]=\left[\begin{array}{cc}0& 1\\ -\frac{g}{L}\cos \theta & 0\end{array}\right].$ $\cos \theta$ kyun? $\frac{d}{d\theta }\left(-\frac{g}{L}\sin \theta \right)=-\frac{g}{L}\cos \theta$. Step 2 — $\theta =0$ par: $\cos 0=1$, toh $F=\left[\begin{array}{cc}0& 1\\ -9.8& 0\end{array}\right].$ Bottom-left $-9.8$ restoring stiffness hai. Full strength ⇒ yeh ordinary small-angle oscillator hai. Step 3 — $\theta =\pi /2$ par: $\cos (\pi /2)=0$, toh $F=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right].$ Stiffness gaayab ho gayi. Horizontal pendulum: gravity us instant par koi restoring twist nahi deti. KAISA DIKHTA HAI (figure s02 dekho): restoring term $-\frac{g}{L}\sin \theta$ ek curve hai; uska slope (stiffness entry) $\theta =0$ par steepest hai (orange tangent) aur $\theta =\pm \pi /2$ par flat (plum tangent). Jacobian genuinely state ke saath change hota hai — yahi wajah hai ki hum har step mein re-linearize karte hain.

Mean (nonlinear $f$): ${\hat{\mathbf{x}}}_k^{-}=\left[\begin{array}{c}\theta +\Delta t\omega \\ \omega +\Delta t(-\frac{g}{L}\sin \theta )\end{array}\right]=\left[\begin{array}{c}0.2+0.1\cdot 0\\ 0+0.1\cdot (-9.8\sin 0.2)\end{array}\right].$ $\sin 0.2=0.19867$, toh $\omega$ entry $=0.1\cdot (-9.8\cdot 0.19867)=-0.19470$. ${\hat{\mathbf{x}}}_k^{-}=\left[\begin{array}{c}0.2\\ -0.19470\end{array}\right].$ Covariance — pehle, KYU discrete Jacobian $F_d=I+\Delta tF$ hai. Covariance ko discrete one-step map ${\mathbf{x}}_k=g_d({\mathbf{x}}_{k-1})={\mathbf{x}}_{k-1}+\Delta tf({\mathbf{x}}_{k-1})$ ke slope se transport karna hoga. Iska Jacobian term-by-term lo: ${\mathbf{x}}_{k-1}$ ka khud se derivative identity $I$ hai; $\Delta tf({\mathbf{x}}_{k-1})$ ka derivative $\Delta t$ times $f$ ka khud ka Jacobian $F$ hai. Dono add karo: $F_d=\frac{\partial g_d}{\partial \mathbf{x}}=\underset{\text{from }{\mathbf{x}}_{k-1}}{\underbrace{I}}+\Delta t\underset{\text{from }\Delta tf}{\underbrace{F}}.$ Yeh koi memorized formula nahi hai — yeh bas "Euler step differentiate karo" hai. (Yeh flow ka $\Delta t$ par first-order Taylor Series & Linearization hai.) Ab $F_d$ banao L4.1 se $F$ use karke $\theta =0.2$ par: $\cos 0.2=0.98007$, toh $-\frac{g}{L}\cos \theta =-9.60464$. $F=\left[\begin{array}{cc}0& 1\\ -9.60464& 0\end{array}\right],F_d=I+0.1F=\left[\begin{array}{cc}1& 0.1\\ -0.960464& 1\end{array}\right].$ $P_{k-1}=I$, $Q=0$ ke saath: $P_k^{-}=F_{d}I{F}_d^{\top }=F_d{F}_d^{\top }.$ Har entry carefully compute karo (row $i$ of $F_d$ dot 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^{-}=\left[\begin{array}{cc}1.01& -0.860464\\ -0.860464& 1.922491\end{array}\right].$ KYU split: mean ne real curve $f$ pe ride ki; uncertainty $P$ ne tangent $F_d$ pe ride ki via $F_{d}P{F}_d^{\top }$. Covariance ek second-moment object hai aur "$f(P)$" jaisi koi meaningful cheez nahi hai.

Step 1 — predicted measurement. $h({\hat{\mathbf{x}}}^{-})=[5,\text{atan2}(4,3){]}^{\top }$, aur $\text{atan2}(4,3)=0.927295$ rad. Toh predicted bearing ${\theta }_{\text{pred}}=0.927295$ hai. Step 2 — sensor ne numerically kya bheja. True bearing error ek chhota $-0.05$ rad hai. Lekin angle sensor values modulo $2\pi$ report karta hai, aur is baar usne apni reading ko $2\pi$ ke paas wrap kar liya: usne report kiya ${\theta }_z=(0.927295-0.05)+2\pi \text{(wrapped high)}=0.877295+6.283185=7.160480\text{rad}.$ Agar tum naively subtract karo, toh raw residual hai ${\theta }_z-{\theta }_{\text{pred}}=7.160480-0.927295=6.233185=2\pi -\mathrm{0.05.}$ Yeh ek bahut bada $+6.23$ rad error jaisa lagta hai — lekin $6.23$ rad ka turn almost ek full circle hai, yaani essentially same direction. Real error tiny hai. Step 3 — bearing innovation ko $(-\pi ,\pi ]$ mein wrap karo. Kyun? Do angles jo ek full turn se different hain woh same direction point karte hain; meaningful residual $(-\pi ,\pi ]$ mein rehni chahiye. $2\pi$ subtract karo: $6.233185-2\pi =-0.05$ rad. atan2 & Angle Wrapping dekho. $\mathbf{y}=\left[\begin{array}{c}{z}_r-r_{\text{pred}}\\ \text{wrap}({\theta }_z-{\theta }_{\text{pred}})\end{array}\right]=\left[\begin{array}{c}5.5-5\\ -0.05\end{array}\right]=\left[\begin{array}{c}0.5\\ -0.05\end{array}\right].$ Step 4 — $(3,4)$ par Jacobian (L3.1 se): $H=\left[\begin{array}{cc}0.6& 0.8\\ -0.16& 0.12\end{array}\right]$. Step 5 — innovation covariance $S=H{P}^{-}{H}^{\top }+R$. $P^{-}=I$ ke saath, $H{P}^{-}{H}^{\top }=H{H}^{\top }$: $H{H}^{\top }=\left[\begin{array}{cc}0.6^2+0.8^2& 0.6(-0.16)+0.8(0.12)\\ (-0.16)(0.6)+(0.12)(0.8)& 0.16^2+0.12^2\end{array}\right]=\left[\begin{array}{cc}1.0& 0\\ 0& 0.04\end{array}\right].$ KYU cross terms vanish hote hain (yeh skip mat karo): off-diagonal $H{H}_{12}^{\top }$ $H$ ki do rows ka dot product hai — range gradient $[0.6,0.8]$ aur bearing gradient $[-0.16,0.12]$. Unka dot product hai $0.6(-0.16)+0.8(0.12)=-0.096+0.096=0$: do rows orthogonal hain. Yeh luck nahi hai — range radially badhti hai jabki bearing tangentially badhti hai, aur radial aur tangential directions hamesha perpendicular hote hain. Toh $P^{-}=I$ ke saath mapped innovation covariance exactly diagonal hai is operating point par. Generally (koi bhi $P^{-}\ne I$, ya alag $x,y$) yeh diagonal nahi hogi. $S=\left[\begin{array}{cc}1.0+0.25& 0\\ 0& 0.04+0.01\end{array}\right]=\left[\begin{array}{cc}1.25& 0\\ 0& 0.05\end{array}\right].$ Step 6 — $S$ invert karo aur gain $K=P^{-}{H}^{\top }{S}^{-1}=H^{\top }{S}^{-1}$ form karo (kyunki $P^{-}=I$). Yahan $S$ diagonal hai, isliye $S^{-1}$ bas diagonal ko reciprocate karta hai: $H^{\top }=\left[\begin{array}{cc}0.6& -0.16\\ 0.8& 0.12\end{array}\right],S^{-1}=\left[\begin{array}{cc}1/1.25& 0\\ 0& 1/0.05\end{array}\right]=\left[\begin{array}{cc}0.8& 0\\ 0& 20\end{array}\right].$ $K=H^{\top }{S}^{-1}=\left[\begin{array}{cc}0.6\cdot 0.8& (-0.16)\cdot 20\\ 0.8\cdot 0.8& 0.12\cdot 20\end{array}\right]=\left[\begin{array}{cc}0.48& -3.2\\ 0.64& 2.4\end{array}\right].$ Step 7 — posterior state ${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K\mathbf{y}$: $K\mathbf{y}=\left[\begin{array}{c}0.48\cdot 0.5+(-3.2)(-0.05)\\ 0.64\cdot 0.5+2.4\cdot (-0.05)\end{array}\right]=\left[\begin{array}{c}0.24+0.16\\ 0.32-0.12\end{array}\right]=\left[\begin{array}{c}0.4\\ 0.2\end{array}\right].$ ${\hat{\mathbf{x}}}_k=\left[\begin{array}{c}3\\ 4\end{array}\right]+\left[\begin{array}{c}0.4\\ 0.2\end{array}\right]=\left[\begin{array}{c}3.4\\ 4.2\end{array}\right].$ Step 8 — posterior covariance $P_k=(I-KH)P_k^{-}$ (update complete karta hai): $KH=\left[\begin{array}{cc}0.48& -3.2\\ 0.64& 2.4\end{array}\right]\left[\begin{array}{cc}0.6& 0.8\\ -0.16& 0.12\end{array}\right]=\left[\begin{array}{cc}0.48(0.6)+(-3.2)(-0.16)& 0.48(0.8)+(-3.2)(0.12)\\ 0.64(0.6)+2.4(-0.16)& 0.64(0.8)+2.4(0.12)\end{array}\right]=\left[\begin{array}{cc}0.8& 0\\ 0& 0.8\end{array}\right].$ $P_k=(I-KH)I=\left[\begin{array}{cc}1-0.8& 0\\ 0& 1-0.8\end{array}\right]=\left[\begin{array}{cc}0.2& 0\\ 0& 0.2\end{array}\right].$ Uncertainty $1.0$ se shrink hokar $0.2$ ho gayi har axis par — measurement ne genuinely estimate ko sharpen kiya. Interpretation: range surprise ($+0.5$) estimate ko outward push karta hai; (chhota, correctly-wrapped) bearing surprise ise thoda nudge karta hai. Agar hum wrap nahi karte, toh bearing innovation $\approx 6.23$ estimate ko catastrophically throw kar deta — yahi wajah hai ki wrapping mandatory hai, cosmetic nahi.

Conditioning guard (general case). $S$ clean aur diagonal sirf isliye nikla kyunki $P^{-}=I$ tha aur $H$ ki rows yahan orthogonal thi. Real filter mein invert karne se pehle hamesha $S$ check karo: agar $\det S$ near zero hai (e.g. chhota range bearing row huge bana deta hai, ya do channels almost redundant ho jaate hain), toh $S^{-1}$ noise amplify karta hai aur $K$ explode karta hai. Fix: $S^{-1}$ explicitly form karne ki jagah $KS=P^{-}{H}^{\top }$ ko stable linear solve (Cholesky) se solve karo, $R$ mein chhota jitter $\epsilon I$ add karo regularize karne ke liye, aur har step mein $\det S$ / condition number monitor karo. Covariance Propagation dekho.

Condition A — singular linearization (L3.2): $r=0$ ke paas bearing Jacobian blow up ho jaata hai ($1/r^2$) aur Taylor ke neglected $\mathcal{O}(\Vert \cdot {\Vert }^2)$ terms dominate karne lagte hain. Fix: Unscented Kalman Filter (UKF) par switch karo (sample points use karta hai, koi Jacobian nahi) ya bearing channel skip karo. Condition B — large linearization error / bad initial guess: tangent sirf $\hat{\mathbf{x}}$ ke paas valid hai (parent-note mistake #5). Ek door ka seed ⇒ galat $F,H$ ⇒ divergence. Fix: ek achha ${\hat{\mathbf{x}}}_0$ seed karo, $P_0$ inflate karo, ya UKF use karo. Plain Kalman Filter (linear) se compare karo, jise yeh problem kabhi nahi hoti kyunki uska map exactly linear hai. KYU yeh same root cause hai: dono "curve ≈ line locally" ki failures hain. Jab tumhari uncertainty region par curvature chhoti nahi hai, first-order Taylor Series & Linearization simply kaafi accurate nahi hai.

Ek 12-saal ke bacche ko explain karo kyun EKF guess ko real curved rule se push karta hai lekin uncertainty ko us rule ki flat straight-line copy se push karta hai ::: Kyunki tumhara single best guess real physics follow karna chahiye, lekin "tum kitne unsure ho" woh guess ke around ek chhota cloud hai — aur dekhne ke liye ki ek chhota cloud kaise stretch hota hai, tumhe sirf rule ki local steepness (slope) chahiye, jo flat tangent = Jacobian hai.