Metoda Wilcox

Pentru a determina atitudinea unei aeronave, atât cuaternionul de atitudine, cât şi matricea cosinusurilor
directori care face trecerea de la triedrul fix la triedrul aeronavei, sunt folositi. Valorile cunoscute la
fiecare punct de timp sunt componentele de viteză unghiulară măsurate folosind giroscoapele sistemului
inertial. Ecuaţia cuaternionului care urmează să fie integrată este:

 0 z  y x   q1 
  y   q2 
1  z 0 x
 
Q
2  y x 0 z   q3 
  
 x  y z 0   q4 

Conform acestei metode, elementele cuaternionului la momentul tn 1 se calculează din elementele la

momentul t n utilizând relaţia:

 q1   Cm Sm z (t n )  Sm  y (t n ) Sm x (t n )   q1 
q    S  (t ) Cm Sm x (t n ) Sm  y (t n )   q2 
 2   
m z n

 q3   Sm  y (t n )  Sm x (t n ) Cm Sm z (t n )   q3 
    
 q4  t n1
  Sm x (t n )  Sm  y (t n )  Sm z (t n ) Cm   q4  t n

unde Cm , Sm sunt coeficienţii de ordin m, iar x ,  y , z reprezintă incrementele unghiurilor de ruliu,
tangaj şi giraţie, care au forma:

tn1
x (t n )   x (t n ) dt  x (t n )(t n 1  t n )  x (t n ) t
tn
tn1
 y (t n )    y (t n ) dt   y (t n )(t n 1  t n )   y (t n ) t
tn
tn1
z (t n )   z (t n ) dt  z (t n )(t n 1  t n )  z (t n ) t
tn

n Cn Sn

1 1 1/ 2

2 1  02 / 8 1/ 2

3 1  02 / 8 1/ 2  02 / 48

4 1  02 / 8  04 / 384 1/ 2  02 / 48

 5 1  02 / 8  04 / 384 1/ 2  02 / 48  04 / 3840

6 1  02 / 8  04 / 384  06 / 46080 1/ 2  02 / 48  04 / 3840

În relațiile stabilite pentru incrementele unghiurilor sa considerat că unghiul

vitezelor este aproximativ constant în timpul unui ciclu Δt de achiziționare de date de la giroscoape în
calculator.
În acest tabel apare norma 0 , care se poate calcula cu formula:

0 (t n )  x2 (t n )   y2 (t n )  z2 (t n )
Astfel, cuaternionul de atitudine se poate scrie:
q1 (t n 1 )  Cm q1 (t n )  Sm z (t n )q2 (t n )  Sm  y (t n ) q3 (t n )  Sm x (t n ) q0 (t n )
q2 (t n 1 )   Sm z (t n )q1 (t n )  Cm q2 (t n )  Sm x (t n )q3 (t n )  Sm  y (t n ) q0 (t n )
q3 (t n 1 )  Sm  y (t n )q1 (t n )  Sm x (t n )q2 (t n )  Cm q3 (t n )  Sm z (t n ) q0 (t n )
q0 (t n 1 )   Sm x (t n )q1 (t n )  Sm  y (t n )q2 (t n )  Sm z (t n ) q3 (t n )  Cm q0 (t n )  
Conform procedeului de ortonormare rezultă norma:
| Q(t n1 ) | q02 (t n1 )  q12 (t n1 )  q22 (t n1 )  q32 (t n1 )
Noile componente ale cuaternionului sunt:
q1 (t n 1 ) q (t )
q1 (t n 1 )  , q2 (t n 1 )  2 n 1
| Q(t n 1 ) | | Q(t n 1 ) |
q3 (t n 1 ) q (t )
q3 (t n 1 )  , q0 (t n 1 )  0 n 1
| Q(t n 1 ) | | Q(t n 1 ) |
În forma matricială avem:
 q02  q12  q22  q32 2(q1q 2  q 0 q3 ) 2(q1q3  q 0 q 2 ) 
 
