Implementao de um Filtro de Kalman

Alunos:

Claudiney Calixto Silva Evandro Henrique Juliano Augusto Pereira Marco Antnio Braga Robson Mendona

Carlos H. C. Ribeiro

ndice

1. Introduo ..................................................................................... 3 2. Filtro de Kalman ............................................................................. 4 2.1. O processo a ser estimado ............................................................ 4 2.2. A Origem Computacional do Filtro .................................................. 5 2.3. As Origens Probabilisticas do Filtro ................................................ 6 2.4. O Algoritmo Discreto do Filtro de Kalman ....................................... 6 2.5. Parmetros e ajustes do filtro ....................................................... 8 3. JMR ............................................................................................. 10 3.1 O que consiste ............................................................................ 10 3.2 Objetivos ................................................................................... 10 3.3 Caractersticas ............................................................................ 10 3.3.1 Cliente .................................................................................... 10 3.3.2 Servidor Java 3D ....................................................................... 11 3.3.3 Servidor Saphira ....................................................................... 11 4. Detalhes de Implementao .......................................................... 13 5. Testes Realizados ......................................................................... 16 6. Consideraes Finais ..................................................................... 22 6.1. Concluso ................................................................................. 22 6.2. Sugestes ................................................................................. 22 6. Atividades Realizadas ................................................................... 23 7. Bibliografia .................................................................................. 24 I. Apndice - Cdigo Fonte ................................................................ 25 I.1 gui.GuiGrade.java ........................................................................ 25 I.2 gui.Kalman.java .......................................................................... 30 I.3 rmi.ServidorCentral.java ............................................................... 33 I.5 rmi.Servidor.java .......................................................................... 34 I.5 rmi.Central.java........................................................................... 34 I.6 user.process.FiltroKalman.java ...................................................... 34

Implementao de um Filtro de Kalman

1. Introduo

O filtro de kalman tenta resolver problemas de localizao em robtica mvel. Ele um algortmo de rastreamento local que tenta estimar a localizao do rob num mundo conhecido. O Objetivo deste projeto a implementao (em simulao) e anlise de uma tcnica de localizao por Filtro de Kalman para robs mveis. O algoritmo utilizado assumir a existncia de um mapa preciso do ambiente e informao sensorial obtida por sensores de proximidade simples.

Implementao de um Filtro de Kalman

2. Filtro de Kalman
Em 1960, Rudolph.E. Kalman publicou seu famoso paper que descreve uma soluo recursiva para o problema de filtragem de dados discretos em um sistema linear. Dados alguns valores iniciais, pode-se predizer e ajustar os parmetros do modelo atravs de cada nova medio, obtendo a estimativa do erro em cada atualizao. A sua habilidade para incorporar os efeitos de erros e sua estrutura computacional fez com que o filtro de Kalman tivesse um amplo campo de aplicaes, especialmente no que se refere anlise de trajetrias em viso computacional. 2.1. O processo a ser estimado Filtro de Kalman se destina ao problema geral de tentar estimar o estado de um tempo discreto em um processo controlado que governado pela equao diferencial estocastica , (2.1) com uma medio isto . (2.2)

As variveis aleatrias e representam os rudos de estado e das medidas (respectivamente).Assume-se serem independentes (uma da outra), e com distribuio normal de probabilidade. , (2.3) . (2.4) Na pratica, o processo de covariancia do ruido e as medidas de covariancia do ruido, matriz podem mudar, em cada medio ou temporalmente, contudo assume-se que so constantes.

Implementao de um Filtro de Kalman A matriz na equaao diferencial descreve o estado no tempo para o estado atual , na falta de uma funo de referencia ou anterior ruido processado. Na prtica muda em cada passo, porm assume-se que ele constante. A matriz B demonstra o controle opcional da entrada a o estado x. A matriz na equao de medio descreve o estado para a medida zk. Na prtica pode mudar em cada nova medio ou temporalmente, porm assumese que ela constante. 2.2. A Origem Computacional do Filtro Define-se para ser o estado a priori estimado no passo k dado um conhecimento do processo anterior para o passo k , e ser o estado estimado a posteriori em um passo k dado uma medio . Assim o erro pode ser definido a priori e a posteriori como

A covarincia da estimativa do erro a priori (2.5) e a covarincia da estimativa do erro a posteriori (2.6) Derivando a equao para o Filtro de Kalman, comea-se com o objetivo de achar uma eqquaa que computa uma estimativa a posteriori do estado com uma combinao linear de uma estimativa a priori e uma diferena ponderada entre a atual medida e uma previso de medida mostrada na equao 2.7 (2.7)

A diferena em (2.7) chamada de inovao na medio, ou resduo. Este resduo reflete a discrepncia entre a medida estimada e a medida atual . A matriz K na equao 2.7 escolhido para ser o ganho ou o fator de combinao que minimiza a covarincia do erro a posteriori. Esta minimizao pode ser acompanhada por primeiro substituir 2.7 na definio a cima por, substituindo, ento na equao 2.6, demonstrando a expectativa indicada derivando, os pontos dos resultados de K. Setando o resultado igual a zero e resolvendo para K.

Implementao de um Filtro de Kalman

(2.8) A covarincia do erro da medida tende a zero.O ganho K possui um peso residual maior

Por outro lado, como uma uma priori covarincia da estimativa do erro tende a zero, o ganho K possui um peso residual menor

Outro caminho para pensar sobre o balanceamento por K aquele que a covarincia do erro tende a zero, a atual medio cada vez mais confivel, quando uma medida prevista cada vez menos confivel. Por outro lado, quando a priori covarincia da estimativa do erro tende a zero a atual medida cada vez menos confivel ce a medio prevista cada vez mais confivel. 2.3. As Origens Probabilisticas do Filtro A Justificativa para a equao 2.7 baseada na probabilidade da estimativa a priori condicionada com todas as medidas anteriores . At este momento deve se considerar o ponto em que o Filtro de Kalman mantm os dois primeiros momentos de distribuio do estado.

