Kalman Filtering And Smoothing
-
Upload
katherine-boone -
Category
Documents
-
view
267 -
download
1
description
Transcript of Kalman Filtering And Smoothing
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