R   2(q1q 2  q 0 q 3 ) q02  q22  q12  q32 2(q 2 q3  q 0 q1 ) 
 2(q1q 3  q 0 q 2 ) 2(q 3q 2  q 0 q1 ) q02  q32  q12  q22 

Elementele rezultante ale matricii sunt:
 r11 (t n 1 )  q 02 (t n 1 )  q12 (t n 1 )  q 22 (t n 1 )  q 32 (t n 1 )
r12 (t n 1 )  2[q1 (t n 1 ) q 2 (t n 1 )  q 0 (t n 1 ) q 3 (t n 1 )]
r13 (t n 1 )  2[q1 (t n 1 ) q 3 (t n 1 )  q 0 (t n 1 ) q 2 (t n 1 )]
r21 (t n 1 )  2[q1 (t n 1 ) q 2 (t n 1 )  q 0 (t n 1 ) q 3 (t n 1 )]
r22 (t n 1 )  q 02 (t n 1 )  q 22 (t n 1 )  q12 (t n 1 )  q 32 (t n 1 )
r23 (t n 1 )  2[q 3 (t n 1 ) q 2 (t n 1 )  q 0 (t n 1 ) q1 (t n 1 )]
r31 (t n 1 )  2[q1 (t n 1 ) q 3 (t n 1 )  q 0 (t n 1 ) q 2 (t n 1 )]
r32 (t n 1 )  2[q 2 (t n 1 ) q 3 (t n 1 )  q 0 (t n 1 ) q1 (t n 1 )]
r33 (t n 1 )  q 02 (t n 1 )  q 32 (t n 1 )  q12 (t n 1 )  q 22 (t n 1 )

Se obţin unghiurile de atitudine:

  arctg (r23 / r33 )
  arcsin(r13 )
  arctg (r12 / r11 )

Metoda filtrului complementar

Filtrul complementar actioneaza prin efectuarea unui filtraj cu pondere mai mica pentru datele
masurate de catre accelerometru si un filtraj cu pondere mai mare pentru datele achizitionate de
giroscop care implica practic calcule de mediere a valorilor celor doi senzori. O astfel de metoda da
rezultate satisfacatoare oferind un reglaj bun, dar lent din cauza medierii, lucru care produce întârzieri.
Mai mult, acesta neglijează tendinta giroscopului, ceea ce poate provoca mari compensatii ale referintei
în cazul în care tendinta giroscopului se modfica. Mai jos se poate vedea schema generala a unui filtru
complementar.

Această metoda poate fi folosită pentru a integra separat datele de la accelerometru cu

componentele x şi y de la giroscop şi componenta z cu datele de la magnetometru.
 k  ( k 1    dt )  a   acc (1  a)

Această ecuaţie lucrează în domeniul de frecvenţă şi realizează o însumare a estimatei accelerometrului

printr-un filtru trece jos cu estimata giroscopului printr-un filtru trece sus.
Metoda Kalman
Filtrul Kalman, de asemenea cunoscut sub numele de estimator pătratic liniar (LQE), este un
algoritm care utilizează o serie de măsurători observate în timp, care contin zgomot (variatii aleatorii) sau
alte inexactităti si produce estimări ale vaiabilelor necunoscute, care tind să fie mai precise decât cele
bazate pe o singură măsurare. Mai formal, filtrul Kalman functionează recursiv pe fluxuri de date de
intrare zgomotoase pentru a produce o estimare optimala statistica a starii de la baza sistemului. Filtrul
este numit dupa R udolf (Rudy) E. Kálmán, unul dintre dezvoltatorii principali ai teoriei sale.
Algoritmul are la baza un proces în două etape. Etapa de anticipare produce estimatele variabilelor
de stare curente, cât şi incertitudinile. Această etapă poate fi realizată de mai multe ori, anticiparea
devenind din ce în ce mai puţin precisă cu fiecare iteraţie. Etapa de actualizare compară aceste estimări cu
măsurătorile care au zgomot, modificând estimatele utilizând o combinaţie ale valorilor estimate şi a celor
măsurate, valorile estimate cu precizie mare având un aport mai mare. În final, covarianţa estimatei poate
fi actualizată, reprezentând acurateţea noii estimate după comparaţia cu datele măsurate. Estimata
realizată poate fi descrisă ca o soluţie optimă doar atunci când sistemul poate fi considerat unul liniar,
afectat doar de procese şi zgomotul măsurătorilor cu o distribuţie gaussiană.
Se poate scrie:

