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

a situação. A ideia era descartar o acelerômetro quando a medida dele não era coerente com a
medida do campo gravitacional.
O modelo de predição do Decoupled EKF é idêntico ao do CEKF, descrito pela Equação 5.53.
Na etapa de correção, foi utilizada a mesma abordagem de utilizar correções sequenciais. A
diferença na etapa de correção é não utilizar o TRIAD para estimar a atitude, e sim as medidas
provindas diretamente dos sensores. Os outros passos (correção por GPS, sonar e altímetro) são
idênticos. Essa abordagem é baseada em [77, 79].
Voltando a Equação 5.31, reproduzida aqui por conveniência:
s b = C b ns n (5.69)
temos que o dado vindo do sensor no sistema do corpo s b é o dado no sistema inercial rotacionado
pela orientação do corpo C b ns n . Assumindo que o sensor só mede o campo no sistema inercial c n ,
s b = C b nc n (5.70)
Basta então representar a matriz de rotação em função dos estados, ou seja, em função de q b n.
Utilizando a notação de [1, 5, 63], temos que:
⎡
q0 2
C b n(qn) b + q2 1 − q2 2 − ⎤
q2 3 2q 1 q 2 + 2q 0 q 3 2q 1 q 3 − 2q 0 q 2
= ⎢
⎣ 2q 1 q 2 − 2q 0 q 3 q0 2 − q2 1 + q2 2 − q2 3 2q 2 q 3 + 2q 0 q 1
⎥
⎦ (5.71)
2q 1 q 3 + 2q 0 q 2 2q 2 q 3 − 2q 0 q 1 q0 2 − q2 1 − q2 2 + q2 3
A fim de dar mais robustez ao sistema, os dados dos sensores e do campo de referência são
normalizado para ter norma unitária. Dessa forma, o termo de inovação v é dado por:
s b
c n
v =
||s b || − Cb n
||c n ||
e a matriz de medição H dada por:
H = ∂h(x (k))
= ∂ (
C b
∂x (k) ∂x
n(qn) b c )
n
(k) ||c n ||
(5.72)
(5.73)
Essa abordagem foi utilizada para as medidas do acelerômetro e magnetômetro, utilizando os
valores de c i obtidos dos modelos EGM96 e WMM2010.
Esse filtro também utiliza a modulação da matriz de covariância da medida R de acordo com o
valor da medida. Como sabe-se que o sensor deve medir somente o campo, utiliza-se a modulação
do valor de acordo da seguinte forma:
onde λ é um valor de ajuste de acordo com o sensor.
R (k) = exp(λ ||( ||s b || − ||c n || )||) (5.74)
Dessa forma, a implementação do Decoupled EKF é idêntica ao do CEKF, salvo a correção da
atitude. Isso permitiu que boa parte do código fosse reutilizado.
Os experimentos iniciais mostraram que essa solução mostrava uma variância maior em regime
permanente que o CEKF, porém, ela não mostrou os problemas associados aos pontos de operação
com aceleração linear diferente de zero, como exibido pelo CEKF com TRIAD.
112