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

Algoritmo 1 Fusão Sensorial GPS/IMU utilizando o CEKF
Inicialização: ˆx (0) , P (0)
Parâmetros ajustáveis: Q (k) , R (k)
Laço Principal
Se há dados não processados , então
Se dados provenientes da IMU , então
Etapa de Predição :
˜Q (k) = F u η u F u T + Q (k)
ˆx (k|k−1) ← f(ˆx (k−1) , u (k) )
ˆP (k|k−1) ← F x ˆP(k−1) F x T + ˜Q (k)
Etapa de Correção (atitude) :
˜R (k) = G u η u G T u + G m η m G T m + R (k)
˜S (k) = F u η u G u
(
K (k) = ˆP(k|k−1) H T + ˜S
)
(k)
(H ˆP (k|k−1) H T + H˜S (k) + ˜S T (k) HT + ˜R
) −1
(k)
y [1:4]
(k)
= g(u (k) , m (k) )
)
ˆx (k) ← ˆx (k|k−1) + K (k)
(y (k) − h(ˆx (k|k−1) )
(
ˆP (k) ← I − K (k) H)
ˆP(k|k−1)
Se dados provenientes do GPS , então
Etapa de Correção (posição e velocidade) :
K (k) = ˆP (k|k−1) H T ( H ˆP (k|k−1) H T + ˜R (k)
) −1
y [5:10]
(k)
= y GP S
)
ˆx (k) ← ˆx (k|k−1) + K (k)
(y (k) − h(ˆx (k|k−1) )
(
ˆP (k) ← I − K (k) H)
ˆP(k|k−1)
Etapa de Correção (pseudo-medição) :
K (k) = ˆP (k|k−1) H T ( H ˆP (k|k−1) H T + ˜R (k)
) −1
y [11] ∣∣ ∣∣ (k) = 1 − ∣∣ ∣∣q b ∣∣ ∣∣
n
)
ˆx (k) ← ˆx (k|k−1) + K (k)
(y (k) − h(ˆx (k|k−1) )
(
ˆP (k) ← I − K (k) H)
ˆP(k|k−1)
5.5.6 Decoupled EKF
Durante as simulações, detectou-se um problema com o implementação utilizada do CEKF:
em situações onde o acelerômetro está sujeito a forças diferentes da gravidade por longos períodos
de tempo, a estimativa de orientação ficava ruim, errando em mais de 20 graus a estimativa.
Esse tipo de situação é comum em situações de pouso, decolagem e curvas, que são os momentos
que o sistema de controle mais depende da estimativa de orientação. O desempenho do filtro
também não era muito bom em situações onde o acelerômetro está sujeito a muito ruído, que foi
observado na prática em alguns voos iniciais. Dessa forma, optou-se por desenvolver uma nova
forma de incorporar as medidas de orientação provindas do magnetômetro e acelerômetro de forma
independente, permitindo assim modular a matriz de covariância da medição R (k) de acordo com
111