EE240/2009
Filtragem Estocástica
EE240/2009
Tempo Discreto
Sistema Linear
Ruídos Gaussianos
Sistema Não-Linear
Ruídos Não Gaussianos
Filtro de Partículas
Unscented Kalman Filter
Extended Kalman Filter
Filtro de Kalman
Tempo Contínuo
Filtro de Fujisaki-Kalliampur-Kunita
Filtro de Wonham-Stratonovich
Filtro de Zakai
Filtro Robusto de Davis
EE240/2009
Filtro de Partículas
EE240/2009
Dadas as equações de Estado e das Observações:
x k  f x k 1 , w k 
Estado:
px k | x k 1 
pz k | x k 
Observações: z k  gx k , v k 
zk-2
zk
zk-1
p(zk|xk)
xk-2
xk-1
p(xk|xk-1)
xk
Obter:
E  x k  | z 0 , z1 ,...,z N 
p( x k | z 0 ,z1 ,...,z k )
N = k  Filtragem
N < k  Predição
N > k  Suavização
EE240/2009
Regra de Bayes
A
AB
P( A | B ) = P( A  B ) / P ( B )
B
P( B | A ) = P( A  B ) / P ( A )
P( B | A ) P ( A ) = P( A | B ) P ( B )
Fórmula de Bayes
P( B | A ) =
P( A | B ) P ( B )
P(A)
EE240/2009
z0
p(x0)
Update
p(x0|z0)
p( x 0 | z 0 ) 
p( z 0 | x 0 )
p( x 0 )
p( z 0 )
P( B | A ) =
P( A | B ) P ( B )
P(A)
EE240/2009
z0
p(x0)
Update
Propagate
p(x0|z0)
p( x 0 | z 0 ) 
p(x1|z0)
p( z 0 | x 0 )
p( x 0 )
p( z 0 )
p( x 1 | z 0 )   p( x 1 | x 0 ) p( x 0 | z 0 )dx 0
EE240/2009
z0
p(x0)
z1
Update
Propagate
p(x0|z0)
p( x 0 | z 0 ) 
p(x1|z0)
Update
p(x1|z1)
p( z 0 | x 0 )
p( x 0 )
p( z 0 )
p( x 1 | z 0 )   p( x 1 | x 0 ) p( x 0 | z 0 )dx 0
p( x 1 | z1 ) 
p( z1 | x 1 )
p( x 1 | z 0 )
p( z1 | z 0 )
EE240/2009
z0
p(x0)
z1
Update
Propagate
p(x0|z0)
p( x 0 | z 0 ) 
p(x1|z0)
Update
Propagate
p(x1|z1)
p(x2|z1)
p( z 0 | x 0 )
p( x 0 )
p( z 0 )
p( x 1 | z 0 )   p( x 1 | x 0 ) p( x 0 | z 0 )dx 0
p( x 1 | z1 ) 
p( z1 | x 1 )
p( x 1 | z 0 )
p( z1 | z 0 )
p( x 2 | z1 )   p( x 2 | x 1 ) p( x 1 | z1 )dx 1
EE240/2009
z0
p(x0)
z1
Update
Propagate
p(x0|z0)
p( x 0 | z 0 ) 
p(x1|z0)
zk-1
Update
Propagate
p(x1|z1)
…
p(x2|z1)
Update
Propagate
p(xk-1|zk-1)
p(xk|zk-1)
p( z 0 | x 0 )
p( x 0 )
p( z 0 )
p( x 1 | z 0 )   p( x 1 | x 0 ) p( x 0 | z 0 )dx 0
p( x 1 | z1 ) 
p( z1 | x 1 )
p( x 1 | z 0 )
p( z1 | z 0 )
p( x 2 | z1 )   p( x 2 | x 1 ) p( x 1 | z1 )dx 1
p( x k 1 | z k 1 ) 
p( z k 1 | x k 1 )
p( x k 1 | z k 2 )
p( z k 1 | z k 2 )
p( x k | z k 1 )   p( x k | x k 1 ) p( x k 1 | z k 1 )dx k 1
EE240/2009
M
M
 ~ ( m) 1 
 x k 1 , 
