Kalman filter derivation — predict step, update step

WHY does this exist?

In GNC you never observe the true state $x$ (position, velocity, attitude) directly. You have:

A model of how the state evolves ("if I fire the thruster, velocity increases by…") — but the model is imperfect.
Sensors (GPS, IMU, star tracker) — but they are noisy.

Neither alone is trustworthy. The Kalman filter answers: given a shaky prediction AND a shaky measurement, what is the single best estimate? The answer is a precision-weighted average, and the filter tracks not just the estimate but also how confident it is.

The core intuition: combining two Gaussians

Derivation of the 1-D fusion (from first principles)

Two independent Gaussians multiply. Their exponents add: $-\frac{(x-a)^2}{2\sigma_a^2} - \frac{(x-b)^2}{2\sigma_b^2}.$ Minimise over $x$ (the peak of a Gaussian = its mean). Differentiate, set to zero: $\frac{x-a}{\sigma_a^2} + \frac{x-b}{\sigma_b^2} = 0 \;\Rightarrow\; x\!\left(\tfrac{1}{\sigma_a^2}+\tfrac{1}{\sigma_b^2}\right) = \tfrac{a}{\sigma_a^2}+\tfrac{b}{\sigma_b^2}.$ Solve → the weighted average above. Why this step? The product of Gaussians is Gaussian, so finding its mean = finding where the log-likelihood is maximal.

Rewrite in "correction" form. Let $K = \dfrac{\sigma_a^2}{\sigma_a^2+\sigma_b^2}$ (the gain): $\hat x = a + K(b-a), \qquad \sigma^2 = (1-K)\sigma_a^2.$ This is the seed of the full Kalman update: old estimate + gain × (measurement − prediction).

PREDICT step (time update)

Mean. Take expectation of $x_k = Fx_{k-1}+Bu_k+w_k$ , with $\mathbb E[w_k]=0$ : $\boxed{\hat x_k^- = F\hat x_{k-1} + B u_k}$

Covariance. Define the prior error $e_k^- = x_k - \hat x_k^-$ . Substitute: $e_k^- = (Fx_{k-1}+Bu_k+w_k) - (F\hat x_{k-1}+Bu_k) = F e_{k-1} + w_k.$ Then $P_k^- = \mathbb E[e_k^- e_k^{-\top}] = \mathbb E[(Fe_{k-1}+w_k)(Fe_{k-1}+w_k)^\top].$ Expand. Cross terms vanish because $w_k$ is independent of the previous error: $\boxed{P_k^- = F P_{k-1} F^\top + Q}$

UPDATE step (measurement update)

We have a prior $(\hat x_k^-, P_k^-)$ and a fresh measurement $z_k$ . Correct the estimate.

Innovation (surprise): $y_k = z_k - H\hat x_k^-$ — how far the measurement is from what we predicted.

Corrected estimate: old + gain × surprise: $\hat x_k = \hat x_k^- + K_k\, y_k.$

Derive the gain $K_k$ . We choose $K_k$ to minimise the posterior error variance $\mathrm{tr}(P_k)$ .

Posterior error: $e_k = x_k - \hat x_k$ . Since $z_k = Hx_k + v_k$ : $e_k = x_k - \hat x_k^- - K_k(Hx_k + v_k - H\hat x_k^-) = (I-K_kH)e_k^- - K_k v_k.$ Compute covariance (using $e_k^-$ independent of $v_k$ ): $P_k = (I-K_kH)P_k^-(I-K_kH)^\top + K_k R K_k^\top. \quad(\star)$ This is the Joseph form (always valid, numerically stable).

Minimise $\mathrm{tr}(P_k)$ . Differentiate w.r.t. $K_k$ and set to zero (using $\partial\,\mathrm{tr}(KAK^\top)/\partial K = 2KA$ ): $\frac{\partial\,\mathrm{tr}(P_k)}{\partial K_k} = -2(I-K_kH)P_k^-H^\top + 2K_kR = 0.$ Solve: $K_k P_k^- H^\top H^{... } \Rightarrow K_k(HP_k^-H^\top + R) = P_k^-H^\top$ $\boxed{K_k = P_k^- H^\top (H P_k^- H^\top + R)^{-1}}$

Simplify covariance. Substitute the optimal $K_k$ into $(\star)$ — the $KRK^\top$ term collapses: $\boxed{P_k = (I - K_k H)P_k^-}$

Figure — Kalman filter derivation — predict step, update step

Worked examples

Worked example Example 2 — Predict step, constant velocity

State $x=\begin{bmatrix}p\\v\end{bmatrix}$ , timestep $\Delta t = 1$ s, so $F=\begin{bmatrix}1&1\\0&1\end{bmatrix}$ , no control. Start $\hat x = [0, 2]^\top$ , $P=\begin{bmatrix}1&0\\0&1\end{bmatrix}$ , $Q=0$ .

$\hat x^- = F\hat x = [0+2,\ 2]^\top = [2,2]^\top$ . Why: moved 2 m in 1 s at 2 m/s.
$P^- = FPF^\top = \begin{bmatrix}1&1\\0&1\end{bmatrix}\begin{bmatrix}1&1\\0&1\end{bmatrix}^\top = \begin{bmatrix}2&1\\1&1\end{bmatrix}$ .
Why the off-diagonal 1? Position and velocity became correlated: uncertain velocity feeds into future position. This is the filter "remembering" that a velocity error causes a position error next step.

