Sistema de Navegação Indoor por Imagens para VANT Quadricóptero

INSTITUTO TECNOLÓGICO DE AERONÁUTICA
CONSELHO NACIONAL DE DESENVOLVIMENTO CIENTÍFICO E
TECNOLÓGICO - CNPq
CNPq
CONSELHO NACIONAL DE DESENVOLVIMENTO
CIENTÍFICO E TECNOLÓGICO
PROGRAMA INSTITUCIONAL DE BOLSAS DE INICIAÇÃO CIENTÍFICA -
PIBIC
Desenvolvimento de um Sistema de
Navegação Indoor por Imagens para um
Veículo Aéreo não Tripulado do tipo
Quadricóptero
Roberto Brusnicki
RELATÓRIO FINAL DE ATIVIDADES
Orientador:
Dr. Davi Antônio dos Santos
08 / 2013

CONSELHO NACIONAL DE DESENVOLVIMENTO CIENTÍFICO E
TECNOLÓGICO - CNPq
CNPq
CONSELHO NACIONAL DE DESENVOLVIMENTO
CIENTÍFICO E TECNOLÓGICO
PROGRAMA INSTITUCIONAL DE BOLSAS DE INICIAÇÃO CIENTÍFICA -
PIBIC
Relatório Final de Atividades
Desenvolvimento de um Sistema de
Navegação indoor por Imagens para um
Veículo Aéreo não Tripulado do tipo
Quadricóptero
São José dos Campos, 03 / 08 / 2013
Nome do aluno
Roberto Brusnicki
Assinatura do aluno
Nome do orientador
Davi Antônio dos Santos
Assinatura do orientador

PROGRAMA INSTITUCIONAL DE BOLSAS DE INICIAÇÃO CIENTÍFICA - PIBIC
Formulário de Aprovação de Relatório pelo Orientador
Relatório:
Rel. Parcial
X
Rel. Final
1- CONSIDERO O RELATÓRIO APROVADO COM BASE NOS SEGUINTES ASPECTOS
2- APRECIAÇÕES DO ORIENTADOR SOBRE O DESEMPENHO DO BOLSISTA NA EXECUÇÃO DO TRABALHO DE INICIAÇÃO CIENTÍFICA
Local e data:
Assinatura do Orientador:

ÍNDICE
1. INTRODUÇÃO .................................................................................................................... 4
2. ANDAMENTO E PLANEJAMENTO ........................................................................................... 6
2.1. RESUMO DO PLANO INICIAL ............................................................................................. 6
2.2. RESUMO DAS ATIVIDADES REALIZADAS ............................................................................... 7
3. DEFINIÇÃO DO PROBLEMA ................................................................................................... 9
4. FORMULAÇÃO DO FILTRO .................................................................................................. 10
4.1. ESTIMAÇÃO DE QUATÉRNIO ........................................................................................... 10
4.2. QEKF ....................................................................................................................... 13
4.2. OBTENÇÃO DOS VETORES UNITÁRIOS .............................................................................. 14
5. SIMULAÇÕES COMPUTACIONAIS ......................................................................................... 16
6. OBTENÇÃO DOS DADOS EM TEMPO REAL POR COMUNICAÇÃO SERIAL ........................................ 21
7. CONCLUSÕES .................................................................................................................. 22
8. AGRADECIMENTOS........................................................................................................... 23
9. BIBLIOGRAFIA ................................................................................................................. 23
CÓDIGOS DO FILTRO DE KALMAN PARA ESTIMAÇÃO DE QUATÉRNIO DE ROTAÇÃO
ANEXO 1: (MATLAB) ............................................................................................................. 24
ANEXO 2: (C - ARDUINO)....................................................................................................... 27
1. INTRODUÇÃO
Este trabalho está envolvido com VANTS do tipo quadricóptero, que consiste em veículo com capacidade de decolagem e aterrissagem verticais constituído de quatro rotores acionados por motores elétricos individuais e independentes.
Para que um VANT quadricóptero tenha sua trajetória controlada em malha fechada e de forma autônoma, faz-se necessário que esse veículo embarque um Sistema de Navegação (SN), que consiste em um dispositivo capaz de estimar, em tempo real, sua posição, sua velocidade linear, sua atitude (orientação) e sua velocidade angular. A função de um sistema de controle por realimentação é essencialmente fazer com que medidas/estimativas de variáveis dinâmicas (como atitude, posição e velocidade) rastreiem trajetórias desejadas. Por essa razão, um controle confiável, acurado e robusto requer um SN com as mesmas propriedades qualitativas. A importância de um SN para o funcionamento adequado de VANTs autônomos motiva uma avaliação detalhada de seus componentes e métodos.
Em particular, a chamada navegação inercial é a forma tradicional de se realizar navegação por meio da integração das medidas dos sensores inerciais (acelerômetros e girômetros) embarcados no veículo de interesse. A principal vantagem da navegação inercial pura é sua independência em relação a sinais externos e a condições de visibilidade e sua imunidade a interferências intencionais, como o jamming. Graças a essas características, os sensores inerciais são bastante apropriados para operar em ambientes como túneis, cavernas e interiores de prédios.

A despeito de sua independência em relação a sinais externos ao sistema, a navegação inercial pura apresenta a significativa desvantagem de produzir estimativas de posição, velocidade e atitude cujos erros divergem sem limites com o tempo. Essa é uma característica bem conhecida, que é explicada, em breves palavras, pelo fato de que junto com acelerações e velocidades angulares, o sistema também integra ao longo do tempo os vieses e derivas dos sensores. De forma a limitar os erros divergentes de sistemas de navegação inerciais puros, invoca-se frequentemente o uso de fusão de sensores para permitir a combinação das medidas inerciais com aquelas providas por outros sensores, como magnetômetros, sensores ultrassônicos, GPS, câmera, entre outros.
Os métodos de fusão sensorial mais comuns são aqueles baseados no filtro de Kalman, ferramenta que provê um framework para estimar variáveis dinâmicas (estados) de um sistema a partir de sequências de medidas adquiridas de diversos sensores.
A fusão de dados de GPS e sensores inerciais é um tópico importante e já bem estudado. Os esquemas de fusão GPS-INS têm aplicações em diversas áreas, como aviação, guiamento de satélites, aproximação e pouso automáticos de aeronaves, aplicações marítimas e, em especial, em navegação de VANTs. O sucesso desses esquemas se deve principalmente à complementaridade das características típicas dos erros de medição de GPS e de sensores inerciais. A navegação baseada apenas na integração de medidas dos sensores inerciais (navegação inercial pura) é precisa num curto intervalo de tempo, no entanto, resulta em estimativas de posição que se degradam sem limites devido à existência de vieses em acelerômetros e de derivas em girômetros. Com base na estrutura do filtro de Kalman, medidas de sensores inerciais e de GPS podem ser fundidas, segundo uma variedade de esquemas de implementação, permitindo um desempenho de estimação (de posição, velocidade e atitude) superior a ambos os subsistemas individualmente.
Infelizmente, a navegação inercial assistida por GPS pode carecer de confiabilidade quando opera em lugares com linhas de visada deficientes em relação aos satélites do sistema GPS. Exemplos de tais lugares são: interiores de prédios, cavernas, minas, cânions urbanos e em florestas (entre árvores, por exemplo). De forma a compensar essas limitações, o uso de câmera digital como auxílio à navegação tem recentemente atraído o interesse de pesquisadores. A fusão INS-Câmera basicamente consiste no uso de câmera(s) digital(is) para medir/rastrear a posição de features numa sequência de imagens e, com base na estrutura do filtro de Kalman, usar tais informações para estimar erros de navegação/erros de sensores e compensá-los nas estimativas de posição, velocidade e atitude. Esses esquemas de fusão parecem ser especialmente adequados para a navegação em ambientes indoor contendo landmarks, que oferecem informações substancialmente ricas para a navegação do veículo.
O restante deste trabalho está organizado da seguinte maneira. O Capítulo 2 apresenta um resumo da proposta inicial, a descrição dos trabalhos realizados durante o

