of 15/15
Kalman Smoothing Jur van den Berg

Kalman Smoothing - University of Utaharl.cs.utah.edu/resources/Kalman Smoothing.pdf · 2013-09-18 · Kalman Filtering vs. Smoothing •Dynamics and Observation model •Kalman Filter:

  • View
    2

  • Download
    0

Embed Size (px)

Text of Kalman Smoothing - University of Utaharl.cs.utah.edu/resources/Kalman Smoothing.pdf ·...

  • 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

    21122121122122121221 ,| zz NZZ

    ttttTtttt

    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

    Ttttttttttttttttt

    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|

    ttT

    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 XXXEEXE Tt

    ))Var(),(( ||| TtTtTt XXENX

  • Unconditioning

    • Recall from step 2 that

    • So,

    ttTttttTttttt XLXXXE |1|1||1|1| ˆˆ)|( xxT

    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)