Kalman filter derivation — predict step, update step: Worked examples

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

Shuru karne se pehle, notation ke baare mein ek promise. Neeche har symbol parent note mein define kiya gaya tha. Safe rehne ke liye, yeh pocket dictionary hai jo tum HAR example mein reuse karoge:

Recall Plain words mein quantities

$\overset{x}{^}^{-}$ (x-hat-minus) ::: hidden state ki humari prediction, nayi measurement dekhne se pehle banayi gayi. Minus ka matlab hai "prior". $P^{-}$ ::: woh prediction kitni uncertain hai (ek variance; bada = zyada shakier). Minus ka matlab phir se "prior" hai. $z$ ::: woh measurement jo sensor ne abhi di hai. $R$ ::: sensor kitna noisy hai (uske error ka variance). $H$ ::: woh dial jo "state" ko "sensor kya read karta" mein convert karta hai. Agar sensor state directly read karta hai, toh $H = 1$ . $F$ (state-transition) ::: physics step — woh rule jo state ko time mein ek tick aage push karta hai (jaise "position += velocity × time"). Agar kuch move nahi karta, $F = 1$ . $Q$ (process noise) ::: physics model khud per step kitna galat hai (ek variance jo tum predict ke dauran add karte ho). Bada $Q$ = tum apne model pe kam trust karte ho. $K$ (gain) ::: blend weight — 1-D mein $0$ aur $1$ ke beech ki ek number jo bataati hai ki surprise ka kitna hissa believe karna hai. $y = z - H \overset{x}{^}^{-}$ ::: innovation (surprise): measurement minus prediction.

Scenario matrix

Har Kalman problem neeche ke cells mein se ek (ya mix) hai. Har example us cell ke saath tagged hai jise woh hit karta hai.

Cell	Kya special hai	$K$ kahan land karta hai	Example
A. Equal trust	$P^{-} = R$	$K = 0.5$ (exact midpoint)	Ex 1
B. Great sensor	$R ≪ P^{-}$	$K \to 1$ (measurement adopt karo)	Ex 2
C. Great prediction	$P^{-} ≪ R$	$K \to 0$ (measurement ignore karo)	Ex 3
D. Degenerate: perfect sensor	$R = 0$	$K = 1/ H$ exactly, $P \to 0$	Ex 4
E. Degenerate: perfect prior	$P^{-} = 0$	$K = 0$ , measurement reject hoti hai	Ex 5
F. Predict grows uncertainty	predict run karo, $P$ ko swell karte dekho	(koi $K$ nahi; time update)	Ex 6
G. Vector / correlated states	$2 \times 2$ matrices, off-diagonal	$K$ ek matrix hai	Ex 7
H. Limiting: steady state	convergence tak iterate karo	$K$ change karna band kar deta hai	Ex 8
I. Word problem (sensor fusion)	real GPS + IMU numbers	end-to-end worked out	Ex 9
J. Exam twist: $H \neq = 1$	measurement scaled state hai	gain ko $H$ undo karna chahiye	Ex 10

Agar koi bhi cell shaky lagti hai, prerequisites: Covariance matrices and Gaussian distributions, State-space representation.

Cell A — Equal trust

Worked example Ex 1 — prior aur sensor equally uncertain (

$P^{-} = R$ ) Prior $\overset{x}{^}^{-} = 10$ m, $P^{-} = 4$ m². Measurement $z = 14$ m, $R = 4$ m², $H = 1$ .

Forecast: equal variances ke saath, blended estimate kahan land honi chahiye — 10 ke paas, 14 ke paas, ya exactly halfway? Padhne se pehle guess karo.

Innovation. $y = z - H \overset{x}{^}^{-} = 14 - 10 = 4$ m. Yeh step kyun? Filter sirf surprise pe react karta hai, raw measurement pe nahi — wahi ek computable error hai.
Innovation covariance. $S = H P^{-} H^{⊤} + R = 4 + 4 = 8$ m². Yeh step kyun? $S$ surprise ki total uncertainty hai: prior wobble plus sensor wobble.
Gain. $K = P^{-} H^{⊤} S^{- 1} = 4/8 = 0.5$ . Yeh step kyun? $K$ = (prior uncertainty)/(total uncertainty). Equal shares → exactly $0.5$ .
Estimate. $\overset{x}{^} = 10 + 0.5 \cdot 4 = 12$ m. Yeh step kyun? old + gain × surprise. 10 se 14 ki taraf halfway move karne se 12 milta hai.
Covariance. $P = (1 - K) P^{-} = 0.5 \cdot 4 = 2$ m². Yeh step kyun? Do independent estimates fuse karna hamesha dono se better hota hai — $2 < 4$ .

