Visual walkthrough — Sensor fusion — complementary filter (simple), Kalman filter (optimal)

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

Kisi bhi algebra se pehle, teen plain-English promises un words ke baare mein jo hum use karenge:

---

Step 1 — Guess ko dot nahi, hill ki tarah draw karo

KYA HAI. Apni guess ko number line par ek single point ki tarah draw karne ki jagah, hum ise ek bell-shaped hill ki tarah draw karte hain (ek Gaussian distribution). Peak hamaare estimate $\hat{x}$ par hoti hai; hill ki width uncertainty $P$ hai.

KYUN. Ek single dot pretend karta hai ki hum answer exactly jaante hain — hum jaante nahi. Ek hill honestly kehti hai "peak ke paas probably, thoda idhar-udhar bhi shayad." Poora Kalman machinery tabhi kaam karta hai jab yeh poori hills ke baare mein reason kare, dots ke baare mein nahi. Yeh Bayesian estimation ki language hai.

PICTURE. Do hills: ek narrow tall (confident guess, chhota $P$) aur ek wide flat (unsure guess, bada $P$). Same peak location, bohot alag meaning.

$\underset{\text{peak = best guess}}{\underbrace{\hat{x}}}\underset{\begin{array}{c}{\text{width}}^2\\ \text{= uncertainty}\end{array}}{\underbrace{P}}$

---

Step 2 — Hamare paas DO hills hain, aur woh disagree karte hain

KYA HAI. Ab dono informants ko ek saath laao. Hill A hamaari prior guess hai (gyro ki prediction se): peak ${\hat{x}}^{-}$, width $P^{-}$. Minus sign (${}^{-}$) matlab hai "naye measurement ko dekhne se pehle." Hill B measurement hill hai: peak $z$, width $R$.

KYUN. Fusion ka matlab hai do disagreeing hills ko ek behtar hill mein combine karna. Combine karne ke liye pehle unhe side by side dekhna hoga aur notice karna hoga ki woh overlap karte hain par coincide nahi karte — truth kahin overlap mein hai.

PICTURE. Blue prior hill aur orange measurement hill same axis par drawn, peaks apart, dono kaafi wide.

$\text{prior: }\mathcal{N}(\underset{\text{peak}}{\underbrace{{\hat{x}}^{-}}},\underset{{\text{width}}^2}{\underbrace{P^{-}}})\text{measurement: }\mathcal{N}(\underset{\text{peak}}{\underbrace{z}},\underset{{\text{width}}^2}{\underbrace{R}})$

Yahan $\mathcal{N}(\text{peak},{\text{width}}^2)$ sirf shorthand hai "ek bell hill jo peak par centred hai is width ke saath."

---

Step 3 — Hills ko multiply karo → fused hill unke beech land karti hai

KYA HAI. Do independent hills ko fuse karne ke liye hum unhe point by point multiply karte hain aur re-normalise karte hain. Result ek teesri bell hill hai — dono parents se taller aur narrower, apना peak jis parent ki taraf khicha hua ho jo narrower thi.