M m 1

 (m) 1 
 xk  2 , 
M  m 1

x
(m)
k 1
, wk( m1)

M
m 1
M
M
 ~ ( m) 1 
xk , 
M m 1

 (m) 1 
 xk 1 , 
M  m 1

x
(m)
k
, wk( m )

M
m 1
M
 (m) 1 
 xk 1 , 
M  m 1

x
EE240/2009
t
p(xk)
xk
EE240/2009
t=0
EE240/2009
t=1
EE240/2009
t=2
EE240/2009
z
t=0
pz
z
EE240/2009
Start!
t=0
pz
z
px
x
EE240/2009
t=0
pz
z
px
x
EE240/2009
t=0
pz
z
px
x
EE240/2009
z
t=0
pz
z
px
x
EE240/2009
z
t=0
pz
z
px
x
EE240/2009
t=0
pz
z
px
x
EE240/2009
t=1
pz
z
px
x
EE240/2009
t=1
pz
z
px
x
EE240/2009
z
t=1
pz
z
px
x
EE240/2009
t=1
pz
z
px
x
EE240/2009
t=2
pz
z
px
x
EE240/2009
t=2
pz
z
px
x
EE240/2009
Filtro de Kalman
EE240/2009
Problema:
Dados:
{ y1, .. .,yn } correlacionados com x
Obter:
uma estimativa de x a partir de { y1, .. .,yn }
Estimador g(.)
xˆ  g( y1 , y2 ,..., yn )  g( Y )
Exemplos:

1 n
g( y1 , y2 ,..., yn )    yk 
n  k 1 
g( y1 , y2 ,..., yn )  n
n
 yk
k 1
 n 1 

g( y1 , y2 ,..., yn )   
 k 1 yk 
1
EE240/2009
Projeção Ortogonal
Critério para determinação do g(.):
Achar g(.) de modo que se minimize:
J[ g ]  E[ (x  g(Y) )2 | Y ]
Projeção Ortogonal:
x
Seja g(y) = K y
Achar K de modo que xˆ  g ( y)
Minimiza J  x  xˆ
2
x  xˆ
2
y
 x  xˆ | x  xˆ 
 x  Ky| x  Ky 
 x | x  2K  y | x  K 2  y | y 
EE240/2009
min J 
K
dJ
0
dK K K*
J  x  xˆ
2
  x | x   2K  y | x   K 2  y | y 
dJ
  2  y | x   2K*  y | y   0
dK K K*
K
xˆ  g( y)  K y 

 y|x 
y
 y|y 
 y|x 
 y|y 
x
~  x  xˆ  y
x
.
y x cos(y, x )
y
y y
y
 x cos(y, x )
y
y
xˆ
EE240/2009
Problema Original
xˆ  g ( y1 ,..., yn )
 proj x  sobre span { y1 ,..., yn }
~  x  xˆ  y , i  1,...,n
x
i
a, b ~ N(0,2)
<a|b>=E[ab]
 x  xˆ | yi   0
 x | yi    xˆ | yi   0
 x | yi    xˆ | yi 
EE240/2009
Expressão para  xˆ | yi  :
xˆ  span { y1 ,..., yn }  xˆ  K1y1    Knyn
 y1 
 K1  Kn     K1
 
 yn 
 Kn  Y
 xˆ | yi    K1y1    Knyn | yi 
 K1  y1 | yi     Kn  yn | yi 
 K1
  y1 | yi  

 Kn 



 yn | yi  
EE240/2009
 x | yi    xˆ | yi 
 x | y1 
  x | yn   K1
  y1 | y1    y1 | yn  

 Kn 





 yn | y1    yn | yn  
  y1 | y1    y1 | yn  

P





 yn | y1    yn | yn  
 K1
 Kn    x | y1    x | yn  P1
 xˆ  K1  Kn  Y
  x | y1    x | yn   P 1Y
 
 E xYT P 1Y
