FILTROS DE PARTÍCULAS APLICADOS À ESTIMAÇÃO DE TRAJETÓRIAS

FILTROS DE PARTÍCULAS APLICADOS À ESTIMAÇÃO DE TRAJETÓRIAS VICTOR A. F. de CAMPOS, DOUGLAS D. S. SANTANA, CELSO M. FURUKAWA, NEWTON MARUYAMA Depto. de Engenharia Mecatrônica e de Sistemas Mecânicos, Escola Politécnica da Universidade de São Paulo Av. Prof. Mello Moraes, 2231 CEP 05508-900 – São Paulo, SP, BRASIL E-mails: [email protected], [email protected], [email protected], [email protected] Abstract The Inertial Navigation makes use of gyroscopes and accelerometers to maintain position estimates of a vehicle in which an Inertial Measurements Unit (cluster of 3 gyros and 3 accelerometers) is embedded. The modern Inertial Navigation Systems apply the Kalman Filter to estimate vehicles trajectories, what creates a sensor fusion system – the stochastic filters integrate measurements given by an IMU with reference measurements (which are given by another kind of sensor). An application that has been studied recently is the implementation of Inertial Navigation Systems through particle filters – particle filters are predictive filters that estimate states of systems, being based on the sequential Monte Carlo method. These filters yield weighted samples to approximate a certain density probability function. The aim of this paper is to apply the particle filters and the Kalman filter to the trajectory estimation, evaluating advantages and drawbacks and comparing the performance of each kind of filter. Keywords sequential Monte Carlo methods, recursive Bayesian filtering, probabilistic sampling, Inercial Navigation, trajectory estimation, sensor fusion systems. Resumo A Navegação Inercial utiliza giroscópios e acelerômetros para manter estimativas de posição de um veículo no qual uma Unidade de Medidas Inercial (conjunto de 3 acelerômetros e 3 giroscópios) está embarcada. Os Sistemas de Navegação Inercial modernos utilizam o filtro de Kalman para calcular estimativas de trajetórias de veículos, o que constitui um sistema de fusão sensorial – nos filtros estocásticos há uma integração das medidas dadas pela UMI com medidas de referência (que são medidas dadas por um outro sensor). Uma aplicação em estudo recentemente é a implementação de Sistemas de Navegação Inercial através de filtros de partículas, que são filtros preditivos destinados à estimação de estados em sistemas, sendo baseados no método de Monte Carlo seqüencial. Estes filtros geram amostras (ou partículas) ponderadas para aproximar uma determinada função densidade de probabilidade. A finalidade deste artigo é aplicar os filtros de partículas e o filtro de Kalman à estimação de trajetórias (através de um experimento automotivo) e apontar as vantagens e desvantagens de cada um destes filtros, comparando ainda o seu desempenho. Palavras-chave métodos de Monte Carlo seqüenciais, filtragem Bayesiana recursiva, amostragem probabilística, Navegação Inercial, estimação de trajetórias, sistemas de fusão sensorial.

1

Introdução

A estimação de trajetórias em Navegação Inercial é um importante caso particular da estimação de estados na Teoria de Sistemas. A Navegação Inercial utiliza giroscópios (sensores que medem velocidade de rotação angular) e acelerômetros (sensores que medem aceleração) para manter estimativas de posição e velocidade do veículo no qual uma Unidade de Medidas Inercial (conjunto de 3 acelerômetros e 3 giroscópios) está embarcada. Este veículo pode ser uma aeronave, um navio, um submarino, uma sonda ou um veículo terrestre. Os Sistemas de Navegação Inercial modernos normalmente utilizam o filtro de Kalman para calcular estimativas de trajetórias de veículos, o que constitui um sistema de fusão sensorial – nos filtros estocásticos há uma integração das medidas dadas pela Unidade de Medidas Inercial (UMI) com medidas de referência (que podem ser medidas de posição dadas por um odômetro, ou medidas de velocidade dadas por um velocímetro, por exemplo). Uma aplicação em estudo recentemente é a implementação de Sistemas de Navegação Inercial através de filtros de partículas. Filtros de partículas são filtros preditivos destinados à estimação de

