Exercises — Sensor fusion — complementary filter (simple), Kalman filter (optimal)

$\alpha =\frac{\tau }{\tau +\Delta t}=\frac{0.49}{0.49+0.01}=\frac{0.49}{0.50}=0.98$. WHAT this means: $0.98$ gyro prediction ko multiply karta hai, isliye hum is instant mein gyro par heavily trust karte hain. Remaining $1-\alpha =0.02$ gently accel ki taraf khichta hai — long-term drift mitane ke liye kaafi, fast accel noise reject karne ke liye itna chota.

$K=\frac{P^{-}}{P^{-}+R}=\frac{0.9}{0.9+0.3}=\frac{0.9}{1.2}=0.75$. WHAT it does: update hai $\hat{x}={\hat{x}}^{-}+K(z-{\hat{x}}^{-})$, isliye hum apne prior se new reading $z$ ki taraf 75% raasta move karte hain. WHY itna zyada? Kyunki prior ($P^{-}=0.9$) sensor ($R=0.3$) se zyada uncertain hai, isliye sensor ko louder vote milna chahiye.

Step A — WHAT: $\alpha$ build karo. $\alpha =\frac{0.38}{0.38+0.02}=\frac{0.38}{0.40}=0.95$. Step B — WHY pehle predict karein: angle kahan gaya yeh guess karne ke liye rate integrate karo. Gyro prediction $=20+(-30)(0.02)=20-0.6=19.4^{\circ }$. Step C — Fuse: ${\hat{\theta }}_k=0.95(19.4)+0.05(18)=18.43+0.9=19.33^{\circ }$. WHAT it looks like: humne mostly gyro ke downward swing ko follow kiya, phir accel ne hume thoda aur neeche kheecha (kyunki $18<19.4$).

Predict. ${\hat{x}}^{-}=5$ (no motion model but the state); $P^{-}=P_0+Q=2+0.04=2.04$. WHY $+Q$: uncertainty hamesha measurements ke beech badhti hai. Gain. $K=\frac{2.04}{2.04+1}=\frac{2.04}{3.04}=0.6711$. Update. $\hat{x}=5+0.6711(8-5)=5+2.0132=7.0132$. Variance. $P=(1-K)P^{-}=(0.3289)(2.04)=0.6711$. WHAT it looks like: estimate reading ki taraf two-thirds of the way tak phunga, aur uski variance $2.04$ se $0.67$ tak drop ho gayi — dono prior aur sensor se neeche. Fusion hamesha sharpen karta hai.

Case $\Delta t\to 0$ (ya $\Delta t\ll \tau$): $\alpha =\frac{\tau }{\tau +\Delta t}\to \frac{\tau }{\tau }=1$. WHAT it looks like: ${\hat{\theta }}_k\to {\hat{\theta }}_{k-1}+{\omega }_k\Delta t$ — pure gyro integration, accel term $(1-\alpha ){\theta }_a\to 0$. WHY: ticks arbitrarily fast aane se, corrections ke beech sirf thodi si drift build hoti hai, isliye smooth gyro rule karta hai aur noisy accel ignore hota hai. Yeh high-frequency / short-term regime hai. Case $\Delta t\gg \tau$: $\alpha =\frac{\tau }{\tau +\Delta t}\to 0$. WHAT it looks like: ${\hat{\theta }}_k\to {\theta }_{a,k}$ — estimate sirf accel copy karta hai; gyro prediction discard ho jaati hai. WHY: ek lambe gap mein gyro ki drift bahut badi ho gayi hai, isliye hum stale prediction throw away karte hain aur drift-free accel par re-anchor karte hain. Yeh low-frequency / long-term regime hai. Knob $\tau$ tumhe in poles ke beech slide karta hai: bada $\tau$ ($\Delta t$ ke relative) → $\alpha \to 1$ → gyro-dominated; chota $\tau$ → $\alpha \to 0$ → accel-dominated. Crossover exactly wahan hai jahan $\Delta t=\tau$, deta hai $\alpha =\frac{1}{2}$.