A uma estimativa a posteriori do estado (2.7) reflete a mdia (o primeiro momento) de uma distribuio do estado- uma distribuio normal se as condies de 2.3 e 2.4 so satisfeitas. Uma posteriori estimativa da covarincia do erro (2.6) reflete a variao da distribuio do estado (num segundo momento) , ou seja:

2.4. O Algoritmo Discreto do Filtro de Kalman O filtro de Kalman estima um processo usando uma forma de controle por realimentao: o filtro estima o estado do processo em dado instante e obtm ento o feedback na forma de medidas (ruidosas). Assim, as equaes para o filtro de Kalman caem em dois grupos: atualizao das equaes de tempo e a atualizao das equaes de medio. As equaes de atualizao do tempo so responsveis para projetar com antecedncia (no tempo) as 6

Implementao de um Filtro de Kalman estimativas da covarincia do estado atual e do erro, para obter as estimativas a priori para a prxima etapa. As equaes de atualizao da medida so responsveis pelo feedback, isto incorpora-se uma nova medida na estimativa a priori para obter a posteriori uma estimativa melhorada. As equaes de atualizao do tempo podem tambm ser pensadas como equaes de predio, quando as equaes de atualizao da medida podem ser pensadas como equaes de correo. Certamente o algoritmo final de estimao assemelha-se quele de um algoritmo do predictor- corretor para resolver problemas numricos como mostrado abaixo em figura 1-1 .

Figura 2-1. O ciclo do Filtro de Kalman As especificas equaes para a atualizao por tempo e medidas, esto apresentadas abaixo na tabela 2.1 e na tabela 2.2:

Tabela 2-1: Equaes de Atualizao de Tempo do Filtro de Kalman (2.9) (2.10)

Novamente se descreve como as equaes de tempo atualizadas na tabela 2.1 projeta o estado e a covarincia estimada antes do tempo no passo k-1 para o passo k. A e B so para a tabela 2.1 o que o Q para (2.3)

Tabela 2-2: Equaes de Atualizao das medidas do Filtro de Kalman (2.11) (2.12) (2.13)

Implementao de um Filtro de Kalman A primeira tarefa durante a atualizao da medida computar o ganho de Kalman , Em seguida, aps cada atualizao do tempo e da medida, o processo repetido a posteriori e as estimativas precedentes usadas para projetar ou predizer as novas estimativas a priori. Esta natureza recursive uma das caractersticas mais atraentes do filtro de Kalman - faz execues prticas muito mais simples do que (por exemplo) uma execuo de um filtro do wiener que seja projetado para operar sobre todos os dados diretamente para cada estimativa. O filtro de Kalman preferivelmente condiciona recursivamente a estimativa atual em todas as medidas passadas. Figura 12 oferece abaixo um retrato completo da operao do filtro, combinando o diagrama de alto nvel da figura 1-1 com as equaes da tabela 1-1 e da tabela 1-2 . 2.5. Parmetros e ajustes do filtro Na execuo real do filtro, a covarincia do rudo da medida R medida geralmente antes da operao do filtro. Medir a covarincia do erro de medida R geralmente possvel, pois se necessita medir o processo de qualquer maneira (ao operar o filtro) geralmente pode fazer exame de algumas medidas off-line da amostra a fim determinar a variao do rudo da medida. A determinao do processo de covarincia do rudo Q geralmente mais difcil porque tipicamente no se tem a facilidade de observar diretamente o processo que se est estimando. s vezes um processo do modelo (pobre) relativamente simples pode produzir resultados aceitveis se for incorporada bastante incertezas ao processo via seleo de Q. Certamente neste caso se esperaria que as medies fossem confiveis. Quando no se tem uma base racional para escolher os parmetros, frequentemente se cronometra o desempenho melhor do filtro (estatsticamente falando) pode ser obtido ajustando os parmetros do filtro Q e R. Ajusta-se geralmente executando off-line, freqentemente com a ajuda de um outro filtro (distinto) de Kalman em um processo consultado geralmente para identificao do sistema.

Figura 1-2 Operao completa do filtro de Kalman,

Implementao de um Filtro de Kalman Nota-se as circunstncias onde e o so constantes de fato, e a estimao da covariancia do erro e o ganho de Kalman se estabilizar rapidamente e permanecero constante. Estes parmetros podem ser pr computados funcionando o filtro off-line, ou por exemplo determinando o valor de estado estacionrio de como descritos. O caso entretanto que o erro da medida no permanece constante.

Implementao de um Filtro de Kalman

3. JMR
3.1 O que consiste Java Mobile Robots um ambiente para simulao de robs desenvolvido em Java . baseado em arquitetura cliente-servidor, onde o ambiente virtual o servidor do sistema e cada rob conectado a esse ambiente um cliente. 3.2 Objetivos JMR permite definir e avaliar o comportamento de vrios robs dentro de um ambiente virtual atravs de uma interface para interagir com o simulador de robs Saphira, ou com os robs reais manipulados por esse simulador ou analisar o comportamento dos robs virtuais em um ambiente 3D virtual. 3.3 Caractersticas O sistema se divide em 3 partes: cliente; servidor Java; servidor Saphira. O servidor Java consiste de um mdulo que permite criar ambientes 3D para avaliao do comportamento de robes . O Servidor Saphira permite a interao do cliente JMR com o simulador Saphira, ou mesmo com um rob real Pioneer atravs dessa interface . O cliente uma aplicao separada que identifica e configura as caractersticas dos robs virtuais que estaro conectados a um dos servidores. 3.3.1 Cliente Controla os robs que sero simuados dentro de um dos servidores de ambiente. No JMR os paramtros de um rob so definidos de maneira similar ao Saphira, com alguns melhoramentos. Entra outros parmetros podemos citar: Largura, altura e comprimento do rob, velocidade mxima de rotao e de movimentao, nmero e posi cionamento de sensores, etc... 10