período 2012/2 e o planejamento para as próximas atividades. O Capítulo 3 define o problema de controle de determinação de atitude e navegação usando sensores inerciais e uma câmera embarcada apontada verticalmente para baixo. O Capítulo 4 apresenta a formulação de um filtro de Kalman que soluciona o problema de determinação de atitude. O Capítulo 5 apresenta alguns resultados de avaliações realizadas por simulações computacionais. Por fim, o Capítulo 6 apresenta as conclusões da etapa parcial a que se refere este relatório.
2. ANDAMENTO E PLANEJAMENTO
2.1. RESUMO DO PLANO INICIAL
Este trabalho tem como objetivo desenvolver um sistema de navegação de baixo custo apropriado para voo indoor equipado com, dentre outros sensores, uma câmera, a ser utilizado em um VANT (Veículo Aéreo Não Tripulado) de pequeno porte do tipo quadricóptero (que consiste de um veículo com capacidade de decolagem e aterrissagem verticais constituído de quatro rotores acionados por motores elétricos individuais e independentes), que será projetado, fabricado e posteriormente avaliado experimentalmente.
Para alcançar-se tal objetivo, faz-se necessário dotar o sistema de navegação a ser desenvolvido para o VANT de vários sensores que por meio de medidas possibilite o voo autônomo estável e controlado. Esse sistema deve ser equipado com um acelerômetro de 3 eixos para determinação de acelerações lineares, um giroscópio de 3 eixos para determinação de velocidades angulares, um sensor ultrassônico para medição de altitude, e de uma câmera para obtenção de imagens.
A Metodologia adotada consiste em inicialmente construir-se um quadricóptero elétrico utilizando componentes de aeromodelismo, e verificar seu funcionamento através de testes rádio controlados. Dado isto, integrar ao quadricóptero a parte física do sistema de navegação a ser desenvolvido, ou seja, uma plataforma em que se encontrem os sensores supracitados. Posteriormente, o desenvolvimento do software do sistema de navegação, o qual consiste basicamente de um filtro de Kalman para fusão dos sensores inerciais, sensor ultrassônico de altitude e a câmera. Este último passo possui implícito todo o estudo a cerca de processos estocásticos, bem como o estudo do filtro de Kalman, e da física do problema, assim como simulação de seu funcionamento anteriormente a sua utilização no VANT propriamente dito.

2.2. RESUMO DAS ATIVIDADES REALIZADAS
1) Construção
Inicialmente construiu-se o quadricóptero com peças de aeromodelismo. O que se consiste basicamente do frame em formato de cruz, os 4 motores elétricos do tipo brushless, suas respectivas hélices, e o sistema de alimentação, constituído pelos eletronic speed controllers (ESC’s) cabos de alimentação e da bateria de polímero de Lítio. Além disto, tal quadricóptero possui também um Arduíno Mega como micro controlador utilizado para seu sistema de navegação. O software utilizado para fazer a estabilização de atitude consiste do Aeroquad open-sourse.
Esta primeira fase permitiu a familiarização com o funcionamento deste modelo relativamente novo de VANTs, sem que houvesse excessiva preocupação com eventuais danos causados aos seus componentes devido a um mau controle de seu voo, pois não estaria voando de maneira autônoma, mas sim de maneira radio-controlada.
2) Embasamento Teórico
Com a finalidade de se adquirir o embasamento matemático necessário para a realização deste trabalho, foram estudados os seguintes tópicos:
 Variáveis aleatórias e processos estocásticos. Parte dessa preparação foi realizada através da disciplina de graduação MOQ-13 (Probabilidade e Estatística);
 Filtro de Kalman. Foram estudadas diversas formulações do filtro de Kalman: discreta-discreta, contínua-discreta tando para o filtro linear quanto para o filtro de Kalman estendido. As referências utilizadas foram (Gelb, 1974) [1] e (Hwang e Brown, 1997) [2];
 Cinemática de atitude. Foram estudadas as diversas representações de atitude bem como as respectivas equações diferenciais que modelam a dinâmica de atitude em função das velocidades angulares do veículo ao longo do tempo. Existem várias possibilidades de parametrização de atitude, dentre elas, as mais recorrentes são: ângulos de Euler, matriz de cossenos diretores (DCM), quaternion de rotação e parâmetros modificados de Rodrigues. A referência adotada foi (Shuster, 1993) [4].
3) Simulações computacionais
Adotou-se o MATLAB como ambiente de simulação. Foram simuladas a cinemática de atitude do veículo bem como as medidas dos sensores, que são uma

câmera embarcada com apontamento vertical voltado para baixo e uma tríade de girômetros. Nesse mesmo ambiente, implementou-se também as equações de um filtro de Kalman estendido discreto-discreto para estimação da atitude representada por quaternion de rotação. Foram realizados diversos testes que possibilitaram uma boa sintonia do filtro e comprovaram sua capacidade de convergência e robustez.
4) Familiarização com os sensores utilizados
Adotou-se o uso do sensor GY-521 para obtenção das medidas de velocidade angular, e a CMUCam4 como sensor de rastreamento de cores para obtenção dos vetores unitários indicados na figura 3. Ambos os sensores escolhidos atendem ao proposto pelo projeto, sendo sensores de baixo custo, mas que com sistema de navegação desenvolvido fornecem dados bom o suficiente para se fazer a estimação da atitude do quadricóptero com alta precisão. Uma das vantagens de se ter trabalho com o sensor GY-521, é que este também possui um acelerômetro de 3 eixos, sendo estas medidas necessárias em um trabalho posterior em que utilizará o sistema de navegação em movimento. A CMUCam4 consiste em um sensor de visão computacional programável, que pode ser utilizado como rastreador de cor, entre inúmeras outras funções. Ambos os sensores foram utilizados neste projeto em conjunto com o microprocessador Arduino Mega, tendo sido necessário um tempo de estudo de suas bibliotecas, funções, configurações, e utilização de modo a otimizar a obtenção dos dados necessários.
Figura 1: (a) Sensor de rastreamento de cores CMUCam4. (b) IMU de 6 graus GY-521.

5) Simulações computacionais com obtenção dos dados em tempo real, e familiarização com comunicação serial entre Arduino e MATLAB
Tendo-se o código no MATLAB funcionando de maneira precisa com a simulação, adaptou-se este para que utilizasse os dados provenientes em tempo real dos sensores acoplados ao Arduino Mega. Para tal utilização, foi-se necessário significativo tempo na familiarização com a comunicação serial necessária entre ambos os softwares (Arduino IDE, e MATLAB). Além disto, foi-se necessário escrever o código do Arduíno que obtivesse os dados dos sensores, e os enviassem para o computador através da porta serial. Foram realizados alguns testes, como levantamento das distribuições das medidas dos sensores, que possibilitaram uma excelente sintonia do filtro e uma vez mais comprovaram sua capacidade de convergência e robustez.
6) Tradução do código do Filtro de Kalman para linguagem C.
Após atingido o sucesso na simulação com com obtenção de dados em tempo real, passou-se à tradução completa do código do MATLAB para a linguagem C utilizada pelo Arduino, a fim de poder-se testar a viabilidade de utilização do sistema de navegação desenvolvido de maneira totalmente embarcada, não dependendo mais de um computador para nenhum tipo de cálculo. O código completo em linguagem C encontra- se ao término deste relatório no anexo 2.
3. DEFINIÇÃO DO PROBLEMA
Com o intuito de modelar o problema de determinação de atitude a partir de medidas vetoriais obtidas pela câmera, considera-se um sistema de eixos cartesianos SB solidário ao corpo do quadricóptero, um sistema de eixos cartesianos SR de referencia, e outro sistema de eixos cartesianos em um referencial inercial (a Terra, por exemplo, assumindo que seu movimento é desprezível). Tais sistemas podem ser visualizados na figura 2. Tanto SB como SR possuem suas origens no centro geométrico do quadricóptero.
A câmera utilizada será a CMUCam, que já predispõe de um processamento de imagens e fornece alguns dados, como por exemplo o centroide de regiões de determinada cor. Juntamente com o sensor ultrassônico utilizado (Maxbotix LV-EZ0) obtêm-se as medidas vetoriais que fornecem a direção (do ponto de vista da câmera) de landmarks coloridas dispostas no chão. A câmera encontrara-se fixa ao corpo do quadricóptero, portanto as medidas vetoriais obtidas serão em relação ao sistema de eixos SB. As landmarks estão dispostas no chão de modo a sempre terem-se ao menos duas no campo de visão da câmera. Tais especificações podem ser visualizadas na figura 3.

