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

A escolha das matrizes R de covariância das medidas é feita de acordo com a característica
dos sensores. Um caso especial é para o algoritmo TRIAD, onde a covariância pode ser calculada
com base na transformada Unscented a partir da incerteza das medidas do acelerômetro e
magnetômetro. Outra forma é sobrestimar o valor de R. O implementado código visa propagar a
incerteza utilizando uma linearização numérica em torno de ponto de operação [5].
Um detalhe importante é a restrição: a norma do quaternion deve ser igual a 1 para representar
uma rotação, ou seja, esse norma deve ser mantida para garantir a estabilidade do algoritmo de
localização. Essa restrição é incorporada diretamente no filtro utilizando uma pseudo-medida:
∣
e pseudo(k) = 1 − ∣ ∣∣ (5.59)
∣q b n(k)
A principal diferença dessa implementação é levar em conta que as medidas do acelerômetro são
utilizadas na etapa de correção e predição, o que leva a uma implementação correlata. Na prática,
isso significa utilizar um conjunto de equações um pouco diferente da abordagem tradicional.
A seguir, estão as equações e o algoritmo da implementação do CEKF, adaptadas de [5], e
verificadas de acordo com [75, 76].
⎡
−
−
f(x (k−1) , u (k) ) =
⎢
⎣
Modelo não-linear
cos(d/2)q 0 − δ x
sen(d/2)
d
q 1 − δ y
sen(d/2)
d
q 2 − δ z
sen(d/2)
d
q 3
δ x
sen(d/2)
d
q 0 + cos(d/2)q 1 + δ z
sen(d/2)
d
q 2 − δ y
sen(d/2)
d
q 3
δ y
sen(d/2)
d
q 0 + δ z
sen(d/2)
d
q 1 + cos(d/2)q 2 − δ x
sen(d/2)
d
q 3
δ z
sen(d/2)
d
q 0 − δ y
sen(d/2)
d
q 1 + δ x
sen(d/2)
d
q 2 + cos(d/2)q 3
v x + (c x + g x )∆T
v y + (c x + g y )∆T
v z + (c z + g z )∆T
r x + v x ∆T + (c x + g x ) ∆T 2
2
r y + v y ∆T + (c y + g y ) ∆T 2
2
r z + v z ∆T + (c z + g z ) ∆T 2
2
b fx
b fy
b fz
⎤
⎥
⎦
(5.60)
[
h(x (k) ) =
∣
q 0 q 1 q 2 q 3 v x v y v z r x r y r z 1 − ∣
∣qn
b ∣
∣
∣
] T
(5.61)
c x = (q 2 0 + q 2 1 − q 2 2 − q 2 3)f x + (2q 1 q 2 − 2q 0 q 3 )f y + (2q 1 q 3 + 2q 0 q 2 )f z
c y = (2q 1 q 2 + 2q 0 q 3 )f x + (q 2 0 − q 2 1 + q 2 2 − q 2 3)f y + (2q 2 q 3 − 2q 0 q 1 )f z
c z = (2q 1 q 3 − 2q 0 q 2 )f x + (2q 2 q 3 + 2q 0 q 1 )f y + (q 2 0 − q 2 1 − q 2 2 + q 2 3)f z
(5.62a)
(5.62b)
(5.62c)
108