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

3.5.25 · D3 · Physics › Guidance, Navigation & Control (GNC) › Unscented Kalman Filter (UKF) — sigma points, better for non

Yeh ek Deep Dive child note hai Unscented Kalman Filter (UKF) ka. Parent note ne machinery banayi thi — sigma points, weights, transform. Yahan hum use stress-test karte hain har us situation ke against jo ek real filter face karta hai: gentle curvature, brutal curvature, woh exact quadratic jo EKF ko fool karta hai, ek degenerate covariance, ek full predict step with noise, aur ek non-differentiable measurement jahan EKF ka Jacobian literally exist hi nahi karta.

Shuru karne se pehle, ek reminder plain words mein. Ek sigma point ek hand-picked sample hota hai state ka. Ek weight batata hai ki hum us sample par kitna trust karte hain jab hum mean ya spread rebuild karte hain. Hum har sample ko true nonlinear function se push karte hain aur statistics padhte hain. Koi derivatives nahi. Kabhi bhi.

Weight formulas, ek baar dobara likhi gayi taaki kuch unexplained na lage

Neeche ke har example mein same do weight sets use hoti hain jo parent note se hain. Kyunki ek reviewer ne (sahi) flag kiya ki covariance-weight correction baar baar bina explanation ke aa raha tha, isliye yahan poora rulebook ek jagah hai, plain words mein, pehle kisi bhi example se pehle.

Neeche do everyday choices dikhengi:

• $\alpha =1,\beta =0$: tab $1-{\alpha }^2+\beta =0$, isliye $W_0^{(c)}=W_0^{(m)}$ — mean aur covariance weights same hain. Examples 1–7, 9, 10 mein clean arithmetic ke liye use hota hai.
• $\alpha \ll 1$ (e.g. $10^{-3}$), $\beta =2$: Example 8 ka scaled UT, jahan correction genuinely matter karta hai.

Poore note mein, jab tak kaha na jaye, hum $n$ = state dimension aur $c=n+\lambda ={\alpha }^2(n+\kappa )$ use karte hain.

Scenario matrix

Har UKF question in cells mein se kisi ek mein aata hai. Neeche ke examples labeled hain un cells ke saath jo woh cover karte hain, taaki end mein koi cell empty na rahe.

Example 1 — Cell A: linear sanity check

1. Sigma points. Matrix square root: $\sqrt{(n+\lambda )P}=\sqrt{3\cdot 4}=\sqrt{12}=2\sqrt{3}$. ${\mathcal{X}}_0=2,{\mathcal{X}}_1=2+2\sqrt{3},{\mathcal{X}}_2=2-2\sqrt{3}.$ Yeh step kyun? Teeno points ko mean $2$ aur variance $4$ reproduce karna chahiye; woh $2$ ke around symmetric hain isliye mean untouched rehta hai aur unka spread $P$ carry karta hai.

2. Weights. $W_0^{(m)}=\lambda /c=2/3$, aur $W_1=W_2=\frac{1}{2c}=1/6$. $\alpha =1,\beta =0$ ke saath correction $1-{\alpha }^2+\beta =0$ hai, to $W_0^{(c)}=2/3+0=2/3$. Yeh step kyun? $\alpha =1$ $W^{(m)}=W^{(c)}$ banata hai — koi scaled-UT correction nahi, isliye plain Kalman se cleanly compare kar sakte hain.

3. Transform. ${\mathcal{Y}}_0=7,{\mathcal{Y}}_1=7+6\sqrt{3},{\mathcal{Y}}_2=7-6\sqrt{3}.$ Yeh step kyun? Hum actual $f$ apply karte hain, linearization nahi — lekin $f$ hai linear yahan, isliye yeh exact map hai.

4. Mean recover karo (use $W^{(m)}$). $\bar{y}=\frac{2}{3}(7)+\frac{1}{6}(7+6\sqrt{3})+\frac{1}{6}(7-6\sqrt{3})=7.$ Yeh step kyun? $\pm 6\sqrt{3}$ terms cancel ho jaate hain; symmetric points ek symmetric mean dete hain.

5. Covariance recover karo (use $W^{(c)}$). $7$ se deviations: $0,+6\sqrt{3},-6\sqrt{3}$. $P_y=\frac{1}{6}(6\sqrt{3}{)}^2+\frac{1}{6}(6\sqrt{3}{)}^2=\frac{1}{6}(108)+\frac{1}{6}(108)=36.$ Yeh step kyun? Covariance squared deviations ka weighted mean hai; centre point yahan kuch contribute nahi karta kyunki uski deviation $0$ hai.

