DOUGLAS DANIEL SAMPAIO SANTANA

ESTIMAO DE TRAJETRIAS TERRESTRES UTILIZANDO

UNIDADE DE MEDIO INERCIAL DE BAIXO CUSTO E
FUSO SENSORIAL






Dissertao apresentada Escola
Politcnica da Universidade de So
Paulo para obteno do Ttulo de
Mestre em Engenharia











So Paulo
2005

DOUGLAS DANIEL SAMPAIO SANTANA









ESTIMAO DE TRAJETRIAS TERRESTRES UTILIZANDO
UNIDADE DE MEDIO INERCIAL DE BAIXO CUSTO E
FUSO SENSORIAL






Dissertao apresentada Escola
Politcnica da Universidade de So
Paulo para obteno do Ttulo de
Mestre em Engenharia


rea de Concentrao:
Engenharia Mecatrnica e de Sistemas
Mecnicos Grande rea Mecnica


Orientador:
Prof. Doutor
Celso Massatoshi Furukawa







So Paulo
2005


Este exemplar foi revisado e alterado em relao verso original, sob
responsabilidade nica do autor e com a anuncia de seu orientador.

So Paulo, 22 de novembro de 2005

Assinatura do autor :

Assinatura do orientador :



































FICHA CATALOGRFICA

Santana, Douglas Daniel Sampaio

Estimao de trajetrias terrestres utilizando unidade de
medio inercial de baixo custo e fuso sensorial / D.D.S.
Santana. So Paulo, 2005.

138p.

Dissertao (Mestrado) - Escola Politcnica da Universidade
de So Paulo. Departamento de Engenharia Mecatrnica e de
Sistemas Mecnicos.

1.Navegao inercial 2.Filtros de Kalman I.Universidade de
So Paulo. Escola Politcnica. Departamento de Engenharia
Mecatrnica e de Sistemas Mecnicos II.t.

DEDICATRIA


























A minha me Rosa de Almeida Camargo
pelo exemplo de coragem, dedicao e
perseverana.


AGRADECIMENTOS



Ao meu orientador Prof. Dr. Celso Massatoshi Furukawa pelo firme apoio e pelas
muitas vezes em que me reconduziu ao caminho correto da pesquisa de forma que
este trabalho pudesse ser concludo.

Ao Prof. Dr. Newton Maruyama e ao Prof. Dr. Julio Csar Adamowski do
Departamento de Engenharia Mecatrnica e de Sistemas Mecnicos da EPUSP pelo
apoio e co-orientao no decorrer do trabalho, ao Prof. Dr. Edvaldo Simes da
Fonseca Junior do departamento de Engenharia de Transportes da EPUSP pela
significativa contribuio prestada no campo da cartografia, aos amigos Paulo de
Tarso Peres e Victor A. F. de Campos pelas muitas vezes em que me ajudaram nos
ensaios de campo e laboratrio, ao amigo Nilson Noris Franceschetti pelas sugestes
apresentadas, a todos os demais colegas de pesquisa do departamento sem citar
nomes para no incorrer num gravssimo erro de omisso.

Ao amigo Ulisses Loureiro de Lima pelas significativas contribuies a mim
prestadas durante todos esses anos.

A Companhia do Metropolitano de So Paulo (METR) na pessoa do Engenheiro
Francisco Jos Valentim pela contribuio prestada na obteno e interpretao da
planta cartogrfica da linha trs do Metr (linha verde).

Ao RENAPIGI (Rede Nacional de PIGs Instrumentados) da FINEP/CTPETRO,
FINEP, ANP e a CAPES pelo apoio financeiro recebido, e ao CENPES-
PETROBRAS pelo apoio para a realizao de experimentos.



i
RESUMO


Este trabalho apresenta um estudo e implementao de um sistema de navegao
inercial, utilizado para estimar coordenadas cartogrficas e reconstruir a trajetria
percorrida por um veculo terrestre (no caso, um automvel). O sistema emprega
uma tcnica denominada fuso sensorial. A fuso sensorial obtida a partir de um
filtro de Kalman discreto que combina os dados provenientes de uma unidade de
medio inercial (UMI) do tipo strapdown, de baixo custo, com informaes externas
fornecidas por um velocmetro e um conjunto de marcas topogrficas (landmarks).
Estas medidas so ento processadas pelo filtro, produzindo estimativas de
acelerao, velocidade e posio. So apresentadas as modelagens terrestre e
cinemtica do veculo, e tambm as principais fontes de erros que degradam o
processo de estimao, incluindo algumas tcnicas para limitar o crescimento da
covarincia destes erros, reduzindo dessa forma a incerteza associada a trajetria
estimada. Finalmente, o sistema modelado de forma estocstica no espao de
estados discreto, e resultados grficos so apresentados. Estes resultados indicam
que no possvel reconstruir as trajetrias de forma satisfatria utilizando-se apenas
a UMI, devido a sua baixa preciso. No entanto, a fuso sensorial desenvolvida
neste trabalho permite reconstruir as trajetrias com expressiva melhora.

ii
ABSTRACT


This work presents a study and implementation of an inertial navigation system,
designed to estimate chart coordinates and to reconstruct a terrain trajectory of a
vehicle (in this case, an automobile). The system uses a technique named sensor
fusion. The sensor fusion is implemented by a discrete Kalman Filter, that processes
the data coming from a low cost strapdown inertial measurement unit (IMU), and
them combines external information provided by the vehicle speedometer and a set
of landmarks. These measurements are processed by the Kalman Filter, producing
acceleration, speed and position estimates. It is presented the terrestrial and
kinematical vehicle modeling, and the main sources of errors that degrades the
estimation process, including some techniques to limit the growth of the covariance
of those errors, reducing in this way the uncertainty related to the estimated
trajectory. Finally, the system is modeled as a discrete state space stochastic system,
and graphic results are presented. These results indicate that it is impossible to
reconstruct the trajectories successfully using only the IMU, due to its low accuracy.
However, the sensor fusion algorithm developed in this work succeeds to reconstruct
the trajectories with a notable improvement.
iii
SUMRIO


RESUMO i
ABSTRACT ii
SUMRIO iii
LISTA DE FIGURAS viii
LISTA DE TABELAS xi
LISTA DE ABREVIATURAS E SIGLAS xii
LISTA DE SMBOLOS xiii

1. INTRODUO 1
1.1. Descrio do problema 1
1.2. Navegao inercial 2
1.3. Objetivos 4
1.4. Reviso bibliogrfica 5

2. FUNDAMENTOS TERICOS E MATEMTICOS 8
2.1. Sensores inerciais 8
2.1.1. Acelermetros 8
2.1.2. Giroscpios 8
2.1.3. Configurao strapdown e plataforma 9
2.1.4. Rudos e erros de sensores 10
2.1.5. Classificao de uma UMI 11
2.2. Sistemas de coordenadas e transformaes 12
2.2.1. Referenciais utilizados em navegao inercial 12
iv
2.2.2. Transformao de coordenadas 16
2.2.2.1. Transformaes de coordenadas entre o sistema RPY e o
sistema NED por ngulos de Euler 17
2.2.2.2. Transformaes de coordenadas entre o sistema RPY e o
sistema NED por Co-senos Diretores 19
2.2.2.3. Transformaes de coordenadas entre o sistema NED e o
sistema ECEF 21
2.2.2.4. Transformao de coordenadas entre o sistema ECEF e o
sistema ECI 21
2.3. Modelagem terrestre da navegao inercial strapdown 22
2.3.1. Conceituando a acelerao denominada fora especfica 22
2.3.2. Modelagem da navegao num sistema de coordenadas fixo 23
2.3.3. Modelagem da navegao num sistema de coordenadas girante 24
2.3.4. Modelagem da navegao no sistema de coordenadas inercial
ECI 25
2.3.5. Modelagem da navegao no sistema de coordenadas terrestre
ECEF 28
2.3.6. Modelagem da navegao no sistema de coordenadas da
navegao local NED 29
2.3.7. Representao da navegao local NED atravs das
coordenadas curvilneas ou geodsicas 31
2.3.8. O formato da Terra segundo a conveno WGS-84 34
2.3.9. Deflexo gravitacional sobre a superfcie da Terra 36
2.3.10. Mecanizao do sistema de navegao local NED 37

3. FORMULAO DO PROBLEMA 39
3.1. Representao no espao de estados 39
v
3.1.1. Estimao de estados 40
3.2. O filtro de Kalman discreto 42
3.2.1. O algoritmo do filtro de Kalman discreto 43
3.3. Modelo proposto para estimar estados de giroscpio 44
3.3.1. Modelo dinmico dos giroscpios 42
3.3.2. Filtro de Kalman utilizado para estimar estados de giroscpio 46
3.4. Ortogonalizao e normalizao da matriz de orientao 47
3.4.1. Ortogonalizao 48
3.4.2. Normalizao 49
3.5. Correes da acelerao 50
3.5.1. Correes determinsticas 50
3.5.1.1. Gravidade local 50
3.5.1.2. Acelerao de Coriolis 51
3.5.1.3. Acelerao centrpeta 52
3.5.2. Correes estocsticas 52
3.5.2.1. Influncia dos erros de giroscpios na acelerao 52
3.6. Modelo proposto para estimar estados da plataforma 55
3.6.1. Modelo dinmico da plataforma 55
3.6.2. Modelo dinmico alternativo 58
3.6.3. Filtro de Kalman utilizado para estimar estados da plataforma 59
3.7. Alinhamento inicial e calibrao da plataforma 62
3.7.1. Alinhamento inicial 62
3.7.2. Alinhamento terrestre pelo mtodo da matriz de orientao 62
3.7.3. Calibrao 64

vi

4. MATERIAIS E MTODOS 66
4.1. Testes efetuados 66
4.2. Equipamentos utilizados 67
4.3. Metodologia dos testes 68
4.3.1. Monitoramento do filtro de Kalman 72
4.3.2. Sintonia do filtro de Kalman 72
4.3.3. Insero de rudos de medio 74
4.4. Constantes utilizadas 75

5. RESULTADOS E DISCUSSES 76
5.1. Velocidades angulares, aceleraes e velocidades lineares 76
5.1.1. Sistema de coordenadas da plataforma RPY 77
5.1.2. Sistema de coordenadas da navegao NED 80
5.2. Reconstruo da trajetria para o teste em percurso fechado 82
5.2.1. Reconstruo por dupla integrao e por filtro de Kalman sem a
utilizao de medidas de referncia 83
5.2.2. Reconstruo por dupla integrao e por filtro de Kalman com a
utilizao de medidas de referncia 85
5.2.3. Desempenho dos reconstrutores na presena de rudos de
medio 86
5.3. Anlise do efeito da deriva dos giroscpios 88
5.4. Utilizao de marcas topogrficas para auxiliar o filtro de Kalman 90
5.5. Monitoramento do filtro de Kalman 95

6. CONCLUSES E RECOMENDAES 97
vii

ANEXOS 100
A. O filtro de Kalman discreto 100
B. Transformaes de coordenadas 112
C. Scripts Matlab 123

REFERNCIAS BIBLIOGRFICAS 132

BIBLIOGRAFIA 137

viii
LISTA DE FIGURAS



Figura 1.1 Sistema de navegao inercial isolado de rotao 2
Figura 2.1 Configurao de uma UMI strapdown 9
Figura 2.2 Sistema de coordenadas roll-pitch-yaw (RPY) 14
Figura 2.3 Sistemas de coordenadas: inercial, terrestre, local e da
plataforma 15
Figura 2.4 Sistemas de coordenadas: terrestre, local e geodsico 15
Figura 2.5 Foras atuando sobre um acelermetro 22
Figura 2.6 Sistemas de coordenadas fixo e mvel 23
Figura 2.7 Acelerao centrpeta e gravidade local 27
Figura 2.8 Modelo terrestre elptico adotado pelo comit WGS84 34
Figura 2.9 Representao da deflexo gravitacional latitudinal 37
Figura 2.10 Modelo do sistema de navegao 38
Figura 3.1 Estimador de estados 41
Figura 3.2 Estimador de estados estocstico 41
Figura 3.3 Ciclo do filtro de Kalman 44
Figura 3.4 Filtro de Kalman para estimar ngulos de rotao 47
Figura 3.5 Influencia da deriva do giroscpio z nos acelermetros x e y 54
Figura 3.6 Filtro de Kalman para estimar estados da plataforma 61
Figura 4.1 UMI strapdown Crossbow modelo VG700AA-202 67
Figura 4.2 Plataforma utilizada nos ensaios automotivos 69
Figura 4.3 Algoritmo do duplo integrador 71
Figura 4.4 Histrico da freqncia de amostragem 71
ix
Figura 5.1 Trajetria para o teste em percurso fechado 76
Figura 5.2 Velocidade angular no eixo x (roll) 77
Figura 5.3 Velocidade angular no eixo y (pitch) 77
Figura 5.4 Velocidade angular no eixo z (yaw) 78
Figura 5.5 Acelerao no eixo x - RPY 79
Figura 5.6 Acelerao no eixo y - RPY 79
Figura 5.7 Acelerao no eixo z - RPY 79
Figura 5.8 Velocidade no eixo x - RPY 80
Figura 5.9 Acelerao no eixo x - NED 80
Figura 5.10 Acelerao no eixo y - NED 81
Figura 5.11 Acelerao no eixo z - NED 81
Figura 5.12 Velocidade no eixo x - NED 81
Figura 5.13 Velocidade no eixo y - NED 82
Figura 5.14 Velocidade no eixo z - NED 82
Figura 5.15 Velocidades x e y obtidas por integrao 84
Figura 5.16 Trajetrias reconstrudas por dupla integrao e por filtro de
Kalman sem utilizar medidas de referncia 84
Figura 5.17 Comparao entre os reconstrutores 85
Figura 5.18 Trajetrias reconstrudas por dupla integrao e por filtro de
Kalman com medidas de referncia da velocidade 86
Figura 5.19 Comparao entre os reconstrutores e deslocamento no eixo z 86
Figura 5.20 Comparao entre os reconstrutores na presena de rudo de
medio - Primeira simulao 87
Figura 5.21 Comparao entre os reconstrutores na presena de rudo de
medio - Segunda simulao 87
x

Figura 5.22 Comparao entre os reconstrutores na presena de rudo de
medio - Terceira simulao 88
Figura 5.23 Comparao entre os reconstrutores na presena de rudo de
medio - Quarta simulao 88
Figura 5.24 Trajetrias reconstrudas em percurso aberto 89
Figura 5.25 Incremento angular devido ao bias de giroscpio 89
Figura 5.26 Teste de insero de landmarks para = 1m 91
Figura 5.27 Teste de insero de landmarks para = 2m 92
Figura 5.28 Teste de insero de landmarks para = 3m 93
Figura 5.29 Teste de insero de landmarks nas curvas 94
Figura 5.30 Monitoramento do filtro de Kalman em percurso fechado
utilizando a velocidade como medida de referncia 95
Figura 5.31 Monitoramento do filtro de Kalman em percurso fechado
utilizando a velocidade + landmarks como medida de
referncia 96

xi
LISTA DE TABELAS




Tabela 2.1 Classificao de uma UMI 12
Tabela 2.2 Parmetros do modelo terrestre elptico (WGS84) 34
Tabela 4.1 Especificaes da UMI Crossbow VG700AA-202 68
Tabela 4.2 Constantes utilizadas nos testes de reconstruo de trajetrias 75


xii
LISTA DE ABREVIATURAS E SIGLAS


EPUSP Escola Politcnica da Universidade de So Paulo
UMI Unidade de Medio Inercial
ECI Earth - Centered - Inertial
ECEF Earth - Centered, Earth - Fixed
LPT Local Tangent Plane
NED North - East - Down
ENU East - North - Up
RPY Roll - Pitch - Yaw
MCD Matriz de Co-senos Diretores
MO Matriz de Orientao
MTC Matriz de Transformao de Coordenadas
MR Matriz de Rotao
MT Marcas Topogrficas
FK Filtro de Kalman
WGS World Geodetic System
GPS Global Position System
MEMS Micro Eletro-Mechanical System
FOG Fiber Optic Gyro
DSP Digital Signal Processing

xiii
LISTA DE SMBOLOS


f Vetor fora especfica
a Vetor acelerao inercial
g
Vetor acelerao gravitacional
p
x
Vetor representado no sistema de coordenadas da plataforma (RPY)
n
x
Vetor representado no sistema de coordenadas da navegao (NED)
e
x
Vetor representado no sistema de coordenadas terrestre (ECEF)
i
x
Vetor representado no sistema de coordenadas inercial (ECI)
y
x
C
Matriz de rotao que transporta um vetor na base x para a base y

ngulo de rotao positiva sobre o eixo Z ou ngulo de yaw
ngulo de rotao sobre o eixo Y ou ngulo de pitch
ngulo de rotao sobre o eixo X ou ngulo de roll
Velocidade angular
r Vetor posio
Velocidade de rotao da Terra
P Vetor fora peso
F Vetor de fora
m Massa de um corpo
V Velocidade de um corpo
e
v Vetor de velocidade da plataforma representado no sistema terrestre
i
v Vetor de velocidade da plataforma representado no sistema inercial
xiv
e
e
v
Velocidade terrestre projetada no sistema ECEF
i
e
v
Velocidade terrestre projetada no sistema ECI
ie
Vetor de rotao da Terra projetado no sistema inercial
en
Vetor de velocidade angular do sistema NED em relao ao sistema
ECEF
p
ep

Vetor de velocidade angular da plataforma em relao ao sistema
terrestre, projetado no sistema da plataforma
l
g Vetor gravidade local
x
f
Vetor fora especfica representado no sistema de coordenadas x
L Latitude
Longitude
h Altitude

Deflexo magntica longitudinal
Deflexo magntica latitudinal
1
( ),
k
t
+
x x Vetor de estados contnuo e discreto
( ),
k
t z z Vetor de medidas contnuo e discreto
( ),
k
t u u Vetor de entradas associado ao processo contnuo e discreto
( ),
k
t w w Vetor de rudos associado ao processo contnuo e discreto
( ),
k
t e e Vetor de rudos associado medio contnuo e discreto
( ),
k
t v v Vetor de rudos associado medio contnuo e discreto
, A Matrizes contnua e discreta que modelam a transio de estados
associada ao processo
, B Matrizes contnua e discreta que modelam as entradas associadas ao
processo
xv
C Matriz que modela os rudos associados ao processo
H Matriz que modela os estados associados medio
G Matriz que modela os rudos associados medio
Q Covarincia do rudo de processo
R Covarincia do rudo de medio
( ) p x Probabilidade de x
2
( , ) N Distribuio normal com mdia e varincia
2

x Estimativa do vetor x

x
Estimativa a priori do vetor x

k
+
x
Estimativa a posteriori do vetor x
P Matriz de covarincia do erro (filtro de Kalman)
K Ganho do filtro de Kalman
xy
Erro de ortogonalidade entre os vetores x e y
xx
Erro de normalizao do vetor x
i
C i-sima linha ou coluna da matriz de orientao C
, t T Perodo de amostragem
( ) i Notao para designar o produto escalar
( ) Notao para designar o produto vetorial
0
, R R Raio da Terra
b Sinal de bias
t tempo
v Rudo aleatrio, Random Walk
g
b Bias de giroscpio
xvi
, ,
x y z
c c c Coordenadas x, y e z
a Erro de acelerao
Desvio padro
2

Varincia
2
,
c c

Desvio padro e varincia da estimativa de posio









1
1. INTRODUO


1.1. Descrio do problema Contextualizao

Admita-se um sistema mvel ou veculo no qual se deseja determinar a todo instante
os valores da acelerao, velocidade, posio e orientao deste sistema sobre a
superfcie do globo terrestre, e que neste sistema os valores iniciais das grandezas
que se quer determinar so conhecidos. Admita ainda que o sistema est em
constante movimento, sendo que os movimentos podem se dar em qualquer direo e
sentido.

Dispondo-se no veculo de acelermetros, velocmetro e odmetro, torna-se possvel
determinar a acelerao, a velocidade e o deslocamento, entretanto no possvel
reconstruir a trajetria percorrida, pois no se dispe de qualquer informao sobre a
direo e sentido do movimento. Para contornar este problema, deve-se utilizar um
sensor que indique a todo instante a orientao do movimento.

Se a configurao acima puder ser implementada, tem-se ento uma soluo para
reconstruo de trajetrias. Sistemas capazes de reconstruir (estimar) trajetrias
possuem muitas aplicaes, dentre as quais destacam-se: a navegao autnoma de
veculos terrestres, aquticos ou areos, equipamentos para inspeo e mapeamento
de dutos, equipamentos que determinam coordenadas cartogrficas, etc.

Uma tcnica muito utilizada porm nada trivial, de se obter um sistema capaz de
estimar trajetrias emprega sensores inerciais em conjunto com um algoritmo
denominado estimador de estados. Estes sistemas so denominados sistemas de
navegao inercial.




2
1.2. Navegao inercial

Navegao inercial o processo pelo qual se estabelecem informaes sobre a
posio, velocidade, atitude
1
e direo de um veculo com relao a um referencial,
utilizando informaes fornecidas por sensores inerciais tais como acelermetros e
giroscpios (Titterton, 1997). Os sistemas de navegao inercial se baseiam na
segunda lei da mecnica clssica de Newton. Medindo-se as aceleraes de um
corpo, torna-se possvel calcular as mudanas de velocidade e posio atravs de
sucessivas integraes matemticas.

Basicamente existem dois tipos de sistemas de navegao inercial: os sistemas
isolados de rotao ou gimbaled systems
2
ilustrado na figura 1.1, e os sistemas
no isolados de rotao ou strapdown systems. Neste trabalho ser empregado o
segundo sistema, e o mesmo ser visto em detalhes no prximo captulo.


Fig. 1.1 Sistema de navegao inercial isolado de rotao gimbaled system

1
Atitude ou orientao: posio de um corpo que determinada pela inclinao dos seus eixos com
relao a algum sistema de referncia.

2
Gimbaled Systems o nome dado aos sistemas de navegao inercial com trs graus de liberdade.
Neste sistema, os sensores inerciais esto isolados do movimento de rotao atravs de anis rotativos
denominados gimbals.

3
Um sistema de navegao inercial geralmente possui trs acelermetros montados
sobre eixos ortogonais. Movimentos rotacionais podem ser obtidos a partir de
giroscpios, que medem a velocidade de rotao em torno de um eixo. A maior
contribuio de um giroscpio determinar a orientao dos acelermetros a todo
instante (Lawrence, 1998). Sistemas inerciais compostos por acelermetros e
giroscpios alinhados aos trs eixos cartesianos so tambm conhecidos por centrais
inerciais.

A construo de um sistema de navegao inercial preciso uma tarefa bastante
complexa e conseqentemente possui custo elevado. A complexidade justificada
basicamente por dois motivos (Sukkarieh, 2000): o alinhamento perfeito entre
acelermetros e giroscpios uma tarefa bastante difcil, e os sinais disponibilizados
por estes sensores sempre estaro contaminados por algum tipo de rudo e deriva
3
.

Os sistemas de navegao inercial so divididos basicamente em trs categorias: alto,
mdio e baixo desempenho.

Os sistemas de alto desempenho apresentam pequenos nveis de rudo e deriva, custo
elevado (acima de US$100.000,00) e so utilizados principalmente nas aplicaes de
uso militar e na aviao comercial (Shin, 2001). J os equipamentos de baixo e
mdio desempenho (conhecidos como sistemas de baixo custo) possuem maiores
nveis de rudos e deriva, sendo que suas principais aplicaes se do no campo da
navegao de veculos autnomos (terrestres e aquticos), na robtica, entre outros.
O grande inconveniente dos sistemas de baixo custo (que, em particular, utilizado
neste trabalho) a grande quantidade de rudo e deriva presente nas medies, o que
torna invivel sua utilizao isolada em aplicaes confiveis de navegao.


3
Taxa na qual o erro em um sensor ou sistema acumula-se com o passar do tempo.




4
Neste sentido, grandes esforos tm sido feitos para desenvolver novas tcnicas que
aprimorem os sistemas de baixo custo. O desafio de se desenvolver novas tcnicas
possui seu foco principal na modelagem do sistema, pois os rudos e derivas
presentes nos sensores inerciais so de natureza aleatria.

Devido ao comportamento dinmico e caracterstica aleatria dos rudos e deriva
presentes nos sistemas de navegao inercial, a modelagem matemtica dos mesmos
desenvolvida geralmente por meio de processos estocsticos. Um estimador
estocstico de estados denominado Filtro de Kalman empregado para compor
uma fuso sensorial e obter melhores estimativas de estados. A formulao do filtro
de Kalman pode ser encontrada em detalhes em (Kalman, 1960), (Jazwinski, 1970),
(Maybeck, 1976), (Gelb, 1986) e (Brown, 1997). Na seo Anexos apresenta-se a
formulao completa do filtro seguida de um exemplo de aplicao.


1.3. Objetivos

Este trabalho tem por objetivo estudar, implementar e avaliar um sistema para
estimar a reconstruo de trajetrias utilizando uma central inercial de baixo custo do
tipo strapdown. Utilizando apenas este equipamento, a preciso da reconstruo
extremamente baixa, e por este motivo empregam-se tcnicas de fuso sensorial.

A fuso dos dados feita por um filtro de Kalman discreto, cuja funo processar e
combinar os dados para obter as estimativas de estados de acelerao, velocidade e
posio do veculo.

Inicialmente, investiga-se a adio de medidas de velocidade no processo de
reconstruo.

Em seguida, emprega-se um conjunto de marcas topogrficas (landmarks)
conjuntamente com a velocidade. Em algumas aplicaes importantes de navegao,
landmarks so empregados para restringir o erro de estimao. Obviamente,
5
quanto maior o nmero e mais prximos estes forem, menor ser o erro. No entanto,
h um custo operacional associado instalao e manuteno dos landmarks.
Neste trabalho investigam-se alguns critrios para estabelecer a distribuio de
landmarks, levando-se em conta a preciso desejada para a reconstruo.

Como a reconstruo da trajetria depende do referencial adotado, os sistemas de
coordenadas utilizados na navegao inercial e suas relaes de transformao so
apresentados, assim como os procedimentos necessrios para corrigir os efeitos da
gravidade e o movimento de rotao da Terra, que afetam a leitura dos acelermetros
e giroscpios.

Para modelar o processo corretamente, so desenvolvidas as equaes matemticas
relevantes ao problema da estimao inercial de trajetria. As fontes significativas de
erros associadas instrumentao empregada tambm so modeladas, incluindo-se
algumas tcnicas que visam minimizar o efeito causado por estes erros.

A reconstruo da trajetria se d a partir de experimentos realizados nas
proximidades da EPUSP, com uma central inercial montada no interior de um
veculo. Os experimentos realizados se limitaram a trajetrias admitidas
aproximadamente planas. Estas trajetrias so estimadas tridimensionalmente,
porm a anlise dos resultados se concentra na reconstruo no plano xy.

