Imprimir

Sair

Menu

Aplicao do Filtro de Kalman para a Estimao de Estados em um Problema de Rastreamento

Maria Cecilia Pereira de Faria, Marcelo Lopes de Oliveira e Souza,
INPE Instituto Nacional de Pesquisas Espaciais, Curso de Engenharia e Tecnologia EspaciaisMecnica Espacial e Controle, So Jos dos Campos, SP E-mail: sisa@uai.com.br, marcelo@dem.inpe.br

1. Resumo: r2 - vetor posio de V2 Para uma navegao adequada, veculos v 2 - vetor velocidade de V2 aeroespaciais precisam conter sensores que mediro a1 - vetor acelerao de V1 grandezas fsicas que determinaro sua trajetria e atitude. Porm, estes sensores no oferecem uma F1 - fora aplicada em V1 pelos seus atuadores medida exata do estado do veculo, uma vez que K ganho de Kalman existem rudos nas medidas e erros devidos x - estado estimado de V2 impreciso dos instrumentos. Para se obter y medidas do estado de V1 e do ngulo resultados confiveis, necessrio fazer uma P covarincia das medidas do estado de V1 ponderao entre as medidas obtidas e as previses Q covarincia do processo branco feitas pela teoria. Tal ponderao feita atravs do R rudo na medida de algoritmo do filtro de Kalman h funo que relaciona o estado de V2 s medidas Neste trabalho apresentamos a utilizao do filtro disponveis de Kalman na estimao de estados de um problema de rastreamento de um veculo de dinmica 5. Estimao de estados utilizando o Filtro de desconhecida a partir de medidas de outro veculo, Kalman tal como o rendez-vous entre um navio em mar aberto e um artefato teleguiado. Formulao do problema Este problema de rastreamento envolve dois veculos: 2. Palavras chaves o veculo medidor (V1) e o veculo medido (V2). Veculos Aeroespaciais, Rastreamento, Estimao medida que V1 mede a posio de V2, ele controla a sua de Estados, Filtro de Kalman direo, rastreando V2. V1 possui uma plataforma inercial que permite a medida de sua posio r1 e 3. Introduo Este trabalho tem como objetivo utilizar as velocidade v1 inerciais, alm de um autodiretor que ngulo medidas de posio e de velocidade de um veculo, mede o ngulo da linha de visada e do ngulo que este faz com um segundo veculo entre seu vetor velocidade e o veculo 2). Caso V1 esteja impossibilitado de captar o sinal de com velocidade muito menor que a velocidade do primeiro para estimar a trajetria do segundo V2, ele deve ser capaz de predizer a posio r2 e a veculo. velocidade v 2 de V2 a partir dos dados de medidas Assim, estas estimativas do segundo veculo sero anteriores (mesmo com incertezas e rudos) ainda que feitas a partir de todas as medidas disponveis e, no conhea a dinmica de V2. caso se perca a habilidade de obteno da medida do Para permitir a resoluo deste problema, fizemos ngulo, o procedimento utilizar sua capacidade de algumas consideraes (simplificaes): (i) os predio para estimar suas posies futuras. movimentos dos veculos se do em um mesmo plano; (ii) a velocidade de V2 muito menor que a velocidade Nomenclatura de V1; (iii) os veculos so puntuais; (iv) a distncia V1 Primeiro veculo (rastreador) horizontal entre os veculos tal que o referencial V2 Segundo veculo (rastreado) inercial pode ser fixado na Terra; (v) as variveis de ngulo entre o vetor velocidade de V1 e estado a serem estimadas so [ r , v 2 2 posio de V2 (ngulo da linha de visada) sujeito a rudos do sensor modelados por distribuio r1 - vetor posio de V1 gaussiana de mdia nula e covarincia conhecida; (vii) v1 - vetor velocidade de V1 medidas e equaes so dadas no referencial do corpo 1, fazendo-se necessria, portanto, uma transformao para 4.
6 4 753 1 )' & $ # 20(%" ! ! !       !

Imprimir

Sair