Implementao de um Filtro de Kalman Atravs desse cliente podemos enviar os seguintes comandos para um rob: velocidade linear; velocidade de rotao; mover n mm; girar n graus. Tambm possvel adicionar vrios processo a um rob. Processos so comportamentos inteligentes que permitem controlar as aes do rob de forma mais aprimorada. Evitar obstculos, controlar a velocidade, caminhar por um corredor, ..., so exemplos de processos. Cada processo corresponde a uma classe Java, essas classes podem ser modificadas e at mesmo novos processos podem ser desenvolvidos pelo usurio. Em cada rob existe um controle de cmera, que pode capturar imagens do simulador, ou no caso de uma conexo com uma camra e um rob real, podem capturar imagens reais. 3.3.2 Servidor Java 3D Esse o servidor original do JMR permite controlar vrios robs dentro de um ambiente tridimensional. Esse ambiente no muito sofisticado, permite criar um ambiente com paredes, e robos e outros objetos dentro de uma superfcie plana. Somente reconhece a leitura dos sensores dos robs e no possui nenhum recurso de iluminao de ambiente, nem de deteco de luz pelos robs. Os arquivos de configurao de mundos possuem o mesmo formato do Saphira, com algumas extenses, pois o simulador Saphira bidimensional. Apesar de ser um simulador 3D, ainda faltam alguns recursos importantes ao mundo virtual. como relevo diferenciado e gravidade. O sistema atual apenas permite adicionar paredes e obstculos a um mundo plano, sem aclives e declives. 3.3.3 Servidor Saphira O servidor Saphira permite que o cliente JMR, comunique-se com o simulador Pioneer Saphira, ou mesmo com robs reais controlados por esse simulador. Esse servidor intermedirio foi desenvolvido utilizando JNI (Java 11

Implementao de um Filtro de Kalman Native Interface), que permite que cdigo Java possa ser utilizado com outras linguagems como C++ por exemplo. O servidor Saphira no possui no possui interface interface e somente um conector para a arquitetura Saphira. Assim, a apartir de um cliente JMR possvel enviar comandos para os orobs Pioneer ou para o simulador bidimensional Saphira. Ao contrrio do simulador Java, o simuladora Saphira permite conexo de apenas um rob.

12

Implementao de um Filtro de Kalman

4. Detalhes de Implementao

Figura 4-1 Diagrama de caso de uso do Filtro de Kalman Para elaborar um filtro de Kalman compatvel com o simulador JMR, foi necessrio inicialmente, estudar a estrutura do framework oferecido pelo simulador. JMR um simulador de robtica totalmente orientado a objetos e dividido em trs mdulos principais. Um pacote de classes cliente, um pacote servidor e outro pacote auxiliar, com uma srie de componentes que podem ser usados para efetuar clculos de distribuio ou fazer uso de lgica Fuzzy, etc. Assim a implementao do filtro de Kalman Figura 4-2 Diagrama de classes seguiu essa mesma do pacote servidor Kalman metodologia. O primeiro passo foi definir um conjunto de classes que pudessem receber informaes do cliente JMR, de maneira igual ao Simulador3D. Essas classes poderiam realizar a comunicao com qualquer outra interface que o usurio desenvolver.

13

Implementao de um Filtro de Kalman Esse pacote de classes com caracterstcas de servidor foram implementadas, com alto ndice de reusabilidade de cdigo, e elas puderam ser perfeitamente compartilhadas por todos os trabalhos realizados com o simulador JMR. O prximo pacote constitui-se das interfaces de apresentao dos resultados fornecidos pelo pacote servidor. Essa classe tem a responsabilidade de apresentar, em tempo real e em um mundo discretizado, a posio atual do rob no mundo simulado, a posio calculada pelo sistema de odometria do rob simulado e as estimaes efetuadas pelo filtro de Kalman. Todos esses resultados podem tambm ser gravados em um arquivo ASCII tabulado, e posteriormente ser analisado em aplicaes de planilha Figura 4-3 Diagrama de classes eletrnica, como o do pacote clienteKalman Excel, por exemplo. Assim como o pacote servidor, a estrutura principal da interface de apresentao, foi cedida ao outro grupo, de forma a permitir uma futura integrao dos trabalhos. E para monitorar os movimentos do rob no mundo simulado, foi incorporado um comportamento de rastreio, que a todo instante envia ao servidor kalman, a velocidade atual e a orientao do rob, o servidor transmitia essas informaes interface, onde eram efetuados os clculos de posio com base na velocidade. Figura 4-4 Diagrama de Esse comportamento tambm classes do comportamento indica ao servidor quando deve ser que monitora o rob 14

Implementao de um Filtro de Kalman feito um refresh do filtro de Kalman. Esse refresh uma atualizao da posio do rob, em relao a uma leitura de sonar que foi capaz de perceber um marco conhecido no mundo. Assim de tempos em tempos, o erro de estimativa acumulado pelo filtro de kalman corrigido quando o rob percebe que est prximo de um obstculo. E passa a fazer todos os cculos do filtro de Kalman a partir dessa nova posio.

15

Implementao de um Filtro de Kalman