O referencial adotado para reconstruo da trajetria o sistema de navegao local
(NED) que ser visto adiante.


1.4. Reviso bibliogrfica

Um estudo introdutrio, com relativo rigor matemtico, sobre os aspectos que
envolvem a navegao inercial, pode ser obtido em (Kelly, 1996) e (Stovall, 1997).
Os trabalhos de (Grewal, 2001) e (Rogers, 2000) apresentam a navegao inercial
modelada em variveis de estado e posteriormente submetida a um filtro de Kalman
6
para estimar os estados. O filtro de Kalman extensamente empregado em sistemas
de navegao inercial, como se pode observar em vrios trabalhos abordados nesta
reviso bibliogrfica.

Modelos de navegao inercial strapdown aplicados a veculos terrestres, so
obtidos em (Dorobantu, 1999), (Sukkarieh, 2000) e (Shin, 2001). Nesses trabalhos
so abordadas tcnicas de navegao inercial para veculos terrestres auxiliada por
GPS (Global Position System). O trabalho de Savage (1998) apresenta tcnicas
mais refinadas sobre a modelagem dinmica da navegao terrestre.

Uma abordagem navegao inercial de preciso (indicada para estudos avanados)
pode ser obtida em (Chatfield, 1997). Esta obra explora uma enorme quantidade de
conceitos e fontes de erros que afetam a navegao inercial, mas no aborda
explicitamente o problema da navegao strapdown.

Tcnicas de calibrao de sensores inerciais so encontradas em (Nebot, 1999). Esse
trabalho apresenta um algoritmo de calibrao auxiliado por GPS, para sistemas de
baixo custo utilizados em aplicaes terrestres. Em (Grewal, 1991) descrito um
algoritmo que utiliza o filtro de Kalman para calibrar centrais inerciais embarcadas
em msseis. Tcnicas de como se efetuar o alinhamento terrestre esttico e o
alinhamento terrestre dinmico so descritas em (Titterton, 1997) e (Shin, 2001)
respectivamente.

Conforme ser visto adiante, os sistemas de navegao inercial strapdown de baixo
custo no so suficientemente confiveis na estimativa de trajetrias. Portanto, torna-
se necessrio combinar informaes provenientes de outros sensores a fim de
aumentar o desempenho do sistema. Neste sentido os trabalhos de (Shin, 2001),
(Maybeck, 1976) e (Santana, 2004) apresentam modelos que utilizam fuso sensorial
em sistemas de medio compostos por centrais inerciais apoiadas por,
respectivamente, leituras de GPS (Global Positioning System), sensores Doppler, e
velocmetro.

7
No campo da robtica assistida por sensores inerciais, podem-se consultar os
trabalhos de (Dudek, 2000), (Walchko, 2002a e 2002b) e (Cleyner, 2002). Em
(Grenon, 2000) encontra-se uma modelagem strapdown para veculo autnomo
subaqutico auxiliado por GPS, sensores de velocidade Doppler, sensores de
profundidade e bssola eletrnica.

Os trabalhos de (Gustafsson, 2001) e (Campos, 2004a, 2004b e 2004c), apresentam
tcnicas de estimativa de trajetrias atravs de algoritmos denominados filtros de
partculas. Embora os filtros de partculas no sejam objetos deste estudo, o modelo
matemtico apresentado nesses trabalhos mostrou-se bastante adequado ao problema
da navegao terrestre e foi adaptado para este trabalho.

Informaes sobre tecnologias de sensores inerciais e suas aplicaes, so
apresentadas por (Lawrence, 1998). Tcnicas para melhorar o desempenho dos
sensores inerciais so encontradas em (Crossbow, 2002).

A modelagem terrestre e os principais sistemas de coordenadas (referenciais)
empregados na navegao strapadown podem ser obtidas com bastante
profundidade em (Titterton, 1997) e (Rogers, 2000), sendo que em (Craig, 1989) e
(Santos, 2001) o leitor encontrar subsdios introdutrios.

Por fim menciona-se o tratado World Geodetic System de 1984 (WGS-84, 1984),
que define a forma geomtrica terrestre e tambm indica as correes que devem ser
procedidas ao se implementar modelos de navegao inercial.


8
2. FUNDAMENTOS TERICOS E MATEMTICOS

Este captulo apresenta os fundamentos tericos e matemticos relacionados
navegao inercial strapdown. Investigam-se os sensores inerciais, os principais
referenciais utilizados na navegao terrestre e suas relaes de transformao, e por
fim, a modelagem terrestre da navegao sobre os referenciais adotados.

2.1. Sensores inerciais

2.1.1. Acelermetros

Acelermetros so sensores utilizados para medir a acelerao linear em sistemas
inerciais. O sinal de sada de um acelermetro fornece uma medida denominada
fora especifica, ou seja, um acelermetro mede a acelerao do corpo juntamente
com a acelerao gravitacional, a qual deve ser compensada. Em formulao vetorial
tem-se
f = a - g ,
(2.1)

onde f o vetor fora especifica, a vetor da acelerao com relao a um
referencial inercial e g o vetor acelerao gravitacional.

Para exemplificar, admita que um corpo possui um acelermetro fixado
paralelamente ao seu eixo vertical (eixo da ao gravitacional), orientado para cima,
e que o mesmo encontra-se em repouso e perfeitamente nivelado sobre a superfcie
terrestre. Nestas condies, a leitura do acelermetro ser -g e, portanto dever ser
compensada. O conceito de fora especifica ser visto em detalhes adiante.

2.1.2. Giroscpios

Os giroscpios utilizados em sistemas strapdown, so sensores capazes de medir a
rotao de um corpo com relao a um sistema inercial. A sada de um giroscpio


9
fornece a taxa angular ou velocidade angular e portanto deve ser integrada para que
se possa obter o ngulo de rotao.

2.1.3. Configurao strapdown e plataforma

Strapdown o nome dado para um sistema de medio inercial integrado onde
trs acelermetros e trs giroscpios so montados sobre eixos ortogonais
coincidentes e fixados sobre uma mesma base rgida (Titterton, 1997). Neste sistema
as origens dos eixos dos acelermetros e dos giroscpios devem ser coincidentes e
seus eixos devem ser paralelos, para que se possa realizar transformaes de
coordenadas.

Quando uma trade de sensores inerciais (acelermetros e giroscpios) montada
sobre uma base rgida e embarcada juntamente com um sistema de controle
eletrnico dedicado, tem-se ento uma estrutura denominada unidade de medio
inercial strapdown ou simplesmente UMI. Fixando-se uma UMI num dado corpo
ou veculo, tem-se uma estrutura denominada Plataforma. Esta estrutura passa a
formar o sistema de coordenadas do veculo tambm denominado como sistema de
coordenadas da plataforma.



Fig. 2.1 Configurao de uma UMI strapdown


10
2.1.4. Rudos e erros de sensores

As principais fontes de erros que corrompem a preciso de um sistema de navegao
inercial so: imperfeio dos sensores e erros devido a distrbios aleatrios. Os erros
devido imperfeio dos sensores podem ser identificados com elevada preciso por
um processo denominado calibrao e so classificados como erros determinsticos,
ou seja, podem ser expressos por uma funo matemtica bem determinada. J as
fontes de erros de natureza aleatria no possuem uma funo matemtica bem
determinada, podendo apenas ser descritos atravs da teoria de processos
estocsticos
4
(Papoulis, 2001). A seguir so indicadas as fontes de erros mais
significativas que degradam o processo de navegao inercial strapdown.

Bias (ou polarizao) um erro de bias ou erro de polarizao, pode ser
interpretado como sendo um nvel de sinal constante ou que varia muito lentamente,
independentemente do sinal de entrada. Mesmo sendo constante, um sinal de bias
pode mudar de valor em algumas condies, por exemplo, ao se religar o
equipamento.

Fator de escala: trata-se de um erro que proporcional ao sinal de entrada, ou seja,
o erro de fator de escala comporta-se como um erro de inclinao do coeficiente
angular da equao de uma reta. Embora sejam modelados como lineares, erros deste
tipo geralmente exibem algum grau de no linearidade.

Erro de quantizao: os sinais de sada dos acelermetros e giroscpios presentes
numa UMI so obtidos em instantes discretos de tempo. Isto produz um rudo branco
na sada que proporcional magnitude da quantizao, ou seja, depende da ordem
de grandeza com que um sinal eltrico aproximado para valores diferentes daquele
cuja amostra foi obtida.


4
Processo estocstico o nome dado a um processo onde as variveis so aleatrias (incertas) durante
o intervalo de tempo em que so medidas ou observadas.


11
Drift (ou deriva trmica): os sensores inerciais so afetados pela temperatura e
devem ser compensados eletronicamente. Como esta compensao nunca perfeita,
a deriva trmica deve ser modelada como um processo aleatrio.

Desalinhamento: refere-se ao desalinhamento mecnico entre eixos. Idealmente os
giroscpios e os acelermetros definem uma base ortogonal idealizada como
estrutura da plataforma. Como impossvel se obter um perfeito alinhamento
mecnico da plataforma na prtica, descrevem-se ento os erros de alinhamento de
cada sensor com relao aos eixos da plataforma como constantes aleatrias.


2.1.5. Classificao de uma UMI

A qualidade de uma UMI est diretamente relacionada qualidade de seus sensores,
pois os erros e rudos dos sensores afetam crucialmente o desempenho da navegao
inercial. UMIs strapdown comerciais so divididas basicamente em trs
categorias: alta qualidade, mdia qualidade e baixa qualidade.

Alta qualidade refere-se a sistemas capazes de navegar e sentir as mudanas de
orientaes com excelente preciso por longo tempo de durao (tipicamente horas)
somente com a UMI.

Mdia qualidade requer o auxilio de sensores externos para manter a capacidade
oferecida pelos sistemas de alta qualidade. Sistemas de mdia qualidade conseguem
operar durante curtos perodos de durao (tipicamente minutos) somente com a
UMI.

Baixa qualidade requer sensores externos para proporcionar performance til e so
capazes de operar somente num curtssimo intervalo de tempo (tipicamente
segundos) utilizando apenas a UMI.



12
A tabela 2.1, extrada de Brown (1997), mostra como so classificadas as UMIs do
tipo strapdown segundo alguns parmetros de sensores. Dentre estes parmetros
esto o bias apresentado na seo anterior e o random walk que ser visto
adiante.

Tabela 2.1 Classificao de uma UMI
Parmetro do
Sensor
Alta
Qualidade
Mdia
Qualidade
Baixa
Qualidade
Bias mximo
de Giroscpio
< 0,01
o
/hr 0,1-1,0
o
/hr 10
o
/hr
Random Walk
de giroscpio
4
3.10

/h 0,01 /h > 0,01 /h

Bias mximo
de
acelermetro
1

10-50 g 200-500 g > 1000 g
Random Walk
de
acelermetro
1

3-10 g/ Hz 50 g/ Hz > 50 g/ Hz

1
g representa a acelerao gravitacional terrestre, e g equivale a 10
-6
g


2.2. Sistemas de coordenadas e transformaes

2.2.1. Referenciais utilizados em navegao inercial

Os sistemas de coordenadas so utilizados para referenciar ou localizar
geograficamente um determinado corpo ou veculo. Portanto para se definir a
posio de um objeto, torna-se necessrio especificar o sistema de coordenadas no
qual o objeto est representado. Segundo (Grewal, 2001) os sistemas de coordenadas
utilizados em navegao inercial so constitudos basicamente por sistemas de
coordenadas cartesianas e sistemas de coordenadas esfricas, sendo que os mais
utilizados so:



13
Sistema ECI (Earth-Centered-Inertial) sua origem est no centro de massa da
Terra, sendo que seu eixo x aponta para o Sol, passando pela linha do equador, seu
eixo z passa pelo plo norte e seu eixo y orientado seguindo a regra da mo
direita. O sistema ECI dito inercial por ser considerado fixo em relao a um corpo
estelar distante.

Sistema ECEF (Earth-Centered, Earth-Fixed) sua origem est no centro de
massa da Terra, sendo que seu eixo x passa pelo cruzamento entre a linha do
equador e o meridiano de Greenwich, seu eixo z passa pelo plo norte e seu eixo y
orientado seguindo a regra da mo direita.

Sistema LPT (Local Tangent Plane) so designados como sistemas de
coordenadas locais, onde a Terra representada como uma superfcie plana no ponto
onde a navegao est sendo analisada. Os sistemas LPT so compostos pelos
sistemas de coordenadas NED e ENU, descritos a seguir.

Sistema NED (North-East-Down) conhecido como sistema de navegao
local, sua origem pode ser estabelecida em qualquer ponto do globo terrestre, sendo
que seu eixo x aponta para o norte geogrfico, seu eixo z aponta para o centro da
Terra e seu eixo y referenciado de acordo com regra da mo direita. Este sistema
de coordenadas amplamente utilizado para navegao terrestre.

Sistema ENU (East-North-Up) tambm um sistema de navegao local, sua
origem pode ser estabelecida em qualquer ponto do globo terrestre, sendo que seu
eixo x aponta para o norte geogrfico, seu eixo z normal superfcie da Terra e
aponta para cima e seu eixo y referenciado de acordo com regra da mo direita.
Este sistema de coordenadas amplamente utilizado para navegao area.



14
Sistema RPY (Roll-Pitch-Yaw) conhecido como sistema de navegao da
plataforma, um sistema cujos eixos so fixos num veculo, constituindo assim uma
estrutura denominada estrutura mvel. O eixo x (eixo de roll) do sistema RPY
deve apontar para a direo frontal do movimento do veculo, o eixo y (eixo de
pitch) deve ser ortogonal a x e apontar para a direita enquanto que o eixo z (eixo
de yaw) aponta para baixo (regra da mo direita). A figura 2.2 mostra como so
representados os eixos onde ocorrem os movimentos de rotao denominados: roll-
pitch-yaw (RPY) que podem ser traduzidos como rolamento arfagem guinada.

Sistema Curvilneo ou Geodsico trata-se de um sistema de coordenadas
esfricas, onde a localizao de um corpo representada por um deslocamento
angular anti-horrio cuja origem o meridiano de Greenwhich denominado
longitude, seguido por um movimento angular tambm anti-horrio cuja origem a
linha do equador denominado latitude e uma cota de elevao referenciada ao nvel
do mar denominada altitude.




Fig. 2.2 Sistema de coordenadas roll-pitch-yaw (RPY)






15
Rotao terrestre
Meridiano de
Greenwich
Eixo inercial
Eixo terrestre
Plano equatorial
Plo
Plano meridiano
local
Eixo de navegao
tangencial local
Xi
Yi
Zi
Xe
Ye
Ze
N
E
D
O
Roll
Pitch
Yaw


Fig. 2.3 Sistemas de coordenadas: inercial, terrestre, local e da plataforma


Rotao terrestre
Meridiano de
Greenwich
Plano equatorial
Plo
Plano meridiano
local
Eixo de navegao
tangencial local
Xe
Ye
Ze
N
E
D
O

Latitude Local
Longitude Local

L


Fig. 2.4 Sistemas de coordenadas: terrestre, local e geodsico

A figura 2.3 ilustra a relao entre os quatro sistemas de coordenadas. No globo
terrestre esto os sistemas ECI, cujos eixos so designados pelo prefixo i, o sistema
ECEF, com eixos designados pelo prefixo e e o sistema NED ou sistema de
navegao local, onde os eixos so designados pelas letras n, e e d.
O sistema RPY est fixo ao corpo do veculo (plataforma) que por sua vez se
movimenta com o sistema de navegao local. A figura 2.4 mostra a relao entre os
sistemas de navegao local, terrestre e geodsico.


16
2.2.2. Transformao de coordenadas

As transformaes de coordenadas so utilizadas para converter um vetor
representado num determinado sistema de coordenadas para um outro sistema de
coordenadas conveniente
5
. Para exemplificar, considere um vetor de aceleraes
tridimensional no sistema de navegao local NED que representado por


| | a a a
T
n
N E D
a =
.

(2.2)

A matriz de transformao de coordenadas que leva o vetor de acelerao do sistema
de navegao local NED para o sistema de navegao da plataforma RPY definida
como


RPY p
NED n
= C C , (2.3)

assim o novo vetor de aceleraes no sistema RPY dado por


p p n
n
= a C a . (2.4)

As transformaes de coordenadas podem ser obtidas por matriz de Co-senos
Diretores (MCD), ngulos de Euler ou Quaternions. Dependendo da aplicao,
um tipo de transformao pode ser mais adequado que o outro, como no caso dos
quaternions que evitam o problema da singularidade na matriz de transformao de
coordenadas (MTC
6
) tambm conhecido como Gimbal Lock
7
. Como neste
trabalho no existe a possibilidade de ocorrer a singularidade da MTC devido s

5
Nas equaes a seguir, os ndices i, e, n e p so utilizados para indicar os referenciais: inercial (ECI),
terrestre (ECEF), da navegao local (NED) e da plataforma (RPY) respectivamente.

6
Neste texto as notaes Matriz de Transformao de Coordenadas, Matriz de Rotao e Matriz
de Orientao so sinnimas e sero utilizadas convenientemente.

7
Gimbal Lock a impossibilidade de se obter a matriz de transformao de coordenadas, devido a
uma ou mais matrizes de rotao estarem prximas da singularidade, ou seja, quando os movimentos
angulares esto prximos 90
o
os co-senos das matrizes tornam-se nulos.



17
condies dinmicas nas quais o sistema ser ensaiado, optou-se pela utilizao das
matrizes de transformao de coordenadas por ngulos de Euler e por co-senos
diretores. A seguir so apresentadas as matrizes de transformao entre os vrios
sistemas de coordenadas, onde se optou pela apresentao direta das mesmas. Na
seo Anexos estas transformaes so apresentadas com mais detalhes.


2.2.2.1. Transformaes de coordenadas entre o sistema RPY e o sistema NED
por ngulos de Euler

A seguir so mostradas as matrizes ngulos de Euler que levam um vetor
representado no sistema de coordenadas da plataforma RPY para o sistema de
coordenadas de navegao local NED (equao 2.5), do sistema NED para RPY
(equao 2.6). Os ngulos , e so definidos como rotaes em torno,
respectivamente, dos eixos x, y e z no sistema RPY (veja a figura B.1 do anexo B)

cos sin 0 cos 0 sin 1 0 0
sin cos 0 . 0 1 0 . 0 cos sin
0 0 1 sin 0 cos 0 sin cos
n
p