Verify: 10 aur 14 ke beech halfway $12$ hai ✔. Final variance $2 < 4$ , toh hum pehle se zyada confident hain ✔. Units poore m aur m² mein rehte hain ✔.

Cells B & C — trust ke do extremes

Worked example Ex 3 — ek great prediction (Cell C,

$P^{-} ≪ R$ ) Prior $\overset{x}{^}^{-} = 0$ , $P^{-} = 1$ . Measurement $z = 50$ , $R = 100$ , $H = 1$ . (Sensor junk hai.)

Forecast: sensor chilla raha hai "50!" lekin hum use barely trust karte hain. Kya estimate zyada move karegi?

$y = 50 - 0 = 50$ . Kyun: Ex 2 jaisi badi surprise, lekin ab sensor shaky wala hai.
$S = 1 + 100 = 101$ . Kyun: noisy sensor se dominated hai.
$K = 1/101 \approx 0.0099$ . Kyun: measurement pe almost koi weight nahi.
$\overset{x}{^} = 0 + 0.0099 \cdot 50 \approx 0.495$ . Kyun: hum noisy reading ki taraf sirf thoda sa nudge karte hain.
$P = (1 - 0.0099) \cdot 1 \approx 0.990$ . Kyun: humne kuch barely learn kiya, toh $P$ near $1$ rehta hai.

Verify: $K \to 0$ as $R \to \infty$ ✔. Estimate trusted prior (0) ke paas rahi, junk reading (50) ke paas nahi ✔. Ex 2 se compare karo: same $z$ , opposite behaviour — variances decide karte hain, numbers nahi ✔.

Cells D & E — do degenerate (zero-variance) cases

Yeh "divide by zero" traps hain. Inhe ek baar dikhao aur tum inse kabhi nahi daroge.

Worked example Ex 4 — ek PERFECT sensor (Cell D,

$R = 0$ ) Prior $\overset{x}{^}^{-} = 10$ , $P^{-} = 4$ . Measurement $z = 14$ , $R = 0$ , $H = 1$ .

Forecast: ek noiseless sensor. Humari apni prediction ka kitna hissa bachta hai?

$y = 14 - 10 = 4$ . Yeh step kyun? Perfect sensor ke saath bhi hum pehle usse apni prediction se compare karte hain — surprise (4) hi correction drive karta hai.
$S = 4 + 0 = 4$ . Kyun: surprise ab sirf prior uncertainty carry karta hai.
$K = 4/4 = 1$ . Kyun: $R = 0$ force karta hai $K = P^{-} / P^{-} = 1$ (ya generally $K = H^{- 1} = 1$ ). Hum sensor pe poora believe karte hain.
$\overset{x}{^} = 10 + 1 \cdot 4 = 14$ . Kyun: hum exactly measurement pe snap kar lete hain.
$P = (1 - 1) \cdot 4 = 0$ . Kyun: ek perfect measurement SAARI uncertainty remove kar deti hai.

Verify: $\overset{x}{^} = z = 14$ ✔ (perfect sensor prediction ko override karta hai). $P = 0$ ✔ (zero doubt). Koi division by zero nahi hua kyunki $S = P^{-} \neq = 0$ ✔.

Cell F — predict step uncertainty ko swell karta hai

Cell G — vectors, matrices, aur correlation (predict AND matrix-gain update)

Worked example Ex 7 — 2-D predict, phir ek position-only measurement

State $x = [p v]$ , $Δ t = 1$ s, $F = [1011]$ (position += velocity, velocity unchanged), koi control nahi, $Q = 0$ . Start $\overset{x}{^} = [02]$ , $P = [1001]$ . Phir ek GPS position only deta hai: $z = 2.5$ m, $R = 1$ , toh $H = [10]$ .

Forecast: do questions. (a) Predict ke baad, kya ek off-diagonal (correlation) term appear karta hai? (b) Jab hum sirf position measure karte hain, toh kya gain $K$ velocity bhi correct karta hai?