Example 2 — Cell B: woh quadratic jo EKF ko blind kar deta hai

1. Sigma points. $\sqrt{3{\sigma }^2}=\sqrt{3}\sigma$, to $\mathcal{X}=\{0,\sqrt{3}\sigma ,-\sqrt{3}\sigma \}$. Yeh step kyun? Hum linearize karne se mana karte hain; do outer points $\pm \sqrt{c}\sigma$ par exactly isliye rakhe jaate hain taaki unka weighted spread true variance ${\sigma }^2$ reproduce kare — hum $f$ ko wahan sample karte hain jahan uncertainty actually hai.

2. Transform. $\mathcal{Y}=\{0,3{\sigma }^2,3{\sigma }^2\}$. Yeh step kyun? Dono outer points ko square karne se same positive value aati hai — curvature ek upward shift ke roop mein dikhayi deti hai jo EKF ka zero slope kabhi nahi dekh pata.

3. Mean (use $W^{(m)}$). $\bar{y}=\frac{2}{3}(0)+\frac{1}{6}(3{\sigma }^2)+\frac{1}{6}(3{\sigma }^2)={\sigma }^2$. Yeh step kyun? Bhaले hi centre point $0$ par map hota hai, do lifted outer points weighted average ko ${\sigma }^2$ tak upar kheench lete hain — yahi linearize na karne ka poora point hai.

Example 3 — Cell C: nonzero mean, curvature + offset

1. Sigma points. $\sqrt{3\cdot 1}=\sqrt{3}$, to $\mathcal{X}=\{1,1+\sqrt{3},1-\sqrt{3}\}$. Yeh step kyun? Points actual mean $1$ par centred hain (na ki $0$ par) aur $\pm \sqrt{c}\cdot \text{std}=\pm \sqrt{3}$ ke steps mein nikle hain taaki unka spread $P=1$ ke barabar ho — $x$ ki mean aur covariance dono $f$ ko touch karne se pehle reproduce ho jaate hain.
2. Transform. $\mathcal{Y}=\{1,(1+\sqrt{3}{)}^2,(1-\sqrt{3}{)}^2\}=\{1,4+2\sqrt{3},4-2\sqrt{3}\}$. Yeh step kyun? Har shifted sample par true $f$ — cross-term $2\sqrt{3}$ exactly woh curvature information hai jo EKF discard karta hai.
3. Mean (use $W^{(m)}$). $\bar{y}=\frac{2}{3}(1)+\frac{1}{6}(4+2\sqrt{3})+\frac{1}{6}(4-2\sqrt{3})=\frac{2}{3}+\frac{1}{6}(8)=\frac{2}{3}+\frac{4}{3}=2.$ Yeh step kyun? Average mein $\pm 2\sqrt{3}$ terms cancel ho jaate hain; jo bachta hai woh curvature bias hai jo mean ko $h(\bar{x})=1$ se upar true $2$ tak lift karta hai.
4. Covariance (use $W^{(c)}$). $2$ se deviations: $\{-1,2+2\sqrt{3},2-2\sqrt{3}\}$. $P_y=\frac{2}{3}(1)+\frac{1}{6}(2+2\sqrt{3}{)}^2+\frac{1}{6}(2-2\sqrt{3}{)}^2.$ $(2\pm 2\sqrt{3}{)}^2=4\pm 8\sqrt{3}+12=16\pm 8\sqrt{3}$; unka sum hai $32$. $P_y=\frac{2}{3}+\frac{1}{6}(32)=\frac{2}{3}+\frac{16}{3}=\frac{18}{3}=6.$ Yeh step kyun? Deviations ko square karke weight-average karna variance ki definition hai; yahan $\beta =0$ hai isliye $W_0^{(c)}=W_0^{(m)}=\frac{2}{3}$, aur centre deviation $(-1)$ contribute karta hai.

Example 4 — Cell D: 2D covariance, principal axes ke saath spread (figure)