EE240/2009
Estimação Recursiva
Fórmula de Recursão para Projeções Ortogonais
Suponha que y1, ... ,yn sejam dados seqüencialmente e que
xˆ n  estimativa de x | y1 ,, yn
Problema: Calcular
xˆ n  1 dado xˆ n e yn  1
Proposta:
xˆ n  1  an xˆ n  bn yn  1
EE240/2009
Lk = span { y1, ... , yk }
Lk-1 é subespaço de Lk
yˆ k|k 1 : yk projetado sobre Lk-1
yk
~
ˆ k|k 1
yk|k 1  yk  y
yˆ k|k 1
.
Lk-1
Para um z genérico em L,
z = z1 + z2
 Lk 1
~
yk|k 1
 Lk 1
EE240/2009
Em particular, se z = xˆ k
z = z1 + z2
 Lk 1
xˆ k = z1 + z2
 Lk 1
~ z z x
~
Portanto, x  xˆ k  x
k
1
2
k
x
~
x
k
z  xˆ k
z1
Lk
z2
Lk-1
EE240/2009
Em particular, se z = xˆ k
z = z1 + z2
 Lk 1
xˆ k = z1 + z2
 Lk 1
~ z z x
~
Portanto, x  xˆ k  x
k
1
2
k
x
 Lk 1
 Lk 1
 Lk 1
~
x
k
~
x
k -1
 Lk 1
Lk
z1
~
x  xˆ k 1  x
k 1
z1  xˆ k 1
z2
Lk-1
EE240/2009
z 2  proj x  sobre ~
yk|k 1
x
Fórmula de Projeção
de x sobre Y
~
yk |k  1
 