( ( (
( ( (
=
( ( (
( ( (

C
cos cos cos sin sin sin cos sin sin cos sin cos
cos sin cos cos sin sin sin sin cos cos sin sin
sin sin cos cos cos
n
p



+ + (
(
= + +
(
(

C
(2.5)

1 0 0 cos 0 sin cos sin 0
0 cos sin . 0 1 0 . sin cos 0
0 sin cos sin 0 cos 0 0 1
p
n



( ( (
( ( (
=
( ( (
( ( (

C
cos cos cos sin sin
cos sin sin sin cos cos cos sin sin sin sin cos
sin sin cos sin cos sin cos cos sin sin cos cos
p
n



(
(
= + +
(
( + +

C
(2.6)

Utilizando-se sensores angulares (giroscpios) com freqncias de amostragem
elevadas, o perodo de amostragem muito pequeno e T tende a zero. Admitindo-
se ainda que a leitura dos giroscpios no se alteram significativamente entre duas


18
leituras consecutivas, tem-se como conseqncia disso que pequenos ngulos de
rotao sero obtidos ao se integrar as velocidades angulares entre duas leituras
consecutivas.

Para pequenos ngulos de rotao as equaes 2.5 e 2.6 podem ser linearizadas e,
portanto podem-se admitir as seguintes aproximaes: sen , sen ,
sen e os co-senos destes ngulos se tornam unitrios. Neste caso particular,
as novas matrizes de transformaes de coordenadas so aproximadas por

1
1
1
n
p



(
(
=
(
(

C ,
(2.7)
e
1
1
1
p
n



(
(
=
(
(

C ,
(2.8)

onde: o ngulo de rotao positiva sobre o eixo z (ngulo de yaw), o
ngulo de rotao positiva sobre o novo eixo y (ngulo de pitch) e

o ngulo de
rotao positiva sobre o eixo x resultante (ngulo de roll).

Propagao dos ngulos de Euler no tempo

Os ngulos de Euler , e so obtidos pelo conjunto de trs rotaes em relao a
uma plataforma estvel num dado sistema de navegao inercial (Titterton, 1997).
Conseqentemente , e

so as trs componentes das velocidades angulares no
mesmo referencial. As velocidades angulares ,
x y z
e do sistema de coordenadas
da plataforma RPY podem ser relacionadas com , e

do sistema de coordenadas
de navegao NED, por




19
1 0 0 0 1 0 0 cos 0 sin 0
0 0 cos sin . 0 cos sin . 0 1 0 . 0
0 0 sin cos 0 0 sin cos sin 0 cos
x
y
z

= + +

| | | | ( | | ( ( | |
| | | | ( ( (
| | | | ( ( (
| | | |
( ( (
\ . \ . \ . \ .

. (2.9)

Arranjando a equao acima e expressando-a na forma das velocidades angulares do
sistema NED obtm-se


( sen cos ) tan
cos sen
( sen cos ) sec
y z x
y z
y z



= + +
=
= +

. (2.10)

Estas equaes podem ser utilizadas num sistema strapdown para atualizar os
ngulos de Euler obtidos no sistema RPY (giroscpios), para o sistema NED.
Entretanto o uso destas equaes est limitado at que as solues para e no se
tornem indefinidas, ou seja, at que no ocorra a singularidade gimbal lock, onde
0
90 = .


2.2.2.2. Transformaes de coordenadas entre o sistema RPY e o sistema NED
por Co-senos Diretores

A transformao de coordenadas atravs da matriz de co-senos diretores (MCD)
definida por uma matriz 3x3, cujas colunas representam os vetores unitrios no eixo
da plataforma e projetados ao longo do sistema de coordenadas de referncia (neste
exemplo o sistema de referncia o sistema de navegao local NED).


11 12 13
21 22 23
31 32 33
n
p
c c c
c c c
c c c
(
(
=
(
(

C .
(2.11)



20
O elemento da i-sima linha e j-sima coluna representa o co-seno do ngulo entre o
eixo do sistema de coordenadas de referncia i e o eixo j do sistema de coordenadas
da plataforma.

Um vetor definido no sistema de coordenadas da plataforma pode ser expresso no
sistema de coordenadas de navegao por


n n p
p
= r C r ,
(2.12)

onde a matriz
n
p
C aquela obtida atravs dos ngulos de Eule (equao 2.5).

Propagao da matriz co-seno diretor no tempo

A matriz de transformao Co-seno Diretor se propaga no tempo atravs da seguinte
equao


n n p
p p np
= C C

,
(2.13)

onde


0
0
0
z y
p
np z x
y x



(
(
=
(
(

.
(2.14)

Na equao 2.13,
n
p
C a matriz de orientao inicial (tambm conhecida como
matriz de alinhamento inicial) e
p
np
a matriz de propagao da velocidade de
rotao do sistema da plataforma (RPY) em relao ao sistema de navegao local
(NED).





21
2.2.2.3. Transformaes de coordenadas entre o sistema NED e o sistema ECEF

Esta seo apresenta a matrizes de transformao do sistema de navegao local
NED para o sistema Terra-Fixa ECEF (equao 2.15) e vice-versa (equao 2.16).
Estas transformaes so obtidas a partir da longitude e da latitude L.


cos sin 0 cos 0 sin 0 0 1
sin cos 0 . 0 1 0 . 0 1 0
0 0 1 sin 0 cos 1 0 0
e
n
L L
L L


( ( (
( ( (
=
( ( (
( ( (

C ,
sin cos sin cos cos
sin sin cos cos sin
cos 0 sin
e
n
L L
L L
L L


(
(
=
(
(

C .
(2.15)


0 0 1 cos 0 sin cos sin 0
0 1 0 . 0 1 0 . sin cos 0
1 0 0 sin 0 cos 0 0 1
n
e
L L
L L


( ( (
( ( (
=
( ( (
( ( (

C ,
sin cos sin sin cos
sin cos 0
cos cos cos sin sin
n
e
L L L
L L L



(
(
=
(
(

C .
(2.16)



2.2.2.4. Transformao de coordenadas entre o sistema ECEF e o sistema ECI

As equaes 2.17 e 2.18 representam as matrizes de transformao de coordenadas
do sistema ECEF para ECI e do sistema ECI para ECEF respectivamente. Estas
transformaes so obtidas a partir da longitude (ver anexos).


cos sin 0
sin cos 0
0 0 1
i
e


(
(
=
(
(

C , (2.17)



22

cos sin 0
sin cos 0
0 0 1
e
i


(
(
=
(
(

C . (2.18)


2.3. Modelagem terrestre da navegao inercial strapdown

2.3.1. Conceituando a acelerao denominada fora especfica

Conforme mencionado anteriormente, um acelermetro fornece uma medida de
acelerao denominada fora especfica. Para conceituar esta acelerao considere a
figura 2.5, onde uma fora peso P atua sobre uma massa m e, o corpo sofre um
deslocamento produzido por uma fora F.


Fig. 2.5 Foras atuando sobre um acelermetro


A segunda lei de Newton aplicada ao sistema produz


i
Foras m = + =

F P a ,
(2.19)

e a acelerao inercial obtida por


i
m m
= + = +
F P
a f g . (2.20)



23
Assim, a acelerao sentida pelo acelermetro, aquela que produz o movimento, ou
seja, a acelerao denominada fora especfica f, dada por


i
= f a g .
(2.21)


2.3.2. Modelagem da navegao num sistema de coordenadas fixo

Considere a situao onde se deseja navegar em um sistema de coordenadas inercial.
As componentes da fora especfica e da acelerao gravitacional so somadas para
determinar as componentes da acelerao em relao a este sistema de coordenadas.
Estas quantidades podem ento ser integradas uma vez para se obter estimativas da
velocidade e duas vezes para se obter estimativas da posio.

Este processo pode ser expresso matematicamente da seguinte forma. Seja r um
vetor posio que mapeia o ponto P em relao origem do sistema de coordenadas
inerciais (figura 2.6 a).

Y e
Z e
X e
O
P
Y i
Z i
X i
O
P
(a) Referencial fixo (inercial) (b) Referencial rotacional (terrestre)
r
r

Fig. 2.6 Sistemas de coordenadas fixo e mvel

A velocidade e a acelerao de P em relao ao sistema de coordenadas inercial so
dadas por



24

i
d
dt
(
=
(

i
r
v ,
(2.22)


2
2
i
d
dt
(
=
(

i
r
a .
(2.23)

Dado que os acelermetros medem a fora especfica que atua sobre o ponto P, tem-
se que


2
2
i
d
dt
(
= +
(

r
f g .
(2.24)

A equao acima chamada de equao da navegao inercial, onde a primeira
integrao fornece a velocidade e a segunda integrao fornece a posio.


2.3.3. Modelagem da navegao num sistema de coordenadas girante

Na prtica, geralmente necessita-se obter informaes sobre a velocidade e a
acelerao de um veculo em relao a um sistema de coordenadas girante. Este o
caso quando se navega na superfcie terrestre. Nesta situao surge a atuao de
foras aparentes sobre o veculo devido ao movimento de rotao terrestre. O
teorema de Coriolis relaciona as velocidades de deslocamento de um veculo sobre
um referencial girante e um referencial inercial atravs da seguinte equao


e ie
e i
d d
dt dt
( (
= =
( (

r r
v r .
(2.25)
Ou seja,

e i ie
e
d
dt
(
= =
(

r
v v r ,
(2.26)



25
onde | | 0 0 =
T
ie
o vetor que expressa a velocidade de rotao da Terra em
relao ao sistema de referencia inercial e o smbolo representa o produto
vetorial.


2.3.4. Modelagem da navegao no sistema de coordenadas inercial ECI

Deseja-se determinar a velocidade de deslocamento de um veculo sobre a superfcie
da Terra num sistema cujos eixos coincidem com os eixos do sistema de coordenadas
inercial. Esta velocidade denotada pelo smbolo v
i
e
e pode ser obtida expressando-
se a equao da navegao 2.24 em termos da velocidade terrestre por


e
e
d
dt
(
=
(

r
v .
(2.27)

A velocidade inercial pode ser obtida por


( ( (
= = = +
( ( (

r r r
v r
i e e
i ie e
i i e
d d d
dt dt dt
,
(2.28)

cuja derivada produz a acelerao inercial

| |
2
2
e
i ie e
i
i i
d d d
dt dt dt
(
(
= = +
(
(

v r
a r , (2.29)
isto ,

2
2
e ie
i ie
i
i i
d d d d
dt dt dt dt
(
( (
= = + +
(
( (


v r r
a r . (2.30)

Como 0
ie
d
dt
=

e
ie e ie
i e
d d
dt dt
( (
= + = +
( (

r r
r v r , obtm-se



26

2
2
( )
e
i ie e ie
i
i
d d
dt dt
(
(
= = + +
(
(


v r
a v r , (2.31)
e portanto

2
2
( )
e
i ie e ie ie
i
i
d d
dt dt
(
(
= = + +
(
(


v r
a v r . (2.32)

Lembrando que
2
2 i
i
d
dt
(
= = +
(

r
a f g , substituindo na equao acima obtm-se a
equao da navegao inercial no sistema de coordenadas inercial (ECI)


( )
e
ie e ie ie
i
d
dt
(
= +
(

v
f v g r .
(2.33)

Nesta equao f representa a acelerao denominada fora especfica, na qual o
sistema de navegao est submetido, enquanto que o termo
ie e
v representa a
acelerao causada pela velocidade do veculo movendo-se sobre a Terra, sendo que
esta acelerao geralmente chamada de acelerao de Coriolis. O termo
( )
ie ie
r define a acelerao centrpeta experimentada pela plataforma devido
a rotao da Terra, e no pode ser distinguida da acelerao gravitacional.

A soma vetorial da acelerao gravitacional g e da acelerao centrpeta constitui
uma componente gravitacional denominada gravidade local
l
g , ou seja, esta
gravidade aquela em que um prumo fica normal a superfcie em qualquer ponto do
globo terrestre, como mostra a figura a seguir.









27
Equador
Rotao
Terrestre
Circulo de Latitude
Constante
Acelerao Centrpeta
r
g
l
g
Gravidade Local

ie ie
r


Fig. 2.7 Acelerao centrpeta e gravidade local


Analisando-se a figura 2.7, conclui-se que ( )
l ie ie
= + g g r ; substituindo na
equao 2.33, obtm-se


e
ie e l
i
d
dt
(
= +
(

v
f v g .
(2.34)

Esta equao pode ser expressa no sistema inercial, por


i i i i i
e ie e l
= + v f v g . (2.35)

Como os acelermetros medem a fora especifica no sistema de coordenadas da
plataforma RPY, necessrio aplicar uma transformao de coordenadas. Assim a
equao 2.35, pode ser expressa como


i i p i i i
e p ie e l
= + v C f v g .
(2.36)



28
2.3.5. Modelagem da navegao no sistema de coordenadas terrestre ECEF


Neste sistema, a velocidade de deslocamento terrestre v
e
expressa ou projetada em
um sistema de coordenadas denominado Terra-Centralizada, Terra-fixa (ECEF), e
passa a ser denominada
e
e
v . Atravs da equao de Coriolis, conhecido que a taxa
de variao da velocidade terrestre v
e
em relao ao eixo da Terra pode ser expressa
em termos do sistema inercial por


e e
ie e
e i
d d
dt dt
( (
=
( (

v v
v .
(2.37)

A derivada da velocidade em relao ao referencial inercial


e
ie e l
i
d
dt
(
= +
(

v
f v g .
(2.38)

Substituindo na equao 2.35, obtm-se a equao da navegao inercial no sistema
de coordenadas Terra-Centralizada Terra-Fixa (ECEF)


,
2 ,
e
ie e l ie e
e
e
ie e l
e
d
dt
d
dt
(
= +
(

(
= +
(

v
f v g v
v
f v g
(2.39)

que pode ser expressa no sistema ECEF, por

2
e e e e e
e ie e l
= + v f v g . (2.40)

Para corrigir as sadas dos acelermetros que fornecem a fora especifica no sistema
RPY, aplica-se a transformao de coordenadas e chega-se



29

2
e e p e e e
e p ie e l
= + v C f v g ,
(2.41)

onde
e
p
C a matriz de rotao usada para transformar o vetor fora especfica (corpo
do veculo) para o sistema ECEF (Terra-Fixa). Esta matriz se propaga no tempo
atravs da equao


e e p
p p ep
= C C

, (2.42)

onde
p
ep
a forma anti-simtrica (
T
= A A) de
p
ep
que a matriz das velocidades
angulares dos giroscpios em relao ao sistema de coordenadas ECEF. Esta matriz
obtida derivando-se as velocidades angulares que so expressas no sistema de
coordenadas da plataforma
p
ip
, e a velocidade de rotao terrestre
e
ie
, que
expressa no sistema ECI e projetada no sistema de coordenadas ECEF.


p p p e
ep ip e ie
= C (2.43)

onde:
T
p e
e p
( =

C C .


2.3.6. Modelagem da navegao no sistema de coordenadas da navegao
local NED

Para se navegar longas distncias sobre a Terra, as informaes de navegao so
mais requeridas no sistema geogrfico local ou de navegao local NED em termos
das componentes de velocidade Norte e Leste ou em termos da Longitude, Latitude e
Altitude. Embora tais informaes possam ser computadas utilizando as estimativas
de posio fornecidas pelos sistemas ECI ou ECEF, elas envolvem transformaes
da velocidades
i
e
v ,
e
e
v . Estas dificuldades aumentam ao representar o campo
gravitacional com preciso em um computador. Por estas razes, o sistema de


30
referncia navegao local, descrito a seguir freqentemente utilizado quando se
navega sobre a superfcie terrestre (Chatfield, 1997).

Neste modelo, a velocidade terrestre da plataforma
e
v expressa no sistema de
coordenadas da navegao local para fornecer
n
e
v . A taxa de variao da velocidade
n
e
v em relao ao eixo de navegao local pode ser expressa em termos da taxa de
variao da velocidade no eixo inercial como


[ ]
e e
ie en e
n i
d d
dt dt
( (
= +
( (

v v
v .
(2.44)

Como
e
ie e l
i
d
dt
(
= +
(

v
f v g , obtm-se a equao da navegao inercial no
sistema de coordenadas da navegao local NED


[2 ]
e
ie en e l
e
d
dt
(
= + +
(

v
f v g .
(2.45)

Convertendo-se as sadas dos acelermetros, a equao anterior pode ser expressa no
sistema de coordenadas da navegao local NED, por


[2 ] ,
[2 ] ,
n n p n n n n
e p ie en e l
n n n n n n
e ie en e l
= + +
= + +
v C f v g
v f v g

(2.46)

onde
n
p
C a matriz de rotao usada para transformar o vetor fora especfica da
plataforma para o sistema NED. Esta matriz propaga-se no tempo atravs da equao


n n p
p p np
= C C

, (2.47)



31
onde
p
np
a forma anti-simtrica de
p
np
, que a matriz de velocidade angular dos
giroscpios em relao ao sistema de coordenadas navegao local. Esta matriz
obtida derivando-se as velocidades angulares expressas no sistema de referncia da
plataforma
p
ip
e a velocidade de rotao terrestre
in
, onde
in ie en
= + , e


[ ]
p p p n n
np ip n ie en
= + C . (2.48)


2.3.7. Representao da navegao local NED atravs das coordenadas
curvilneas ou geodsicas

A acelerao de um sistema de navegao inercial referenciada no sistema de
navegao local NED expressa pela equao

[2 ]
n n n n n n
e ie en e l
= + + v f v g . (2.49)

Nesta equao,
n
e
v representa a velocidade da plataforma em relao a superfcie
terrestre, expressa no sistema NED por

| |
T
n
e N E D
v v v = v . (2.50)

Neste vetor, as componentes
N
v ,
E
v e
D
v , representam as velocidades nos sentidos
norte, leste e para baixo, respectivamente.

A componente
n
f representa o vetor de aceleraes (fora especfica) fornecido pelos
acelermetros no referencial de navegao local, e pode ser representada atravs de

| | | |
T T
n
N E D N E D
a a a v v v = = f . (2.51)



32
O vetor
n
ie
o vetor de velocidade de rotao da Terra em relao ao referencial
inercial, expresso no sistema de coordenadas de navegao local, e pode ser obtido
por

0 0 1 cos 0 sin cos sin 0 0 cos
. 0 1 0 . 0 1 0 . sin cos 0 . 0 0
1 0 0 sin 0 cos 0 0 1 sin
n n e
ie e ie
L L L
L L L



= = =

( ( ( ( (
( ( ( ( (
( ( ( ( (
( ( ( ( (

C ,
(2.52)

onde
n
en
representa a taxa de rotao do sistema NED em relao ao sistema ECEF.
O vetor
n
en
tambm denominado como vetor taxa de transporte (Titterton, 1997),
e pode ser expresso em termos das coordenadas geodsicas latitude (), longitude (L)
e altitude (h) por


cos sin
T
n
en
L L L ( =

.
(2.53)

As componentes

e L

representam as taxas de variaes (derivadas) da longitude e

latitude respectivamente, podendo ser representadas por


0
( ) cos
E
v
R h L
=
+

,
(2.54)


0
( )
N
v
L
R h
=
+

.
(2.55)

Substituindo

e L

na equao 2.53, tem-se

0 0 0
tan
( ) ( ) ( )
T
n N E E
en
v v v
L
R h R h R h
(
=
(
+ + +

, (2.56)

onde
0
R o raio da Terra e h a altitude da plataforma representada no sistema
NED.


33

O vetor
l
g a componente gravitacional local que inclui os efeitos combinados da
gravitao terrestre g e da acelerao centrpeta
ie ie
r , ou seja,


0
( ),
( ( )).
n n n n
l ie ie
n n n n
l ie ie
R h
=
= +
g g r
g g
(2.57)

Substituindo-se a equao 2.52 na equao 2.57 e desenvolvendo-se o produto
vetorial, obtm-se uma representao para a gravidade local, dada por


( ) ( )
( )
2 2
0 0
sin 2 1 cos 2
0
2 2
T
n n
l
R h L R h L ( + + +
=
(

g g .
(2.58)

As componentes da fora especifica
n
f , que so as sadas dos acelermetros, podem
ser expressas no sistema NED atravs das coordenadas geodsicas (Titterton, 1997),
pelas equaes


( )
( )
( )
2
0
2 sin ,
tan
2 sin ,
N N N E D
N D E
N N E
a v f v L v L g
v v v L
a f v L g
R h

= = + + +

= + +
+

(2.59)


( ) ( )
( )
( )
( )
0
2 sin 2 cos ,
2 sin cos tan ,
E E E N D
E
E E N D D N
a v f v L v L g
v
a f v L v L v v L g
R h

= = + + + +
= +
+

(2.60)


( )
( )
( )
2 2
0
2 cos ,
2 cos ,
D D D E N
E N
D D E
a v f v L v L g
v v
a f v L g
R h
= = + +
+
= +
+

(2.61)

onde e representam as deflexes angulares e sero discutidas adiante.



34
2.3.8. O formato da Terra segundo a conveno WGS-84

O modelo que adota a Terra como uma esfera perfeita no suficiente para
representar a navegao inercial de preciso. Neste sentido um modelo mais realista
deve ser adotado. Devido ao achatamento nos plos, o formato da Terra assemelha-
se a uma elipse. O comit World Geodetic System em sua conveno de 1984
(WGS-84), props que a forma da Terra uma elipside como ilustra a figura a
seguir.
R
r
Eixo Equat orial
Plano meridiano
local
Elipse de referncia
Superfcie t errest re
(fora de escala)
Eixo Polar

Fig. 2.8 Modelo terrestre elptico adotado pelo comit WGS84

Na tabela 2.2, definem-se os parmetros do modelo terrestre elptico.

Tabela 2.2 Parmetros do modelo terrestre elptico (WGS84)
Parmetro Definio Magnitude Unidade
R Semi-eixo maior 6378137,0
| |
m
(1 ) r R f =
Semi-eixo menor 6356752,3142
| |
m
R r
f
R

= Achatamento da elipside
1
298, 257223563

--
( ) 2 e f f =
Maior excentricidade da
elipside
0,0818191908426 --
Rotao da Terra
7,292115e-6
ou
15,041067
| |
/ rad s
ou
| |
/ h



35
Adotando-se o modelo terrestre definido pelo comit WGS-84, as taxas de variao
da latitude e da longitude devem ser corrigidas atravs das seguintes equaes
(Titterton, 1997)


( ) cos
E
E
v
R h L
=
+

,
(2.62)


( )
N
N
v
L
R h
=
+

,
(2.63)

onde


( )
2 2
1 sin
E
R
R
e L
=

,
(2.64)


( )
( )
2
3
2 2
1
1 sin
N
R e
R
e L

.
(2.65)

Assim, a equao 2.56 deve ser corrigida e passa a ser representada matricialmente
como


tan ,
( ) ( ) ( )
1
0 0
( )
1
0 0 .
( )
tan
0 0
( )
T
n N E E
en
E N E
E
N
n
en E
N
E
E
v v v
L
R h R h R h
R h
v
v
R h
v
L
R h
(
=
(
+ + +

(
(
+
(
(
(
(
=
(
(
+
(
(

(

(
+

.
(2.66)

As componentes da velocidade em termos das coordenadas curvilneas (geodsicas)
so obtidas por



36

( )
( )
0 0
0 cos 0 .
0 0 1
N N
n
e E E
D
v R h L
v R h L
v h

( + ( (
( ( (
= = +
( ( (
( ( (

v

. (2.67)

Logo, as derivadas das coordenadas geodsicas podem ser escritas como


( )
( )
1
0 0
1
0 0 .
cos
0 0 1
N
N
n
e E
E
D
R h
L v
v
R h L
h v

(
(
+
(
(
(
(
(
(
= =
(
(
(
+
(
(
(

(

.
(2.68)


2.3.9. Deflexo gravitacional sobre a superfcie da Terra

Conforme mencionado anteriormente, existem variaes da atrao gravitacional
sobre a superfcie terrestre denominadas deflexes angulares. Este fenmeno ocorre
devido anomalias na gravidade terrestre, tais como distribuio no uniforme da
massa da Terra, variaes gravitacionais devido a acelerao centrfuga, que varia
em funo da latitude, etc. A figura 2.9 ilustra a deflexo meridional que ocorre
no sentido da latitude. Para entender a deflexo , imagine que a deflexo que
ocorre no sentido da longitude.

Portanto, devido ao fenmeno da deflexo gravitacional, expressa-se o vetor
gravidade local em termos da decomposio de suas componentes nas direes norte,
leste e para baixo, atravs de


_
_
_
l N
l l E
l D
g g
g g
g g

( (
( (
= =
( (
( (


g .
(2.69)




37
R
r
Eixo Equat orial
Plano meridiano
local
Elipse de referncia
Eixo Polar
L
l
g

Lat it ude
geogrfica
Deflexo
meridional


Fig. 2.9 Representao da deflexo gravitacional latitudinal ()


2.3.10. Mecanizao do sistema de navegao local NED

A figura a seguir ilustra um diagrama que pode ser implementado para estimar
coordenadas de posies no sistema de coordenadas locais NED, a partir de uma
central inercial strapdown.

Este diagrama apresenta um fechamento das etapas que devem ser seguidas para se
implementar um algoritmo que possibilite a navegao no sistema tangencial local a
partir de uma UMI strapdown. As grandezas indicadas no diagrama, so aquelas
obtidas nas sees: (2.2.2), (2.3.5), (2.3.6) e (2.3.7).










38

Acelermetros
Giroscpios
n
p
C
p
n
C

p
ip

p
f
n
f
n
in

( ) 2
n n n
ie en e
+ v
n
ie

e
ie

n
e
v
n
e
v
n
e
r

Computa
n
en

n
e
C
,
n n
en e
C
0
r
0
v
Computa a
gravidade local
n
g
Coordenadas
local
p
np

+
+
+
+
+

UMI
Fig. 2.10 Modelo do sistema de navegao


39
3. FORMULAO DO PROBLEMA

Descreve-se a seguir a formulao do problema da estimao de trajetria atravs da
navegao inercial. Inicialmente descrita a representao no espao de estados, os
modelos matemticos que regem a dinmica do sistema e a implementao do filtro
de Kalman atravs destes modelos. A estimao de estados est dividida em duas
partes: na primeira parte so estimados os ngulos de rotao da plataforma e na
segunda parte so estimados os estados de acelerao, velocidade e posio. O
processo de se estimar primeiramente os ngulos de rotao se faz necessrio para
realizar as transformaes de coordenadas com maior preciso, pois antes de se
estimar a acelerao, a velocidade e a posio da plataforma, estas grandezas devem
estar projetadas no sistema de coordenadas da navegao local NED.


3.1. Representao no espao de estados

O espao de estados uma tcnica que utilizada para representar as equaes
diferenciais que descrevem o comportamento de um sistema dinmico. A vantagem
de se utilizar a representao no espao de estados, que ela se d no domnio do
tempo e pode ser aplicada diretamente a sistemas lineares, no lineares e
multivariveis, como o caso deste trabalho. Outra vantagem que ela pode ser
facilmente discretizada e, portanto implementada em computadores digitais.

Um sistema linear pode ser descrito no espao de estados por (Franklin, 1998)

( ) ( ) ( ) ( ),
( ) ( ) ( ).
= + +
= +
x x u Cw
z Hx Ge
t t t t
t t t
.
(3.1)

Esta equaes podem ser discretizadas e representadas no espao de estados discretos
por (Chen, 2000)



40

k+1 k k k
k k k
,
.
= + +
= +
x Ax Bu Cw
z Hx Ge

(3.2)

As matrizes ( , A) e ( , B) nas equaes 3.1 e 3.2, se relacionam atravs de


( )
0
,
.
T
T
e
e d

=
=

A
B

(3.3)

Na equao 3.2, x
k
o vetor de estados, u
k
o vetor de entradas, z
k
o vetor de
medidas de referncia (ou sadas), w
k
um vetor de rudos aleatrios do processo e
e
k
um vetor de erros de medio. As matrizes A, B e C so matrizes que definem
a dinmica do processo, enquanto que G e H so matrizes que definem a equao de
medio.


3.1.1. Estimao de estados

A reconstruo de trajetrias atravs da navegao inercial interpretada como um
problema de estimao de estados, onde, dado o modelo matemtico que descreve a
navegao e um conjunto de medidas, torna-se possvel estimar o valor de seus
estados a cada instante.

A figura 3.1 ilustra o problema de estimao de estados. Neste mecanismo
(Furukawa, 1992), x so os estados a serem estimados, u so as entradas do sistema
(que devem ser conhecidas) e z so algumas sadas relacionadas a x por h(x) e que
podem ser observadas. O estimador determina as estimativas x dos estados a partir
de modelos matemticos do sistema e da funo de sada h.






41



Fig. 3.1 Estimador de estados

Ocorre entretanto que os sistemas reais esto sujeitos a erros e rudos, os quais no
esto representados no modelo anterior. Para contornar este problema, as fontes de
erros podem ser modeladas como rudos adicionados as entradas () e as sadas (v)
do sistema (figura 3.2). Os rudos adicionados s entradas so conhecidos como
rudos de processo e os rudos adicionados s sadas so conhecidos como rudos de
medio.

Devido a presena de rudos aleatrios, o novo sistema passa a ter natureza
estocstica (aleatria), onde tem-se agora que determinar no somente a estimativa
dos estados x , mas tambm a incerteza p associada a esta estimativa.



Fig. 3.2 Estimador de estados estocstico


42
Adequando-se os conceitos ora mencionados ao problema da navegao inercial,
tem-se que o veculo (plataforma) passa a ser o sistema dinmico, as entradas so as
leituras dos acelermetros e os estados (variveis) a serem estimados so as posies,
velocidades e erros de aceleraes.


3.2. O filtro de Kalman discreto

Conforme mencionado anteriormente, neste trabalho ser empregada uma tcnica
estocstica de estimao de estados conhecida como Filtro de Kalman (FK). O
filtro de Kalman o ncleo do algoritmo de navegao proposto, sendo que o mesmo
realiza conjuntamente a fuso sensorial, gerando assim as estimativas dos estados e
efetuando a correo destas estimativas atravs de medidas auxiliares que podem ser
medidas de velocidade, marcas topogrficas ou ambas.

O filtro de Kalman essencialmente um conjunto de equaes matemticas que
implementam um estimador de estados conhecido como preditor-corretor. Quando
algumas condies so satisfeitas, o filtro de Kalman considerado um estimador
timo e minimiza a covarincia do erro estimado. O filtro de Kalman empregado
no sentido de tentar estimar o estado
n
x R de um processo controlado em instantes
discretos de tempo, que governado por uma equao de diferenas linear
estocstica


1 k k k k +
= + + x Ax Bu w ,
(3.4)

e uma equao de medio
m
z R que dada por


k k k
= + z Hx v .
(3.5)

As variveis aleatrias
k
w e
k
v representam os rudos do processo e da medio
respectivamente. Considera-se que estas variveis so rudos brancos, independentes


43
(estatisticamente no correlacionadas) e com distribuio normal de probabilidade
dadas por

( ) (0, ),
( ) (0, ).
p N
p N
w Q
v R

(3.6)


3.2.1. O algoritmo do filtro de Kalman discreto

O FK estima um processo utilizando uma forma de controle por realimentao o
filtro estima o estado do processo em algum instante de tempo e ento obtm a
realimentao a partir de medies. Assim, as equaes para o filtro de Kalman so
divididas em dois grupos, atualizao temporal (predio) e atualizao da medio
(correo). As equaes de atualizao temporal so responsveis em projetar a
frente (no tempo) o estado corrente e a estimativa da covarincia do erro para obter
uma estimava do erro para o prximo instante de tempo. As equaes para
atualizao da medio so responsveis pela realimentao, ou seja, elas
incorporam uma nova medio ao estado estimado a priori para obter uma estimativa
melhorada a posteriori.

As equaes de atualizao temporal do filtro de Kalman discreto so


1

k k k

= + x Ax Bu , (3.7)


1
T
k k

= + P AP A Q. (3.8)

As equaes de atualizao da medio do filtro de Kalman discreto so


1
( )
T T
k k k

= + K P H HP H R , (3.9)


( )
k k k k k

= + x x K z Hx , (3.10)


( )
k k k

= P I K H P . (3.11)


44
O primeiro passo da atualizao da medio computar o ganho do filtro de Kalman
(K
k
). O prximo passo atualizar a medio do processo para obter x
k
, e ento gerar
uma estimativa do estado a posteriori atravs da incorporao de uma medio. O
passo final obter uma estimativa da covarincia do erro a posteriori.

Aps cada ciclo de atualizao temporal e atualizao da medio, o processo
repetido atravs da estimativa a posteriori anterior que utilizada para projetar ou
predizer uma nova estimativa a priori. A figura 3.3 mostra como funciona o ciclo de
operao do filtro de Kalman discreto.

(1) Projeta o estado a frente
(2) Projeta a covarincia do erro a frente
(1) Computa o ganho K do filtro de Kalman
(2) Atualiza a estimativa com a medio
k
z
(3) Atualiza a covariancia do erro
( )
k k k k k
x x K z Hx

= +
1

k k k
x Ax Bu

= +
1
T
k k
P AP A Q

= +
1
( )
T T
k k k
K P H HP H R

= +
( )
k k k
P I K H P

=
Atualizao Temporal (Predio)
Atualizao da Medio (Correo)
Entra com as estimativas
iniciais
Entradas
Medidas de referncia

Estados estimados
0 0
x e P
k
u
k
z

k
x

Fig. 3.3 Ciclo do filtro de Kalman


3.3. Modelo proposto para estimar estados de giroscpio

3.3.1. Modelo dinmico dos giroscpios

O modelo adotado para definir a dinmica dos giroscpios baseia-se na derivada
angular onde
d
dt

= , e portanto
0
0
t
d dt

=

. Resolvendo-se esta integral
definida, o valor instantneo de um ngulo pode ser definido por



45

0
( ) . t t = + .
(3.12)

Admitindo-se que a velocidade angular fornecida pelos giroscpios, e que a
mesma pode estar contaminada por um erro e um rudo w, a equao 3.12 pode
ser escrita como


0
( ) ( ). t w t = + + + .
(3.13)

Esta equao pode ser discretizada atravs da seguinte tcnica


( 1)
0
( )
lim
k t kt
t
d t
w
dt t

= = + +

. (3.14)

Para sistemas amostrados o intervalo de tempo t corresponde a um perodo de
amostragem T . Fazendo este perodo de amostragem muito pequeno a equao 3.14
pode ser escrita como:


1
1
,
.
k k
k k
w
T
T T wT



+
+

+ +
+ + +

(3.15)

Esta equao pode ser representada no espao de estados discreto por


1 k k k k +
= + + x Ax Bu Cw ,
(3.16)

onde A, B e C so as matrizes que definem a dinmica do sistema, x um vetor de
estados que composto pelas grandezas que se deseja estimar, ou seja, os ngulos
(
x
,
y
e
z
) e os erros dos giroscpios (
x
,
y
e
z
) ; u o vetor de entradas
e composto pelas velocidades angulares (
x
,
y
e
z
) fornecidas pelos
giroscpios e w um vetor de rudos aleatrios presente nas medidas dos
giroscpios. A equao 3.16 pode ser escrita na forma matricial por


46


1
1 0 0 0 0 0 0
0 1 0 0 0 0 0
0 0 1 0 0 0 0
. .
0 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
0 0
x x
y y
x
z z
y
x x
z
K
y y
z z
K K
T T
T T
T T
T
T

+
( ( ( (
( ( ( (
( ( ( (
(
( ( ( (
(
= + +
( ( ( (
(
( ( ( (
(

( ( ( (
( ( ( (
( (

1
2
3
4
5
6
0 0 0
. .
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
k
w
w
w T
w T
w T
w T
( (
( (
( (
( (
( (
( (
( (
( (
(


(3.17)


3.3.2. Filtro de Kalman utilizado para estimar estados de giroscpio

Giroscpios apresentam uma parcela de erros que no so determinsticos. Portanto,a
forma mais adequada de minimizar seus efeitos atravs do Filtro de Kalman
(Brown, 1997) e (Rogers, 2000).

Para se utilizar o filtro de Kalman, deve-se obter uma referncia do vetor de estados
(que podem ser, por exemplo, medidas angulares fornecidas por um inclinmetro).
importante que o rudo do processo w (descrito na equao 3.16) no possua as
mesmas caractersticas do rudo de referncia e (equao 3.18), pois no caso de
ambos possurem a mesma caracterstica ter-se-ia uma correlao estatstica entre
eles e o Filtro de Kalman no funcionar adequadamente.

A seguir apresentado o modelo da medio que ser utilizado com o filtro. Neste
modelo, e um vetor de erros de medidas e z o vetor de medidas de referncias
(ngulos).


47

1
2
3
1 0 0 0 0 0 1 0 0
0 1 0 0 0 0 . 0 1 0 .
0 0 1 0 0 0 0 0 1
x
y
x
z
y
x
z
k k
y
z
K
z e
z e
z e

(
(
(
( ( ( (
(
( ( ( (
= +
(
( ( ( (
(
( ( ( (

(
(
(

.
(3.18)

A figura a seguir ilustra um diagrama de implementao do filtro de Kalman para se
obter as estimativas angulares, a partir do vetor de velocidades angulares
k

fornecido pelos giroscpios da UMI e do vetor de medidas de referncia
k
z fornecido
por sensores adicionais.

(1) Projeta o estado a frente
(2) Projeta a covarincia do erro a frente
(1) Computa o ganho K do filtro de Kalman
(2) Atualiza a estimativa com a medio
k
z
(3) Atualiza a covariancia do erro
( )
k k k k k
x x K z Hx

= +
1

k k k
x Ax Bu

= +
1
T
k k
P AP A Q

= +
1
( )
T T
k k k
K P H HP H R

= +
( )
k k k
P I K H P

=
Atualizao Temporal (Predio)
Atualizao da Medio (Correo)
Entra com as estimativas
iniciais
Entradas
(velocidades angulares)
Medidas de referncia
(ngulos)
Estados estimados
(ngulos e erros angulares)
0 0
x e P
, , , ...
k k k k k
z x y z = =
, ,
k k k k k
u x y z = =
, , , , ,
k k k k k k k
x x y z x y z =


Fig. 3.4 Filtro de Kalman para estimar ngulos de rotao


3.4. Ortogonalizao e normalizao da matriz de orientao

Em sistemas de navegao inercial strapdown, ao se implementar algoritmos para
determinar a orientao da plataforma, interessante aplicar um teste de consistncia
na matriz de orientao (MO) para evitar que ocorra a propagao de erros (Titterton,
1997). Dado que as linhas e colunas de uma MO representam a projeo dos vetores
unitrios de cada eixo do sistema de coordenadas da plataforma, as mesmas devem


48
sempre ser ortogonais entre si, e a soma do quadrado dos elementos de cada linha ou
coluna devem ser unitrios.


3.4.1. Ortogonalizao

As condies para ortogonalidade das i-sima, j-sima e k-sima linhas ou colunas
de uma MO, aqui denotada por
i
C ,
j
C e
k
C que o produto escalar entre duas
linhas ou colunas quaisquer sejam igual a zero. Em aplicaes prticas, este no
necessariamente o caso ento se pode definir um resduo a partir de


T
ij i j
= CC
T
ik i k
= CC
T
jk j k
= C C ,
(3.19)

onde
ij
,
ik
e
jk
so os erros de ortogonalidade entre as componentes
i
C ,
j
C e
k
C .

Admitindo que cada linha ou coluna de
i
C ,
j
C e
k
C possuam erros igualmente
distribudos, pode-se efetuar uma correo a partir de


1

2
1

2
i i ij j
i i ik k

C C C
C C C
,
(3.20)


1

2
1

2
j j ij i
j j jk k

C C C
C C C
,
(3.21)



49

1

2
1

2
k k ik i
k k jk j

C C C
C C C
.
(3.22)

Agrupando-se a contribuio de erro de ortogonalidade de cada componente nas
equaes 3.20 a 3.22, obtm-se as linhas ou colunas estimadas


1 1

2 2
1 1

2 2
1 1

2 2
i i ij j ik k
j j ij i jk k
k k ik i jk j

C C C C
C C C C
C C C C
.
(3.23)


3.4.2. Normalizao

Os erros de normalizao podem ser identificados comparando-se a soma dos
quadrados dos elementos em cada linha ou coluna, com um valor unitrio, ou seja


1
1
1
T
ii i i
T
jj j j
T
kk k k

= - CC
= - C C
= - C C
.
(3.24)

Rearranjando as equaes acima e utilizando o critrio de distribuio de erros
discutido anteriormente, chega-se


1

2
1

2
1

2
i i ii i
j j jj j
k k kk k

C C C
C C C
C C C
.
(3.25)


50

Nas equaes 3.20 a 3.25, a notao ^ representa a quantidade estimada (corrigida).


3.5. Correes da acelerao

3.5.1. Correes determinsticas

A acelerao da plataforma no sistema de coordenadas NED representada pela
equao


[2 ] ,
2 .
n n p n n n n
e p ie en e l
n n n n n n n
e l ie e en e
= + +
= +
a C f v g
a f g v v
(3.26)

O primeiro termo da segunda equao representa a fora especifica medida pelos
acelermetros e projetada no sistema NED. O segundo, terceiro e quarto termos
representam a gravidade local, a acelerao de Coriolis e a acelerao centrpeta
respectivamente e, portanto, devem ser efetuadas as correes destas aceleraes
para evitar os efeitos indesejados das mesmas sobre a acelerao da plataforma.


3.5.1.1. Gravidade local

A primeira correo a ser efetuada a compensao da gravidade local que dada
pelo termo
n
l
g , e pode ser expressa no sistema NED pela equao


( ) ( )
( )
2 2
0 0
sin 2 1 cos 2
0
2 2
T
n n
l
R h L R h L ( + + +
=
(

g g ,
(3.27)

onde a componente da acelerao
n
g , a acelerao gravitacional expressa no
sistema de navegao local. Ou seja,


51


2
0
0 /
9.80665
n
m s
(
(
=
(
(

g .
(3.28)


3.5.1.2. Acelerao de Coriolis

A segunda correo determinstica refere-se ao terceiro termo da equao 3.26. Este
termo representa a acelerao sentida pela plataforma ao se deslocar sobre um
referencial girante, e conhecido como acelerao de Coriolis.

A rotao da Terra no sistema de coordenadas ECEF,


0 0
0 0 /
0.00007292
e
ie
rad s
( (
( (
= =
( (
( (

.
(3.29)

Esta rotao pode ser transportada para o sistema NED, fazendo-se

sin cos sin sin cos 0 cos
. sin cos 0 . 0 0
cos cos cos sin sin sin
n n e
ie e ie
L L L L
L L L L



( ( (
( ( (
= = =
( ( (
( ( (

C .
(3.30)

A velocidade da plataforma (
p
e
v ) pode ser projetada para o sistema NED, atravs de


x N
n n p n
e p e p y E
z D
p n
v v
v v
v v
( (
( (
= = =
( (
( (

v C v C .
(3.31)

Logo, a correo da acelerao de Coriolis feita atravs de



52
cos sin
2 2 0 2 ( sin cos )
sin cos
N D
n n
Coriolis ie e E N D
D E
L v v L
v v L v L
L v v L
( ( (
( ( (
= = = +
( ( (
( ( (

a v . (3.32)


3.5.1.3. Acelerao centrpeta

A terceira e ltima correo que deve ser feita refere-se ao quarto termo da equao
3.26. Esta parcela representa a acelerao sentida pela plataforma ao se deslocar
sobre uma superfcie curva e conhecida como acelerao centrpeta

O vetor que representa a taxa de transporte sobre a superfcie terrestre pode ser
expresso em termos das coordenadas curvilneas (Titterton, 1997), por


0 0 0
tan
( ) ( ) ( )
T
n N E E
en
v v v
L
R h R h R h
(
=
(
+ + +

. (3.33)

Portanto, a correo da acelerao centrpeta feita atravs do vetor


2
0 0
0 0
2 2
0
0
tan
( ) ( )
tan
( ) ( )
tan
( )
( )
E N D E
N
n n N E N E D
Centripeta en e E
D
E
E N
v L v v v
R h R h
v
v v v L v v
v
R h R h
v
v L
v v
R h
R h
(
(
(
(
+ +
(
(
(
(
(
(
= = =
(
(
(
+ +
(
(
(

(
(

(
(
+
+
(

a v .
(3.34)


3.5.2. Correes estocsticas

3.5.2.1. Influncia dos erros de giroscpios na acelerao

Os acelermetros medem a acelerao absoluta com relao a um referencial
inercial. Na estimao de velocidades e trajetrias, interessa somente a acelerao


53
linear (translacional), portanto devem-se compensar todos os outros erros de
acelerao. Para objetivos prticos, considera-se nesta anlise que a gravidade a
nica componente indesejvel do sistema. A orientao do acelermetro deve ser
conhecida com grande preciso para que se possa compensar a componente
gravitacional, evitando-se assim a adio de erros sobre a acelerao translacional,
que a que realmente interessa.

A orientao dos acelermetros pode ser obtida atravs de giroscpios. Os
giroscpios de uma UMI produzem uma sada proporcional velocidade de rotao.
Os sinais produzidos pelos giroscpios esto contaminados por rudos e deriva
(Nebot, 1999). Para pequenos intervalos de tempo, a deriva pode ser aproximada
como um sinal de bias constante. Assim, a orientao pode ser obtida por


( )
m
b dt bt dt = + + = + +

.
(3.35)

Nota-se que a integrao do sinal do giroscpio produz um ngulo de rotao
juntamente com mais dois termos indesejveis, ou seja, um termo que cresce com o
tempo devido a deriva b, e outro termo conhecido como Random Walk
8
ou
Passeio Aleatrio devido ao rudo .

Outra fonte muito importante de erro que deve ser modelada a deriva (drift) do
giroscpio. A figura a seguir mostra a situao onde um erro de deriva constante b,
atua sobre o eixo de rotao z.


8
Random Walk uma somatria de processos com rudo branco. Tambm conhecido como
processos de Wiener (Grewal, 2001).


54
Drift de
Giroscpio
Acelermet ro x
y
Acelermet ro y
y x
a a bt =
( )
x x
a sen bt a bt =
x
x
a


Fig. 3.5 Influncia da deriva do giroscpio z nos acelermetros x e y

Como pode ser observado, este erro de rotao produz uma leitura incorreta de
orientao para os acelermetros x e y, alm de introduzir um acoplamento do
acelermetro x no acelermetro y. Conforme mostra a figura, uma constante de
acelerao a
x
na direo do eixo x, produz erros de acelerao, velocidade e posio
no eixo y. Assumindo que a deriva de giroscpio seja pequena, pode-se linearizar
sen( ) bt bt = , e portanto os erros de acelerao, velocidade e posio so obtidos por



2
3
1
2
1
6
ay x
vy x
py x
e a bt
e a bt
e a bt
=
=
=
.
(3.36)

Observa-se ento que uma pequena deriva de giroscpio produz erros de acelerao
proporcional a t, erros de velocidade proporcional a t
2
e erros de posio
proporcional a t
3
.

Tambm pode ser observado na figura 3.5 que uma deriva de giroscpio introduz
uma deriva no acelermetro y, e como conseqncia temos que esta componente
induzida ir produzir erros de posio e velocidades dados por


55

2
1
2
vy
py
e bt
e bt
=
=
.
(3.37)


3.6. Modelo proposto para estimar estados da plataforma

3.6.1. Modelo dinmico da plataforma

O modelo dinmico da plataforma baseado nas leis do movimento de Newton, que
possibilita obter a velocidade e a posio a partir da acelerao. Pelas leis de
Newton, a acelerao pode ser expressa a partir da derivada da velocidade em
relao ao tempo, ou seja

dv
a
dt
= . (3.38)

Admitindo que esta acelerao seja a acelerao fornecida pela UMI e que a mesma
possui erros devido a impreciso dos acelermetros ( a ) e erros devido a influncia
de bias de giroscpio (
g
b at ), pode-se escrever

( )
medido real g
a a a b at = + + .
(3.39)

Isolando-se a acelerao real, substituindo na equao 3.38 e integrando, obtm-se a
velocidade, que dada por:


0
0 0
( ),
( ) ,
medida g
v t t
medida g
v
dv
a a b at
dt
dv a a dt b at dt

=
=

,
(3.40)


2
0
1
( )
2
medida g
v v a a t b at w = + + . (3.41)


56

A velocidade pode ser expressa em termos da derivada da posio em relao ao
tempo por

dp
v
dt
= . (3.42)

Substituindo a equao 3.41 na equao 3.42 e integrando, obtm-se a posio


0
2
0
2
0
0 0 0
1
( ) ,
2
1
( ) ,
2
medida g
p t t t
medida g
p
dp
v a a t b at
dt
dp v dt a a tdt b at dt

= +
= +


(3.43)


2 3
0 0
1 1
( )
2 6
medida g
p p v t a a t b at w = + + . (3.44)

As equaes 3.41 e 3.44 podem ser discretizadas em intervalos de tempo igual a T , e
representadas vetorialmente, por


2 2 3
1
1 1 1
2 2 6
k k k k k g k k
T T T T
+
= + + +
p
p p v a a b a w , (3.45)


2
1
1
2
k k k k g k k
T T T
+
= + +
v
v v a a b a w . (3.46)

Estas equaes definem o modelo discreto da dinmica da plataforma e podem ser
combinadas para fornecer uma representao no espao de estados discreto, do tipo


1 k k k k +
= + + x Ax Bu Cw .
(3.47)

Nas equaes 3.45 e 3.46, o vetor
k
a contm as aceleraes fornecidas pela UMI nas
direes x, y e z, j transformadas para o sistema NED e feitas as correes


57
determinsticas. Assim
k
a ser o vetor de entradas. O erro de acelerao a
desconhecido, ou seja, uma grandeza que se deseja estimar e, portanto far parte do
vetor de estados. Matricialmente tem-se


2
2
2
1
1 0 0 0 0 0 0
2
0 1 0 0 0 0 0
2
0 0 1 0 0 0 0
2
.
0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 1
x x
y y
z z
x x
y y
z z
x x
y y
z z k
T
T
c c
T
c c
T
c c
T
T v v
v v
T
v v
T
a a
T
a a
a a



+
=
(
(
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (

(
(

3
2
3
2
3
2
2
2
3
0
2 6
0
6 2
0
6 2
0
.
2
0
2
0
6
0 0 0
0 0 0
0 0 0
gy
gz
gx
x
gy
y
z k gz
gx
k
b T
T
b T
T
b T
T
a
b T
T
a
a b T
T
b T
T

+ +

(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(

(
(
(
(
(
(
(
(
(
(
(
(
(
(




1
2
3
4
5
6
7
8
9
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
. 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 1
cx
cy
cz
vx
vy
vz
k
w
w
w
w
w
w
w
w
w

( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (

.
(3.48)

Evidentemente, este modelo no se aplica a sistemas com acelerao varivel,
entretanto para intervalos de amostragem (T ) muito pequenos, pode-se admitir que a
acelerao da plataforma se mantm constante entre duas amostras consecutivas e,
portanto as equaes 3.45 e 3.46 podem ser empregadas neste sistema.




58
3.6.2. Modelo dinmico alternativo

O modelo dinmico da plataforma obtido a partir das equaes 3.45 e 3.46 exige que
seja conhecido o valor do bias dos giroscpios. Entretanto, obter o bias de um
giroscpio uma tarefa difcil que depende de um laboratrio com instrumentos
sofisticados. Para sistemas onde no se conhea o valor de bias dos giroscpios,
uma alternativa modelar o mesmo como um rudo aleatrio.


2 2
1
1 1
2 2
k k k k k k
T T T
+
= + + +
p
p p v a a w , (3.49)


1 k k k k k
T T
+
= + +
v
v v a a w .
(3.50)

Assim, os efeitos que o bias dos giroscpios produzem na posio e na velocidade
da plataforma so inseridos na matriz C da equao 3.47 .

Realocando os termos convenientemente, as equaes 3.49 e 3.50 podem ser escritas
na forma matricial como


2
2
2
1
1 0 0 0 0 0 0
2
0 1 0 0 0 0 0
2
0 0 1 0 0 0 0
2
.
0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 1
x x
y y
z z
x x
y y
z z
x x
y y
z z
k
T
T
c c
T
c c
T
c c
T
T v v
v v
T
v v
T
a a
T
a a
a a



+
(
(
( (
( (
( (
( (
( (
( (
( (
=
( (
( (
( (
( (
( (
( (

(
(

2
2
2
0 0
2
0 0
2
0 0
2
.
0 0
0 0
0 0
0 0 0
0 0 0
0 0 0
x
y
z
k
k
T
T
T
a
a
T
a
T
T
(
(
( (
( (
( (
( (
( (
(
( (
(
( (
+ +
(
( (
(
( (
( (
( (
( (
( (

(
(




59


3
3
1
2
3
3
4
2
5
2
6
7
2
8
9
0 0 0 0 0 0 0 0
6
0 0 0 0 0 0 0 0
6
0 0 0 0 0 0 0 0
6
0 0 0 0 0 0 0 0
. 2
0 0 0 0 0 0 0 0
2
0 0 0 0 0 0 0 0
2
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
k
T
T
w
w
T
w
w
T
w
T
w
w
T
w
w
T
T
T
(
(
(
(
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
( (
(
(
(
(
(

.
(3.51)

Nos modelos descritos pelas equaes 3.48 e 3.51, o vetor de estados x formado
por nove elementos:
x
c ,
y
c e
z
c so as trs coordenadas de posio,
x
v ,
y
v e
z
v so
as trs componentes da velocidade e
x
a ,
y
a e
z
a so as componentes do erro de
acelerao. O vetor de entradas u formado por trs elementos que so as
componentes da acelerao
x
a ,
y
a e
z
a fornecidas pela UMI e, conforme
mencionado anteriormente, j devem estar feitas as correes determinsticas e a
transformao para o sistema NED. O vetor w, composto por nove elementos
independentes um vetor de rudos aleatrios do processo. A constante T o
perodo de amostragem utilizado entre duas medies consecutivas e
g
b o bias
do giroscpio.


3.6.3. Filtro de Kalman utilizado para estimar estados da plataforma

Uma simulao simples do sistema ir gerar erros de posio elevados, aps poucos
minutos, e isso se deve presena dos rudos aleatrios nas medidas fornecidas pelos
acelermetros e giroscpios da UMI. Para melhor estimar as variveis e minimizar a
influncia dos erros, adota-se um filtro de Kalman, que utiliza como medidas de


60
referncia a velocidade fornecida por um velocmetro, a posio fornecida por
marcas topogrficas ou ambas.

Como o rudo da UMI um rudo cumulativo e o rudo das medidas de referncia
(marcas topogrficas ou velocmetro) que so obtidos a partir de uma planta
cartogrfica e um encoder digital incremental rotativo no so cumulativos, o Filtro
de Kalman consegue combinar satisfatoriamente estas caractersticas e estimar a
posio do veculo com erros relativamente pequenos.

A seguir so apresentados os modelos das medies que sero utilizados com o filtro,
onde e um vetor de erros de medidas e z o vetor de medidas de referncia de
velocidade (equao 3.52), coordenadas de posio (equao 3.53) e ambas (equao
3.54).


1
2
3
0 0 0 1 0 0 0 0 0 1 0 0
0 0 0 0 1 0 0 0 0 . 0 1 0 .
0 0 0 0 0 1 0 0 0 0 0 1
x
y
z
x x
y y
z z
k
k
x
y
z
k
c
c
c
z e v
z e v
z e v
a
a
a

(
(
(
(
(
(
( ( (
(
(
( ( (
(
= +
(
( ( (
(
(
( ( (
(

(
(
(
(

.
(3.52)


1
2
3
1 0 0 0 0 0 0 0 0 1 0 0
0 1 0 0 0 0 0 0 0 . 0 1 0 .
0 0 1 0 0 0 0 0 0 0 0 1
x
y
z
x x
y y
z z
k
k
x
y
z
k
c
c
c
z e v
z e v
z e v
a
a
a

(
(
(
(
(
(
( ( (
(
(
( ( (
(
= +
(
( ( (
(
(
( ( (
(

(
(
(
(

.
(3.53)



61



1
2
3
1 0 0 1 0 0 0 0 0 1 0 0
0 1 0 0 1 0 0 0 0 . 0 1 0 .
0 0 1 0 0 1 0 0 0 0 0 1
x
y
z
x x
y y
z z
k
k
x
y
z
k
c
c
c
z e v
z e v
z e v
a
a
a

(
(
(
(
(
(
( ( (
(
(
( ( (
(
= +
(
( ( (
(
(
( ( (
(

(
(
(
(

.
(3.54)

A figura a seguir ilustra o diagrama de implementao do FK para se obter as
estimativas de estados do sistema, a partir do vetor de medidas de acelerao
k
a
fornecidas pelos acelermetros da UMI e do vetor de medidas de referncia
k
z
fornecido pelo velocmetro.

(1) Projeta o estado a frente
(2) Projeta a covarincia do erro a frente
(1) Computa o ganho K do filtro de Kalman
(2) Atualiza a estimativa com a medio
k
z
(3) Atualiza a covariancia do erro
( )
k k k k k
x x K z Hx

= +
1

k k k
x Ax Bu

= +
1
T
k k
P AP A Q

= +
1
( )
T T
k k k
K P H HP H R

= +
( )
k k k
P I K H P

=
Atualizao Temporal (Predio)
Atualizao da Medio (Correo)
Entra com as estimativas
iniciais
, , ,... = =
k k xk yk zk
u a a a a
, , ,... = =
k k xk yk zk
z v v v v
, , , , , , , , ,... =
k xk yk zk xk yk zk xk yk zk
x p p p v v v a a a
Entradas
(aceleraes)
Medidas de referncia
(velocidades)
Estados estimados
(posices, velocidades e erros de aceleraes)
0 0
x e P

Fig. 3.6 Filtro de Kalman utilizado para estimar estados da plataforma






62
3.7. Alinhamento inicial e calibrao da plataforma

3.7.1. Alinhamento inicial

O alinhamento inicial ou orientao inicial necessrio para transportar as grandezas
medidas pelos acelermetros fixados na plataforma para o sistema de coordenadas
NED. Portanto o alinhamento inicial a determinao da matriz de orientao inicial
que alinha o sistema de coordenadas da plataforma com o sistema de navegao local
NED.

O alinhamento inicial pode ser feito de muitas formas, sendo que a mais simples a
utilizao de uma tcnica conhecida por girocompassing, que utiliza os
acelermetros da UMI para efetuar o nivelamento e os giroscpios para determinar o
norte geogrfico.

O nivelamento obtido inclinando-se a plataforma at que apenas o acelermetro do
eixo z apresente leitura igual acelerao gravitacional (-g). Para determinao do
norte geogrfico, gira-se a UMI em torno do eixo z at que o giroscpio do eixo x
apresente leitura nula e o giroscpio do eixo y apresente a velocidade de rotao da
Terra ().

A tcnica de girocompassing no indicada para UMIs que possuam giroscpios
com erros prximos ou superiores a 15,04
O
/h, devido impossibilidade de se medir
a velocidade de rotao da Terra (Shin, 2001).


3.7.2. Alinhamento terrestre pelo mtodo da matriz de orientao

Na seo anterior mencionou-se o mtodo de alinhamento inicial pela tcnica de
girocompassing, entretanto tal tcnica exige que a plataforma seja nivelada e
depois rotacionada para se obter o norte geogrfico. Uma tcnica semelhante que no


63
necessita rotacionar a plataforma o alinhamento terrestre pelo mtodo da matriz de
orientao, mostrado a seguir.

Os acelermetros e giroscpios montados na plataforma medem respectivamente a
fora especfica
p
f e a velocidade angular
p
ie
. Estas grandezas se relacionam com o
sistema de navegao local NED da seguinte forma:


p p n
n
= f C f , (3.55)


p p n
ie n ie
= C . (3.56)

Estas equaes podem ser escritas como


11 12 13
21 22 23
31 32 33
0
. 0
x
y
z
f c c c
f c c c
f c c c g
( ( (
( ( (
=
( ( (
( ( (

,
(3.57)


11 12 13
21 22 23
31 32 33
cos
. 0
sin
x
y
z
c c c L
c c c
c c c L

( ( (
( ( (
=
( ( (
( ( (

.
(3.58)

Analisando a equao 3.57, nota-se que os elementos da terceira coluna podem ser
obtidos por


13 13
x
x
f
f gc c
g
= = ,
23 23
y
y
f
f gc c
g
= = ,
33 33
z
z
f
f gc c
g
= = .
(3.59)




64
Na equao 3.58, os elementos da primeira coluna podem ser obtidos atravs de


11 13 11
tan
cos sin
cos
x x
x
f L
Lc Lc c
L g

= =

,
21 23 21
tan
cos sin
cos
y y
y
f L
Lc Lc c
L g

= =

,
31 33 31
tan
cos sin
cos
z z
z
f L
Lc Lc c
L g

= =

.
(3.60)

Finalmente, os elementos
12
c ,
22
c e
32
c podem ser determinados utilizando-se a
propriedade da ortogonalidade da (MO), ou seja, dada uma matriz ortonormal, da
lgebra linear sabe-se que


12 21 33 31 23
22 11 33 13 31
32 11 23 13 21
,
,
.
c c c c c
c c c c c
c c c c c
= +
=
= +

(3.61)


3.7.3. Calibrao

A calibrao dos sensores inerciais no propriamente uma interveno fsica nos
sensores da UMI. Na verdade trata-se de um processo onde se procura estimar os
valores de bias e fatores de escalas de giroscpios e atravs destes determinar os
bias e fatores de escala dos acelermetros. Este procedimento bastante sensvel e
demorado, e deve ser feito com a plataforma em repouso.

A calibrao inicial da UMI muito importante, ou mesmo imprescindvel, em
sistemas de navegao inercial onde no se disponha de informaes auxiliares para
estimar estados de acelermetro e giroscpios usando fuso sensorial, pois sem a
calibrao inicial torna-se impossvel reconstruir com relativa preciso as trajetrias
percorridas pela plataforma. Para se efetuar a calibrao inicial, deve-se dispor de
sensores auxiliares com preciso elevada (inclinmetros, GPS, etc.) e ento combinar


65
as informaes atravs de um filtro de Kalman para se obter as estimativas dos erros
de acelermetros e giroscpios (Nebot, 1999) e (Maybeck, 1976).
66
4. MATERIAIS E MTODOS


A seguir apresentam-se os materiais e os mtodos empregados na execuo dos
ensaios de estimao de trajetrias atravs de uma UMI strapdown e fuso
sensorial.


4.1. Testes efetuados

Para o ensaio de estimao de trajetrias, foram efetuados quatro testes automotivos,
sendo que trs foram feitos com durao mdia de 5 minutos cada e a distncia
percorrida foi de aproximadamente 2800 metros em percurso fechado (sada e
chegada no mesmo ponto), em uma regio aproximadamente plana.

A escolha de um percurso em trajetria fechada possui algumas caractersticas
favorveis. Primeiramente porque permite testar e avaliar visualmente dois tipos de
reconstrutores: o Duplo Integrador e o Filtro de Kalman. Como o percurso
fechado, espera-se reconstruir graficamente uma trajetria fechada, isto justificvel
pela facilidade de compreenso dos resultados obtidos, ou seja, visualmente
consegue-se distinguir se a trajetria reconstruda parecida com a trajetria
percorrida.

A segunda razo para se escolher a trajetria fechada que neste tipo de ensaio
torna-se possvel avaliar o efeito da deriva do giroscpio responsvel pelo
movimento de guinada, pois o mesmo ser solicitado continuamente. A desvantagem
do percurso fechado que tanto os erros de acelermetros e de giroscpios afetam
negativamente a reconstruo do trajeto. Neste sentido, um quarto teste em percurso
aberto e relativamente reto foi efetuado, no qual os giroscpios so pouco solicitados
e h predominncia de excitao dos acelermetros.


67
4.2. Equipamentos utilizados

A unidade de medio inercial empregada para os experimentos uma UMI do tipo
strapdown de baixo custo modelo VG700AA-202 da Crossbow (figura 4.1). Este
tipo de UMI destina-se principalmente a aplicaes em testes automotivos e tem
como principais sensores embarcados
3 acelermetros do tipo MEMS (Micro Eletro Mechanical System)
3 giroscpios do tipo FOG (Fiber Optic Gyro)
Sensor de temperatura
Processador digital de sinais (DSP) de 32 bits
Comunicao analgica (4096 pontos 0 a 5V) e digital (RS-232, 38400 bps)
Temporizador interno



Fig. 4.1 UMI strapdown Crossbow modelo VG700AA-202

Outras especificaes relevantes da UMI VG700AA-202 so apresentadas no quadro
a seguir:



68
Tabela 4.1 Especificaes da UMI Crossbow VG700AA-202
Especificao Faixa de Operao Unidade
Mxima taxa angular (roll/pitch/yaw) 200
o
/seg.
Bias mximo de giroscpio < 20
o
/hr.
Random Walk de giroscpio < 0,4
/ h
Resoluo mxima de giroscpio 0,025
o
/seg.
Mxima faixa de acelerao (X/Y/Z) 2 g
Bias mximo de acelermetro 0,085 m/s
2
Random Walk de acelermetro < 0,1
m/(s h)
Resoluo mxima de acelermetro 0,01 m/s
2

Taxa mxima de amostragem 100 Amostras/seg.
Temperatura de operao -40 a 71
o
C
Tenso de alimentao 10 a 30 V
DC
Dimenses 12,7x15,24x10,16 cm
Peso 1,6 kg



4.3. Metodologia dos testes

Neste trabalho, a UMI foi embarcada no interior de um automvel e conectada a um
computador porttil, onde a coleta de dados foi efetuada a partir do software
Giroview fornecido pelo fabricante da UMI. Para alimentao da UMI utilizou-se a
tenso disponvel na bateria do veculo.

O processamento de dados para estimao da trajetria realizada foi efetuado off-line
num computador PC atravs do software Matlab verso 6,5 Release 13.
A figura a seguir mostra a plataforma montada para a realizao dos testes na
EPUSP.

69


Fig. 4.2 Plataforma utilizada nos ensaios automotivos

Os algoritmos testados foram o duplo integrador no qual foi empregada a
mecanizao da figura 4.3 para obteno da posio, e o filtro de Kalman, baseado
no modelo alternativo, descrito pelas equaes 3.51 a 3.54. Para a simulao atravs
do filtro de Kalman, adotaram-se trs situaes: primeira simulao sem medidas de
referncia, ou seja, a matriz de medio H foi declarada nula, uma segunda
simulao tendo como medida de referncia a velocidade da plataforma, e finalmente
uma terceira simulao tendo como medidas de referncias a velocidade da
plataforma juntamente com um conjunto de marcas topogrficas landmarks. A
velocidade da plataforma foi obtida a partir da leitura do velocmetro do veculo.
Esta leitura foi coletada atravs de uma filmadora digital e os dados foram extrados
e interpolados posteriormente a fim de se obter a mesma quantidade de medidas
fornecidas pela a UMI. As marcas topogrficas (MT) utilizadas como medidas de
referncia foram convenientemente escolhidas e foram obtidas junto ao setor de
geodsica e cartografia do Departamento de Engenharia de Transportes da EPUSP.
Utilizou-se ainda o odmetro do veculo para fornecer o deslocamento relativo.
70
Dado que a navegao de interesse feita no sistema de coordenadas da plataforma
RPY e depois projetada para o sistema de navegao local NED, no seria necessrio
o alinhamento inicial da plataforma, pois o interesse na reconstruo da trajetria
xy, e portanto a determinao do norte geogrfico no seria importante. Entretanto,
devido a problemas de desnivelamentos, existem decomposies da acelerao
gravitacional nos acelermetros xy, tornando-se necessrio conhecer a orientao
inicial da plataforma para em seguida efetuar a propagao da orientao instantnea.
A orientao inicial foi obtida com o auxilio dos acelermetros, mantendo-se a
plataforma em repouso e seguindo o procedimento de alinhamento descrito na seo
(3.7.2).

Devido a indisponibilidade de equipamentos adequados, a calibrao externa inicial
da UMI no foi efetuada, entretanto tal deficincia no crucial, pois conforme
mencionado anteriormente este procedimento imprescindvel apenas quando no se
dispe de medies auxiliares para compor uma fuso sensorial.

Antes de se iniciar os testes, a UMI ficou num processo de calibrao interna atravs
do software Giroview (fornecido pelo fabricante) por aproximadamente 10
minutos. Este procedimento recomendado pelo fabricante e tem por objetivo
minimizar atravs de software os erros relativos deriva trmica e fator de escala
(UMI Crossbow VG700AA-202).

Para comparar a trajetria estimada com a trajetria real, utiliza-se um conjunto de
marcas topogrficas do trajeto onde o ensaio foi efetuado.

Os modelos adotados para reconstruir as trajetrias so o duplo integrador, cujo
diagrama de implementao ilustrado atravs da figura 4.3, e o filtro de Kalman
apresentado na seo 3.6. Em ambos os modelos, as aceleraes, velocidades e
posies so transportadas para o sistema NED, e as correes determinsticas so
efetuadas.

71

Fig. 4.3 Algoritmo do duplo integrador

Os ngulos de roll, pitch e yaw so obtidos integrando-se as velocidades
angulares em instantes discretos de tempo
k
T , pois a UMI utilizada apresenta
variao no perodo de amostragem T como mostra a figura a seguir.

0.5 1 1.5 2 2.5 3
40
45
50
55
Histrico da freqncia de amostragem
tempo(s)
A
m
o
s
t
r
a
s

p
o
r

s
e
g
u
n
d
o

Fig. 4.4 Histrico da freqncia de amostragem da UMI VG700AA-202
72
4.3.1. Monitoramento do filtro de Kalman

Para testar se o filtro de Kalman est funcionando adequadamente, seu desempenho
monitorado atravs da anlise da matriz de covarincia do erro P e da checagem da
mdia das inovaes ou resduos ( )
k k k

z H x .
A anlise de P consiste de dois procedimentos: primeiro testar se P semi-definida
positiva e segundo testar os elementos da diagonal principal de P.

Para testar se P semi-definida positiva, primeiramente simetriza-se a matriz P por
( ) / 2
T
= + P P P e ento aplica-se a fatorao modificada de Cholesky (Greewal,
2001 e Golub, 1996) sobre a matriz P a cada iterao. Esta tcnica tambm
conhecida como fatorao
T
UDU , e no caso de P no ser semi-definida positiva o
algoritmo falhar e o processo ser interrompido. O teste da diagonal de P consiste
em extrair a norma euclidiana (norma 2) a cada iterao e verificar seu
comportamento.

A checagem da mdia das inovaes feita extraindo-se uma amostra de
( )
k k k

z H x a cada iterao e plotando seu histograma. Se o filtro estiver

trabalhando corretamente, o histograma deve apresentar uma distribuio com mdia
zero (Greewal, 2001).


4.3.2. Sintonia do filtro de Kalman

Para sintonizar corretamente o filtro de Kalman necessrio conhecer as matrizes de
covarincia do rudo de processo Q e do rudo de medio R.

Em geral, o rudo de processo Q de difcil caracterizao, demandando um grande
esforo experimental e de modelagem. Por esse motivo, existem vrias tcnicas
descritas na literatura para se chegar a uma estimativa inicial, passvel ainda de
73
ajustes por tentativa e erro. Emprega-se neste trabalho, uma tcnica numrica
extrada de (Brown, 1997) para a estimao de Q.

Primeiramente forma-se uma matriz bloco, tal que


(9 9) (9 9)
(9 9) (9 9)
.
T
T
t



=



W
M
0
,
(4.1)

onde e so as matrizes de transio de estados e de entradas respectivamente
(vide equao 3.1). W a densidade espectral de potncia dos rudos associados ao
vetor de entradas u, no caso as leituras dos acelermetros. Como no trivial a
caracterizao de W, admite-se inicialmente que este rudo seja normalmente
distribudo com mdia zero e covarincia unitria. Neste caso particular, a densidade
espectral de potncia do rudo torna-se unitria e portanto W fica sendo


1 0 0
0 1 0
0 0 1


=



W .
(4.2)

A operao e
M
produz uma nova matriz bloco , cujos elementos so


1
(9 9) (9 9)
(9 9) (9 9)
...
T
e




=



M
A Q
0 A
.
(4.3)

Portanto, Q obtida transpondo-se A (segunda linha e segunda coluna), e em
seguida multiplica-se este resultado por
1
A Q (primeira linha segunda coluna). O
resultado obtido, e utilizado neste trabalho em particular,

74
7,1289e-7 0 0 2,9370e-5 0 0 0 0 0
0 7,1289e-7 0 0 2,9370e-5 0 0 0 0
0 0 7,1289e-7 0 0 2,9370e-5 0 0 0
2,9370e-5 0 0 1,2725e-3 0 0 0 0 0
0 2,9370e-5 0 0 1,2725e-3 0 0 0 0
0 0 2,9370e-5 0 0 1,2725e-3 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
=

.
(4.4)

A matriz da covarincia do rudo de medio R pode ser obtida numericamente
atravs do erro das velocidades xyz. Neste experimento, no se dispe de
instrumentos que permitam obter a velocidade real, portanto no se dispe do erro
produzido pelo velocmetro do veculo. Neste sentido, uma aferio simplificada foi
implementada. Esta aferio consiste em determinar a velocidade do veculo
medindo-se o tempo que o mesmo leva para percorrer uma distncia conhecida. Este
ensaio foi repetido 5 vezes, e o erro mdio oscilou em torno de 5% do valor lido no
velocmetro. A partir deste erro, torna-se possvel obter uma matriz com os erros de
velocidade nas direes xyz, e a partir desses valores extrair a covarincia. O
resultado obtido o valor de R a ser adotado, que para este caso em particular vale


2 2
1,9335e-1 4,3569e-3 1,8150e-1
4,3569e-3 1,9494e-1 -9,7583e-2
1,8150e-1 -9,7583e-2 1,0164e+2
m /s =







R .
(4.5)



4.3.3. Insero de rudos de medio

Para testar a robustez dos reconstrutores, so adicionados rudos s medies de
velocidades. Os rudos adicionados so produzidos artificialmente pela funo
randn do Matlab, so normalmente distribudos em torno de zero e possuem
covarincia unitria (
2
1 = ). A adio artificial de rudo feita a cada instante k,
75
adicionando-se velocidade medida
k
v o rudo percentual . .randn()
k r v
p = v ,
onde
r
p uma constante que estabelece a potncia de rudo adicionado e
v
o
desvio padro do erro das medidas de velocidade, admitido gaussiano, e portanto seu
quadrado (
2
v
) determina a potncia de rudo originalmente presente no vetor de
medidas de velocidade
k
v .
Uma vez que a covarincia de
k
v corresponde potncia do rudo adicionado, dada
por
2
.
r v
p , tem-se que
r
p determina a frao percentual do rudo original adicionada
da s medidas de velocidade. Por exemplo, 1
r
p = implica em 100% de potncia de
rudo adicionado.


4.4. Constantes utilizadas

A tabela a seguir apresenta os valores das constantes que foram empregadas nos
testes efetuados.

Tabela 4.2 Constantes utilizadas nos testes de reconstruo de trajetrias
Grandeza Smbolo Valor adotado
Acelerao gravitacional
g
2
9,80665 m/s
Velocidade de rotao da Terra 0,7292e-4 rad/s
Latitude local L
0
23,32
Longitude local
0
46, 43
Raio da Terra
0
R
6378137 m

76
5. RESULTADOS E DISCUSSES

Este captulo apresenta os resultados obtidos nos testes automotivos realizados na
EPUSP. Todos os resultados so apresentados graficamente, sendo que em alguns
casos tambm so apresentadas as distribuies estatsticas atravs de histogramas.
A utilizao de histogramas melhora a interpretao dos resultados onde a anlise
grfica muito carregada, como se ver mais a frente.


5.1. Velocidades angulares, aceleraes e velocidades lineares

Apresentam-se a seguir os dados obtidos atravs de um teste em percurso fechado no
permetro externo da EPUSP, com durao aproximada de 5 minutos e distncia
percorrida de aproximadamente 2800 metros (figura 5.1). As velocidades angulares
e as aceleraes foram extradas da UMI numa taxa nominal de 50 amostras por
segundo (lembrando que o tempo entre uma amostragem e outra apresenta pequenas
flutuaes) e so apresentadas nas direes x, y e z respectivamente. A velocidade
linear foi extrada do velocmetro do veculo numa taxa de 1 amostra a cada 5
segundos. Inicialmente os dados so apresentados no sistema de coordenadas da
plataforma (RPY) e posteriormente so transportados para o sistema de coordenadas
da navegao local (NED).
-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Coordenadas Cartograficas no plano xy - NED
Coordenada X(m)
C
o
o
r
d
e
n
a
d
a

Y

(
m
)

Fig. 5.1 Trajetria para o teste em percurso fechado
77
5.1.1. Sistema de coordenadas da plataforma RPY

As figuras 5.2 a 5.4 ilustram o comportamento da atitude do veculo durante o teste
em trajetria fechada. As velocidades angulares em torno dos eixos x e y (vide figura
5.2) possuem predominncia de componentes de alta freqncia indicando que uma
grande quantidade de movimentos de rolamento e arfagem ocorreram. Estes
resultados esto coerentes com o ensaio efetuado, pois o percurso percorrido
apresenta em toda sua extenso desnivelamentos, lombadas e depresses causando
assim uma grande quantidade de movimentos de roll e pitch respectivamente.

0 50 100 150 200 250 300 350
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
Velocidade angular noeixo X(roll)
tempo(s)
V
e
l
.

A
n
g
u
l
a
r

(
r
a
d
/
s
)

-0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25
0
1000
2000
3000
4000
5000
6000
7000
8000
Histograma davelocidade angular no eixoX(roll)
Velocidade angular (rad/s)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 0.034585
Media = 0.0006822

Fig. 5.2 Velocidade angular no eixo x (roll)



0 50 100 150 200 250 300 350
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
Velocidadeangular no eixoY (pitch)
tempo(s)
V
e
l
.

A
n
g
u
l
a
r

(
r
a
d
/
s
)

-0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4
0
1000
2000
3000
4000
5000
6000
7000
8000
9000
Histogramada velocidadeangular noeixoY (pitch)
Velocidade angular (rad/s)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 0.052608
Media= 0.0011332

Fig. 5.3 Velocidade angular no eixo y (pitch)

78
Os histogramas das figuras 5.2 e 5.3 indicam que os movimentos de rolagem e
arfagem no so normalmente distribudos em torno de zero, e tambm no
apresentam mdia nula, tendendo a favorecer (ou seja, acumular) movimentos
angulares numa nica direo. Provavelmente isto ocorre devido ao efeito dos
amortecedores do veculo, que apresentam diferentes taxas de deslocamentos durante
os movimentos de compresso e distenso.

A velocidade angular em torno do eixo z (figura 5.4) apresenta seis pontos
pronunciados, onde os valores positivos indicam movimento de guinada no sentido
horrio e vice-versa. Estas mudanas de direo esto de acordo com o percurso
descrito pela figura 5.1.

0 50 100 150 200 250 300 350
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
Velocidadeangular noeixoZ(yaw)
tempo(s)
V
e
l
.

A
n
g
u
l
a
r

(
r
a
d
/
s
)

-0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35
0
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
Histograma davelocidadeangular no eixoZ(yaw)
Velocidadeangular (rad/s)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro = 0.058978
Media= 0.020156

Fig. 5.4 Velocidade angular no eixo z (yaw)

As figuras 5.5 a 5.7 descrevem o comportamento das aceleraes que atuam no
veculo. A acelerao no eixo x atinge valores de
2
3 m/s com mdia positiva muito
pequena, como mostra seu histograma. A acelerao no eixo y atinge picos
superiores do eixo x, com mdia negativa, o que indica que os desnivelamentos
causaram movimentos de rolamento e conseqente decomposio da acelerao
gravitacional em y. O grfico da figura 5.5 mostra que, devido ao acoplamento das
aceleraes causados pelos movimentos de roll e pitch, a acelerao no eixo z
oscilou em torno de um valor mdio acima de
2
10 m/s , entretanto seu histograma
indica que a acelerao mdia no eixo z ficou em torno de
2
9, 76 m/s , que
corresponde a aproximadamente a acelerao gravitacional (
2
9, 8 m/s g ).
79
0 50 100 150 200 250 300 350
-4
-3
-2
-1
0
1
2
3
4
Aceleraono eixoX- RPY
tempo(s)
A
c
e
l
e
r
a

o


(
m
/
s
2
)

-4 -3 -2 -1 0 1 2 3 4
0
1000
2000
3000
4000
5000
6000
Histogramada acelerao noeixo X- RPY
Acelerao (m/s
2
)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 0.75246
Media = 0.058465

Fig. 5.5 Acelerao no eixo x



0 50 100 150 200 250 300 350
-4
-3
-2
-1
0
1
2
3
4
Aceleraono eixoY - RPY
tempo(s)
A
c
e
l
e
r
a

o


(
m
/
s
2
)

-4 -3 -2 -1 0 1 2 3 4
0
1000
2000
3000
4000
5000
6000
7000
8000
Histograma daaceleraonoeixo Y - RPY
Acelerao (m/s
2
)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 0.6524
Media = -0.41472

Fig. 5.6 Acelerao no eixo y



0 50 100 150 200 250 300 350
-15
-10
-5
0
Acelerao no eixo Z- RPY
tempo(s)
A
c
e
l
e
r
a

o


(
m
/
s
2
)
-15 -10 -5 0
0
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
Histogramada acelerao noeixo Z- RPY
Acelerao (m/s
2
)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 1.0117
Media= -9.7561
Fig. 5.7 Acelerao no eixo z

80
No sistema de coordenadas da plataforma, a velocidade linear apresentada a seguir
possui componente apenas na direo x, pois o sentido de deslocamento do veculo
est paralelo ao eixo x da UMI

0 50 100 150 200 250 300 350
-10
0
10
20
30
40
50
Velocidade daplataforma(velocimetro) - RPY
tempo(s)
v
e
l
o
c
i
d
a
d
e

(
m
/
s
)

-10 0 10 20 30 40 50
0
500
1000
1500
2000
2500
3000
3500
4000
Histograma davelocidadeda plataforma(velocimetro) - RPY
Velocidade(km/h)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 9.3061
Media= 31.9868

Fig. 5.8 Velocidade no eixo x


5.1.2. Sistema de coordenadas da navegao NED

Nesta seo, as aceleraes e a velocidade linear obtidas anteriormente so
transportadas para o sistema de coordenadas NED. A transformao foi efetuada
utilizando-se ngulos de Euler, segundo a equao 2.5 e os incrementos angulares
foram obtidos integrando-se as velocidades angulares entre duas amostras
consecutivas. No sistema NED, as aceleraes sero utilizadas como entradas para os
algoritmos de reconstruo de trajetrias.

0 50 100 150 200 250 300 350
-4
-3
-2
-1
0
1
2
3
4
5
Aceleraono eixoX- NED
tempo(s)
A
c
e
l
e
r
a

o


(
m
/
s
2
)

-4 -3 -2 -1 0 1 2 3 4 5
0
1000
2000
3000
4000
5000
6000
Histogramada acelerao noeixo X- NED
Acelerao (m/s
2
)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 0.94769
Media= 0.3691

Fig. 5.9 Acelerao no eixo x

81
0 50 100 150 200 250 300 350
-5
-4
-3
-2
-1
0
1
2
3
4
Aceleraono eixoY - NED
tempo(s)
A
c
e
l
e
r
a

o


(
m
/
s
2
)

-5 -4 -3 -2 -1 0 1 2 3 4
0
1000
2000
3000
4000
5000
6000
7000
Histograma daaceleraonoeixo Y - NED
Acelerao (m/s
2
)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 0.87721
Media = -0.8093

Fig. 5.10 Acelerao no eixo y


0 50 100 150 200 250 300 350
-5
0
5
10
Aceleraono eixoZ- NED
tempo(s)
A
c
e
l
e
r
a

o


(
m
/
s
2
)

-5 0 5 10
0
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
Histogramada acelerao noeixo Z- NED
Acelerao (m/s
2
)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro = 1.017
Media= 0.020177

Fig. 5.11 Acelerao no eixo z

Ao transportar as grandezas para o sistema de coordenadas NED, a velocidade linear
que anteriormente possua componentes somente na direo x passa a ter
componentes nas trs direes como mostram as figuras 5.12 a 5.14. Estas
velocidades so utilizadas como medidas de referncia na reconstruo da trajetria.

0 50 100 150 200 250 300 350
-40
-30
-20
-10
0
10
20
30
40
Velocidadeno eixoX- NED
tempo(s)
V
e
l
o
c
i
d
a
d
e


(
K
m
/
h
)

-40 -30 -20 -10 0 10 20 30 40
0
500
1000
1500
2000
2500
3000
3500
4000
Histogramada velocidadenoeixo X- NED
Velocidade (Km/h)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 22.4216
Media= 1.06

Fig. 5.12 Velocidade no eixo x
82
0 50 100 150 200 250 300 350
-40
-30
-20
-10
0
10
20
30
40
50
Velocidadeno eixoY - NED
tempo(s)
V
e
l
o
c
i
d
a
d
e


(
K
m
/
h
)

-40 -30 -20 -10 0 10 20 30 40 50
0
500
1000
1500
2000
2500
3000
Histogramada velocidadenoeixoY - NED
Velocidade (Km/h)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 22.6353
Media= -0.37684

Fig. 5.13 Velocidade no eixo y



0 50 100 150 200 250 300 350
-3
-2
-1
0
1
2
3
4
5
Velocidadeno eixoZ- NED
tempo(s)
V
e
l
o
c
i
d
a
d
e


(
K
m
/
h
)

-3 -2 -1 0 1 2 3 4 5
0
500
1000
1500
2000
2500
3000
3500
4000
4500
Histogramada velocidadenoeixo Z- NED
Velocidade (Km/h)
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 1.7172
Media= 0.70605

Fig. 5.14 Velocidade no eixo z


5.2. Reconstruo da trajetria para o teste em percurso fechado

Apresentam-se a seguir as trajetrias no sistema NED obtidas atravs dos algoritmos
Duplo Integrador e Filtro de Kalman. Ambos os reconstrutores foram
implementados sem a utilizao de medidas de referncia (ou seja, baseando-se
apenas nas medies fornecidas pela UMI) e com a utilizao de medidas de
referncia. Adotaram-se como medidas de referncias as velocidades no sistema
NED e um conjunto de marcas topogrficas landmarks.



83
5.2.1. Reconstruo por dupla integrao e por filtro de Kalman sem a
utilizao de medidas de referncia

A primeira investigao apresenta as trajetrias reconstrudas por dupla integrao
trapezoidal das aceleraes e pelo filtro de Kalman sem utilizar nenhuma medida de
referncia. As aceleraes utilizadas esto no sistema NED.

Analisando os histogramas das aceleraes x e y (figura 5.9 e 5.10) nota-se que as
aceleraes x e y possuem uma predominncia de amostras com valores positivos e
negativos respectivamente, portanto a primeira integrao cumulativa produz
velocidade na direo x com predominncia positiva e velocidade na direo y com
predominncia negativa (figuras 5.14 e 5.15). Integrando-se as velocidades obtm-se
os deslocamentos x e y, onde se observa que o duplo integrador produz uma trajetria
em percurso aberto e no fechado (figura 5.16a).

A trajetria reconstruda pelo filtro de Kalman sem a utilizao de medidas de
referncia tambm apresenta percurso aberto e valores incompatveis com a trajetria
real, (como mostra a figura 5.16b), mas muito semelhante ao percurso obtido pelo
duplo integrador (vide figura 5.17) . Neste processamento, no utilizado o modelo
descrito nas equaes 3.51, com 9 estados, e sim um modelo mais simples com
apenas seis estados (sem a estimao dos erros de acelerao a), uma vez que neste
caso no se dispe de informaes adicionais para estimar a. Alm disso, a matriz
H declarada nula, fazendo com que a matriz de ganho K seja tambm sempre nula,
e portanto os estados estimados deixam de ser corrigidos pelo vetor de entradas z.
Em resumo, as equaes 3.49 e 3.50 foram simplificas por:
2
1
0, 5
k k k k k
T T
+
= + + + p p v a w e
1 k k k k
T
+
= + + v v a w respectivamente.

84

0 50 100 150 200 250 300 350
-200
-100
0
100
200
300
400
500
Velocidadeno eixoX- NED
tempo(s)
V
e
l
o
c
i
d
a
d
e


(
k
m
/
h
)

0 50 100 150 200 250 300 350
-1000
-900
-800
-700
-600
-500
-400
-300
-200
-100
0
Velocidade noeixo Y - NED
tempo(s)
V
e
l
o
c
i
d
a
d
e


(
k
m
/
h
)

Fig. 5.15 Velocidades x e y obtidas por integrao


-6000 -5000 -4000 -3000 -2000 -1000 0 1000 2000 3000 4000
-6
-5
-4
-3
-2
-1
0
1
x 10
4
TrajetoriaXY - NED
coordenada x (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Erro Final = 53212,6607metros
Duplo Integrador

(a)
-6000 -5000 -4000 -3000 -2000 -1000 0 1000 2000 3000 4000
-6
-5
-4
-3
-2
-1
0
1
x 10
4
Trajetria(X,Y) NED- "Filtro deKalman"
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
ErroFinal = 53208,2683metros
FiltrodeKalman
(b)

Fig. 5.16 Trajetrias reconstrudas por dupla integrao e por filtro de Kalman sem
utilizar medidas de referncia

A figura 5.16 mostra que nenhum dos reconstrutores apresentou resultados
compatveis com a trajetria real (figura 5.1), indicando que as informaes geradas
pela UMI no so precisas o suficiente para reconstruir a trajetria. Para cada
reconstrutor tambm so apresentados os erros finais da trajetria reconstruda. No
plano xy e no eixo z, os erros finais de reconstruo so definidos respectivamente
por
2 2
( ) ( )
xy f i f i
c cx cx cy cy = + e
z f i
c cz cz = . Como a trajetria real
fechada, esses valores idealmente seriam nulos.

85
Analisa-se o erro no plano xy (
xy
c ) em separado do erro na coordenada z (
z
c ),
porque este ltimo tende a crescer muito devido dinmica dos amortecedores, que
influenciam as medidas do giroscpio de pitch, como ser visto adiante. A
separao entre
xy
c e
z
c evita que erros elevados no eixo z venham a mascarar o
erro final da trajetria reconstruda no plano.

-6000 -5000 -4000 -3000 -2000 -1000 0 1000 2000 3000 4000
-6
-5
-4
-3
-2
-1
0
1
x 10
4
Trajetria (X,Y) NED- "Filtro de Kalman x Duplo Integrador"
coordenada x (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Filtro de Kalman
Duplo Integrador

Fig. 5.17 Comparao entre os reconstrutores


5.2.2. Reconstruo por dupla integrao e por filtro de Kalman com a
utilizao de medidas de referncia

Apresentam-se as trajetrias reconstrudas com a utilizao da velocidade como
medidas de referncia. Para o duplo integrador as medidas de referncia so
utilizadas a cada iterao no termo
k
v da equao
2
1
0, 5
k k k k
T T
+
= + + p p v a ,
enquanto que para o filtro de Kalman as medidas de referncia so utilizadas no
termo
k
z da equao ( )
k k k k k

= + x x K z Hx . A figura 5.18 ilustra as trajetrias
reconstrudas pelo duplo integrador e pelo filtro de Kalman. Nestas trajetrias, fica
evidente que somente a incluso da velocidade como medida de referncia melhora
drasticamente o desempenho do sistema, e que sem isso o processo da navegao
fica invivel.


86
-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetoria(X,Y) - "Real x Duplo Integrador" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Erro Final = 107.9952metros
Real
DuploIntegrador

-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetria(X,Y) NED- "Real x FiltrodeKalman" - NED
coordenada x (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
ErroFinal = 97.9873 metros
Real
FiltrodeKalman

Fig. 5.18 Trajetrias reconstrudas por dupla integrao e por filtro de Kalman com
medidas de referncia da velocidade

A seguir apresenta-se a comparao entre os reconstrutores, e tambm o
comportamento do deslocamento no eixo z ao longo do percurso. Note que o
deslocamento no eixo z no volta para a origem, indicando que o filtro no atuou
satisfatoriamente nesta direo. A causa provvel para esta ocorrncia seria a atuao
do giroscpio y (pitch) que atua de forma assimtrica durante as freadas e
arrancadas do veculo. Como conseqncia disso a velocidade no eixo z no foi
corretamente transportada do sistema RPY para o sistema NED.

-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetoria(X,Y) - "Real x FiltrodeKalmanx DuploIntegrador " - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Real
FiltrodeKalman
Duplo Integrador

(a)
0 50 100 150 200 250 300 350
-10
0
10
20
30
40
50
60
70
Trajetria(Z) estimada pelofiltro deKalman - NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)
Erro Final = 60.7034metros

(b)
Fig. 5.19 Comparao entre os reconstrutores (a) - Deslocamento no eixo z (b)


5.2.3. Desempenho dos reconstrutores na presena de rudos de medio

A seguir so apresentados os resultados obtidos pelo duplo integrador e pelo filtro de
Kalman, quando se aplica um rudo aleatrio s medidas de velocidade. Os rudos
87
foram produzidos artificialmente pela funo randn do Matlab e inseridos no
vetor de velocidades NED. Foram executadas quatro simulaes nas quais as
potncias dos rudos inseridos correspondem respectivamente a 50%, 100%, 150% e
200%, do valor da covarincia do erro de medio R . Nestas simulaes, nota-se que
o filtro de Kalman combinou satisfatoriamente as medidas de referncias ruidosas,
produzindo resultados superiores ao duplo integrador (vide figuras 5.20 a 5.24).
Apesar do erro final observado nos dois mtodos serem prximos, a trajetria
reconstruda pelo filtro de Kalman adere melhor trajetria real.

-600 -500 -400 -300 -200 -100 0 100 200 300 400 500
-600
-400
-200
0
200
400
600
Trajetoria(X,Y) - "Real x FiltrodeKalmanx DuploIntegrador " - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Erro Final Kalman = 113,3521 metros
ErroFinal DuploIntegrador = 126,7882metros
Real
FiltrodeKalman
Duplo Integrador

(a)
0 50 100 150 200 250 300 350
-10
0
10
20
30
40
50
60
70
Trajetria(Z) estimada pelofiltro deKalman - NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)
Erro Final = 66,9468metros

(b)

Fig. 5.20 Comparao entre os reconstrutores na presena de rudo de medio
Primeira simulao: potncia de rudo = 50% do rudo de medio R


-600 -500 -400 -300 -200 -100 0 100 200 300 400 500
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetoria(X,Y) - "Real x FiltrodeKalmanx DuploIntegrador " - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
ErroFinal Kalman= 105,8379metros
ErroFinal DuploIntegrador = 119,7476metros
Real
FiltrodeKalman
Duplo Integrador

(a)
0 50 100 150 200 250 300 350
-20
0
20
40
60
80
100
120
Trajetria(Z) estimada pelofiltro deKalman - NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)
ErroFinal = 111,4169metros

(b)

Fig. 5.21 Comparao entre os reconstrutores na presena de rudo de medio
Segunda simulao: potencia de rudo = 100% do rudo de medio R


88

-600 -500 -400 -300 -200 -100 0 100 200 300 400 500
-600
-400
-200
0
200
400
600
Trajetoria(X,Y) - "Real x FiltrodeKalmanx DuploIntegrador " - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
ErroFinal Kalman= 56,0546metros
ErroFinal DuploIntegrador = 65,8552 metros
Real
FiltrodeKalman
Duplo Integrador

(a)
0 50 100 150 200 250 300 350
0
20
40
60
80
100
120
140
160
Trajetria (Z) estimadapelo filtrode Kalman- NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)
ErroFinal = 155,5671metros

(b)

Fig. 5.22 Comparao entre os reconstrutores na presena de rudo de medio
Terceira simulao: potncia de rudo = 150% do rudo de medio R


-600 -500 -400 -300 -200 -100 0 100 200 300 400 500
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetoria(X,Y) - "Real x FiltrodeKalmanx DuploIntegrador " - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
ErroFinal Kalman= 132,6662metros
ErroFinal DuploIntegrador = 141,0986metros
Real
FiltrodeKalman
Duplo Integrador

(a)
0 50 100 150 200 250 300 350
0
50
100
150
200
250
Trajetria (Z) estimadapelo filtrode Kalman- NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)
ErroFinal = 230,2261metros

(b)

Fig. 5.23 Comparao entre os reconstrutores na presena de rudo de medio
Quarta simulao: potncia de rudo = 200% do rudo de medio R


5.3. Anlise do efeito da deriva dos giroscpios

A seguir apresenta-se um teste em trajetria aberta e aproximadamente reta, cujo
objetivo investigar o efeito da deriva dos giroscpios. A figura 5.24 mostra a
trajetria reconstruda pelo duplo integrador. Note que durante o trecho em que a
trajetria real reta, o reconstrutor acompanha o percurso sem sofrer desvio
significativo. Entretanto, quando se inicia um movimento de guinada, a trajetria
reconstruda pelo duplo integrador passa a acumular um desvio angular sobre o eixo
89
z. Este desvio cumulativo um indcio de que o giroscpio z (guinada) aps ser
submetido a uma taxa de rotao significativa pela primeira vez, passa a apresentar
deriva.

0 100 200 300 400 500 600 700 800 900
-400
-350
-300
-250
-200
-150
-100
-50
0
50
100
Trajetoria (X,Y) - "Real x Duplo Integrador" - NED
coordenada x (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Real
Duplo Integrador

Fig. 5.24 Trajetrias reconstrudas em percurso aberto


Alm do efeito da deriva, os giroscpios tambm apresentam um sinal aleatrio de
valor constante denominado bias. Este sinal tambm se soma deriva contribuindo
para o desvio angular durante a reconstruo da trajetria. A figura a seguir ilustra o
incremento angular produzido pelo bias dos giroscpios durante aproximadamente
trs minutos. Estes grficos foram obtidos a partir de testes consecutivos com a UMI
em repouso sobre uma base nivelada.

0 20 40 60 80 100 120 140 160 180 200
-4
-2
0
2
4
6
8
10
12
Incremento angular nos eixos x,y e z (Bias dos giros)
tempo(s)
A
n
g
u
l
o

(
g
r
a
u
s
)
Giro X
Giro Y
Giro Z

0 20 40 60 80 100 120 140 160 180 200
-4
-2
0
2
4
6
8
10
Incremento angular nos eixos x,y e z (Bias dos giros)
tempo(s)
A
n
g
u
l
o

(
g
r
a
u
s
)
Giro X
Giro Y
Giro Z

Fig. 5.25 Incremento angular devido ao bias de giroscpio

90
Analisando as figuras 5.21 e 5.22, conclui-se que os erros dos giroscpios
contribuem fortemente para inviabilizar a navegao inercial atravs da UMI apenas.
Essa concluso corroborada na prxima seo, quando se observa que a incluso de
landmarks nas curvas melhora significativamente a reconstruo da trajetria,
atenuando o efeito da deriva dos giroscpios.


5.4. Utilizao de marcas topogrficas para auxiliar o filtro de Kalman

Esta seo investiga a contribuio obtida na reconstruo da trajetria ao se
empregar um conjunto de marcas topogrficas landmarks no filtro de Kalman,
conjuntamente com a velocidade.

O critrio adotado para inserir as marcas topogrficas fundamenta-se na monitorao
da matriz de covarincia do erro P. Nesta matriz, os trs primeiros elementos da
diagonal principal so as covarincias dos erros de deslocamento x, y e z
respectivamente. Neste sentido, admitindo-se que
x
,
y
e
z
sejam os desvios
padro nas trs direes, a norma
2 2 2
c x y z
= + + fornece uma medida da
incerteza da estimativa de posio. Portanto, ao limitar o crescimento da covarincia
do erro a um valor arbitrrio representado por
c
, estatisticamente tem-se que, em
99% das ocorrncias, a posio estimada em um ponto encontra-se entre 3
c
da
trajetria real. A insero de landmarks feita atravs da matriz H na equao de
medidas de referncia
k k k
= + z Hx Ge (vide equao 3.53).

As figuras 5.26 a 5.28 ilustram as trajetrias reconstrudas e os landmarks inseridos
para valores de
c
= 1m,
c
= 2m e
c
= 3m respectivamente. Nestas figuras
tambm so apresentados os histricos temporais da covarincia do erro de posio e
o deslocamento no eixo z.



91


-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetria (X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Quantidade deLandmarks = 124
Real
FiltrodeKalman
Landmarks
ErroFinal = 0,4532 metros

(a)
-505.5 -505 -504.5 -504 -503.5 -503 -502.5 -502 -501.5
-39.5
-39
-38.5
-38
-37.5
-37
-36.5
Trajetria (X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Real
Filtrode Kalman
Landmark

(b)

0 50 100 150 200 250 300 350
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
1.5
Historico dacovarianciado erro deposicao - NED
tempo(s)
N
o
r
m
a

d
a

C
o
v
a
r
i
a
n
c
i
a

(c)
0 50 100 150 200 250 300 350
-10
-8
-6
-4
-2
0
2
4
Trajetria (Z) estimadapelo filtrode Kalman- NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)

(d)

Fig. 5.26 - Teste para = 1m.
(a) Trajetria e landmarks (b) Trecho ampliado
(c) Variao de
c
(d) Deslocamento no eixo z


Para
c
= 1m, o desvio entre a trajetria real e estimada ficou em aproximadamente
2,9 m, correspondendo a expectativa de 3
c
. Alm disso, o crescimento do erro de
deslocamento no eixo z foi atenuado significativamente. Em contra-partida, a
quantidade de landmarks inseridos relativamente elevada, correspondendo a uma
taxa de aproximadamente 1 landmark a cada 25,6 metros de deslocamento linear.


92


-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetria (X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Quantidade deLandmarks = 24
Real
FiltrodeKalman
Landmarks
Errofinal = 16,1250 metros

(a)
-338 -337 -336 -335 -334 -333 -332 -331 -330 -329 -328
287
288
289
290
291
292
293
294
295
296
Trajetria (X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Real
Filtrode Kalman
Landmark

(b)

0 50 100 150 200 250 300 350
0.6
0.8
1
1.2
1.4
1.6
1.8
2
2.2
Historico dacovarianciado erro deposicao - NED
tempo(s)
N
o
r
m
a

d
a

C
o
v
a
r
i
a
n
c
i
a

(c)
0 50 100 150 200 250 300 350
-6
-5
-4
-3
-2
-1
0
1
2
3
4
Trajetria (Z) estimadapelo filtrode Kalman- NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)

(d)

Fig. 5.27 - Teste para = 2m
(a) Trajetria e landmarks (b) Trecho ampliado
(c) Variao de (d) Deslocamento no eixo z


Para
c
= 2m, o erro entre a trajetria real e estimada ficou em aproximadamente
5,7 m (mantendo-se portanto dentro da faixa 3
c
. O crescimento do erro de
deslocamento no eixo z foi novamente atenuado, e a quantidade de landmarks
inseridos foi reduzida a uma taxa de aproximadamente 1 landmarks a cada 116,7
metros. Entretanto, com a reduo de landmarks, dois pontos da trajetria
apresentaram desvios muito elevados (acima de 50 m).
93

-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetria (X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Quantidadede Landmarks = 10
Real
FiltrodeKalman
Landmarks
Erro Final = 23,266metros

(a)
33 34 35 36 37 38 39 40 41 42 43
-438
-436
-434
-432
-430
-428
-426
Trajetria(X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Real
Filtrode Kalman
Landmark

(b)

0 50 100 150 200 250 300 350
0.5
1
1.5
2
2.5
3
3.5
Historico dacovarianciado erro deposicao - NED
tempo(s)
N
o
r
m
a

d
a

C
o
v
a
r
i
a
n
c
i
a

(c)
0 50 100 150 200 250 300 350
-4
-3
-2
-1
0
1
2
3
Trajetria (Z) estimadapelo filtrode Kalman- NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)

(d)

Fig. 5.28 - Teste para = 3m
(a) Trajetria e landmarks (b) Trecho ampliado
(c) Variao de
c
(d) Deslocamento no eixo z


Para
c
= 3m, o erro entre a trajetria real e estimada ficou em aproximadamente
8,95 m, mantendo-se novamente na faixa de 3
c
. O crescimento do erro de
deslocamento no eixo z continua sendo atenuado, porm com menor eficincia, e a
quantidade de landmarks inseridos foi reduzida a uma taxa de aproximadamente 1
landmarks a cada 280 metros. A reduo excessiva de landmarks mostra
entretanto que a trajetria reconstruda apresenta desvios elevados em relao
trajetria real.
94
Analisando os trs testes anteriores, observou-se que o critrio adotado levou a uma
concentrao de insero de landmarks nas curvas. Observou-se tambm que, para
os testes que utilizam a velocidade como medidas de referncia, ocorre um aumento
da deriva de giroscpio quando a plataforma apresenta movimento de guinada, ou
seja, quando passa por uma curva. Baseado nestas observaes, uma nova
investigao foi implementada, priorizando a minimizao da incerteza gerada pelos
giroscpios nas curvas. Neste novo teste foram inseridos 10 landmarks sendo: 1 no
inicio, 1 no fim e 8 nas curvas como mostra a figura 5.29a.

-600 -500 -400 -300 -200 -100 0 100 200 300 400
-500
-400
-300
-200
-100
0
100
200
300
400
500
Trajetria (X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Quantidade deLandmarks = 10
Real
Filtrode Kalman
Landmark

(a)
10 20 30 40 50 60
-445
-440
-435
-430
-425
-420
-415
-410
-405
Trajetria (X,Y) NED- "Real x Filtro deKalman" - NED
coordenadax (m)
c
o
o
r
d
e
n
a
d
a

y

(
m
)
Real
Filtrode Kalman
Landmark

(b)

0 50 100 150 200 250 300 350
0
5
10
15
20
25
Historicodacovariancia doerro demedicao- NED
tempo(s)
N
o
r
m
a

d
a

C
o
v
a
r
i
a
n
c
i
a

(c)
0 50 100 150 200 250 300 350
-30
-20
-10
0
10
20
30
40
50
Trajetria (Z) estimadapelo filtrode Kalman- NED
tempo(s)
c
o
o
r
d
e
n
a
d
a

z

(
m
)

(d)

Fig. 5.29 - Teste para landmarks inseridos nas curvas
(a) Trajetria e landmarks (b) Trecho ampliado
(c) Variao de
c
(d) Deslocamento no eixo z


95
O resultado deste experimento mostra que a trajetria reconstruda aproxima-se mais
da trajetria real se comparado ao teste anterior, e que o desvio observado na mesma
regio foi reduzido para aproximadamente 20 m, como mostra a figura 5.26b.


5.5. Monitoramento do filtro de Kalman

Nesta seo, verifica-se se o filtro de Kalman est funcionando adequadamente,
aplicando as tcnicas descritas na seo 4.3.1, em Materiais e Mtodos. O
monitoramento do filtro feito atravs da anlise da matriz de covarincia do erro P
e da checagem da mdia das inovaes ou resduos ( )
k k k

z H x .

Apresentam-se a seguir os resultados obtidos a partir dos testes efetuados em
percurso fechado e aberto, nos quais utilizaram-se como medidas de referncia
somente a velocidade e a velocidade + landmarks.

0 50 100 150 200 250 300 350
0
10
20
30
40
50
60
70
80
Historicodacovariancia doerro demedicao- NED
tempo(s)
N
o
r
m
a

d
a

C
o
v
a
r
i
a
n
c
i
a

(a)
-40 -30 -20 -10 0 10 20 30 40 50
0
1000
2000
3000
4000
5000
6000
Historicodainovao (residuo)
Erro deinovao
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro= 10.266
Media = 0.017701

(b)

Fig. 5.30 Monitoramento do filtro de Kalman Percurso fechado
(a) Teste de covarincia do erro (b) Teste de inovao
medida de referncia: velocidade
96
0 50 100 150 200 250 300 350
0
5
10
15
20
25
Historicodacovariancia doerro demedicao- NED
tempo(s)
N
o
r
m
a

d
a

C
o
v
a
r
i
a
n
c
i
a

(a)
-50 -40 -30 -20 -10 0 10 20 30 40 50
0
1000
2000
3000
4000
5000
6000
Historicodainovao (residuo)
Erro deinovao
Q
u
a
n
t
i
d
a
d
e

d
e

a
m
o
s
t
r
a
s
Desv. Padro = 10.2781
Media= -0.0013968

(b)

Fig. 5.31 Monitoramento do filtro de Kalman Percurso fechado
(b) Teste de covarincia do erro (b) Teste de inovao
medida de referncia: velocidade + landmarks


As figuras 5.28 a 5.31 mostram que a covarincia do erro aumenta com o tempo para
os testes onde se utiliza somente a velocidade como medida de referncia e
atenuada quando se utiliza velocidade + landmarks como medidas de referncia.
Quanto ao teste que monitora a seqncia das inovaes, observa-se que todos os
histogramas possuem uma distribuio muito parecida com uma distribuio
gaussiana de mdia zero, o que indica que o filtro est trabalhando corretamente.
97
6. CONCLUSES E RECOMENDAES


O principal objetivo deste trabalho foi desenvolver e testar um sistema de navegao
inercial para aplicaes terrestres baseado em uma UMI strapdown de baixo custo,
empregando tcnicas de fuso sensorial com leituras adicionais de um velocmetro e
landmarks. Embora no seja indita, esta pesquisa conduziu as seguintes
contribuies:

Realizou-se uma extensiva reviso bibliogrfica sobre navegao inercial
terrestre, consolidando-se diversos modelos matemticos e equacionamentos.

Foi modelado e implementado um filtro de Kalman com nove estados, sendo que
os resultados mostraram-se satisfatrios em se tratando de uma UMI de baixo
custo.

Baseado no desenvolvimento e nos testes conduzidos neste trabalho apresentam-se a
seguir as seguintes concluses e recomendaes:

A UMI de baixo custo utilizada no permite por si s a reconstruo das
trajetrias ensaiadas de forma minimamente satisfatria, devido principalmente
baixa preciso de seus giroscpios e acelermetros.

No entanto, a utilizao de medidas de velocidade como leituras externas
auxiliares melhora drasticamente o desempenho dos reconstrutores
desenvolvidos.

O reconstrutor baseado no filtro de Kalman superior ao duplo integrador pois,
alm de mostrar-se mais robusto na presena de rudos aleatrios, permite ainda
implementar a fuso sensorial de forma bastante flexvel, na medida em que
permite incorporar medidas fornecidas a intervalos irregulares por sensores
adicionais.
98

Como era de se esperar, a utilizao de landmarks diminui sensivelmente o
erro de reconstruo da trajetria, mostrando-se superior em relao aquela que
utiliza apenas a velocidade como medida de referncia.

Investigou-se um critrio para estabelecer a quantidade de landmarks
necessria para uma desejada preciso de reconstruo, monitorando-se a matriz
de covarincia P. A aplicao desse critrio levou a resultados coerentes, em
que a distncia mdia entre landmarks diminui com o aumento da preciso
estipulada, o que valida o critrio proposto.

A deriva e o bias dos giroscpios afetam sensivelmente a acelerao e
principalmente as matrizes que efetuam a transformao de coordenadas da
acelerao e da velocidade, influenciando diretamente na reconstruo da
trajetria. Neste sentido, deve-se investigar o emprego de sensores para medir a
orientao da plataforma, tais como giroscpios adicionais (mais precisos),
inclinmetros, bssolas, etc.

Verificou-se que a impreciso dos giroscpios pde ser compensada com
sucesso, incluindo-se estrategicamente landmarks nas curvas do trajeto. Abre-
se assim uma possvel frente de pesquisa para a determinao de pontos timos
de insero de landmarks, com base nas curvas da trajetria.

Baseado na experincia adquirida pelo autor ao longo da execuo deste trabalho, o
mesmo julga importante fazer outras recomendaes alm daquelas expostas
anteriormente :

Outras tcnicas que verifiquem a divergncia do filtro de Kalman devem ser
implementadas a fim de aumentar a confiabilidade das estimativas. Alm disso,
rudos coloridos e no gaussianos devem ser abordados e testados com o filtro,
pois esta prtica pode reduzir a possibilidade da instabilidade e divergncia do
filtro, que geralmente ocorre quando o sistema mal modelado.
99

Para minimizar o efeito produzido pelo bias de giroscpio, recomenda-se
tambm empregar as tcnicas de ortogonalizao e normalizao para evitar a
propagao de erro das matrizes que efetuam as transformaes de coordenadas.

Outros modelos devem ser testados, incluindo-se o modelo que emprega o efeito
do bias de giroscpio, o qual foi desenvolvido neste trabalho. No entanto, para
empregar este modelo, deve-se dispor de equipamentos e mtodos para efetuar
com preciso a calibrao e o alinhamento inicial.

Desenvolver um sistema de aquisio de dados mais elaborado que possibilite
efetuar a aquisio de dados de forma sincronizada, e conseqentemente inferir
medidas mais significativas e confiveis no vetor de medies do filtro de
Kalman.

Testar o modelo e algoritmos utilizados neste trabalho em aplicaes com
diferentes faixas de acelerao, velocidade e mudana de orientao.

Entre os vrios tipos de sensores que podem ser utilizados como medidas de
referncia para aumentar o sistema de fuso sensorial, a utilizao do GPS
promissora, pois este equipamento permite obter dados de velocidade e posio
cada vez mais precisos, a um custo acessvel.
100

ANEXOS



A. O filtro de Kalman discreto

A.1. Introduo

O filtro de Kalman essencialmente um conjunto de equaes matemticas que
implementam um estimador de estados conhecido como preditor-corretor. Quando
algumas condies so satisfeitas, o filtro de Kalman considerado um estimador
timo e minimiza a covarincia do erro estimado. Desde sua introduo, o filtro de
Kalman objeto de extensas pesquisas e aplicaes, particularmente na rea de
navegao autnoma ou assistida. Isto talvez se deva em grande parte aos avanos da
computao digital, que torna o uso do filtro aplicvel na prtica, e tambm a sua
simplicidade e robustez. Raramente as condies necessrias para robustez e
otimalidade so encontradas em aplicaes prticas, e mesmo assim o filtro ainda
opera de forma bastante satisfatria.


A.2. O Filtro de Kalman Discreto

Esta seo descreve o filtro discreto de Kalman baseado na sua formulao do artigo
(Kalman, 1960) onde as ocorrncias de medies e estados so estimados em
instantes discretos de tempo.

O filtro de Kalman empregado no sentido de tentar estimar o estado
n
x R de um
processo controlado em instantes discretos de tempo, que governado por uma
equao de diferenas linear estocstica


1 +
= + +
k k k k
x Ax Bu w ,
(A.1)


101
E uma equao de medio
m
z R que dada por


k k k
z Hx v = + .
(A.2)

As variveis aleatrias
k
w e
k
v representam os rudos do processo e da medio
respectivamente. Considera-se que estas variveis so rudos brancos, independentes
e com distribuio normal de probabilidade dada por:

( ) (0, ) p w N Q ,
(A.3)

( ) (0, ) p v N R .
(A.4)

Na prtica, as matrizes que representam a covarincia Q do rudo do processo e a
covarincia R do rudo de medio podem mudar a cada instante de tempo ou a cada
medio. Entretanto, assume-se por ora que as mesmas so constantes.

A matriz
n n
A

na equao de diferenas A.1 relaciona o estado no instante de tempo
k (anterior) ao estado no instante de tempo 1 k + (presente). Note que, na prtica, A
pode mudar a cada instante de tempo; porm aqui A considerada constante. A
matriz
n p
B relaciona a entrada de controle
p
u R (opcional) ao estado x . A matriz
m n
H na equao de medio (A.2) relaciona o estado x a medio
k
z . Na prtica H
pode mudar a cada instante de tempo ou medio, porm aqui H considerada
constante.


A.3. A formulao computacional do filtro

Define-se
n
k
x R

(note o sobrescrito - ) como uma estimativa de estado a priori

no instante de tempo k, conhecendo-se o processo anterior ao instante de tempo k, e

n
k
x R como uma estimativa de estado a posteriori no instante de tempo k dada a
medio z
k
. Pode ento se definir estimativas de erros a priori e a posteriori como

102

k k k
e x x

, (A.5)

k k k
e x x . (A.6)

A estimativa da covarincia do erro a-priori dada ento por


[ ]
T
k k k
P E e e

= , (A.7)


[ ]
T
k k k
P E e e = . (A.8)

Ao desenvolver as equaes para implementar o filtro de Kalman, o objetivo
encontrar uma equao que compute uma estimativa de estado a posteriori
k
x , como
uma combinao linear de uma estimativa de estado a priori
k
x

, uma diferena
proporcional entre a medio atual z
k
e uma predio de medio
k
Hx

, como mostra
a equao A.9.


( )
k k k k
x x K z Hx

= + (A.9)

A diferena ( )
k k
z Hx

na equao A.9 chamada de inovao da medio ou

resduo. O resduo reflete a discrepncia entre a medio predita
k
Hx

e a medio
atual z
k.


A matriz K
(nxm)
na equao A.9 o fator de ganho que minimiza a covarincia do
erro a posteriori da equao A.8 (para maiores detalhes ver Brown e Hwang 1996).
Uma forma de obter o ganho K que minimiza a equao A.8 dada por


1
( )
T T
k k
K P H HP H R

= + , (A.10)

Ou na forma escalar por


T
k
T
k
P H
K
HP H R

=
+
.
(A.11)
103

Analisando a equao A.11, nota-se que na medida em que a covarincia do erro de
medio R se aproxima de zero, o ganho K atua mais sobre o resduo, ou seja:


1
0
lim
k
k
R
K H

= .
(A.12)

Por outro lado, na medida em que a covarincia do erro estimado a priori
k
P

se
aproxima de zero, o ganho K atua menos, sobre o resduo, ou seja:


0
lim 0
k
k
P
K

= .
(A.13)

Outra forma de se pensar em como K atua sobre o sistema que, quando a
covarincia do erro de medio R se aproxima de zero, a medio atual z
k
torna-se
mais confivel do que a medio predita
k
Hx

. Por outro lado, na medida em que a

covarincia do erro estimado a priori
k
P

se aproxima de zero, a medio predita

k
Hx

torna-se mais confivel do que os valores efetivamente medidos z

k
.


A.4. A formulao probabilstica do filtro

A justificativa para a equao A.9 tem origem na probabilidade da estimativa a
priori
k
x

condicionada a todas as medidas anteriormente obtidas (regra de Bayes).

Por enquanto suficiente pensar que o filtro de Kalman mantm os dois primeiros
momentos da distribuio de estados

[ ]
k k
E x x = ,
(A.14)


[( )( ) ]
T
k k k k k
E x x x x P = . (A.15)

A equao do estado estimado a posteriori (A.9), reflete a mdia (primeiro
momento) da distribuio de estado ela normalmente distribuda se as condies
104
da equao A.3 e A.4 forem satisfeitas. A equao A.8, da covarincia do erro
estimado a posteriori, reflete a varincia da distribuio de estado (segundo
momento). Em outra palavras:


( | ) ( [ ], [( )( ) ])
T
k k k k k k k
p x z N E x E x x x x , (A.16)

( | ) ( , )
k k k k
p x z N x P = .
(A.17)

Para maiores detalhes sobre a formulao probabilstica do filtro de Kalman,
consulte (Brown, 1997).


A.5. O algoritmo do filtro de Kalman discreto

O filtro de Kalman estima um processo utilizando uma forma de controle por
realimentao o filtro estima o estado do processo em algum instante de tempo e
ento obtm a realimentao (estimao do rudo) na forma de medies. Assim, as
equaes para o filtro de Kalman so divididas em dois grupos, atualizao temporal
(predio) e atualizao da medio (correo). As equaes de atualizao temporal
so responsveis em projetar a frente (no tempo) o estado corrente e a estimativa da
covarincia do erro para obter uma estimava do erro a priori para o prximo instante
de tempo (lembre-se que as atualizaes ocorrem em instantes discretos de tempo).
As equaes para atualizao da medio so responsveis pela realimentao, ou
seja, elas incorporam uma nova medio ao estado estimado a priori para obter uma
estimativa melhorada a posteriori.





105
Atualizao Temporal
Predio
Atualizao da Medio
Correo


Fig. A.1 O ciclo contnuo do filtro de Kalman discreto.


Na figura A.1, a atualizao temporal projeta o estado estimado corrente (estado
atual) frente em instantes discretos de tempo. A atualizao da medio ajusta a
estimativa projetada atravs de uma medio atual no mesmo instante de tempo

As equaes de atualizao temporal do filtro de Kalman discreto so:


1

k k k
x Ax Bu

= + (A.18)


1
T
k k
P AP A Q

= + (A.19)

Note que as equaes de atualizao temporal projetam as estimativas de estado e
covarincia frente a partir do instante de tempo 1 k ao instante de tempo k .

As equaes de atualizao da medio do filtro de Kalman discreto so:


1
( )
T T
k k k
K P H HP H R

= + (A.20)


( )
k k k k k
x x K z Hx

= + (A.21)


( )
k k k
P I K H P

= (A.22)

O primeiro passo da atualizao da medio computar o ganho do filtro de Kalman
(
k
K ). O prximo passo atualizar a medio do processo para obter
k
x , e ento
106
gerar uma estimativa do estado a posteriori atravs da incorporao de uma medio.
O passo final obter uma estimativa da covarincia do erro a posteriori.
Aps cada ciclo de atualizao temporal e atualizao da medio, o processo
repetido atravs da estimativa a posteriori anterior que utilizada para projetar ou
predizer uma nova estimativa a priori. Esta natureza recursiva uma das mais
agradveis caractersticas do filtro de Kalman, pois elas permitem implementaes
prticas computacionalmente muito simples. A figura A.2 mostra como funciona o
ciclo de operao do filtro de Kalman discreto.


(1) Projeta o estado a frente
(2) Projeta a covarincia do erro a frente
(1) Computa o ganho K do filtro de Kalman
(2) Atualiza a estimativa com a medio
k
z
(3) Atualiza a covariancia do erro
( )
k k k k k
x x K z Hx

= +
1

k k k
x Ax Bu

= +
1
T
k k
P AP A Q

= +
1
( )
T T
k k k
K P H HP H R

= +
( )
k k k
P I K H P

=
Atualizao Temporal (Predio)
Atualizao da Medio (Correo)
Entra com as estimativas
iniciais
1 1

k k
x e P



Fig. A.2 - O ciclo de operao do filtro de Kalman discreto


A.6. Exemplo de aplicao: Estimando uma constante aleatria

Na seo anterior foi apresentada a forma discreta do filtro de Kalman linear. Para
que se possa entender melhor o funcionamento do filtro de Kalman, esta seo
apresenta um exemplo de aplicao bastante simples.


107
A.6.1. O modelo do processo

Neste exemplo simples o objetivo tentar estimar uma constante escalar aleatria
(uma tenso, por exemplo). Assume-se que se pode medir a tenso (constante
aleatria) e que a mesma esta corrompida por um rudo branco de 0,1 V
RMS
(pense
que o conversor A/D do voltmetro no possui uma preciso adequada medio).
Assim o processo pode ser modelado pela seguinte equao de diferenas linear
estocstica


1 k k k k
x Ax Bu w

= + +


1 k k k
x Ax w

= + ,

E a medio
m
z R pode ser modelada por


k k k
z Hx v = +


k k k
z x v = + .

Como o estado a ser estimado no muda a cada instante de tempo, e o mesmo um
escalar, tem-se que, A = 1. Dado que no existe uma entrada de controle, tem-se que
0
k
= u . A medio depende diretamente do estado
k
x , portanto 1 = H .


A.6.2. Equaes e parmetros do filtro

As equaes de atualizao temporal (predio) so:


1

k k
x x

=


1 k k
P P Q

= +

As equaes de atualizao da medio (correo) so:
108


1
( )
k k k
K P P R

= +


k
k
k
P
K
P R

=
+



( )
k k k k k
x x K z x

= +


(1 )
k k k
P K P

=

Assume-se que a varincia do processo seja muito pequena, Q = 10
-5
(certamente
poderia se admitir Q = 0 , porm assumindo que Q muito pequeno e Q > 0, tem-se
uma flexibilidade maior na sintonia do filtro, como ser mostrado adiante). Assume
ainda que por experincia se conhece que o valor verdadeiro da constante aleatria
possui uma distribuio padro normal de probabilidade, ento admite-se que o valor
inicial da constante zero, em outras palavras, assume-se o valor da estimativa
inicial
1
0
k
x

= .

Similarmente escolha de
1

k
x

, tambm necessrio escolher o valor inicial de
1 k
P


(admita que
1 0 k
P P

= ). Se houver certeza absoluta que a estimativa para o estado

inicial seja
1
0
k
x

= , pode-se admitir ento que
0
0 P = . No entanto se a estimativa
para o estado inicial for
1
0
k
x

, escolher
0
0 P = poder fazer com que o filtro
estime inicialmente e para sempre 0
k
x = . Para contornar este problema, escolhe-se
0
0 P . Inicialmente ser escolhido
0
1 P = .


A.6.3. Simulaes e Resultados

Inicialmente escolheu-se uma constante escalar z = 0,5287 (veja que z no possui o
acento de circunflexo porque ela representa o valor verdadeiro e no o valor
estimado). Foram simuladas 50 medies distintas z
k
que possui erro normalmente
distribudo e com um desvio padro de 0,1 (lembre-se que foi assumido que a
109
medio corrompida por um rudo branco de medio de 0,1 V
RMS
). A fim de obter
resultados mais elucidativos, foram executadas simulaes com 50 iteraes e com
diferentes parmetros para o filtro.

Na primeira simulao, fixou-se a varincia do erro de medio
2
(0,1) 0, 01 = = R .
Dado que este o valor verdadeiro da varincia do erro de medio, espera-se uma
melhor performance do filtro em termos da estimativa da varincia. Isto se tornar
mais evidente a partir da segunda e terceira simulao. O valor verdadeiro da
constante aleatria 0,5287 = x dado pela linha slida, o rudo de medio dado
pelas marcas cruzadas +, e a estimativa do filtro pela curva restante.


0 5 10 15 20 25 30 35 40 45 50
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
Valor Real + Valor Medido + Valor estimado
Iteraoes
T
e
n
s
a
o

[
V
]


Fig. A.3 Resultado da primeira simulao
2
(0,1) 0, 01 = = R





110
0 5 10 15 20 25 30 35 40 45 50
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
Valor Real + Valor Medido + Valor estimado
Iteraoes
T
e
n
s
a
o

[
V
]

Fig. A.4 Resultado da segunda simulao
2
(1) 1 R = =


0 5 10 15 20 25 30 35 40 45 50
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Valor Real + Valor Medido + Valor estimado
Iteraoes
T
e
n
s
a
o

[
V
]

Fig. A.5 Resultado da terceira simulao
2
(0, 01) 0, 0001 = = R


111
Enquanto que a estimao de uma constante aleatria bastante simples e direta, ela
claramente demonstra o funcionamento do filtro de Kalman. Na figura A.5 em
particular, nota-se que a estimativa da filtragem de Kalman aparece
consideravelmente mais suavizada do que o rudo de medio.

0 5 10 15 20 25 30 35 40 45 50
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Covariancia do Erro x Numero de Iteraoes
Iteraoes
C
o
v
a
r
i
a
n
c
i
a

d
o

E
r
r
o

Fig. A.6 Resultado da convergncia de P
k
aps 50 iteraes

Foi dito anteriormente que no critico considerar a escolha inicial de
0
0 P , pois,
o filtro eventualmente tenderia a convergir para o estado esperado (ou seja, melhor
estimativa para x
k
). A figura A.6 mostra o valor de P
k
em funo da iterao. Com
um total de 50 iteraes o valor de P
k
que inicialmente foi declarado como
0
1 P = ,
assentou se por volta de 0 volts aps a quinta iterao.

112
B. Transformaes de coordenadas


B.1. ngulos de Euler

A seguir descrevem-se as matrizes de rotao que transportam grandezas do sistema
de navegao mvel (RPY) representada pelo ndice p, para o sistema de
navegao local (NED) representada pelo ndice n. A operao inversa tambm
ser descrita.

n
i
x
Rotao positiva
no eixo y
z
n
k

n
j
y
Rotao positiva
no eixo x
z
n
k

n
i
x
Rotao positiva
no eixo z
y
n
j

p
j
p
i
p
k p
j
p
j
p
j
p
i
p
i
p
i
p
k
p
k
p
k

(a) (b) (c)
Fig. B.1 Rotaes: yaw (a), pitch (b) e roll (c)

Rotao em torno do eixo z (yaw)

Na figura B.1(a) os vetores unitrios que representam o sistema de navegao local
(NED) em funo do sistema de navegao mvel (RPY), pode ser escrito como

cos . sin . 0. ,
sin . cos . 0. ,
0. 0. ,
n p p p
n p p p
n p p p


= +
= + +
= + +
i i j k
j i j k
k i j k
(B.1)

ou na forma matricial, por:

113

cos sin 0
sin cos 0 .
0 0 1
n p
n p
n p




=




i i
j j
k k
.
(B.2)

Assim, a matriz de rotao no eixo z que transporta o sistema mvel (RPY) para o
sistema de navegao local (NED)


( ) 1
cos sin 0
sin cos 0
0 0 1
yaw




= =



R C . (B.3)


Rotao em torno do eixo y (pitch)

Os vetores unitrios que representam o sistema de navegao local (NED) em funo
do sistema de navegao mvel (RPY), so obtidos a partir da figura B.1b por

cos . 0. sin . ,
0. . 0. ,
sin . 0. cos . ,
n p p p
n p p p
n p p p


= + +
= + +
= + +
i i j k
j i j k
k i j k
(B.4)

ou na forma matricial, por


cos 0 sin
0 1 0 .
sin 0 cos
n p
n p
n p




=




i i
j j
k k
.
(B.5)

Assim, a matriz de rotao no eixo y que transporta o sistema mvel (RPY) para o
sistema de navegao local (NED)

114

( ) 2
cos 0 sin
0 1 0
sin 0 cos
pitch




= =



R C . (B.6)


Rotao em torno do eixo x (roll)

Finalmente, a figura B.1(c) permite escrever os vetores unitrios do sistema de
navegao local (NED) em funo do sistema de navegao mvel (RPY), como

0. 0. ,
0. cos . sin . ,
0. sin . cos . ,
n p p p
n p p p
n p p p


= + +
= +
= + +
i i j k
j i j k
k i j k
(B.7)

ou na forma matricial, por:


1 0 0
0 cos sin .
0 sin cos
n p
n p
n p




=




i i
j j
k k
.
(B.8)

Portanto, a matriz de rotao no eixo x que transporta o sistema mvel (RPY) para o
sistema tangencial (NED)


( ) 3
1 0 0
0 cos sin
0 sin cos
roll




= =



R C . (B.9)

Para efetuar uma rotao nos trs eixos, rotaciona-se primeiro em x, depois no novo
eixo y e finalmente no novo eixo z. Matematicamente tem-se


1 2 3
n
p
= C C C C ,
(B.10)

115

cos sin 0 cos 0 sin 1 0 0
sin cos 0 . 0 1 0 . 0 cos sin
0 0 1 sin 0 cos 0 sin cos
n
p





=



C , (B.11)

cos cos cos sin sin sin cos sin sin cos sin cos
cos sin cos cos sin sin sin sin cos cos sin sin
sin sin cos cos cos
n
p



+ +
= + +

C , (B.12)

A operao inversa da equao B.10 fornece a matriz de rotao do sistema de
navegao local (NED) para o sistema de navegao mvel (RPY), ou seja


1
1 2 3
[ ]
p
n

= C C C C . (B.13)

Dado que as matrizes de rotao so ortonormais, da lgebra linear tem-se que a sua
inversa igual a sua transposta, portanto a matriz de rotao do sistema de
navegao local (NED) para o sistema de navegao mvel (RPY) fica sendo


1 2 3 3 2 1
[ ]
p T T T T
n
= = C C C C C C C , (B.14)


1 0 0 cos 0 sin cos sin 0
0 cos sin . 0 1 0 . sin cos 0
0 sin cos sin 0 cos 0 0 1
p
n





=



C , (B.15)

cos cos cos sin sin
cos sin sin sin cos cos cos sin sin sin sin cos
sin sin cos sin cos sin cos cos sin sin cos cos
p
n

= + +
+ +





C , (B.16)

116
B.2. Matriz de Co-Senos Diretores

Alem dos ngulos de Euler, outra forma de efetuar a transformao de coordenadas
atravs de uma matriz de rotao denominada Matriz de Co-Senos Diretores
(MCD). A MCD definida por uma matriz 3x3, cujas colunas so os vetores
unitrios representados no eixo da plataforma e projetados ao longo do sistema de
coordenadas de referncia (neste exemplo o sistema de referncia o plano
tangencial local NED).


11 12 13
21 22 23
31 32 33
n
p
c c c
C c c c
c c c


=



.
(B.17)

O elemento da i-sima linha e j-sima coluna representam o co-seno do ngulo entre
o eixo do sistema de coordenadas de referncia i e o eixo j do sistema de
coordenadas da plataforma.


B.2.1. Transformao de coordenadas atravs da matriz co-seno diretor

Um vetor definido no sistema de coordenadas da plataforma pode ser expresso no
sistema de coordenadas da navegao local por


n n p
p
= r C r ,
(B.18)

onde a matriz
n
p
C aquela obtida atravs dos ngulos de Euler


B.2.2. Propagao da matriz co-seno diretor no tempo

A taxa de variao da matriz
n
p
C com o tempo dada por
117


0 0
( ) ( )
lim lim
n n n
p p p n
p
t t
t t t
t t

+
= =

C C C
C

, (B.19)

onde ( )
n
p
t C e ( )
n
p
t t + C representam a MCD no instante de tempo t e t t +
respectivamente. A matriz ( )
n
p
t t + C pode ser escrita como um produto de duas
matrizes, por


( ) ( ) ( )
n n
p p
t t t t + = C C A ,
(B.20)

onde A(t) a MCD que relaciona o sistema de coordenadas da plataforma no instante
de tempo t ao sistema de coordenadas de navegao local no instante de tempo
t t + . Para pequenos ngulos de rotao, a matriz A(t) pode ser escrita como
(Titterton, 1997):

( ) [ ] t = + A I .
(B.21)

Na equao acima, I uma matriz identidade 3x3 e definida por:


0
0
0





=



.
(B.22)

Nesta matriz , e so os pequenos ngulos de rotao no qual a plataforma
rotacionada sobre os eixos de yaw, pitch e roll,respectivamente. No limite,quando
t tende a zero,a aproximao por pequenos ngulos torna-se vlida e a ordem de
rotao no importante.

Substituindo ( )
n
p
t t + C na equao B.19, obtm-se

118

0 0
( )( ) ( ) ( )( )
lim lim
n n n
p p p n
p
t t
t t t
t t

+ +
= =

C I C C I I
C

, (B.23)

e como ( )
n
p
t C torna-se invariante no tempo, pode-se escrever


0
lim
n n
p p
t
t

C C

. (B.24)

No limite quando t tende a zero
t

, torna-se a forma anti-simtrica (

T
= A A)
do vetor da taxa angular
T
p
np x y z
=

, o qual representa a taxa de rotao
do sistema da plataforma em relao ao sistema de navegao local, ou seja


0
lim
p
np
t
t

. (B.25)

Substituindo na equao B.24 tem-se


n n p
p p np
= C C

,
(B.26)

onde


0
0
0
z y
p
np z x
y x





=

.
(B.27)

Na equao B.26,
n
p
C a matriz de orientao inicial (tambm conhecida como
matriz de alinhamento inicial) e
b
np
a matriz de propagao da taxa de rotao do
sistema da plataforma (RPY) para o sistema de navegao local (NED).


119
B.3. Transformao de coordenadas entre os sistemas ECEF, NED e ECI


Nesta seo so apresentadas as relaes de transformao entre os sistemas de
coordenadas terrestre (ECEF), da navegao local (NED) e inercial (ECI).


Rotao terrestre
Meridiano de
Greenwich
Plano equatorial
Plo
Plano meridiano
local
Eixo de navegao
tangencial local
Xe
Ye
Ze
N
E
D
O

Latitude Local
Longitude Local

L


Fig. B.2 Sistemas de coordenadas do globo terrestre



B.3.1. Transformao do sistema ECEF para NED

Na figura B.2 a transformao do sistema ECEF para o sistema NED obtida a partir
das seguintes aes:

1. Rotaciona-se o globo no eixo z de um ngulo de , e obtm-se a seguinte matriz
de rotao


1
cos sin 0
sin cos 0
0 0 1




=



C . (B.28)


2. Em seguida rotaciona-se o globo em torno do novo eixo y de um ngulo de e
obtm-se
120


2
cos 0 sin
0 1 0
sin 0 cos
L L
L L


=



C . (B.29)


3. Ao executar as duas operaes anteriores, o novo eixo x ficou normal superfcie
terrestre, o novo eixo y ficou normal ao plano do meridiano local e o novo eixo z
tangencia o meridiano local e aponta para o norte geogrfico da Terra. Portanto,
para fazer com que o eixo x aponte para o norte geogrfico, o eixo z fique normal
a superfcie da Terra apontando para o seu centro de massa e o eixo y completar
o sistema de coordenadas atravs da regra da mo direita, basta rotacionar o novo
sistema de coordenadas de 90 positivos em torno do eixo y. Assim a matriz de
rotao no eixo y dada ento por


3
cos 0 sin
0 1 0
sin 0 cos




=



C , (B.30)


3
cos90 0 sin90 0 0 1
0 1 0 0 1 0
sin90 0 cos90 1 0 0


= =



C . (B.31)

Portanto, para efetuar a rotao nos trs eixos, rotaciona-se primeiro em , depois em
L e finalmente em . Esta operao representada matematicamente por:


3 2 1
n
e
= C C C C , (B.32)


0 0 1 cos 0 sin cos sin 0
0 1 0 . 0 1 0 . sin cos 0
1 0 0 sin 0 cos 0 0 1
n
e
L L
L L




=



C . (B.33)

121

B.3.2. Transformao do sistema NED para ECEF

A transformao do sistema de coordenadas NED para ECEF obtida atravs da
inversa da matriz
n
e
C , ou seja


1
1 2 3
T
e n n T T T
n e e

= = =

C C C C C C , (B.34)


cos sin 0 cos 0 sin 0 0 1
sin cos 0 . 0 1 0 . 0 1 0
0 0 1 sin 0 cos 1 0 0
e
n
L L
L L




=



C . (B.35)


B.3.3. Converso das coordenadas cartesianas em funo da latitude L e da
longitude

Na figura B.2 as coordenadas cartesianas [x y z]
T
podem ser representadas em funo
da latitude L e da longitude , ou seja

sin
e
z r L = .
(B.36)

Como a projeo entre o meridiano local e a linha do equador uma diagonal dada
por

cos
meridiano equador
diagonal r L

= ,
(B.37)

as projees da linha diagonal no eixo x
e
e y
e
so

cos cos
e
x r L = ,
cos sin
e
y r L = .
(B.38)

122
Assim, o sistema cartesiano pode ser representado em termos da latitude e da
longitude por


cos cos
cos sin
sin
e
e
e
x r L
y r L
z r L

=



.
(B.39)


B.3.4. Transformao do sistema ECEF para ECI

Analisando a figura B.2, verifica-se que existe um movimento de rotao em torno
do eixo z. Portanto a transformao de coordenadas de ECEF para ECI obtida a
partir da matriz que descreve esta rotao.


1
cos sin 0
sin cos 0
0 0 1
i
e




= =



C C . (B.40)

Assim, um vetor no sistema ECEF pode ser representado no sistema ECI por


i i e
e
= v C v . (B.41)


B.3.5. Transformao do sistema ECI para ECEF

Como a transformao anterior produziu uma matriz ortonormal, a transformao do
sistema de coordenadas ECI para ECEF obtida atravs da transposta de
i
e
C ,


1
cos sin 0
sin cos 0
0 0 1
e T
i




= =



C C . (B.42)

123
C. Scripts Matlab

A seguir so apresentados os trechos mais relevantes que foram utilizados para
implementar a reconstruo das trajetrias.


C.1. Script para correes determinsticas

%===========================================================================
% Script para correao deterministica dos dados dos giroscopios
%===========================================================================
clc;
close;
%===========================================================================
% Declara dados de entrada (Latitude, Longitude e Velocidade de Rotaao)
%===========================================================================

% latitude e longitude

Latitude = 23.32*pi/180;
Longitude = 46.43*pi/180;

Lat = Latitude;
Long = Longitude;

% velocidade de rotao da Terra [rad/s]

wie = [0 0 0.7292e-4]';

%===========================================================================
% transforma velocidade de rotaao da Terra para o sistema NED
%===========================================================================

% matriz de rotaao NED => ECEF

C1 = [ 0 0 1;0 1 0;-1 0 0];
C2 = [cos(Lat) 0 sin(Lat);0 1 0;-sin(Lat) 0 cos(Lat)];
C3 = [cos(Long) -sin(Long) 0; sin(Long) cos(Long) 0;0 0 1];

Ce_n = C3*C2*C1;
wie_n = Ce_n*wie;

%===========================================================================
% captura saidas dos giroscopios e transforma em radianos/s
%===========================================================================

% as saidas dos giroscopios estao no sistema da plataforma

Wx_p = Dados_UMI(:,ncol_UMI-7)*pi/180;
Wy_p = Dados_UMI(:,ncol_UMI-6)*pi/180;
Wz_p = Dados_UMI(:,ncol_UMI-5)*pi/180;

% gera vetor de tempos discretos "T" (UMI em Angle Mode)

for (i=1:nlin_UMI)
if (i==1)
T = Dados_UMI(i+1,1)-Dados_UMI(i,1);
124
else
T = Dados_UMI(i,1)-Dados_UMI(i-1,1);
end


% trasforma velocidades angulares (W) em angulos (radianos)

Ox_p = -Wx_p(i)*T;
Oy_p = -Wy_p(i)*T;
Oz_p = -Wz_p(i)*T;

% gera matriz anti-simetrica (forma linearizada de NED ==> RPY)

Anti_simetrica = [1 -Oz_p Oy_p ; Oz_p 1 -Ox_p ; -Oy_p Ox_p 1]';

%===========================================================================
% transforma velocidade da Terra do sistema NED para o sistema RPY
%===========================================================================

wie_p = Anti_simetrica*wie_n;

%===========================================================================
% compensa velocidade da Terra nas medidas de giroscopios
%===========================================================================

Wx_p(i) = Wx_p(i)-wie_p(1);
Wy_p(i) = Wy_p(i)-wie_p(2);
Wz_p(i) = Wz_p(i)-wie_p(3);

end
beep

%---------------------------------------------------------------------------
% Os dados de saida sao:
% --------------------------------------------------------------------------
% Vetor com os angulos de rotaao (compensados) em torno dos eixos xyz no
% sistema de coordenadas da plataforma (RPY) ==> Ox_p, Oy_p e Oz_p
% --------------------------------------------------------------------------


C.2. Script para transformao de coordenadas

%===========================================================================
% Script para transformar aceleraao e velocidade do sistema de coordenadas
% da plataforma (RPY) para o sistema de coordenadas de navegaao (NED)
% atraves das medidas de giroscopio nao estimadas (GNE) por filtro de Kalman
%===========================================================================
clc;
close;

%===========================================================================
% Declara constantes: aceleraao gravitacional, raio terrestre, velocidade
% de rotaao terrestre, latitude local, longitude local, altitude local e
% demais parametros do modelo da Terra, segundo WGS84.
%===========================================================================

% latitude e longitude

Latitude = 23.32*pi/180;
Longitude = 46.43*pi/180;

125
Lat = Latitude;
Long = Longitude;

% velocidade de rotao da Terra [rad/s]

wie = [0 0 0.7292e-4]';

% constante de acelerao gravitacional (g)

g = 9.80665;

% raio aproximado da Terra (m)

T_raio = 6378137;

%===========================================================================
% Matriz de transformaao de coordenadas inicial (Matriz de alinhamento
% inicial)
%===========================================================================

roll_ini = Dados_UMI(1,ncol_UMI-9)*pi/180;
pitch_ini = Dados_UMI(1,ncol_UMI-8)*pi/180;
yaw_ini = -12.5*pi/180;

% mudana de variavel

R = roll_ini;
P = pitch_ini;
Y = yaw_ini;

% gera matriz de transformaao de coordenada inicial (Angulos de Euler)

Rx = [1 0 0 ; 0 cos(R) -sin(R) ; 0 sin(R) cos(R)];
Ry = [cos(P) 0 sin(P) ; 0 1 0 ; -sin(P) 0 cos(P)];
Rz = [cos(Y) -sin(Y) 0 ; sin(Y) cos(Y) 0 ; 0 0 1];

% Matriz de alinhamento inicial RPY ==> NED

Cpn_ini = Rz*Ry*Rz;
Cpn = Cpn_ini;

%===========================================================================
% captura saidas dos giroscopios (UMI em Angle Mode)e transforma em
% radianos
%===========================================================================

% Gx = Dados_UMI(:,ncol_UMI-7);
% Gy = Dados_UMI(:,ncol_UMI-6);
% Gz = Dados_UMI(:,ncol_UMI-5);

%===========================================================================
% captura saidas dos acelerometros e converte em m/s^2 (UMI em Angle Mode)
% estas componentes de acelerao estao no sistema de coordenadas RPY
% ( Roll Pitch Yaw)(sistema da plataforma)
%===========================================================================

Ax_p = g*Dados_UMI(:,ncol_UMI-4)';
Ay_p = g*Dados_UMI(:,ncol_UMI-3)';
Az_p = g*Dados_UMI(:,ncol_UMI-2)';

%===========================================================================
% captura angulos de roll e pitch para coorigir decomposiao gravitacional
% (UMI em Angle Mode)e transforma em radianos
%===========================================================================

126
roll = Dados_UMI(:,ncol_UMI-9)'*pi/180;
pitch = Dados_UMI(:,ncol_UMI-8)'*pi/180;

Ax_p = Ax_p - g*sin(pitch);
Ay_p = Ay_p - g*sin(roll);

%===========================================================================
% gera vetor de tempos discretos "T" (UMI em Angle Mode)
%===========================================================================

for (i=1:nlin_UMI)
if (i==1)
T = Dados_UMI(i+1,1)-Dados_UMI(i,1);
else
T = Dados_UMI(i,1)-Dados_UMI(i-1,1);
end

%===========================================================================
% captura velocidades angulares [rad/s] com rotaao da Terra corrigida
% e converte em angulos [rad] (sistema de coordenadas da plataforma (RPY)
%===========================================================================

Ox_p(i) = -Wx_p(i)*T;
Oy_p(i) = -Wy_p(i)*T;
Oz_p(i) = -Wz_p(i)*T;

%===========================================================================
% sugestao para computar o angulo de rotaao - Titterton - pg 301
%===========================================================================

% teta = [Ox_p(i) Oy_p(i) Oz_p(i)];
% omega = [Ox_p(i) Oy_p(i) Oz_p(i)]/T;
% dteta = cross(teta,omega)*T;
% teta = teta+dteta;
%
% Ox_p(i) = teta(1);
% Oy_p(i) = teta(2);
% Oz_p(i) = teta(3);

%===========================================================================
% gera matriz anti-simetrica (RPY ==> NED)
%===========================================================================

Anti_simetrica = [1 -Oz_p(i) Oy_p(i) ; Oz_p(i) 1 -Ox_p(i) ; -Oy_p(i) Ox_p(i)
1];

%===========================================================================
% gera vetor com aceleracoes [Ax Ay Az] e velocidade [Vx Vy Vz] no
% instante (i)estes vetores encontram-se no sistema de coordenadas RPY
%===========================================================================

% ajuste do fator de escala da velocidade (kv)

kv = 1
A_p = [Ax_p(i) Ay_p(i) Az_p(i)]';
V_p = [Velocidade(i) 0 0]'*kv;

%===========================================================================
% transforma vetor de aceleracoes [Ax Ay Az] e velocidades [Vx Vy Vz] que
% estao no sistema de coordenadas da plataforma (RPY) para o sistema de
% coordenadas de navegaao (NED). Se i=1 utiliza matriz de orientaao
% inicial. Se i > 1 rotaciona a matriz a partir da rotaao anterior.
%===========================================================================


127
%===========================================================================
% efetua correao de ortogonalizaao da matriz de orientaao
%===========================================================================
Cijk = Cpn*Anti_simetrica;

dij = Cijk(1,:)*Cijk(2,:)';
dik = Cijk(1,:)*Cijk(3,:)';
djk = Cijk(2,:)*Cijk(3,:)';

Ci = Cijk(1,:)-0.5*dij*Cijk(2,:)-0.5*dik*Cijk(3,:);
Cj = Cijk(2,:)-0.5*dij*Cijk(1,:)-0.5*djk*Cijk(3,:);
Ck = Cijk(3,:)-0.5*dik*Cijk(1,:)-0.5*djk*Cijk(2,:);

Cijk = [Ci;Cj;Ck];
Cpn = Cijk;

%===========================================================================
% fim da ortogonalizaao e normalizaao da matriz de orientaao
%===========================================================================

A_n(:,i)=Cpn*A_p;
V_n(:,i)=Cpn*V_p;

end

% --------------------------------------------------------------------------
% separa as componentes de acelerao e velocidade dos eixos [x y z] que ja
% estao convertidas para o sistema de coordenadas NED
% --------------------------------------------------------------------------
% --------------------------------------------------------------------------
% adiciona ruido de mediao k (sem ruido ==> k = 0)
% necessario para simulaao do filtro de Kalman
% --------------------------------------------------------------------------

k=0

Ax_n = A_n(1,:);
Ay_n = A_n(2,:);
Az_n = A_n(3,:);

Vx_n = V_n(1,:)+randn(1,nlin_UMI)*k;
Vy_n = V_n(2,:)+randn(1,nlin_UMI)*k;
Vz_n = V_n(3,:)+randn(1,nlin_UMI)*k;

% ka=0;
% kv=0;

% Ax_n = A_n(1,:)+randn(1,nlin_UMI)*ka;
% Ay_n = A_n(2,:)+randn(1,nlin_UMI)*ka;
% Az_n = A_n(3,:)+randn(1,nlin_UMI)*ka;

% Vx_n = V_n(1,:)+randn(1,nlin_UMI)*kv;
% Vy_n = V_n(2,:)+randn(1,nlin_UMI)*kv;
% Vz_n = V_n(3,:)+randn(1,nlin_UMI)*kv;

% --------------------------------------------------------------------------
% compensa acelerao gravitacional que no sistema NED esta
% presente somente no eixo z
% --------------------------------------------------------------------------

Az_n = Az_n + g;

%===========================================================================
% Nome e formato dos dados de saida
%===========================================================================
128
% Os dados de saida sao:
% --------------------------------------------------------------------------
% Vetores de aceleraoes e velocidades nao estimados no sistema NED
% --------------------------------------------------------------------------
% Aceleraao NED no eixo (X) ==> Nome: Ax_n (vetor nx1)
% Aceleraao NED no eixo (y) ==> Nome: Ay_n (vetor nx1)
% Aceleraao NED no eixo (z) ==> Nome: Az_n (vetor nx1)

% Velocidade NED no eixo (x) ==> Nome: Vx_n (vetor nx1)
% Velocidade NED no eixo (y) ==> Nome: Vy_n (vetor nx1)
% Velocidade NED no eixo (z) ==> Nome: Vz_n (vetor nx1)

% --------------------------------------------------------------------------
% Vetores com angulos de rotaao nao estimados em radianos
% --------------------------------------------------------------------------
% Angulo de rotaao RPY no eixo (x) ==> Nome: Ox_p (vetor nx1)
% Angulo de rotaao RPY no eixo (y) ==> Nome: Oy_p (vetor nx1)
% Angulo de rotaao RPY no eixo (z) ==> Nome: Oz_p (vetor nx1)
% --------------------------------------------------------------------------

% --------------------------------------------------------------------------
% Matriz de alinhamento inicial => Cpn_ini = Rz*Ry*Rz;
% Matriz de rotaao (plataforma (RPY) => navegaao (NED)) => Cpn;
% --------------------------------------------------------------------------



C.3. Script do filtro de Kalman

%===========================================================================
% Implementao do filtro de kalman para o teste automotivo
%===========================================================================

clc;
close;
% --------------------------------------------------------------------------
% dados iniciais
% --------------------------------------------------------------------------
%===========================================================================
% gera vetores de posiao, velocidade e aceleraao inicial
%===========================================================================

Po = [Coordenada_x(1) Coordenada_y(1) 0]
Vo = [Vx_n(1) Vy_n(1) Vz_n(1)]
Ao = [Ax_n(1) Ay_n(1) Az_n(1)]

% inicializao do vetor de estados

Xest = [Po Vo Ao]';

% inicializa matriz de covariancia do erro

P = eye(9)*1e0;

% inicializao do vetor de entradas

U = [0 0 0]';

% --------------------------------------------------------------------------
% declara ruido de mediao da velocidade Rv (erro de velocidade = 10%)
% --------------------------------------------------------------------------
129

Rvn = 0.1*V_n';
G = cov(Rvn);
Krm = 1;
Rv = Krm*G;

% --------------------------------------------------------------------------
% declara desvio padrao do ruido de mediao da posiao
% (erro de posiao ep = 0,5m)
% --------------------------------------------------------------------------

epx = .5;
epy = .5;
epz = .5;

Rp = diag([epx epy epz]);
Rp = Rp*Rp';

% --------------------------------------------------------------------------
% matrizes que representam a mediao
% --------------------------------------------------------------------------
% H_Coord: quando tem-se medidas de referencia das coordenadas
% --------------------------------------------------------------------------

H_Coord = [1 0 0 0 0 0 0 0 0; ...
0 1 0 0 0 0 0 0 0; ...
0 0 1 0 0 0 0 0 0];


H_Vel = [0 0 0 1 0 0 0 0 0; ...
0 0 0 0 1 0 0 0 0; ...
0 0 0 0 0 1 0 0 0];

% --------------------------------------------------------------------------
% inicio do loop - filtro de Kalman
% --------------------------------------------------------------------------

% calculo do intervalo de amostragem

for (i=1:nlin_UMI)
if (i==1)
T = Dados_UMI(i+1,1)-Dados_UMI(i,1);
else
T = Dados_UMI(i,1)-Dados_UMI(i-1,1);
end

% --------------------------------------------------------------------------
% matrizes da dinamica do sistema ==> x(k+1) = A.x(k)+B.u(k)+C.w(k)
% --------------------------------------------------------------------------

A = [1 0 0 T 0 0 .5*T^2 0 0; ...
0 1 0 0 T 0 0 .5*T^2 0; ...
0 0 1 0 0 T 0 0 .5*T^2; ...
0 0 0 1 0 0 T 0 0; ...
0 0 0 0 1 0 0 T 0; ...
0 0 0 0 0 1 0 0 T; ...
0 0 0 0 0 0 1 0 0; ...
0 0 0 0 0 0 0 1 0; ...
0 0 0 0 0 0 0 0 1];






130
B = [.5*T^2 0 0; ...
0 .5*T^2 0; ...
0 0 .5*T^2; ...
T 0 0; ...
0 T 0; ...
0 0 T; ...
0 0 0; ...
0 0 0; ...
0 0 0];

% --------------------------------------------------------------------------
% matriz do ruido associado ao processo (erro de posiao proporcional ao
% cubo do tempo, erro de velocidade proporcional ao quadrado do tempo e
% erro de aceleraao proporcional ao tempo.
% --------------------------------------------------------------------------

C = [T^3/6 0 0 0 0 0 0 0 0; ...
0 T^3/6 0 0 0 0 0 0 0; ...
0 0 T^3/6 0 0 0 0 0 0; ...
0 0 0 T^2/2 0 0 0 0 0; ...
0 0 0 0 T^2/2 0 0 0 0; ...
0 0 0 0 0 T^2/2 0 0 0; ...
0 0 0 0 0 0 T 0 0; ...
0 0 0 0 0 0 0 T 0; ...
0 0 0 0 0 0 0 0 T];

% declara ruido de processo Q

Q = 10^-2.4*C*C' ;

% entradas do sistema u(k) => (leituras dos acelerometros transformadas para
NED)

U = [Ax_n(i) Ay_n(i) Az_n(i)]';

% --------------------------------------------------------------------------
% verifica se utiliza medidas de coordenadas ou medidas de velocidade
% --------------------------------------------------------------------------

if landmark(i,1) = = 0
H = H_Vel;
Z = [Vx_n(i) Vy_n(i) Vz_n(i)]';
R = Rv;
else
H = H_Coord;
Z = [landmark(i,1) landmark(i,2) 0]';
R = Rp;
end

% --------------------------------------------------------------------------
% Atualizaao temporal (Prediao)
% --------------------------------------------------------------------------

% projeta o estado a frente

Xest = A*Xest + B*U;

% projeta a covariancia do erro a frente

P = A*P*A'+Q;

% --------------------------------------------------------------------------
% Atualizaao da mediao (Correao)
% --------------------------------------------------------------------------
% computa o ganho K do filtro de Kalman
131

K = P*H'*inv(H*P*H'+R);

% atualiza a estimativa Xk com a mediao Zk

Xest = Xest + K*(Z-H*Xest);

% atualiza a covariancia do erro

P = P-K*H*P;

Var_P =[P(1,1) P(2,2) P(3,3)];
STD_(i) = norm(Var_P);

% simetriza a matriz P

P = 0.5*(P+P');

% armazena estados estimados (gera uma matriz onde cada coluna da matriz
% representa uma estimativa dos estados no instantante (i))

M_Xest(:,i)=Xest;

%===========================================================================
% Geraao da trajetoria com duplo integrador a partir dos vetores de
% dados no sistema NED
%===========================================================================

if i = =1
Px(i)=Po(1)+Vx_n(i)*T+.5*Ax_n(i)*T^2;
Py(i)=Po(2)+Vy_n(i)*T+.5*Ay_n(i)*T^2;
Pz(i)=Po(3)+Vz_n(i)*T+.5*Az_n(i)*T^2;

else
Px(i)=Px(i-1)+Vx_n(i)*T+.5*Ax_n(i)*T^2;
Py(i)=Py(i-1)+Vy_n(i)*T+.5*Ay_n(i)*T^2;
Pz(i)=Pz(i-1)+Vz_n(i)*T+.5*Az_n(i)*T^2;
end

%===========================================================================
% Fim do Teste com duplo integrador
%===========================================================================

end

132
REFERNCIAS BIBLIOGRFICAS


BROWN, R. G.; HWANG, P. Y. C. Introduction to Random Signals and Applied
Kalman Filtering. 3. Ed.. New York, USA: John Wiley and Sons, 1997

CAMPOS, V. A. et al. (2004a). Filtros de Partculas Aplicados Estimao de
Trajetrias. Gramado: Artigo aceito para publicao nos anais do XV CBA
(Congresso Brasileiro de Automtica), 2004.

CAMPOS, V. A. et al. (2004b). Trajectory Estimation of a PIG Using a Low Cost
Inertial Measurement Unit and Odometry. Joinville: Artigo aceito para publicao
nos anais do VI INDUSCON (Conferncia para Aplicaes Industriais), 2004.

CAMPOS, V. A. F. (2004c). Aplicao do Filtro de Kalman e dos Filtros de
Amostras Estimao de Trajetrias em Navegao Inercial. So Paulo, Brasil:
Dissertao de Mestrado, EPUSP, 2004

CHATFIELD, A. B. Fundamentals of High Accuracy Inertial Navigation. USA:
The American Institute of Aeronautics and Astronautics, 1997. vol. 174

CHEN, CHI-TSONG. Linear System Theory and Design. 3 Ed. USA: IE-Oxford,
1998.

CLEYNER, S. P. et al. (2002). Formulao de um Sistema de Navegao Inercial
para Veculos Robticos. Natal: Artigo aceito para publicao nos anais do XIV
CBA (Congresso Brasileiro de Automtica), 2002

CRAIG, J. J. Introduction to Robotics Mechanics and Control. 2. Ed.. USA:
Addison-Wesley, 1989

CROSSBOW. Measurement of a Vehicles Dynamic Motion - Combine Angular
133
Rate Sensors with Accelerometers. USA: CROSSBOW IMU Application Note,
1999.

CROSSBOW. VG700AA Users Manual. Setembro de 2002 (Document 7430-
0074-01)

DOROBANTU, R.; ZEBHAUSER, B. Field Evaluation of Low Cost Strapdown
IMU by means GPS. Germany: Technische Universitt Mnchen, 1999

DUDEK, G.; JENKIN, M. Computational Principles of Mobile Robotics. USA:
Cambridge University Press, 2000.

FRANKLIN, G. F.; POWELL, J. D.; WORKMAN, M. Digital Control of
Dynamic Systems. 3. ed.. USA: Addison Wesley Longman, Inc., 1998

FURUKAWA, C. M. Navegao de um Veculo Autnomo por Ultra-Som em
Ambiente Estruturado. So Paulo, Brasil: Dissertao de Mestrado EPUSP, 1992

GELB, A. et al. Applied optimal Estimation. USA: MIT Press, 1986

GRENON, G.; AN, E.; SMITH, S. Enhancement of the Inertial Navigation System
of the Florida Atlantic University Autonomous Underwater Vehicles. Florida, USA:
Institute of Ocean and System Engineering Sea Tech, Florida Atlantic University,
2000

GREWAL, M. S.; HENDERSON, V. D.; MIYASAKO, R. S. Applications of
Kalman Filtering to the Calibration and Alignment of Inertial Navigation Systems.
USA: IEEE Transactions on Automatic Control, vol. 36, no. 1, Janeiro de 1991

GREWAL, M. S.; WEILL, L. R.; ANDREWS, A. P. Global Positioning Systems,
Inertial Navigation and Integration. New York, USA: John Wiley & Sons, 2001.

134
GUSTAFSSON, F. et al. (2001) Particle Filters for Positioning, Navigation and
Tracking. IEEE Transactions on Signal Processing, Special Issue on Monte Carlo
Methods for Statistical Signal Processing, pp. 1-13.

JAZWINSKI, A. H. Stochastic Process and Filtering Theory. Mathematic in
Science and Engineering vol.64 USA: Academic Press, 1970.

KALMAN, E. R.; BUCY, R. New Results in Linear Filtering and Prediction
Theory. USA: Journal of Basic Engineering. Transactions ASME Series D, 1961.
vol. 83, pp. 95-108

KALMAN, R. E. A New Approach to Linear Filtering and Prediction Problems.
USA: Journal of Basic Engineering. Transactions ASME Series D, vol 82, pp. 35-
45,1960

KELLY, A. Introduction to Mobile Robots, Position Estimation 4: Inertial
Navigation Systems. USA: University of Carnegie Mellon,1996

LAWRENCE, A. Modern Inertial Technology Navigation, Guidance and
Control. 2. Ed. New York, USA: Springer-Verlag, 1998

MATLAB verso 6.5 Release 13. USA: The Mathworks, Inc., Junho de 2002

MAYBECK, P. S. Stochastic Models, Estimation and Control, Volume 1. USA:
Academic Press, Inc., 1976.

NEBOT, E.; DURRANT-WHITE, H. Initial Calibration and Alignment of Low
Cost Inertial Navigation Units for Land Vehicle Applications. USA: Journal of
Robotics Systems, v. 16, no.2, pp. 81-92, Fevereiro de 1999

PAPOULIS, A.; PILLAI, S. U. Probability, Random Variables and Stochastic
Processes. 4. Ed. New York, USA: Mc Graw Hill, 2001
135

ROGERS, R. M. Applied Mathematics in Integrated Navigation Systems.
Virginia, USA: AIAA Education Series, 2000

SANTANA, D. D. S. et al. (2004). Estimao de Trajetrias Utilizando Sistema de
Navegao Inercial Strapdown. Gramado: Artigo aceito para publicao nos anais
do XV CBA (Congresso Brasileiro de Automtica), 2004.

SANTOS, I. F. Dinmica de Sistemas Mecnicos. So Paulo: Makron Books, 2001

SAVAGE, P. G. Strapdown Inertial Navigation Integration Algorithm Design. USA:
Part1. Journal of Guidance, Control and Dynamics, vol. 21(1), pp.19-28, 1998

SAVAGE, P. G. Strapdown Inertial Navigation Integration Algorithm Design. USA:
Part2. Journal of Guidance, Control and Dynamics, vol. 21(2), pp.208-221, 1998

SHIN, E. Accuracy Improvement of Low Cost INS/GPS for Land Applications.
Calgary, Canad: Dissertao de Mestrado, The University of Calgary, 2001

STOVAL, S. H. Basic Inertial Navigation. California, USA: Naval Air Warfare
Center Weapons Division, September 1997 (report number NAWCWPNS TM
8128)

SUKKARIEH, S. Low Cost, High Integrity, Aided Inertial Navigation Systems for
Autonomous Land Vehicles. Australian: PhD Thesis, Australian Centre for Field
Robotics, Department of Mechanical and Mechatronic Engineering, University of
Sydney, 2000

TITTERTON, D. H.; WESTON, J. L. Strapdown Inertial Navigation
Technology. London, UK: IEE Radar, Sonar, Navigation and Avionics Series 5,
1997

136
WALCHKO, K. J. ; MASON, P. A. C. (2002a). Inertial Navigation. Florida, USA:
Florida Conference on Recent Advances in Robotics, 2002

WALCHKO, K. J. et al (2002b). Embedded Low Cost Inertial Navigation System.
Gainesville, USA: University of Florida, 2002

WGS-84. Departament of Defense World Geodetic System 1984: Its Definition
and Relationship With Local Geodetic Systems. Belgium: DMA TR 8350.2,
September 1984.




































137
BIBLIOGRAFIA



BAR-SHALOM, Y. ; LI, X. R. Estimation and Tracking: Principles,
Techiniques and Software. USA: Artech House, 1993.

BAR-SHALOM, Y. ; LI, X. R. Estimation With Applications to Tracking and
Navigation: Theory, Algorithms and Software. USA: John Wiley and Sons, 2001.

BAR-SHALOM, Y. ; LI, X. R. Multitarget Multisensor Tracking: Principles
and Techniques. USA: YBS, 1995.

BURRUS, C. S. et al. Computer Based Exercises for Signal Processing Using
Matlab. New Jersey, USA: Prentice Hall, 1994.

CLUDIO, D. M. ; MARINS, J. M. Clculo Numrico Computacional. 2. Ed..
So Paulo: Editora Atlas S.A, 1994

CRUZ, C. ; RIBEIRO, U. Metodologia Cientifica: Teoria e Prtica. So Paulo:
Editora Axcel Books, 2003.

DANTAS, C. A. B. Probabilidade: Um Curso Introdutrio. 2. Ed.. So Paulo,
Brasil: EDUSP, 2000.

DONHA, D. C. Estudo, Implementao, Teste e Avaliao de um sistema de
Posicionamento Dinmico. So Paulo: Tese de Doutorado, EPUSP, 1989.

FURUKAWA, C. M.; ADAMOWSKI, J. C.; CAMERINI, C. S. Ultrasonic Pig for
Oil Pipeline Inspection. In: IEEE INDUSTRY APPLICATIONS SOCIETY, III
INDUSCON: Anais. So Paulo, Brasil: IEEE Sul Brasil, Escola Politcnica da USP,
1998

138
GOLUB, G. H. ; LOAN, C. F. V. Matrix Computations. 3. Ed.. USA: The Johns
Hopkins University Press, 1996

JAFFE, R. C. Random Signals for Engineers Using Matlab and Mathcad.
New Jersey, USA: Springer-Verlag, Cherry Hill, 2000

KAILATH, T. Linear Systems. New Jersey, USA: Prentice-Hall, Inc., Englewood
Cliffs, 1980

KING, A. D. Inertial Navigation Forty Years of Evolution. UK: GEC Review,
vol.13, pp. 140-149, 1998

KREYSZIG, E . Advanced Engineering Mathematics. 7. Ed.. New York, USA:
John Wiley and Sons, 1993

LIPSCHUTZ, S. lgebra Linear. 3. Ed.. So Paulo: Makron Books, 1994

LJUNG, L. System Identification: Theory for the User. 2. Ed.. New Jersey, USA:
Prentice-Hall, Inc., Englewood Cliffs, 1999

SCHULTZ, T. W. C and the 8051 Programming for Multitasking. New Jersey,
USA: Prentice Hall, Englewood Cliffs, 1993

STRANG, G. Introduction to Linear Algebra. USA: MIT Press, 1998

USP. Diretrizes para Apresentao de Dissertaes e Teses. So Paulo, Brasil:
Servio de Bibliotecas da USP, 2001.

VU, H.; ESFANDIARI, R. Dynamic Systems Modeling and Analysis. Singapore:
MacGraw-Hill International, 1998