Desenvolvimento de um Veículo Aéreo Não-Tripulado - LARA ...

Conforme visto na Seção 5.5.4.1, a inicialização do sistema utiliza o ponto de partida como
origem do sistema de coordenadas local, ou seja, a estimativa de posição inicial (x, y, z) é (0, 0, 0).
O mesmo vale para a estimativa de velocidade - parte-se da hipótese que o sistema está parado,
ou seja, (v x , v y , v z ) = (0, 0, 0)
A estimativa de orientação é fornecida utilizando o TRIAD, descrito na Seção 5.4.2, utilizando
as medidas dos acelerômetros e magnetômetros e as referências dos campos magnéticos e gravitacionais
fornecidas pelos modelos WMM2010 e EGM96. A matriz de rotação obtida é transformada
em um quaternion, fornecendo (q 0 , q 1 , q 3 , q 4 ).
5.5.5 CEKF
O Filtro de Kalman Correlato (CEKF) é a implementação padrão do LARA [1, 4, 5]. Sua
implementação original foi feita para o projeto Caracarah [58], com base no trabalho de Bó [1], e
na bibliografia clássica sobre o assunto [75, 76].
Esse filtro e sua implementação estão descritas em detalhe em [5]. Esse trabalho só irá citar
as principais equações e suas implicações, deixando a análise para o Decoupled EKF desenvolvido
para esse projeto.
O vetores de estado estimados, entrada e medição são dados por:
⎡ ⎤
q n
b r n
x =
⎢
⎣v n
⎥
⎦
b f
13x1
; u =
[ ]
ω
b
ib
f b
6x1
⎡ ⎤
q n
b v n
; y =
⎢
⎣ r n
⎥
⎦
e pseudo
11x1
(5.52)
onde q b n é o quaternion que representa a orientação, r n é o vetor posição, v n é o vetor velocidade,
e b f é o viés do acelerômetro.
O modelo de processo para predição discreto é dado por:
q b n(k) = e−W (k)∆T q b n(k−1)
⎡
sen(d/2)
cos(d/2) −δ x d
sen(d/2)
sen(d/2)
δ
=
x d
cos(d/2) δ z d
sen(d/2)
⎢δ ⎣ y d
δ z
sen(d/2)
d
−δ y
sen(d/2)
d
sen(d/2)
sen(d/2)
−δ z d
cos(d/2) δ x d
δ y
sen(d/2)
d
−δ x
sen(d/2)
d
v n (k) = v n (k−1) + (C b n(k−1) f b (k) + g n )∆T
r n (k) = r n (k−1) + v n (k−1)∆T + (C b n(k−1) f b (k) + g n ) ∆T 2
⎤
sen(d/2)
−δ z d
sen(d/2)
−δ y d
qn(k−1)
b ⎥
⎦
cos(d/2)
2
(5.53a)
(5.53b)
(5.53c)
(5.53d)
106