Common mistakes

Flashcards

What are the two Kalman predict equations?

\hat x^- = F\hat x + Bu

and

P^- = FPF^\top + Q

Why does covariance grow in the predict step?

Because process noise

Q

(unmodelled dynamics) is added; prediction can only reduce confidence.

Define the innovation.

y_k = z_k - H\hat x_k^-

= measurement minus predicted measurement (the "surprise").

Write the Kalman gain.

K_k = P_k^- H^\top (HP_k^-H^\top + R)^{-1}

What is the innovation covariance

S_k

S_k = HP_k^-H^\top + R

, the uncertainty of the innovation.

What criterion determines the optimal gain?

Minimising the trace of the posterior covariance

P_k

(mean-squared error).

Posterior covariance update (optimal

K

P_k = (I - K_kH)P_k^-

R \to 0

(perfect sensor), what happens to

K

K \to H^{-1}

; the filter fully trusts the measurement.

P^- \to 0

(perfect model), what happens to

K

K \to 0

; the filter ignores the measurement.

What is the Joseph form and when must you use it?

P=(I-KH)P^-(I-KH)^\top + KRK^\top

; use it for any non-optimal

K

or for numerical stability.

The 1-D fusion of two Gaussians combines variances how?

Precisions add:

1/\sigma^2 = 1/\sigma_a^2 + 1/\sigma_b^2

Setting

Q=0

causes what failure mode?

Filter divergence —

P^-\to0

K\to0

, measurements ignored.

Recall Feynman: explain to a 12-year-old

Imagine you're guessing where your friend is walking. Your brain says "he was going that way, so he's probably here now" — but you're not sure. Then you glance and sort of see him — but your eyes are blurry too. So you make a smart guess between the two. If your glimpse was clear, you trust your eyes more; if it was really blurry, you trust your brain-guess more. The Kalman filter is a calculator that does exactly this blending — and it also keeps score of how sure it is, so next time it knows whether to trust its memory or its eyes.

Connections

State-space representation — the $(F,B,H)$ model this filter runs on.
Extended Kalman Filter (EKF) — linearise $F,H$ via Jacobians for nonlinear GNC.
Unscented Kalman Filter (UKF) — sigma points instead of linearisation.
Covariance matrices and Gaussian distributions — the statistical foundation.
Recursive Least Squares — the Kalman filter is its dynamic generalisation.
Bayesian inference — predict = prior, update = posterior via Bayes' rule.
IMU and GPS sensor fusion — the canonical GNC application.

Concept Map

Hinglish (regional understanding)

Intuition Hinglish mein samjho

Dekho, Kalman filter ka core idea bahut simple hai: tumhare paas do "guesses" hain ek hidden state (jaise position ya velocity) ke baare mein. Ek guess aata hai tumhare physics model se — "pichli baar yahan tha, itni speed thi, toh ab yahan hoga" — ye hai predict step. Doosra guess aata hai sensor se (GPS, IMU) — lekin dono hi thode galat hain, dono mein noise hai. Kalman filter dono ko uncertainty ke hisaab se weight karke ek best estimate banata hai. Jiski variance chhoti (jo zyada bharosemand), usko zyada weight milta hai.

Predict step mein tum estimate ko aage badhate ho ( $\hat x^- = F\hat x + Bu$ ) aur uncertainty badha dete ho ( $P^- = FPF^\top + Q$ ), kyunki model perfect nahi hai — isliye $+Q$ . Update step mein tum measurement lete ho, innovation nikalte ho ( $y = z - H\hat x^-$ = surprise, matlab kitna galat tha prediction), aur Kalman gain $K$ se estimate ko correct karte ho. Gain ka formula $K = P^-H^\top/S$ basically bolta hai "prior uncertainty divided by total uncertainty" — bilkul 1-D wale weighted average jaisa.

Sabse important intuition: agar sensor bahut accha hai ( $R$ chhota), $K$ bada ho jaata hai, filter measurement pe zyada bharosa karta hai. Agar model bahut accha hai ( $P^-$ chhota), $K$ chhota, filter apni prediction pe rehta hai. Aur ek common galti — kabhi $Q=0$ mat karo, warna filter sochta hai "mera model perfect hai" aur saare measurements ignore karke diverge kar jaata hai.

GNC mein ye filter har jagah use hota hai — rocket, drone, satellite ki position/attitude estimate karne ke liye. Real state kabhi directly nahi dikhta, toh Kalman filter noisy sensors aur imperfect model ko blend karke best estimate deta hai, saath mein confidence bhi track karta hai. Yaad rakho loop: Predict → Update → Predict → Update, chalta rehta hai.

Kalman filter derivation — predict step, update step

WHY does this exist?

The core intuition: combining two Gaussians

Derivation of the 1-D fusion (from first principles)

PREDICT step (time update)

UPDATE step (measurement update)

Worked examples

Common mistakes

Flashcards

Connections

Concept Map

Hinglish (regional understanding)

Go deeper — visual, from zero

Test yourself — Guidance, Navigation & Control (GNC)

Connections