O problema proposto consiste em estimar em tempo real a posição e atitude do quadricóptero como exposto acima. Para estimar-se a atitude, além das medidas vetoriais, utilizam-se também os valores de velocidade angular obtidos pelo girômetro. Além disso, para estimar-se a posição utilizam-se também acelerômetros.
Figura 2: Sistemas de eixos cartesianos utilizados para a determinação de atitude. SB, SR e SI correspondem respectivamente aos sistemas de eixos coordenados solidário ao corpo do VANT, de referência e inercial (i.e. no referencial da Terra).
Figura 3: Câmera fixa ao quadricóptero, e disposição das landmarks no chão de modo à sempre haver no mínimo duas no campo de visão da câmera a fim de se obter vetores unitários que apontam para as mesmas.

4. FORMULAÇÃO DO FILTRO
O método escolhido para resolver o problema definido na Seção 2 consiste num filtro de Kalman estendido para estimação de quatérnio a partir de medidas vetoriais. Para manter a norma do quatérnio estimado próxima de unitária, emprega-se simplesmente uma etapa de normalização Euclidiana das estimativas. Esse filtro será referido como QEKF (quaternion extended Kalman filter). Em seguida, as equações do QEKF são revisadas com base em (SANTOS, 2008).
4.1. Estimação de quatérnio
O vetor de estado do estimador de quatérnio de rotação, QEKF, é dado por , onde q1 e são as representações escalar e vetorial, respectivamente, dos componentes real e imaginário de q. Assim, tem-se que a equação de medidas não-linear discreta no tempo dos estimadores de quatérnio de rotação é:
( )
(1)
onde, ( ) [ ( ) ( ) ( ) ( ) ( ) ( ) ]
: medidas vetoriais feitas no sistema de eixos cartesianos do VANT;
: medidas vetoriais em relação ao sistema de referência;
: ruído de medição.
O modelo de cinemática de atitude para o quatérnio de rotação é dado pela seguinte equação diferencial linear:
̇( ) ( ) ( )
(2)
onde,
( ) [ ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ]
(3)

Em que ( ) ( ) ( ) correspondem respectivamente as velocidades angulares do VANT em relação aos eixos .
Integrando (2) de , obtém-se:
( )
(4)
Onde ( ) é a matriz de transição de estado. Considerando-se que a velocidade angular ( ) ( ) ( ) ( ) seja constante durante o intervalo de tempo entre amostragens sucessivas das medidas vetoriais, essa matriz é dada por:
( )
(5)
onde é computada por (3), mas utilizando em vez de ( ) Substituindo, então, ( ) ̂( ) ( ) na equação anterior:
( ) ( ̂ ) ̂
(6)
onde as matrizes ̂ são dadas por (3), porém substituindo ( ) ̂ , respectivamente. Representando o segundo fator do lado direito de (6) pela correspondente série de potências, obtém-se:
( ) ̂ ( )
(7)
Considerando-se que sejam ambos pequenos, o truncamento da série em (7) após termos de primeira ordem permite que (4) seja aproximada por:
̂ ̂
(8)
Pode-se verificar que:
(9)
onde,
[ ]
(10)
Sendo assim, utilizando (9) na manipulação do segundo termo do lado direito de (8), finalmente se obtém a seguinte equação de estado discreta no tempo:

̂ ̂
(11)
Onde é dado por (10), mas utilizando o quatérnio verdadeiro no instante , no lugar de . Para fins de implementação dos estimadores de quatérnio, a covariância do ruído de estado (segundo termo à direita em (11)) é aproximada por:
(12)
onde,
̂ ̂
(13)
e ̂ é computada por (10), porém utilizando ̂ no lugar de .
A exponencial matricial que define a matriz de transição de estado em (5) é aqui computada por:
(‖ ‖ ) ‖ ‖ (‖ ‖ )
(14)
O quatérnio de rotação apresenta norma unitária e, dessa forma, seus componentes obedecem à restrição . No entanto, a atualização linear do KF, por envolver a operação de soma, não garante que as estimativas de mantenham essa propriedade ao longo da estimação. A forma mais simples de lidar com tal restrição consiste no uso de normalização Euclidiana das estimativas após o estágio de atualização aditiva.
Em seguida é apresentado o estimador QEKF, o qual utiliza normalização Euclidiana para garantir que as estimativas tenham normas unitárias.
4.2. QEKF
Mediante uso do EKF (Extended Kalman Filter) e tendo-se em vista a equação de estado linear (11) e a equação de medidas não linear (1), obtêm-se os estágios de propagação e de atualização do QEKF. Para garantir que as normas de suas estimativas sejam unitárias, utiliza-se normalização Euclidiana.
A estimativa normalizada ̂ ‖ ̂ ‖, apresenta um erro cuja covariância é aproximada no QEKF simplesmente por:
(15)
O QEKF requer a matriz Jacobiana das medidas,