5. Testes Realizados
Estimao do Filtro de Kalman - Teste 01
140 130 120 110 100 90 80 70 60 50 40 30 20 10 0 -10 0 50 100 150 200 Real 250 300 Estimado 350 Odometro 400 Refresh 450 500 550 600

Primeiros experimentos realizados com o filtro de Kalman, a cada momento que o sonar detectava um obstculo o filtro de Kalman era reiniciado junto com o vetor de covarincias do filtro. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Com a execuo normal do Filtro de Kalman, pde-se notar que a cada iterao da Funo Refrescar, reinicializando o vetor de Covarincia, o Filtro passa estimar novamente a localizao a partir de uma nova posio real coletada, gerando, com isso, um grande erro inicial, que reduzido a medida que o rob caminha, o que esperado.
Execu o sem reinicializao do vetor de C o varin cia (A tualizao a cada 50 iteraes) - Teste 02
25

20

15

10

-5 0 10 20 R ea l 30 O do m e tro R efre sh 40 E stim ado 50 60

16

Implementao de um Filtro de Kalman

Mesmos parmetros de configurao do teste anterior. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Com a execuo normal do Filtro de Kalman, pde-se notar que a cada ocasio em que a leitura do sonar reiniciava a estimao da posio do rob, no reinicializando o vetor de Covarincia, o Filtro passa estimar novamente a localizao a partir de uma nova posio real coletada, gerando, desta vez, um menor erro inicial, se comparado com o Teste 1.

Execuo sem reinicializao do vetor de Covarincia (Atualizao a cada 10 iteraes) - Teste 03

25

20

15

10

0 0 10 20 Real 30 Estimado Odometro 40 Refresh 50 60

Mesmos parmetros de configurao do teste anterior. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Com a execuo normal do Filtro de Kalman, pde-se notar que a cada iterao da Funo Refrescar, porm agora com maior intensidade e no reinicializando o vetor de Covarincia, o Filtro passa estimar a localizao com mais preciso, se comparado com o Teste 2.

17

Implementao de um Filtro de Kalman

Estimao do Filtro de Kalman - Teste 04
25

20

15

10

-5 0 10 20 Real 30 Estimado Odometro 40 50 60

Mesmos parmetros de configurao do teste anterior. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Com a execuo normal do Filtro de Kalman, porm efetuar nenhum refresh no filtro de Kalman tendo como base as leituras do sonar, pode-se notar que a ausncia da informao sensorial nos leva a um erro de estimao do Filtro de Kalman maior do que aquele verificado no Teste 2, por exemplo.
Estimao do Filtro de Kalman - Teste 05
50

40

30

20

10

-10 -20

-10

0 Real

10 Estimado

20 Odometro Refresh

30

40

50

18

Implementao de um Filtro de Kalman

Estimao do Filtro de Kalman - Teste 06
80

70

60

50

40

30

20

10

-10 -10 0 10 20 30 Real 40 Estimado 50 Odometro 60 Refresh 70 80 90 100

O filtro de Kalman nos dois testes acima possuem os mesmos parmetros dos demais testes. Porm foi fornecido ao simulador, indices de erros extremamente altos para os autuadores. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Ao impor um grande erro de posicionamento e velocidade no atuador do rob, pde-se perceber que o prprio simulador encontra dificuldades em posicion-lo no mundo, porm, por intermdio da Funo Refresca o Filtro de Kalman faz algumas estimativas de posicionamento, no caso do Teste 6. Porm com um erro maior nos atuadores, que o caso do Teste 5, nem o Simulador e consequentemente nem o Filtro de Kalman, mesmo com a ajuda da do refresh efetuado pelos sonares, no conseguem estimar uma posio confivel do rob no mundo.
Estimao do Filtro de Kalman - Teste 07
25

20
Sequestro

15

Parede

10

0 0 10 20 Real 30 Estimado Odometro 40 50 60

19

Implementao de um Filtro de Kalman Mesmos parmetros de configurao do teste anterior. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Ao analisar este Teste de "sequestro" do rob, pde-se verificar que mesmo com a ao (destacada no grfico), o Filtro continua com seu processo de Estimativa, no considerando a mudana de posio. Ao se deparar com a parede (destacada no grfico), as leituras do Odmetro e a posio Real do rob, se mantiveram constantes, porm o Filtro ainda continua executando suas estimativas.
Estim ao do Filtro de Kalm an - Teste 08
25

20

15

10

-5 -10 0 10 20 Real Estimado 30 O dom etro 40 50 60

Nesse teste foram efetuadas mudanas nos parmetros do filtro de kalman. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Com a execuo do Filtro de Kalman, considerando as variveis de Rudo de Estado (w) e Rudo de medida (v) como no ortogonais (Pwv <> 0), nota-se que no h grandes alteraes no resultado final da estimativa, provavelmente em funo do algoritmo implementado fazer uso de valores pontuais ao invs de vetores para representar os rudos citados anteriormente.
Estim ao do Filtro de K alm an - Teste 09
25

20

15

10

0 0 100 200 300 R eal 400 E stim ado Odom etro 500 Refresh 600 700 800

20

Implementao de um Filtro de Kalman Nesse teste foram todos os parmetros do filtro de kalman foram zerados. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Nesse teste foi simulado a inexistncia de rudo de estado e de medida, porm o Filtro de Kalman foi projetado para funcionar com a existncia desses rudos, logo no h estimativas a serem feitas.
Estimao do Filtro de Kalman - Teste 10
25

20

15

10

0 0 10 20 Real 30 Estimado Odometro 40 Refresh 50 60

Nesse teste foram utilizados os parmetros defaults do filtro de Kalman. O rob partiu da posio (0,0) foi at (2600,2200) em seguida caminhou at (5000,2000) e por fim caminhou at (5000,1000). Neste teste foi simulado que os atuadores do rob so "perfeitos" e nessas circunstncias a medidas odomtricas so reais. Nessas circunstncias, o filtro de Kalman no boa soluo para estimativa de posio. Somente as medidas do odomtro so suficientes para estimar a posio do rob com preciso do que com a utilizao do filtro.