KYUN multiply? Har hill ek statement hai ki "har value kitni believable hai." Jab do independent informants dono weigh in karte hain, believability multiply hoti hai (yeh independent evidence ke liye Bayes' rule hai). Aur do bells ko multiply karne se hamesha ek aur bell milti hai — yeh lucky fact hi reason hai ki Kalman filter simple rehta hai.

PICTURE. Blue aur orange parent hills, green fused hill unke peaks ke beech baith kar, clearly dono se taller aur thinner.

---

Step 4 — Fused peak ko "A se B ki taraf walk" ki tarah likho

KYA HAI. Messy product formula par seedha jump karne ki jagah, hum answer ko parameterise karte hain. Fused peak $\hat{x}$ necessarily ${\hat{x}}^{-}$ aur $z$ ke beech mein honi chahiye, toh ise prior se measurement ki taraf partial step ki tarah likho:

$\hat{x}={\hat{x}}^{-}+K(z-{\hat{x}}^{-})$

KYUN yeh shape. Yeh guarantee karta hai ki kisi bhi $K$ ke liye jo $0$ aur $1$ ke beech ho, answer do peaks ke beech hoga, aur yeh ek ek unknown ko isolate karta hai jis par humein actually care karna hai: kitna door walk karna hai, woh number $K$. Humne ise gain ka naam diya.

PICTURE. Ek number line: ${\hat{x}}^{-}$ (blue dot) se start karo, target $z$ (orange dot). $K\times (\text{gap})$ length ka ek arrow humein green fused point par land karta hai. Gap $(z-{\hat{x}}^{-})$ ko innovation label kiya gaya hai — surprise, "measurement ne hamaari guess se kitna disagree kiya."

$\hat{x}=\underset{\begin{array}{c}\text{start:}\\ \text{purani guess}\end{array}}{\underbrace{{\hat{x}}^{-}}}+\underset{\begin{array}{c}\text{walk ka}\\ \text{fraction}\end{array}}{\underbrace{K}}\underset{\begin{array}{c}\text{innovation}\\ \text{= surprise gap}\end{array}}{\underbrace{(z-{\hat{x}}^{-})}}$

• $K=0$ → move mat karo → measurement ignore karo, purani guess rakhho.
• $K=1$ → poora walk karo → guess phenko, measurement maano.
• $K$ beech mein → ek blend. Kaun sa $K$ best hai? Yahi Step 5 mein hai.

---

Step 5 — Hamare walk ki uncertainty $K$ mein ek parabola hai

KYA HAI. $K$ ki har choice ek fused guess produce karti hai kuch leftover uncertainty $P$ ke saath. Kyunki do error sources independent hain, unke variances weighting ke baad add hote hain:

$P=(1-K{)}^2{P}^{-}+K^{2}R$

KYUN yeh weights. Prior ka error $(1-K)$ se scale hoke survive karta hai (humne uska $1-K$ fraction rakha); measurement ka error $K$ se scale hoke enter karta hai (humne uska $K$ fraction andar aane diya). Square karne se "scaled error" "scaled variance" ban jaata hai. Koi cross term nahi, kyunki do noises independent hain.

PICTURE. $P$ ko $K$ ke against plot karo. Yeh ek muskurata hua parabola hai (ek valley). Far left ($K=0$): $P=P^{-}$, poori prior uncertainty. Far right ($K=1$): $P=R$, poori measurement uncertainty. Beech mein kahin lowest point hai — best $K$.

$P=\underset{\begin{array}{c}\text{prior error}\\ \text{jo humne rakha}\end{array}}{\underbrace{(1-K{)}^2{P}^{-}}}+\underset{\begin{array}{c}\text{sensor noise}\\ \text{jo humne andar aane diya}\end{array}}{\underbrace{K^{2}R}}$

---

Step 6 — Valley ke bottom par slide karo → gain formula

KYA HAI. Ek smooth valley ke bilkul bottom par, ground momentarily flat hoti hai: slope zero hai. $P$ ka slope $K$ ke respect mein $\frac{dP}{dK}$ likha jaata hai (ek derivative — yeh jawab deta hai "jab main $K$ ko thoda nudge karta hoon toh $P$ kitni tezi se change hoti hai?"). Us slope ko zero set karo.

KYUN derivative aur kuch aur nahi. Humein parabola ka turning point chahiye. Derivative exactly woh tool hai jo local slope report karta hai; jahan yeh zero hit karta hai wahi curve girna band karke upar jaana shuru karta hai — minimum. Koi aur tool bottom ko itni directly pinpoint nahi karta.

$\frac{dP}{dK}=-2(1-K)P^{-}+2KR=0$

PICTURE. Wahi parabola, uske lowest point ko touch karta hua ek flat tangent line, aur $K$-axis par $K^{\star }$ mark karta hua ek dashed line.

$K$ ke liye solve karo (2 se divide karo, expand karo, $K$ gather karo): $-(P^{-}-K{P}^{-})+KR=0\Rightarrow -P^{-}+K{P}^{-}+KR=0\Rightarrow K(P^{-}+R)=P^{-}$

---

Step 7 — Fused width, aur kyun "information add hoti hai"

KYA HAI. $K=\frac{P^{-}}{P^{-}+R}$ ko parabola mein wapas daalo. Algebra ke baad yeh clean bottom-value mein collapse ho jaata hai:

$P=(1-K)P^{-}=\frac{P^{-}R}{P^{-}+R}⟺\frac{1}{P}=\frac{1}{P^{-}}+\frac{1}{R}$

KYUN yeh beautiful hai. Har variance ko uske reciprocal $1/P$ mein flip karo — ise precision (certainty ki bigness) kaho. Aakhri equation kehti hai: precisions simply add hote hain. Do clues → unki information add karo → fused hill hamesha dono parents se sharper hoti hai.

PICTURE. Teen horizontal "certainty bars": prior precision $1/P^{-}$, sensor precision $1/R$, aur unka sum $1/P$ — fused bar literally do ko end-to-end stack kiya hua hai.

$\underset{\begin{array}{c}\text{fused}\\ \text{certainty}\end{array}}{\underbrace{\frac{1}{P}}}=\underset{\begin{array}{c}\text{guess}\\ \text{certainty}\end{array}}{\underbrace{\frac{1}{P^{-}}}}+\underset{\begin{array}{c}\text{sensor}\\ \text{certainty}\end{array}}{\underbrace{\frac{1}{R}}}$

---

Step 8 — Edge cases (kuch bhi surprise nahi karna chahiye)

KYA HAI & KYUN. Jis formula par tum trust karte ho woh har extreme se survive karna chahiye. $K=\frac{P^{-}}{P^{-}+R}$ ke corners walk karo:

Situation	$K$ ban jaata hai	Meaning
Perfect sensor, $R\to 0$	$K\to 1$	Poora walk karo — measurement par poora believe karo. Fused $P\to 0$.
Useless sensor, $R\to \infty$	$K\to 0$	Mat hilo — purani guess rakhho. Fused $P\to P^{-}$.
Pehle se perfectly sure, $P^{-}\to 0$	$K\to 0$	Sensor ignore karo — certainty par improve nahi kar sakte.
Bilkul lost, $P^{-}\to \infty$	$K\to 1$	Sensor ko dono haathon se pakdo.
$P^{-}=R$ (equal trust)	$K=\frac{1}{2}$	Difference split karo — plain average.

PICTURE. Trust-fraction $K$ ko plot karo jab $R$ $0$ se large sweep karta hai, fixed $P^{-}$ ke liye: ek smooth curve jo $1$ se $0$ ki taraf girta hai, exactly $\frac{1}{2}$ se guzarta hai jahan $R=P^{-}$ ho.

---

Ek-picture summary

Upar sab kuch ek loop hai: Predict (guess drift karti hai, hill $Q$ se wide hoti hai) → gain compute karo (valley ka bottom) → Update (measurement ki taraf walk karo, hill narrow hoti hai). Phir repeat.

Recall Feynman retelling — poora walk plain words mein

Socho drone ke angle ke baare mein tumhara belief ek line par ek hill hai: peak tumhari best guess hai, width tumhari unsureness hai. Tumhe ek sensor se doosri hill milti hai — alag peak, apni width. Do honest hills combine karne ke liye tum unhe multiply karte ho, jo magically ek teesri hill deta hai peaks ke beech baith kar, dono se narrower — kyunki do clues hamesha ek se behtar hote hain. Ab naya peak exactly kahan baithega? Ise "purani guess se shuru karo aur sensor ki taraf fraction $K$ walk karo" ki tarah likho. Har $K$ tumhe kuch leftover fuzziness deta hai, aur agar tum us fuzziness ko $K$ ke against plot karo toh tumhe ek valley milti hai. Best $K$ valley ka bottom hai, yeh poochh kar milta hai "ground flat kahan hai?" — wahi derivative equals zero hai. Nikalta hai $K=\frac{P^{-}}{P^{-}+R}$: ek simple trust fraction jo sensor ki taraf lean karta hai jab tumhari apni guess shaky ho aur tumhari guess ki taraf jab sensor noisy ho. Widths ko "certainties" mein flip karo aur punchline saamne aata hai — certainties bas add hoti hain. Complementary filter (dekho Low-pass and High-pass filters) frozen $K$ ke saath wahi walk hai; Kalman filter har single tick mein perfect $K$ recompute karta hai. Yahi poora idea hai Attitude estimation (AHRS) aur Inertial Navigation Systems ke peeche, aur Extended Kalman Filter sirf yahi walk hai curved terrain par ki gayi.

Recall Quick self-check

Innovation $(z-{\hat{x}}^{-})$ words mein kya matlab hai? ::: "Surprise" — fresh measurement ne hamaari prediction se kitna disagree kiya. Fused hill hamesha dono parents se narrower kyun hoti hai? ::: Kyunki independent clues certainty add karte hain (precisions add hote hain), kabhi subtract nahi karte. Jab sensor noise $R$ badhti hai, toh gain $K$ rise karta hai ya fall? ::: Woh fall karta hai — hum ek shakier sensor par kam trust karte hain. Optimal $K$ geometrically kahan se aata hai? ::: Variance-vs-$K$ parabola ke bottom se, jahan slope $dP/dK=0$ ho.