( ) | ̂
(16)
A Tabela 1 apresenta o QEKF.
Tabela 1: Estimador de quatérnio de rotação – QEKF (SANTOS, 2008 – pg. 31).
Em anexo, ao fim deste relatório encontram-se os códigos do Filtro descrito acima.
4.3. Obtenção dos vetores unitários
Após algumas soluções iniciais, em que seriam necessários um sensor a mais a fim de obter-se também uma medida de distância entre a câmera, e o plano em que se encontra a landmark, (neste caso utilizar-se-ia o sensor sonar: Parallax's PING), ou então obter-se a altitude do quadricóptero (para este caso, foi escolhido o sensor altímetro: Sparkfun’s BMP085), e após alguns experimentos com ambos os sensores para determinação da qualidade das medidas fornecidas por estes, abandonou-se ambas as abordagens iniciais, e optou-se por outra cuja necessidade de novo sensor tornou-se desnecessária.

Somente com a imagem fornecida pela CMUCam4, é possível determinar-se as coordenadas do vetor unitário que aponta para uma landmark no sistema de eixos do quadricóptero (que consiste no mesmo da câmera). Para tal, faz-se necessário o levantamento de um parâmetro da câmera, denotado aqui por K.
K representa a distância entre o plano da imagem vista pela câmera, e a câmera propriamente dita. Porém, o valor de K é dado pelo número de pixels que esta distancia ocuparia (no eixo x ou y) em uma imagem obtida pela câmera, caso esta medida fosse posta em um plano perpendicular ao eixo ótico da câmera afastado exatamente da mesma medida.
O valor obtido para K foi de aproximadamente 171, isto significa que para a câmera, em uma imagem visualizada a 1 metro de distância, um objeto com 171 pixels em linha reta (no eixo x ou y da imagem obtida) de dimensão, possui na realidade dimensão de 1 metro. Em uma imagem obtida pela câmera a uma distancia 2 metros de uma parede, 171 pixels desta imagem corresponderiam a um comprimento de 2 metros na parede. E assim por diante.
Figura 4: Significado do parâmetro K: Na imagem obtida, K pixels em linha reta correspondem a uma medida no anteparo equivalente à distância entre a câmera e o anteparo.
A imagem de trabalho da CMUCam4 consiste em uma matriz de pixels de 120x160, sendo estes numerados de 0 a 119 para o primeiro eixo, e de 0 a 159 para o segundo eixo da imagem. Sendo possível obter esta numeração para os pixels rastreados. Apesar de a CMUCam4 ser, em sua essência, um sensor de cor, por fornecer os valores de RGB de cada pixel de seu campo de visão, é possível utiliza-la para tirar fotos (porém de maneira muito ineficiente e demorada). Sendo assim, foi-se possível realizar experimentos de medições para determinar-se o valor de K com alta precisão.

Tendo este parâmetro determinado, por simplicidade supondo-se o centro da imagem vista pela câmera como sendo o pixel com coordenadas ( ), e tendo o eixo ótico da câmera perfeitamente alinhado com o eixo z do sistema de eixos , um vetor que aponta para uma landmark com coordenadas do centroide dadas por ( ) (na imagem fornecida pela CMUCam4, ou seja, x e y são a numeração dos pixels), e com origem na própria câmera, é dado por:
⃗ ( )
(17)
Foi-se utilizado o sinal de menos na coordenada z, pois a câmera estará acoplada à parte inferior do quadricóptero, tendo assim sem campo de visão direcionado à parte negativa do semi-eixo z do sistema de eixos . Portanto, sendo a norma do vetor dada por:
‖ ⃗‖ √
(18)
Tem-se então que o vetor unitário necessário ao algoritmo desenvolvido é dado por:
⃗⃗ ( ‖ ⃗‖) ⃗
(19)
5. SIMULAÇÕES COMPUTACIONAIS
Concentram-se aqui os resultados posteriores as fases de construção e estudos, já que estes são mais significativos como um todo para o trabalho.
Inicialmente, a fim de simular os dados de entrada provenientes dos sensores relativos à determinação de atitude, ou seja, girômetros e câmera, plotou-se arbitrariamente funções senoidais bem comportadas para as velocidades angulares (o motivo desta escolha deve-se puramente a tentativa de evitar-se que as velocidades angulares escolhidas impliquem em um movimento muito brusco e distante do que ocorre na realidade com o VANT, fornecendo assim dados mais fidedignos a posterior análise da simulação). A partir destas funções, colhendo-se seus valores em pontos igualmente espaçados de tempo, e adicionando-se um ruído aleatório normalmente distribuído, obtém-se um bom exemplo de valores de medidas provenientes dos giroscópios. Um exemplo deste procedimento pode ser verificado na figura 5.

Figura 5: Simulação de velocidades angulares medidas pelos girômetros. Respectivamente em azul, vermelho e verde tem-se: velocidade angular em relação ao eixo x, y e z. Onde x, y e z correspondem a um sistema de eixos ortogonais solidário ao quadricóptero.
A partir das funções que geraram as medidas de velocidades angulares, obteve- se qual seria o movimento descrito pelo VANT, ou seja, como os valores dos ângulos que descrevem sua atitude variam com o tempo. Para tal, utilizou-se o método de Runge-Kutta de 4ª ordem, o resultado pode ser visto na figura 6.
Figura 6: Atitude parametrizada pelos ângulos de Euler obtida a partir das velocidades angulares escolhidas. Respectivamente em verde, azul e vermelho tem-se 휑, 휃 e 휙.
Este último gráfico foi plotado apenas para fim de melhor visualização do movimento descrito pelo quadricóptero. O estudo apresentado aqui trabalha com a parametrização de atitude através de quatérnio de rotação. Sendo assim, segue o correspondente gráfico da variação dos parâmetros do real quatérnio de rotação que descreve o movimento:

Figura 7: Variação dos parâmetros do real quatérnio de rotação q=[q0 q1 q2 q3]T. Respectivamente em verde, azul, vermelho e preto tem-se q0, q1, q2 e q3.
A partir dos valores do real quatérnio de rotação correspondente ao movimento, determinou-se quais deveriam ser os reais vetores unitários que, a parte de um ruído gaussiano, correspondem as medidas vetoriais a serem obtidas pela câmera que representam as direções de landmarks postas no chão.
Para obterem-se tais medidas, rotacionou-se dois vetores conhecidos, supostos serem as medidas iniciais da câmera para as landmarks, utilizando-se os valores do quatérnio do gráfico anterior. Os vetores iniciais escolhidos foram r1=[0, 5/13, -12/13]T e r2=[0, -5/13, -12/13]T (representações em relação a um sistema de eixos fixo ao solo coincidente com o sistema de eixos solidário ao VANT na situação inicial). No intuito de fornecer uma visão mais limpa deste último resultado, na figura 8 é fornecida uma visualização das primeiras 30 medidas dos vetores, ainda não acrescidas do respectivo ruído.
Acrescentando-se o devido ruído a este ultimo resultado, obtemos assim dados coerentes que simulam bem velocidades angulares e medidas vetoriais obtidos respectivamente pelos girômetros e pela câmera a partir de um determinado movimento do quadricóptero. Estes dados podem agora ser utilizados como dados de entrada para a implementação do filtro de Kalman que irá estimar o quatérnio de rotação a partir destes dado.

Figura 8: Primeiras 30 medidas dos vetores unitários que apontam para as landmarks em relação ao sistema de eixos fixo ao quadricóptero. Nesta imagem, para melhor visualização, está suprimido o ruído acrescido aos vetores, o que corresponde de maneira mais fidedigna às medidas vetoriais obtida pela câmera.
Na figura 9 pode-se verificar a estimação obtida para o quatérnio de rotação correspondente ao caso real da figura 7. Percebem-se pouquíssimas diferenças. Na figura 10 é possível verificar-se em detalhe ambos os gráficos superpostos para outro caso. Nota-se que o filtro estima de maneira bastante razoável os parâmetros do quatérnio de rotação, mesmo depois de passado um grande período de tempo, e inúmeras medições já efetuadas.
Figura 9: Variação dos parâmetros do quatérnio de rotação estimado pelo filtro de Kalman qe=[q0 q1 q2 q3]T. Respectivamente em verde, azul, vermelho e preto tem-se q0, q1, q2 e q3.

Figura 10: Detalhe de comparação entre quatérnio real e quatérnio estimado pelo filtro de Kalman. Em linha continua encontram-se os valores reais, enquanto os pontos correspondem às estimativas obtidas pelo filtro.
Figura 11: Índice de ortogonalidade e erro angular em função do tempo. A escala vertical do primeiro gráfico está multiplicada pelo fator .

6. OBTENÇÃO DOS DADOS EM TEMPO REAL POR COMUNIÇÃO SERIAL
Concentram-se aqui os resultados posteriores à fase de formulação e simulação do filtro proposto.
Inicialmente, pretendia-se reescrever todo o código implementado no MATLAB em linguagem C (o que foi feito posteriormente), e verificar diretamente seu funcionamento final rodando o algoritmo de maneira totalmente embarcada no quadricóptero. Porém, em vista dos inúmeros problemas que poderiam surgir nesta grande passagem do projeto, optou-se por um passo intermediário, que consiste na simulação com obtenção dos dados em tempo real. A rotina implementada anteriormente foi adaptada para utilizar os dados provenientes dos sensores, através de comunicação serial entre o Arduino e o MATLAB, correspondendo assim ao real movimento destes, e não de uma simulação. Houve a necessidade de abordar-se o problema da determinação dos vetores unitários que apontam da câmera para a landmark, explicitada na seção 4.3.
Os demais valores que antes eram provenientes da simulação de uma dinâmica do quadricóptero, ou seja, as velocidades angulares, são diretamente lidas através do valor fornecido pelo girômetro, sendo apenas necessário fazer uma correção de unidade.
O giroscópio presente no sensor GY-521 possui 4 configurações possíveis de ranges para se trabalhar, sendo a escolhida para este trabalho a que apresenta maior precisão das medidas, porém menor range. Esta decisão foi tomada levando-se em consideração que os testes preliminares seriam executados com movimentos de pequenas e médias amplitudes, pois o campo de visão da câmera escolhida apresenta um campo de visão de cerca de 30º apenas. Além disto, verificou-se experimentalmente que com o menor range, a distribuição das medidas obtidas com o sensor aproxima-se melhor de uma gaussiana, o que melhoraria o desempenho do filtro, dado que esta é uma das premissas de sua formulação. Esta verificação está indicada na figura 12.
Figura 12: Dados de velocidade angular em relação ao mesmo eixo obtido com o giromêtro configurado para as duas opções extremas de range. Em (a) e (b), tem-se respectivamente as configurações que fornecem ranges com amplitudes de 2000º/s, e 250º/s. Os dados apresentados

na figura encontram-se em rad/s. O pico de (a) encontra-se com cerca de 1600 amostras, e o pico de (b) com cerca de 500 amostras.
Com isso, pode-se executar-se novamente a rotina do filtro, e plotar os valores das inovações de todas as componentes de ambos os vetores. Verificou-se que como esperado, estes gráficos possuem média nula, são descorrelacionados. Para cada um dos 6 gráficos, plotou-se também o sigma correspondente (raiz quadrada do respectivo elemento da diagonal principal da matriz de covariância da inovação).
Figura 13: Em azul, valores da inovação de cada uma das componentes de ambos os vetores obtidos. E verde, respectivo valor de sigma, proveniente da matriz de covariância da inovação. Sendo de cima para baixo, as coordenadas de x, y e z, sendo os 3 gráficos iniciais correspondentes aos valores do vetor B1 (no código em MATLAB) e os últimos 3, correspondentes aos valores do vetor B2.

7. CONCLUSÕES
O projeto obtido significativo progresso, obtendo bom resultados nas simulações, e nas primeiras fases de sua parte prática. Pode-se verificar até o momento que o filtro tem bom desempenho não só em simulações, mas também em na prática, em que o sistema de navegação apesar de ainda não ter sido testado em voo no VANT construído inicialmente, foi testado inúmeras vezes em uma plataforma similar, estimando a atitude da mesma de maneira acurada. Contudo, o teste em voo é sem dúvida uma fase crucial do projeto, e que se espera ser executado em breve a fim de alcançar-se o objetivo pré-estabelecido, isto é, desenvolver um sistema de navegação de baixo custo, porém de excelente qualidade. Ainda assim, a idealização do sistema, e a verificação de sua viabilidade, e performance já foram plenamente confirmadas neste projeto.
Como extensão ao trabalho aqui realizado, sugere-se as seguintes atividades:
 Alterar o firmware da CMUCam4 de modo a obter dados de maneira mais rápida, e assim não haver-se a necessidade de utilizar mais de uma câmera. No estágio atual em que se encontra o projeto, a alteração dos parâmetros de qual cor deve ser rastreada pela câmera faz com que sua taxa de amostragem caia de 30 fps para 4 fps, os motivos desta perda está relacionado com o modo de implementação das funções utilizadas no código deste sensor. A possibilidade de utilização de uma firmware costumizada já foi esclarecida e confirmada com o fabricante do mesmo;
.
 Realizar testes experimentais de bancada, tendo uma plataforma com determinação de atitude de modo independente (por exemplo: guimbals instrumentados com encoders), sendo assim, poder-se-ia comparar a atitude estimada pelo sistema de navegação desenvolvido com a fornecida pela plataforma.
8. AGRADECIMENTOS
Agradeço ao Prof. Davi Antônio dos Santos por ter cedido seu tempo e paciência ao fornecer auxílio no desenvolvimento deste projeto, e pelo seu sempre constante incentivo. Ao Instituto Tecnológico de Aeronáutica pelo ensino de qualidade, sem o qual este trabalho não teria sido possível, e pelas inúmeras outras oportunidades oferecidas. Agradeço ainda em especial ao CNPq que financiou este projeto através do programa PIBIC. Este incentivo é de suma importância ao desenvolvimento da pesquisa científica no Brasil.

9. BIBLIOGRAFIA
1. GELB, A. Applied Optimal Estimation. Boston: M.I.T. Press, 1974.
2. BROWN, R.G.; HWANG, P.Y.C. Introduction to Random Signals and Applied Kalman Filtering. New York: John Wiley & Sons, 1997.
3. ANDERSON, B.D.O.; MOORE, J.B. Optimal Filtering. New Jersey: Prentice- Hall, 1979.
4. SHUSTER, M. D. A Survey of Attitude Representations. The Journal of the Astronautical Sciences, Vol. 41, No. 4. October-December 1993, pp.439-517.
5. SANTOS, D. A. Estimação de Atitude e Velocidade Angular de Satélites Utilizando Medidas do Campo Geomagnético e da Direção do Sol. Dissertação de Mestrado. Instituto Tecnológico de Aeronáutica, São José dos Campos, 2008.
ANEXO 1:
CÓDIGOS DO FILTRO DE KALMAN PARA ESTIMAÇÃO DE QUATÉRNIO DE ROTAÇÃO (MATLAB)
A) TESTE_GERAL.m
clc;
clear;
% parâmetros de tempo
dt = 0.1; %intervalo de tempo entre medidas
Tmax=100; %intervalo total de tempo em que são feitas medidas
%% covariâncias usadas na simulação do sistema
sigma_w = 0.005; %incerteza na medição da velocidades angulares
sigma_b=0.01; %incerteza nas medidas das coordenadas dos vetores b1 e b2
Rb = sigma_b^2*eye(6); %covariância do ruído de medidas no sistema Sb
Rw = sigma_w^2*eye(3); %covariância do ruído de medidas dos girômetros
%% Covariância usadas no filtro
R = Rb; Q = 0.005^2*eye(3);
%% posições das landmarks
r1= [0; 5/13; -12/13]; %representações em Sr das observações vetoriais
r2= [0; -5/13; -12/13];
%% Condições iniciais do sistema
P(:,:,1)=0.01*eye(4); %covariância do erro de estimação no instante 0
q(:,1) = [1 0 0 0]';
%% Condições iniciais do filtro
qe(:,1) = [1 0 0 0]';
for k=1:Tmax/dt+1, %loop único para a simulação e o filtro de Kalman
[q(:,k+1),wm(:,k), b1k1, b2k1]=simulacao(q(:,k),r1,r2,Rb,Rw,dt,k);
[qe(:,k+1),P(:,:,k+1)] =
KalmanFilter(qe(:,k),P(:,:,k),wm(:,k),Q,R,r1,r2,b1k1,b2k1,dt);
% Calcula figuras de mérito
% Ik - erro angular; Jk - índice de ortogonalidade
[Ik(k+1),Jk(k+1)] = figuras_de_merito(q(:,k+1),qe(:,k+1));
end
figure; hold on;
plot(wm');title('Velocidade angular medida em rad/s');

figure; plot(qe','b'); plot(q','r');
figure; plot(Jk); title('Índice de Ortogonalidade');
figure; plot(Ik); title('Erro Angular em graus');
B) simulacao.m
function [qk1,wm, b1k1, b2k1] = simulacao(qk,r1,r2,Rb,Rw,dt,i)
%% simulação da velocidade angular.
wk = [0.1*sin(i*dt) 0.1*cos(i*dt) -0.1*sin(i*dt)*cos(i*dt)]';
%% propagação de atitude
[qk1,fi]=propaga_q(qk,wk,dt);
qk1 = (1/norm(qk1))*qk1;
% atitude representada em DCM
D = q2DCM(qk1);
%% simulação das medidas
wm = wk + sqrtm(Rw)*randn(3,1); % girômetros no instante k
b=[D*r1;D*r2]+sqrtm(Rb)*randn(6,1);% medidas vet. no instante k+1
b1k1 = b(1:3);
b2k1 = b(4:6);
C) KalmanFilter.m
function [qk1,Pk1] = KalmanFilter(qk,Pk,wk,Q,R,r1,r2,b1,b2,dt)
% Inputs:
% qk – estimativa do quaternio no instante k
% Pk - convariância do erro estimado correspondente à qk
% wk – medida de velocidade angular no instante k
% dt – tempo de amostra
% Q - covariância do ruído de processamento
% R - covariância do ruído das medidas
% r1, r2, b1, b2 – medidas vetoriais no instante k+1
% Outputs:
% qk1 - quaternio estimado no instante k+1
% Pk1 – covariância do erro de estimação correspondente à qk1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%1a - Propagação do estado estimado
[q_,fi] = propaga_q(qk,wk,dt);
Csi_k = [-qk(2) -qk(3) -qk(4);
qk(1) -qk(4) qk(3);
qk(4) qk(1) -qk(2);
-qk(3) qk(2) qk(1)];
Gama_k=dt/2*fi*Csi_k;
Q_q_k=Gama_k*Q*Gama_k';
P_=fi*Pk*fi'+Q_q_k;
%---------------------------------------------------------------------
%1b - Predição das medidas
D_q_k= q2DCM(q_);
b_estimado=[(D_q_k*r1)' (D_q_k*r2)']';
dDdq1 = [ 2*q_(1) 2*q_(4) -2*q_(3);
-2*q_(4) 2*q_(1) 2*q_(2);
2*q_(3) -2*q_(2) 2*q_(1)];
dDdq2 = [ 2*q_(2) 2*q_(3) 2*q_(4);
2*q_(3) -2*q_(2) 2*q_(1);
2*q_(4) -2*q_(1) -2*q_(2)];
dDdq3 = [-2*q_(3) 2*q_(2) -2*q_(1);
2*q_(2) 2*q_(3) 2*q_(4);
2*q_(1) 2*q_(4) -2*q_(3)];