WHAT we do: steady state par $P_k=P_{k-1}=P_{\infty }$. $P^{-}=P_{\infty }+Q$ ke saath $P=(1-K)P^{-}$ mein gain substitute karo. Step 1 — fixed-point equation assemble karo. $K=\frac{P^{-}}{P^{-}+R}$ use karke, updated variance hai $P=(1-K)P^{-}=\frac{P^{-}R}{P^{-}+R}$ (yeh "$\text{precisions add}$" form hai). Steady state par $P=P_{\infty }$ aur $P^{-}=P_{\infty }+Q$, isliye $P_{\infty }=\frac{(P_{\infty }+Q)R}{(P_{\infty }+Q)+R}.$ Step 2 — denominator clear karo. Dono sides ko $((P_{\infty }+Q)+R)$ se multiply karo. WHY: fraction ko ek polynomial mein baadalna jo hum solve kar sakein. $P_{\infty }((P_{\infty }+Q)+R)=(P_{\infty }+Q)R.$ Step 3 — har side expand karo. Left: $P_{\infty }^2+P_{\infty }Q+P_{\infty }R$. Right: $P_{\infty }R+QR$. $P_{\infty }^2+P_{\infty }Q+P_{\infty }R=P_{\infty }R+QR.$ Step 4 — shared term $P_{\infty }R$ cancel karo dono sides se (yeh identically appear karta hai), ek clean quadratic bach jaati hai: $P_{\infty }^2+P_{\infty }Q-QR=0.$ Step 5 — quadratic solve karo. $Q=0.1,R=1$ ke saath: $P_{\infty }^2+0.1P_{\infty }-0.1=0$. Quadratic formula do roots deta hai $P_{\infty }=\frac{-0.1\pm \sqrt{0.01+0.4}}{2}=\frac{-0.1\pm 0.6403}{2}.$ Step 6 — physically valid root choose karo. WHY hum $(-)$ root discard karte hain: $P_{\infty }$ ek variance hai, aur variance ek squared spread hai — yeh kabhi negative nahi ho sakta. $(-)$ root deta hai $\frac{-0.1-0.6403}{2}=-0.3701<0$, jo variance ke liye impossible hai, isliye hum use reject karte hain. $(+)$ root hai $P_{\infty }=\frac{-0.1+0.6403}{2}=\mathrm{0.2701.}$ Phir $S=P_{\infty }+Q=0.3701$, aur $K_{\infty }=\frac{0.3701}{0.3701+1}=0.2701$. WHAT it means: ek baar settle hone ke baad, Kalman gain ek constant $0.2701$ hai — matlab ek settled Kalman filter hai hi ek complementary filter jisme $1-\alpha =K_{\infty }$.

Kalman way. $K=\frac{0.4}{0.4+0.6}=0.4$, isliye $P=(1-0.4)(0.4)=0.24$. Precision rule check karo. $\frac{1}{P^{-}}+\frac{1}{R}=2.5+1.6667=4.1667$, aur $\frac{1}{0.24}=4.1667$. ✓ Naive average ($\frac{1}{2}$ each). Do independent estimates ka average karne par, average ki variance hai $(\frac{1}{2}{)}^2{P}^{-}+(\frac{1}{2}{)}^{2}R=\frac{1}{4}(0.4)+\frac{1}{4}(0.6)=0.1+0.15=0.25$ — optimal $0.24$ se worse. WHY: equal weighting ignore karta hai ki pehla source zyada precise hai. Optimal weight sharper source ki taraf lean karta hai, variance aur neeche squeeze karta hai. Information (= inverse variance) simply add hoti hai.

WHAT the match requires: $K_{\infty }=1-\alpha =0.02$. L3·1 se settled relations yaad karo: $P_{\infty }^2+P_{\infty }Q-QR=0$ aur $K_{\infty }=\frac{P_{\infty }+Q}{P_{\infty }+Q+R}$. Gain se: $K_{\infty }=\frac{S}{S+R}$ jahan $S=P_{\infty }+Q$, isliye $S=\frac{K_{\infty }R}{1-K_{\infty }}=\frac{0.02\cdot 1}{0.98}=0.020408$. Aur $P_{\infty }=(1-K_{\infty })S=0.98\times 0.020408=0.02$. Phir $Q=S-P_{\infty }=0.020408-0.02=0.000408$. WHAT it means: tiny $Q=4.08\times 10^{-4}$ (aur $R=1$) wala Kalman filter usi $2\%$ blend par lock ho jaata hai jo uska complementary filter use karta hai. Dono filters steady state par same machine hain — Kalman bas noisy start-up transient bhi handle karta hai.

Step 1. $P^{-}=4.1$, $K=\frac{4.1}{5.1}=0.8039$, $P_1=(1-0.8039)(4.1)=0.8039$. Step 2. $P^{-}=0.9039$, $K=\frac{0.9039}{1.9039}=0.4748$, $P_2=(0.5252)(0.9039)=0.4748$. Step 3. $P^{-}=0.5748$, $K=\frac{0.5748}{1.5748}=0.3650$, $P_3=(0.6350)(0.5748)=0.3650$. WHAT it looks like (neeche figure dekho): $P$ plunge karta hai $4\to 0.80\to 0.47\to 0.37\to \dots$, $P_{\infty }=0.2701$ ki taraf settle hota hua. Gain uske saath neeche ride karta hai. Woh falling curve exactly wahi hai jo Kalman ko fixed $\alpha$ se beat karta hai: shuruaat mein yeh measurements ko hard grab karta hai (high $K$), phir relax ho jaata hai jaise confidence build hoti hai.

