Kalman filter derivation — predict step, update step: Exercises

3.5.21 · D4 · Physics › Guidance, Navigation & Control (GNC) › Kalman filter derivation — predict step, update step

Un paanch equations ki reminder jo tum use karoge (sab parent mein define hain):

Formula Paanch lines

Predict: $\overset{x}{^}_{k}^{-} = F \overset{x}{^}_{k - 1} + B u_{k}$ , aur $P_{k}^{-} = F P_{k - 1} F^{⊤} + Q$ . Update: $y_{k} = z_{k} - H \overset{x}{^}_{k}^{-}$ ; $S_{k} = H P_{k}^{-} H^{⊤} + R$ ; $K_{k} = P_{k}^{-} H^{⊤} S_{k}^{- 1}$ ; $\overset{x}{^}_{k} = \overset{x}{^}_{k}^{-} + K_{k} y_{k}$ ; $P_{k} = (I - K_{k} H) P_{k}^{-}$ . Yahan $\overset{x}{^}$ estimate hai, $P$ uski covariance (spread/uncertainty), $F$ state-transition matrix hai, $H$ observation matrix hai, $Q$ / $R$ process/measurement noise covariances hain, $K$ Kalman gain hai (blend weight). Term $u_{k}$ known control input hai — ek command jo tumne system ko bheji (jaise "thruster fire karo", "yeh acceleration apply karo"), aur $B$ us command ko map karta hai ki state kaise badlegi. Yeh bilkul known hai (tumne khud choose kiya), isliye predicted mean ko shift karta hai par koi uncertainty add nahi karta. Neeche ke har exercise mein $u_{k} = 0$ hai (koi commanded input nahi), isliye yeh term gayab ho jaati hai.

Level 1 — Recognition

Exercise 1.1

Apne words mein batao ki paanch filter equations mein se har ek kya karti hai. Kaun si do predict mein hain aur kaun si teen update mein?

Recall Solution

$\overset{x}{^}_{k}^{-} = F \overset{x}{^}_{k - 1} + B u_{k}$ — predict mean: estimate ko physics ke through aage coast karo, plus kisi bhi known command $u_{k}$ ka effect.
$P_{k}^{-} = F P_{k - 1} F^{⊤} + Q$ — predict covariance: uncertainty ko propagate karo aur $Q$ se badhao.
$y_{k} = z_{k} - H \overset{x}{^}_{k}^{-}$ — innovation: measurement minus jo measure hone ki humein expectation thi.
$K_{k} = P_{k}^{-} H^{⊤} S_{k}^{- 1}$ — gain: optimal blend weight, prior-uncertainty over total-uncertainty.
$\overset{x}{^}_{k} = \overset{x}{^}_{k}^{-} + K_{k} y_{k}$ aur $P_{k} = (I - K_{k} H) P_{k}^{-}$ — update mean & covariance: estimate ko correct karo aur uncertainty ko ghataao. Pehli do = predict; aakhri teen (innovation + gain + correction) = update.

Exercise 1.2

1-D scalar filter, $H = 1$ . Prior $P^{-} = 9$ , measurement noise $R = 3$ . Innovation covariance $S$ compute karo.

Recall Solution

$S = H P^{-} H^{⊤} + R = 1 \cdot 9 \cdot 1 + 3 = 12$ . Kyun: $S$ surprise ki total uncertainty hai: sensor ke through dekhi gayi prior uncertainty ( $9$ ) plus sensor ka apna noise ( $3$ ).

Exercise 1.3

Same numbers ( $P^{-} = 9$ , $R = 3$ , $H = 1$ ). Gain $K$ compute karo. Kya yeh $\frac{1}{2}$ se bada hai ya chhota, aur yeh tumhe kya batata hai?

Recall Solution

$K = P^{-} H^{⊤} S^{- 1} = 9/12 = 0.75$ . $K > \frac{1}{2}$ : prior zyada uncertain hai ( $9$ ) sensor se ( $3$ ), isliye hum measurement ki taraf jhuk rahe hain. Bilkul equally-trusted case mein exactly $0.5$ milta.

Level 2 — Application

