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

Before we touch numbers, one reminder of the two update laws we will reuse everywhere.

Here $\hat{θ}$ is our angle estimate, $ω_{k}$ the gyro rate, $θ_{a, k}$ the accelerometer angle, $Δ t$ the sample time, $τ$ the filter time-constant. For Kalman, $Q$ is process-noise variance (how much the truth wanders per step), $R$ is measurement-noise variance (how jittery the sensor is), $P$ is our current uncertainty (variance) and $K$ the gain (how far we jump toward the new reading).

The scenario matrix

Every filter behaviour lives in one of these cells. The examples below each carry a [Cell N] tag.

Cell	Case class	What makes it special
1	$α \to 1$ (large $τ$ )	trust gyro almost fully — drift survives
2	$α \to 0$ (small $τ$ )	trust accel almost fully — noise survives
3	Balanced $α$ , many steps	drift-cancellation shown over time
4	Kalman: $R \to 0$ (perfect sensor)	$K \to 1$ , snap to measurement
5	Kalman: $R \to \infty$ (dead sensor)	$K \to 0$ , ignore measurement
6	Kalman: $Q = 0$ (frozen truth)	$P$ and $K$ shrink to zero — converges
7	Kalman: multi-step steady state	$K$ converges to a constant = complementary $α$
8	Degenerate: $z = \overset{x}{^}^{-}$ (agreement)	measurement adds no correction
9	Word problem — drone pitch	real signs, real units
10	Exam twist — pick $τ$ for a cutoff	design, not just simulate

Complementary filter cases

Worked example Example 1 —

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

Forecast: with such a huge $τ$ we barely listen to the accel — guess the answer stays near the gyro prediction.

$α = \frac{9.99}{9.99 + 0.01} = 0.999$ . Why this step? $α$ near 1 means we chose to trust the smooth gyro almost entirely.
Gyro prediction: $20 + (- 30) (0.01) = 20 - 0.3 = 19. 7^{\circ}$ . Why? integrate rate over one tick to get a predicted angle.
Fuse: $\hat{θ}_{k} = 0.999 (19.7) + 0.001 (25) = 19.6803 + 0.025 = 19.705 3^{\circ}$ . Why? the accel pulls us up by only $0.005 3^{\circ}$ — a whisper.

Verify: the fused answer sits $19.705 3^{\circ}$ , essentially on the gyro prediction $19. 7^{\circ}$ , exactly as forecast. Over many steps this weak accel pull is too small to cancel drift — that is the danger of $α \to 1$ .

Worked example Example 3 — balanced

$α$ , watching drift die [Cell 3] Statement. A gyro has a constant bias so it thinks $ω_{k} = + 2^{\circ} / s$ while the drone is actually still ( $θ_{a}$ stays $0^{\circ}$ ). Use $α = 0.98$ , $Δ t = 0.01 s$ , start $\hat{θ}_{0} = 0^{\circ}$ . Run 3 steps and find where $\hat{θ}$ settles.

Forecast: a pure gyro would drift up forever ( $+ 0.0 2^{\circ}$ per step, unbounded). The accel pull should cap it at some small steady value, not infinity.

Gyro increment per step: $ω Δ t = 2 (0.01) = 0.0 2^{\circ}$ . Why? this is the fake angle the bias injects each tick.
Step 1: $\hat{θ}_{1} = 0.98 (0 + 0.02) + 0.02 (0) = 0.019 6^{\circ}$ . Why? predict then bleed toward accel $0$ .
Step 2: $\hat{θ}_{2} = 0.98 (0.0196 + 0.02) + 0.02 (0) = 0.98 (0.0396) = 0.03880 8^{\circ}$ . Why? the estimate creeps but the $0.98$ factor keeps shaving it.
Step 3: $\hat{θ}_{3} = 0.98 (0.038808 + 0.02) = 0.98 (0.058808) = 0.0576 3^{\circ}$ . Why? same creep, and it is decelerating toward a ceiling.
Steady state: set $θ^{*} = 0.98 (θ^{*} + 0.02) \Rightarrow θ^{*} (1 - 0.98) = 0.98 (0.02) \Rightarrow θ^{*} = \frac{0.0196}{0.02} = 0.9 8^{\circ}$ . Why? at equilibrium the accel pull-down exactly balances the gyro drift-up.

Verify: drift is bounded at $0.9 8^{\circ}$ instead of running to infinity. That bounded residual is the price of a fixed $α$ ; a bigger accel weight (smaller $α$ ) would shrink it. Steps 1–3 ( $0.0196, 0.0388, 0.0576$ ) are indeed climbing toward $0.9 8^{\circ}$ .