$\overset{x}{^}^{-} = F \overset{x}{^} = [0 + 2 2] = [22]$ . Kyun: 1 s mein 2 m/s pe 2 m move kiya.
$F P = [1011] [1001] = [1011]$ . Kyun: sandwich $F P F^{⊤}$ ka pehla half.
$P^{-} = (F P) F^{⊤} = [1011] [1101] = [2111]$ . Kyun: sandwich complete karo; off-diagonal $1$ ka matlab hai ek uncertain velocity ab position mein forward feed hoti hai — woh correlated hain. Figure dekho.
Innovation covariance (yahan scalar). $S = H P^{-} H^{⊤} + R = [10] [2111] [10] + 1 = 2 + 1 = 3$ . Kyun: $H P^{-} H^{⊤}$ top-left position-variance ( $2$ ) pick karta hai, phir sensor noise add karo.
Matrix gain. $K = P^{-} H^{⊤} S^{- 1} = [2111] [10] \cdot \frac{1}{3} = [21] \cdot \frac{1}{3} = [2/3 1/3]$ . Kyun: velocity entry $1/3$ non-zero hai even though humne kabhi velocity measure nahi ki — step 3 se correlation ek position measurement ko velocity bhi fix karne deta hai. Yeh vector filter ka magic hai.
Innovation & update. $y = z - H \overset{x}{^}^{-} = 2.5 - 2 = 0.5$ . Phir $\overset{x}{^} = \overset{x}{^}^{-} + K y = [22] + [2/3 1/3] (0.5) = [2.333 2.167]$ . Kyun: position AUR velocity dono reading ke saath consistency ki taraf shift karte hain.
Covariance. $P = (I - K H) P^{-} = ([1001] - [2/3 1/3] [10]) [2111] = [2/3 1/3 1/3 2/3]$ . Kyun: har entry shrank — velocity variance bhi $1$ se $2/3$ drop hua, correlation ki wajah se.

Verify: $P^{-}$ symmetric with off-diagonal $0 \to 1$ ✔. Gain ka velocity entry $1/3 \neq = 0$ ✔ (position measurement velocity correct karta hai). Posterior velocity variance $2/3 < 1$ ✔. $F$ ki woh shape kyun hai iske liye State-space representation dekho, aur multi-sensor version ke liye IMU and GPS sensor fusion dekho.

Cell H — limiting steady state

Worked example Ex 8 — predict+update iterate karo jab tak

$K$ move karna band na kar de 1-D constant position, $F = 1$ , $H = 1$ , $Q = 1$ , $R = 1$ . Bade prior $P_{0} = 100$ se start karo. Baar baar predict phir update run karo. Gain kahan settle karta hai?

Forecast: kya $K$ hamesha ke liye change karta rehta hai, ya ek value pe lock ho jaata hai?

Har cycle: predict $P^{-} = P + Q = P + 1$ ; phir $K = \frac{P ^{-}}{P ^{-} + 1}$ , update $P = (1 - K) P^{-}$ .

Step 1: $P^{-} = 100 + 1 = 101$ , $K = 101/102 \approx 0.9902$ , $P = (1 - K) \cdot 101 \approx 0.9902$ . Yeh step kyun? Predict $Q = 1$ add karta hai; phir huge prior ke saath measurement dominate karta hai ( $K \approx 1$ ), aur $P$ sensor ke $R = 1$ ki taraf collapse ho jaata hai.
Step 2: $P^{-} = 0.9902 + 1 = 1.9902$ , $K = 1.9902/2.9902 \approx 0.6656$ , $P \approx 0.6656$ . Yeh step kyun? Ab prior already sharp hai, toh predict ka $+ Q$ relatively bada bump hai; gain well below $1$ drop ho jaata hai — hum apni prediction pe zyada trust karne lagte hain.
Step 3: $P^{-} = 0.6656 + 1 = 1.6656$ , $K = 1.6656/2.6656 \approx 0.6248$ , $P \approx 0.6248$ . Yeh step kyun? Numbers ab barely move kar rahi hain — har cycle $K$ ko thoda sa neeche uske balance point ki taraf nudge karta hai.
Chalte raho… $P$ fixed point $P_{\infty}$ ki taraf approach karta hai jo $P = (1 - K) (P + 1)$ ko $K = \frac{P + 1}{P + 2}$ ke saath solve karta hai. Yeh step kyun? Steady state mein predict se incoming uncertainty ( $+ Q$ ) exactly update se shrink ko balance karti hai. Solve karne pe $P_{\infty} = \frac{- 1 + 5}{2} \approx 0.618$ milta hai, aur $K_{\infty} = \frac{P _{\infty} + 1}{P _{\infty} + 2} \approx 0.618$ .

Verify: golden ratio $ϕ - 1 = 0.618$ nikalta hai ✔. $K$ huge $P_{0} = 100$ se regardless $\approx 0.618$ pe converge karta hai ✔ — yehi reason hai ki parent note ki galti ("K ek fixed tuning knob hai") subtly galat hai: $K$ does become constant, lekin sirf ek emergent limit ke roop mein, hand-picked value ke roop mein nahi.

Cell I — word problem (real sensor fusion)

Worked example Ex 9 — GPS ko IMU dead-reckon ke saath fuse karna