Exercise 2.1

1-D constant-position filter. Prior $\overset{x}{^}^{-} = 20$ , $P^{-} = 6$ . Measurement $z = 26$ , $R = 2$ , $H = 1$ . Ek full update run karo: $y$ , $S$ , $K$ , $\overset{x}{^}$ , $P$ nikalo.

Recall Solution

$y = z - H \overset{x}{^}^{-} = 26 - 20 = 6$ .
$S = 1 \cdot 6 + 2 = 8$ .
$K = 6/8 = 0.75$ .
$\overset{x}{^} = 20 + 0.75 \cdot 6 = 24.5$ .
$P = (1 - 0.75) \cdot 6 = 1.5$ . Padho: hum zyaatar $26$ ki taraf move kar gaye (gain $0.75$ ) aur uncertainty $6$ se $1.5$ tak gir gayi — measuring ne humein confidence diya.

Exercise 2.2

Do-step predict only, constant velocity, $Δ t = 1$ , $F = [1011]$ , $u_{k} = 0$ (koi command nahi), $Q = 0$ . Shuru karo $\overset{x}{^} = [0, 3]^{⊤}$ , $P = [1002]$ se. Ek baar predict karo, phir dobara. Har ek ke baad $\overset{x}{^}^{-}$ aur $P^{-}$ do.

Recall Solution

Step 1 mean: $\overset{x}{^}^{-} = F [0, 3]^{⊤} = [0 + 3, 3]^{⊤} = [3, 3]^{⊤}$ . Step 1 covariance: $F P F^{⊤}$ . Pehle $F P = [1022]$ , phir $F^{⊤} = [1101]$ se multiply: $P^{-} = [3222] .$ Step 2 mean: $F [3, 3]^{⊤} = [6, 3]^{⊤}$ . Step 2 covariance: $P = [3222]$ ke saath, $F P = [5242]$ , $F^{⊤} = [1101]$ se multiply: $P^{-} = [9442] .$ (Dhyan se: $F P$ ki row 1 hai $[5, 4]$ ; $F^{⊤}$ ke columns se dot karne par $5 \cdot 1 + 4 \cdot 1 = 9$ aur $5 \cdot 0 + 4 \cdot 1 = 4$ milta hai. Row 2 hai $[2, 2]$ : $2 + 2 = 4$ aur $2$ .) Kyun badhti hai? $Q = 0$ ke saath model "perfect" hai, phir bhi uncertainty badhti hai: uncertain velocity position ko corrupt karti rehti hai, aur position variance (top-left) har step $1 \to 3 \to 9$ badhti hai.

Exercise 2.3

Gain ke "collapse" limits dikhao. Scalar $H = 1$ ke liye: $R \to 0$ par $K$ kya hai? $P^{-} \to 0$ par? Aur $R \to \infty$ par? Har case mein resulting $\overset{x}{^}$ do.

Recall Solution

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

$R \to 0$ (perfect sensor): $K \to 1$ , isliye $\overset{x}{^} = \overset{x}{^}^{-} + 1 \cdot (z - \overset{x}{^}^{-}) = z$ . Measurement ko poora adopt karo.
$P^{-} \to 0$ (perfect prior): $K \to 0$ , isliye $\overset{x}{^} = \overset{x}{^}^{-}$ . Measurement ignore karo.
$R \to \infty$ (useless, infinitely noisy sensor): $K = P^{-} / (P^{-} + \infty) \to 0$ , isliye phir $\overset{x}{^} = \overset{x}{^}^{-}$ aur $P = (1 - 0) P^{-} = P^{-}$ . Measurement discard ho jaati hai aur uncertainty shrink nahi hoti — ek garbage reading sahi taur par kuch nahi badlati. Yeh teen cases woh anchors hain jis beech har gain rehta hai: $K$ , $0$ aur $1$ ke beech ride karta hai, prior aur sensor uncertainty ke ratio se driven.

Level 3 — Analysis

Exercise 3.1

