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

The Complementary Filter (the "simple" fuse)

HOW we derive it from scratch

We want an estimate $\hat\theta$ that:

equals the accelerometer angle $\theta_a$ at low frequency (removes gyro drift),
equals the integrated gyro at high frequency (removes accel noise).

Take the simplest first-order low-pass $H_{\text{lp}}(s)=\dfrac{1}{1+\tau s}$ . Then the complement is forced: $H_{\text{hp}}(s)=1-\frac{1}{1+\tau s}=\frac{\tau s}{1+\tau s}.$

Notice $H_{\text{hp}}$ contains a factor $s$ (a derivative). The gyro already gives us $\dot\theta=s\,\theta$ , so applying $H_{\text{hp}}$ to the angle is like applying $\dfrac{\tau}{1+\tau s}$ to the rate — no explicit differentiation needed. Combine:

$\hat\theta(s)=\underbrace{\frac{1}{1+\tau s}}_{\text{accel}}\theta_a(s)+\underbrace{\frac{\tau s}{1+\tau s}}_{\text{gyro}}\theta_g(s)$

Discretize. Replace $s\theta \to \dot\theta = \omega_{\text{gyro}}$ (the measured rate), sample at $\Delta t$ . Doing the bilinear/Euler discretization gives the famous update:

Why this step? The term $\hat\theta_{k-1}+\omega_k\Delta t$ is the gyro prediction (integrate rate). Multiplying by $\alpha<1$ bleeds away the drift a little each step; $(1-\alpha)\theta_{a,k}$ gently pulls the estimate back toward the absolute accel reading. That gentle pull is exactly the low-pass filtering of accel noise.

Worked example Numeric complementary-filter step

Given $\Delta t = 0.01\,\text{s}$ , choose cutoff time constant $\tau=0.99\,\text{s}$ . $\alpha=\dfrac{0.99}{0.99+0.01}=0.99$ . Previous estimate $\hat\theta_{k-1}=10^\circ$ , gyro rate $\omega_k=50^\circ/\text{s}$ , accel angle $\theta_a=9^\circ$ .

Gyro prediction: $10 + 50(0.01)=10.5^\circ$ — Why? integrate rate over one tick.
Fuse: $\hat\theta_k = 0.99(10.5)+0.01(9)=10.395+0.09=10.485^\circ$ .
Why this result? We mostly believed the smooth gyro this instant, but the accel dragged us $0.015^\circ$ downward — over many steps that small drag cancels the gyro's long-term drift.

The Kalman Filter (the "optimal" fuse)

HOW we derive the scalar Kalman filter from scratch

Model a 1-D state $x$ (e.g. angle) with process and measurement: $x_k = x_{k-1} + w,\quad w\sim\mathcal N(0,Q)\qquad\text{(process noise, variance }Q)$ $z_k = x_k + v,\quad v\sim\mathcal N(0,R)\qquad\text{(measurement noise, variance }R)$

Step 1 — Predict. With no new measurement, our best guess stays, but uncertainty grows: $\hat x_k^- = \hat x_{k-1},\qquad P_k^- = P_{k-1} + Q.$ Why? Adding independent process noise adds variances.

Step 2 — Fuse two Gaussians. We have a prior $\mathcal N(\hat x_k^-, P_k^-)$ and a measurement $\mathcal N(z_k, R)$ . The product of two Gaussians is a Gaussian; minimizing the combined variance gives the fused mean. Derive by writing the posterior mean as a weighted average and choosing the weight to minimize the posterior variance.

Let the estimate be $\hat x = \hat x^- + K(z-\hat x^-)$ . Its error variance: $P = (1-K)^2 P^- + K^2 R.$ Minimize over $K$ : $\dfrac{dP}{dK}= -2(1-K)P^- + 2KR = 0$ $\Rightarrow K = \frac{P^-}{P^- + R}.$

Worked example One full Kalman cycle

Start: $\hat x_0=0$ , $P_0=1$ . Take $Q=0.01$ , $R=0.5$ , measurement $z_1=2$ .

