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

Quick symbol reminder so you never guess:

Definition Symbols used on this page

$\hat{θ}_{k}$ — our estimate of the angle at step $k$ (the "hat" means estimate, not the true value).
$ω_{k}$ — measured angular rate from the gyro (degrees per second).
$θ_{a, k}$ — the absolute angle read from the accel at step $k$ .
$Δ t$ — the sample interval, seconds between two ticks.
$α$ — the fixed blend weight of the complementary filter, a pure number in $(0, 1)$ .
$τ$ — the filter time constant, seconds; sets where "slow" turns to "fast".
$x, \overset{x}{^}^{-}, \overset{x}{^}$ — true state, predicted estimate (minus = before measurement), updated estimate.
$P$ — the variance of our estimate: "how wrong am I likely to be, squared". Big $P$ = unsure.
$Q$ — variance of the process noise (how much reality wanders each step).
$R$ — variance of the measurement noise (how jumpy the sensor is).
$K$ — the Kalman gain, the self-tuning blend weight.
$z_{k}$ — the raw sensor reading at step $k$ .

Level 1 — Recognition

L1·1 — Name the weight

The complementary filter update is $\hat{θ}_{k} = α (\hat{θ}_{k - 1} + ω_{k} Δ t) + (1 - α) θ_{a, k} .$ With $τ = 0.49 s$ and $Δ t = 0.01 s$ , compute $α$ . Which sensor does this value trust more?

Recall Solution

$α = \frac{τ}{τ + Δ t} = \frac{0.49}{0.49 + 0.01} = \frac{0.49}{0.50} = 0.98$ . WHAT this means: $0.98$ multiplies the gyro prediction, so we trust the gyro heavily this instant. The remaining $1 - α = 0.02$ gently pulls toward the accel — enough to erase long-term drift, small enough to reject fast accel noise.

L1·2 — Read the Kalman gain

In one Kalman step the prior variance is $P^{-} = 0.9$ and the measurement noise is $R = 0.3$ . Compute the gain $K$ and say, in words, what a gain of that size does to a measurement.

Recall Solution

$K = \frac{P ^{-}}{P ^{-} + R} = \frac{0.9}{0.9 + 0.3} = \frac{0.9}{1.2} = 0.75$ . WHAT it does: the update is $\overset{x}{^} = \overset{x}{^}^{-} + K (z - \overset{x}{^}^{-})$ , so we move 75% of the way from our prior toward the new reading $z$ . WHY so much? Because the prior ( $P^{-} = 0.9$ ) is more uncertain than the sensor ( $R = 0.3$ ), so the sensor deserves the louder vote.

Level 2 — Application

L2·1 — One complementary step

Given $Δ t = 0.02 s$ , $τ = 0.38 s$ , previous estimate $\hat{θ}_{k - 1} = 2 0^{\circ}$ , gyro rate $ω_{k} = - 3 0^{\circ} / s$ , accel angle $θ_{a, k} = 1 8^{\circ}$ . Find $\hat{θ}_{k}$ .

Recall Solution

Step A — WHAT: build $α$ . $α = \frac{0.38}{0.38 + 0.02} = \frac{0.38}{0.40} = 0.95$ . Step B — WHY predict first: integrate the rate to guess where the angle went. Gyro prediction $= 20 + (- 30) (0.02) = 20 - 0.6 = 19. 4^{\circ}$ . Step C — Fuse: $\hat{θ}_{k} = 0.95 (19.4) + 0.05 (18) = 18.43 + 0.9 = 19.3 3^{\circ}$ . WHAT it looks like: we mostly followed the gyro's downward swing, then the accel tugged us a hair further down (since $18 < 19.4$ ).

L2·2 — One full Kalman cycle

Start $\overset{x}{^}_{0} = 5$ , $P_{0} = 2$ , with $Q = 0.04$ , $R = 1$ , and a reading $z_{1} = 8$ . Run predict → gain → update → variance.

Recall Solution