Ek scalar constant-position filter ke liye no process noise ( $Q = 0$ ), $H = 1$ , constant $R$ ke saath, prove karo ki posterior variance recursion follow karta hai $\frac{1}{P _{k}} = \frac{1}{P _{k - 1}} + \frac{1}{R}$ (precisions add hote hain). Phir prior $P_{0}$ se shuru karke $n$ identical measurements ke baad $P_{k}$ nikalo.

Recall Solution

Constant position, $Q = 0$ ⇒ predict kuch nahi karta: $P_{k}^{-} = P_{k - 1}$ . Update: $K = \frac{P ^{-}}{P ^{-} + R}$ , aur $P_{k} = (1 - K) P^{-} = \frac{R}{P ^{-} + R} P^{-} = \frac{P ^{-} R}{P ^{-} + R}$ . Reciprocal lo: $\frac{1}{P _{k}} = \frac{P ^{-} + R}{P ^{-} R} = \frac{1}{R} + \frac{1}{P ^{-}} = \frac{1}{P _{k - 1}} + \frac{1}{R}$ . ∎ $n$ steps ke baad: har measurement precision mein $1/ R$ add karta hai, isliye $\frac{1}{P _{n}} = \frac{1}{P _{0}} + \frac{n}{R}$ , jo deta hai $P_{n} = \frac{P _{0} R}{R + n P _{0}} .$ Padho: bilkul $n$ samples average karne jaisa — variance $R / n$ ki tarah girta hai jab prior overwhelm ho jaata hai. Seedha Recursive Least Squares se connect hota hai.

Exercise 3.2

Numeric: prior $P_{0} = 10$ , $R = 2$ , $Q = 0$ , constant position. Ex 3.1 use karke, $P_{3}$ compute karo (teen measurements). Phir argue karo kyun $P_{n} \to 0$ aur isme kaun sa real-world danger chhupta hai.

Recall Solution

$P_{3} = \frac{P _{0} R}{R + 3 P _{0}} = \frac{10 \cdot 2}{2 + 30} = \frac{20}{32} = 0.625$ . Jaise $n \to \infty$ , $P_{n} \to 0$ : filter infinitely confident ho jaata hai. Danger: $P \to 0$ ke saath gain $K \to 0$ , isliye filter nayi data sunna band kar deta hai. Agar sach mein state drift kare (real life mein hoti hai), filter react nahi kar sakta — filter divergence. Isliye real filters $Q > 0$ rakhte hain.

Exercise 3.3 — figure

Ek scalar update prior $N (\overset{x}{^}^{-} = 4, P^{-} = 4)$ ko measurement likelihood $N (z = 10, R = 1)$ ke saath fuse karta hai. Predict karo ki posterior mean kahan hogi ( $4$ ke paas ya $10$ ke paas?) aur kya uska peak dono se uuncha hoga. Phir confirm karne ke liye $\overset{x}{^}$ aur $P$ compute karo.

Figure — Kalman filter derivation — predict step, update step

Recall Solution

Picture se prediction: measurement bahut sharper hai (variance $1$ vs $4$ ), isliye posterior $10$ ke paas honi chahiye aur dono se uunchi/narrow honi chahiye. Compute karo: $K = 4/ (4 + 1) = 0.8$ ; $\overset{x}{^} = 4 + 0.8 (10 - 4) = 8.8$ ; $P = (1 - 0.8) \cdot 4 = 0.8$ . $\overset{x}{^} = 8.8$ sach mein $10$ ke paas hai, aur $P = 0.8 < 1 < 4$ — dono inputs se narrow. Figure description (greyscale-safe): "density vs state $x$ " plot par teen bell curves. $x = 4$ par centred ek wide, short bell prior hai. $x = 10$ par centred ek narrow, tall bell measurement hai. Tallest, narrowest bell, $x = 8.8$ par centred (dono ke beech par sharp measurement ki taraf jhuka hua), posterior hai — visually confirm karta hai ki confident input dominate karti hai aur fused peak dono ko beat karta hai.

Level 4 — Synthesis

Exercise 4.1