Kalman filter cases

Worked example Example 6 —

$Q = 0$ : frozen truth, filter converges [Cell 6] Statement. Truth is a fixed constant, $Q = 0$ , $R = 1$ . Start $P_{0} = 1$ . Feed three measurements. Track $P_{k}$ and $K_{k}$ .

Forecast: with no process noise, each measurement should keep shrinking $P$ and the gain, converging toward zero uncertainty.

Step 1: $P^{-} = 1 + 0 = 1$ , $K_{1} = \frac{1}{1 + 1} = 0.5$ , $P_{1} = (1 - 0.5) (1) = 0.5$ . Why? first fusion halves the variance.
Step 2: $P^{-} = 0.5$ , $K_{2} = \frac{0.5}{0.5 + 1} = 0.3333$ , $P_{2} = (1 - 0.3333) (0.5) = 0.3333$ . Why? precisions add: $\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$ . Why? $\frac{1}{P _{3}} = 2 + 1 + 1 = 4 \Rightarrow P_{3} = 0.25$ .

Verify: $P$ follows $1, \frac{1}{2}, \frac{1}{3}, \frac{1}{4} = \frac{1}{n + 1}$ and $K$ follows $\frac{1}{2}, \frac{1}{3}, \frac{1}{4}$ . Precision $1/ P$ grows by exactly $1/ R = 1$ each step — the "precisions add" magic. With $Q = 0$ the filter eventually stops listening ( $K \to 0$ ): the exact divergence trap (steel-man #4) unless $Q > 0$ .

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

Statement. Now let $Q = 0.02$ , $R = 0.5$ . The gain converges to a constant $K_{\infty}$ . Find it, and show the Kalman filter becomes a complementary filter with $α = 1 - K_{\infty}$ .

Forecast: with $Q > 0$ , $P$ can't reach zero — it settles, so $K$ settles too. Expect a fixed gain between 0 and 1.

Steady state means $P_{k} = P_{k - 1} = P^{*}$ . Substitute into $P = (1 - K) P^{-}$ with $P^{-} = P^{*} + Q$ and $K = \frac{P ^{*} + Q}{P ^{*} + Q + R}$ : $P^{*} = \frac{( P ^{*} + Q ) R}{P ^{*} + Q + R} .$ Why this step? at equilibrium the shrink from fusing exactly cancels the growth $+ Q$ .
Let $M = P^{*} + Q$ . Then $P^{*} = \frac{M R}{M + R}$ and $M = P^{*} + Q$ . Substituting: $M - Q = \frac{M R}{M + R} \Rightarrow (M - Q) (M + R) = M R$ . Expand: $M^{2} + M R - QM - QR = M R \Rightarrow M^{2} - QM - QR = 0$ . Why? a clean quadratic in $M$ — solvable exactly.
$M = \frac{Q + Q ^{2} + 4 QR}{2} = \frac{0.02 + 0.0004 + 0.04}{2} = \frac{0.02 + 0.20100}{2} = 0.11050$ . Why? take the positive root ( $M = P^{-} \geq 0$ ).
$K_{\infty} = \frac{M}{M + R} = \frac{0.11050}{0.11050 + 0.5} = 0.18100$ . Why? the converged gain.
Equivalent complementary weight: $α = 1 - K_{\infty} = 0.81900$ . Why? recall $\overset{x}{^} = (1 - K) \overset{x}{^}^{-} + K z$ mirrors $\hat{θ} = α (\dots) + (1 - α) θ_{a}$ .

Verify: plug $K_{\infty} = 0.181$ , $M = 0.1105$ back: $P^{*} = (1 - 0.181) (0.1105) = 0.0905$ , and $P^{*} + Q = 0.0905 + 0.02 = 0.1105 = M$ . ✔ Consistent. So at steady state the "optimal" Kalman is literally a fixed- $α$ complementary filter with $α \approx 0.819$ .

Worked example Example 8 — degenerate: measurement agrees with prediction [Cell 8]

Statement. $\overset{x}{^}^{-} = 3.2$ , $P^{-} = 0.4$ , $R = 0.6$ , and the measurement happens to be exactly $z = 3.2$ . Find $K$ , $\overset{x}{^}$ , $P$ .

Forecast: if the sensor confirms what we already believed, the estimate shouldn't move — but the uncertainty should still drop, because agreement is evidence.

$K = \frac{0.4}{0.4 + 0.6} = 0.4$ . Why? the gain depends only on variances, not on the value of $z$ .
$\overset{x}{^} = 3.2 + 0.4 (3.2 - 3.2) = 3.2 + 0 = 3.2$ . Why? the innovation $z - \overset{x}{^}^{-} = 0$ , so no correction term.
$P = (1 - 0.4) (0.4) = 0.24$ . Why? uncertainty still shrinks from $0.4$ to $0.24$ — agreement is informative even with zero move.

Verify: estimate unchanged ( $3.2$ ) yet $P$ fell to $0.24$ (precision $1/0.24 = 4.1 \overline{6} = 1/0.4 + 1/0.6 = 2.5 + 1.6 \overline{6}$ ). ✔ The mean can stand still while confidence still grows.

Word problems

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

Statement. A drone pitches nose-down. The gyro reads $ω_{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$ . What pitch does the complementary filter report?

Forecast: nose-down means the angle should decrease from $1 2^{\circ}$ . Both sensors agree on downward, so expect a value below $1 2^{\circ}$ , somewhere near $1 1^{\circ}$ .

$α = \frac{0.18}{0.18 + 0.02} = \frac{0.18}{0.20} = 0.9$ . Why? moderate $τ$ : trust gyro 90%, accel 10%.
Gyro prediction: $12 + (- 45) (0.02) = 12 - 0.9 = 11. 1^{\circ}$ . Why? the negative rate correctly lowers the angle — signs matter.
Fuse: $\hat{θ}_{k} = 0.9 (11.1) + 0.1 (8) = 9.99 + 0.8 = 10.7 9^{\circ}$ . Why? the accel ( $8^{\circ}$ ) nudges the estimate a little further down.

Verify: result $10.7 9^{\circ} < 1 2^{\circ}$ , nose-down as forecast, in degrees (unit consistent: $ω Δ t$ is $^{\circ} / s \cdot s =^{\circ}$ ). The tiny extra $0.3 1^{\circ}$ drop is the accel fighting any gyro bias.

Worked example Example 10 — exam twist: DESIGN

$τ$ for a target cutoff [Cell 10] Statement. You must fuse a gyro sampled at $Δ t = 0.005 s$ so that the crossover frequency (where low-pass and high-pass split trust) is $f_{c} = 2 Hz$ . (a) Find the required $τ$ . (b) Compute the resulting $α$ . (c) State which sensor dominates below $2 Hz$ .

Forecast: higher cutoff frequency ⇒ shorter time-constant ⇒ we hand over to the accel sooner ⇒ smaller-ish $τ$ , $α$ close to but below 1.

The first-order low-pass $\frac{1}{1 + τ s}$ has cutoff $ω_{c} = 1/ τ$ , and $ω_{c} = 2 π f_{c}$ . So $τ = \frac{1}{2 π f _{c}} = \frac{1}{2 π ( 2 )} = \frac{1}{12.566} = 0.07958 s$ . Why this tool? the $- 3 dB$ point of a first-order filter is exactly $1/ τ$ — that is the frequency where trust crosses over.
$α = \frac{τ}{τ + Δ t} = \frac{0.07958}{0.07958 + 0.005} = \frac{0.07958}{0.08458} = 0.94089$ . Why? convert the design $τ$ into the discrete blend weight.
Below $2 Hz$ the low-pass path passes ⇒ the accelerometer (drift-free) dominates; above it, the gyro (smooth) dominates. Why? that is the definition of the crossover we designed.

Verify: $τ = 0.0796 s$ , $α = 0.9409$ . Sanity: $1/ τ = 12.57 rad/s = 12.57/ (2 π) = 2.00 Hz$ ✔ back to the target. $α$ is close to 1 (fast sampling), consistent with the forecast.

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

The figure above places every example on one map: the complementary gain axis $α$ (Examples 1, 2, 9, 10) and the Kalman gain axis $K$ (Examples 4–8) are the same axis viewed twice, joined at the steady state (Example 7) where $α = 1 - K$ .

Recall Quick self-check

Big $R$ (noisy sensor) makes the Kalman gain do what? ::: Shrink toward 0 — trust the sensor less. With $Q = 0$ and constant truth, what happens to $P$ over many steps? ::: It shrinks toward 0 as $1/ (n + 1)$ ; the filter eventually stops listening. At steady state, the Kalman filter equals which simpler filter? ::: A complementary filter with fixed $α = 1 - K_{\infty}$ . If the measurement equals the prediction, does the estimate move? Does uncertainty drop? ::: Estimate stays put; uncertainty still drops. Higher crossover frequency $f_{c}$ needs a larger or smaller $τ$ ? ::: Smaller $τ$ , since $τ = 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.