dDdq4 = [-2*q_(4) 2*q_(1) 2*q_(2);
-2*q_(1) -2*q_(4) 2*q_(3);
2*q_(2) 2*q_(3) 2*q_(4)];
H_1_q = [dDdq1*r1 dDdq2*r1 dDdq3*r1 dDdq4*r1];
H_2_q = [dDdq1*r2 dDdq2*r2 dDdq3*r2 dDdq4*r2];
H_q = [H_1_q; H_2_q];
P_b = H_q*P_*H_q' + R;
%---------------------------------------------------------------------
%1c - Covariância cruzada
P_b_q = P_*H_q'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%2a - Ganho
K = P_b_q*inv(P_b);
%---------------------------------------------------------------------
%2b - Atualização da estimativa
qk1 = q_ + K*( [b1' b2']' - b_estimado );
Pk1 = P_ - K*P_b*K';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%3 - Normalização
qk1 = (1/norm(qk1))*qk1;
D) Figuras_de_merito.m
function [Ik,Jk]=figuras_de_merito(q,qe)
De = q2DCM(qe); % DCM estimada
D = q2DCM(q); % DCM verdadeira
Ik = 180/pi*abs(acos(0.5*(trace(De'*D) - 1))); %erro angular em graus
Jk = trace((De'*De-eye(3))*(De'*De-eye(3))');%Índice de ortogonalidade
E) propaga_q.m
function [q2,fi] = propaga_q(q1,w1,dt)
% cálculo da matriz de transição de estado
W = 0.5*[0 -w1(1) -w1(2) -w1(3);
w1(1) 0 w1(3) -w1(2);
w1(2) -w1(3) 0 w1(1);
w1(3) w1(2) -w1(1) 0];
n_w = norm(w1);
fi=cos(n_w*dt/2)*eye(4)+1/n_w*sin(n_w*dt/2)*W;
%matriz de transição de estado
q2 = fi*q1; % propagacão do quatérnion
F) q2DCM.m
function D = q2DCM(q)
D11 = 1-2*q(3)^2-2*q(4)^2;
D12 = 2*(q(2)*q(3)+q(1)*q(4));
D13 = 2*(q(2)*q(4)-q(1)*q(3));
D21 = 2*(q(2)*q(3)-q(1)*q(4));
D22 = 1-2*q(2)^2-2*q(4)^2;
D23 = 2*(q(4)*q(3)+q(1)*q(2));
D31 = 2*(q(2)*q(4)+q(1)*q(3));
D32 = 2*(q(4)*q(3)-q(1)*q(2));
D33 = 1-2*q(2)^2-2*q(3)^2;
D = [ D11 D12 D13;
D21 D22 D23;
D31 D32 D33];

ANEXO 2:
CÓDIGOS DO FILTRO DE KALMAN PARA ESTIMAÇÃO DE QUATÉRNIO DE ROTAÇÃO (C - ARDUINO)
A) arduino_matlab_serial.ino
#include <CMUcam4.h>
#include <CMUcom4.h>
#define R_MIN_1 235 //amarelo
#define R_MAX_1 255
#define G_MIN_1 235
#define G_MAX_1 255
#define B_MIN_1 160
#define B_MAX_1 190
#define R_MIN_2 235 //vermelho
#define R_MAX_2 255
#define G_MIN_2 160
#define G_MAX_2 210
#define B_MIN_2 140
#define B_MAX_2 200
#define LED_BLINK 5 // 5 Hz
#define WAIT_TIME 1000 // 5 seconds
#define FILTRO 12
#define K1 683
#define pi 3.1416
#define LED_PIN 13
//----------------------------------------------------------- #include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
//-----------------------------------------------------------
//########################################################### // Variáveis globais
bool blinkState = false;
CMUcam4 cam1(CMUCOM4_SERIAL1);
CMUcam4_tracking_data_t data1, data2;
MPU6050 accelgyro;
int16_t ax, ay, az; int16_t gx, gy, gz;
float a_x, a_y, a_z;
float g_x, g_y, g_z;
unsigned long T1, T2;
//###########################################################

//########################################################### void setup()
{
cam1.begin();
cam2.begin();
cam1.LEDOn(LED_BLINK);
delay(WAIT_TIME);
cam1.autoGainControl(false); cam2.autoGainControl(false);
cam1.autoWhiteBalance(false); cam2.autoWhiteBalance(false); cam1.LEDOn(CMUCAM4_LED_ON);
cam2.LEDOn(CMUCAM4_LED_ON);
//----------------------------------------------------------- Wire.begin();
accelgyro.initialize();
//-----------------------------------------------------------
Serial.begin(9600);
} //########################################################### void loop(){
cam1.trackColor(R_MIN_1, R_MAX_1, G_MIN_1, G_MAX_1, B_MIN_1, B_MAX_1);
cam2.trackColor(R_MIN_2, R_MAX_2, G_MIN_2, G_MAX_2, B_MIN_2, B_MAX_2);
delay(WAIT_TIME);
cam1.getTypeTDataPacket(&data1);
cam2.getTypeTDataPacket(&data2); Serial.println(data1.mx);
Serial.println(data1.my);
Serial.println(data2.mx);
for(;;){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //medidas feitas juntas
cam1.getTypeTDataPacket(&data1); cam2.getTypeTDataPacket(&data2);
g_x=gx*pi/(131*180)-0.0244; // conversão para
g_y=gy*pi/(131*180)-0.0155; // a unidade correta
g_z=gz*pi/(131*180)+0.0069; // e calibração
Serial.println(g_y,5);
Serial.println(g_x,5);
Serial.println(g_z,5);
}
}

B) KF_C.ino
#include <CMUcam4.h>
#include <CMUcom4.h>
#define R_MIN_1 75
#define R_MAX_1 255
#define G_MIN_1 0
#define G_MAX_1 125
#define B_MIN_1 0
#define B_MAX_1 100
#define R_MIN_2 75
#define R_MAX_2 255
#define G_MIN_2 0
#define G_MAX_2 125
#define B_MIN_2 0
#define B_MAX_2 100
#define LED_BLINK 5 // 5 Hz
#define WAIT_TIME 5000 // 5 seconds
#define FILTRO 12
#define K1 171
#define pi 3.1416
//----------------------------------------------------------- #include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
//###########################################################
// Variáveis globais
CMUcam4_tracking_data_t data1, data2;
MPU6050 accelgyro;
int16_t gx, gy, gz;
double n_w, c, s, **C, **S, **fi, **q, **wm, dt=0.03, **W,
**Csi_k, **Gama_k, **Q_q_k, **Q, **Pk, **D_q_k,
**b_e, **b1_e, **b2_e, **r1, **r2, **dDdq1,
**dDdq2, **dDdq3, **dDdq4, **H_1_q1, **H_1_q2,
**H_1_q3, **H_1_q4, **H_2_q1, **H_2_q2, **H_2_q3,
**H_2_q4, **H_q, **P_b, **R, **P_b_q, **K, **P_b_n_inv, **b;
//###########################################################
// Produto de Escalar por Matriz
double** PdeEeM(double e, double** A, int a, int b )
{
for(int i=0; i<a; i++)
for(int j=0; j<b; j++)
A[i][j]=e*A[i][j];
return A;
} //###########################################################