Full predict + update cycle, constant velocity. $F = [1011]$ , $u_{k} = 0$ , $Q = 0$ . Hum sirf position measure karte hain: $H = [10]$ , $R = 1$ . Start $\overset{x}{^} = [0, 1]^{⊤}$ , $P = [1001]$ se. Ek predict ke baad position measurement $z = 3$ aati hai. $\overset{x}{^}^{-}, P^{-}, y, S, K, \overset{x}{^}, P$ compute karo.

Recall Solution

Predict mean: $\overset{x}{^}^{-} = F [0, 1]^{⊤} = [1, 1]^{⊤}$ . Predict cov: $F P F^{⊤} = [2111]$ (parent Example 2 jaisa). Innovation: $H \overset{x}{^}^{-} = 1$ , isliye $y = 3 - 1 = 2$ . $S$ : $H P^{-} H^{⊤} + R$ . Yahan $H P^{-} H^{⊤} = P_{11}^{-} = 2$ , isliye $S = 2 + 1 = 3$ . Gain: $K = P^{-} H^{⊤} S^{- 1}$ . $P^{-} H^{⊤} = [21]$ (pehla column), isliye $K = \frac{1}{3} [21] = [0.667 0.333]$ . Corrected mean: $\overset{x}{^} = [1, 1]^{⊤} + [0.667 0.333] \cdot 2 = [2.333, 1.667]^{⊤}$ . Covariance: $P = (I - K H) P^{-}$ . Yahan $K H = [0.667 0.333 00]$ , isliye $I - K H = [0.333 - 0.333 01]$ ; $P^{-}$ se multiply karne par: $P = [0.667 0.333 0.333 0.667] .$ Jaadu yeh hai: humne sirf position measure ki, phir bhi velocity estimate bhi badli ( $1 \to 1.667$ ) aur uski variance bhi giri ( $1 \to 0.667$ ). Predict step ki off-diagonal correlation ne position measurement ko velocity inform karne diya. Yeh choti si IMU and GPS sensor fusion hai.

Exercise 4.2

Verify karo ki Joseph form $P_{k} = (I - K H) P^{-} (I - K H)^{⊤} + K R K^{⊤}$ wahi $P$ deta hai jo short form $(I - K H) P^{-}$ deta hai jab $K$ optimal ho, Ex 2.1 ke scalar numbers use karke ( $P^{-} = 6, R = 2, K = 0.75, H = 1$ ).

Recall Solution

Short form: $P = (1 - 0.75) \cdot 6 = 1.5$ . Joseph: $(1 - 0.75)^{2} \cdot 6 + 0.7 5^{2} \cdot 2 = 0.0625 \cdot 6 + 0.5625 \cdot 2 = 0.375 + 1.125 = 1.5$ . ✔ Dono agree karte hain kyunki $K = 0.75$ optimal gain hai; cross-terms exactly cancel ho jaati hain. Kisi bhi non-optimal $K$ ke liye yeh differ karte, aur sirf Joseph correct rehta.

Level 5 — Mastery

Exercise 5.1

Ek designer ne constant-position filter ke liye $Q = 0$ set kiya aur hairan hai ki kai GPS fixes ke baad estimate dheere-dheere drift karne wali sach mein position ko track karna band kar deti hai. Apne Ex 3.1/3.2 results use karke, mechanism precisely explain karo aur sabse chhota fix propose karo.

Recall Solution

Mechanism: $Q = 0$ ke saath, $P_{n} = \frac{P _{0} R}{R + n P _{0}} \to 0$ . Phir $K_{n} = P_{n}^{-} / (P_{n}^{-} + R) \to 0$ , isliye $\overset{x}{^}_{k} = \overset{x}{^}_{k}^{-} + K_{k} y_{k} \approx \overset{x}{^}_{k}^{-}$ : incoming measurements ko near-zero se multiply kiya jaata hai aur ignore kar diya jaata hai. Filter ek purani value ke baare mein certain ho jaata hai — divergence. Fix: $Q > 0$ set karo. Yeh har predict step par $P^{-}$ mein ek floor add karta hai, $K$ ko $0$ tak collapse hone se rokta hai, isliye filter drift ke liye responsive rehta hai. $Q$ encode karta hai "model thoda galat ho sakta hai" — bilkul woh humility jo real system ko chahiye. (Formally yeh Bayesian inference se connect hota hai: $Q$ har step prior ko re-inflate karta hai.)

