Sensor fusion — complementary filter (simple), Kalman filter (optimal): Worked examples

3.5.20 · D3 · Physics › Guidance, Navigation & Control (GNC) › Sensor fusion — complementary filter (simple), Kalman filter

Numbers touch karne se pehle, ek reminder — do update laws jo hum baar baar use karenge.

Yahan $\hat{θ}$ hamara angle estimate hai, $ω_{k}$ gyro rate hai, $θ_{a, k}$ accelerometer angle hai, $Δ t$ sample time hai, $τ$ filter time-constant hai. Kalman ke liye, $Q$ process-noise variance hai (truth har step mein kitna wander karta hai), $R$ measurement-noise variance hai (sensor kitna jittery hai), $P$ hamari current uncertainty (variance) hai aur $K$ gain hai (hum new reading ki taraf kitna jump karte hain).

The scenario matrix

Har filter behaviour in cells mein se ek mein rehta hai. Neeche ke examples mein har ek ke saath [Cell N] tag hai.

Cell	Case class	Kya special hai
1	$α \to 1$ (large $τ$ )	gyro par almost poora trust — drift bachta hai
2	$α \to 0$ (small $τ$ )	accel par almost poora trust — noise bachta hai
3	Balanced $α$ , many steps	drift-cancellation time ke saath dikhaya
4	Kalman: $R \to 0$ (perfect sensor)	$K \to 1$ , measurement par snap karo
5	Kalman: $R \to \infty$ (dead sensor)	$K \to 0$ , measurement ignore karo
6	Kalman: $Q = 0$ (frozen truth)	$P$ aur $K$ zero tak shrink — converge hota hai
7	Kalman: multi-step steady state	$K$ ek constant tak converge karta hai = complementary $α$
8	Degenerate: $z = \overset{x}{^}^{-}$ (agreement)	measurement koi correction add nahi karta
9	Word problem — drone pitch	real signs, real units
10	Exam twist — cutoff ke liye $τ$ choose karo	design, sirf simulate nahi

Complementary filter cases

Worked example Example 1 —

$α \to 1$ : gyro dominates [Cell 1] Statement. $Δ t = 0.01 s$ , $τ = 9.99 s$ (bahut bada). Previous estimate $\hat{θ}_{k - 1} = 2 0^{\circ}$ , gyro rate $ω_{k} = - 3 0^{\circ} / s$ , accelerometer kehta hai $θ_{a} = 2 5^{\circ}$ . $\hat{θ}_{k}$ nikalo.

Forecast: itne bade $τ$ ke saath hum accel ko almost sunte hi nahi — guess karo answer gyro prediction ke paas rahega.

$α = \frac{9.99}{9.99 + 0.01} = 0.999$ . Yeh step kyun? $α$ 1 ke paas matlab hum smooth gyro par almost completely trust karna choose kiya.
Gyro prediction: $20 + (- 30) (0.01) = 20 - 0.3 = 19. 7^{\circ}$ . Kyun? ek tick mein predicted angle pane ke liye rate ko integrate karo.
Fuse: $\hat{θ}_{k} = 0.999 (19.7) + 0.001 (25) = 19.6803 + 0.025 = 19.705 3^{\circ}$ . Kyun? accel sirf $0.005 3^{\circ}$ se upar kheenchta hai — ek whisper jaisa.

Verify: fused answer $19.705 3^{\circ}$ par hai, essentially gyro prediction $19. 7^{\circ}$ par, bilkul forecast ki tarah. Kaafi steps mein yeh weak accel pull drift cancel karne ke liye bahut chhota hai — yahi $α \to 1$ ka khatara hai.

Worked example Example 2 —

$α \to 0$ : accel dominates [Cell 2] Statement. Same instant lekin ab $τ = 0.001 s$ (tiny), $Δ t = 0.01 s$ . Same $\hat{θ}_{k - 1} = 2 0^{\circ}$ , $ω_{k} = - 3 0^{\circ} / s$ , $θ_{a} = 2 5^{\circ}$ .

Forecast: tiny $τ$ ⇒ hum gyro ko almost ignore karte hain ⇒ answer accel $2 5^{\circ}$ ke paas hona chahiye.

$α = \frac{0.001}{0.001 + 0.01} = \frac{0.001}{0.011} = 0.0909$ . Kyun? chhota $τ$ $α$ ko 0 ki taraf dhakelta hai — drift-free accel par trust karo.
Gyro prediction: abhi bhi $20 - 0.3 = 19. 7^{\circ}$ . Kyun? same integration; sirf blend mein change hai.
Fuse: $\hat{θ}_{k} = 0.0909 (19.7) + 0.9091 (25) = 1.7909 + 22.7273 = 24.518 2^{\circ}$ . Kyun? ab accel dominate karta hai aur hume $\approx 24. 5^{\circ}$ par kheenchta hai.

