Kalman Filters

Emtiyaz
CS,UBC

Kalman Filters – p.1/12

 A Linear State-Space model
Consider the following linear (time-invariant) system:

xt = Axt−1 + Bwt + F ut (1)

yt = Cxt + Dvt + Gut (2)

with x0 ∼ N (µ0 , Σ0 ), (i.i.d.) wt ∼ N (0, Q), vt ∼ N (0, R).

Also x0 ⊥ wt ⊥ vt for all t.
This is also called a linear-dynamical system.
Problem: Given measurements yt and matrices A − G,
find optimal xt and ut .
Separability of Control and Estimation: Find optimal
state estimate x̂t|t with ut = 0, then optimal policy is a
linear function of state estimation ut = KL x̂t|t .

Kalman Filters – p.2/12

 A Linear State-Space model
We compute state estimates for the following model:

xt = Axt−1 + Bwt (3)

yt = Cxt + Dvt (4)

Example A 2-D vehicle with speed control driven by noise,

2 3 2 32 3 2 3
pxt 1 T 0 0 pxt−1 0 0
6 7 6 76 7 6 72 3
6
6 sxt
7
7
6 0
6 1 0 0 7
76
6 sx
t−1
7 6 0 1
7 6
7 x
7 4 wt 5
6 7 = 6 76 7+6 7 (5)
6
4 pyt 7
5
6 0
4 0 1 T 7
54
6 pyt−1 7 6 0 0
5 4
7
5 wty
syt syt−1
| {z }
0 0 0 1 0 1 wt
| {z } | {z }| {z } | {z }
xt A xt−1 B
2 3
2 3 2 36 pxt
7 2 3
xt 1 0 0 0 6 sxt
7 x
7 4 vt 5
4 5 = 4 566 7+ (6)
yt 0 0 1 0 6
4 pyt 7
5 vty
syt
| {z } | {z } | {z }
yt C vt
| {z } Kalman Filters – p.3/12
xt
Demo

Kalman Filters – p.4/12

 Linear Projection
Denote an estimate of x given y as x̂|y and error
covariance P|y = E[(x̂|y − x)(x̂|y − x)T ].
Minimum mean-square estimate = x̂|y = E(x|y).
The space of (zero-mean) random variable is an
inner-product space with inner-product hx, yi = E(xyT ).
Hence MMSE estimates can be found by ortho. proj.:

x̂|y = hx, yi||y||−2 y = E(xyT )E(yyT )−1 y (8)

If x is uncorrelated with y, x̂|y = 0 and P|y = E(xxT ).

x̂|y1 ,y2 = x̂|y1 + x̂|y2 iff y1 ⊥ y2 , and P|y = P|y1 + P|y2 .
Notation: x̂t1|t2 , Pt1|t2 and ext1|t2 = x̂t1|t2 − xt1
Kalman Filters – p.5/12
 Prediction Estimates
Predict using state equations: xt = Axt−1 + Bwt .
Given an estimate at t − 1, propagate as follows:

Prediction : x̂t|t−1 = Ax̂t−1|t−1

Pt|t−1 = APt−1|t−1 AT + BQB T
Proof : x̂t|t−1 = Ax̂t−1|t−1 + B ŵt = Ax̂t−1|t−1
ext|t−1 = x̂t|t−1 − xt
= Ax̂t−1|t−1 − xt
= Ax̂t−1|t−1 − Axt−1 − Bwt
= Aext−1|t−1 − Bwt
⇒ Pt|t−1 = APt−1|t−1 AT + BQB T

Initialize with some x̂0|0 , P0|0 . Error keeps increasing!! Kalman Filters – p.6/12
 Innovation
Find “new information" in the current measurement (use
measurement equation yt = Cxt + Dvt ):

Innovations : et = yt − ŷt|t−1 = yt − C x̂t|t−1

Pe,t = CPt|t−1 C T + DRDT
Proof : ŷt|t−1 = C x̂t|t−1 + Dv̂t|t−1 = C x̂t|t−1
et = Cxt + Dvt − C x̂t|t−1
= Cext|t−1 + Dvt
⇒ Pe,t = CPt|t−1 C T + DRDT

Kalman Filters – p.7/12

 Correction with Kalman Gain
Correction : x̂t|t = x̂t|t−1 + Kt et (9)

Pt|t = (I − Kt C)Pt|t−1 + Kt Pe,t KtT (10)

Proof : ex
t|t = x̂t|t − xt (11)
= x̂t|t−1 + Kt et − xt (12)
= ext|t−1 + Kt et (13)

E[ext|t−1 eTt KtT ] = E[ext|t−1 (Cext|t−1 + Dvt )T KtT ] (14)

= Pt|t−1 C T KtT = Kt CPt|t−1 (15)

Pt|t = (I − Kt C)Pt|t−1 + Kt Pe,t KtT (16)

Find Kt∗ (called Kalman gain) such that Pt|t is minimized

−1
Kt∗ = arg min Kt Pe,t KtT − Kt CPt|t−1 = Pt|t−1 C T Pe,t (17)
Kalman Filters – p.8/12
 Kalman Filters

Predictions : x̂t|t−1 = Ax̂t−1|t−1

Pt|t−1 = APt−1|t−1 AT + BQB T
Innovations : et = yt − C x̂t|t−1
Pe,t = CPt|t−1 C T + DRDT
−1
Kalman Gain : Kt = Pt|t−1 C T Pe,t
Correction : x̂t|t = x̂t|t−1 + Kt et
Pt|t = (I − Kt C)Pt|t−1 + Kt Pe,t KtT

Kalman Filters – p.9/12

 Optimality
Unbiased if x̂0|0 is unbiased, otherwise asymptotic.
Pt|t is covariance, achieves Cramer-Rao Lower Bound.
Best linear unbiased estimator.
Solve the following Riccatti equation for P to get the
converged covariance.

P1 = AP AT + BQB T
Pe = CP1 C T + DRDT
K = P1 C T Pe−1
P = (I − KC)P1 + KPe K T

then K = P C T Pe−1 can be computed beforehand,

reduces the computational complexity.
Kalman Filters – p.10/12
 Implementation
Square-Root filters.
Information filters (IF).
Sparse Kalman Filters.
EM algorithm to find A, B, C, D.
Extensions : Extended KF, IF, Unscented KF.

Kalman Filters – p.11/12

 References
Optimal Filtering, Anderson and Moore (contains
almost everything)
Linear Estimation, T. Kailath (for orthog.-proj.
derivations etc.)

Kalman Filters – p.12/12