xˆ  E xYT P1Y
z 2  E [x ~
ykT|k 1 ] E [ ~
yk |k 1 ~
ykT|k 1 ]1 ~
yk |k 1
Lk
xˆ k
z1  xˆ k 1
z2
yk
Lk-1
xˆ k = z1 + z2
z1  xˆ k 1
inovações
xˆ k  xˆ k 1  E [x ~
ykT|k 1 ] E [ ~
yk |k 1 ~
ykT|k 1 ]1 ~
yk |k 1
xˆ k  xˆ k 1  E [x ~
ykT|k 1 ] E [ ~
yk |k 1 ~
ykT|k 1 ]1 ( yk  yˆ k |k 1 )
EE240/2009
Importante!
xˆ k  xˆ k 1  E [x ~
ykT|k 1 ] E [ ~
yk |k 1 ~
ykT|k 1 ]1 ( yk  yˆ k |k 1 )
EE240/2009
Filtro de Kalman
Filtro de Kalman
Dado um sistema:
Obter:
xˆ k a partir de { y0 ,..., yk }
xk+1 = Axk + Buk +Cwk
yk = Hxk + Gvk
Ruído de Estado
x0 ~N(m,P0)
y0 = 0
Ruído de Medida
w k  vk
E [vk vkT ]  R
E [wk wkT ]  Q
i.i.d.
i.i.d.
EE240/2009
Escrevendo xk no lugar de x na fórmula da projeção:
xˆ k  xˆ k 1  E [x ~
ykT|k 1 ] E [ ~
yk |k 1 ~
ykT|k 1 ]1 ( yk  yˆ k |k 1 )
xˆ k |k  xˆ k |k 1  E [xk ~
ykT|k 1 ] E [ ~
yk |k 1 ~
ykT|k 1 ]1 ( yk  yˆ k |k 1 )
ykT|k 1 ]
Cálculo de E [xk ~
~
yk|k 1  yk  yˆ k|k 1
 yk  E [ yk | y0 ,..., yk 1 ]
 yk  E [ Hxk | y0 ,..., yk 1 ]
 yk  Hxˆ k|k 1
~
yk|k 1  Hxk  Gvk  Hxˆ k|k 1
~
 Hx
 Gv
k|k 1
k
T
~
E [ xk ~
ykT|k 1 ]  E[xk (Hx

Gv
)
k |k 1
k ]
~ T HT ]
 E[xk x
k |k 1
T
~
~T
 E[xˆ k|k 1  x
k|k 1 xk |k 1H ]
T
~
~T
 E[x
k |k 1xk|k 1 ]H
 Pk HT
EE240/2009
Escrevendo xk no lugar de x na fórmula da projeção:
xˆ k |k  xˆ k |k 1  E [xk ~
ykT|k 1 ] E [ ~
yk |k 1 ~
ykT|k 1 ]1 ( yk  yˆ k |k 1 )
yk|k 1 ~
ykT|k 1 ]
Cálculo de E [ ~
~
yk|k 1  yk  yˆ k|k 1
 yk  E [ yk | y0 ,..., yk 1 ]
 yk  E [ Hxk | y0 ,..., yk 1 ]
 yk  Hxˆ k|k 1
~
yk|k 1  Hxk  Gvk  Hxˆ k|k 1
~
 Hx
 Gv
k|k 1

E ~
yk|k 1 ~
ykT|k 1



~
~
 E Hx
k|k 1  Gvk Hxk |k 1  Gvk




T 
T
T T
~
~T
 HE x
k |k 1xk |k 1 H  GE vk vk G
 HPkHT  GRGT
k
EE240/2009
Por outro lado,
xk+1 = Axk + Cwk
ˆ k|k
xˆ k 1|k  Axˆ k|k  Cw
Já calculado
Fórmula de Projeção
de x sobre Y
 
xˆ  E xYT P1Y
ˆ k|k  E [ wk ~
w
ykT|k 1 ] E [ ~
yk|k 1 ~
ykT|k 1 ]1 ~
yk|k 1
Já calculado
EE240/2009
ykT|k 1 ]
Cálculo de E [ w k ~
T
~
E [wk ~
ykT|k 1 ]  E[ w k (Hx
k|k 1  Gvk ) ]
~ T HT  w v T GT ]
 E[ w k x
k|k 1
k k
0
Fórmula para o estado:
ˆ k|k
xˆ k 1|k  Axˆ k|k  Cw
 Axˆ k|k

 Axˆ k|k 1  APkHT HPkHT  GRGT

1

[ yk  Hxˆ k|k 1 ]
Kk  PkHT HPkHT  GRGT
1
xˆ k 1|k  Axˆ k|k 1  AK k [yk  Hxˆ k|k 1 ]
EE240/2009
~
~T
Cálculo de Pk 1  E [ xk 1|k xk 1|k ]
~
ˆ k 1|k
x
k 1|k  xk 1  x
 xk 1  Axˆ k|k 1  Kk [ yk  Hxˆ k|k 1 ]
 Axk  Cwk  Axˆ k|k 1  KkHxk  KkHxˆ k|k 1




 A xk  xˆ k|k 1  KkH xk  xˆ k|k 1  Cwk


 A  KkH xk  xˆ k|k 1  Cwk
~
 A  KkHx
k|k 1  Cwk
~
~T
Pk 1  E [ x
k 1|k xk 1|k ]
T
~
~
 E [ (A  KkHx
k|k 1  Cwk ) (A  KkHxk|k 1  Cwk ) ]
T
T
T
~
~T
 A  KkHE[x
k|k 1xk|k 1 ]A  KkH  C E [ w k w k ]C
 A  KkHPk A  KkHT  C Q CT
EE240/2009

Kk  APkHT HPkHT  GRGT
1

xˆ k|k  xˆ k|k 1  Kk yk  Hxˆ k|k 1
Atualização

xˆ k 1|k  Axˆ k|k
Propagação
Pk 1  A  KkHPk A  KkHT  C Q CT
Kk-1
xk-1|k-2 xk-1|k-1
yk-1
Kk+1
Kk
xk|k-1
xk|k
yk
xk+1|k
xk+1|k+1
yk+1
EE240/2009
4
y
2
0
-2
0
1
2
3
4
5
6
7
8
9
10
0
1
2
3
4
5
6
7
8
9
10
0
1
2
3
4
5
6
7
8
9
10
2
x1
1
0
-1
4
x2
2
0
-2
EE240/2009
Mecanização do Filtro de Kalman:
xk+1 = Axk + Buk +Cwk
yk = Hxk + vk
y0 = 0
Atualização:
w k  vk
E [wk wkT ]  Q
xˆ k  Axˆ k  Buk
Pk  APk1 A T  CQCT
x0 ~ N(m,P0)
E [vk vkT ]  R
Propagação:
i.i.d.
i.i.d.
Kk
xˆ k
Pk



1
 T
 T
 Pk H HPk H  R
 xˆ k  Kk yk  Hxˆ k
 I  KkH Pk



EE240/2009
Problemas Numéricos:
Simétrico
Pk   I  KkH  Pk
Positivo Definido
Fórmula de Joseph:

xˆ k  xˆ k  Kk yk  Hxˆ k

xˆ k  I  KkHxˆ k  Kk yk
 T ]   I  KkH Pk  I  KkHT  KkE [ yk ykT ]KkT