1. Matrix square root. Kyunki $P$ diagonal hai, $\sqrt{P}=\left(\begin{array}{cc}2& 0\\ 0& 1\end{array}\right)$ (yeh Cholesky factor hai). Scale: $\sqrt{(n+\lambda )P}=\sqrt{2}\cdot \sqrt{P}$, columns $(2\sqrt{2},0)$ aur $(0,\sqrt{2})$. Yeh step kyun? Scaled square root ke columns principal directions hain — uncertainty ellipse (Figure 1 mein lavender curve) ki har axis ke saath kitna step karna hai.
2. Sigma points. ${\mathcal{X}}_0=(0,0)$ (coral dot); ${\mathcal{X}}_{1,3}=(\pm 2\sqrt{2},0)$ aur ${\mathcal{X}}_{2,4}=(0,\pm \sqrt{2})$ (chaar mint dots). Yeh step kyun? Figure 1 mein do measuring arrows compare karo: wide axis (variance $4$) points ko $\pm 2.83$ tak push karta hai, narrow axis (variance $1$) sirf $\pm 1.41$ tak — point cloud exactly ellipse ki tarah stretched hai, isliye woh sahi shape carry karta hai.
3. Weights. $W_0^{(m)}=\lambda /c=0/2=0$; $W_i=1/(2\cdot 2)=1/4$ chaar outer points ke liye. Yeh step kyun? $\lambda =0$ ke saath centre zero mean-weight carry karta hai — sirf chaar symmetric points hi mean rebuild karte hain (isliye Figure 1 mein coral dot kuch bias nahi karta).

Example 5 — Cell E: ek full predict step, $Q$ mat bhoolo

1. Sigma points. $\sqrt{3}\cdot 1=\sqrt{3}$: $\mathcal{X}=\{0,\sqrt{3},-\sqrt{3}\}$. Yeh step kyun? Outer points $\pm \sqrt{c}\cdot \text{std}=\pm \sqrt{3}$ par prior variance $P=1$ reproduce karte hain; hum prior uncertainty propagate karte hain, abhi kuch aur nahi.
2. Propagate. $f(x)=x+0.5x^2$: $f(0)=0$, $f(\sqrt{3})=\sqrt{3}+1.5$, $f(-\sqrt{3})=-\sqrt{3}+1.5$. Yeh step kyun? Har point par true dynamics — $0.5x^2$ dono outer points ko $1.5$ se upar bump karta hai, jo woh curvature hai jo EKF miss kar deta.
3. Predicted mean (use $W^{(m)}$). ${\hat{x}}_k^{-}=\frac{2}{3}(0)+\frac{1}{6}(\sqrt{3}+1.5)+\frac{1}{6}(-\sqrt{3}+1.5)=\frac{1}{6}(3)=\mathrm{0.5.}$ Yeh step kyun? $\pm \sqrt{3}$ cancel ho jaate hain; har outer point par surviving $+1.5$, weighted hoke, mean ko $0.5$ tak drift karta hai — yeh curvature ne produce kiya, koi input mean ne nahi.
4. Predicted covariance before $Q$ (use $W^{(c)}$). $0.5$ se deviations: $\{-0.5,\sqrt{3}+1,-\sqrt{3}+1\}$. $(\sqrt{3}+1{)}^2=4+2\sqrt{3}$, $(-\sqrt{3}+1{)}^2=4-2\sqrt{3}$, sum $=8$. $P_{\text{prop}}=\frac{2}{3}(0.25)+\frac{1}{6}(8)=\frac{1}{6}+\frac{8}{6}=\frac{9}{6}=\mathrm{1.5.}$ Yeh step kyun? Same variance recipe pehle ki tarah ($\beta =0$ to $W_0^{(c)}=\frac{2}{3}$); yeh measure karta hai ki propagated points kitne spread hain — lekin yeh sirf prior uncertainty hai jo forward carry hui hai.
5. Process noise add karo. $P_k^{-}=P_{\text{prop}}+Q=1.5+0.2=\mathrm{1.7.}$ Yeh step kyun? Sigma points sirf prior uncertainty carry karte hain; $Q$ woh nai uncertainty hai jo duniya har step mein inject karti hai. Ise bhoolo aur filter overconfident ho jaayega aur diverge karega.

Example 6 — Cell F: non-differentiable measurement $h(x)=\Vert x\Vert$ (figure)

