Kalman Smoothing
Jur van den Berg
Kalman Filtering vs. Smoothing
• Dynamics and Observation model
• Kalman Filter:– Compute
– Real-time, given data so far
• Kalman Smoother:– Compute
– Post-processing, given all data
),(,1 QNWWAXX tttt 0
),(, RNVVCXY tttt 0
ttt YYX yy ,,| 00
TtYYX TTt ,,,| 00 yy
Kalman Filtering Recap
• Time update
–
• Measurement update:
–
– Compute joint distribution
– Compute conditional
X0 X1 X2 X3 X4 X5
Y1 Y2 Y3 Y4 Y5
…
ttttt WAXX ||1
1|1|1 ttttt VCXY
),( |1|1 tttt YX
)|( 1|1|11|1 ttttttt YXX y
Kalman filter summary
• Model:
• Algorithm: repeat…
– Time update:
– Measurement update:
QAAPP
A
T
tttt
tttt
||1
||1ˆˆ xx
ttttttt
tttttttt
T
tt
T
ttt
CPKPP
CK
RCCPCPK
|11|11|1
|111|11|1
1
|1|11
ˆˆˆ
xyxx
),(,1 QNWWAXX tttt 0
),(, RNVVCXY tttt 0
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
• 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:
– 3. “Uncondition” on xt+1 to compute Xt|T using laws of total expectation and variance
Ttt X |11 ~ x
),ˆ( |1|1|1 TtTtTt PNX x
Backward pass. Step 1
• Compute joint distribution of Xt|t and Xt+1|t:
where
tttt
T
tttt
tt
tt
tttttt
tttttt
tt
tt
tttt
PAP
APPN
XXX
XXX
X
XNXX
|1|
||
|1
|
|1||1
|1||
|1
|
|1|
,ˆ
ˆ
Var,Cov
,CovVar,
E
E,
x
x
tt
tt
ttttttt
ttttttttt
AP
XA
XWXXA
XWAXXX
|
|
|||
||||1
Var
,Cov,Cov
,Cov,Cov
Backward pass. Step 2
• Recall that if
then
• Compute (Xt|t|Xt+1|t = xt+1):
2221
1211
2
1
21 ,,
NZZ
21
1
22121122
1
22121221 ,| zz NZZ
tttt
T
tttt
ttttt
T
ttttttttt
APPAPP
PAPNXX
|
1
|1||
|11
1
|1||1|1| ,ˆˆ|
xxxx
Backward pass Step 3
• Conditional only valid for given xt+1.
– Where
• But we don’t know its value, but only its distribution:
• Uncondition on xt+1 to compute Xt|T using law of total expectation and law of total variance
T
tttttttttttt
tttt
T
tttt
ttttt
T
ttttttttt
LPLPLN
APPAPP
PAPNXX
|1||11|
|
1
|1||
|11
1
|1||1|1|
,ˆˆ
,ˆˆ|
xxx
xxxx
Ttt X |11 ~ x
1
|1|
tt
T
ttt PAPL
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
– where
)|(Var
)|Var()Var(
|1|1|
|1|1||
|1
|1
TtttttX
TtttttXTt
XXXE
XXXEX
Tt
Tt
)|()( |1|1|| |1 TtttttXTt XXXEEXETt
))Var(),(( ||| TtTtTt XXENX
Unconditioning
• Recall from step 2 that
• So,
ttTttttTttttt XLXXXE |1|1||1|1|ˆˆ)|( xx
T
ttttttTttttt LPLPXXX |1||1|1| )|Var(
ttTtttt
TtttttXTt
L
XXXEEXETt
|1|1|
|1|1||
ˆˆˆ
)|()(|1
xxx
T
tttTtttt
T
tTtt
T
tttttt
TtttttX
TtttttXTt
LPPLP
LPLLPLP
XXXE
XXXEX
Tt
Tt
)(
)|(Var
)|Var()Var(
|1|1|
|1|1|
|1|1|
|1|1||
|1
|1
Backward pass
• Summary:
T
tttTttttTt
ttTttttTt
tt
T
ttt
LPPLPP
L
PAPL
)(
ˆˆˆˆ
|1|1||
|1|1||
1
|1|
xxxx
Kalman smoother algorithm
• for (t = 0; t < T; ++t) // Kalman filter
• for (t = T – 1; t ≥ 0; --t) // Backward pass
QAAPP
A
T
tttt
tttt
||1
||1ˆˆ xx
ttttttt
tttttttt
T
tt
T
ttt
CPKPP
CK
RCCPCPK
|11|11|1
|111|11|1
1
|1|11
ˆˆˆ
xyxx
T
tttTttttTt
ttTttttTt
tt
T
ttt
LPPLPP
L
PAPL
)(
ˆˆˆˆ
|1|1||
|1|1||
1
|1|
xxxx
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)
Recommended