Pk  E[ xˆ k xˆ k
Pk   I  KkH Pk  I  KkHT  KkR KkT
EE240/2009
Covariança Inversa:
Pk
 

  Pk

1

H R H 

T
1
1
Lema de Inversão de Matrizes:
( A  BC1D )1  A 1  A 1B( DA1B  C )1DA1
Pk  Pk  PkHT HPkHT  R

1HPk

HPkHT
Kk  Pk HT
R

1
Pk   I  KkH  Pk
EE240/2009
Fatorização da Matriz P:
P = S ST
Filtro Raiz Quadrada
de Potter
~ [10-N,10N ]
~ [10-N/2,10N/2 ]
P+ = U+ D+ (U+)T
P- = U- D- (U-)T
Filtro U-D de
Bierman
EE240/2009
Erros de Modelamento:

xˆ k1  Axˆ k  AKk yk  Hxˆ k
Modelo

Medida
Pk  APk1AT  CQCT


1
 T
 T
Kk  Pk H HPk H  R
xˆ k  xˆ k  Kk yk  Hxˆ k


Q   Pk   Kk   xˆ k  xˆ k
EE240/2009
y
5
0
-5
0
1
2
3
4
5
6
7
8
9
10
0
1
2
3
4
5
6
7
8
9
10
0
1
2
3
4
5
6
7
8
9
10
4
x1
2
0
-2
20
x2
10
0
-10
EE240/2009
Filtro de Kalman Estendido
EE240/2009
Filtro de Kalman Linearizado
xk  1  f ( xk , uk )  Cwk
yk  h( xk )  vk
w k ~ N(0, Q) e vk ~ N(0, R ), i.i.d.
Dado uk.
nom
xnom
, uk )
k  1  f ( xk
ynom
 h( xnom
)
k
k
xk  xk  xnom
yk  yk  ynom
k
k
nom
xk  1  xnom
, uk )  Cwk
k  1  f ( x k , uk )  f ( x k
nom
yk  ynom

h
(
x
)

h
(
x
)  vk
k
k
k
EE240/2009
xk  1  f ( xnom
 xk , uk )  f ( xnom
, uk )  Cwk
k
k
yk  h( xnom
 xk )  h( xnom
)  vk
k
k
Fórmula de Taylor:
f ( xnom
 xk , uk )  f ( xnom
, uk )  fx ( xnom
, uk )xk  o
k
k
k
h( xnom
 xk )  h( xnom
)  hx ( xnom
)xk  o
k
k
k
xk  1  fx ( xnom
, uk )xk  Cwk
k
yk  h( xnom
)xk  vk
k
Filtro de Kalman Estendido
xk  1  fx ( xˆ k , uk )xk  Cwk
yk  1  h( xˆ k )xk  vk
EE240/2009
Muito Obrigado!
EE240/2009