Verify: answer $2 5^{\circ}$ (accel) ke paas gaya, $19. 7^{\circ}$ (gyro) ke paas nahi — Example 1 ki bilkul opposite extreme. Trade-off: har accel jitter ab estimate ko hila dega.

Worked example Example 3 — balanced

$α$ , drift ko marte hue dekhna [Cell 3] Statement. Ek gyro mein constant bias hai isliye yeh sochta hai $ω_{k} = + 2^{\circ} / s$ jabki drone actually still hai ( $θ_{a}$ $0^{\circ}$ par rehta hai). $α = 0.98$ , $Δ t = 0.01 s$ use karo, $\hat{θ}_{0} = 0^{\circ}$ se shuru karo. 3 steps run karo aur dekho $\hat{θ}$ kahan settle karta hai.

Forecast: ek pure gyro forever upar drift karta ( $+ 0.0 2^{\circ}$ per step, unbounded). Accel pull isko kisi chhoti steady value par cap kar deni chahiye, infinity nahi.

Gyro increment per step: $ω Δ t = 2 (0.01) = 0.0 2^{\circ}$ . Kyun? yeh woh fake angle hai jo bias har tick mein inject karta hai.
Step 1: $\hat{θ}_{1} = 0.98 (0 + 0.02) + 0.02 (0) = 0.019 6^{\circ}$ . Kyun? predict karo phir accel $0$ ki taraf bleed karo.
Step 2: $\hat{θ}_{2} = 0.98 (0.0196 + 0.02) + 0.02 (0) = 0.98 (0.0396) = 0.03880 8^{\circ}$ . Kyun? estimate creep karta hai lekin $0.98$ factor use shave karta rehta hai.
Step 3: $\hat{θ}_{3} = 0.98 (0.038808 + 0.02) = 0.98 (0.058808) = 0.0576 3^{\circ}$ . Kyun? same creep, aur yeh ek ceiling ki taraf decelerate kar raha hai.
Steady state: set karo $θ^{*} = 0.98 (θ^{*} + 0.02) \Rightarrow θ^{*} (1 - 0.98) = 0.98 (0.02) \Rightarrow θ^{*} = \frac{0.0196}{0.02} = 0.9 8^{\circ}$ . Kyun? equilibrium mein accel pull-down exactly gyro drift-up ko balance karta hai.

Verify: drift bounded hai $0.9 8^{\circ}$ par, infinity tak nahi jaata. Yeh bounded residual fixed $α$ ki keemat hai; chhota accel weight (smaller $α$ ) ise aur shrink kar deta. Steps 1–3 ( $0.0196, 0.0388, 0.0576$ ) sach mein $0.9 8^{\circ}$ ki taraf climb kar rahe hain.

Kalman filter cases

Worked example Example 6 —

$Q = 0$ : frozen truth, filter converges [Cell 6] Statement. Truth ek fixed constant hai, $Q = 0$ , $R = 1$ . $P_{0} = 1$ se shuru karo. Teen measurements feed karo. $P_{k}$ aur $K_{k}$ track karo.

Forecast: koi process noise nahi, toh har measurement $P$ aur gain ko shrink karta rehna chahiye, zero uncertainty ki taraf converge karta hua.

Step 1: $P^{-} = 1 + 0 = 1$ , $K_{1} = \frac{1}{1 + 1} = 0.5$ , $P_{1} = (1 - 0.5) (1) = 0.5$ . Kyun? pehle fusion se variance half ho jaata hai.
Step 2: $P^{-} = 0.5$ , $K_{2} = \frac{0.5}{0.5 + 1} = 0.3333$ , $P_{2} = (1 - 0.3333) (0.5) = 0.3333$ . Kyun? precisions add hote hain: $\frac{1}{P _{2}} = \frac{1}{0.5} + \frac{1}{1} = 3$ .
Step 3: $P^{-} = 0.3333$ , $K_{3} = \frac{0.3333}{0.3333 + 1} = 0.25$ , $P_{3} = (1 - 0.25) (0.3333) = 0.25$ . Kyun? $\frac{1}{P _{3}} = 2 + 1 + 1 = 4 \Rightarrow P_{3} = 0.25$ .