estados em sistemas, sendo baseados no método de Monte Carlo seqüencial. Estes filtros geram amostras (ou partículas) ponderadas para aproximar uma determinada função densidade de probabilidade. Nosso objetivo é, então, aplicar tanto o filtro de Kalman quanto os filtros de partículas à estimação de posição de um veículo contendo uma UMI, e posteriormente comparar o desempenho destes dois tipos de filtro. Este trabalho descreve a implementação de um Sistema de Navegação Inercial do tipo strapdown com uma UMI de baixo custo para estimação de trajetórias de veículos. Este sistema realiza estimações utilizando o filtro de Kalman e os filtros de partículas, e são apresentados os resultados de um experimento automotivo. Analisando os resultados deste experimento, podemos tirar conclusões a respeito do funcionamento e aspectos práticos de aplicação dos filtros de partículas. Na seção 2, apresentaremos o filtro de partículas SIS (Sequential Importance Sampling, ou Amostragem por Importância Seqüencial), que apresenta a estrutura básica para os outros filtros de partículas: o filtro SIR (Sampling Importance Resampling, ou Amostragem e Reamostragem por Importância), apresentado na seção 3, e o filtro ASIR (Auxiliary Sampling Importance Resampling, ou

Amostragem e Reamostragem por Importância Auxiliar), apresentado na seção 4. Na seção 5 apresentamos a aplicação dos filtros SIR e ASIR à estimação de trajetórias em Sistemas de Navegação Inercial (e comparamos seus resultados com o do filtro de Kalman), e na seção 6 apresentamos as conclusões do trabalho. 2 O Filtro Sequential Importance Sampling O filtro SIS é um algoritmo baseado no método de Monte Carlo. A idéia central é representar uma função densidade de probabilidade por um conjunto de amostras aleatórias, cada amostra tendo um peso associado. Desta forma, podemos calcular estimativas da função densidade de probabilidade tendo como base estas amostras e seus respectivos pesos. Conforme o número de amostras cresce, esta representação de Monte Carlo torna-se um modelo aproximado da função densidade de probabilidade a posteriori desejada, e o resultado apresentado pelo filtro SIS se aproxima da estimativa Bayesiana ótima. De modo a detalhar o algoritmo, seja { x0(i), x1(i), ..., xk(i), wk(i) ; i = 1, ..., N } um conjunto de amostras de estados do sistema a cada instante de tempo, cada amostra com um peso associado, que caracterizam a função densidade de probabilidade p(x0,...,xk /y1,..., yk). Os pesos são normalizados tal que Σi wk(i) = 1. Estes pesos são escolhidos usandose o princípio da Amostragem por Importância, ou Importance Sampling. Este princípio é baseado no seguinte: suponha que p(x) seja uma função densidade de probabilidade da qual é difícil extrairmos amostras, mas que podemos calcular c(x) tal que p(x) ∝ c(x). Sejam, ainda, x(i), i = 1,..., N, pontos amostrados de uma função q(x). Essa função é chamada densidade de importância (ou Importance Density), e é escolhida de forma que possamos extrair amostras a partir dela (Arulampalam,2002). Assim, uma aproximação ponderada para p(x) será dada por: N N c( x (i )) p( x ) = w(i )δ ( x − x (i )) = .δ ( x − x (i )) (1)

∑ i =1

∑ q( x(i )) i =1

Na equação acima, w(i) é o peso normalizado da iésima amostra e δ é a função impulso, ou delta de Dirac. Portanto, se as amostras x0(i), ..., xk(i) forem extraídas de uma função densidade de importância q(x0, ..., xk / y1, ..., yk), os pesos serão dados por: p ( x 0 ( i ),..., x k ( i ) / y 1 ,..., y k ) (2) w (i ) ∝ k

q ( x 0 ( i ),..., x k ( i ) / y 1 ,..., y k )

A função densidade de importância é escolhida de tal forma que: q(x0,...,xk / y1,...,yk ) = q(xk / x0,...,xk −1, y1,...,yk )q(x0,...,xk −1 / y1,...,yk −1) (3) A função densidade de probabilidade conjunta pode ser escrita do seguinte modo, após algumas fatorações utilizando a fórmula de Bayes (Arulampalam, 2002): p(x0,...,xk / y1,...,yk ) ∝ p(yk / xk )p(xk / xk−1)p(x0,...,xk −1 / y1,...,yk −1) (4) Substituindo as equações (3) e (4) na equação (2), obtemos a equação de atualização dos pesos:

p( yk / xk (i)) p( xk (i) / xk −1 (i)) (5) q( xk (i) / x0 (i),...,xk −1 (i), y1,..., yk ) Além disso, se q(xk / x0,..., xk-1, y1,..., yk) = q(xk / xk-1, yk), a função densidade de importância dependerá apenas de xk-1 e yk.. Assim, a nova equação de atualização dos pesos será (Arulampalam, 2002): p( yk / xk (i )) p( xk (i ) / xk −1 (i )) (6) wk (i ) ∝ wk −1 (i ) q( xk (i ) / xk −1 (i ), yk ) Portanto, a função densidade de probabilidades filtrada p(xk / y1,..., yk) será aproximada de acordo com a equação abaixo: wk (i) ∝ wk −1 (i)

N

p ( xk / y1 ,..., yk ) ≈ ∑ wk (i )δ ( xk − xk (i ))

(7)

i =1

Os pesos wk(i) da equação acima são aqueles definidos na equação (6). Assim, o algoritmo SIS consiste numa propagação recursiva de pesos e pontos auxiliares. Essa propagação é feita seqüencialmente, conforme cada nova medida é obtida (Arulampalam, 2002). 2.1 O Problema da Degeneração e a Reamostragem Um problema comum que ocorre com o filtro SIS é a degeneração. Esse fenômeno ocorre quando todas as amostras exceto uma têm um peso muito pequeno após algumas iterações do algoritmo SIS. A degeneração faz com que um grande esforço computacional seja feito para atualizar amostras que contribuem muito pouco para a aproximação de p(xk / y1,..., yk). Uma medida de degeneração do algoritmo SIS é dada pelo tamanho amostral efetivo Nef , que pode ser obtido através da seguinte equação (Arulampalam, 2002): 1 (8) N' = ef

N

∑ (w (i)) i =1

2

k

Valores de Nef pequenos indicam a ocorrência de degeneração severa. Sendo o fenômeno da degeneração indesejável, devemos tentar reduzi-lo. O primeiro modo de reduzirmos este efeito é escolher um valor elevado para N, o que é, em muitos casos, impraticável. O modo mais comum de reduzirmos este efeito é utilizando a reamostragem (resampling). A reamostragem pode ser realizada quando o tamanho amostral efetivo Nef' for menor que um certo limiar NT, o que seria um indicador da ocorrência de degeneração. A idéia básica da reamostragem é eliminar as amostras que têm um peso muito pequeno, mantendo apenas as amostras que têm um peso grande. Na reamostragem é gerado um novo conjunto { xk*(i) , i = 1, 2, ..., N } através de uma amostragem (com reposição) realizada a partir da aproximação discreta de p(xk / y1, ..., yk), repetida N vezes, e tal que P( xk*(i) = xk(j) ) = wk(j). Esta aproximação discreta é dada pela equação (7). O resultado será uma amostra da densidade discreta dada pela equação (7) e, feito isso, os pesos são ajustados para valerem 1/ N. Embora a reamostragem reduza os efeitos da degeneração, ela introduz outros

problemas no filtro amostral. O principal problema é que as amostras com pesos maiores serão selecionadas estatisticamente muitas vezes. Isso leva a uma perda de diversidade amostral, já que a amostra resultante conterá muitos pontos repetidos. Esse problema é conhecido como empobrecimento amostral (em inglês, sample impoverishment), e é significativo nos processos com baixo nível de ruído (Arulampalam, 2002). 3 O Filtro Sampling Importance Resampling O filtro Sampling Importance Resampling (SIR, ou Amostragem e Reamostragem por Importância) (Gordon et al., 1993) pode ser derivado a partir do algoritmo SIS através da seguinte escolha da função densidade de importância: q(xk / xk-1(i), y1,..., yk) = p(xk / xk-1(i)). Para completar, a reamostragem deve ser feita a cada iteração do algoritmo. A escolha da função densidade de importância indicada acima requere a extração de amostras de p(xk / xk-1(i)). Uma amostra xk(i) extraída de p(xk / xk-1(i)) pode ser gerada do seguinte modo: inicialmente, extraímos uma densidade de amostra rk-1(i) da função probabilidades do ruído do sistema, pr(rk-1); a seguir, fazemos xk(i) = fk(xk-1(i), rk-1(i)) (esta é a equação do sistema no espaço de estados). Para essa escolha da função densidade de importância, os pesos associados às amostras serão dados por: (9) wk(i) ∝ wk-1(i).p(yk/xk(i)) Os pesos dados pela equação acima devem ser normalizados antes de executarmos o algoritmo de reamostragem. O principal problema apresentado pelo filtro SIR deve-se ao fato de a função densidade de importância ser independente da observação yk. Assim, o espaço de estados é explorado sem o conhecimento das observações e o filtro poderá ser ineficiente. Outrossim, como a reamostragem é feita a cada iteração do algoritmo, pode-se ter uma rápida perda de diversidade amostral. Entretanto, o filtro SIR tem as vantagens de os pesos das amostras poderem ser facilmente calculados e de podermos amostrar com facilidade a partir da função densidade de importância (Arulampalam, 2002). 4 O Filtro Auxiliary Sampling Importance Resampling O filtro Auxiliary Sampling Importance Resampling (ou Amostragem e Reamostragem por Importância Auxiliar) (Pitt and Shephard, 1999) é uma variação do filtro SIR. Ele pode ser obtido a partir do filtro genérico SIS através da escolha da função densidade de importância q(xk, i / y1, ..., yk), que amostra { xk(j), i(j), j = 1, ..., N }, onde i(j) é o índice da amostra no instante de tempo k-1. Aplicando-se a regra de Bayes, podemos obter o seguinte resultado (Arulampalam, 2002): (10) p( xk , i / y1,..., yk ) ∝ p( yk / xk ) p( xk / xk −1 (i))wk −1 (i)

