ee240/2009 filtragem estocástica
DESCRIPTION
EE240/2009 Filtragem Estocástica. Unscented Kalman Filter Extended Kalman Filter. Tempo Contínuo. Filtro de Fujisaki-Kalliampur-Kunita Filtro de Wonham-Stratonovich Filtro de Zakai Filtro Robusto de Davis. Tempo Discreto. Sistema Linear Ruídos Gaussianos. Sistema Não-Linear - PowerPoint PPT PresentationTRANSCRIPT
EE240/2009
Sistema LinearRuídos Gaussianos
Filtro de Kalman
Sistema Não-LinearRuídos Não Gaussianos
Filtro de Partículas Unscented Kalman FilterExtended Kalman Filter
Tempo Discreto
Tempo Contínuo
Filtro de Fujisaki-Kalliampur-KunitaFiltro de Wonham-StratonovichFiltro de ZakaiFiltro Robusto de Davis
EE240/2009
zk
xk
zk-1
xk-1
p(xk|xk-1)p(zk|xk)
zk-2
xk-2
kkk
k1kk
vxgzwxfx
,,
Estado:
Observações: kk
1kk
xzxx|p|p
Dadas as equações de Estado e das Observações:
Obter:
N10k zzzxE ,...,,|N = k Filtragem
N < k Predição
N > k Suavização)zzzx( k10k ,...,,|p
EE240/2009
P( A | B ) = P( A B ) / P ( B )
ABA B
P( B | A ) = P( A B ) / P ( A )
Fórmula de BayesP( B | A ) P ( A ) = P( A | B ) P ( B )
P( B | A ) =P( A | B ) P ( B )
P ( A )
Regra de Bayes
EE240/2009
)p()p()|p(
)|p( 00
0000 x
zxz
zx
Updatep(x0)
p(x0|z0)
z0
P( B | A ) =P( A | B ) P ( B )
P ( A )
EE240/2009
)p()p()|p(
)|p( 00
0000 x
zxz
zx
PropagateUpdatep(x0)
p(x0|z0) p(x1|z0)
z0
0000101 dxzxxxzx )|p()|p()|p(
EE240/2009
)p()p()|p(
)|p( 00
0000 x
zxz
zx
PropagateUpdate Updatep(x0)
p(x0|z0) p(x1|z1)p(x1|z0)
z0 z1
0000101 dxzxxxzx )|p()|p()|p(
)|p()|p()|p(
)|p( 0101
1111 zx
zzxz
zx
EE240/2009
)p()p()|p(
)|p( 00
0000 x
zxz
zx
PropagateUpdate PropagateUpdatep(x0)
p(x0|z0) p(x1|z1)p(x1|z0) p(x2|z1)
z0 z1
1111212 dxzxxxzx )|p()|p()|p(
0000101 dxzxxxzx )|p()|p()|p(
)|p()|p()|p(
)|p( 0101
1111 zx
zzxz
zx
EE240/2009
)p()p()|p(
)|p( 00
0000 x
zxz
zx
PropagateUpdate Propagate PropagateUpdate Update…p(x0)
p(x0|z0) p(x1|z1)p(x1|z0) p(x2|z1) p(xk|zk-1)
z0 z1 zk-1
1k1k1k1kk1kk dxzxxxzx )|p()|p()|p(
p(xk-1|zk-1)
)|p()|p()|p(
)|p( 2k1k2k1k
1k1k1k1k zx
zzxz
zx
1111212 dxzxxxzx )|p()|p()|p(
0000101 dxzxxxzx )|p()|p()|p(
)|p()|p()|p(
)|p( 0101
1111 zx
zzxz
zx
EE240/2009
M
m
mk Mx
1
)(1
1,
x
Mmmk
mk wx 1
)()( ,
M
m
m
kM
x1
)(~ 1,
M
m
mk Mx
1
)(1
1,
Mmmk
mk wx 1
)(1
)(1 ,
M
m
m
kM
x1
)(
1
~ 1,
M
m
mk Mx
1
)(2
1,
EE240/2009
Dados: { y1, .. .,yn } correlacionados com x
Obter: uma estimativa de x a partir de { y1, .. .,yn }
Problema:
Estimador g(.)
)(),...,,(ˆ 21 Ygyyygx n
Exemplos:
1
121
121
121
1),...,,(
),...,,(
1),...,,(
n
k kn
nn
kkn
n
kkn
yyyyg
yyyyg
yn
yyyg
EE240/2009
Projeção Ortogonal
Achar g(.) de modo que se minimize:
Critério para determinação do g(.):
]|))(([][ 2 YYgxEgJ
x
y
Projeção Ortogonal:
Seja g(y) = K y
Achar K de modo que
Minimiza
)(ˆ ygx
2x̂xJ
yyKxyKxx
KyxKyxxxxxxx
||2|
|
ˆ|ˆˆ
2
2
EE240/2009
0min*
KKK dKdJJ yyKxyKxxxxJ ||2|ˆ 22
0|2|2 **
yyKxydKdJ
KK
yyxyK
||
yyyxyyKygx
||)(ˆ
yyxyx
yyy
xyxy
),cos(
),cos(
x
yx̂
.yxxx ˆ~
EE240/2009
Problema Original
},...,{),...,(ˆ
1
1
n
nyyspansobrexproj
yygx
niyxxx i ,...,1,ˆ~
ii
ii
i
yxyxyxyx
yxx
|ˆ|0|ˆ|
0|ˆ
a, b ~ N(0,2)< a | b > = E [ a b ]
EE240/2009
inni yyKyKyx ||ˆ 11
inni yyKyyK ||11
in
i
nyy
yyKK
|
|1
1
Expressão para : iyx |ˆ
nnn yKyKxyyspanx 111 ˆ},...,{ˆ
n
ny
yKK
1
1 YKK n1
EE240/2009
nn
n
n
nnyy
yy
yy
yyKKyxyx
|
|
|
|||
1
1
11
11
ii yxyx |ˆ|
nn
n
n yy
yy
yy
yyP
|
|
|
| 1
1
11
111 || PyxyxKK nn
YPxYE
YPyxyx
YKKx
Tn
n
1
11
1
||
ˆ
EE240/2009
Estimação Recursiva
Fórmula de Recursão para Projeções Ortogonais
Suponha que y1, ... ,yn sejam dados seqüencialmente e que
nn yyxdeestimativax ,,|ˆ 1
Problema: Calcular
11 ˆˆ nnn yexdadox
Proposta:
11 ˆˆ nnnnn ybxax
EE240/2009
Lk = span { y1, ... , yk } Lk-1 é subespaço de Lk
Para um z genérico em L,
z = z1 + z2
1 kL 1|~
kky1 kL
yk projetado sobre Lk-1 :ˆ 1|kky
Lk-1
yk
1|ˆ kky
1|1| ˆ~ kkkkk yyy
.
EE240/2009
z = z1 + z2
1 kL
Em particular, se z = kx̂kx̂ = z1 + z2
1 kL
Portanto, kkk xzzxxx ~~ˆ 21
x
z1 z2
kxz ˆ
kx~
Lk-1
Lk
EE240/2009
z = z1 + z2
1 kL
Em particular, se z = kx̂kx̂ = z1 + z2
1 kL
1 kL 1 kL
Portanto, kkk xzzxxx ~~ˆ 21
1 kL
1 kL
11~ˆ kk xxx11 ˆ kxz
x
z1 z2
kx~
Lk-1
Lk
kx~ -1
EE240/2009
inovações
1|2~
kkysobrexprojz
x
z2
Lk-1
Lk
Fórmula de Projeçãode x sobre Y
YPxYEx T 1ˆ
1|1
1|1|1|2~]~~[]~[
kk
Tkkkk
Tkk yyyEyxEz
11 ˆ kxzkx̂ = z1 + z2
1|1
1|1|1|1~]~~[]~[ˆˆ
kk
Tkkkk
Tkkkk yyyEyxExx
)ˆ(]~~[]~[ˆˆ 1|1
1|1|1|1
kkkT
kkkkT
kkkk yyyyEyxExx
1|~
kky
yk
kx̂
11 ˆ kxz
EE240/2009
Filtro de KalmanFiltro de Kalman
Dado um sistema:
xk+1 = Axk + Buk +Cwk
yk = Hxk + Gvk
y0 = 0
kk vw
Obter:
kx̂ a partir de { y0 ,..., yk }
Ruído de Estado
Ruído de Medida
QwwE Tkk ][
RvvE Tkk ][
x0 ~N(m,P0)
i.i.d.
i.i.d.
EE240/2009
)ˆ(]~~[]~[ˆˆ 1|1
1|1|1|1||
kkkT
kkkkT
kkkkkkk yyyyEyxExx
Escrevendo xk no lugar de x na fórmula da projeção:
Cálculo de ]~[ 1|T
kkk yxE
kkk
kkkkkk
GvxHxHGvHxy
1|
1|1|~
ˆ~
1|
10
10
1|1|
ˆ],...,|[
],...,|[
ˆ~
kkk
kkk
kkk
kkkkk
xHyyyHxEy
yyyEyyyy
Tk
TTkkkk
TTkkkkkk
TTkkk
Tkkkk
Tkkk
HP
HxxE
HxxxE
HxxE
GvxHxEyxE
]~~[
]~~ˆ[
]~[
])~([]~[
1|1|
1|1|1|
1|
1|1|
)ˆ(]~~[]~[ˆˆ 1|1
1|1|1|1
kkkT
kkkkT
kkkk yyyyEyxExx
EE240/2009
)ˆ(]~~[]~[ˆˆ 1|1
1|1|1|1||
kkkT
kkkkT
kkkkkkk yyyyEyxExx
Escrevendo xk no lugar de x na fórmula da projeção:
kkk
kkkkkk
GvxHxHGvHxy
1|
1|1|~
ˆ~
1|
10
10
1|1|
ˆ],...,|[
],...,|[
ˆ~
kkk
kkk
kkk
kkkkk
xHyyyHxEy
yyyEyyyy
Cálculo de ]~~[ 1|1|T
kkkk yyE
TTk
TTkk
TTkkkk
Tkkkkkk
Tkkkk
GRGHHP
GvvGEHxxHE
GvxHGvxHE
yyE
1|1|
1|1|
1|1|
~~
~~
~~
EE240/2009
Por outro lado,
xk+1 = Axk + Cwk
kkkkkk wCxAx |||1 ˆˆˆ
Já calculadoFórmula de Projeção
de x sobre Y
YPxYEx T 1ˆ
1|1
1|1|1||~]~~[]~[ˆ
kk
Tkkkk
Tkkkkk yyyEywEw
Já calculado
EE240/2009
Cálculo de ]~[ 1|T
kkk ywE
0
]~[
])~([]~[
1|
1|1|
TTkk
TTkkk
Tkkkk
Tkkk
GvwHxwE
GvxHwEywE
]ˆ[ˆ
ˆ
ˆˆˆ
1|1
1|
|
|||1
kkkTT
kT
kkk
kk
kkkkkk
xHyGRGHHPHAPxA
xAwCxAx
Fórmula para o estado:
1 TT
kT
kk GRGHHPHPK
]ˆ[ˆˆ 1|1||1 kkkkkkkk xHyAKxAx
EE240/2009
Cálculo de ]~~[ |1|11T
kkkkk xxEP
kkkk
kkkkk
kkkkkkkk
kkkkkkkkk
kkkkkkk
kkkkk
CwxHKACwxxHKA
CwxxHKxxAxHKHxKxACwAx
xHyKxAxxxx
1|
1|
1|1|
1|1|
1|1|1
|11|1
~ˆ
ˆˆ
ˆˆ]ˆ[ˆ
ˆ~
TTkkk
TTkk
Tk
Tkkkkk
Tkkkkkkkk
Tkkkkk
CQCHKAPHKA
CwwECHKAxxEHKA
CwxHKACwxHKAE
xxEP
][]~~[
])~()~([
]~~[
1|1|
1|1|
|1|11
EE240/2009
kkkk xAx ||1 ˆˆ
TTkkkk CQCHKAPHKAP 1
1|1|| ˆˆˆ kkkkkkkk xHyKxx
1 TT
kT
kk GRGHHPHAPKAtualização
Propagação
xk-1|k-1xk-1|k-2 xk|kxk|k-1 xk+1|k+1xk+1|k
yk-1 yk yk+1
Kk Kk+1Kk-1
EE240/2009
0 1 2 3 4 5 6 7 8 9 10-2
0
2
4y
0 1 2 3 4 5 6 7 8 9 10-1
0
1
2
x1
0 1 2 3 4 5 6 7 8 9 10-2
0
2
4
x2
EE240/2009
Mecanização do Filtro de Kalman:
xk+1 = Axk + Buk +Cwk
yk = Hxk + vk
y0 = 0
kk vw
QwwE Tkk ][
RvvE Tkk ][
x0 ~ N(m,P0)
i.i.d.
i.i.d.
TTkk
kkk
CQCAAPP
BuxAx
1
ˆˆ
Propagação:
Atualização:
kkk
kkkkk
Tk
Tkk
PHKIP
xHyKxx
RHHPHPKˆˆˆ
1
EE240/2009
Problemas Numéricos:
kkk PHKIPSimétrico
Positivo Definido
Fórmula de Joseph:
kkkkk xHyKxx ˆˆˆ
kkkkk yKxHKIx ˆˆ
Tk
Tkkk
Tkkk
Tkkk KyyEKHKIPHKIxxEP ][]ˆˆ[
Tkk
Tkkkk KRKHKIPHKIP
EE240/2009
Covariança Inversa:
kkk PHKIP
111
HRHPP T
kk
Lema de Inversão de Matrizes:1111111 )()( DACBDABAADBCA
kT
kT
kkk PHRHHPHPPP1
1 RHHPHPK Tk
Tkk
EE240/2009
Fatorização da Matriz P:
P = S ST
~ [10-N,10N ] ~ [10-N/2,10N/2 ]
Filtro Raiz Quadradade Potter
P+ = U+ D+ (U+)T
P- = U- D- (U-)T
Filtro U-D deBierman
EE240/2009
Erros de Modelamento:
Modelo Medida
1 RHHPHPK Tk
Tkk
TTkk CQCAAPP
1
kkkk xxKPQ ˆˆ
kkkkk xHyKxx ˆˆˆ
kkkkk xHyAKxAx ˆˆˆ 1
EE240/2009
0 1 2 3 4 5 6 7 8 9 10-5
0
5y
0 1 2 3 4 5 6 7 8 9 10-2
0
2
4
x1
0 1 2 3 4 5 6 7 8 9 10-10
0
10
20
x2
EE240/2009
Filtro de Kalman Linearizado
kkk
kkkkvxhy
Cwuxfx
)(),(1
...),,0(~),0(~ diiRNveQNw kk
Dado uk.
)(
),(1nomk
nomk
knomk
nomk
xhy
uxfx
nomkkk
nomkkk yyyxxx
knomkk
nomkk
kknomkkk
nomkk
vxhxhyy
Cwuxfuxfxx
)()(
),(),(11
EE240/2009
knomkk
nomkk
kknomkkk
nomkk
vxhxxhy
Cwuxfuxxfx
)()(
),(),(1
oxxhxhxxh
oxuxfuxfuxxf
knomkx
nomkk
nomk
kknomkxk
nomkkk
nomk
)()()(
),(),(),(Fórmula de Taylor:
kknomkk
kkknomkxk
vxxhy
Cwxuxfx
)(
),(1
Filtro de Kalman Estendido
kkkk
kkkkxkvxxhy
Cwxuxfx
)ˆ(),ˆ(
1
1