// Produto de matrizes
double** PdeM(double** A, double** B, int a, int b, int c )
{
double **P, aux;
P=(double **) malloc (a * sizeof(double*));
P[i]=(double *) malloc (c * sizeof(double));
for(int j=0; j<c; j++){
aux=0;
for(int k=0; k<b; k++)
aux=aux+A[i][k]*B[k][j];
P[i][j]=aux;
}
return P;
} //############################################################ // Transposta de Matriz
double** TdeM(double** A, int a, int b)
{
double **B;
B=(double **) malloc (b * sizeof(double*));
for(int i=0; i<b; i++)
B[i]=(double *) malloc (a * sizeof(double));
for(int i=0; i<b; i++)
for(int j=0; j<a; j++)
B[i][j]=A[j][i];
return B;
} //############################################################ // Soma de Matriz
double** SdeM(double** A, double** B, int a, int b)
{
double **C;
C=(double **) malloc (a * sizeof(double*));
C[i]=(double *) malloc (b * sizeof(double));
C[i][j]=B[i][j]+A[i][j];
return C;
} //############################################################ // Subtração de Matriz
double** SUdeM(double** A, double** B, int a, int b)
{
double **C;
C=(double **) malloc (a * sizeof(double*));
C[i]=(double *) malloc (b * sizeof(double));
C[i][j]=A[i][j]-B[i][j];
return C;
}