A função densidade de importância utilizada para extrair a amostra {xk(j),i(j), j = 1,..., N} é definida de modo a satisfazer a equação (Arulampalam, 2002): q( xk , i / y1,..., yk ) ∝ p( yk / µk (i)) p( xk / xk −1 (i))wk −1 (i) (11) µk(i) é uma caracterização de xk dado xk-1(i) e pode, portanto, ser definida como a seguinte expectância: µk(i) = E(xk/xk-1(i)), ou ainda uma amostra µk(i) extraída de p(xk/xk-1(i)). Fazendo: (12) q( xk , i / y1 ,..., yk ) = q(i / y1 ,..., yk )q( xk / i, y1 ,..., yk ) ∆

(13) q ( xk / i , y1 ,..., y k ) = p ( xk / xk −1 (i )) Utilizando as equações (11), (12) e (13), chegamos a: (14) q (i / y1 ,..., yk ) ∝ p( yk / µ k (i )) wk −1 (i ) A amostra { xk(j), i(j), j = 1,..., N } recebe então um peso proporcional a uma grandeza definida pela relação entre as equações (10) e (11) (Arulampalam, 2002): p( yk / xk ( j))p(xk ( j) / xk −1(i( j))) p( yk / xk ( j)) (15) wk ( j) ∝ wk −1(i( j))

q(xk ( j),i( j) / y1,...,yk )

=

p( yk / µk (i( j)))

Comparado ao filtro SIR, o filtro ASIR tem a vantagem de gerar pontos a partir da amostra no instante k-1 que com maior probabilidade estarão próximos ao estado real do sistema. O filtro ASIR pode ser entendido como o algoritmo do filtro SIR com uma reamostragem no instante de tempo anterior, baseada em algumas estimativas µk(i) que caracterizam a função p(xk/xk-1(i)). Se o nível de ruído do processo for pequeno, e desse modo a função densidade de probabilidade p(xk/xk-1(i)) for bem definida por µk(i), o filtro ASIR funcionará melhor que o filtro SIR. Entretanto, se o nível de ruído do processo for alto, µk(i) não caracterizará a função p(xk/xk-1(i)) de modo adequado. Neste caso, o uso do algoritmo ASIR não é recomendável (Arulam., 2002). 5 Sistemas de Navegação Inercial com Filtros de Partículas e Resultados Experimentais Recentemente, os filtros de partículas têm sido utilizados com sucesso na estimação de trajetórias para Navegação Inercial (Gustafsson et al., 2001). Para implementarmos um Sistema de Navegação Inercial, uma Unidade de Medidas Inercial (UMI) deve ser embarcada num veículo (no caso, um automóvel) e, a partir das medidas fornecidas por ela, são realizadas estimativas da posição atual do veículo. As medidas fornecidas por uma UMI são acelerações nos 3 eixos ortogonais do veículo e rotações angulares em torno dos mesmos 3 eixos (os sensores que constituem uma UMI são 3 acelerômetros e 3 giroscópios, sendo que as medidas dos giroscópios são utilizadas para corrigir as direções dos eixos de medida das acelerações). Para UMI’s de baixo custo, pode-se aumentar a precisão das estimativas através da integração das medidas da UMI com medidas de referência (neste caso, as medidas de referência são as velocidades lidas no velocímetro de um automóvel). Esta integração é feita diretamente através do filtro de Kalman ou dos