Predict: $\hat x^-=0$ , $P^-=1+0.01=1.01$ . Why? uncertainty grew by $Q$ .
Gain: $K=\dfrac{1.01}{1.01+0.5}=0.669$ . Why? prior is more uncertain than the sensor, so lean toward the measurement.
Update: $\hat x=0+0.669(2-0)=1.338$ . Why? pulled two-thirds of the way to $z$ .
Variance: $P=(1-0.669)(1.01)=0.334$ . Why? fusing reduced uncertainty below both inputs. Next step $z_2$ arrives with $P^-=0.334+0.01=0.344$ ⇒ $K=0.408$ — the filter now trusts the measurement less, because it's already confident.

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

Common mistake Steel-manned common errors

1. "Kalman gain should be constant like $\alpha$ ." Feels right because complementary filters work with a fixed weight. Fix: $K$ must adapt because uncertainty changes over time (transients vs steady state). Fixed $K$ is only optimal at steady state, where $P$ converges — then Kalman literally becomes a complementary filter.

2. "Bigger $R$ means trust the measurement more." Feels right if you think "R = reading." Fix: $R$ is measurement-noise variance. Larger $R$ = noisier sensor ⇒ smaller $K$ ⇒ trust it less.

3. "Complementary weights don't need to sum to 1." Feels right — "just tune two knobs." Fix: if $H_{\text{lp}}+H_{\text{hp}}\neq1$ , a constant (DC) input gets scaled wrongly ⇒ steady-state bias. Summing to 1 guarantees unity DC gain.

4. Forgetting to grow $P$ in predict. Feels right to "just carry $P$ forward." Fix: without $+Q$ , the filter thinks it's infinitely certain and stops listening to sensors (gain → 0). That's filter divergence.

Recall Feynman: explain to a 12-year-old

You have two friends guessing a temperature. One friend, "Gyro-Guy," reacts super fast to changes but slowly starts lying more and more. The other, "Accel-Anne," is honest on average but jumpy and shouts random wrong numbers. If you average them the smart way — believe Gyro-Guy for quick changes but always nudge back toward honest Anne — you get a guess better than either. The complementary filter uses a fixed trust ratio. The Kalman filter is cleverer: it keeps a "how-sure-am-I" number, and each moment picks the perfect trust ratio, listening more to whichever friend is more reliable right now.

Flashcards

What condition must the two transfer functions in a complementary filter satisfy?

They must sum to unity,

H_{\text{lp}}(s)+H_{\text{hp}}(s)=1

, giving unity DC gain (no steady-state bias).

In the discrete complementary filter

\hat\theta_k=\alpha(\hat\theta_{k-1}+\omega_k\Delta t)+(1-\alpha)\theta_a

, what does

\alpha

control?

The trust split:

\alpha\to1

trusts the gyro (smooth, drift-prone),

\alpha\to0

trusts the accelerometer (drift-free, noisy).

\alpha=\tau/(\tau+\Delta t)

Why does a gyroscope need help from an accelerometer?

Integrating gyro rate accumulates bias → unbounded slow drift; the accelerometer gives an absolute (drift-free) angle at low frequency to correct it.

Write the scalar Kalman gain formula and its meaning.

K=\dfrac{P^-}{P^-+R}

; ratio of prior uncertainty to total uncertainty — more prior doubt or less measurement noise ⇒ larger gain (trust measurement more).

Derive the optimal Kalman gain.

Minimize

P=(1-K)^2P^-+K^2R

over

K

dP/dK=-2(1-K)P^-+2KR=0\Rightarrow K=P^-/(P^-+R)

What is the "precisions add" identity in the Kalman update?

1/P_k = 1/P_k^- + 1/R

— fusing two Gaussian sources adds their inverse variances (information).

How does the Kalman filter relate to the complementary filter?

The complementary filter is a Kalman filter with a fixed gain; at steady state

P

converges and

K

