EE240/2009

EE240/2009Filtragem Estocástica

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

Filtro de Partículas

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

xk

tp(xk)

EE240/2009

t = 0

EE240/2009

t = 1

EE240/2009

t = 2

EE240/2009

t = 0

z

pz

z

EE240/2009

t = 0

x

z

pz

px

Start!

EE240/2009

t = 0

x

z

pz

px

EE240/2009

t = 0

x

z

pz

px

EE240/2009

t = 0

z

x

z

pz

px

EE240/2009

t = 0

z

x

z

pz

px

EE240/2009

t = 0

x

z

pz

px

EE240/2009

t = 1

x

z

pz

px

EE240/2009

t = 1

x

z

pz

px

EE240/2009

t = 1

x

z

pz

px

z

EE240/2009

t = 1

x

z

pz

px

EE240/2009

x

z

pz

px

t = 2

EE240/2009

x

z

pz

px

t = 2

EE240/2009

Filtro de Kalman

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

)ˆ(]~~[]~[ˆˆ 1|1

1|1|1|1

kkkT

kkkkT

kkkk yyyyEyxExx

Importante!

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 Estendido

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

EE240/2009

Muito Obrigado!