Kalman Filtering And Smoothing Jayashri
Welcome message from author
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
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
Related Documents
