Kalman filter derivation — predict step, update step

3.5.21 · Physics › Guidance, Navigation & Control (GNC)

YEH EXIST KYUN KARTA HAI?

GNC mein aap kabhi bhi true state $x$ (position, velocity, attitude) directly observe nahi kar sakte. Aapke paas hai:

State kaise evolve hoti hai iska ek model ("agar main thruster fire karun, toh velocity itni badhegi…") — lekin model imperfect hota hai.
Sensors (GPS, IMU, star tracker) — lekin woh noisy hote hain.

Akela koi bhi trustworthy nahi hai. Kalman filter ka jawab hai: ek shaky prediction AUR ek shaky measurement dono diye hon, toh sabse best estimate kya hogi? Jawab hai ek precision-weighted average, aur filter sirf estimate hi nahi balki yeh bhi track karta hai ki woh kitna confident hai.

Core intuition: do Gaussians ko combine karna

1-D fusion ki derivation (first principles se)

Do independent Gaussians multiply hote hain. Unke exponents add hote hain: $- \frac{( x - a ) ^{2}}{2 σ _{a}^{2}} - \frac{( x - b ) ^{2}}{2 σ _{b}^{2}} .$ $x$ ke upar minimise karo (Gaussian ka peak = uska mean). Differentiate karo, zero set karo: $\frac{x - a}{σ _{a}^{2}} + \frac{x - b}{σ _{b}^{2}} = 0 \Rightarrow x (\frac{1}{σ _{a}^{2}} + \frac{1}{σ _{b}^{2}}) = \frac{a}{σ _{a}^{2}} + \frac{b}{σ _{b}^{2}} .$ Solve karo → upar wala weighted average. Yeh step kyun? Gaussians ka product Gaussian hota hai, isliye uska mean dhundna = log-likelihood maximize karna.

"Correction" form mein likhte hain. $K = \frac{σ _{a}^{2}}{σ _{a}^{2} + σ _{b}^{2}}$ maano (yeh gain hai): $\overset{x}{^} = a + K (b - a), σ^{2} = (1 - K) σ_{a}^{2} .$ Yahi poore Kalman update ki seed hai: old estimate + gain × (measurement − prediction).

PREDICT step (time update)

Mean. $x_{k} = F x_{k - 1} + B u_{k} + w_{k}$ ka expectation lo, $E [w_{k}] = 0$ ke saath: $\overset{x}{^}_{k}^{-} = F \overset{x}{^}_{k - 1} + B u_{k}$

Covariance. Prior error $e_{k}^{-} = x_{k} - \overset{x}{^}_{k}^{-}$ define karo. Substitute karo: $e_{k}^{-} = (F x_{k - 1} + B u_{k} + w_{k}) - (F \overset{x}{^}_{k - 1} + B u_{k}) = F e_{k - 1} + w_{k} .$ Phir $P_{k}^{-} = E [e_{k}^{-} e_{k}^{- ⊤}] = E [(F e_{k - 1} + w_{k}) (F e_{k - 1} + w_{k})^{⊤}] .$ Expand karo. Cross terms vanish ho jaate hain kyunki $w_{k}$ previous error se independent hai: $P_{k}^{-} = F P_{k - 1} F^{⊤} + Q$

UPDATE step (measurement update)

Humare paas ek prior $(\overset{x}{^}_{k}^{-}, P_{k}^{-})$ hai aur ek fresh measurement $z_{k}$ . Estimate ko correct karo.

Innovation (surprise): $y_{k} = z_{k} - H \overset{x}{^}_{k}^{-}$ — measurement humne jo predict kiya tha usse kitni door hai.

Corrected estimate: old + gain × surprise: $\overset{x}{^}_{k} = \overset{x}{^}_{k}^{-} + K_{k} y_{k} .$

Gain $K_{k}$ derive karo. Hum $K_{k}$ choose karte hain taaki posterior error variance $tr (P_{k})$ minimise ho.

Posterior error: $e_{k} = x_{k} - \overset{x}{^}_{k}$ . Kyunki $z_{k} = H x_{k} + v_{k}$ : $e_{k} = x_{k} - \overset{x}{^}_{k}^{-} - K_{k} (H x_{k} + v_{k} - H \overset{x}{^}_{k}^{-}) = (I - K_{k} H) e_{k}^{-} - K_{k} v_{k} .$ Covariance compute karo ( $e_{k}^{-}$ independent of $v_{k}$ using): $P_{k} = (I - K_{k} H) P_{k}^{-} (I - K_{k} H)^{⊤} + K_{k} R K_{k}^{⊤} . (⋆)$ Yeh Joseph form hai (hamesha valid, numerically stable).