1. Sigma points. $\sqrt{(n+\lambda )P}=\sqrt{2}I$: ${\mathcal{X}}_0=(0,0)$, ${\mathcal{X}}_{1,3}=(\pm \sqrt{2},0)$, ${\mathcal{X}}_{2,4}=(0,\pm \sqrt{2})$. Yeh step kyun? $c=2$ hai, std $=1$ har direction mein, isliye outer points radius $\sqrt{2}$ par land karte hain — exactly Figure 2 mein butter circle par mint dots.
2. $h$ se push karo. $h(0,0)=0$; har outer point ka $\Vert \cdot \Vert =\sqrt{2}$ hai. To $\mathcal{Z}=\{0,\sqrt{2},\sqrt{2},\sqrt{2},\sqrt{2}\}$. Yeh step kyun? $h$ ko evaluate kiya jaata hai, differentiate kabhi nahi — origin corner (Figure 2 mein coral dot ke roop mein visible) arithmetic mein simply kabhi enter nahi karta.
3. Weights & mean (use $W^{(m)}$). $W_0=0$, $W_i=1/4$. $\hat{z}=0\cdot 0+4\cdot \frac{1}{4}\cdot \sqrt{2}=\sqrt{2}\approx \mathrm{1.414.}$ Yeh step kyun? Figure 2 dekho: chaar mint arrows sab ki length $\sqrt{2}$ hai, isliye measured range $\sqrt{2}$ par average karta hai — ek finite, sensible, derivative-free estimate exactly wahan jahan EKF fail hota hai.

Example 7 — Cell G: degenerate / zero-variance direction

1. Square root. $\sqrt{P}=\left(\begin{array}{cc}3& 0\\ 0& 0\end{array}\right)$; scaled columns $(3\sqrt{2},0)$ aur $(0,0)$. Yeh step kyun? Ek zero eigenvalue ek zero column produce karta hai, isliye transform automatically ek aisi direction mein spread karne se mana kar deta hai jahan koi uncertainty nahi — koi divide-by-zero nahi hoti kyunki hum square root lete hain, $P$ ka inverse kabhi nahi.
2. Sigma points. ${\mathcal{X}}_0=(3,5)$; ${\mathcal{X}}_{1,3}=(3\pm 3\sqrt{2},5)$; ${\mathcal{X}}_{2,4}=(3,5)$. Yeh step kyun? Doosra pair zero column inherit karta hai, isliye woh exactly centre par land karte hain — woh abhi bhi weight hold karte hain lekin $\bar{x}$ par baithe hain, har point ko line $y=5$ par rakhte hain.
3. Reconstruct. Saare points $y=5$ rakhte hain exactly → $y$ mein reconstructed variance $0$ hai ✔. $x$ mein: $\frac{1}{4}[(3\sqrt{2}{)}^2+(3\sqrt{2}{)}^2]=\frac{1}{4}(18+18)=9$ ✔. Yeh step kyun? $y$-deviations par variance recipe automatically $0$ deti hai, aur $x$-deviations par $9$ — degenerate case ko koi special handling nahi chahiye.

Example 8 — Cell H: scaled UT, tiny $\alpha$, $W^{(c)}$ rescue

1. $\lambda$ apni definition se compute karo. Actual formula use karo, koi shortcut nahi: $\lambda ={\alpha }^2(n+\kappa )-n=(10^{-6})(3)-1=3\times 10^{-6}-1=-\mathrm{0.999997.}$ To $c=n+\lambda =3\times 10^{-6}$. $\lambda$ ko exact rakho ($=3\times 10^{-6}-1$); ise $-1$ par mat round karo, kyunki neeche ke weights tiny $c$ se divide karte hain aur $\lambda$ ki koi bhi rounding unhe corrupt kar degi.
2. Sigma points. ${\mathcal{X}}_1=\sqrt{c}=\sqrt{3\times 10^{-6}}=1.732\times 10^{-3}$, ${\mathcal{X}}_2=-1.732\times 10^{-3}$, ${\mathcal{X}}_0=0$. Yeh step kyun? Itne small $c$ ke saath, outer points barely mean chhod paate hain — hum $f$ ko sirf bahut locally sample karte hain, jo exactly wahi hai jo scaled UT karne ke liye design kiya gaya hai.
3. Mean weights (use $W^{(m)}$). $W_0^{(m)}=\lambda /c=\frac{3\times 10^{-6}-1}{3\times 10^{-6}}$, ek bada negative number; $W_{1,2}^{(m)}=\frac{1}{2c}=\frac{1}{6\times 10^{-6}}\approx 1.667\times 10^5$, bahut bada. Yeh step kyun? Points mean ke paas hain, isliye unke weights blow up ho jaate hain (aur centre negative ho jaata hai) taaki $x$ ka reconstructed spread sahi rahe — yeh local sampling ki keemat hai.
4. Transform & mean. $\mathcal{Y}=\{0,3\times 10^{-6},3\times 10^{-6}\}$. $\bar{y}=W_0^{(m)}\cdot 0+2\cdot (1.667\times 10^5)(3\times 10^{-6})=2\cdot 0.5=1.$ Yeh step kyun? Tiny transformed values times giant weights exactly ${\sigma }^2=1$ mein recombine ho jaate hain — sampling ki locality ne mean mein koi accuracy nahi khoyi.
5. Covariance weight (use $W^{(c)}$). $W_0^{(c)}=W_0^{(m)}+(1-{\alpha }^2+\beta )=W_0^{(m)}+(1-10^{-6}+2)\approx W_0^{(m)}+2.999999$. Yeh extra $\approx +3$ centre par woh covariance repair karta hai jo tight spacing warna underestimate kar deti. Yeh step kyun? $\beta =2$ Gaussian-optimal correction hai; yeh sirf $W_0^{(c)}$ mein rehta hai (upar rulebook ke anusaar), isliye step 4 ka mean untouched hai — precisely isliye hum $\bar{y}=1$ rakh sake.