//########################################################### void propaga_q (){
W[0][0] = 0;
W[0][1] = -wm[0][0]/2;
W[0][2] = -wm[1][0]/2;
W[0][3] = -wm[2][0]/2;
W[1][0] = wm[0][0]/2;
W[1][1] = 0;
W[1][2] = wm[2][0]/2;
W[1][3] = -wm[1][0]/2;
W[2][0] = wm[1][0]/2;
W[2][1] = -wm[2][0]/2;
W[2][2] = 0;
W[2][3] = wm[0][0]/2;
W[3][0] = wm[2][0]/2;
W[3][1] = wm[1][0]/2;
W[3][2] = -wm[0][0]/2;
W[3][3] = 0;
n_w = sqrt(wm[0][0]*wm[0][0]
+wm[1][0]*wm[1][0]
+wm[2][0]*wm[2][0]);
c = cos(n_w*dt/2);
s = sin(n_w*dt/2)/n_w;
for(int i=0; i<4; i++)
for(int j=0; j<4; j++)
if(i==j){
S[i][j] = s;
C[i][j] = c;
}
else{
S[i][j] = 0;
C[i][j] = 0;
}
S = PdeM(S, W, 4, 4, 4);
fi = SdeM(C, S, 4, 4); //matriz de transição de estado
q = PdeM(fi, q, 4, 4, 1); // propagacão do quatérnion
} //########################################################### void FCsi_k(){
Csi_k[0][0] = -q[1][0];
Csi_k[0][1] = -q[2][0];
Csi_k[0][2] = -q[3][0];
Csi_k[1][0] = q[0][0];
Csi_k[1][1] = -q[3][0];
Csi_k[1][2] = q[2][0];
Csi_k[2][0] = q[3][0];
Csi_k[2][1] = q[0][0];
Csi_k[2][2] = -q[1][0];
Csi_k[3][0] = -q[2][0];
Csi_k[3][1] = q[1][0];
Csi_k[3][2] = q[0][0];
}

//########################################################## void q2DCM() {
D_q_k[0][0] = 1-2*(q[2][0]*q[2][0]-q[3][0]*q[3][0]); D_q_k[0][1] = 2*(q[1][0]*q[2][0]+q[0][0]*q[3][0]); D_q_k[0][2] = 2*(q[1][0]*q[3][0]-q[0][0]*q[2][0]); D_q_k[1][0] = 2*(q[1][0]*q[2][0]-q[0][0]*q[3][0]); D_q_k[1][1] = 1-2*(q[1][0]*q[1][0]-q[3][0]*q[3][0]); D_q_k[1][2] = 2*(q[3][0]*q[2][0]+q[0][0]*q[1][0]); D_q_k[2][0] = 2*(q[1][0]*q[3][0]+q[0][0]*q[2][0]); D_q_k[2][1] = 2*(q[3][0]*q[2][0]-q[0][0]*q[1][0]); D_q_k[2][2] = 1-2*(q[1][0]*q[1][0]-q[2][0]*q[2][0]);
} //########################################################## void dDdq() {
dDdq1[0][0] = 2*q[0][0];
dDdq1[0][1] = 2*q[3][0];
dDdq1[0][2] = -2*q[2][0];
dDdq1[1][0] = -2*q[3][0];
dDdq1[1][1] = 2*q[0][0];
dDdq1[1][2] = 2*q[1][0];
dDdq1[2][0] = 2*q[2][0];
dDdq1[2][1] = -2*q[1][0];
dDdq1[2][2] = 2*q[0][0];
dDdq2[0][0] = 2*q[1][0];
dDdq2[0][1] = 2*q[2][0];
dDdq2[0][2] = 2*q[3][0];
dDdq2[1][0] = 2*q[2][0];
dDdq2[1][1] = -2*q[1][0];
dDdq2[1][2] = 2*q[0][0];
dDdq2[2][0] = 2*q[3][0];
dDdq2[2][1] = -2*q[0][0];
dDdq2[2][2] = -2*q[1][0];
dDdq3[0][0] = -2*q[2][0];
dDdq3[0][1] = 2*q[1][0];
dDdq3[0][2] = -2*q[0][0];
dDdq3[1][0] = 2*q[1][0];
dDdq3[1][1] = 2*q[2][0];
dDdq3[1][2] = 2*q[3][0];
dDdq3[2][0] = 2*q[0][0];
dDdq3[2][1] = 2*q[3][0];
dDdq3[2][2] = -2*q[2][0];
dDdq4[0][0] = -2*q[3][0];
dDdq4[0][1] = 2*q[0][0];
dDdq4[0][2] = 2*q[1][0];
dDdq4[1][0] = -2*q[0][0];
dDdq4[1][1] = -2*q[3][0];
dDdq4[1][2] = 2*q[2][0];
dDdq4[2][0] = 2*q[1][0];
dDdq4[2][1] = 2*q[2][0];
dDdq4[2][2] = 2*q[3][0];
}