xk  Fxk 1  Buk  N (0, Qk )

z k  Hx  N (0, R k )

unde:

- F este matricea modelului tranziţional care leagă starea precedentă xk 1 cu starea următoare xk
- B este matricea modelului de control care leagă vectorul de control uk de starea următoare xk
- H este matricea modelului de observaţie care leagă starea estimată de vectorul măsurătorilor zk
- N (0,Qk ) şi N (0, Rk ) reprezintă sursele de zgomot gaussian ale varianţelor Qk , Rk şi medie
0.

Ecuaţiile filtrului Kalman

Etapa de anticipare
Estimata stării anticipate: xk |k 1  Fxk 1|k 1  Bu

Covarianţa estimării anticipate: Pk|k 1  FPk 1|k 1F T  Q

Etapa de actualizare
Reziduul masurătorii: y  z  Hxk |k 1
Covarianţa reziduală: S  HPk |k 1 H ' R
S încearcă să prezică cât de credibilă este măsurătoare bazată pe matricea de covarianță P și pe
matricea de covarianță a măsurătorilor . Cu cât este mai mare valoarea zgomotului de măsurare este mai
mare și valoarea lui S, acest lucru înseamnă că nu avem încredere în măsurătorile de intrare atât de mult.
Amplificarea Kalman optimă: K  Pk|k 1H T S 1
Estimata stării actualizate: xk |k  xk |k 1  K y
Covarianţa estimatei actualizate: Pk |k  (1  KH ) Pk |k 1

Metoda Madgwick
Această metoda realizează un filtru care foloseşte reprezentarea cuaternionică a atitudinii
pentru a descrie natura cuplată a acesteia în trei dimensiuni, neavând singularităţi asociate cu
reprezentarea care utilizează unghiuri Euler.
Se consideră cuaternionul care reprezintă orientarea triedrului B în raport cu triedrul A:
A
B qˆ  [q1q 2 q 3 q 4 ]
Toţi cuaternionii care reprezintă o orientare au lungimea unitate, fiind normalizaţi în acest sens:
   
A
B qˆ  [cos  rx sin  ry sin  rz sin ]
2 2 2 2

Unghiurile de atitudine se pot scrie în funcţie de componentele cuaternionului:

 2q2 q3  2q1q4 
  atan  
 2q1  2q2  1 
2 2

  asin  2q2 q4  2q1q3 

 2q3 q4  2q1q2 
  atan  
 2q1  2q4  1 
2 2

Pentru un giroscop cu trei axe se poate scrie vectorul măsurătorilor astfel:

s  [0 x  y  z ]
Orientarea unui triedru legat legat de Pământ faţă de triedrul legat de senzor se obţine cu relaţia:
S
E q ,t SE qˆest ,t 1 SE q ,t t
unde
S
qˆ  s
S
E q E

2
În cazul accelerometrului şi magnetometrului este necesară o metoda de optimizare a calculului, din
cauza utilizării formei cuaternionice a orientării.
f
S
q,t  SE qˆest ,t 1  t
|| f ||
E

Pentru integrarea datelor de la toţi senzorii se utilizează relaţia:

S
E qest ,t   tES q,t  (1   t )SE q ,t ,0   t  1