Kalman Filtering And Smoothing

Jayashri

Outline

Introduction State Space Model Parameterization Inference

Filtering Smoothing

Introduction

Two Categories of Latent variable Models

• Discrete Latent variable -> Mixture Models

• Continuous Latent Variable-> Factor Analysis Models

Mixture Models -> Hidden Markov Model

Factor Analysis -> Kalman Filter

Application

Applications of Kalman filter are endless!

Control theory Tracking Computer vision Navigation and guidance system

State Space Model

C

…0x 1x 2x TxA A

0y 1y 2y Ty

C C C0

Independence Relationships:

• Given the state at one moment in the time, the states in the future are conditionally independent of those in the past.

• The observation of the output nodes fails to separate any of the state nodes.

Parameterization

1t t tx Ax Gw

ttt vCxy

.matrix covariance andmean 0 with noise whiteis where Rvt

Transition From one node to another:

Tttt GQGAxxx is covariance and mean has ,upon lConditiona 1

.matrix covariance andmean 0 with noise whiteis where Qwt

RCxyx ttt is covariance and mean has ,upon Condtional

00 covariance and 0mean has state Initial x

Unconditional Distribution

1 1 1[ ]Tt t tE x x

1[( )( ) ]Tt t t tE Ax Gw Ax Gw

[ ] [ ]T T T Tt t t t

T Tt

AE x x A GE w w G

A A GQG

•Unconditional mean of tx is zero.

•Unconditional covariance is:

Inference

Calculation of the posterior probability of the states given an output sequence Two Classes of Problems:

•Filtering

•Smoothing