21

Implementao de um Filtro de Kalman

6. Consideraes Finais

6.1. Concluso A implementao da localizao de um rob com utilizao do filtro de Kalman, por meio de medidas sensoriais e de sua atuao perfeitamente vivel, quando utilizamos os fatores de projeto adequados para o filtro e um modelo coerente para os dispositivos de atuao e dos sensores. Neste trabalho, mesmo com as simplificaes didticas assumidas, observouse que o objetivo final da estimao da posio do rob, com um certo grau de confiabilidade pde ser alcanado. Foi possvel reproduzir esta utilizao do filtro e vrios de seus problemas, sendo que todos eles podem ser trabalhados para que no prejudiquem o resultado da estimativa de localizao. 6.2. Sugestes Como atividade sugerida, pode-se implementar uma nova funo de Refresh do filtro de Kalman com utilizao de fuso sensorial, que aumentaria ainda mais a qualidade e confiabilidade dos resultados de estimao do Filtro.

22

Implementao de um Filtro de Kalman

7. Atividades Realizadas

Atividade Pesquisas bibliogrficas Pesquisas de algoritmos Pesquisa de Adequabilidade dos Simuladores Escolha do Simulador Anlise do Framework do Simulador Implementao da Interface Reutilizvel Implementao do Servidor Implementao do Comportamento de Rastreio Execuo dos 50 Testes Escolha dos 10 Testes do Relatrio Anlise dos Resultados Preparao da Apresentao

Participante Evandro, Robson Marco, Evandro Juliano, Robson Todos Juliano e Claudiney Claudiney Claudiney Juliano e Claudiney Todos Marco, Juliano e Claudiney Marco, Juliano e Claudiney Claudiney, Evandro, Robson e Marco

Preparao do contedo do relatrio Compilao e Arte final do Relatrio

Todos Juliano

23

Implementao de um Filtro de Kalman

8. Bibliografia

BAEZA, Ignacio Iborra; ORTEGA, Miguel Angel Lozano; LPEZ Domingo Gallardo. JMR 1.0 - Manual do Programador. HEMERLY, Elder M. Controle por computador de Sistemas Dinmicos. JAVADOC. JavaTM 2. Plataforma std.ed.v1.4.1. Documento HTML com help de Java. RIBEIRO, H. C. Fundamentos Computacionais de Robtica Mvel. Notas de Aula. 1 Semestre 2003. WELCH, Greg e BISHOP, Gary. An Introduction to the Kalman Filter. Artigo internet em: http://www.cs.unc.edu/~welch/kalman/kalman_filter/ kalman.html. Acesso em Junho de 2003.

24

Implementao de um Filtro de Kalman

I. Apndice - Cdigo Fonte

I.1 gui.GuiGrade.java
package gui; import import import import import import import import import import javax.swing.*; java.awt.*; java.awt.event.*; java.util.*; javaGL.*; java.io.*; java.net.URL; java.util.*; javax.swing.text.*; javax.swing.undo.*;