$tr (P_{k})$ minimise karo. $K_{k}$ ke w.r.t. differentiate karo aur zero set karo ( $\partial tr (K A K^{⊤}) / \partial K = 2 K A$ use karke): $\frac{\partial tr ( P _{k} )}{\partial K _{k}} = - 2 (I - K_{k} H) P_{k}^{-} H^{⊤} + 2 K_{k} R = 0.$ Solve karo: $K_{k} P_{k}^{-} H^{⊤} H^{...} \Rightarrow K_{k} (H P_{k}^{-} H^{⊤} + R) = P_{k}^{-} H^{⊤}$ $K_{k} = P_{k}^{-} H^{⊤} (H P_{k}^{-} H^{⊤} + R)^{- 1}$

Covariance simplify karo. Optimal $K_{k}$ ko $(⋆)$ mein substitute karo — $K R K^{⊤}$ term collapse ho jaata hai: $P_{k} = (I - K_{k} H) P_{k}^{-}$

Figure — Kalman filter derivation — predict step, update step

Worked examples

Common mistakes

Flashcards

Do Kalman predict equations kya hain?

\overset{x}{^}^{-} = F \overset{x}{^} + B u

aur

P^{-} = F P F^{⊤} + Q

Predict step mein covariance kyun badhti hai?

Kyunki process noise

Q

(unmodelled dynamics) add hoti hai; prediction sirf confidence reduce kar sakti hai.

Innovation define karo.

y_{k} = z_{k} - H \overset{x}{^}_{k}^{-}

= measurement minus predicted measurement ("surprise").

Kalman gain likho.

K_{k} = P_{k}^{-} H^{⊤} (H P_{k}^{-} H^{⊤} + R)^{- 1}

Innovation covariance

S_{k}

kya hai?

S_{k} = H P_{k}^{-} H^{⊤} + R

, innovation ki uncertainty.

Optimal gain kaunse criterion se determine hota hai?

Posterior covariance

P_{k}

ka trace minimise karna (mean-squared error).

Posterior covariance update (optimal

K

P_{k} = (I - K_{k} H) P_{k}^{-}

Agar

R \to 0

(perfect sensor), toh

K

ka kya hota hai?

K \to H^{- 1}

; filter measurement ko poora trust karta hai.

Agar

P^{-} \to 0

(perfect model), toh

K

ka kya hota hai?

K \to 0

; filter measurement ignore karta hai.

Joseph form kya hai aur ise kab use karna chahiye?

P = (I - K H) P^{-} (I - K H)^{⊤} + K R K^{⊤}

; use karo kisi bhi non-optimal

K

ke liye ya numerical stability ke liye.

Do Gaussians ka 1-D fusion variances ko kaise combine karta hai?

Precisions add hoti hain:

1/ σ^{2} = 1/ σ_{a}^{2} + 1/ σ_{b}^{2}

Q = 0

set karna kaunsa failure mode cause karta hai?

Filter divergence —

P^{-} \to 0

K \to 0

, measurements ignore ho jaate hain.

Recall Feynman: 12-saal ke bachche ko explain karo

Socho tum guess kar rahe ho tumhara dost kahan chal raha hai. Tumhara dimaag kehta hai "woh us taraf ja raha tha, toh ab probably yahan hoga" — lekin tum sure nahi ho. Phir tum ek jhalak dete ho aur thoda-sa dekhte ho — lekin tumhari aankhein bhi blurry hain. Toh tum dono ke beech mein ek smart guess karte ho. Agar tumhari jhalak saaf thi, toh aankhon pe zyada trust karo; agar bahut blurry thi, toh brain-guess pe zyada trust karo. Kalman filter ek calculator hai jo exactly yahi blending karta hai — aur yeh score bhi rakhta hai ki woh kitna sure hai, taaki agली baar yeh jaane ki memory pe trust karna hai ya aankhon pe.

Connections

State-space representation — $(F, B, H)$ model jis par yeh filter run karta hai.
Extended Kalman Filter (EKF) — nonlinear GNC ke liye $F, H$ ko Jacobians se linearise karo.
Unscented Kalman Filter (UKF) — linearisation ki jagah sigma points.
Covariance matrices and Gaussian distributions — statistical foundation.
Recursive Least Squares — Kalman filter uska dynamic generalisation hai.
Bayesian inference — predict = prior, update = Bayes' rule se posterior.
IMU and GPS sensor fusion — canonical GNC application.