Instituto Tecnológico de Aeronáutica

EEC-D - Dispositivos e Sistemas Eletrônicos

EA-292
Profa. Neusa Maria Franco de Oliveira
Prof.Karl H. Kienitz

Problema:
Suponha que desejamos estudar o movimento de um veículo que se locomove em linha
reta. Consideremos como estados do sistema a posição p e a velocidade v. A entrada do
sistema é a aceleração u.

Em um sistema ideal, a velocidade do veículo pode ser calculada por:

vk +1 = vk + Tuk

No entanto, o sistema está sujeito a perturbações e ruídos, de modo que um modelo mais
realista para v é:
vk +1 = vk + Tuk + vk−
onde vk- é o ruído de velocidade no instante k.

A posição p pode ser modelada como:

1
pk +1 = pk + Tvk + T 2uk + pk−
2

onde pk- é o ruído da posição no instante k.

p 
Definindo o vetor de estados como xk =  k  , podemos escrever o sistema linear de
 vk 
equações na forma matricial:

1 T  T 2 / 2 
xk +1 =  x +
 k   uk + wk
0 1   T 
yk = [1 0] xk + zk
 pk− 
com wk =  −  , cujos elementos já foram definidos, e zk o ruído de medida devido aos
 vk 
erros de sensores.

Quando se deseja controlar o veículo através de malhas de realimentação, é necessário se

ter uma estimativa com acurácia da posição p e da velocidade v. Em outras palavras,
deseja-se ter um modo de estimar o estado x.
Filtro de Kalman

O problema proposto, em outras palavras, é:

Quer-se utilizar as medidas y disponíveis para estimar o estado x do sistema. Deseja-se

um estimador que nos dê uma estimativa acurada dos verdadeiros estados, apesar de não
podermos medir diretamente estes estados.

Que critérios este estimador deveria satisfazer?

1- Deseja-se que o valor médio da estimativa de estados seja igual ao valor médio
dos estados verdadeiros. Dito matematicamente, queremos que o valor esperado
da estimativa seja igual ao valor esperado do estado.
2- Queremos que o valor estimado divirja do valor verdadeiro dos estados o mínimo
possível. Ou seja, não apenas queremos que a média dos estados estimados seja
igual à média dos estados verdadeiros, como também queremos um estimador que
dê a menor variação possível no estado estimado. Dito matematicamente,
queremos encontrar um estimador com a variância do erro a menor possível.

O Filtro de Kalman satisfaz estes dois requisitos!

Exercício: Encontre nas notas de aulas as passagens onde estão matematicamente

provados o cumprimento dos dois requisitos.

Porém, para a aplicação da solução do Filtro de Kalman é necessário que sejam satisfeitas
certas hipóteses sobre os ruídos que afetam o sistema, que aqui foram estabelecidos
anteriormente como w, o ruído de estado, e z, o ruído de medida.

Por hipótese temos que o valor médio de w é zero e que o valor médio de z é zero.
Também, por hipótese, não existe correlação entre w e z. Ou seja, em qualquer instante k,
wk e zk são variáveis aleatórias independentes. Sendo assim,as matrizes de covariância de
ruído Sw e Sz são definidas como:

Covariância de ruído de estado: S w = E ( wk wkT )

Covariância de ruído de medida: S z = E ( zk zkT )

Como foi visto anteriormente, existem várias formulações alternativas para o FK. Uma
delas é:

K k = APk C T ( CPk C T + S z )
−1

xˆk +1 = ( Axˆk + Buk ) + K k ( yk +1 − Cxˆk )

Pk +1 = APAT + S w − APk C T S z−1CPk AT
A equação de K mostra que se o ruído de medida é grande, Sz será grande, logo K será
pequeno e não se dará muita credibilidade à medida y quando de faz o cálculo do
próximo x̂ . Por outro lado, se o ruído de medida é pequeno, Sk será pequeno, logo K será
grande e se dará bastante credibilidade à medida quando se faz o cálculo do próximo x̂ .

Problema específico
Considere um veículo viajando ao longo de uma estrada. A posição é medida com um
erro de 10 feet (desvio padrão). A entrada do sistema é uma aceleração constante de 1
foot/s2. O ruído de aceleração é 0.2feet/s2 (desvio padrão). A posição é medida 10 vezes
por segundo (T=0.1).

Como podemos fazer a melhor estimativa do veículo em movimento?

