Kalman Smoothing
Jur van den Berg
Kalman Filtering vs. Smoothing
• Dynamics and Observation model
X t 1  AX t  Wt , Wt  N (0, Q)
Yt  CX t  Vt , Vt  N (0, R)
• Kalman Filter:
– Compute  X t | Y0  y 0 ,, Yt  y t 
– Real-time, given data so far
• Kalman Smoother:
– Compute  X t | Y0  y 0 ,, YT  yT , t  T
– Post-processing, given all data
Kalman Filtering Recap
• Time update
– X t 1|t  AX t|t  Wt
• Measurement update:
– Yt 1|t  CX t 1|t  Vt 1
– Compute joint distribution ( X t 1|t , Yt 1|t )
– Compute conditional X t 1|t 1  ( X t 1|t | Yt 1|t  y t 1 )
X0
X1
X2
X3
X4
X5
…
Y1
Y2
Y3
Y4
Y5
Kalman filter summary
• Model: X t 1  AX t  Wt , Wt  N (0, Q)
Yt  CX t  Vt , Vt  N (0, R)
• Algorithm: repeat…
– Time update:
xˆ t 1|t  Axˆ t|t
Pt 1|t  APt|t AT  Q
– Measurement update:
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 
T
Pt 1|t 1

T
Pt 1|t  K t 1CPt 1|t
1
Kalman Smoothing
• Input: initial distribution X0 and data y1, …, yT
• Algorithm: forward-backward pass
(Rauch-Tung-Striebel algorithm)
• Forward pass:
– Kalman filter: compute Xt+1|t and Xt+1|t+1 for 0 ≤ t < T
• Backward pass:
– Compute Xt|T for 0 ≤ t < T
– Reverse “horizontal” arrow in graph
Backward Pass
• Compute Xt|T given X t 1|T  N (xˆ t 1|T , Pt 1|T )
• Reverse arrow: Xt|t → Xt+1|t
• Same as incorporating measurement in filter
– 1. Compute joint (Xt|t, Xt+1|t)
– 2. Compute conditional (Xt|t | Xt+1|t = xt+1)
• New: xt+1 is not “known”, we only know its
distribution: xt 1 ~ X t 1|T
– 3. “Uncondition” on xt+1 to compute Xt|T using
laws of total expectation and variance
Backward pass. Step 1
• Compute joint distribution of Xt|t and Xt+1|t:
X
t |t
, X t 1|t 
  EX t|t    Var X t|t 
CovX t|t , X t 1|t  
, 

 N  



 EX t 1|t  CovX t 1|t , X t|t 


Var
X
t

1
|
t



  xˆ t|t   Pt|t Pt|t AT  

, 

N  
  xˆ t 1|t   APt|t Pt 1|t  



where
CovX t 1|t , X t|t  
CovAX t|t  Wt , X t|t 


A CovX t|t , X t|t   CovWt , X t|t 
A Var X t|t 

APt|t
Backward pass. Step 2
• Recall that if
  1   11 12  
 
Z1 , Z 2   N   , 
   2    21  22  
then
Z1 | Z2  z 2   N 1  12221 z 2  2 , 11  1222121 
• Compute (Xt|t|Xt+1|t = xt+1):
X
t |t

1
| X t 1|t  xt 1   N xˆ t|t  Pt|t AT Pt 1|t xt 1  xˆ t 1|t ,
1
Pt|t  Pt|t AT Pt 1|t APt|t

Backward pass Step 3
• Conditional only valid for given xt+1.
X
t |t

| X t 1|t  xt 1  
1
N xˆ t|t  Pt|t AT Pt 1|t xt 1  xˆ t 1|t ,
1

Pt|t  Pt|t AT Pt 1|t APt|t
 N xˆ t|t  Lt xt 1  xˆ t 1|t , Pt|t  Lt Pt 1|t LTt

– Where
Lt  Pt|t A Pt 1|t
T

1
• But we don’t know its value, but only its
distribution: xt 1 ~ X t 1|T
• Uncondition on xt+1 to compute Xt|T using law
of total expectation and law of total variance
Law of total expectation/variance
• Law of total expectation:
– E(X) = EZ( E(X|Y = Z) )
• Law of total variance:
– Var(X) = EZ( Var(X|Y = Z) ) + VarZ( E(X|Y = Z) )
• Compute X t|T  N ( E( X t|T ), Var( X t|T ))
– where
E ( X t|T )  EX t1|T E ( X t|t | X t 1|t  X t 1|T )
Var( X t|T )  E X t1|T Var( X t|t | X t 1|t  X t 1|T )  
Var X t 1|T E ( X t|t | X t 1|t  X t 1|T ) 
Unconditioning
• Recall from step 2 that
E( X t|t | X t 1|t  X t 1|T )  xˆ t|t  Lt X t 1|T  xˆ t 1|t 
Var( X t|t | X t 1|t  X t 1|T )  Pt|t  Lt Pt 1|t LTt
• So, E ( X t|T )  E X

| X t 1|t  X t 1|T ) 
xˆ t|t  Lt xˆ t 1|T  xˆ t 1|t 
t 1|T
E ( X
t |t
Var( X t|T )  E X t 1|T Var( X t|t | X t 1|t  X t 1|T )  
Var X t 1|T E ( X t|t | X t 1|t  X t 1|T ) 

Pt|t  Lt Pt 1|t LTt  Lt Pt 1|T LTt

Pt|t  Lt ( Pt 1|T  Pt 1|t ) LTt
Backward pass
• Summary:
1
Lt
xˆ t|T

Pt|T
 Pt|t  Lt ( Pt 1|T  Pt 1|t ) LTt

T
Pt|t A Pt 1|t
xˆ t|t  Lt xˆ t 1|T  xˆ t 1|t 
Kalman smoother algorithm
• for (t = 0; t < T; ++t)
// Kalman filter
xˆ t 1|t  Axˆ t|t
Pt 1|t  APt|t AT  Q
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 
T
Pt 1|t 1

T
Pt 1|t  K t 1CPt 1|t
• for (t = T – 1; t ≥ 0; --t)
// Backward pass
1
Lt
xˆ t|T

Pt|T
 Pt|t  Lt ( Pt 1|T  Pt 1|t ) LTt

1
T
Pt|t A Pt 1|t
xˆ t|t  Lt xˆ t 1|T  xˆ t 1|t 
Conclusion
• Kalman smoother can in used a post-processing
• Use xt|T’s as optimal estimate of state at time t,
and use Pt|T as a measure of uncertainty.
Extensions
• Automatic parameter (Q and R) fitting using
EM-algorithm
– Use Kalman Smoother on “training data” to learn
Q and R (and A and C)