H i s t o g r a m a d o r u i d o d e a c e le r a ç a o n o e ix o x

6 000

5 000

Numero de amostras

filtros de partículas. Um diagrama esquemático do filtro de Kalman é apresentado na figura 2. O modelo do sistema no espaço de estados é o seguinte: 2 2  p k +1  I T .I 0.5.T .I   p k  0.5.T .I   v k +1  = 0 I T .I . v k  +  T .I .u k + C.rk     δa  0 0 I  δa k   0   k +1  

4 000

3 000

2 000

1 000

0 -0.2

-0 . 1 5

-0 . 1

- 0 .0 5

0

0.05

F a i x a s d e a c e l e ra ç a o - m / s 4 000

0.15

3 500

(16)

No modelo acima, pk é um vetor constituído de 3 coordenadas de posição: x, y e z, num dado sistema de coordenadas. vk é um vetor formado pelas velocidades do veículo nos 3 eixos ortogonais do sistema de coordenadas utilizado, e δak é um vetor formado pelas 3 componentes do erro de aceleração presente nas medidas fornecidas pelos acelerômetros da UMI. pk , vk e δak constituem o vetor de estados do sistema. uk é o vetor de entradas deste sistema – as entradas são as 3 acelerações fornecidas pela UMI e corrigidas de acordo com as medidas dos giroscópios (acelerações nos eixos x, y e z do sistema de coordenadas). rk é o vetor de ruídos do processo (ruídos apresentados pelos sensores da UMI), também constituído por 3 componentes (uma para cada eixo de medição). yk é o vetor de medidas de referência, que neste caso é a velocidade lida no velocímetro do carro decomposta nos 3 eixos de medição do sistema, e ek é o vetor de erros de medição (neste caso, são os erros de medida do velocímetro do auto), formado por 3 componentes. I é uma matriz identidade 3 x 3 e T é o período de amostragem do sistema (foi utilizado um período de 0.02s). C e G são matrizes que definem a covariância dos ruídos de processo e medição, respectivamente. O desenvolvimento detalhado deste modelo e a mecanização de um Sistema de Navegação Inercial estão descritos detalhadamente em (Santana et. al., 2004). Os ruídos de uma Unidade de Medidas Inercial geralmente não são Gaussianos. Esta é uma constatação baseada em dados experimentais, e pode ser feita utilizando-se uma coleta de dados de ruído de uma UMI. Isto é feito da seguinte forma: deixamos uma UMI parada sobre uma mesa de desempeno nivelada (para que os acelerômetros x e y da UMI não meçam componentes da aceleração da gravidade) e coletamos medidas dos 3 acelerômetros. Com as devidas correções, as leituras deveriam indicar zero (pois a UMI está parada). O que for medido desta forma será, então, ruído de medição dos acelerômetros. Uma amostra deste tipo foi coletada durante aproximadamente 3 minutos (a uma taxa de 50 amostras por segundo), e a figura 1 mostra histogramas dos ruídos dos acelerômetros x e y. Fica claro, assim, que as distribuições destes ruídos não são Normais.

3 000 Numero de amostras

 pk  y k = [0 I 0].  v k  + G.e k δa   k

0 .1

2

H i s t o g r a m a d o r u i d o d e a c e le r a ç a o n o e ix o y

2 500 2 000 1 500 1 000 500 0 -0.4

- 0 .3

-0 . 2

-0. 1

0

0.1

0.2

F a i x a s d e a c e l e ra ç a o - m / s

0 .3

0.4

0 .5

2

Fig.1 – Histograma dos ruídos de aceleração da UMI

Para calcularmos os pesos associados às amostras do filtro SIR, precisamos conhecer a função densidade de probabilidades p(yk / xk). Esta função densidade de probabilidades pode ser escrita do seguinte modo: yk = H.xk +G.ek ⇒ek = yk − H.xk ⇒ p(yk / xk ) = p(ek ) = p(yk − H.xk ) (17) Na equação acima, fizemos a suposição que a matriz G é uma matriz identidade. Assim, tendo yk e xk , podemos calcular p(yk / xk), desde que se conheça a função densidade de probabilidades do erro ek (pois p(yk / xk) = p(ek)). Logicamente, esta função depende sempre da aplicação e das medições que estão sendo utilizadas, mas a princípio consideraremos que ela é uma Gaussiana multivariável: p (ek ) =