Menu

o referencial inercial; (viii) os valores de r1 e v1 so conhecidos exatamente. Modelamos os veculos seguinte forma: V1 obedecer as leis de movimento, sendo lanado com velocidade conhecida; o veculo 2 tem dinmica desconhecida pela prpria natureza do problema, de forma que faremos sua modelagem cinemtica inicialmente com acelerao nula, havendo movimento retilneo e uniforme somente na horizontal. A Figura 1 a seguir esquematiza o problema:
yy 1
d 8 8

Qualificao do procedimento Para se qualificar o procedimento em tempo real do filtro de Kalman no problema abordado, utilizamos a metodologia descrita abaixo. Inicialmente, simulamos a dinmica de V1. Simulamos tambm o estado real de V2 para avaliao do desempenho do filtro de Kalman. Escolhemos ento valores para P0, R, Q para utilizao no filtro. Simulamos a medida de scimo de rudo, para representar as incertezas envolvidas na medida. A partir destes elementos, o filtro de Kalman deve propagar o estado de V2 e a respectiva covarincia. Havendo mais medidas de o haver
a c pr0VB5VVdr5(`(BB g W g P h a eT U a g Y P Y WU T R P I g q h a q T q g h T a

ngulo da linha de visada

2 x Figura l: Formulao do problema xx x

propagao do estado. Depois de o filtro de Kalman estimar o estado de V2, comparamos os valores estimados com os reais, simulados anteriormente. Caso a diferena seja menor que uma tolerncia escolhida, aceitamos o desempenho do filtro. Caso contrrio, escolhemos outros valores de P0, R, Q, a fim de melhorar os resultados obtidos.

Implementao do filtro de Kalman Para se estimar a posio e a velocidade de V2, As leis dinmicas que governam o movimento de implementamos um filtro de Kalman, que se constitui de duas etapas: propagao e atualizao. Na fase de V1 [1], [2] so: propagao do estado, utilizamos as equaes de movimento dos veculos, considerando uma dinmica r 1x = v1x contnua, com medidas discretas [3], [4]. Para a propagao da covarincia, faremos uma suposio de r 1 y = v1 y valor inicial da covarincia do processo branco, Q, que ser ajustado para melhor desempenho do filtro. Assim, m v 1x = 0 as equaes de propagao do estado do veculo so: m v1 y = mg rk +1 = rk + vk t Este veculo est submetido s condies iniciais: r1x (0 ) = 0 v k +1 = v k enquanto a equao de propagao da covarincia :
T T Pk +1 = k ,k 1 Pk 1 k ,k 1 + k Qk k

v1x (0 ) = v1x 0

r1 y (0 ) = h

v1 y (0 ) = v1 y 0

Pela prpria natureza do problema, tanto a massa 1 t quanto a dinmica do veculo 2 so desconhecidas, k ,k 1 = de forma que no podemos utilizar equaes 0 1 dinmicas para model-la. Portanto, para que suas equaes possam ser utilizadas na formulao do Na fase de atualizao, utilizamos as seguintes Filtro de Kalman, consideraremos apenas suas equaes: equaes cinemticas [1], [2]. Como uma primeira abordagem, consideraremos que o veculo 2 tem T T 1 movimento apenas horizontal, com acelerao nula, K k = Pk H k ( H k Pk H k + Rk ) e velocidade constante. Pk = ( I K k H k ) Pk x k = x k + K k ( y k h( x k ))

onde a matriz de transio

g q P h a P y w v t g q P q a W h g P a eT U a c W a P Y WU T R P urb55sxuu(sr5pi5Sf0VBd`Gb`XVSQI H

F @ D GBEB@ C A

dada por:

Imprimir

Sair

Menu

r1 y arctg r1x r2 x problema no linear especfico. onde H = r2 x 6.

para

4 foi obtido para um valor da covarincia do processo este branco, Q, que assumiu o valor de 1e-6 ao longo de todo o perodo testado.

Resultados Implementamos em MATLAB o filtro de Kalman, estimando-se com ele o estado (posio e velocidade horizontais) de V2. Os resultados apresentados abaixo foram obtidos com 30s de simulao para os seguintes dados: Altura inicial de V1: 5000 m; Distncia horizontal entre V1 e V2: 8000 m; Velocidade horizontal de V1: 150 m/s; Velocidade vertical inicial de V1: 5 m/s; Velocidade horizontal de V2: 10 m/s; Desvio padro do erro de medida de 1e-3 rad 6e-5o ; Incerteza inicial na posio horizontal de V2: 100 m; Incerteza inicial na velocidade horizontal de V2: 1m/s; Erro inicial de medida da posio horizontal de V2: 10 m; Erro inicial de medida da velocidade horizontal de V2: 1 m/s; Simulamos a posio e a velocidade horizontais de V2 exatas (estado real) para uma comparao com os valores estimados pelo filtro. A estatstica do erro de representada na figura 2:
m l k i5j h g e f f f f f i i i i

Figura 3: Erro da estimao da medida da posio horizontal do veculo 2

Figura 4: Erro da estimao da medida de velocidade horizontal do veculo 2

Podemos observar que o filtro faz com que os valores de estado convirjam para o valor correto, embora tenhamos comeado a simulao com um erro de 10 m na posio horizontal e 1m/s na velocidade horizontal. A covarincia tende a se estabilizar e o erro no estado tende a ficar dentro da faixa estabelecida pela covarincia. Com isto, percebemos que o desempenho do filtro na estimao do estado do veculo 2 foi eficiente, embora no tivssemos nenhuma informao sobre sua dinmica.
n

Figura 2: Estatstica dos resduos do ngulo

7.

Grr

Podemos observar que o resduo na medida do ngulo positivos e negativos, como era esperado. O desvio padro deste resduo, como esperado, da ordem do desvio padro no erro da medida. O desempenho do filtro mostrado nas figuras 3 e
o p u { y 5VBx5(5~r|r5S5zx(V7rqo {  { u p } { r u { r p u { y s w vs u t o s r p j

Concluses Este trabalho mostra uma investigao preliminar para o tratamento de um problema de rastreamento mais complexo, onde se consideram incertezas em outras medidas, como as da plataforma inercial de V1, alm de mais graus de liberdade no problema. Com uma abordagem mais realista, o estado estimado, tanto de V2, como de V1 e do ngulo ser utilizado para o projeto de um controle [5] que permita o rendez-vous

Imprimir

Sair

Menu

entre os veculos 1 e 2, e no apenas medida e estimao de estados. Conclumos que, para esta primeira abordagem, o filtro desenvolvido foi adequado, produzindo estimativas de estado bem prximas do estado real. O filtro de Kalman apresentou: resduos consistentes com a formulao do problema; estimativas de erro prximas da realidade; tempo de convergncia bastante favorvel.

8.

Agradecimentos

Agradecemos ao professor Dr. Hlio Koiti Kuga, pela orientao e acompanhamento deste trabalho, que um trabalho final da disciplina CMC-306-4, Otimizao de Sistemas Dinmicos II do Curso ETE/CMC do INPE. Agradecemos ainda ao colega Yasser Mahmud Abdallah que auxiliou na utilizao do MATLAB na compreenso e implementao do filtro de Kalman.

Referncias
[l] H. Goldstein, Classical Mechanics, AddisonWesley Publishing Company, Inc., Massachussetts, (1962) [2] S.H. Stovall, Basic Inertial Navigation, Naval Air Warfare Center Weapons Division, China Lake, CA, USA, (1997) - (NAWCWPNS TM 8128) [3] P.S. Maybeck, Stochastic Models, Estimation, and Control, V1, Academic Press, New York, USA , (1979) [4] H.K. Kuga, Sobre a Utilizao Prtica de Tcnicas de Estimao, Notas de Aula, (2004) [5] D. E. Kirk, Optimal Control Theory Na Introduction, Prentice-Hall, Inc., Englewood Cliffs, New Jersey, (1970)