//########################################################## void FH_q() {
H_1_q1 = PdeM(dDdq1, r1, 3, 3, 1);
H_1_q2 = PdeM(dDdq2, r1, 3, 3, 1);
H_1_q3 = PdeM(dDdq3, r1, 3, 3, 1);
H_1_q4 = PdeM(dDdq4, r1, 3, 3, 1);
H_2_q1 = PdeM(dDdq1, r2, 3, 3, 1);
H_2_q2 = PdeM(dDdq2, r2, 3, 3, 1);
H_2_q3 = PdeM(dDdq3, r2, 3, 3, 1);
H_2_q4 = PdeM(dDdq4, r2, 3, 3, 1);
H_q[0][0] = H_1_q1[0][0];
H_q[0][1] = H_1_q2[0][0];
H_q[0][2] = H_1_q3[0][0];
H_q[0][3] = H_1_q4[0][0];
H_q[1][0] = H_1_q1[1][0];
H_q[1][1] = H_1_q2[1][0];
H_q[1][2] = H_1_q3[1][0];
H_q[1][3] = H_1_q4[1][0];
H_q[2][0] = H_1_q1[2][0];
H_q[2][1] = H_1_q2[2][0];
H_q[2][2] = H_1_q3[2][0];
H_q[2][3] = H_1_q4[2][0];
H_q[3][0] = H_2_q1[0][0];
H_q[3][1] = H_2_q2[0][0];
H_q[3][2] = H_2_q3[0][0];
H_q[3][3] = H_2_q4[0][0];
H_q[4][0] = H_2_q1[1][0];
H_q[4][1] = H_2_q2[1][0];
H_q[4][2] = H_2_q3[1][0];
H_q[4][3] = H_2_q4[1][0];
H_q[5][0] = H_2_q1[2][0];
H_q[5][1] = H_2_q2[2][0];
H_q[5][2] = H_2_q3[2][0];
H_q[5][3] = H_2_q4[2][0];
}
//########################################################## //Inversão de Matriz utilizando o método de Gauss-Jordam
int inv(double **A, int n) {
// A = matriz de entrada e também de saida
int pivrow;
int k,i,j;
int* pivrows;
pivrows=(int *) malloc (n * sizeof(int));
float tmp;
for (k = 0; k < n; k++) {
tmp = 0;
for (i = k; i < n; i++)
if (abs(A[i][k]) >= tmp) {
tmp = abs(A[i][k]);
pivrow = i;
}
if (A[pivrow][k] == 0.0f)
return 0; //Inversão falha pois é matriz
//singular

if (pivrow != k) {
for (j = 0; j < n; j++) {
tmp = A[k][j];
A[k][j] = A[pivrow][j];
A[pivrow][j] = tmp;
}
}
pivrows[k] = pivrow;
tmp = 1.0f/A[k][k];
A[k][k] = 1.0f;
for (j = 0; j < n; j++)
A[k][j] = A[k][j]*tmp;
for (i = 0; i < n; i++) {
if (i != k) {
tmp = A[i][k];
A[i][k] = 0.0f;
for (j = 0; j < n; j++) {
A[i][j] = A[i][j] - A[k][j]*tmp;
}
}
}
}
for (k = n-1; k >= 0; k--) {
if (pivrows[k] != k) {
for (i = 0; i < n; i++) {
tmp = A[i][k];
A[i][k] = A[i][pivrows[k]];
A[i][pivrows[k]] = tmp;
}
}
}
return 1;
}
//##########################################################
void setup() {
cam1.begin();
cam2.begin();
delay(WAIT_TIME);
cam1.autoGainControl(false); cam2.autoGainControl(false);
cam1.autoWhiteBalance(false); cam2.autoWhiteBalance(false);
//---------------------------------------------------------- Wire.begin();
accelgyro.initialize();
//----------------------------------------------------------

C = (double **) malloc (4 * sizeof(double*));
C[i]=(double *) malloc (4 * sizeof(double));
S = (double **) malloc (4 * sizeof(double*));
S[i]=(double *) malloc (4 * sizeof(double));
fi = (double **) malloc (4 * sizeof(double*));
fi[i]=(double *) malloc (4 * sizeof(double));
q=(double **) malloc (4 * sizeof(double*));
q[i]=(double *) malloc (sizeof(double));
wm=(double **) malloc (3 * sizeof(double*));
wm[i]=(double *) malloc (sizeof(double));
W=(double **) malloc (4 * sizeof(double*));
W[i]=(double *) malloc (4 * sizeof(double));
Csi_k=(double **) malloc (4 * sizeof(double*));
Csi_k[i]=(double *) malloc (3 * sizeof(double));
Gama_k=(double **) malloc (4 * sizeof(double*)); for(int i=0; i<4; i++)
Gama_k[i]=(double *) malloc (3 * sizeof(double));
Q_q_k=(double **) malloc (4 * sizeof(double*));
Q_q_k[i]=(double *) malloc (4 * sizeof(double));
Q=(double **) malloc (3 * sizeof(double*));
Q[i]=(double *) malloc (3 * sizeof(double));
Pk=(double **) malloc (4 * sizeof(double*));
Pk[i]=(double *) malloc (4 * sizeof(double));
D_q_k=(double **) malloc (3 * sizeof(double*));
D_q_k[i]=(double *) malloc (3 * sizeof(double));
b_e=(double **) malloc (6 * sizeof(double*));
b_e[i]=(double *) malloc (6 * sizeof(double));

b1_e=(double **) malloc (3 * sizeof(double*));
b1_e[i]=(double *) malloc (3 * sizeof(double));
b2_e=(double **) malloc (3 * sizeof(double*));
b2_e[i]=(double *) malloc (3 * sizeof(double));
r1=(double **) malloc (3 * sizeof(double*));
r1[i]=(double *) malloc (3 * sizeof(double));
r2=(double **) malloc (3 * sizeof(double*));
r2[i]=(double *) malloc (3 * sizeof(double));
dDdq1=(double **) malloc (3 * sizeof(double*)); for(int i=0; i<3; i++)
dDdq1[i]=(double *) malloc (3 * sizeof(double));
H_1_q1=(double **) malloc (3 * sizeof(double*)); for(int i=0; i<3; i++)
H_1_q1[i]=(double *) malloc (sizeof(double));

H_2_q2=(double **) malloc (3 * sizeof(double*));
H_q=(double **) malloc (6 * sizeof(double*));
H_q[i]=(double *) malloc (4 * sizeof(double));
P_b=(double **) malloc (6 * sizeof(double*));
P_b[i]=(double *) malloc (6 * sizeof(double));
R=(double **) malloc (6 * sizeof(double*));
R[i]=(double *) malloc (6 * sizeof(double));
P_b_q=(double **) malloc (4 * sizeof(double*)); for(int i=0; i<4; i++)
P_b_q[i]=(double *) malloc (6 * sizeof(double));
K=(double **) malloc (4 * sizeof(double*));
K[i]=(double *) malloc (6 * sizeof(double));
P_b_n_inv=(double **) malloc (6 * sizeof(double*)); for(int i=0; i<6; i++)
P_b_n_inv[i]=(double *)malloc(6*sizeof(double));
b=(double **) malloc (6 * sizeof(double*));
b[i]=(double *) malloc (sizeof(double));
if(i==j)
Q[i][j] = 0.000025;
else
Q[i][j] = 0;
if(i==j) Pk[i][j] = 0.01;
else Pk[i][j] = 0;

if(i==j)
R[i][j] = 0.0001;
else
R[i][j] = 0;
q[0][0]=1;
q[1][0]=0;
q[2][0]=0;
q[3][0]=0;
}
//########################################################## void loop() {
cam1.trackColor(R_MIN_1, R_MAX_1, G_MIN_1, G_MAX_1,
B_MIN_1, B_MAX_1);
cam2.trackColor(R_MIN_2, R_MAX_2, G_MIN_2, G_MAX_2,
B_MIN_2, B_MAX_2);
delay(WAIT_TIME);
cam1.getTypeTDataPacket(&data1); cam2.getTypeTDataPacket(&data2);
r1[0][0] = sin(atan((data1.mx-80)/K1));
r1[1][0] = sin(atan((data1.my-60)/K1));
r1[2][0] = sqrt(1-r1[0][0]*r1[0][0]-r1[1][0]*r1[1][0]); r2[0][0] = sin(atan((data2.mx-80)/K1));
r2[1][0] = sin(atan((data2.my-60)/K1));
r2[2][0] = sqrt(1-r2[0][0]*r2[0][0]-r2[1][0]*r2[1][0]);
for(;;) {
accelgyro.getRotation(&gx, &gy, &gz);
//medidas feitas juntas
wm[0][0]=gx*pi/(16.4*180);
wm[1][0]=gy*pi/(16.4*180);
wm[2][0]=gz*pi/(16.4*180);
//---------------------------------------------------------- //1a - Propagação do estado estimado
FCsi_k();
propaga_q();
Gama_k = PdeEeM((dt/2),PdeM(fi,Csi_k,4,4,3),4,3);
Q_q_k = PdeM(Gama_k,PdeM(Q,TdeM(Gama_k,4,3),
3,3,4),4,3,4);
Pk = SdeM(PdeM(PdeM(fi,Pk,4,4,4),
TdeM(fi,4,4),4,4,4),Q_q_k,4,4);
//---------------------------------------------------------- //1b - Predição das medidas
q2DCM();
b1_e = PdeM(D_q_k, r1, 3, 3, 1);
b2_e = PdeM(D_q_k, r2, 3, 3, 1);

b_e[0][0] = b1_e[0][0];
b_e[1][0] = b1_e[1][0];
b_e[2][0] = b1_e[2][0];
b_e[3][0] = b2_e[0][0];
b_e[4][0] = b2_e[1][0];
b_e[5][0] = b2_e[2][0];
dDdq();
FH_q();
P_b = SdeM(PdeM(PdeM(H_q,Pk,6,4,4),
TdeM(H_q,6,4),6,4,6),R,6,6);
//---------------------------------------------------------- //1c - Covariância cruzada
P_b_q = PdeM(Pk, TdeM(H_q, 6, 4), 4, 4, 6);
//----------------------------------------------------------
//2a - Ganho
for(int k=0; k<6; k++)
P_b_n_inv[i][k]=P_b[i][k];
inv(P_b, 6); //retorna 0 em falha, e 1 em sucesso
K = PdeM(P_b_q, P_b, 4, 6, 6);
//---------------------------------------------------------- //2b - Atualização da estimativa
b[0][0] = sin(atan((data1.mx-80)/K1));
b[1][0] = sin(atan((data1.my-60)/K1));
b[2][0] = sqrt(1-b[0][0]*b[0][0]-b[1][0]*b[1][0]);
b[3][0] = sin(atan((data2.mx-80)/K1));
b[4][0] = sin(atan((data2.my-60)/K1));
b[5][0] = sqrt(1-b[3][0]*b[3][0]-b[4][0]*b[4][0]);
q = SdeM(q,PdeM(K,SUdeM(b,b_e,6,1),4,6,1),4,1);
Pk = SUdeM(Pk,PdeM(PdeM(K,P_b_n_inv,4,6,6),
TdeM(K,4,6),4,6,4),4,4);
//##########################################################
//3 - Normalização
q = PdeEeM((1/sqrt( q[0][0]*q[0][0]
+q[1][0]*q[1][0]
+q[2][0]*q[2][0]
+q[3][0]*q[3][0])),q,4,1);
}
}

Sistema de Navegação Indoor por Imagens para VANT Quadricóptero

Recomendados

Recomendados

Mais conteúdo relacionado

Semelhante a Sistema de Navegação Indoor por Imagens para VANT Quadricóptero

Semelhante a Sistema de Navegação Indoor por Imagens para VANT Quadricóptero (20)

Sistema de Navegação Indoor por Imagens para VANT Quadricóptero