Como o ruído de medida é grande, espera-se poder fazer obter uma informação melhor
do que simplesmente usar o valor medido.

Utilizando o tempo de amostragem T=0.1s, o modelo linear que representa o sistema de

interesse é escrito como:

1 0.1  0.005
xk +1 =   xk +   uk + wk
0 1   0.1 

yk = [1 0] xk + zk

Breve revisão sobre variáveis aleatórias

Sejam x1, x2, ... , xn variáveis aleatórias com desvios padrão σ1, σ2, ..., σn, com variâncias
σ12, σ22, ..., σn2 e covariâncias σ12, σ13, ..., σ(n-1)n.

Ou seja,
σ i2 = E ( xi − E ( xi ) )  - variância da variável aleatória xi
2

 
σ ij = E ( xi − E ( xi ) ) ( x j − E ( x j ) )  , i ≠ j - covariância das variáveis xi e xj

Reunindo as variâncias e covariâncias em uma matriz, ficamos com

 σ 12 σ 12 L σ 1n 
 
 σ 12 σ 22 L σ 2 n 
Var ( X ) =
 M M M 
 2 
σ 1n σ 2 n L σ n 
Esta é a matriz de covariância ou matriz de dispersão das variáveis aleatórias x1, x2, ... ,
xn. Ela é simétrica e o elemento de posição i,i é a variância da variável xi e o elemento de
posição i,j, para i≠j, é a covariância entre as variáveis xi e xj.

Matrizes de covariância

1 – Matriz de covariância do ruído de medida

Sz = E(ZkZkT) , onde Zk é o ruído de medida

Como o desvio padrão do ruído de medida é 10 feet, a matriz Sz é igual a 100.

2 – Matriz de covariância de erro de estado Sw

a) Variância do ruído de posição

Como a posição é proporcional a 0.005×aceleração e o ruído de aceleração é a- =
0.2 feet/s2, a variância do ruído de posição é:

 T 2 T

−  T −
2
E  .a  .a   = E ( 0.005 × 0.2 )( 0.005 × 0.2 )  = 10−6
T

 2  2    

b) Variância do ruído de velocidade

Como a velocidade é proporcional a 0.1×aceleração e o ruído de aceleração é a- =
0.2 feet/s2, a variância do ruído de velocidade é:
E (T × a − )(T × a − )  = E ( 0.1× 0.2 )( 0.1× 0.2 )  = 4 ×10−4
T T

   

c) A covariância entre o ruído de posição e o ruído de velocidade é igual ao desvio

padrão do ruído de posição vezes o desvio padrão do ruído de velocidade, e pode
ser calculada como:

 T 2  
E  a −  (Ta − )  = E ( 0.005 × 0.2 )( 0.1× 0.2 )  = 2 ×10 −5
 2  

Assim, podemos escrever a matriz de covariância de estado:

  p   p2 pv   10−6 2 × 10−5 
S w = E ( xxT ) = E    [ p v ]  = E  = 
v    vp v 2   2 × 10−5 4 × 10−4 
Problema a ser resolvido utilizando o FK em Matlab:

Considere um veículo viajando ao longo de uma estrada. A posição é medida com um

erro de 10 feet (desvio padrão). A entrada do sistema é uma aceleração constante de 1
foot/s2. O ruído de aceleração é 0.2feet/s2 (desvio padrão). A posição é medida 10 vezes
por segundo (T=0.1). Como podemos fazer a melhor estimativa do veículo em
movimento?
Modelo linear do sistema:

1 0.1  0.005
xk +1 =   xk +   uk + wk
0 1   0.1 

yk = [1 0] xk + zk

A formulação de Filtro de Kalman que usaremos:

K k = APk C T ( CPk C T + S z )
−1

xˆk +1 = ( Axˆk + Buk ) + K k ( yk +1 − Cxˆk )

Pk +1 = APAT + S w − APk C T S z−1CPk AT
onde,

1 0.1 0.005
A=   , B=   , C= [1 0] , Sz e Sw como calculadas acima.
0 1   0.1 

Tomando como entrada as posições medidas posmeas (vetor dado), implemente o filtro
de Kalman para o problema, apresentando como resultado a posição verdadeira (posição
calculada usando a aceleração nominal somada a um ruído com as características dadas),
a posição medida (informação dada no vetor posmeas) e a posição estimada.
ERROR: undefined
OFFENDING COMMAND:

STACK: