Exercises — Kalman filter derivation — predict step, update step

• ${\hat{x}}_k^{-}=F{\hat{x}}_{k-1}+B{u}_k$ — predict mean: coast the estimate forward through the physics, plus the effect of any known command $u_k$.
• $P_k^{-}=F{P}_{k-1}{F}^{\top }+Q$ — predict covariance: propagate uncertainty and grow it by $Q$.
• $y_k=z_k-H{\hat{x}}_k^{-}$ — innovation: measurement minus what we expected to measure.
• $K_k=P_k^{-}{H}^{\top }{S}_k^{-1}$ — gain: the optimal blend weight, prior-uncertainty over total-uncertainty.
• ${\hat{x}}_k={\hat{x}}_k^{-}+K_k{y}_k$ and $P_k=(I-K_{k}H)P_k^{-}$ — update mean & covariance: correct the estimate and shrink the uncertainty. First two = predict; last three (innovation + gain + correction) = update.

$S=H{P}^{-}{H}^{\top }+R=1\cdot 9\cdot 1+3=12$. Why: $S$ is the total uncertainty of the surprise: the prior uncertainty seen through the sensor ($9$) plus the sensor's own noise ($3$).

$K=P^{-}{H}^{\top }{S}^{-1}=9/12=0.75$. $K>\frac{1}{2}$: the prior is more uncertain ($9$) than the sensor ($3$), so we lean toward the measurement. A perfectly-trusted-equally case would give exactly $0.5$.

• $y=z-H{\hat{x}}^{-}=26-20=6$.
• $S=1\cdot 6+2=8$.
• $K=6/8=0.75$.
• $\hat{x}=20+0.75\cdot 6=24.5$.
• $P=(1-0.75)\cdot 6=1.5$. Read it: we moved most of the way toward $26$ (gain $0.75$) and our uncertainty dropped from $6$ to $1.5$ — measuring rewarded us with confidence.

Step 1 mean: ${\hat{x}}^{-}=F[0,3{]}^{\top }=[0+3,3{]}^{\top }=[3,3{]}^{\top }$. Step 1 covariance: $FP{F}^{\top }$. First $FP=\left[\begin{array}{cc}1& 2\\ 0& 2\end{array}\right]$, then times $F^{\top }=\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]$: $P^{-}=\left[\begin{array}{cc}3& 2\\ 2& 2\end{array}\right].$ Step 2 mean: $F[3,3{]}^{\top }=[6,3{]}^{\top }$. Step 2 covariance: with $P=\left[\begin{array}{cc}3& 2\\ 2& 2\end{array}\right]$, $FP=\left[\begin{array}{cc}5& 4\\ 2& 2\end{array}\right]$, times $F^{\top }=\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]$: $P^{-}=\left[\begin{array}{cc}9& 4\\ 4& 2\end{array}\right].$ (Carefully: row 1 of $FP$ is $[5,4]$; dotting with the columns of $F^{\top }$ gives $5\cdot 1+4\cdot 1=9$ and $5\cdot 0+4\cdot 1=4$. Row 2 is $[2,2]$: $2+2=4$ and $2$.) Why the growth? With $Q=0$ the model is "perfect", yet uncertainty still grows: an uncertain velocity keeps corrupting position, and the position variance (top-left) climbs $1\to 3\to 9$ each step.

$K=P^{-}/(P^{-}+R)$.

• $R\to 0$ (perfect sensor): $K\to 1$, so $\hat{x}={\hat{x}}^{-}+1\cdot (z-{\hat{x}}^{-})=z$. Adopt the measurement fully.
• $P^{-}\to 0$ (perfect prior): $K\to 0$, so $\hat{x}={\hat{x}}^{-}$. Ignore the measurement.
• $R\to \infty$ (useless, infinitely noisy sensor): $K=P^{-}/(P^{-}+\infty )\to 0$, so again $\hat{x}={\hat{x}}^{-}$ and $P=(1-0)P^{-}=P^{-}$. The measurement is discarded and uncertainty does not shrink — a garbage reading correctly changes nothing. These three cases are the anchors every gain lives between: $K$ rides between $0$ and $1$, driven by the ratio of prior to sensor uncertainty.

Constant position, $Q=0$ ⇒ predict does nothing: $P_k^{-}=P_{k-1}$. Update: $K=\frac{P^{-}}{P^{-}+R}$, and $P_k=(1-K)P^{-}=\frac{R}{P^{-}+R}{P}^{-}=\frac{P^{-}R}{P^{-}+R}$. Take the reciprocal: $\frac{1}{P_k}=\frac{P^{-}+R}{P^{-}R}=\frac{1}{R}+\frac{1}{P^{-}}=\frac{1}{P_{k-1}}+\frac{1}{R}$. ∎ After $n$ steps: each measurement adds $1/R$ to the precision, so $\frac{1}{P_n}=\frac{1}{P_0}+\frac{n}{R}$, giving $P_n=\frac{P_{0}R}{R+n{P}_0}.$ Read it: exactly like averaging $n$ samples — variance falls like $R/n$ once the prior is swamped. Ties directly to Recursive Least Squares.

$P_3=\frac{P_{0}R}{R+3P_0}=\frac{10\cdot 2}{2+30}=\frac{20}{32}=0.625$. As $n\to \infty$, $P_n\to 0$: the filter becomes infinitely confident. Danger: with $P\to 0$ the gain $K\to 0$, so the filter stops listening to new data. If the true state ever drifts (it always does in reality), the filter cannot react — filter divergence. This is precisely why real filters keep $Q>0$.