becomes constant, so Kalman reduces to a complementary filter.

What happens if you drop

+Q

in the predict step?

P

shrinks to 0, gain

K\to0

, filter stops trusting new measurements → divergence.

Does larger

R

make the filter trust the sensor more or less?

Less —

R

is measurement-noise variance; larger

R

⇒ smaller

K

Why do gyro (high-frequency) and accel (low-frequency) fuse well?

Their dominant errors sit in different frequency bands; complementary filtering keeps each sensor only where it is accurate.

Connections

Gyroscope — rate sensor, source of high-frequency term & drift
Accelerometer — gravity vector, low-frequency absolute reference
Low-pass and High-pass filters — building blocks of the complementary filter
Gaussian distribution — product of Gaussians underlies the Kalman update
Bayesian estimation — Kalman filter is recursive Bayesian inference for linear-Gaussian systems
State-space models — predict/update on $x_k=Ax_{k-1}+w$
Extended Kalman Filter — nonlinear generalization for real attitude/navigation
Attitude estimation (AHRS) — the drone/aircraft application
Inertial Navigation Systems — where drift correction is mission-critical

Concept Map

Hinglish (regional understanding)

Intuition Hinglish mein samjho

Socho tumhare paas do sensor hain jo dono thode "galat" hain. Gyroscope rate deta hai — usko integrate karke angle nikalta hai; ye short-term me bahut smooth hota hai par dheere-dheere drift kar jaata hai (chhoti si bias jud-jud ke bigaad deti hai). Accelerometer gravity se absolute angle deta hai — drift nahi hoti, par har vibration se noise aa jaata hai. Trick ye hai: dono ke error alag frequency me baithte hain, isliye gyro ko fast changes ke liye believe karo aur accel se dheere-dheere wapas kheech lo. Yehi complementary filter hai: $\hat\theta_k=\alpha(\hat\theta_{k-1}+\omega_k\Delta t)+(1-\alpha)\theta_a$ , jahan $\alpha$ aur $(1-\alpha)$ ka sum hamesha 1 hota hai.

Complementary filter ka $\alpha$ fixed hota hai — simple, par optimal nahi. Kalman filter ismein upgrade hai: wo ek "main kitna sure hoon" number rakhta hai jise variance $P$ kehte hain, aur har step pe best trust ratio (Kalman gain $K=P^-/(P^-+R)$ ) khud calculate karta hai. Jab tum kam confident ho, $K$ bada hota hai (measurement pe zyada bharosa); jaise-jaise confident hote ho, $K$ chhota ho jaata hai. Iski jaan ek line me: precisions add — $1/P=1/P^-+1/R$ , matlab do source ki information jud jaati hai aur uncertainty dono se kam ho jaati hai.

Do galtiyan jo sab karte hain: (1) socho $R$ bada matlab sensor pe zyada bharosa — galat! $R$ noise variance hai, bada $R$ matlab sensor gandaa, isliye $K$ chhota, bharosa kam. (2) predict step me $+Q$ bhool jaana — tab $P$ zero ho jaayega, filter sochega main perfect hoon aur naye readings sunna band kar dega (divergence). Yaad rakho loop PUG: Predict, Update-gain, Get estimate.

Real GNC me — drone AHRS, aircraft INS — yehi fusion critical hai, kyunki akela gyro udd ke drift kar jaayega aur akela accel har jhatke pe naach uthega. Steady state pe Kalman ka gain constant ho jaata hai — aur tab wo bilkul complementary filter ban jaata hai. Toh dono ek hi family ke hain, bas ek simple aur ek optimal.

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

WHY sensor fusion exists

The Complementary Filter (the "simple" fuse)

HOW we derive it from scratch

The Kalman Filter (the "optimal" fuse)

HOW we derive the scalar Kalman filter from scratch

Flashcards

Connections

Concept Map

Hinglish (regional understanding)

Go deeper — visual, from zero

Test yourself — Guidance, Navigation & Control (GNC)

Connections