Example 9 — Cell I: real GNC word problem

1. Physics ko $x,P$ mein translate karo. Altitude state hai; uski $\pm 1\sigma$ range $100\pm 5$ km hai. Covariance $P=25$. Yeh step kyun? UT ko ek mean aur covariance chahiye; "std $5$ km" wali baat hi covariance hai ek baar square karne par. Yeh woh modelling move hai jo ek physical scenario ko filter inputs mein badal deta hai.
2. Sigma points. $\sqrt{cP}=\sqrt{3\cdot 25}=\sqrt{75}=5\sqrt{3}\approx 8.660$: $\mathcal{X}=\{100,108.66,91.34\}$. Yeh step kyun? Points altitude ke $\pm 1.73$ std span karte hain — altitudes ka actual band jo sensor read kar raha ho sakta hai, isliye transform true operating range sample karta hai.
3. $h$ se push karo. $h(100)=100$; $h(108.66)=10^6/108.66^2\approx 84.70$; $h(91.34)=10^6/91.34^2\approx 119.86$. Yeh step kyun? Far (high) point near (low) point se zyada power khota hai — woh asymmetry convex bias hai jo EKF ki straight-line slope capture nahi kar sakti.
4. Mean (use $W^{(m)}$). $\hat{z}=\frac{2}{3}(100)+\frac{1}{6}(84.70)+\frac{1}{6}(119.86)=66.667+\frac{1}{6}(204.56)=66.667+34.093=\mathrm{100.76.}$ Yeh step kyun? Transformed points ka weight-averaging; convexity $\hat{z}$ ko naive $h(\hat{x})=100$ se upar nudge karti hai, jo physically correct answer hai jo EKF miss karta hai.

Example 10 — Cell J: exam twist, weight trap

1. Sahi mean (use $W^{(m)}$). $W_0^{(m)}=\lambda /c=2/3$ $\beta$ se independent: Example 3 reuse karte hue, $\bar{y}=2$. Yeh step kyun? Upar ke rulebook ke anusaar, $\beta$ sirf $W_0^{(c)}$ mein rehta hai; mean by construction $\beta$-independent hona chahiye.
2. Galat mean ($W_0^{(c)}=8/3$ galat use karte hue). Weights ab $1$ sum nahi karte: $\frac{8}{3}+2\cdot \frac{1}{6}=\frac{8}{3}+\frac{1}{3}=3\ne 1$. Plug in karte hue: ${\bar{y}}_{\text{wrong}}=\frac{8}{3}(1)+\frac{1}{6}(4+2\sqrt{3})+\frac{1}{6}(4-2\sqrt{3})=\frac{8}{3}+\frac{1}{6}(8)=\frac{8}{3}+\frac{4}{3}=4.$ Yeh step kyun? Weights jo $1$ ki jagah $3$ sum karte hain har contribution ko inflate karte hain — "mean" $4$ tak balloon ho jaati hai, spurious $\beta$ footprint lekar.
3. Diagnosis. Sahi value $2$ hai; error $+2$ hai, precisely woh $(1-{\alpha }^2+\beta )=2$ leak hai jo covariance weight ko mean weight ki jagah rakhne se aaya. Yeh step kyun? Error ki size ko exactly correction term ke barabar isolate karna dikhata hai kyun rule "means use $W^{(m)}$" exist karta hai — weights swap karna covariance correction ko mean mein inject karta hai.

One-line recall

Related: Extended Kalman Filter (EKF) · Kalman Filter (linear) · Particle Filter · Cholesky Decomposition · Taylor Series Expansion · State Estimation in GNC · Nonlinear Systems.