Verify: $P$ follow karta hai $1, \frac{1}{2}, \frac{1}{3}, \frac{1}{4} = \frac{1}{n + 1}$ aur $K$ follow karta hai $\frac{1}{2}, \frac{1}{3}, \frac{1}{4}$ . Precision $1/ P$ exactly $1/ R = 1$ se har step badhti hai — yeh "precisions add" ka magic hai. $Q = 0$ ke saath filter eventually sunna band kar deta hai ( $K \to 0$ ): yahi exact divergence trap hai (steel-man #4) jab tak $Q > 0$ nahi.

Worked example Example 7 — steady state = ek hidden complementary filter [Cell 7]

Statement. Ab $Q = 0.02$ , $R = 0.5$ lo. Gain ek constant $K_{\infty}$ par converge karta hai. Use nikalo, aur dikhao ki Kalman filter $α = 1 - K_{\infty}$ ke saath ek complementary filter ban jaata hai.

Forecast: $Q > 0$ ke saath, $P$ zero reach nahi kar sakta — yeh settle karta hai, isliye $K$ bhi settle karta hai. 0 aur 1 ke beech ek fixed gain expect karo.

Steady state matlab $P_{k} = P_{k - 1} = P^{*}$ . $P = (1 - K) P^{-}$ mein substitute karo $P^{-} = P^{*} + Q$ aur $K = \frac{P ^{*} + Q}{P ^{*} + Q + R}$ ke saath: $P^{*} = \frac{( P ^{*} + Q ) R}{P ^{*} + Q + R} .$ Yeh step kyun? equilibrium mein fusing se shrink exactly $+ Q$ growth ko cancel karta hai.
$M = P^{*} + Q$ lo. Tab $P^{*} = \frac{M R}{M + R}$ aur $M = P^{*} + Q$ . Substitute karo: $M - Q = \frac{M R}{M + R} \Rightarrow (M - Q) (M + R) = M R$ . Expand karo: $M^{2} + M R - QM - QR = M R \Rightarrow M^{2} - QM - QR = 0$ . Kyun? $M$ mein ek clean quadratic — exactly solvable.
$M = \frac{Q + Q ^{2} + 4 QR}{2} = \frac{0.02 + 0.0004 + 0.04}{2} = \frac{0.02 + 0.20100}{2} = 0.11050$ . Kyun? positive root lo ( $M = P^{-} \geq 0$ ).
$K_{\infty} = \frac{M}{M + R} = \frac{0.11050}{0.11050 + 0.5} = 0.18100$ . Kyun? yeh converged gain hai.
Equivalent complementary weight: $α = 1 - K_{\infty} = 0.81900$ . Kyun? yaad karo $\overset{x}{^} = (1 - K) \overset{x}{^}^{-} + K z$ mirrors karta hai $\hat{θ} = α (\dots) + (1 - α) θ_{a}$ .

Verify: $K_{\infty} = 0.181$ , $M = 0.1105$ plug karo: $P^{*} = (1 - 0.181) (0.1105) = 0.0905$ , aur $P^{*} + Q = 0.0905 + 0.02 = 0.1105 = M$ . ✔ Consistent. Toh steady state mein "optimal" Kalman literally $α \approx 0.819$ ke saath ek fixed- $α$ complementary filter hai.

Worked example Example 8 — degenerate: measurement prediction se agree karta hai [Cell 8]

Statement. $\overset{x}{^}^{-} = 3.2$ , $P^{-} = 0.4$ , $R = 0.6$ , aur measurement exactly $z = 3.2$ nikla. $K$ , $\overset{x}{^}$ , $P$ nikalo.

Forecast: agar sensor wahi confirm karta hai jo hum already believe karte the, toh estimate nahi hilna chahiye — lekin uncertainty abhi bhi girnee chahiye, kyunki agreement evidence hai.

$K = \frac{0.4}{0.4 + 0.6} = 0.4$ . Kyun? gain sirf variances par depend karta hai, $z$ ki value par nahi.
$\overset{x}{^} = 3.2 + 0.4 (3.2 - 3.2) = 3.2 + 0 = 3.2$ . Kyun? innovation $z - \overset{x}{^}^{-} = 0$ , isliye koi correction term nahi.
$P = (1 - 0.4) (0.4) = 0.24$ . Kyun? uncertainty abhi bhi $0.4$ se $0.24$ tak shrink hoti hai — zero move ke baad bhi agreement informative hai.

Verify: estimate unchanged ( $3.2$ ) phir bhi $P$ $0.24$ tak gira (precision $1/0.24 = 4.1 \overline{6} = 1/0.4 + 1/0.6 = 2.5 + 1.6 \overline{6}$ ). ✔ Mean khada reh sakta hai jabki confidence abhi bhi badhti hai.

Word problems

Worked example Example 9 — drone pitch, real signs & units [Cell 9]

Statement. Ek drone nose-down pitch karta hai. Gyro read karta hai $ω_{k} = - 4 5^{\circ} / s$ (negative = nose-down). Last estimate $\hat{θ}_{k - 1} = 1 2^{\circ}$ , accelerometer angle $θ_{a} = 8^{\circ}$ , $Δ t = 0.02 s$ , $τ = 0.18 s$ . Complementary filter kya pitch report karega?

Forecast: nose-down matlab angle $1 2^{\circ}$ se decrease hona chahiye. Donon sensors downward par agree karte hain, isliye $1 2^{\circ}$ se neeche koi value expect karo, kahin $1 1^{\circ}$ ke aas paas.

$α = \frac{0.18}{0.18 + 0.02} = \frac{0.18}{0.20} = 0.9$ . Kyun? moderate $τ$ : gyro par 90% trust, accel par 10%.
Gyro prediction: $12 + (- 45) (0.02) = 12 - 0.9 = 11. 1^{\circ}$ . Kyun? negative rate correctly angle ko lower karta hai — signs matter karte hain.
Fuse: $\hat{θ}_{k} = 0.9 (11.1) + 0.1 (8) = 9.99 + 0.8 = 10.7 9^{\circ}$ . Kyun? accel ( $8^{\circ}$ ) estimate ko thoda aur neeche nudge karta hai.

Verify: result $10.7 9^{\circ} < 1 2^{\circ}$ , nose-down as forecast, degrees mein (unit consistent: $ω Δ t$ hai $^{\circ} / s \cdot s =^{\circ}$ ). Woh tiny extra $0.3 1^{\circ}$ drop accel ka gyro bias se ladhna hai.

Worked example Example 10 — exam twist: target cutoff ke liye

$τ$ DESIGN karo [Cell 10] Statement. Aapko ek gyro fuse karna hai jo $Δ t = 0.005 s$ par sampled hai taki crossover frequency (jahan low-pass aur high-pass trust split karte hain) $f_{c} = 2 Hz$ ho. (a) Required $τ$ nikalo. (b) Resulting $α$ compute karo. (c) Batao kaunsa sensor $2 Hz$ se neeche dominate karta hai.

Forecast: zyada cutoff frequency ⇒ chhota time-constant ⇒ hum accel ko jaldi hand over karte hain ⇒ thoda chhota $τ$ , $α$ 1 ke paas lekin neeche.

First-order low-pass $\frac{1}{1 + τ s}$ ka cutoff $ω_{c} = 1/ τ$ hai, aur $ω_{c} = 2 π f_{c}$ . Toh $τ = \frac{1}{2 π f _{c}} = \frac{1}{2 π ( 2 )} = \frac{1}{12.566} = 0.07958 s$ . Yeh tool kyun? ek first-order filter ka $- 3 dB$ point exactly $1/ τ$ hai — yahi woh frequency hai jahan trust crossover karta hai.
$α = \frac{τ}{τ + Δ t} = \frac{0.07958}{0.07958 + 0.005} = \frac{0.07958}{0.08458} = 0.94089$ . Kyun? design $τ$ ko discrete blend weight mein convert karo.
$2 Hz$ se neeche low-pass path pass karta hai ⇒ accelerometer (drift-free) dominate karta hai; usse upar gyro (smooth) dominate karta hai. Kyun? yahi crossover ki definition hai jo humne design ki.

Verify: $τ = 0.0796 s$ , $α = 0.9409$ . Sanity: $1/ τ = 12.57 rad/s = 12.57/ (2 π) = 2.00 Hz$ ✔ target par wapas. $α$ 1 ke paas hai (fast sampling), forecast se consistent.

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

Upar ki figure har example ko ek map par rakhti hai: complementary gain axis $α$ (Examples 1, 2, 9, 10) aur Kalman gain axis $K$ (Examples 4–8) ek hi axis hai jo do baar dekhi gayi hai, steady state (Example 7) par join hoti hai jahan $α = 1 - K$ .

Recall Quick self-check

Bada $R$ (noisy sensor) Kalman gain ko kya karta hai? ::: $0$ ki taraf shrink karta hai — sensor par kam trust karo. $Q = 0$ aur constant truth ke saath, kaafi steps mein $P$ ka kya hota hai? ::: Yeh $1/ (n + 1)$ ki tarah $0$ ki taraf shrink karta hai; filter eventually sunna band kar deta hai. Steady state mein, Kalman filter kaunse simpler filter ke barabar hai? ::: Fixed $α = 1 - K_{\infty}$ wala complementary filter. Agar measurement prediction ke barabar hai, toh kya estimate move karta hai? Kya uncertainty girti hai? ::: Estimate wahi rehta hai; uncertainty abhi bhi girti hai. Zyada crossover frequency $f_{c}$ ke liye bada ya chhota $τ$ chahiye? ::: Chhota $τ$ , kyunki $τ = 1/ (2 π f_{c})$ .

Prerequisite refreshers: Gyroscope, Accelerometer, Low-pass and High-pass filters, Gaussian distribution, Bayesian estimation, State-space models, Extended Kalman Filter, Attitude estimation (AHRS), Inertial Navigation Systems.