Ek drone ka IMU apni east position ko $\overset{x}{^}^{-} = 120.0$ m ke saath $P^{-} = 9.0$ m² (drift-prone) ke saath dead-reckon karta hai. Ek GPS fix aati hai: $z = 123.0$ m with $R = 4.0$ m². GPS position directly read karta hai, toh $H = 1$ .

Forecast: GPS yahan sharper hai ( $R < P^{-}$ ). Kya fused estimate 123 ki taraf lean karni chahiye?

$y = 123.0 - 120.0 = 3.0$ m. Kyun: GPS keh raha hai ki hum IMU guess se 3 m east drift kiye hain.
$S = 9.0 + 4.0 = 13.0$ m². Kyun: IMU + GPS ki combined wobble.
$K = 9.0/13.0 \approx 0.6923$ . Kyun: GPS ko larger share ( $> 0.5$ ) milta hai kyunki woh dono mein se zyada precise hai.
$\overset{x}{^} = 120.0 + 0.6923 \cdot 3.0 \approx 122.08$ m. Kyun: GPS ke 123 ke paas land karta hai IMU ke 120 se zyada, jaise trust dictate karta hai.
$P = (1 - 0.6923) \cdot 9.0 \approx 2.769$ m². Kyun: fused uncertainty ( $2.77$ ) dono inputs (9 aur 4) se beat karta hai — fusion hamesha jeetta hai.

Verify: estimate $122.08$ 120 aur 123 ke beech hai, sharper GPS ki taraf biased ✔. $2.77 < 4 < 9$ ✔. Multi-axis version ke liye IMU and GPS sensor fusion dekho.

Cell J — exam twist ( $H \neq = 1$ )

Worked example Ex 10 — sensor

state ka double report karta hai Ek gauge output karta hai $z = 2 x + noise$ , toh $H = 2$ . Prior $\overset{x}{^}^{-} = 5$ , $P^{-} = 3$ , measurement $z = 12$ , $R = 1$ .

Forecast: kyunki sensor state ko double karta hai, raw reading 12 ka "matlab" hai $x = 6$ . Kya gain automatically factor of 2 undo karta hai?

$y = z - H \overset{x}{^}^{-} = 12 - 2 \cdot 5 = 12 - 10 = 2$ . Kyun: reading ko compare karo us cheez se jo sensor should have read hamari prediction ke liye, jo hai $2 \times 5 = 10$ .
$S = H P^{-} H^{⊤} + R = 2 \cdot 3 \cdot 2 + 1 = 12 + 1 = 13$ . Kyun: $H$ prior uncertainty ko sensor noise add karne se pehle $H^{2} = 4$ se scale karta hai.
$K = P^{-} H^{⊤} S^{- 1} = 3 \cdot 2/13 = 6/13 \approx 0.4615$ . Kyun: gain $H$ ka ek factor carry karta hai taaki woh measurement-space surprise ko state-space correction mein convert kar sake.
$\overset{x}{^} = 5 + 0.4615 \cdot 2 \approx 5.923$ . Kyun: sensor jo value imply karta hai uski taraf nudge hua ( $x \approx 6$ ), 12 ki taraf nahi.
$P = (1 - K H) P^{-} = (1 - 0.4615 \cdot 2) \cdot 3 = (1 - 0.9231) \cdot 3 \approx 0.2308$ . Kyun: note karo ki yeh $(1 - K H)$ hai, $(1 - K)$ nahi — $H$ yahan bhi reappear karna chahiye.

Verify: estimate $5.92$ prior 5 aur implied measurement 6 ke beech hai ✔. Posterior $0.231 < 3$ ✔. Step 5 mein $H$ bhool jaana ( $(1 - K) P^{-}$ likhna) ek galat, bada $P$ dega — classic exam trap ✔.

Recall Self-test

Ex 3 mein sensor "50" chilla raha tha aur hum barely move hue — kyun? ::: Kyunki uski variance $R = 100$ bahut badi thi; trusted prior ( $P^{-} = 1$ ) ne estimate ko 0 ke paas rakha. Variances, values nahi, blend set karti hain. $P$ Ex 6 mein kyun grow karta hai lekin Ex 1 mein shrink karta hai? ::: Predict $Q$ add karta hai (koi data nahi toh uncertainty badhti hai); update $(1 - K H) < 1$ se multiply karta hai (ek measurement uncertainty remove karta hai). Ex 7 mein humne sirf position measure ki — velocity kyun correct hua? ::: Predict ne position aur velocity ko correlated banaya ( $P^{-}$ mein off-diagonal $1$ ), toh gain ek non-zero velocity entry carry karta hai. Kaunsa ek fact Ex 5 ko long-term dangerous banata hai? ::: $P^{-} = 0 \Rightarrow K = 0$ forever after; filter saare future data ignore karta hai (divergence).