public class GuiGrade extends JFrame { // arquivo de configurao private static java.util.ResourceBundle resKalman = java.util.ResourceBundle.getBundle("KalmanConf"); private static java.util.ResourceBundle resMundo = java.util.ResourceBundle.getBundle(resKalman.getString("mapa")); private JPanel tela = null; private JLabel titulo = null; private JPanel grade = null; public int linhas = 8; public int colunas = 8; private String posicaoRobo = "0_0"; private Hashtable pontos = new Hashtable(); private Kalman kalman; // caixa de dialogo de arquivo protected FileDialog fileDialog; public GuiGrade() { super(); initialize(); } public GuiGrade(int l, int c) { super(); linhas = l; colunas = c; initialize(); } public static void main(String[] args) { GuiGrade gg = null; if (args.length>1) gg = new GuiGrade(Integer.parseInt(args[0]), Integer.parseInt(args[1])); else gg = new GuiGrade(); gg.show(); } private void initialize() { this.setContentPane(getTela()); this.setSize(linhas*15, colunas*15);

25

Implementao de um Filtro de Kalman

MenuBar mBar = new MenuBar(); Menu mnuKalman = new Menu("Kalman"); MenuItem itemLer = new MenuItem ("Ler Mundo"); MenuItem itemSalvar = new MenuItem ("Salvar X e Xe"); MenuItem itemSair = new MenuItem ("Sair"); itemLer.addActionListener (new ActionListener () { public void actionPerformed (ActionEvent evt) { System.out.println("Ler"); // Frame frame = new Frame(); if (fileDialog == null) { fileDialog = new FileDialog(frame); } fileDialog.setMode(FileDialog.LOAD); fileDialog.show(); String file = fileDialog.getFile(); if (file == null) { return; } // limpando as grades do mundo /* for (int j = linhas-1; j >= 0; j--) { for (int i = 0; i < colunas; i++) { ( ( J L a b e l ) p o n t o s . g e t ( I n t e g e r . t o S t r i n g ( i ) + " _ " + I n t e g e r . t o S t r i n g ( j ) ) ) . s e t B o r d e r ( B o r d e r F a c t o r y . c r e a t e L i n e B o r d e r ( j a v a . a w t . C o l o r . w h i t e , 1 ) ) ; } } */ // preenchendo o mundo resMundo = java.util.ResourceBundle.getBundle(file.substring(0, file.indexOf("."))); for (int i = 0; i < Integer.parseInt(resMundo.getString("blocos")); i++) { ((JLabel)pontos.get(resMundo.getString("b_"+i))).setBorder(BorderFactory.createLineBorder(java.awt.Color.red,1)); } }} ); itemSalvar.addActionListener (new ActionListener () { public void actionPerformed (ActionEvent evt) { System.out.println("Salvar"); Frame frame = new Frame(); if (fileDialog == null) { fileDialog = new FileDialog(frame); } fileDialog.setMode(FileDialog.SAVE); fileDialog.show(); String file = fileDialog.getFile(); if (file == null) { return; } String directory = fileDialog.getDirectory(); System.out.println(directory+file); BufferedWriter bw = null; try { bw = new BufferedWriter(new FileWriter(directory+file, true)); bw.write("Odometro (x); Odometro (y); Estimado (x); Estimado (y); Real (x); Real (y); Refrescar (x); Refrescar (y)\n");

26

Implementao de um Filtro de Kalman

for (int i = 0; i < kalman.k; i++) { // X real Point2D p2dX = (Point2D)kalman.X.get(i); bw.write(p2dX.getX()+";"+p2dX.getY()+";"); Point2D p2d = (Point2D)kalman.Xe.get(i); bw.write(p2d.getX()*kalman.conversao+";"+p2d.getY()*kalman.conversao+";"); Point2D p2real = (Point2D)kalman.Xreal.get(i); bw.write(p2real.getX()+";"+p2real.getY()); if (i < kalman.Refrescar.size()) { Point2D p2Ref = (Point2D)kalman.Refrescar.get(i); bw.write(";"+p2Ref.getX()*kalman.conversao+";"+p2Ref.getY()*kalman.conversao); } bw.write("\n"); } } catch (IOException ioe) { System.out.println("Erro: "+ioe.toString()); } finally { if (bw!=null) { try { bw.flush(); bw.close(); } catch (IOException ioe) { System.out.println("Erro: "+ioe.toString()); } } } } } ); itemSair.addActionListener (new ActionListener () { public void actionPerformed (ActionEvent evt) { System.exit(0); } } ); mnuKalman.add(itemLer); mnuKalman.add(itemSalvar); mnuKalman.add(itemSair); mBar.add(mnuKalman); setMenuBar (mBar); // Preenchendo os obstculos automaticamente if (Integer.parseInt(resKalman.getString("lerMapa")) == 1) { for (int i = 0; i < Integer.parseInt(resMundo.getString("blocos")); i++) { ((JLabel)pontos.get(resMundo.getString("b_"+i))).setBorder(BorderFactory.createLineBorder(java.awt.Color.red,1)); } }

27

Implementao de um Filtro de Kalman

} private JPanel getTela() { if(tela == null) { tela = new JPanel(); java.awt.BorderLayout bl = new java.awt.BorderLayout(); tela.setLayout(bl); tela.add(getTitulo(), java.awt.BorderLayout.NORTH); tela.add(getGrade(), java.awt.BorderLayout.CENTER); } return tela; } private JLabel getTitulo() { if(titulo == null) { titulo = new JLabel(); titulo.setText("CT-219: Trabalho Prtico 2"); titulo.setHorizontalAlignment(SwingConstants.CENTER); } return titulo; } private JLabel getNewLabel() { JLabel l = new JLabel(); l.setText(""); // l.setBorder(BorderFactory.createLineBorder(java.awt.Color.cyan,1)); l.setHorizontalAlignment(SwingConstants.CENTER); l.setForeground(java.awt.Color.black); return l; } private JPanel getGrade() { if(grade == null) { grade = new javax.swing.JPanel(); grade.setBackground(java.awt.Color.white); GridLayout gl = new GridLayout(linhas,colunas,1,1); grade.setLayout(gl); for (int j = linhas-1; j >= 0; j--) { for (int i = 0; i < colunas; i++) { JLabel jl = getNewLabel(); // jl.setText(Integer.toString(i)+"_"+Integer.toString(j)); pontos.put(Integer.toString(i)+"_"+Integer.toString(j), jl); grade.add(jl); } }

} return grade; } public void setPosicaoRobo(String pos, int xe) { try { Object o = pontos.get(pos); if (o==null) { System.out.println("Posio inexistente "+Integer.toString(xe)); } else { Object o1 = pontos.get(posicaoRobo); if ((xe==1) && (o1!=null)) { JLabel l1 = (JLabel)o1; l1.setText(""); }

28

Implementao de um Filtro de Kalman

if (xe==1) posicaoRobo = pos; JLabel l = (JLabel)o; if (xe==0) { l.setBorder(BorderFactory.createLineBorder(java.awt.Color.gray,1)); } else if (xe==1) { l.setBorder(BorderFactory.createLineBorder(java.awt.Color.cyan,1)); l.setText("="); } else { l.setBorder(BorderFactory.createLineBorder(java.awt.Color.blue,1)); } } } catch (Exception e) { System.out.println("Erro: "+e); e.printStackTrace(System.out); } } public void kalman(Point2D pos, double orient, double vel, boolean refresca) { if (kalman==null) kalman = new Kalman(pos); else if (refresca) kalman.RefreshKalman(pos); else kalman.execKalman(pos, orient, vel); int i = 0; if (kalman.k>0) i = kalman.k-1; // X odometro Point2D p2dX = (Point2D)kalman.X.get(i); Double Xx = new Double(p2dX.getX()); Double Xy = new Double(p2dX.getY()); setPosicaoRobo(Integer.toString(Xx.intValue())+"_"+Integer.toString(Xy.intValue()), 2); // X estimado Point2D p2d = (Point2D)kalman.Xe.get(i); Double x = new Double(p2d.getX()*kalman.conversao); Double y = new Double(p2d.getY()*kalman.conversao); setPosicaoRobo(Integer.toString(x.intValue())+"_"+Integer.toString(y.intValue()), 1); // X real Point2D p2real= (Point2D)kalman.Xreal.get(i); Double xr = new Double(p2real.getX()); Double yr = new Double(p2real.getY()); setPosicaoRobo(Integer.toString(xr.intValue())+"_"+Integer.toString(yr.intValue()), 0); } }

29

Implementao de um Filtro de Kalman I.2 gui.Kalman.java

package gui; import import import import import import java.util.*; javaGL.*; simulator.Config; loaders.ConfLoader; robots.RobotParams; simulator.Monitor;

public class Kalman { // arquivo de configurao private static java.util.ResourceBundle resKalman = java.util.ResourceBundle.getBundle("KalmanConf"); // numero de iteracoes int k = 0; // vetores de posicoes 2D Vector Xreal = new Vector(); Vector X = new Vector(); Vector Xe = new Vector(); Vector Y = new Vector(); // posicao real // velocidade lida odometro

Vector Refrescar = new Vector(); Vector P = new Vector(); Vector K = new Vector(); Vector erro = new Vector(); // fator de conversao int conversao = Integer.parseInt(resKalman.getString("conversao")); double A = Double.parseDouble(resKalman.getString("a")); double C = Double.parseDouble(resKalman.getString("c")); double Pw = Double.parseDouble(resKalman.getString("pw")); double Pv = Double.parseDouble(resKalman.getString("pv")); int Pwv = Integer.parseInt(resKalman.getString("pvw")); // arquivo de configuracao ConfLoader cl; Config conf; double tempoAnterior; public Kalman(Point2D pos) { super(); try { cl = new ConfLoader("simulator.conf"); conf = cl.getConfig(); } catch(Exception e) { System.out.println("*****kalman.kalman()*****:" + e.getMessage()); } Xreal.add(k,new Point2D(pos.getX()/conversao, pos.getY()/ conversao)); X.add(k, new Point2D(pos.getX()/conversao, pos.getY()/ conversao)); Xe.add(k, X.get(k)); P.add(k, new Point2D(conf.dLVelError, conf.dPositionError)); Varincia de X; tempoAnterior = System.currentTimeMillis(); } public void execKalman(Point2D pos, double orient, double vel) { //

30

Implementao de um Filtro de Kalman

double tempo = System.currentTimeMillis(); double tempoAtual = tempo - tempoAnterior; tempoAnterior = tempo; Point2D p2dX = (Point2D)X.get(k); Point2D p2dX_1 = new Point2D(); p2dX_1.setX(pos.getX()/conversao); p2dX_1.setY(pos.getY()/conversao); Xreal.add(k+1, p2dX_1); // leitura a partir de odometro Point2D p2x_odo = new Point2D(); // frequencia de atualizacao deve vir da classe p2x_odo.setX (p2dX.getX() + (vel*(tempoAtual/ 1000)*Math.cos(Math.toRadians(orient)))/conversao); p2x_odo.setY (p2dX.getY() + (vel*(tempoAtual/ 1000)*Math.sin(Math.toRadians(orient)))/conversao); X.add(k+1, p2x_odo); Point2D p2dY = new Point2D(); p2dY.setX(C*p2dX.getX()+Math.sqrt(Pv)*(new Random()).nextDouble()); p2dY.setY(C*p2dX.getY()+Math.sqrt(Pv)*(new Random()).nextDouble()); Y.add(k, p2dY); Point2D p2dP = (Point2D)P.get(k); Point2D p2dK = new Point2D(); p2dK.setX(A*p2dP.getX()*C*(1/ (C*p2dP.getX()*C+Pv))); p2dK.setY(A*p2dP.getY()*C*(1/ (C*p2dP.getY()*C+Pv))); K.add(k, p2dK); Point2D p2dXe = (Point2D)Xe.get(k); Point2D p2dXe_1 = new Point2D(); p2dXe_1.setX(A*p2dXe.getX()+p2dK.getX()*p2dY.getX()-C*p2dXe.getX()); p2dXe_1.setY(A*p2dXe.getY()+p2dK.getY()*p2dY.getY()-C*p2dXe.getY()); Xe.add(k+1, p2dXe_1); Point2D p2dP_1 = new Point2D(); p2dP_1.setX(A*p2dP.getX()*Ap2dK.getX()*A*p2dP.getX()*C+Pw); p2dP_1.setY(A*p2dP.getY()*Ap2dK.getY()*A*p2dP.getY()*C+Pw); P.add(k+1, p2dP_1); System.out.println("Xreal-x: "+p2dX_1.getX()+" Xreal-y: "+p2dX_1.getY()); System.out.println("X-x: "+p2dX.getX()+" X-y: "+p2dX.getY()); System.out.println("Xe-x: "+p2dXe.getX()*conversao+" Xe-y: "+p2dXe.getY()*conversao); Double x = new Double(p2dXe.getX()); Double y = new Double(p2dXe.getY());

31

Implementao de um Filtro de Kalman

k = k + 1; } public void RefreshKalman(Point2D pos) { Y.add(k, new Point2D(1, 1)); K.add(k, new Point2D(1, 1)); k = k + 1; tempoAnterior = System.currentTimeMillis(); Point2D estimadoconvertido = new Point2D(); Xreal.add(k,new Point2D(pos.getX()/conversao, pos.getY()/ conversao)); X.add(k, new Point2D(pos.getX()/conversao, pos.getY()/ conversao)); estimadoconvertido.setX(((Point2D)X.get(k)).getX()/conversao); estimadoconvertido.setY(((Point2D)X.get(k)).getY()/conversao); Xe.add(k, estimadoconvertido); Refrescar.add(estimadoconvertido); // verifica se vai reiniciar a convariancia ou no if (Integer.parseInt(resKalman.getString("resetConvariancia"))==0) P.add(k, P.get(k-1)); else P.add(k, new Point2D(conf.dLVelError,conf.dPositionError)); System.out.println("Xreal-x: "+((Point2D)Xreal.get(k)).getX()+" Xreal-y: "+((Point2D)Xreal.get(k)).getY()); System.out.println("X-x: "+((Point2D)X.get(k)).getX()+" X-y: "+((Point2D)X.get(k)).getY()); System.out.println("Xe-x: "+((Point2D)Xe.get(k)).getX()*conversao+" Xe-y: "+((Point2D)Xe.get(k)).getY()*conversao); System.out.println("*************Refrescado***************"); } }

32

Implementao de um Filtro de Kalman

I.3 rmi.ServidorCentral.java
package rmi; import import import import import gui.*; java.rmi.*; java.rmi.server.*; java.util.*; javaGL.*;

public class ServidorCentral extends UnicastRemoteObject implements Central { // arquivo de configurao private static java.util.ResourceBundle resKalman = java.util.ResourceBundle.getBundle("KalmanConf"); private GuiGrade gg = new GuiGrade(Integer.parseInt(resKalman.getString("gradeLinhas")), Integer.parseInt(resKalman.getString("gradeColunas"))); public GuiGrade getGuiGrade() { return gg; } public ServidorCentral() throws RemoteException { super(); gg.show(); } public ServidorCentral(int port) throws RemoteException { super(port); } public ServidorCentral( int port, RMIClientSocketFactory csf, RMIServerSocketFactory ssf) throws RemoteException { super(port, csf, ssf); } public void setarPosicao(String pos) throws RemoteException { getGuiGrade().setPosicaoRobo(pos,1); } public void kalman(Point2D pos, double orient, double vel, boolean refresca) throws RemoteException { getGuiGrade().kalman(pos, orient, vel, refresca); } }

33

Implementao de um Filtro de Kalman I.4 rmi.Servidor.java

package rmi; import import import import java.io.*; java.rmi.*; java.rmi.server.*; javaGL.*;

public class Servidor { public static void main(String[] args) throws RemoteException, java.net.MalformedURLException { ServidorCentral sc = new ServidorCentral(); Naming.rebind( "rmi://localhost/ServidorCentral", sc); } }

I.5 rmi.Central.java
package rmi; import java.rmi.*; import java.util.*; import javaGL.*; public interface Central extends Remote { public void setarPosicao(String pos) throws RemoteException; public void kalman(Point2D pos, double orient, double vel, boolean refresca) throws RemoteException; }

I.6 user.process.FiltroKalman.java
package user.processes; import import import import robots.*; robots.process.*; fuzzy.*; javaGL.*;

import rmi.*; import java.rmi.*; public class FiltroKalman extends FuzzyBehavior { private static java.util.ResourceBundle resKalman = java.util.ResourceBundle.getBundle("KalmanConf"); /** * Reiniciar filtro de kalman */ boolean refresca; /** * Limiar de sonares */ public static double limiar = Double.parseDouble(resKalman.getString("limiar")); /** * Posio inicial X */ double dX; /**

34

Implementao de um Filtro de Kalman

* Posio inicial Y */ double dY; double []lsonars; /** * Fuzzy variable for left-front distance */ FuzzyVariable distFrontL; /** * Fuzzy variable for right-front distance */ FuzzyVariable distFrontR; /** * Fuzzy variable for left-side distance */ FuzzyVariable distSideL; /** * Fuzzy variable for right-side distance */ FuzzyVariable distSideR; /** * Robot sonars */ Sonar []sonars; /** * Number of sonars */ int iSonarNum; int contRefresca = 0; /** * Parameter names */ public static String []sParamNames = {}; public FiltroKalman() { setBlock(false); setTimeout(-1); } public void initValues() { sonars = rhand.getParams().SonarUnit; iSonarNum = rhand.getParams().iSonarNum; Position2D pos = getRobotState().pos; Point2D p2d = new Point2D(); p2d.setX(pos.dX); p2d.setY(pos.dY); double orient = pos.dTh; double vel = getRobotState().dLVel; try { Central c = (Central) Naming.lookup("ServidorCentral"); c.kalman(p2d, orient, vel, false); } catch (Exception ex) { ex.printStackTrace(); } }

35

Implementao de um Filtro de Kalman

/** * Updates behavior parameters */ public void updateValues() { if (contRefresca<12000) contRefresca = contRefresca + 1; refresca = false; sonars = rhand.getParams().SonarUnit; double leituramedia; if (Integer.parseInt(resKalman.getString("refrescar"))==1) { for (int i = 0; i < iSonarNum; i++) { leituramedia=0; for (int j = 0; j <sonars[i].getReadings().length; j++) { System.out.println(i+"--"+j+" --> "+sonars[i].getReadings()[j].dRange); leituramedia+=sonars[i].getReadings()[j].dRange; } leituramedia=leituramedia/ sonars[i].getReadings().length; if ((leituramedia>0) && (leituramedia<limiar) && (contRefresca > Integer.parseInt(resKalman.getString("valorRefresca")))) { System.out.println("Refrescar"); refresca = true; contRefresca = 0; break; } if (refresca) break; } } Position2D pos = getRobotState().pos; Point2D p2d = new Point2D(); p2d.setX(pos.dX); p2d.setY(pos.dY); double orient = pos.dTh; double vel = getRobotState().dLVel; try { Central c = (Central) Naming.lookup("ServidorCentral"); c.kalman(p2d, orient, vel, refresca); } catch (Exception ex) { ex.printStackTrace(); } } }

36