Exercises — Unscented Kalman Filter (UKF) — sigma points, better for nonlinear

WHAT: $x$ ki entries count karo. Wahan $4$ numbers hain, isliye $n=4$. WHY: parent deta hai ek center point plus har principal axis ke dono taraf ek-ek point, yaani $2n+1$. $2n+1=2(4)+1===9==$ Answer: 9 sigma points.

• (a) $W_0^{(m)}$ = center mean weight.
• (b) $\lambda$ = scaling parameter.
• (c) $W_0^{(c)}$ = center covariance weight ($W_0^{(m)}$ plus Gaussian-tuning term $(1-{\alpha }^2+\beta )$). WHY it matters: (a) aur (c) mein fark sirf center pe hota hai. Yahi woh poora trick hai jo hume covariance tune karne deta hai mean shift kiye bina.

Step 1 — $\lambda$ (WHY: yeh fix karta hai ki points kitne door/weighted hain): $\lambda ={\alpha }^2(n+\kappa )-n=1\cdot (2+1)-2===1==$ Toh $n+\lambda =2+1=3$. Step 2 — center weights: $W_0^{(m)}=\frac{\lambda }{n+\lambda }=\frac{1}{3}$ $W_0^{(c)}=\frac{1}{3}+(1-{\alpha }^2+\beta )=\frac{1}{3}+(1-1+2)=\frac{1}{3}+2=\frac{7}{3}$ Step 3 — outer weights ($2n=4$ hain): $W_i^{(m)}=W_i^{(c)}=\frac{1}{2(n+\lambda )}=\frac{1}{6}$ Sanity check (mean weights ka sum 1 hona chahiye): $\frac{1}{3}+4\cdot \frac{1}{6}=\frac{1}{3}+\frac{2}{3}=1$. ✔

WHAT: humein spread distance $\sqrt{(n+\lambda )P}$ chahiye. WHY square root: covariance spread² (variance) measure karta hai; use axis ke saath actual distance mein convert karne ke liye hum square root lete hain — wahi reason jis wajah se standard deviation $\sqrt{\text{variance}}$ hota hai. $\sqrt{(n+\lambda )P}=\sqrt{3\cdot 4}=\sqrt{12}=2\sqrt{3}\approx 3.464$ ${\mathcal{X}}_0=5,{\mathcal{X}}_1=5+2\sqrt{3}\approx 8.464,{\mathcal{X}}_2=5-2\sqrt{3}\approx 1.536$ Figure s01 dekho: red center point $5$ pe baitha hai, aur do black points symmetrically uske dono taraf hain.

(a) UT. Sigma points: ${\mathcal{X}}_0=0,{\mathcal{X}}_{1,2}=\pm \sqrt{3}\sigma$. Weights $W_0=\frac{2}{3},W_{1,2}=\frac{1}{6}$. Har ek transform karo: ${\mathcal{Y}}_0=0^2=0,{\mathcal{Y}}_{1,2}=(\sqrt{3}\sigma {)}^2=3{\sigma }^2$. ${\bar{y}}_{\text{UT}}=\frac{2}{3}(0)+\frac{1}{6}(3{\sigma }^2)+\frac{1}{6}(3{\sigma }^2)={\sigma }^2$ (b) EKF. Linearize karo: $F={\frac{d}{dx}{x}^2|}_0=2x{|}_0=0$. Toh ${\bar{y}}_{\text{EKF}}=f(0)+F\cdot 0=0$. (c) Sahi mean ${\sigma }^2$ hai. UT bilkul exact hai; EKF completely galat hai (0 predict karta hai). WHY: $x=0$ pe $x^2$ ki slope flat hai, isliye EKF ka linear model "koi change nahin dikh raha" — woh curvature ke liye andha hai. UT, $f$ ko mean se door points pe sample karta hai, isliye woh upar ki taraf bend feel karta hai aur ${\sigma }^2$ lift recover karta hai. Figure s02 dekho: kali parabola, red sigma points curve pe land kar rahe hain flat tangent (dashed) se kaafi upar.

(a) $\lambda ={\alpha }^2(n+\kappa )-n=(10^{-6})(1)-1=10^{-6}-1$. Tab $n+\lambda =1+(10^{-6}-1)=10^{-6}$. (b) Distance $=\sqrt{(n+\lambda )P}=\sqrt{10^{-6}}\sigma =10^{-3}\sigma$. Points $\pm 10^{-3}\sigma$ pe — mean ke bahut karib. (c) Covariance weight $W_i^{(c)}=\frac{1}{2(n+\lambda )}=\frac{1}{2\cdot 10^{-6}}=5\times 10^5$ bahut bada hai, exactly chhoti squared distances ko cancel karta hai taaki $P_y$ sahi se reconstruct ho. Spread ↓, weight ↑ — yeh design se trade off karte hain.

Step 1 — sigma points. Spread $=\sqrt{3\cdot 4}=\sqrt{12}=2\sqrt{3}$. ${\mathcal{X}}_0=1,{\mathcal{X}}_1=1+2\sqrt{3},{\mathcal{X}}_2=1-2\sqrt{3}$ Numerically: ${\mathcal{X}}_1\approx 4.4641$, ${\mathcal{X}}_2\approx -2.4641$. Step 2 — $f$ se push through karo.