Filtering

),...,|( 0 tt yyxP

],...,|[ˆ 0| tttt yyxEx

Notations:

],...,|)ˆ)(ˆ[( 0||| tT

tttttttt yyxxxxEP

Problem is to calculate the mean vector and Covariance matrix.

tt yyx ,...,on dconditione ofmean 0

tt yyx ,...,on dconditione ofmatrix covariance 0

Filtering Cont’d

)|(),...|( ,...,010 tttt yyxPyyxP

),...,|(),...|( 10101 tttt yyxPyyxP

tttt xAx ||1 ˆˆ

tx 1tx

ty 1ty

tx 1tx

1tyty

Time update:

Measurement update:

Time Update step:

],...|)ˆ)(ˆ[( 0|11|11|1 tT

tttttttt yyxxxxEP

],...|)ˆ)(ˆ[( 0|| tT

tttttttt yyxAGwAxxAGwAxE TT

tt GQGAAP |

Measurement Update step:

tt

ttttt

xCyyvCxEyyyE

|1

01101

ˆ ],...,|[],...,|[

RCCP

yyxCvCxxCvCxE

yyyyyyE

Ttt

tT

tttttttt

tT

tttttt

|1

0|111|111

0|11|11

],...|)ˆ)(ˆ[(

],...|)ˆ)(ˆ[(

tt

tT

ttttttt

tT

tttttt

CP

yyxxyCvCxE

yyxxyyE

|1

0|11|111

0|11|11

],...|)ˆ)(ˆ[(

],...|)ˆ)(ˆ[(

, ofmean lConditiona 1ty

, of covariance lConditiona 1ty

, and of covariance lConditiona 11 tt yx

Equations

tt

tt

xC

x

|1

|1

ˆ

ˆ

RCCPCP

CPPT

tttt

Ttttt

|1|1

|1|1

))(

)ˆ()(ˆˆ

|11

|11|11|1

|111

|1|1|11|1

ttT

ttT

ttttt

tttT

ttT

tttttt

CPRCCPCPPP

xCyRCCPCPxx

Using the equations 13.26 and 13.27

Mean Covariance

have, ,...on dconditione and ofon distributijoint The 011 ttt yyyx

1tx

1ty),...,|(),...|(),...|,( 01101011 tttttttt yyxyPyyxPyyyxP

Equations

tttt xAx ||1 ˆˆ

TTtttt GQGAAPP ||1

))(

)ˆ()(ˆˆ

|11

|11|11|1

|111

|1|1|11|1

ttT

ttT

ttttt

tttT

ttT

tttttt

CPRCCPCPPP

xCyRCCPCPxx

Summary of the update equations

11|1

1|1

1|1|1|1

111|1

1|1|11

))((

)(

)(

RCP

RCCPRCCPCPP

RCRCCP

RCCPCPK

Ttt

Ttt

Ttt

Ttttt

TTtt

Ttt

Tttt

)ˆ(ˆˆ |111|11|1 tttttttt xCyKxx

Kalman Gain Matrix

Update Equation:

Interpretation and Relation to LMS

tTtt vxy

tTttttt xxyRP )ˆ(ˆˆ

11

11

)ˆ(ˆˆ |11|1|1 tttttttt xCAyKxAx

The update equation can be written as,

•Matrix A is identity matrix and noise term w is zero

•Matrix C be replaced by the Ttx

tt Ixx 1

Update equation becomes,

Information Filter (Inverse Covariance Filter)

TGQGH

1 1

1|ˆ

tt1| ttS

ttS | tt|̂

Conversion of moment parameters to canonical parameters:

… Eqn. 13.5

Canonical parameters of the distribution of ly.respective ),...,|( and ),...,|( 010 tttt yyxPyyxP

CRCSS

HAAHASAHHS

yRC

HAASAH

Tttt

TTtttt

tT

tttt

ttT

tttt

111|1

111|

11|1

11

|11|1

|1

|1

|1

)(

ˆˆ

ˆ)(ˆ

Smoothing

Estimation of state x at time t given the data up to time t and later time T

•Rauch-Tung-Striebel (RTS) smoother (alpha-gamma algorithm)

•Two-filter smoother (alpha-beta algorithm)

0( | ,..., ) for t TP x y y t T

RTS Smoother

),...|( 0 tt yyxP

),...,|( 01 ttt yyxxP

•Recurses directly on the filtered-and-smoothed estimates i.e.

Alpha-gamma algorithm

tx 1tx

ty 1ty),...,|(),...,|( 0101 Tttttt yyxxPyyxxP

tx 1tx

1tyty),...|(),...,|( 001 TtTtt yyxPyyxxP

(RTS) Forward pass:

tt

tt

x

x

|1

|

ˆ

ˆ

tttt

Ttttt

PAP

APP

|1|

| |

have, ,...on lconditiona and ofon distributiJoint 01 ttt yyxx

Mean Covariance

pass.filter kalman from ),...|( havealready We 0 tt yyxPtx 1tx

ty 1ty

Backward filtering pass:

1|1|

|11|

|111|1||01

where

)ˆ(ˆ

)ˆ(ˆ],...,|[

ttT

ttt

tttttt

tttttT

ttttttt

PAPL

xxLx

xxPAPxyyxxE

tx 1txEstimate the probability of conditioned on

Ttttttt

ttttT

ttttttt

LPLP

APPAPPyyxx

|1|

|1|1||01

],...,|[Var

)ˆ(ˆ ],...,|[],...,|[

|11|

0101

tttttt

tttTtt

xxLxyyxxEyyxxE

Ttttttt

tttTtt

LPLP

yyxxyyxx

|1|

0101

],...,|[Var],...,|[Var

)ˆ(ˆ

],...|)ˆ(ˆ[ ],...,|],...,|[[

],...|[ˆ

|1|1|

0|11|

001

0|

ttTtttt

Ttttttt

TTtt

TtTt

xxLx

yyxxLxEyyyyxxEE

yyxEx

TtttTtttt

TttTtt

TtTt

LPPLP

yyxxVarEyyxxEVar

yyxVarP

)(

],...,,|[[],...,|[[

],...|[

|1|1|

0101

0|

]|],|[[]|[ ZZYXEEZXE Identities:

]|],|[[]|],|[[]|[ ZZYXVarEZZYXEVarZXVar

Ttt yyZxYxX ,...,, caseour In 01

Equations

TtttTttttTt

ttTttttTt

LPPLPP

xxLxx

)(

)ˆ(ˆˆ

|1|1||

|1|1||

Summary of update equations:

matrix.gain is where 1|1|

tt

Tttt PAPL

Two-Filter smoother

ttt GwAxAx 11

1

Forward Pass: ),...|( 0 tt yyxP

Backward Pass: ),...|( 1 Ttt yyxP

Naive approach to invert the dynamics which does not work is:

i.e. ),...,|( and ),...,|( Combines, 10 Ttttt yyxPyyxP

Alpha-beta algorithm

Cont’d

TTtt GQGAA 1

TT

tt

Tt

GQGAAA

A

t

TTTtt AGQGAAA

11

1

TTTtt AGQGAA 1

),,(For 1tt xxP

Covariance Matrix is:

We can invert the arrow between as, , and 1tt xx

tx

C C

A

ty 1ty

1tx

Which is backward Lyapunov equation.

1t1

11

-111

1

A TTT

t

Tt

TTTt

AGQGA

GQGAAGQGAAA

)(~ 11

11

tTGQGAIAA

Covariance matrix can be written as:

1t1

1

~

~ T

t

tt

A

A

TTtt GQGAA ~~~~~

1

11~~~

ttt wGxAx

We can define Inverse dynamics as:

GAG 1~

1111

~

tt

Ttt xQGQww

GQQGQ

wwEQ

tT

Ttt

11

11

]~~[~

Last issue is to fuse the two filter estimates.

Summary:

)ˆˆ(ˆ 1|11||

1|||

ttttttttTtTt xPxPPx

11~~~

ttt wGxAx

1t t tx Ax Gw

1111|

1|| )(

tttttTt PPP

Forward dynamics:

Backward dynamics:

tttt Px || and ˆ

1|1| and ˆ tttt Px

Fusion Of Guassian Posterior Probability

T

T

M

M MM R

x

z

1

1 1 -1 1

ˆ ( )

( )

T T

T T

x M M M R z

M R M M R z

z Mx v

1

1 1 1

( )

( )

T T

T

P M M M R M

M R M

where is independent of and has covariance v x RCovariance matix of ( , ) is,x z

1 2 1 2Problem is to fuse ( | ) and ( | ) into ( | , )P x z P x z P x z z

1 2 1 2, and random variables, and given , and are independentx z z x z z

13.36,eqn usingby )|( estimatecan We zxP

Fusion Cont’dx

1z 2z

1 1

2 2

z M x vz M x v

1 2 1 2 and are independent of and has covariance matrices and v v x R R

1 1 1 11 1 1 1 1 1 1

1 1 1 12 2 2 2 2 2 2

ˆ ( )ˆ ( )

T T

T T

x M R M M R z

x M R M M R z

1 1 11 1 1 1

1 1 12 2 2 2

( )

( )

T

T

P M R M

P M R M

1

2

MM

M

1

2

00 R

RR

1 2To calculate ( | , ),P x z z

1 1 1 11 2( )P P P P

1 11 1 2 2ˆ ˆ ˆ( )x P P x P x