Figure — kya observe karna hai (alt-text): Ek blueprint-style plot jisme horizontal axis par step number $k$ hai aur vertical axis par value hai. Circles wala cyan curve estimate variance $P_k$ hai: yeh upar $P_0=4$ se start hota hai aur steeply drop karta hai — $4\to 0.80\to 0.47\to 0.37$ — dashed white line $P_{\infty }=0.270$ ki taraf flatten hota hua. Squares wala amber curve Kalman gain $K_k$ hai: yeh upar se start hota hai (near $0.80$, "shuruaat mein measurements hard grab karta hai") aur $0.27$ ki taraf settle hota hai ("jaise confident hota hai relax ho jaata hai"). Jo tumhe samajhna chahiye: dono uncertainty aur measurement-par-trust saath mein sikhte hain aur L3·1 mein compute ki gayi steady-state value par level off karte hain — yeh converged, frozen gain precisely complementary filter ka fixed $\alpha$ hai.

Step A — WHAT: error likho. True value $x$ ho. Prior error $e^{-}={\hat{x}}^{-}-x$ ki variance $P^{-}$ hai; measurement error $z-x=v$ ki variance $R$ hai, $e^{-}$ se independent. Tab $\hat{x}-x=({\hat{x}}^{-}-x)+K((z-x)-({\hat{x}}^{-}-x))=(1-K)e^{-}+Kv.$ Step B — WHY variances add karte hain: $e^{-}$ aur $v$ independent hain, isliye unki variances squared coefficients ke saath add hoti hain: $P=\mathrm{Var}(\hat{x}-x)=(1-K{)}^2{P}^{-}+K^{2}R.$ Step C — Minimise karo. WHY derivative? $P$ mein $K$ ki ek smooth upward parabola hai; uske lowest point par slope zero hota hai. $\frac{dP}{dK}=-2(1-K)P^{-}+2KR=0\Rightarrow (1-K)P^{-}=KR\Rightarrow K=\frac{P^{-}}{P^{-}+R}.$ Step D — Back-substitute karo. $P=(1-K{)}^2{P}^{-}+K^{2}R$. Kyunki $(1-K)P^{-}=KR$ hai, $(1-K)$ se multiply karo: $(1-K{)}^2{P}^{-}=(1-K)KR$, isliye $P=(1-K)KR+K^{2}R=KR[(1-K)+K]=KR=\frac{P^{-}R}{P^{-}+R}=(1-K)P^{-}.$ WHAT it means: "$\text{precisions add}$" law $\frac{1}{P}=\frac{1}{P^{-}}+\frac{1}{R}$ directly follow karta hai, aur gain kuch nahi bas "har source par uski precision ke proportion mein trust karo" hai.

Sum. $H_{\text{lp}}+H_{\text{hp}}=\frac{1+\tau s}{1+\tau s}=1$ har $s$ ke liye. WHY yeh matter karta hai: jo bhi frequency ho, dono paths ke weights ka total hamesha $1$ hota hai, isliye koi scaling error creep in nahi ho sakti. DC, $s=0$: $H_{\text{lp}}=1$, $H_{\text{hp}}=0$. Ek constant angle poori tarah accel path se pass hota hai — jo drift-free hai, exactly wahi jo hum long-term truth ke liye chahte hain. Fast, $s\to \infty$: $H_{\text{lp}}\to 0$, $H_{\text{hp}}\to 1$. Quick wiggles poori tarah gyro path se pass hoti hain — short-term smooth aur noise-free. WHAT it means: har sensor woh band rule karta hai jahan woh trustworthy hai, aur kyunki transfer functions ka sum $1$ hai, ek steady input apna exact size rakhta hai (unity DC gain) — koi steady-state bias nahi. Yeh L3·0 ke $\Delta t$ limits ka frequency-domain mirror hai: DC ↔ $\Delta t\gg \tau$ (accel), high frequency ↔ $\Delta t\to 0$ (gyro).

Complementary blend weight hai ::: $\alpha =\frac{\tau }{\tau +\Delta t}$ Kalman gain hai ::: $K=\frac{P^{-}}{P^{-}+R}$ Predict variance ko badhata hai add karke ::: process noise $Q$, deta hai $P^{-}=P_{k-1}+Q$ Bada $R$ filter ko measurement par trust karna ::: kam karta hai (chota $K$) Do Gaussians fuse karne par unke ::: precisions (inverse variances) add hote hain Jab $\Delta t\to 0$ complementary filter trust karta hai ::: gyro ko poori tarah ($\alpha \to 1$) Jab $\Delta t\gg \tau$ complementary filter trust karta hai ::: accel ko poori tarah ($\alpha \to 0$) Angles blend karne se pehle tumhe ::: difference ko $(-180^{\circ },180^{\circ }]$ mein wrap karna hoga (shortest arc) Steady-state Kalman filter equivalent hai ek ::: complementary filter ke jisme fixed gain $K_{\infty }=1-\alpha$ ho