• ${\mathcal{X}}_0^{-}=f(1)=1+0.5=1.5$
• ${\mathcal{X}}_1^{-}=f(4.4641)=4.4641+0.5(4.4641{)}^2=4.4641+9.9641=14.4282$
• ${\mathcal{X}}_2^{-}=f(-2.4641)=-2.4641+0.5(-2.4641{)}^2=-2.4641+3.0359=0.5718$ Step 3 — predicted mean (weights $W_0=\frac{2}{3}$, $W_{1,2}=\frac{1}{6}$): ${\hat{x}}_k^{-}=\frac{2}{3}(1.5)+\frac{1}{6}(14.4282)+\frac{1}{6}(0.5718)=1.0+2.5=3.5$ Step 4 — predicted covariance ($Q$ add karo! yeh classic trap hai): $P_k^{-}={\sum }_i{W}_i^{(c)}({\mathcal{X}}_i^{-}-{\hat{x}}_k^{-}{)}^2+Q$ Deviations: $1.5-3.5=-2$; $14.4282-3.5=10.9282$; $0.5718-3.5=-2.9282$. $=\frac{2}{3}(4)+\frac{1}{6}(119.4256)+\frac{1}{6}(8.5744)+1$ $=2.6667+19.9043+1.4291+1=25.0000$ Answer: ${\hat{x}}_k^{-}=3.5$, $P_k^{-}=25$. (Gaur karo ki covariance 4 se 25 tak balloon ho gayi — $f$ ki curvature ne uncertainty ko stretch kar diya, aur UT ne ise capture kar liya.)

Step 1 — naye sigma points. Spread $=\sqrt{3\cdot 25}=\sqrt{75}=5\sqrt{3}\approx 8.6603$. ${\mathcal{X}}_0^{-}=3.5,{\mathcal{X}}_1^{-}=3.5+5\sqrt{3}\approx 12.1603,{\mathcal{X}}_2^{-}=3.5-5\sqrt{3}\approx -5.1603$ Step 2 — $h(x)=2x$ se push through karo: ${\mathcal{Z}}_0=7,{\mathcal{Z}}_1=24.3205,{\mathcal{Z}}_2=-10.3205$ Step 3 — predicted measurement mean: ${\hat{z}}_k=\frac{2}{3}(7)+\frac{1}{6}(24.3205)+\frac{1}{6}(-10.3205)=4.6667+2.3333=7.0$ (Sahi hi hai: $h$ linear hai, isliye ${\hat{z}}_k=2{\hat{x}}_k^{-}=7$.) Step 4 — measurement covariance ($R$ add karo!): Deviations ${\mathcal{Z}}_i-{\hat{z}}_k$: $0$, $17.3205$, $-17.3205$. $P_{zz}=\frac{2}{3}(0)+\frac{1}{6}(300)+\frac{1}{6}(300)+R=100+1=101$ Step 5 — cross-covariance (yahan $R$ nahin): State deviations ${\mathcal{X}}_i^{-}-{\hat{x}}_k^{-}$: $0$, $8.6603$, $-8.6603$. $P_{xz}=\frac{1}{6}(8.6603)(17.3205)+\frac{1}{6}(-8.6603)(-17.3205)=25+25=50$ Step 6 — Kalman gain: $K=P_{xz}{P}_{zz}^{-1}=\frac{50}{101}\approx 0.49505$ Step 7 — corrected state: ${\hat{x}}_k={\hat{x}}_k^{-}+K(z_k-{\hat{z}}_k)=3.5+0.49505(6-7)=3.5-0.49505\approx 3.00495$ Step 8 — corrected covariance: $P_k=P_k^{-}-K{P}_{zz}K=25-0.49505^2(101)=25-24.7525\approx 0.24752$ Answers: $K\approx 0.4950$, ${\hat{x}}_k\approx 3.005$, $P_k\approx 0.2475$. WHY covariance itni zyada shrink hui: measurement bahut informative thi ($h=2x$ directly state ko scale karta hai), isliye update ne estimate ko $z/2=3$ ki taraf kheencha aur uncertainty 25 se ~0.25 tak ghata di.

EKF: origin pe $\frac{x^{\top }}{\Vert x\Vert }=\frac{0}{0}$ — undefined. Linearization simply exist nahin karta, isliye EKF ke paas compute karne ke liye koi gain nahin hai. UKF: $P_{xz}$ products $({\mathcal{X}}_i-0)\cdot (\Vert {\mathcal{X}}_i\Vert -\bar{z})$ ki ek finite weighted sum hai. Har term un numbers ka plain multiplication hai jo hum evaluate kar sakte hain — koi derivative nahin, koi zero se division nahin. Sigma points origin se door baithte hain, isliye $\Vert {\mathcal{X}}_i\Vert \ne 0$ aur sum well-defined hai. The lesson: UKF kabhi nahin poochhta "yahan slope kya hai?" Woh poochhta hai "output, samples ke ek spread ke across input ke saath co-vary kaise karta hai?" — ek aisa sawaal jiska jawab tab bhi hai jab slope ka nahin hota.