1 .( e kT . e k )) 2 ( 2π ) n

exp( −

(18)

Nesta Gaussiana, o vetor de médias é nulo (considera-se que as componentes do vetor de erros ek têm média igual a zero) e a matriz de covariâncias é a identidade (ou seja, as componentes do vetor ek não são correlacionadas entre si e têm variância unitária). Temos ainda que n é a dimensão do vetor ek. Estas simplificações nem sempre são válidas, e em alguns casos podemos ter que ajustar adequadamente o vetor de médias e a matriz de covariâncias desta função Gaussiana. Para o cálculo das estimativas de estado (xk) a partir das N amostras geradas pelo filtro (xk(i), i = 1,..., N), utilizaremos a seguinte média ponderada: N (19) xˆ = w ( i ). x ( i ) k

∑

k

k

i =1

O número de amostras (N) utilizado nos filtros de partículas foi 100. Este número pode ser considerado baixo para algumas aplicações, mas funcionou bem nos testes automotivos realizados. Além disso, um número de amostras muito elevado pode tornar a execução do filtro lenta (o gasto computacional seria elevado, e o ganho adicional de desempenho seria comparativamente pequeno). Para o filtro ASIR, a caracterização µk do estado xk dado o estado xk-1 foi escolhida como sendo uma amostra da função densidade de probabilidade p(xk / xk-1). Os pesos associados às amostras devem ser calculados de acordo com a equação (15). Nesta equação, escolhemos as funções densidade de probabilidades

SISTEMA REAL MEDIDAS DE ACELERAÇÃO (uk)

xk +1 = A.xk + B.uk + C.rk

MEDIDAS DE REFERÊNCIA (yk)

yk = H.xk + G.ek EQUAÇÕES DE RICCATI

K = [APHT + CGT] [HPHT + GGT] -1 T

K

T

T

T

P = (A - KH) P (A - KH) + CC + K.G.G .K

POSIÇÃO ESTIMADA

xˆk +1 = A.xˆ k + B.u k + K .( yk − H .xˆ k ) FILTRO DE KALMAN

Fig.2 – Diagrama esquemático do Filtro de Kalman

O algoritmo do filtro SIR é dado abaixo: • Inicializar xk(i) e wk(i) • For (i = 1 : N) { Extrair uma amostra do ruído rk-1 ; Calcular as amostras do estado atual do sistema: xk(i) = A.xk-1(i) + B.uk-1 + C.rk-1 Calcular os pesos associados às amostras: wk(i) = wk-1(i). exp(-0.5 (yk – H.xk(i))T.(yk – H.xk(i))) / √(2π)3 } • Calcular a soma dos pesos: soma = Σi=1:N wk(i) • Normalizar os pesos: For (i = 1 : N) { wk(i) = wk(i) / soma } • Calcular o número efetivo de amostras: Nef = 1 / ( Σi=1:N wk(i)2 ) • Realizar a reamostragem (se o número efetivo de amostras for baixo): If ( Nef < 2N/3 ) { Executar o algoritmo de reamostragem } • Calcular a estimativa do estado atual do sistema: xk = Σi=1:N xk(i).(1/N)

O algoritmo do filtro ASIR é o seguinte: •Inicializar xk(i) e wk(i); •For (i = 1 : N) { Extrair uma amostra do ruído rk-1; Calcular as amostras da caracterização do estado do sistema: µk(i) = A.xk-1(i) + B.uk-1 + C.rk-1 Calcular os pesos associados a essas amostras: wk(i)=wk-1(i). exp ( -0.5. (yk - H.µk(i))T (yk - H.µk(i)) )/√(2π)3 } •Calcular a soma dos pesos: soma = Σi=1:N wk(i) •Normalizar os pesos: For (i = 1 : N) { wk(i) = wk(i) / soma } •Executar o algoritmo de reamostragem •For (j = 1 : N) { Extrair uma amostra do ruído rk-1; Calcular as amostras do estado xk: xk(j) = A.xk-1(i(j)) + B.uk-1 + C.rk-1 Calcular os pesos associados a essas amostras: T

wk ( j) =

exp( − 0 .5 .( y k − H . x k ( j )) .( y k − H . x k ( j )))

}

T

exp( − 0 .5 .( y k − H .µ k ( i ( j ))) .( y k − H .µ k ( i ( j ))))

•Calcular a soma dos pesos: soma = Σj=1:N wk(j) •Normalizar os pesos: For (j = 1 : N) { wk(j) = wk(j) / soma } •Calcular a estimativa do estado atual do sistema: xk = Σj=1:N wk(j).xk(j)

No algoritmo do filtro ASIR, os índices i(j) são obtidos através do algoritmo de reamostragem (ou

seja, são reamostradas as partículas com pesos maiores e seus índices são obtidos – estes índices são i(j), j = 1,...,N). Um algoritmo de reamostragem eficaz está detalhado em (Arulampalam, 2002). O experimento automotivo foi realizado do seguinte modo: uma UMI de baixo custo foi embarcada num automóvel e coletou medidas de aceleração deste automóvel durante um percurso fechado (ou seja, o ponto inicial da trajetória realizada coincide com o ponto final). Ao mesmo tempo, foram obtidas leituras do velocímetro do automóvel. Ao final do percurso, os dados armazenados foram processados no programa MATLAB utilizando-se algoritmos dos filtros de Kalman (ver figura 2), SIR e ASIR. A UMI utilizada foi o modelo VG700AA-202 da Crossbow, e durante o percurso a velocidade do automóvel foi mantida aproximadamente constante em 20 km/h (este valor foi utilizado como referência nos filtros). A trajetória estimada pelo filtro de Kalman no MATLAB está apresentada na figura 3. 10

Trajeto ria rea lizad a p elo au tom ovel estim ada pelo filtro de Kalma n

0 -10 -20 Eixo y - metros

p(yk / xk) e p(yk / µk) como sendo Gaussianas multivariáveis, com vetor de médias nulo e matriz de covariância unitária.

-30 -40 -50 -60 -70 -80 -100

-50

0 50 Eixo x - m etros

1 00

150

Fig.3– Trajetória de estados estimada pelo filtro de Kalman.

Os erros de posição estimados são calculados no último ponto do percurso. Como o veículo realizou um trajeto em circuito fechado, o ponto inicial deve coincidir com o ponto final da trajetória - assim, se o ponto inicial for ajustado na inicialização do filtro para valer zero nas três coordenadas, então o ponto final também deverá valer zero nas três coordenadas, e a diferença entre zero e o valor final estimado pelos filtros será o erro de estimação. Os erros estimados nos três eixos são os seguintes: 3.09 m no eixo x, 4.09 m no eixo y e 2.52 m no eixo z. Esses erros são da ordem de 0.74% para o eixo y e 0.55% para o eixo x. A trajetória estimada acima é compatível com o percurso realizado pelo automóvel no estacionamento da Escola Politécnica da USP. Contudo, o filtro de partículas SIR apresenta um ganho em relação ao filtro de Kalman quanto ao erro de estimação para este sistema (devido ao fato de que os filtros de partículas são mais adequados para estimar estados de sistemas não-lineares e com ruídos não-gaussianos). Executando o algoritmo que implementa o filtro de partículas SIR no MATLAB, pode-se estimar a trajetória de estados xk do sistema:

6 Conclusão

Tra je toria realizada pelo autom ovel es tim ada pelo filtro SIR

10 0

Eixo y - metros

-10 -20 -30 -40 -50 -60 -70 -100

-50

0 50 Eixo x - m etros

1 00

150

Fig. 4 – Trajetória de estados estimada pelo filtro SIR

Os erros de posição obtidos ao final do percurso são os seguintes: 1 m no eixo x, 0.57 m no eixo y e 6.6 m no eixo z. Estes erros são da ordem de 0.18% para o eixo x e 0.1% para o eixo y. Indubitavelmente, o desempenho do filtro SIR é superior ao do filtro de Kalman. Foram obtidos erros relativamente baixos, e isso provavelmente se deve ao fato de termos utilizado amostras do ruído real do sistema no filtro SIR, enquanto que no filtro de Kalman o ruído é considerado gaussiano (o que não ocorre na prática). Uma desvantagem do filtro SIR em relação ao filtro de Kalman é que ele demora mais tempo para ser executado (em ambiente MATLAB), o que pode impossibilitar o seu uso em aplicações on-line. Aplicando o filtro ASIR ao experimento automotivo, obtivemos as seguintes estimativas: 10

Trajetoria re aliza da pelo a uto mo vel estim ada pe lo filtro ASIR

-10

Eixo y - metros

Agradecimentos Esta pesquisa foi realizada no contexto do projeto RENAPIGI (Rede Nacional de PIGs Instrumentados) da FINEP/CTPETRO. Os autores desejam agradecer à FINEP, ao Programa CTPETRO da ANP e à CAPES pelo apoio financeiro recebido, e ao CENPES-PETROBRAS pelo apoio para a realização dos experimentos. Referências Bibliográficas

0

-20 -30 -40 -50 -60 -70 -100

O filtro de Kalman deixa de ser ótimo quando aplicado a sistemas não-lineares e/ou com ruídos não gaussianos. Nesses casos, os filtros de partículas podem apresentar melhores resultados (no teste automotivo realizado, o desempenho do filtro SIR superou o do filtro de Kalman). Contudo, a escolha do número total de amostras a serem utilizadas (N) e o cálculo da função densidade de probabilidade p(y/x) poderão acarretar dificuldades na aplicação dos filtros de partículas. Por isso, os filtros de partículas são mais dependentes da aplicação que o filtro de Kalman. Finalmente, a escolha dos filtros SIR ou ASIR dependerá de algum conhecimento acerca do ruído do processo. Com relação à aplicação em Navegação Inercial, ambos os filtros apresentaram desempenho satisfatório na estimação de trajetórias, sendo que a execução do filtro SIR em ambiente MATLAB é em média 18 vezes mais lenta que a do filtro de Kalman (para N = 100 amostras).

-50

0 50 Eixo x - m etros

1 00

150

Fig.5 – Trajetória de estados estimada pelo filtro ASIR

Assim como no filtro SIR, no filtro ASIR também são utilizadas amostras do ruído real do sistema. Os erros de posição obtidos são: 4.96 m no eixo x, 5.32 m no eixo y e 4.59 m no eixo z. Estes erros são superiores aos erros apresentados pelo filtro de Kalman. Uma possível razão para este desempenho inferior pode ser a seguinte: conforme visto na seção 4, se o ruído do processo for relativamente alto, µk(i) não será uma caracterização adequada para a função densidade de probabilidade p(xk/xk-1(i)). Desta forma, o filtro ASIR fará a reamostragem baseado numa aproximação ruim de p(xk /xk-1(i)). Portanto, devido ao nível de ruído presente no sistema, não é recomendável utilizarmos o filtro ASIR. Um resultado melhor poderia ser obtido, mas para isso seria necessário utilizarmos um número de amostras muito elevado.

Arulampalam, S. et al. (2002) A Tutorial on Particle Filters for Online Non-linear/Non-Gaussian Bayesian Tracking. IEEE Transactions on Signal Processing, vol 50, no. 2, pp. 174189. Brown, R. G.; Hwang, P. Y. C. (1997) Introduction to Random Signals and Applied Kalman Filtering. Terceira edição. John Wiley and Sons. Campos, V. A. F. (2004) Aplicação do filtro de Kalman e dos filtros de partículas à estimação de trajetórias em Navegação Inercial. Dissertação de Mestrado, EPUSP, São Paulo. Davis, M. H. A.; Vinter, R. B. (1985) Stochastic Modelling and Control. Chapman and Hall. Doucet, A.; Gordon, N.; De Freitas, J. F. G. (2001) Sequential Monte Carlo Methods in Practice. Springer Verlag, New York. Gordon, N.; Salmond, D.; Smith, A. F. M. (1993) Novel Approach to Non-linear and Non-Gaussian Bayesian State Estimation. IEEE Proceedings, vol 140, pp. 107-113. Gustafsson, F. et. al. (2001) Particle Filters for Positioning, Navigation and Tracking. IEEE Transactions on Signal Processing, Special Issue on Monte Carlo Methods for Statistical Signal Processing, Vol. 50, no. 2, pp. 1-13. Papoulis, A.; Pillai, S. U. (2001) Probability, random variables and stochastic processes. Quarta edição, McGraw-Hill, New York. Pitt, M. ; Shephard, N. (1999) Filtering via Simulation: Auxiliary Particle Filters. Journal of the American Statistical Association, vol 94, pp. 590-599. Santana , D. D. S. , Campos , V. A. F. , Furukawa , C. M. e Maruyama, N. (2004) Estimação de Trajetórias utilizando Sistema de Navegação Inercial Strapdown. Aceito para apresentação no XV Congresso Brasileiro de Automática, Gramado, RS, Brasil.