Predict. $\overset{x}{^}^{-} = 5$ (no motion model but the state); $P^{-} = P_{0} + Q = 2 + 0.04 = 2.04$ . WHY $+ Q$ : uncertainty always grows between measurements. Gain. $K = \frac{2.04}{2.04 + 1} = \frac{2.04}{3.04} = 0.6711$ . Update. $\overset{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: the estimate leapt two-thirds of the way to the reading, and its variance dropped from $2.04$ to $0.67$ — below both the prior and the sensor. Fusion always sharpens.

Level 3 — Analysis

L3·0 — Limiting cases of $α$ : where does trust go?

Using $α = \frac{τ}{τ + Δ t}$ , describe what happens to the complementary filter in the two extremes $Δ t \to 0$ (with $τ$ fixed) and $Δ t ≫ τ$ .

Recall Solution

Case $Δ t \to 0$ (or $Δ t ≪ τ$ ): $α = \frac{τ}{τ + Δ t} \to \frac{τ}{τ} = 1$ . WHAT it looks like: $\hat{θ}_{k} \to \hat{θ}_{k - 1} + ω_{k} Δ t$ — pure gyro integration, the accel term $(1 - α) θ_{a} \to 0$ . WHY: with ticks arriving arbitrarily fast, only a whisker of drift builds between corrections, so the smooth gyro rules and the noisy accel is ignored. This is the high-frequency / short-term regime. Case $Δ t ≫ τ$ : $α = \frac{τ}{τ + Δ t} \to 0$ . WHAT it looks like: $\hat{θ}_{k} \to θ_{a, k}$ — the estimate just copies the accel; the gyro prediction is discarded. WHY: over a long gap the gyro's drift has grown huge, so we throw away the stale prediction and re-anchor to the drift-free accel. This is the low-frequency / long-term regime. The knob $τ$ slides you between these poles: large $τ$ (relative to $Δ t$ ) → $α \to 1$ → gyro-dominated; small $τ$ → $α \to 0$ → accel-dominated. The crossover sits exactly where $Δ t = τ$ , giving $α = \frac{1}{2}$ .

L3·1 — The steady-state Kalman gain

Let the filter run forever until $P$ stops changing: call the settled value $P_{\infty}$ . It satisfies $P_{\infty} = (1 - K) (P_{\infty} + Q), K = \frac{P _{\infty} + Q}{P _{\infty} + Q + R} .$ Take $Q = 0.1$ , $R = 1$ . Find $P_{\infty}$ and the steady gain $K_{\infty}$ .

Recall Solution

WHAT we do: at steady state $P_{k} = P_{k - 1} = P_{\infty}$ . Substitute the gain into $P = (1 - K) P^{-}$ with $P^{-} = P_{\infty} + Q$ . Step 1 — assemble the fixed-point equation. Using $K = \frac{P ^{-}}{P ^{-} + R}$ , the updated variance is $P = (1 - K) P^{-} = \frac{P ^{-} R}{P ^{-} + R}$ (the " $precisions add$ " form). At steady state $P = P_{\infty}$ and $P^{-} = P_{\infty} + Q$ , so $P_{\infty} = \frac{( P _{\infty} + Q ) R}{( P _{\infty} + Q ) + R} .$ Step 2 — clear the denominator. Multiply both sides by $((P_{\infty} + Q) + R)$ . WHY: to turn a fraction into a polynomial we can solve. $P_{\infty} ((P_{\infty} + Q) + R) = (P_{\infty} + Q) R .$ Step 3 — expand each side. 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 — cancel the shared term $P_{\infty} R$ from both sides (it appears identically), leaving a clean quadratic: $P_{\infty}^{2} + P_{\infty} Q - QR = 0.$ Step 5 — solve the quadratic. With $Q = 0.1, R = 1$ : $P_{\infty}^{2} + 0.1 P_{\infty} - 0.1 = 0$ . The quadratic formula gives the two roots $P_{\infty} = \frac{- 0.1 \pm 0.01 + 0.4}{2} = \frac{- 0.1 \pm 0.6403}{2} .$ Step 6 — pick the physically valid root. WHY we discard the $(-)$ root: $P_{\infty}$ is a variance, and a variance is a squared spread — it can never be negative. The $(-)$ root gives $\frac{- 0.1 - 0.6403}{2} = - 0.3701 < 0$ , which is impossible for a variance, so we reject it. The $(+)$ root is $P_{\infty} = \frac{- 0.1 + 0.6403}{2} = 0.2701.$ Then $S = P_{\infty} + Q = 0.3701$ , and $K_{\infty} = \frac{0.3701}{0.3701 + 1} = 0.2701$ . WHAT it means: once settled, the Kalman gain is a constant $0.2701$ — meaning a settled Kalman filter is a complementary filter with $1 - α = K_{\infty}$ .

L3·2 — Precisions add

Two independent estimates of the same angle: one with variance $P^{-} = 0.4$ , one (a measurement) with variance $R = 0.6$ . Show numerically that the fused variance obeys $\frac{1}{P} = \frac{1}{P ^{-}} + \frac{1}{R}$ , and compare to what naive averaging would give.

Recall Solution

Kalman way. $K = \frac{0.4}{0.4 + 0.6} = 0.4$ , so $P = (1 - 0.4) (0.4) = 0.24$ . Check precision rule. $\frac{1}{P ^{-}} + \frac{1}{R} = 2.5 + 1.6667 = 4.1667$ , and $\frac{1}{0.24} = 4.1667$ . ✓ Naive average ( $\frac{1}{2}$ each). Averaging two independent estimates, the variance of the average is $(\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$ — worse than the optimal $0.24$ . WHY: equal weighting ignores that the first source is more precise. The optimal weight leans toward the sharper source, squeezing variance lower. Information (=inverse variance) simply adds.

Level 4 — Synthesis

L4·1 — Match a complementary $α$ to a Kalman steady state

An engineer built a complementary filter with $α = 0.98$ at $Δ t = 0.01 s$ . She wants a Kalman filter that, at steady state, behaves identically. If her sensor has $R = 1$ , what process noise $Q$ gives a steady Kalman gain $K_{\infty} = 1 - α$ ?

Recall Solution

WHAT the match requires: $K_{\infty} = 1 - α = 0.02$ . Recall from L3·1 the settled relations $P_{\infty}^{2} + P_{\infty} Q - QR = 0$ and $K_{\infty} = \frac{P _{\infty} + Q}{P _{\infty} + Q + R}$ . From the gain: $K_{\infty} = \frac{S}{S + R}$ with $S = P_{\infty} + Q$ , so $S = \frac{K _{\infty} R}{1 - K _{\infty}} = \frac{0.02 \cdot 1}{0.98} = 0.020408$ . Also $P_{\infty} = (1 - K_{\infty}) S = 0.98 \times 0.020408 = 0.02$ . Then $Q = S - P_{\infty} = 0.020408 - 0.02 = 0.000408$ . WHAT it means: a Kalman filter with tiny $Q = 4.08 \times 1 0^{- 4}$ (and $R = 1$ ) locks to the same $2%$ blend her complementary filter uses. The two filters are the same machine at steady state — Kalman just also handles the noisy start-up transient.

L4·2 — Convergence curve of $P$

Starting from $P_{0} = 4$ , $Q = 0.1$ , $R = 1$ , list $P_{1}, P_{2}, P_{3}$ using $P^{-} = P + Q$ , $K = \frac{P ^{-}}{P ^{-} + R}$ , $P_{new} = (1 - K) P^{-}$ . Confirm it heads toward the $P_{\infty}$ from L3·1.

Recall Solution

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 (see figure below): $P$ plunges $4 \to 0.80 \to 0.47 \to 0.37 \to \dots$ , settling toward $P_{\infty} = 0.2701$ . The gain rides down with it. That falling curve is exactly why Kalman beats a fixed $α$ : early on it grabs measurements hard (high $K$ ), then relaxes as confidence builds.

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

Figure — what to observe (alt-text): A blueprint-style plot with step number $k$ on the horizontal axis and value on the vertical axis. The cyan curve with circles is the estimate variance $P_{k}$ : it starts high at $P_{0} = 4$ and drops steeply — $4 \to 0.80 \to 0.47 \to 0.37$ — flattening toward the dashed white line marked $P_{\infty} = 0.270$ . The amber curve with squares is the Kalman gain $K_{k}$ : it starts high (near $0.80$ , "grabs measurements hard early") and settles toward $0.27$ ("relaxes as it grows confident"). What you should take away: both the uncertainty and the trust-in-measurement shrink together and level off at the steady-state value computed in L3·1 — this converged, frozen gain is precisely the fixed $α$ of a complementary filter.

Level 5 — Mastery

L5·1 — Derive the optimal gain from scratch

The estimate is a weighted correction $\overset{x}{^} = \overset{x}{^}^{-} + K (z - \overset{x}{^}^{-})$ , with prior error variance $P^{-}$ and independent measurement noise variance $R$ . Prove that the $K$ minimising the posterior variance is $K = \frac{P ^{-}}{P ^{-} + R}$ , and that then $P = (1 - K) P^{-}$ .

Recall Solution

Step A — WHAT: write the error. Let the true value be $x$ . Prior error $e^{-} = \overset{x}{^}^{-} - x$ has variance $P^{-}$ ; measurement error $z - x = v$ has variance $R$ , independent of $e^{-}$ . Then $\overset{x}{^} - x = (\overset{x}{^}^{-} - x) + K ((z - x) - (\overset{x}{^}^{-} - x)) = (1 - K) e^{-} + K v .$ Step B — WHY variances add: $e^{-}$ and $v$ are independent, so their variances add with the squared coefficients: $P = Var (\overset{x}{^} - x) = (1 - K)^{2} P^{-} + K^{2} R .$ Step C — Minimise. WHY a derivative? $P$ is a smooth upward parabola in $K$ ; its lowest point has zero slope. $\frac{d P}{d K} = - 2 (1 - K) P^{-} + 2 K R = 0 \Rightarrow (1 - K) P^{-} = K R \Rightarrow K = \frac{P ^{-}}{P ^{-} + R} .$ Step D — Back-substitute. $P = (1 - K)^{2} P^{-} + K^{2} R$ . Since $(1 - K) P^{-} = K R$ , multiply by $(1 - K)$ : $(1 - K)^{2} P^{-} = (1 - K) K R$ , so $P = (1 - K) K R + K^{2} R = K R [(1 - K) + K] = K R = \frac{P ^{-} R}{P ^{-} + R} = (1 - K) P^{-} .$ WHAT it means: the " $precisions add$ " law $\frac{1}{P} = \frac{1}{P ^{-}} + \frac{1}{R}$ follows directly, and the gain is nothing more than "trust each source in proportion to its precision."

L5·2 — Complementary filter has unity DC gain

Show that $H_{lp} (s) = \frac{1}{1 + τ s}$ and $H_{hp} (s) = \frac{τ s}{1 + τ s}$ sum to $1$ for all $s$ , and evaluate each at $s = 0$ (DC, a constant input) and as $s \to \infty$ (very fast input). Interpret.

Recall Solution

Sum. $H_{lp} + H_{hp} = \frac{1 + τ s}{1 + τ s} = 1$ for every $s$ . WHY this matters: whatever the frequency, the two paths' weights always total $1$ , so no scaling error can creep in. DC, $s = 0$ : $H_{lp} = 1$ , $H_{hp} = 0$ . A constant angle is passed entirely by the accel path — which is drift-free, exactly what we want for long-term truth. Fast, $s \to \infty$ : $H_{lp} \to 0$ , $H_{hp} \to 1$ . Quick wiggles pass entirely through the gyro path — smooth and noise-free short-term. WHAT it means: each sensor rules the band where it is trustworthy, and because the transfer functions sum to $1$ , a steady input keeps its exact size (unity DC gain) — no steady-state bias. This is the frequency-domain mirror of the $Δ t$ limits in L3·0: DC ↔ $Δ t ≫ τ$ (accel), high frequency ↔ $Δ t \to 0$ (gyro).

Recall Self-test cloze — cover and recall

The complementary blend weight is ::: $α = \frac{τ}{τ + Δ t}$ The Kalman gain is ::: $K = \frac{P ^{-}}{P ^{-} + R}$ Predict grows variance by adding ::: the process noise $Q$ , giving $P^{-} = P_{k - 1} + Q$ A larger $R$ makes the filter trust the measurement ::: less (smaller $K$ ) Fusing two Gaussians makes their ::: precisions (inverse variances) add As $Δ t \to 0$ the complementary filter trusts ::: the gyro fully ( $α \to 1$ ) As $Δ t ≫ τ$ the complementary filter trusts ::: the accel fully ( $α \to 0$ ) Before blending angles you must ::: wrap the difference into $(- 18 0^{\circ}, 18 0^{\circ}]$ (shortest arc) A steady-state Kalman filter is equivalent to a ::: complementary filter with fixed gain $K_{\infty} = 1 - α$

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