Kalman Filtering.ppt

Kalman Filtering Jur van den Berg

Kalman Filtering • (Optimal) estimation of the (hidden) state of a linear dynamic process of which we obtain noisy (partial) measurements • Example: radar tracking of an airplane. What is the state of an airplane given noisy radar measurements of the airplane’s position?

Model • Discrete time steps, continuous state-space • (Hidden) state: xt , measurement: yt • Airplane example:

 xt    x t   xt , y t  ~ xt   x   t

• Position, speed and acceleration

Dynamics and Observation model • Linear dynamics model describes relation between the state and the next state, and the observation: xt 1  Axt  w t , w t ~ Wt  N (0, Q) y t  Cxt  v t , v t ~ Vt  N (0, R)

• Airplane example (if process has time-step ): 1   A  0 1 0 0 

1 2

2   , C  1 0 0  1 

Normal distributions • Let X0 be a normal distribution of the initial state x0 • Then, every Xt is a normal distribution of hidden state xt. Recursive definition: X t 1  AXt  Wt

• And every Yt is a normal distribution of observation yt. Definition: Yt  CX t  Vt

• Goal of filtering: compute conditional distribution  X t | Y0  y 0 ,, Yt  yt 

Normal distribution • Because Xt’s and Yt’s are normal distributions, X t | Y0  y0 ,, Yt  yt  is also a normal distribution • Normal distribution is fully specified by mean and covariance • We denote: X t |s

 X t | Y0  y 0 ,, Ys  y s    N E X t | Y0  y 0 ,, Ys  y s , Var  X t | Y0  y 0 ,, Ys  y s   N xˆ t|s , Pt|s 

Problem reduces to computing xt|t and Pt|t

Recursive update of state • Kalman filtering algorithm: repeat… – Time update: from Xt|t, compute a priori distrubution Xt+1|t – Measurement update: from Xt+1|t (and given yt+1), compute a posteriori distribution Xt+1|t+1 X0

X1

X2

X3

X4

X5 …

Y1

Y2

Y3

Y4

Y5

Time update • From Xt|t, compute a priori distribution Xt+1|t: X t 1|t

 AX t|t  Wt  N EAX t|t  Wt , Var AX t|t  Wt   N A EX t|t   EWt , A Var X t|t AT  Var Wt  N Axˆ t|t , APt|t AT  Q 



• So, xˆ t 1|t  Axˆ t|t Pt 1|t  APt|t AT  Q







Measurement update • From Xt+1|t (and given yt+1), compute Xt+1|t+1. • 1. Compute a priori distribution of the observation Yt+1|t from Xt+1|t: Yt 1|t

 CX t 1|t  Vt 1  N ECX t 1|t  Vt 1 , Var CX t 1|t  Vt 1   N C EX t 1|t   EVt 1 , C Var X t 1|t C T  Var Vt 1  N Cxˆ t 1|t , CPt 1|t C T  R 









Measurement update (cont’d) • 2. Look at joint distribution of Xt+1|t and Yt+1|t:

X

t 1|t

  EX t 1|t   Var X t 1|t  CovX t 1|t , Yt 1|t   ,    N       EYt 1|t  CovYt 1|t , X t 1|t    Var Y t  1 | t      xˆ t 1|t   Pt 1|t Pt 1|t C T     ,   N     Cxˆ t 1|t   CPt 1|t CPt 1|t C T  R     

, Yt 1|t 

where CovYt 1 , X t 1|t  

CovCX t 1|t  Vt 1 , X t 1|t 

 C CovX t 1|t , X t 1|t   CovVt 1 , X t 1|t  C Var X t 1|t   CPt 1|t 

Measurement update (cont’d) • Recall from undergrad that if   1   11 Z1 , Z 2   N   ,     2    21

12      22  

then Z1 | Z2  z 2   N 1  12221 z 2  2 , 11  1222121  • 3. Compute Xt+1|t +1 = (Xt+1|t|Yt+1|t = yt+1): X t 1|t 1

 



X

t 1|t



| Yt 1|t  y t 1 

 y  Cxˆ ,  R  CP 

N xˆ t 1|t  Pt 1|t C CPt 1|t C  R T

T



Pt 1|t  Pt 1|t C T CPt 1|t C T

1

t 1

t 1|t

1

t 1|t

Measurement update (cont’d): • Often written in terms of Kalman gain matrix:





1

K t 1  Pt 1|t C CPt 1|t C  R xˆ t 1|t 1  xˆ t 1|t  K t 1 y t 1  Cxˆ t 1|t  Pt 1|t 1  Pt 1|t  K t 1CPt 1|t T

T

Kalman filter summary • Model: xt 1  Axt  wt , wt ~ Wt  N (0, Q) 

yt

Cxt  v t ,

v t ~ Vt  N (0, R)

• Algorithm: repeat… – Time update:

xˆ t 1|t  Axˆ t|t Pt 1|t  APt|t AT  Q

– Measurement update:





1

K t 1  Pt 1|t C CPt 1|t C  R xˆ t 1|t 1  xˆ t 1|t  K t 1 y t 1  Cxˆ t 1|t  Pt 1|t 1  Pt 1|t  K t 1CPt 1|t T

T

Initialization • Choose distribution of initial state by picking x0 and P0 • Start with measurement update given measurement y0 • Choice for Q and R (identity) – small Q: dynamics “trusted” more – small R: measurements “trusted” more

Conclusion • Kalman filter can be used in real time • Use xt|t’s as optimal estimate of state at time t, and use Pt|t as a measure of uncertainty.

Extensions • Dynamic process with known control input • Non-linear dynamic process • Kalman smoothing: compute optimal estimate of state xt given all data y1, …, yT, with T > t (not real-time). • Automatic parameter (Q and R) fitting using EM-algorithm