Prediction from the picture: the measurement is much sharper (variance $1$ vs $4$), so the posterior should sit near $10$ and be taller/narrower than both. Compute: $K=4/(4+1)=0.8$; $\hat{x}=4+0.8(10-4)=8.8$; $P=(1-0.8)\cdot 4=0.8$. $\hat{x}=8.8$ is indeed close to $10$, and $P=0.8<1<4$ — narrower than both inputs. Figure description (greyscale-safe): three bell curves on a "density vs state $x$" plot. A wide, short bell centred at $x=4$ is the prior. A narrow, tall bell centred at $x=10$ is the measurement. The tallest, narrowest bell, centred at $x=8.8$ (between the two but hugging the sharp measurement), is the posterior — visually confirming that the confident input dominates and the fused peak beats both.

Predict mean: ${\hat{x}}^{-}=F[0,1{]}^{\top }=[1,1{]}^{\top }$. Predict cov: $FP{F}^{\top }=\left[\begin{array}{cc}2& 1\\ 1& 1\end{array}\right]$ (as in the parent Example 2). Innovation: $H{\hat{x}}^{-}=1$, so $y=3-1=2$. $S$: $H{P}^{-}{H}^{\top }+R$. Here $H{P}^{-}{H}^{\top }=P_{11}^{-}=2$, so $S=2+1=3$. Gain: $K=P^{-}{H}^{\top }{S}^{-1}$. $P^{-}{H}^{\top }=\left[\begin{array}{c}2\\ 1\end{array}\right]$ (first column), so $K=\frac{1}{3}\left[\begin{array}{c}2\\ 1\end{array}\right]=\left[\begin{array}{c}0.667\\ 0.333\end{array}\right]$. Corrected mean: $\hat{x}=[1,1{]}^{\top }+\left[\begin{array}{c}0.667\\ 0.333\end{array}\right]\cdot 2=[2.333,1.667{]}^{\top }$. Covariance: $P=(I-KH)P^{-}$. Here $KH=\left[\begin{array}{cc}0.667& 0\\ 0.333& 0\end{array}\right]$, so $I-KH=\left[\begin{array}{cc}0.333& 0\\ -0.333& 1\end{array}\right]$; multiplying by $P^{-}$: $P=\left[\begin{array}{cc}0.667& 0.333\\ 0.333& 0.667\end{array}\right].$ The magic: we measured position only, yet the velocity estimate also changed ($1\to 1.667$) and its variance dropped ($1\to 0.667$). The off-diagonal correlation from the predict step let a position measurement inform velocity. This is IMU and GPS sensor fusion in miniature.

Short form: $P=(1-0.75)\cdot 6=1.5$. Joseph: $(1-0.75{)}^2\cdot 6+0.75^2\cdot 2=0.0625\cdot 6+0.5625\cdot 2=0.375+1.125=1.5$. ✔ They agree because $K=0.75$ is the optimal gain; the cross-terms cancel exactly. For any non-optimal $K$ they would differ, and only Joseph stays correct.

Mechanism: with $Q=0$, $P_n=\frac{P_{0}R}{R+n{P}_0}\to 0$. Then $K_n=P_n^{-}/(P_n^{-}+R)\to 0$, so ${\hat{x}}_k={\hat{x}}_k^{-}+K_k{y}_k\approx {\hat{x}}_k^{-}$: incoming measurements are multiplied by nearly zero and ignored. The filter is certain of an out-of-date value — divergence. Fix: set $Q>0$. This adds a floor to $P^{-}$ every predict step, keeping $K$ from collapsing to $0$, so the filter stays responsive to drift. $Q$ encodes "the model may be slightly wrong" — exactly the humility a real system needs. (Formally this connects to Bayesian inference: $Q$ re-inflates the prior each step.)

Combine: $P^{-}=P+Q$, then $P=\frac{P^{-}R}{P^{-}+R}$. At steady state let $p=P$: $p=\frac{(p+Q)R}{(p+Q)+R}.$ Multiply out: $p(p+Q+R)=(p+Q)R\Rightarrow p^2+p(Q+R)=pR+QR\Rightarrow p^2+pQ-QR=0.$ With $Q=1,R=3$: $p^2+p-3=0\Rightarrow p=\frac{-1+\sqrt{1+12}}{2}=\frac{-1+\sqrt{13}}{2}\approx 1.3028$. Read it: unlike the $Q=0$ case ($P\to 0$), here $P$ settles at a positive constant $\approx 1.30$. The filter reaches a stable, non-zero confidence — exactly what keeps it alive and tracking.

Start large: $P_0$ big ⇒ first $K$ near $1$ (trust the sensor almost fully while blind). As measurements pour in, $P$ falls toward $P_{\infty }\approx 1.30$, so $K$ falls and settles at $K_{\infty }=P_{\infty }^{-}/(P_{\infty }^{-}+R)$ where $P_{\infty }^{-}=P_{\infty }+Q\approx 2.30$, giving $K_{\infty }=2.30/5.30\approx 0.434$. Figure description (greyscale-safe): a "gain $K$ vs update step $k$" plot. The main curve (circles) for $Q=1$ starts high near $1$, drops steeply over the first few steps, then flattens onto a horizontal dotted line at $K\approx 0.434$ — monotone decreasing to a positive floor, never reaching $0$. A second dashed curve (squares) for $Q=0$ keeps sagging toward $0$, illustrating the dead, unresponsive filter. The positive floor is the gift of $Q>0$.