Exercise 5.2

Process noise $Q > 0$ ke saath scalar constant-position filter ka steady state. Predict hai $P^{-} = P + Q$ ; update hai $P = (1 - K) P^{-}$ jahan $K = P^{-} / (P^{-} + R)$ . Steady state par $P$ constant hai. $Q = 1, R = 3$ ke liye fixed point $P_{\infty}$ nikalo.

Recall Solution

Combine karo: $P^{-} = P + Q$ , phir $P = \frac{P ^{-} R}{P ^{-} + R}$ . Steady state par $p = P$ mano: $p = \frac{( p + Q ) R}{( p + Q ) + R} .$ Multiply out karo: $p (p + Q + R) = (p + Q) R \Rightarrow p^{2} + p (Q + R) = pR + QR \Rightarrow p^{2} + pQ - QR = 0.$ $Q = 1, R = 3$ ke saath: $p^{2} + p - 3 = 0 \Rightarrow p = \frac{- 1 + 1 + 12}{2} = \frac{- 1 + 13}{2} \approx 1.3028$ . Padho: $Q = 0$ case ( $P \to 0$ ) ke unlike, yahan $P$ ek positive constant $\approx 1.30$ par settle hoti hai. Filter ek stable, non-zero confidence par pohonchta hai — bilkul wahi jo ise zinda aur tracking rakhta hai.

Exercise 5.3 — figure

Sketch karo/argue karo ki scalar gain $K_{k}$ time ke saath bade initial $P_{0}$ se steady state tak (Ex 5.2 setup) kaise evolve karta hai. Kya $K$ monotone hai? Kahan settle karta hai?

Recall Solution

Bada shuru karo: $P_{0}$ bada ⇒ pehla $K$ $1$ ke paas (andha rehte waqt sensor par near-fully trust karo). Jaise measurements aati hain, $P$ $P_{\infty} \approx 1.30$ ki taraf girta hai, isliye $K$ girta hai aur $K_{\infty} = P_{\infty}^{-} / (P_{\infty}^{-} + R)$ par settle karta hai jahan $P_{\infty}^{-} = P_{\infty} + Q \approx 2.30$ , jo deta hai $K_{\infty} = 2.30/5.30 \approx 0.434$ . Figure description (greyscale-safe): "gain $K$ vs update step $k$ " plot. $Q = 1$ ke liye main curve (circles) $1$ ke paas uunchi shuru hoti hai, pehle kuch steps mein teezi se girti hai, phir ek horizontal dotted line $K \approx 0.434$ par flat ho jaati hai — monotone decreasing, positive floor tak, kabhi $0$ nahi pohonchti. $Q = 0$ ke liye ek doosra dashed curve (squares) $0$ ki taraf sagging rehta hai, jo dead, unresponsive filter illustrate karta hai. Positive floor $Q > 0$ ka gift hai.

Flashcards

Bada prior covariance

P^{-}

Kalman gain par kya karta hai?

Yeh

K

ko measurement ki taraf utha deta hai — bada

P^{-}

matlab hum apni prediction par distrust karte hain.

Update ke baad

P

kyun shrink honi chahiye?

Kyunki

(I - K H)

ke eigenvalues

< 1

hain; independent info fuse karne se uncertainty sirf kam ho sakti hai (jab tak measurement useless nahi,

R \to \infty

Kai measurements ke baad ek

Q = 0

filter ka kya hota hai?

P \to 0

, isliye

K \to 0

aur yeh nayi data ignore karta hai — divergence agar sach drift kare.

Predict equation mein

u_{k}

kya hai?

Known control input (ek command jo tumne bheji);

B

ise state mein map karta hai, aur yeh koi uncertainty add nahi karta.

Innovation

y_{k}

kiske barabar hai?

Measurement minus prediction,

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

(kabhi true state nahi).

Q = 1, R = 3

ke liye steady-state scalar covariance kya hai?

p^{2} + p - 3 = 0

ka positive root, lagbhag

1.30