Toate3 Cursurile

AHRS
IRS
INS
AHRS
• Reprezentarea atitudinii aeronavei. Ecuaţiile de propagare
• Senzori pentru măsurarea vitezei de rotaţie:
– Giroscopul tehnic. Ecuaţiile lui Euler. Ecuaţiile lui Euler modificate.
Giroscopul de viteza.
– Giroscoape MEMS
– Girolaser-ul
• Senzori pentru măsurarea acceleraţiei
• Senzori pentru măsurarea câmpului magnetic terestru
• Mecanizarea ecuaţiilor de propagare
• Data fusion:
– Filtre complementare
– Filtrul Kalman
– Filtrul Madgwick
• Prezentarea informaţiei. EFIS (PFD)
AHRS
Sisteme de coordonate
AHRS
- Inerţial (i-frame): originea in centrul Pamantului

si axele Oxiyizi orientate către stele fixe (NU se
roteşte odată cu Pamantul). Axa Ozi coincide cu
axa de rotaţie - presupusa invarianta. Deşi teoretic
nu este un referenţial inerţial pentru aplicaţiile
practice aproximaţia este nesemnificativa.
AHRS
- Earth (e-frame): originea in centrul Pamantului si

axele Oxeyeze legate de Pamant (se roteşte odata
cu Pamantul). Axa Oze coincide cu axa de rotaţie,
axa Oxe trece prin intersecţia dintre meridianul
Greenwich si ecuator. Se roteşte cu viteza Ω fata
de referenţialul inerţial in jurul axei Ozi.
Cat este Ω?
Rotaţia Pamantului
Ziua solara adevărata – intervalul de timp intre doua treceri ale soarelui la
amiaza. Variază in timp datorita excentricităţii orbitei si înclinării axei: +30s la
perigeu (22 Dec), -18s la echinocţiu (16 Sept).
Ziua solara medie – Media duratelor unei zile pe un an. Prin definiţie egala cu
86400 secunde solare. In 1964 secunda SI a fost egala cu secunda solara.
Acum durata unei zile este un pic mai mare (0.2ms) in secunde SI din cauza
încetinirii rotaţiei Pamantului.
Ziua stelara - Definita fata de stele fixe are 23h 56m 4.098903691s.
Ziua siderala – Definita fata de echinoxul vernal (care se deplasează datorita

precesiei orbitei) are 23h 56m 4.090 530 832 88s (diferenţa este 8.4ms)
Viteza de rotaţie a Pamantului fata de un sistem inerţial a fost in 2005:
(7.2921150 ± 0.0000001) ×10−5 rad/s

AHRS
- de navigaţie (n-frame): este legat de aeronava

cu originea in centrul de masa al aeronavei P, si
axele orientate nord, est si in jos (după verticala
locului). Viteza de rotaţie in raport cu e-frame este
data de ωen generata de mişcarea punctului P pe
suprafaţa Pamantului (viteza de transport)
AHRS
- local orizontal (w-frame): wander azimuth

frame, are originea in P si orientarea azimutala a
aeronavei. Axa Ozw este orientata in jos. Se obţine
din n-frame prin rotaţia cu unghiul de cap Ψ.
AHRS
- aeronava (b-frame): body frame, are originea in

P (centrul de masa) si orientarea pe axele de
ruliu, tangaj si giraţie. Axa Ozb este “in jos”.
AHRS
Definirea rotatiilor
AHRS
Ordinea de compunere contează!

Rotaţiile NU sunt comutative...
AHRS
Modalităţi de reprezentare
⚫Matricea cosinusurilor directoare: o matrice 3x3 ale

cărei coloane sunt versori sistemului legat de
aeronava proiectaţi pe axele sistemului de referinţa (de
obicei n-frame)
Unghiurile Euler
⚫
⚫Cuaternioni: o reprezentare a trecerii de la un sistem

la altul printr-o singura rotaţie in jurul unui vector definit
fata de sistemul de referinţa. Un cuaternion are o
reprezentare vectoriala ca un vector de dimensiune
patru.
AHRS
Matricea cosinusurilor de rotaţie
O vom nota Cnb (matricea de trecere de la

triedrul aeronava la cel de navigaţie). Are forma:
Elementul de pe rândul i si coloana j reprezintă

cosinusul unghiului dintre axa i a triedrului de
referinţa si axa j a triedrului aeronava.
AHRS

Trecerea de la un sistem la altul folosind matricea
cosinusurilor.
Propagarea matricei cosinusurilor directoare in

timp.
Cele doua matrice ale cosinusurilor directoare provin una din
cealaltă prin rotaţie. Daca timpul este mic se poate scrie in
prima aproximaţie:
Unde cu A s-a notat matricea de trecere de la timpul t la t+δt.

Daca unghiurile sunt mici (viteza de rotaţie este mica si/sau
intervalul de timp este mic) atunci:
Unde cu ψ s-au notat unghiurile de atitudine iar I este matricea

unitate.
δψ, δθ si δφ sunt variaţiile mici ale unghiurilor cu

care triedrul aeronava (b-frame) s-a rotit in
timpul δt in jurul axelor de giraţie, tangaj
respectiv ruliu.
Înlocuind in ecuaţia de propagare obţinem:
La limita, când δt->0 variaţiile unghiurilor devin
viteze unghiulare de rotaţie ale aeronavei fata
de triedrul de navigaţie:
notaţii cu care ecuaţia de propagare devine:

Explicitând termeni din ecuaţia de propagare
avem:
Unghiurile Euler
Deşi ecuaţiile de propagare ale matricei cosinusurilor de
rotaţie rezulta in final simple, lipsa unei înţelegeri
intuitive impune folosirea (cel puţin la interfaţa
om/maşina) a unei reprezentări alternative.
Orice transformare de la un triedru la altul poate fi

modelata prin trei rotaţii succesive. Acestea sunt (in cazul
Euler):
- rotaţie cu unghiul ψ in jurul axei z (giraţie)
- rotaţie cu unghiul θ in jurul noii axe y (tangaj)
- rotaţie cu unghiul φ in jurul noii axe x (ruliu)
Unghiurile Euler
Cele trei rotaţii se pot reprezenta simplu ca matrici
de rotaţie decuplate:
In jurul lui z cu unghiul ψ
In jurul lui y cu unghiul θ
In jurul lui x cu unghiul φ

Unghiurile Euler
O transformare de la triedrul de navigaţie (n) la cel
aeronava (b) se poate scrie simplu:
Iar transformarea inversa (din triedrul aeronava in cel de

navigaţie):
Unghiurile Euler
Explicitând:
Unghiurile Euler
Pentru unghiuri mici cosinusurile tind la 1 iar
sinusurile pot fi aproximate cu unghiul:
Neglijând elementele mici de ordin superior avem:

Ecuaţia de propagare a
unghiurilor Euler
Unghiurile Euler NU sunt definite in axele aeronavei si ca
urmare NU se pot măsura direct cu traductoare de
viteza unghiulara.
Totuşi daca dispunem de o platforma inerţiala mecanica

care modelează triedrul de navigaţie unghiurile Euler
rezulta natural intre cadrele de suspensie ale
platformei si pot fi măsurate. De asemenea vitezele de
rotaţie ale cadrelor sunt derivatele unghiurilor Euler.
Din acest motiv se numesc viteze cardanice.
Ecuatia de propagare a
unghiurilor Euler
Legătura cu viteza de rotaţie ale aeronavei este
data de:
Ecuaţia poate fi rearanjata ca:

Cuaternioni
Cuaternioni (sau quaternioni) sunt numere
hipercomplexe introduse de Hamilton in 1843.
Sunt folosiţi in special pentru reprezentarea
rotaţiilor.
Un cuaternion poate fi reprezentat ca un “vector”
de dimensiune patru.
Cuaternioni
Cuaternioni rezolva trecerea de la un sistem de
referinţa la altul printr-o singura rotatie in jurul
unui vector μ. Scalarul μ este unghiul de rotatie
iar μx, μy, μz sunt proiecţiile vectorului μ pe axele
sistemului de referinţa.
In reprezentare hiper-complexa cuaternioni se pot
scrie ca:
Reprezentarea Cuaternionica
AHRS: Unde am ajuns?
• Reprezentarea atitudinii aeronavei. Ecuaţiile de
propagare
• Senzori pentru măsurarea vitezelor de rotatie
– Giroscopul tehnic. Ecuaţiile lui Euler. Ecuaţiile lui Euler
modificate. Giroscopul de viteza
– Giroscoape MEMS
– Girolaserul
• Senzori pentru măsurarea acceleraţiilor
• Data fusion. Filtrul Kalman
Cuaternioni
Cuaternioni (sau quaternioni) sunt numere
hipercomplexe introduse de Hamilton in 1843.
Sunt folositi in special pentru reprezentarea
rotatiilor.
Un cuaternion poate fi reprezentat ca un “vector”
de dimensiune patru.
Cuaternioni
Cuaternioni rezolva trecerea de la un sistem de
referinta la altul printr-o singura rotatie in jurul
unui vector μ. Scalarul μ este unghiul de rotatie
iar μx, μy, μz sunt proiectiile vectorului μ pe axele
sistemului de referinta.
In reprezentare hipercomplexa cuaternioni se pot
scrie ca:
Cuaternioni
Pe baza relatiilor de definitie se poate deduce
relativ usor regula de multiplicare a
cuaternionilor:
Sau alternativ in format matricial:

Folosirea cuaternionilor pentru
transformari vectoriale
Fie vectorul (in sisteml de coordonate legat de
aeronava):
Asociem o reprezentare cuaternionica avand

partea reala zero:
In aceste conditii reprezentarea cuaternionica in

sistemul de navigatie este
Unde q* este (hiper)complex conjugatul cuaternionului de
rotatie care face trecerea de la b la n.
Deci:
Sau sub forma matriciala:
Unde am folosit notatiile deja introduse:
Iar matricea C rezulta:

Daca suprimam zerourile formularea matriceala
este evident echivalenta cu:
Daca suprimam zerourile formularea matriceala

este evident echivalenta cu Cnb – matricea
cosinusurilor directoare.
Am gasit deci o cale de a scrie matricea
cosinusurilor functie de cuaternionul de rotatie.
Propagarea in timp a
cuaternionului de rotatie
Cuaternionul de rotatie q care asigura trecerea de
la sistemul legat de aeronava la cel de navigatie
se propaga in timp dupa ecuatia:
Unde am notat pbnb cuaternionul asociat vitezei de

rotatie instantanee masurate in axele aeronavei:
Sau explicitand in format matricial:
Pentru implementare efectiva scrierea pe

componente este mai utila asa incat avem:
Sistemul de ecuatii de mai sus se poate folosi pentru

calculul in timp real al lui q din care apoi se obtine apoi
matricea cosinusurilorde rotatie Cnb
Alternativ, q se pote folosii direct pentru conversia de
coordonate dupa ecuatia:
Relatia dintre cuaternioni si
unghiurile Euler
Din pacate nici cuaternionii nici matricea cosinusurilor de
rotatie nu ofera o imagine intuitiva a atitudinii aeronavei.
Plecand de la matricea Cbn
unghiurile Euler
Egaland se obtine sistemul:
unghiurile Euler si matricea
cosinusurilor
Euler C bn
q
Rezolvand sistemul intr-o directie sau in cealalta
putem obtine expresiile elementelor
cuaternionului q in functie de termenii matricii
cosinusurilor, unghiurilor Euler sau invers.
q functie de Cbn
q functie de unghiurile Euler
D=*-*
Unghiurile Euler functie de Cbn
Daca unghiul θ nu este 90o (sau aproape 90o)

putem utiliza relatia simpla:
Daca unghiul θ se apropie de 90o unghiurile φ si ψ
devin nedefinite. Acest lucru poate fi evitat
recurgand la relatii intre elementele din matrice
ce nu devin nule
Pentru θ aproape de +90o
Pentru θ aproape de -90o

Inabilitatea de a gasi expresii separate pentru ψ si
φ se datoreaza faptul ca sistemul unghiurilor
Euler pierde in acest caz un grad de rotatie (o
axa) ambele unghiuri devenind definite pe axe
paralele.
Solutia practica folosita este sa “inghetam” unul
dintre unghiuri, pe rand la fiecare pas de timp,
atunci cand intram in zona de θ apropape 90o si
sa-l calculam pe celalalt cu formulele date.
Acest proces se termina cand θ iese din zona
de nedeterminare.
MATLAB!!!
- Reprezentarea atitudinii.
- Ecuatiile de propagare.
Ecuatiile lui Euler modificate
propagare
– Giroscopul tehnic. Ecuaţiile lui Euler. Ecuaţiile lui
Euler modificate. Giroscopul de viteza
– Giroscoape MEMS
– Girolaserul
Giroscopul. Ecuatiile lui Euler
Ecuatiile lui Euler
Ecuatiile lui Euler
Viteza unui punct de pe un “rigid cu punct fix”
  
V =
Momentul unui vector in raport cu originea
  
M = A
Impulsul unui punct material
 
H = m •V
Ecuatiile lui Euler
In cazul unui sistem de puncte materiale
 n  n 
H =  H i =  mVi
i =1 i =1
Momentul cinetic (momentul cantitatii de

miscare) a unui punct material de masa
m aflat la r de origine ce se misca cu H
este prin definitie momentul impulsului
    
K = r  H = r  mV
Ecuatiile lui Euler
In cazul unui sistem de puncte materiale
 n  n
 
K =  K i =  ri  miVi
i =1 i =1
Pentru un corp solid continuu suma se transforma in

integrala de volum
   
K =    (   )dm

Ecuatiile lui Euler
Pentru rigidul cu punct fix O in originea sistemului de

coordonate Oxyz avem
   
ri = x  i + y  j + z  k
   
 = x  i +  y  j + z  k
   
V = Vx  i + V y  j + Vz  k
Ecuatiile lui Euler
Se obţine
 
K x =  x ( y + z ) −  y xy − z xz dm
2 2
 
K y =   y (x + z ) − z yz − x yx dm
2 2
 
K z =  z (x + y ) − z zx −  y zy dm
2 2

Ecuatiile lui Euler
Sau
K x = J x   x − J xy   y − J xz   z
K y = J y   y − J yz   z − J xy   x
K z = J z   z − J zx   x − J zx   y
Ecuatiile lui Euler
Unde s-au introdus momentele de inertie axiale
(
J x =  y + z dm
2 2
)

(
J y =  z 2 + x 2 dm )

(
J z =  x + y dm
2 2
)

J xy =  xy  dm J xz =  xz  dm etc

 
Ecuatiile lui Euler
Daca sistemul de referinta este astfel ales incat Oxyz

sa fie axe principale de inertie atunci
J xy = J yz = J zx = 0
Si
     
K = J x x  i + J y  y  j + J z z  k = K x  i + K y  j + K z  k
Ecuatiile lui Euler
Teorema momentului cinetic rezulta din derivarea ecuaţiei de
definire a lui K
        
K =  r  miVi +  r  miV = Vi  miV +  ri  mi ai =
i i i i
    
 ri  F (i) +  ri  F (e) =  M i (e)
i i i
Adică variaţia momentului cinetic este datorata momentelor

exterioare (teorema momentului cinetic)

 dK 
K= = M (e)
dt
Ecuatiile lui Euler
Daca vectorul K se roteşte cu viteza 

fata de triedrul fix Ox1y1z1
 
dK K   
= +   K = M (e)
dt t
Daca K este constant in modul ecuaţia devine ecuaţia de precesie
  
  K = M (e)
Sau (scrisa pentru momentul de reacţiune) ecuaţia
momentul giroscopic
  

M g = −  K = K  
Ecuatiile lui Euler
Înlocuind in teorema momentului cinetic obţinem ecuaţiile Euler
J x x + ( J z − J y ) y  z = M x
J y  y + ( J x − J z ) z  x = M y
J z  z + ( J y − J x ) x y = M z
Triedrul Resal
Schimbând sistemul de coordonate la triedrul Resal obţinem

ecuaţiile lui Euler modificate
J x cos  − 2 J y sin  + J z ( +  sin  )  = M x 3

J x  + J y 2 sin  cos  − J z ( +  sin  ) cos( ) = M y 3
 +  cos  +  sin  ) = M
J z ( z3
propagare
– Giroscoape MEMS
– Girolaserul
Ecuatiile Euler
J x x + ( J z − J y ) y z = M x
J y y + ( J x − J z ) z x = M y
J z z + ( J y − J x ) x y = M z
Ecuatiile Euler sunt scrise in axele proprii

(principale) ale giroscopului intr-un sistem de
coordonate care se roteste cu giroscopul…
Triedrul Resal
Triedrul Resal
• Triedrul Resal (Ox3y3z3) este caracterizat prin

faptul ca nu participa la miscarea proprie de
rotatie a giroscopului. Axa Oz3 este axa de
rotatie, Oy3 este axa inelului cardanic interior iar
Ox3 rezulta astfel incat sistemul sa fie drept.
• Triedrul Ox1y1z1 este triedrul fix

Triedrul Resal
• Trecerea se face prin doua rotatii succesive cu
unghiul α in jurul lui Ox1 respectiv ß in jurul lui
Oy3.
• Avantajul principal este ca cele doua unghiuri se

pot masura usor fiind materializate fizic.
• Ecuatiile isi pierd insa din generalitate si

simetrie cele doua unghiuri masurandu-se in
triedre diferite
Momentul cinetic in axele
triedului Resal
Proiectia vitezei de rotatie a triedrului Resal fata de
triedrul fix este
   
 =   cos(  )  i3 +   j3 +   sin(  )  k 3
Momentul cinetic al giroscopului in axele triedrului

Resal rezulta
   
K = J x 3  cos(  )  i3 + J y 3  j3 + J z 3 ( +   sin(  ))  k 3
Momentul fortelor exterioare in raport cu axele triedrului Resal

   
M = M x 3 i3 + M y 3 j3 + M z 3 k 3
Din teorema momentului scrisa in triedrul Resal obţinem

ecuaţiile lui Euler modificate
J x cos  − 2 J y sin  + J z ( +  sin  )  = M x 3


J x  + J y sin  cos  − J z ( +  sin  ) cos( ) = M y 3
 2
 +  cos  +  sin  ) = M

J z ( z3
Daca α si β sunt mici putem aproxima sin-usurile cu

unghiul si cos-inusurile cu 1. Viteza proprie de rotatie
este mult mai mare decat celelalte componente:
   , 
Cu aceste simplificari ecuatiile Euler modificate devin:
J x + J z  = M x 3
J  − J  = M
x z y3
 =M
J z z3
Unde cu J am nota momentul de inertie ecuatorial si cu
Jz=Jp momentul de inertie polar al giroscopului.
OBS: Am neglijat in mod intentionat momentele de inertie
ale cadrelor de suspensie…
Ultima ecuatie este ecuatia de actionare a motorului
giroscopic si in regim stabilizat putem considera Mz3=0 (Ω
este constant).
Primele doua reprezinta ecuatiile simplificate ale
giroscopului rapid in cazul deviatiilor mici. Sa analizam
cazul particular My3=My=const si Mx3=0.
Ecuatiile simplificate ale
giroscopului rapid in cazul
deviatiilor mici
J x + J z  = 0
J  − J  = M
x z y
Integrand obtinem
My JM y  J p 
 (t ) = − t+ sin  t 
J p  ( J p ) 2
 J 
JM y   J p  
 (t ) =  1 − cos  t  
( J p ) 2   J 
 
Ecuatiile simplificate ale
giroscopului rapid in cazul
deviatiilor mici
Distingem doua componente:
• O miscare cu viteza unghiulara constanta
My
 = −
J p
numita miscare de precesie si o miscare oscilatorie
exprimata de termenii armonici numita miscare de
nutatie. Amplitudinea maxima a miscarii de nutatie
este:
JM y
 max =  max =
*
( J p ) 2 MATLAB
Exemplu
Sa consideram cazul unui
rotor giroscopic tipic de
forma cilindrica de raza 1.8
cm si inaltime 2 cm
considerat pentru simplitate
omogen (densitate
8000kg/m3) cu Ω=24000 mr 2
rot/min si un moment Jz = J p =
2
perturbator de My=10gfcm. 1
(
Jx = Jy = Je = m 3r 2 + h 2
12
)
Concluzi (la exemplu)
• Pentru giroscoapele tehnice miscarea de nutatie
poate fi neglijata
• Miscarea de precesie peste care se suprapune o
nutatie de amplitutine mica si frecventa mare se
numeste precesie pseudoregulata
• Ecuatiile se pot simplifica (neglijand nutatie) la
ecuatiile tehnice ale giroscopului:

 p
J  
 = M x3


− J p   = M y3
Influenta suspensiei cardanice.
Colapsul giroscopic
Includerea momentelor de
inertie ale axelor cardanice
conduce la o ecuatie
asemanatoare de forma:
A(  ) + K = M 1
B − K = M 2
Unde cu A,B si C sau notat marimi de natura momentelor de

inertie reduse la axele 1,2,3 iar K este raportat la acelasi axe*
*Pentru o deducere a acestei formule vezi Aron, Aparate de bord pentru aeronave
Colapsul giroscopic
Din ultimile ecuatii, prin neglijarea nutatiei si explicitarea lui
K se obtine:
M2
 = R
C 3' (t ) cos( 2 )
K 0 sin(  0 )
sin  = R
C 3' (t )
Unde CR este moment de inerţie, β2 este unghiul de rotaţie la

care inelul interior loveşte in limitatoare iar ω3 este app. Ω.
La accelerarea motorului giroscopic (ω3 creste) β scade si
giroscopul se aseaza in poziţie canonica, iar la frânare β
creste, viteza de precesie creste si giroscopul colapseaza.
Giroscopul de viteza
In regim stabilizat momentul de precesie este egalat de
momentul elastic si putem scrie:
K
 st =  y 2 cos( st )
k1
Daca se considera β mic atunci cos β=1 si
K
 st =  y 2
k1
Aparatul prevazut cu ac este indicatorul de viraj insa pe
noi ne intereseaza semnalul electric de iesire aparatul
numindu-se transmitator de viteza unghiulara.
Indicatorul de viraj
Ecuatia giroscopul de viteza
In raport cu axa Ox avem:
• Cuplul fortelor de inertie provocat de acceleratia in
jurul axei Ox
( )
M x = J x  −  x 2
1
• Cuplul datorat amortizorului

 2 
M = k2  = ka d 2 
2
x
• Cuplul rezistent elastic

M = k1 = k d 
3
x e 1
2
Ecuatia giroscopului de viteza
• Cuplul perturbator (frecari, …)
M = Mp
4
x
• Cuplul giroscopic
M = M g = − K   y = − K ( y 2 cos(  ) −  z 2 sin(  ) )
5
x
Din echilibrul momentelor pe axa Ox rezulta ecuatia

fundamentala a giroscopului de viteza:
J x  + k 2  + k1 = K y 2 cos(  ) − K z 2 sin(  ) + J  x 2 − M p

Constructia transmitatorului de
viteza unghiulara
Constructia transmitatorului de
viteza unghiulara
propagare
– Giroscoape MEMS
– Girolaserul
Erorile giroscopului de viteza
Ecuaţia fundamentala a giroscopului de viteza este:
J x  + k 2  + k1 = K y 2 cos(  ) − K z 2 sin(  ) + J  x 2 − M p

Pentru a ne face o idee despre diversele surse de eroare
sa consideram câteva cazuri particulare
Daca ωz2 si ωx2 sunt zero, unghiul β este mic (cos β=1) si
considerând doare regimul stabilizat avem:
k1 st = K y 2 − M p
K Mp
 I
st = y2 − =  st − 
k1 k1
Se defineşte deriva giroscopului de viteza:
Mp
d =
K
(expresia erorii ca variaţie a semnalului de intrare). Deriva
unui senzor este exprimata ca eroare a mărimii măsurate.
Ea se manifesta ca o mărime aditiva (zero fals) peste
mărimea măsurata.
• O alta mărime de interes este pragul de sensibilitate.
• In cazul giroscopului aceasta este viteza de rotaţie minima
pe care giroscopul o poate măsura si este determinata de
momentul de frecare (Mf) in axa Ox.
Mf
min =
K
• Determinant este in acest caz momentul de frecare in
lagărul de suspensie
– Creste cu încărcarea normala a lagărului
– Creste cu creşterea diametrului lagărului
Modalitati de reducere a pragului de sensibilitate

– Descarcarea lagarului (flotarea camerei giroscopice)
– Utilizarea unui sistem de suspensie cu bare de
torsiune.
– Cresterea lui K in raport cu Mf. Acest lucru se reduce
la cresterea vitezei proprii de rotatie a giroscopului si
este limitata de tehnologia lagarelor principale.
Erorile dinamice
• De data aceasta vom considera Mp zero si β mic
(cos β=1), dar nu vom mai ignora dinamica:
J x  + k 2  + k1 = K z 2
Impartind cu Jx si folosind notaţiile consacrate avem:
  K
 + 20  + 0  =  z 2
2
unde Jx
k2 k1 k2
20 =  =
2
0 =
Jx Jx 2 J x k1
Funcţia de transfer se obţine ca raport intre ieşire si
intrare, deci:
 ( s) K 1
Y ( s) = =
 y 2 ( s) J x s 2 + 20 s + 02
Raspunsul la treapta unitate (ωy2=1, ωy2(s)=1/s) se
obtine multiplicand functia de transfer cu
transformata semnalului.
K 1
 ( s) =
J x s( s + 20 s + 0 )
2 2
Originalul (transformata Laplace inversa) este
K  e −0t 
 (t ) = 1 − sin(1t +  )
k1  1−  2

unde
 1−  2 
1 = 0 1 −  2
 = arctg  
  
 
Ne intereseaza valoarea maxima a lui β(t) care se
obtine anuland prima derivata:

K − 
 M = 1 + e 1−

2
k1  
 
Suprareglajul este deci:

−
K 1− 2
 M = e
k2
Mărimea ΔβM reprezintă eroarea dinamica a giroscopului
MATLAB
Răspunsul giroscopului la semnal sinusoidal. Semnalul de
intrare are forma:
 y 2 (t ) =  y 0 sin(t )
Iar marimea de iesire (in transformata Laplace) este
K 
 ( s) =
( )(
J x s 2 +  2 s 2 + 20 s + 02 )
Revenind in domeniul timp, obtinem un semnal de forma:
 (t ) = A sin(t −  )
• Unde A este amplitudinea si ε este defazajul
semnalului de iesire fata de cel de excitatie
K y 0 1
A= = A0 
J x0  2 2
 2
 2  
1 −    + 4  
  0  
   0 
2 
20 0
tg = 2 =
0 −  2
  
2
1−  
  0
MATLAB
Concluzii
Banda de trecere a senzorului este deci
limitata la 1/8-1/10 din frecventa proprie de
rezonanta a senzorului.
Amortizarea optima este ξ=0.707.
Eroarea datorata rotirii inelului de suspensie
In acest caz consideram β mic, Mp=0, ωz2<>0 si neglijam

regimul dinamic:
K 1
 st =  y 2 = S ' y 2
k1 1 + K z 2
k1
Marimea S’ se numeste sensibilitate si in acest caz este
dependenta de viteza de rotatie pe o alta axa decat cea de
intrare. Aceasta eroare este multiplicativa si se numeste
eroare de factor de scala.
Notiuni de retinut
• Deriva nulului
• Pragul de sensibilitatea
• Frecventa proprie de oscilatie
• Sensibilitatea
• Eroare de factorul de scala
Giroscoape MEMS
– Giroscoape MEMS. Forţa Coriolis. Giroscoape vibratorii. Tehnologia
MEMS. Elemente funcţionale mecanice. Elemente funcţionale
electrice
– Girolaserul
Forta Coriolis
Forta Coriolis este o forta fictiva (aparenta) care pare sa
actioneaze asupra corpurilor in miscare rectilinie
uniforma atunci cand acestea sunt observate intr-un
referential aflat in miscare de rotatie (neinertial).
Desi cunoscuta de multa vreme a fost explicata in 1835
de Gaspard Gustave Coriolis.
Forta Coriolis este fictiva in sensul ca termenul respectiv

provine dintr-o acceleratie care apare natural la
derivarea traiectorie intr-un sistem neinertial
Forta Coriolis
Exemple tipice:
- devierea cursurilor de
apa si a curentilor de aer
in sensuri diferite in cele
doua emisfere
- pendulul lui Foucault
Giroscoape vibratorii liniare
Notatii (consideram miscarea 2D):
A – Sistemul de referinta inertial
B – Sistemul legat de senzor (neinertial, in miscare de
rotatie)
rA – vectorul pozitie in sistemul inertial
rB – vectorul pozitie in sistemul neinertial
θ – unghiul de rotatie a lui B fata de A
Ω – viteza unghiulara a lui B fata de A, Ω=dθ/dt
R – vectorul de pozitie al originei sistemului B in A
Regula de derivare intr-un sistem neinertial este:

Avem succesiv:
Notam cu vB si aB viteza si acceleratia in sistemul

neinertial, cu aA acceleratia in sistemul inertial si cu A
acceleratia de transport (a lui B fata de A)
Obtinem
Putem identifica trei componente ale acceleratiei

Acceleratia locala in B
Acceleratia centripeda
Acceleratia Coriolis
In cazul unui giroscop vibratoriu cele doua axe locale

sunt respectiv axa de excitatie si cea de masura.
Descompunand forta pe cele doua axe si luand in
considerare constantele elastice si de amortizare
obtinem:
Unde cu τ sau notat fortele exterioare

Unii termeni se pot neglija. Daca:
- consideram viteza de rotatie pe axa z constanta
(derivata zero)
- de asemenea viteza de masurat (Ωz) mult mai mica
decat frecventa de excitatie astfel incat termenii Ωx si Ωy
sunt mici. Putem neglija termeni de forma ΩxΩy si Ωx2,
Ωy2.
Ecuaţia de mai sus descrie dinamica simplificata a
giroscopului vibrator cu doua grade de libertate (2DOF).
In care τx este forţa de excitaţie (sinusoidala) si τy

corespunde unor forte parazite si acceleraţiilor externe
pe axa de măsura.
Termenul Coriolis este cel care ne interesează fiind cel

care generează mărimea de ieşire. O metoda frecventa
de creştere a semnalului de iesire este proiectarea
dinamicii in aşa fel incat frecventa de excitaţie sa fie
frecventa de rezonanta pe direcţia de măsura.
Giroscoape vibratorii de torsiune
Legea de conservare a momentului cinetic (H=Iω) scrisa
intr-un sistem inerţial este:
La trecerea intr-un sistem neinerţial care se roteşte cu

aceeaşi viteza ω, ecuaţia devine:
Pentru o configuraţie ca in figura, explicitand ecuaţia
obţinem:
Unde:
- Is si Id sunt momentele de inerţie proprii ale masei inertiale
respectiv ale cadrului.
- ωss si ωdd sunt vitezele unghiulare ale masei inerţiale respectiv
ale cadrului
- τse si τde sunt momentele elastice
- τsd si τdd sunt momentele amortizoarelor
- Md este momentul de excitaţie
Fie θd unghiul de rotatie al cadrului fata de
substrat si φ unghiul de rotatie al masei inertiale
(fata de substrat). Matricile de rotatie sunt:
Folosind matricile de rotatie vectori viteza de
rotatie devin:
Inlocuind in legea de conservare si facand acelasi
aproximatii ca in cazul giroscopului liniar obtinem
ecuatiile pe cele doua axa (de excitatie si de masura)
Unde am exprimat totul in marimi scalare (indici x, y, z),

marimea D este raportul intre constanta de amortizare
si cea elastica iar marimile K sunt rigiditati la torsiune.
Daca acceleraţia unghiulara pe axa de intrare este 0
(mişcare de rotaţie constanta) si neglijând mărimile
mici de ordinul 2 obţinem:
Termenul este momentul Coriolis pe

care masa inerţiala il exercita pe axa de măsura iar
unghiul φ este mărimea de ieşire.
A doua ecuaţie este ecuaţia de excitaţie.
Giroscoape vibratorii
Comparand ecuatiile giroscopului liniar cu cele ale
giroscopului de torsiune remarcam ca sunt
asemanatoare.
Dinamica si modul de functionare ale ambelor variante
constructive sunt aceleasi.
Ca urmare vom studia mai in detaliu (fara a pierde din
generalitate) doar unul dintre ele.
Din punct de vedere al dinamicii cele doua ecuatii
(excitatie si masura) sunt decuplate asa incat pentru
simplificare putem studia doar dinamica cu un singur
grad de libertate (1DOF)
Oscilatorul inertial monoaxial
(1DOF)
(1DOF)
Ecuatia oscilatorului cu un singur grad de libertate de
masa m avand constanta elastica k si cea de
amortizare c este:
Introducand notatiile consacrate pentru frecventa proprie

si amortizare avem:
(1DOF)
De unde putem scrie imediat functia de transfer
(raportata la forta de excitatie)
Forta de excitatie este in cazul nostru o marime

armonica de forma:
Raspunsul sistemului va fi tot o marime armonica de

forma:
(1DOF)
cu
si
(1DOF)
Daca amortizarea este mare atunci amplitudinea de este
maximizata la:
Daca amortizarea este mica (ξ<<1) atunci amplitudinea

de este maximizata la frecventa naturala (proprie) de
oscilaţie, ωn, iar amplitudinea la rezonanta devine:
(1DOF)
Factorul de calitate (Q) este definit ca raportul intre
amplitudinea maxima in regim dinamic si cea
stabilizata pentru acelaşi modul al forţei de excitaţie.
Factorul de calitate este una dintre mărimile importante

ce caracterizează sistemul. La rezonanta putem
exprima amplitudinea funcţie de Q
(1DOF)
O alta marime importanta este lăţimea de banda
(BW) definita ca diferenţa intre frecventele
(stanga-dreapta frecventei de rezonanta) la care
energia înmagazinata de sistem este jumate din
cea de la rezonanta.
Cum energia depinde de pătratul amplitudinii la
amortizări mici avem:
Caracteristca
amplitudine/frecventa
(1DOF)
La rezonanta faza semnalului de iesire este -90o
fata de excitaţie. La frecvente mai mici faza
tinde către 0o iar la frecvente mai mari către -
180o.
Trecerea de la 0 la -180 se face cu atat mai
repede cu cat Q este mai mare.
Caracteristica faza/frecventa
Giroscoape MEMS
• Senzori pentru măsurarea vitezelor de rotaţie
de fabricatie MEMS. Elemente funcţionale mecanice. Elemente
funcţionale electrice
– Girolaserul
Elemente functionale mecanice
Ne dorim sa fabricam:
• elemente flexibile de
suspensie:
– Liniare
– De torsiune
• Mase inertiale
Sisteme de suspensie liniare
Structura de
suspensie pentru
senzori de axa Z
Structura de
suspensie pentru
senzori de axa Y
Suspensia in configuratie “Crab
leg”
Suspensia in S
Suspensia in “ac de par”
Suspensia in H
Decuplarea directiilor de
actionare si masurare
Drive frame
Avantaje/dezavantaje:
•Decuplarea directiilor de
actionare si masura
permite masurarea mult
mai precisa (1-2 ordine de
marime)
•Mai greu de realizat
tehnologic
•Masa inertiala mai mica
Double Frame (Bosh)
Configuratii in antifaza
(tuning fork gyro)
Eliminarea influentei acceleratiilor si vibratiilor exterioare pe directia

de masura. Este configuratia originala introdusa de Murata
Tuning Fork Gyro
Elemente elastice liniare
Momentele de inertie sunt:
Rigiditatea in miscarea
L – lungime (pe axa x) laterala a unei grinzi este:
w – latime (pe axa y)
t – inaltime (pe axa z)
E – modulul de elasticitate
(Young)
Rigiditatile capatului grinzii pe cele trei axe sunt:
Raportul intre rigiditatea pe axa verticala (miscare

nedorita) si pe axa y (miscare dorita) este:
Iar intre axa x si y este:

In realitate este necesara o analiza cu element finit deoarece se
ajunge repede in zona neliniara a caracteristicilor materialului:
De exemplu pentru L=500μm, w=10μm, t=100μm si E=130GPa:
• la 1μm deflexie (domeniul liniar) => F=142μN (ky=142N/m)
• la 10μm deflexie (domeniul neliniar) => F=1980μN (ky=198N/m)
Problema principala la grinda simpla este ca daca capatul nu se
poate deplasa pe axa x atunci fortele care apar pe aceasta
directie sunt foarte mari putand distruge dispozitivul.
In exemplu de mai sus la 10μm pe axa y forta pe x este 40400μN!
Din aceasta cauza se folosesc de obicei structuri la care unul din
capetele grinzii se poate misca liber pe axa longitudinala.
Grinda indoita (in forma de
U) elimina neliniaritatiile si
solicitarile axiale.
Pentru a obtine aceasi rigiditate

putem pune doua astfel de
grinzi in U in paralel. Obtinem
o grinda dublu indoita (W).
In general regula de compunere a rigiditatilor

(serie/paralel) se poate aplica si la structuri mai
complexe pentru o analiza aproximativa.
Sisteme elastice de torsiune
Structura elastica de
torsiune pentru axa
giroscoape de axa Z
Structura elastica de
torsiune pentru axa
giroscoape de axa Y
Elemente elastice de torsiune
Configuratia tipica si
modurile de oscilatie ale
unui giroscop de axa Z
Elemente elastice de torsiune
Arcul de torsiune
Rigiditatea la torsiune este:
Unde
Daca w<<t atunci coeficientul S se poate scrie

Anizoelasticitatea si Erorile de
quadratura
Amplitudinea oscilatiei pe axa de masura este ordine de
marime mai mica decat pe axa de drive.
Ca urmare orice cuplaj elastic parazit intre axe va acoperi
semnalul util.
Amplitudinea semnalului util este:
Daca consideram cele doua mase egale raportul intre

amplitudinile de drive si sens devine:
Eroarea de quadratura
Pentru un senzor tipic avem:
Pentru o viteza de intrare de 1o/s rezulta un raport de 0.000556.

Daca drive-ul este de 10μm atunci miscarea utila pe sens are
amplitudinea de doar 5.56nm!
Ecuatia de miscare in forma matriciala (de acum doua cursuri):

Efectele de cuplaj elastic parazit intre axe (anizoelastice) se
modeleaza ca dermeni nediagonali in matricea de rigiditate:
Termenul kxy se poate neglija deoarece y<<x si oricum cuplajul

invers nu ne afecteaza. Termenul care ne intereseaza este kyx.
Ingloband anizoelasticitatea ecuatia pe axa y devine:
Matricea de rigiditate globala este suma matricilor de rigiditate ale

elementelor. Daca sistemul de suspensie este perfect simetric
atunci termenii nediagonali se anuleaza reciproc. Mici asimetrii
duc insa in realitate la existenta unui kyx diferit de zero.
In cazul suspensiilor de torsiune efectul este
asemanator. Existenta unghiului de
deplanare induce cuplaje intre cele doua
axe. In acest caz coeficientul de cuplaj
este:
Ca in cazul anterior un senzor perfect

simetric va avea coeficientul de cuplaj
zero, dar in realitate nu se poate realiza.
Sa comparam cele doua forte ce actioneaza pe axa de sense:
Efectul erorii de quadratura exprimat in marimea de intrare este:
Sau daca cele doua mase sunt oarecum egale:

Eroarea de cuadratura
Din pacate frecventa de drive mare face ca sa avem erori uriase chiar pentru un
sistem foarte bine realizat. Un exemplu tipic este f=10KHz si raportul
coeficientilor de rigiditate de doar 1%. In acest caz eroare este 18000o/s!
Vestea buna este ca eroarea este in faza cu pozitia x iar semnalul util este
defazat cu 90o find in vaza cu viteza de drive.
Pentru un sistem aflat la rezonanta avem un defazaj de 90o intre semnalul

electric de AC si pozitie de forma:
Faza semnalului util (Coriolis) si a erorii de quadratura sunt deci:
Eroarea de quadratura este
constanta. Acest lucru permite
extragearea semnalului util ca
defazaj al semnalului de iesire.
Alte modalitati de compensare:
• Echilibrarea mecanica finala
(tunning) prin evaporare laser.
• Compensarea electrostatica cu
motoare de DC care
“diagonalizeaza” matricea de
rigiditate. (rigiditate electrica)
• Compensarea electrostatica cu
motoare de AC pe axa de sense.
Elemente de amortizare
Principala forta de amortizare este frecarea vascoasa cu mediul
gazos in care senzorul este incapsulat.
Doua variante: alunecare laterala si cu deplasare axiala.
Coeficientii de frecare vascoasa in cele doua cazuri este:

Cuplajul vascos
Fortele aerodinamice provoaca nu doar amortizare ci si cuplarea
(de data aceasta vascoasa) intre axa de drive si cea de sense.
Protanta hidrodinamica (efectul placii de surf) este modelat de
termeni nediagonali in matricea de amortizare:
Exlicitand pe axa de sense avem:
De data aceasta semnalul util este in aceasi relatia de faza

eroarea de cuplaj si nu mai poate fi extras pe cale
electronica. Apare in semnalul de iesire ca un offset ce poate
fi anulat pentru o temperatura data.
Amortizarea intrinseca
Pentru a scapa de efectele temperaturii putem sa vidam carcasa.
In acest caz principalul mode de amortizare devine frecarea
interna datorata deformarilor elastice, cunoscuta sub numele de
amortizare termoelastica.
Putem obtine in acest fel factori de calitate de ordinul Q=100000
Anizotropia cristalina
Din cauza anizotropiei
cristalului de Si senzorii
trebuiesc realizat simetrici
fata de ambele axe din
plan pentru a compensa.
Calculul exact al
performantelor (Q) este
de obicei imposibil pentru
structuri complicate.
Giroscoape MEMS
– Girolaser-ul
Condensatorul plan-paralel
Formulele sunt valabile doar

pentru condensatorul liniar
(mediu dielectric liniar) !
Condensatorul plan-paralel
Energia si co-energia inmagazinata de un condensator
plan-paralel:
Functie de sarcina
Functie de tensiune
Daca condensatorul este liniar cele doua formule sunt egale.

In caz contrar, unde se duce diferenta de energie ?!!
Ecuatia energiei ne permite sa calculam fortele mecanice.
Lucrul mecanic este variatia energiei si este integrala fortei pe
directia de deplasare.
Motorul electrostatic
Fortele sunt de natura electrostatica (camp conservativ)
deci energia trebuie sa vina de undeva. Revenind la
formule, forta este gradientul energiei, deci, intr-o situatie
in care voltajul este controlat:
Considerand deplasarea pe o singura directie oarecare

notata cu e avem:
Doua cazuri particulare: cu distanta variabila sau cu arie

variabila.
Actuatorul capacitiv cu distanta
(air-gap) variabila
•Forta variaza neliniar cu gap-ul, crescand spre sfarsitul

cursei
•Acest lucru poate genera instabilitati (snap-down)
•Fortele ce pot fi generate sunt mari dar deplasarile sunt
mici.
Actuatorul capacitiv cu gap
variabil
Exista un caz in care variatia
neliniara ajuta: acordul
mecanic al rezonantei se
poate face printr-o tensiune
de DC.
Constanta pseudo-elastica
electrica este:
Variind offsetul de DC putem reduce constanta elastica

globala (kel este negativ!), lucru util pentru acordul mecanic.
Actuatorul capacitiv cu suprafata
variabila
Forta este independenta de

deplasare si de aria fiind,
pentru configuratiile uzuale,
constanta.
Daca avem N “degete” fiecare formeaza doua capacitati (stinga
dreapta) deci:
Actuatorul capacitiv cu suprafata
variabila
• Forta este mai mica decat
in cazul anterior (y este mic,
z este mare)
•Amplitudinile pot fi mari
•Constanta elasica indusa de

fortele electrice este zero
De obiciei tensiunile de excitatie sunt marimi sinusoidale si ne-

am dori ca fortele sa fie tot sinusoidale. Tensiunea intervine in
formula cu V2!
Actuatorul balansat
O metoda simpla, indiferent de tipul actuatorului, este sa folosim o
configuratie push-pull cu doi actuatori comandati in antifaza.
Daca cele doua capacitati sunt egale:

Actuatorul balansat
Daca notam:
• t – grosimea structurii
• L – lungimea unei armaturi
• d – distanta medie intre armaturi
• N – numarul de armaturi
Pentru actuatorul cu suprafata variabila
Pentru actuatorul cu gap variabil

Senzorul capacitiv
Pentru un condensator plan generic avem
Ca si in cazul actuatorilor putem avea senzori cu gap variabil

si senzori cu suprafata variabila
Distanta intre armaturi (d)

este de ordinul μm sau
sau zecimilor de μm.
Senzorul capacitiv
Gap variabil Suprafata variabila
• deplasari mici • deplasari importante

• sensibilitate mare • sensibilitate buna
• neliniar • liniar
Senzorul diferential
Folosit de obicei pentru a liniariza caracteristica senzorilor cu gap
variabil si pentru rejectia perturbatiilor de mod comun.
Extragerea semnalului util
Scheme de masura. Excitatie
separata de ca si cc
Scheme de masura. Excitatie
comuna
Schema de masura
Excitatie
In ambele cazuri tensiunea de excitatie este:
Iar prin derivarea energiei obtinem forta:
Ignorand termenul de cc si cel de frecventa dubla obtinem

Schema de masura
Senzor
Pe partea de senzor tensiunea intre armaturile senzorilor este VDC
iar mentinerea ei constanta in conditiile variatiei capacitatii
genereaza un curent:
Opamp-ul amplifica acest curent cu K (egal cu valoare rezistentei

de reactie)
Punand cap la cap amplificarile si cuplajul mecanic intre excitatie si

iesire functia de transfer este (in cazul excitatiei separate):
Schema de masura
In cazul excitatiei comune forta nu se schimba dar potentialul pe
condensatorul de iesire este:
Iar curentul pe intrarea opamp-ului rezulta
Functia de transfer in acest caz este: ?!!

Influenta cuplajelor parazite
Modelarea capacitatilor
parazitre (prin
substrat) intre intrare
si iesire
Influenta cuplajelor parazite
Cuplajul parazit prin substrat introduce un semnal de aceasi

frecventa cu semnalul util dar in alta relatie de faza fata de
acesta. Acest lucru face posibil recuperarea (partiala) a
semnalului util prin demodulatie sincrona.
Demodularea sincrona
Demodulatia sincrona
DE continuat …
Giroscoape MEMS
– Girolaser-ul
Girolaser
Lumina. (Versiunea unda)
Amplitudinea si lungimea de
unda
Intensitatea luminoasa
Polarizarea
Dependenta de timp si spatiu
Defazajul
Timp si/sau spatiu
Interferometrul Michelson
Diagrama circuitului optic
Interferenta
He-Ne
Interferograma
Interferometrul Sagnac
Componentele vitezei pe laturile
patratului
Determinarea componentei ds
Unde F este aria

LASER-ul
(varianta cu doua oglinzi)
Laserul circular
Rezonatorul sferic cu oglinzi si
echivalentul cu lentile
Caracterizarea unei lentile subtiri
Traseul luminii prin lentila
Rezonatorul cu trei oglinzi
Traseul echivalent
Laser-ul He-Ne
Ciocnirea electon-atom: Excitatia
Constructia laser-ului He-Ne
Distributia vitezelor atomilor de
Ne
Doppler
Boltzmann
Neomogenitatea liniilor spectrale
indusa de distributia vitezei
Unde stationare
Amplificarea (functie de lungimea
de unda)
Rezonatorul liniar si circular
Cele doua moduri sunt alimentate
de distributia vitezei
Selectia modurilor cu un etalon
Modurile rezonatorului sferic
(direct si transvers)
Eliminarea modului transvers
Diametrul fascicolului laser
Drumul optic
Schema girolaser-ului cu trei
oglinzi
Scattering
Fenomenul de blocare (lock-in)
Anti lock-in (vibratii de rotatie)
Formarea franjurilor de
interferenta pe o prisma
Formarea semnalului de iesire
Frecventa de batai
Formarea interferentei
(varianta push-pull)
Semnalul de iesire
(contrast ideal)
Cazul real
Mems vs LRG vs FOG
Gravity Probe B
1/1000000000000 deg/h
Giroscoape MEMS
– Girolaser-ul
Girolaser
Lumina. (Versiunea unda)
Amplitudinea si lungimea de
unda
Intensitatea luminoasa
Polarizarea
Dependenta de timp si spatiu
Defazajul
Timp si/sau spatiu
Interferometrul Michelson
Diagrama circuitului optic
Interferenta
He-Ne
Interferograma
Interferometrul Sagnac
Componentele vitezei pe laturile
patratului
Determinarea componentei ds
Unde F este aria

LASER-ul
(varianta cu doua oglinzi)
Laserul circular
Rezonatorul sferic cu oglinzi si
echivalentul cu lentile
Caracterizarea unei lentile subtiri
Traseul luminii prin lentila
Rezonatorul cu trei oglinzi
Traseul echivalent
Laser-ul He-Ne
Ciocnirea electon-atom: Excitatia
Constructia laser-ului He-Ne
Distributia vitezelor atomilor de
Ne
Doppler
Boltzmann
Neomogenitatea liniilor spectrale
indusa de distributia vitezei
Unde stationare
Amplificarea (functie de lungimea
de unda)
Rezonatorul liniar si circular
Cele doua moduri sunt alimentate
de distributia vitezei
Selectia modurilor cu un etalon
Modurile rezonatorului sferic
(direct si transvers)
Eliminarea modului transvers
Diametrul fascicolului laser
Drumul optic
Schema girolaser-ului cu trei
oglinzi
Scattering
Fenomenul de blocare (lock-in)
Anti lock-in (vibratii de rotatie)
Formarea franjurilor de
interferenta pe o prisma
Formarea semnalului de iesire
Frecventa de batai
Formarea interferentei
(varianta push-pull)
Semnalul de iesire
(contrast ideal)
Cazul real
Mems vs LRG vs FOG
Gravity Probe B
1/1000000000000 deg/h
Giroscoape MEMS
– Girolaser-ul
• Deducerea atitudinii folosind accelerometre si/sau senzori
magnetici
Accelerometrul in bucla deschisa
MEMS
• Pizoresistiv: Marci tensiometrice
• Capacitiv: Armaturi capacitive
• Resonant: Schimbarea frecventei de rezonanta
• Thermal: Transferul termic depinde de distana

Capacitiv
Piezorezistiv
Piezoelectric
Termic
Marimi de interes
• Latimea de banda [Hz]
• Senzitivitatea [pF/g], [mOhm/g], [T/g] …=>[V/g]
• Gama de masura (dinamica) [g]
• Pragul de sensibilitate [mg]

Configuratii mecanice
Raspunsul in frecventa
Parametrii
Latimea de banda: In practica limitata de ½ din
frecventa proprie
Pragul de sensibilitate: Limitat de agitatia
moleculara a amortizorului
Sensibilitatea: Limitata de zgomot
Gama de masura: Limitata de cursa suspensie
elastice
Dimensionare
Constanta elastica:
E=180GPa
Constanta de amortizare:
Elemente constructive: masa
inertiala si arcul
Exemplu (ADXL320)
Traductorul capacitiv
Schema echivalenta
Conditionarea si amplificarea
semnalului
Deducerea Atitudinii
– Girolaser-ul
• Deducerea atitudinii folosind accelerometre si/sau
senzori magnetici
• Data fusion.
Deducerea unghiurilor de atitudine
folosind acceleratii si campul
magnetic terestru
Aceleratia masurata de accelerometre contine trei
componente:
- acceleratia datorata variatie de viteza cinematica
- componenta datorata fortelor centrifige
- componenta datorata gravitatiei
Separam fortele centrifuge (avem vitezele de rotatie!)
magnetic terestru
Ipoteze simplificatoare:
- Aeronava este avion si stim viteza (fie GPS fie
ADC+wind)
Cu acestea putem rescrie ecuatia acceleratiei

magnetic terestru
Avem un sistem supradererminat:
• sase ecuatii – trei de la accelerometre si trei de
la magnetometre
• trei necunoscute – unghiurile de atitudine
Doua metode “clasice” de rezolvare:

• Gauss-Newton (in sensul celor mai mici patrate)
• Master-Slave
Metoda Gauss - Newton
Scriem ecuatia in forma vectoriala:
Unde
acceleratia gravitationala (in body)
campului magnetic (in body)

Metoda Gauss - Newton
Iar
sunt aceasi vectori in nav
Campul magnetic in nav este dat de WMM functie

de pozitie (pe care trebuie sa o stim appr!)
Trecerea de la un sistem de referinta la altul se
face folosind matricea de rotatie (matricea
cosinusurilor directoare).
Vom folosi cuaternioni pentru reprezentarea
interna.
Ecuatia de propagare in
reprezentare cuaternionica
Matricea de rotatie
Folosind cuaternioni matricea de rotatie este:
Pentru sistemul nostru supradeterminat avem:
Trecerea de la yV la yB
Gauss - Newton
Eroarea in sensul celor mai mici patrate este data
de:
(Attn! daca avem informatii despre precizie putem

utiliza o matrice de ponderare!)
Din cauza neglijarii acceleratiei verticale (pe care o

putem introduce de la ADC!) axa verticala este
estimata cu eroare mai mare si putem fie sa
scadem ponderea fie sa eliminam ecuatia.
Master - Slave
In acest caz vom folosi informatia accelerometrica
pentru ruliu si tangaj si cea magnetica pentru
cap.
Din gB avem:
k este nedefinit pentru gBz=0

Master - Slave
Odata calculate unghiurile de rului si tangaj putem
aduce vectorul BB in local orizontal:
Cu
In final putem deduce capul magnetic din:

Data fusion
Avem informatie de atitudine din doua surse:
• Integrata din masuratori giroscopice
• Instantanee din accelerometre si magnetometre
Trebuie sa luam unificam folosind avantajele
fiecarei surse pentru a minimiza erorile
introduse de cealalta.
Doua abordari
– Filtre complementare
– Filtru Kalman
Structura filtrului pentru tangaj si
rului
Filtre complementare
Ideea este da a folosii natura integrala a erorilor
date de giroscoape (filtru trece sus le va
elimina) si natura instantanee a erorilor date de
accelerometre/magnetometre (filtru trece jos).
Pentru ruliu (de exemplu):
Unde parametrii filtrului (K1 si K2) sunt alesi

(semi)empiric!!!
Filtre complementare
Informatia de cap se obtine direct din masuratorile
de la magnetometre:
Metoda este folosita doar pentru implementari

simple data fiind faptul ca proiectarea filtrelor
este suboptimala
Filtre complementare. Algoritmul
Magdwick
• Reprezentarea atitudinii aeronavei. Ecuațiile de
propagare
• Senzori pentru măsurarea vitezelor de rotație
– Giroscopul tehnic. Ecuațiile lui Euler. Ecuațiile lui Euler
– Giroscoape MEMS
– Gir laserul
• Senzori pentru măsurarea accelerațiilor
• Mecanizarea ecuațiilor de propagare
• Filtre complementare. Algoritmul Magdwick
Algoritmul Magdwick
⚫ Algoritmul Magdwick este o implementare

cuaternionica a unui filtru complementar
adaptiv.
⚫ Principalul avantaj îl constitui efortul de calcul
mult mai mic decât in cazul filtrului Kalman.
⚫ Deși mai puțin precis decât acesta algoritmul
are performante bune pentru majoritatea
aplicațiilor implicând UAV-uri sau aeronave de
aviație generala la viteze mici de zbor.
⚫ A fost introdus in 2014.
Sebastian O. H. Madgwick received the Doctorate degree from the Biomechatronics Laboratory, Department of
Mechanical Engineering, University of Bristol, Bristol, U.K., in 2014.,He is currently the Director and a Lead Engineer
with x-io Technologies, Bristol, U.K.,
Cuaternioni – memory refresh
Un cuaternion poate fi reprezentat ca un “vector” de dimensiune
patru.
Dacă este folosit pentru a descrie o rotație atunci cuaternionul

trebuie să aibă norma 1 și reprezentarea sa constă în unghiul
de rotație și versorul axei în jurul căreia se execută rotația.
Cuaternioni - memory refresh
Scalarul μ este unghiul de rotație iar μx, μy, μz sunt
proiecțiile versorului axei de rotație μ pe axele sistemului
de referință.
Putem să înțelegem mai ușor aritmetica cuaternionilor
dacă ne gândim la un cuaternion ca la o generalizare a
numerelor complexe. Ca numere hiper-complexe
cuaternioni se pot scrie ca:
Cuaternioni
Conjugatul unui cuaternion,
notat cu *, poate fi folosit
pentru descrie rotația
inversă.
Conjugatul lui
qAB =[q1+q2+q3+q4]
se definește prin ecuația:
qAB*=qBA=[q1-q2-q3-q4]
Cuaternioni
Produsul cuaternionilor, notat de obicei cu x, poate fi folosit pentru
a compune rotațiile. De exemplu daca avem trei sisteme de
referință A, B si C atunci:
qCA= qCB x qBA
Produsul cuaternionic este necomutativ si se poate calcula din
reprezentarea hipercomplexa sau folosind regula lui Hamilton:
Fie vectorul (in sisteml de coordonate legat de aeronava):
Asociem o reprezentare cuaternionica avand partea reala

zero:
In aceste conditii reprezentarea cuaternionica in sistemul de

navigatie este
Relatia dintre cuaternioni și DCM
unghiurile Euler
Ψ = atan2(2bc – 2ad, 2a2+2b2-1)
Θ =-asin(2bd+2ad)
Φ =atan2(2cd-2ab, 2a2+2d2-1)
Atitudinea calculata din vitezele unghiulare
Giroscoapele sunt montate in axele aeronavei (body) si masoară

trei viteze unghiulare (ωx, ωy, ωz) in jurul axelor sistemului.
Fie sω vectorul viteză unghiulară in body. Definim cuaternionul
asociat adăugând un zero pe prima poziție:
sω=[0 ωx ωy ωz]
Derivata cuaternionului este (vezi cursul 2):
1 𝑏
𝑞ሶ 𝑛𝑏
= 𝑞𝑛 × 𝑠𝜔
2
Desigur cuaternionul ce reprezintă atitudinea, 𝑞𝑛𝑏 (𝑡) poate fi
obținut prin integrare ecuației de mai sus daca măsurăm vitezele
de rotație.
Atitudinea calculata din vitezele unghiulare
Notand cu ∆𝑡 intervalul de eșantionare, folosind indicele est pentru

mărimile estimate și integrând cu Euler avem:
𝑏 (𝑡)
1 𝑏
𝑞ሶ 𝑛,𝜔 = 𝑞𝑛,𝑒𝑠𝑡 (𝑡 − 1) × 𝑠𝜔 (𝑡)
2
𝑏 (𝑡) = 𝑞 𝑏
𝑞𝑛,𝜔 (𝑡 − 1) + ∆𝑡 ∙ 𝑞ሶ 𝑏 (𝑡)
𝑛,𝑒𝑠𝑡 𝑛,𝜔
unde indicele ω denota ca este vorba de estimarea obținută din

vitezele unghiulare.
Expresia (t-1) se refera la valoarea obținută la pasul anterior al
algoritmului (NU cu o secunda înainte…☺)
Atenție! q este un vector de dimensiune 4…
Atitudinea obținută din mărimi vectoriale cu
orientare cunoscută
Direcția gravitației si a câmpului magnetic terestru sunt de obicei
cunoscute în triedrul de navigație. Cu ajutorul lor, în absența
accelerațiilor datorate mișcării, putem calcula o orientare
„instantanee”.
Pentru oricare dintre cei doi vectori, fie 𝑑𝑛 direcția cunoscută
câmpului in triedrul de navigație si 𝑠𝑏 orientarea măsurată în
body. Cuaternionul 𝑞𝑛𝑏 care face trecerea de la n la b rezultă din
rezolvarea unei probleme de minimizare:
𝑏
min
𝑏
𝑓 𝑞𝑛 , 𝑑𝑛 , 𝑠𝑏
𝑞𝑛 ∈𝑅4
𝑓 𝑞𝑛𝑏 , 𝑑𝑛 , 𝑠𝑏 = 𝑞𝑛𝑏 ∗ × 𝑑𝑛 × 𝑞𝑛𝑏 −𝑠𝑏

unde
𝑠𝑏 = 0, 𝑠𝑥 , 𝑠𝑦 , 𝑠𝑧 𝑑𝑛 = 0, 𝑑𝑥 , 𝑑𝑦 , 𝑑𝑧 𝑞𝑛𝑏 = 𝑞1 𝑞2 𝑞3 𝑞4
iar 𝑓 𝑞𝑛𝑏 , 𝑑𝑛 , 𝑠𝑏 este funcția obiectiv (eroarea pătratică)

Algoritmul de tip gradient
Rezolvarea problemei de minimizare se face folosind un algoritm de tip
gradient. Intuitiv algoritmul încearcă „să meargă la vale” din punctul
curent către următoarea iterație calculând direcția pantei maxima a
(hiper) suprafeței și aproximând lungimea necesara a „pasului” din
„înălțime” și „pantă”.
Panta este dată de derivată (în acest caz Jacobian-ul):
𝑏
𝑏 𝑏
∇𝑓 𝑞𝑛,𝑘 , 𝑑𝑛 , 𝑠𝑏
𝑞𝑛,𝑘+1 = 𝑞𝑛,𝑘 −𝜇 𝑏
, 𝑘 = 0, 1, 2 …
∇𝑓 𝑞𝑛,𝑘 , 𝑑𝑛 , 𝑠𝑏
unde
𝑏 𝑏 𝑏
∇𝑓 𝑞𝑛,𝑘 , 𝑑𝑛 , 𝑠𝑏 = 𝐽𝑇 𝑞𝑛,𝑘 , 𝑑𝑛 ∙ 𝑓 𝑞𝑛,𝑘 , 𝑑𝑛 , 𝑠𝑏
cu
𝑏
𝑓 𝑞𝑛,𝑘 , 𝑑𝑛 , 𝑠𝑏 − funcția care trebuie minizată (eroarea)
J - Jacobian-ul sistemului (matricea derivatelor parțiale)
Jacobian-ul sistemului
Avem trei ecuații de eroare și patru necunoscute plus o relație de normare. Matricea
Jacobian-ului are deci dimensiunile 3x4, vectorul de eroare are dimensiune 3 iar
ecuația de gradient este rezolvata la dimensiunea cuaternionului – 4.
1 𝑠𝑥
𝑑𝑥 − 𝑞32 − 𝑞42 + 𝑑𝑦 𝑞1 𝑞4 + 𝑞2 𝑞3 + 𝑑𝑧 𝑞2 𝑞4 + 𝑞1 𝑞3 −
2 2
𝑏 1 𝑠𝑦
𝑓 𝑞𝑛,𝑘 , 𝑑𝑛 , 𝑠𝑏 = 2 𝑑𝑥 𝑞2 𝑞3 − 𝑞1 𝑞4 + 𝑑𝑦 − 𝑞22 − 𝑞42 + 𝑑𝑧 𝑞1 𝑞2 + 𝑞3 𝑞4 −
2 2
1 𝑠𝑧
𝑑𝑥 𝑞1 𝑞3 − 𝑞2 𝑞4 + 𝑑𝑦 𝑞3 𝑞4 − 𝑞1 𝑞2 + 𝑑𝑧 − 𝑞22 − 𝑞32 −
2 2
𝑑𝑦 𝑞4 +𝑑𝑧 𝑞3 𝑑𝑦 𝑞3 +𝑑𝑧 𝑞4 −2𝑑𝑥 𝑞3 +𝑑𝑦 𝑞2 +𝑑𝑧 𝑞1 −2𝑑𝑥 𝑞4 +𝑑𝑦 𝑞1 +𝑑𝑧 𝑞2

𝑏 𝑑𝑥 𝑞2 +𝑑𝑧 𝑞4 𝑑𝑥 𝑞1 −2𝑑𝑦 𝑞4 −𝑑𝑧 𝑞3
J 𝑞𝑛,𝑘 , 𝑑𝑛 = 2 −𝑑𝑥 𝑞4 +𝑑𝑧 𝑞2 𝑑𝑥 𝑞3 −2𝑑𝑦 𝑞2 +𝑑𝑧 𝑞1
𝑑𝑥 𝑞3 −𝑑𝑦 𝑞2 𝑑𝑥 𝑞4 −2𝑑𝑦 𝑞1 −𝑑𝑧 𝑞2 𝑑𝑥 𝑞1 −𝑑𝑦 𝑞4 −2𝑑𝑧 𝑞3 𝑑𝑥 𝑞2 −𝑑𝑦 𝑞3
Câmpul gravitațional
Ecuațiile se simplifică dramatic pentru vectorul accelerație
gravitaționala normat deoarece nu exista decât componenta az
în n (vectorul ab este si el normat):
𝑔𝑛 = 0, 0, 0, 1 𝑎𝑏 = 0, 𝑎𝑥 , 𝑎𝑦 , 𝑎𝑧
𝑎𝑥
𝑞2 𝑞4 + 𝑞1 𝑞3 −
2
𝑎𝑦
𝑏
𝑓 𝑞𝑛,𝑘 , 𝑔𝑛 , 𝑎𝑏 = 2 𝑞1 𝑞2 + 𝑞3 𝑞4 −
2
1 𝑎𝑧
− 𝑞22 − 𝑞32 −
2 2
−𝑞3 𝑞4 −𝑞1 𝑞2
𝑏
J 𝑞𝑛,𝑘 , 𝑔𝑛 = 2 𝑞2 𝑞1 𝑞4 𝑞3
0 −2𝑞2 −2𝑞3 0
Câmpul magnetic
Utilizând direcția Nord magnetic ca referință (triedrul de navigatie este ușor
modificat printr-o rotire cu declinația locului în jurul axei verticale) câmpul
magnetic nu are componente decât în planul vertical - datorită inclinației. (La
latitudinea României aceasta este între 60o si 65o fata de orizontală). Ecuațiile
se simplifică, dar nu ca în cazul anterior:
𝑏𝑛 = 0, 𝑏𝑥 , 0, 𝑏𝑧 𝑚𝑏 = 0, 𝑚𝑥 , 𝑚𝑦 , 𝑚𝑧
1 2 2
𝑚𝑥
𝑏𝑥 − 𝑞3 − 𝑞4 + 𝑏𝑧 𝑞2 𝑞4 + 𝑞1 𝑞3 −
2 2
𝑏
𝑚𝑦
𝑓 𝑞𝑛,𝑘 , 𝑏𝑛 , 𝑚𝑏 = 2 𝑏𝑥 𝑞2 𝑞3 − 𝑞1 𝑞4 + 𝑏𝑧 𝑞1 𝑞2 + 𝑞3 𝑞4 −
2
1 2 2
𝑚𝑧
𝑏𝑥 𝑞1 𝑞3 − 𝑞2 𝑞4 + 𝑏𝑧 − 𝑞2 − 𝑞3 −
2 2
𝑏𝑧 𝑞3 𝑏𝑧 𝑞4 −2𝑏𝑥 𝑞3 +𝑏𝑧 𝑞1 −2𝑏𝑥 𝑞4 +𝑏𝑧 𝑞2

𝑏 𝑏𝑥 𝑞2 +𝑏𝑧 𝑞4 𝑏𝑥 𝑞1 −𝑏𝑧 𝑞3
J 𝑞𝑛,𝑘 , 𝑏𝑛 = 2 −𝑏𝑥 𝑞4 +𝑏𝑧 𝑞2 𝑏𝑥 𝑞3 +𝑏𝑧 𝑞1
𝑏𝑥 𝑞3 𝑏𝑥 𝑞4 −𝑏𝑧 𝑞2 𝑏𝑥 𝑞1 −2𝑏𝑧 𝑞3 𝑏𝑥 𝑞2
Calculul atitudinii folosind g si b
După cum știm de la cursul anterior un singur vector nu permite
determinarea decât a doua unghiuri (rotația in jurul versorului
este nedefinita) dar cu doi vectori necoliniari (g si b) putem
obține o soluție:
𝑓𝑔 𝑔𝑛𝑏 , 𝑎𝑏
𝑓𝑔,𝑏 𝑞𝑛𝑏 , 𝑎𝑏 , 𝑏𝑛 , 𝑚𝑛 =
𝑓𝑏 𝑞𝑛𝑏 , 𝑏𝑛 , 𝑚𝑏
𝐽𝑔𝑇 𝑞𝑛𝑏
𝐽𝑔,𝑏 𝑞𝑛𝑏 , 𝑏𝑛 =
𝐽𝑏𝑇 𝑞𝑛𝑏 , 𝑏𝑛
In mod normal algoritmul de gradient are nevoie de mai mulți pași
(variabili) pentru a ajunge la o soluție dar în cazul acesta vom
face o singura iterație pe pas de timp.
Calculul atitudinii folosind g si b
Algoritmul rezultat pentru estimarea instantanee a cuaternionului devine (in
funcție de ce senzori avem/folosim):
𝑏 𝑏
∇𝑓
𝑞𝑛,∇ 𝑡 = 𝑞𝑛,𝑒𝑠𝑡 𝑡 − 1 − 𝜇𝑡
∇𝑓
𝑏 𝑏
𝐽𝑔𝑇 𝑞𝑛,𝑒𝑠𝑡 𝑡−1 𝑓𝑔 𝑞𝑛,𝑒𝑠𝑡 𝑡 − 1 , 𝑎𝑏 𝑡
∇𝑓 = ൞
𝑇 𝑏 𝑏
𝐽𝑔,𝑏 𝑞𝑛,𝑒𝑠𝑡 𝑡 − 1 , 𝑏𝑛 𝑓𝑔,𝑏 𝑞𝑛,𝑒𝑠𝑡 (𝑡 − 1), 𝑎𝑏 𝑡 , 𝑏𝑛 , 𝑚𝑏 𝑡
O valoare optima pentru 𝜇𝑡 se poate alege ținând seama de dinamica (vitezele

maxime de rotație) aeronavei. Cerința majoră este ca valoarea aleasa să nu
lase algoritmul de gradient „să rămână în urmă” – pasul de calcul trebuie să
fie mai mare decât viteza de rotație, dar nu atât de mare ca procesu iterativ
să suprareglare si să înceapă să oscileze.
𝑏 (𝑡) ∆𝑡,
𝜇𝑡 = 𝛼 𝑞ሶ 𝑛,𝜔 𝛼>1
Filtrul complementar
𝑏
Cele doua atitudini (cea de la giroscoape 𝑞𝑛,𝜔 𝑡 , și cea de la
𝑏
accelerometre/magnetometre 𝑞𝑛,∆ 𝑡 ) sunt sumate ponderat
pentru a obține atitudinea estimata folosind un factor de
ponderare 𝛾𝑡 .
𝑏 𝑏 𝑏
𝑞𝑛,𝑒𝑠𝑡 𝑡 = 𝛾𝑡 𝑞𝑛,∇ + 1 − 𝛾 𝑞𝑛,𝜔 𝑡 0 ≤ 𝛾𝑡 ≤ 1
O valoare optima a ponderii 𝛾𝑡 ar trebui sa asigure ca divergenta
ponderata a giroscoapelor este perfect compensata de
convergenta ponderata a atitudinii instantanee, depinzand de
calitatea giroscoapelor folosite.
Dacă rata de divergenta a giroscoapelor este 𝛽 atunci condiția de
optim este:
𝜇𝑡
1 − 𝛾𝑡 𝛽 = 𝛾𝑡
∆𝑡
𝛽
𝛾𝑡 = 𝜇
𝑡
+𝛽
∆𝑡
Filtrul complementar
Divergența senzorilor giroscopici este de obicei suficient de mică
𝜇𝑡
(iar pasul de timp este și el mic) astfel încat termenul ≫ 𝛽 și
∆𝑡
ponderea va fi mica și se poate fi aproximata cu:
𝛽∆𝑡
𝛾𝑡 =
𝜇𝑡
Daca pasul de timp este mic constanta 𝛼 poate fi foarte mare si
valoarea relativa a lui 𝜇𝑡 va fi de asemenea mare. Ecuația de
propagare se simplifică la:
𝑏
𝛽∆𝑡 ∇𝑓 𝑏 𝑏 (𝑡)∆𝑡
𝑞𝑛,𝑒𝑠𝑡 𝑡 = −𝜇𝑡 + 1 − 0 𝑞𝑛,𝑒𝑠𝑡 𝑡 − 1 + 𝑞ሶ 𝑛,𝜔
𝜇𝑡 ∇𝑓
Simplificarea algoritmului
Ecuatia se poate simplifica mai departe daca introducem o viteză de
rotație echivalenta egala cu convergența atitudinii accelerometrice.
𝑏
∇𝑓
𝑞ሶ 𝑛,𝑒 𝑡 =
∇𝑓
Putem acum sa definim o viteză de rotație compozită (estimata)
corectând divergența giroscoapelor:
𝑏 𝑏 𝑏 𝑡
𝑞ሶ 𝑛,𝑒𝑠𝑡 𝑡 = 𝑞ሶ 𝑛,𝜔 𝑡 − 𝛽𝑞ሶ 𝑛,𝑒
cu care ecuația de propagare devine:
𝑏 𝑏 𝑏
𝑞𝑛,𝑒𝑠𝑡 𝑡 = 𝑞𝑛,𝑒𝑠𝑡 𝑡 − 1 + ∆𝑡 ∙ 𝑞ሶ 𝑛,𝑒𝑠𝑡 𝑡
formal identică cu ecuația de propagare a cuaternionului.
Schema de calcul pentru IMU
Compensarea erorilor datorate perturbațiilor
magnetice
Distorsionarea câmpului magnetic datorita maselor de fier sau altor
câmpuri permanente poate fi compensata atâta timp cât înclinația
câmpului nu este alterata foarte mult.
Din fericire componenta verticala este cea mai mare la latitudini
medii si deci este mai greu perturbabilă.
Compensarea se face știind ca nu ar trebui sa existe by. Putem
„forța” aceasta informație prin renormarea vectorului mb astfel încât
rotit in n sa nu aibă componentă pe y.
unde h este câmpul magnetic distorsionat.

Compensarea erorilor datorate
variației bias-ului giroscoapelor
Fenomenul se numește „bias drift” și se datorează în principal temperaturii și
vibrațiilor. Compensarea sa se face prin identificarea continua a ofsetului.
Acesta apare ca o componentă de continuu in viteza măsurată. Presupunând ca
pe termen lung mișcarea are viteza medie de rotație zero (nu ne învârtim!)
variația bias-ului poate fi eliminată cu o corecție integrală a erorii de orientare
ponderată adecvat.
Operația se face la nivelul vitezelor
de rotație prin aducerea erorii axei de
rotație (un cuaternion) înapoi în
reprezentare ca și viteza de rotație
(un vector) și integrarea sa.
Evident constanta 𝜁 trebuie să fie

dimensionata astfel încât timpul de
răspuns al compensării sa fie
comparabil cu fenomenul de drift.
Schema de calcul AHRS
Acordul filtrului
Filtrul Madgwick este parametrizabil prin doar doua constante (𝛽 și 𝜁). Pentru a
putea înțelege ușor semnificația lor ele sunt definite în funcție caracteristici
ale giroscoapelor.
𝛽 compensează toate erorilor de măsura ale giroscoapelor altele decât bias-ul
(zgomote, erori de aliniere, alising, eșantionare, erori de calibrare,
neortogonalitate etc) care au medie zero. Toate aceste erori pot fi înglobate
intr-un termen 𝜔𝛽 având semnificația unei viteze unghiulare.
𝜁 compensează bias-ului și poate fi definit funcție de viteza de variație a lui..
Influența ratei de eșantionare
Concluzii
- Algoritmul Magdwick necesită un efortul de calcul relativ mic
- Este relativ simplu de acordat daca se cunosc caracteristicile
girometrelor. Acestea trebuie să fie cât mai apropiate.
- Folosește un triedru de navigație modificat (cu x pe Nord-ul
magnetic).
- Tolerează frecvente de eșantionări sub 100Hz dar recomandabil
peste 30Hz.
- Compensează parțial perturbațiile magnetice.
- Nu ia în considerare accelerațiile datorate mișcării. Recomandat
doar pentru aplicații la viteze mici de zbor, neacrobatice.
- Nu ține seama de caracteristicile aeronavei.
- Este sensibil la rotații continue (patern circular).
– Girolaser-ul
• Deducerea atitudinii folosind accelerometre si/sau
senzori magnetici
• Data fusion. Filtrul Kalman .
Ce face filtrul Kalman?
Fie sistemul liniar:

x
(
k+=
1
)F(
k)
x(
k+
)G(
k)
u(
k+
)v(
k)
y
(=
kH
) (
k)
x(
k+
)w(
k)
x(k ) vector de stare (n - dimensional) - necunoscut, il vrem
u (k ) vector de intrare (comanda, m - dimensional) - il stim
y (k ) vector de iesire ( p - dimensional) - il stim oarecum (masuram)
F (k ), G (k ), H (k ) Matrici de dimensiuni potrivite (cunoscute)
v(k ), w(k ) - zgomotul - (medie zero, Gaussian, alb) (avem o idee cat este de mare)
este definit de matricile de covarianta Q(k ) siR(k )
Filtrul Kalman este un algoritm recursiv care ne da

“cea mai buna” estimare a vectorului de stare x.
De ce ne place?
x
(
k+=
1
)F(
k)
x(
k+
)G(
k)
u(
k+
)v(
k)
y
(=
kH
) (
k)
x(
k+
)w(
k)
• Filtreaza zgomotul (imbunatateste calitatea

semnalului de la senzori)
• Avem o estimare de stare (pentru reglaj, eventual)
• Este recursiv (calculeaza estimarea viitoare doar
din masuratorile curente)
Cum functioneaza?
x
(
k+=
1
)F(
k)
x(
k+
)G(
k)
u(
k+
)v(
k)
y
(=
kH
) (
k)
x(
k+
)w(
k)
1. Predictia se bazeaza pe ultima estimare:
xˆ (k + 1 | k ) = F (k ) xˆ (k | k ) + G(k )u (k )
yˆ (k + 1) = H (k ) xˆ (k + 1 | k )
2. Calculam corectia pe baza ultimei estimari si a masuratorilor curente:

x=f(
y(k+1ˆ
),
x(
k +
1|k
))
3. Actualizam predictia: ˆ
x
(k+
1 +
|k1)=ˆ
x
(k+
1|k
)+
x
Cum calculam corectia?
(deocamdata fara zgomot!)
y = Hx
Fiind data predictia xˆ− si iesirea y, gasitil pe x astfel
incat xˆ = xˆ− + x este " cea mai buna" estimare a lui x.
x̂−
•
=x| Hx
=y
x=H HH 
 T
( ) T−
1
O interpretare geometrica
=x| Hx
=y
x̂−
•
x
Un observator de stare…
x(k + 1) = Fx(k ) + Gu (k )
Sistem:
y (k ) = Hx(k )
1. predictie:
ˆ
x
(k+
1 )=
|k ˆ
F
x(
k |k
)+Gu
(
k)
2. calclul corectie:
Observer:

=
xH (
HH
y(
k+−
1
)Hˆ
x
(+
k1( )
)
T
|k
) T−
1
3. Actualizare:
ˆ
x
(k+
1 +
|k1)=ˆ
x
(k+
1|k
)+
x
Estimarea distributiei lui x
Am neglijat zgomotul! Estimarea noastra pentru x nu este exacta!

Putem sa o inbunatatim estimand si zgomotul (Gaussian) cu distributia p(x).
−
1
−
1 2(xˆ
xT−
)P1
(x−
ˆ
x) ( )
)=

p
(x e
2P
unde P=E (
(x− x−
ˆ)(
x )
ˆ)T este matricea de covarianta
x
Cum calculam corectia
(abordare geometrica)
Fiind data predictia xˆ− , covarianta P si iesirea y,
gasitil pe x astfel incat xˆ = xˆ− + x este " cea mai buna"
(adica probabila) estimare a lui x.
=x| Hx
=y
−
1
−
1 2(xˆ
x)
−
T−
P1
(x−
ˆ
x(
)
− )
= e

p
(x
)
2P
Cel mai probabil x este cel care :

x̂−
• 1. satisface xˆ = xˆ− + x
x −1
2. minimizeaz a x P xT
Un now fel de distanta ☺
Sa definim un " nou" produs intern pe R n ca

x1 , x2 = x1T P −1 x2 (inlocuies te definitia " normala" a produsului - x1T x2 )
Putem definii o " noua" norma x = x, x = xT P −1 x

2
xˆ in  care minimizeaza x este proiectia ortogonala a lui xˆ−

pe dreapta , asfel incat x este ortogonal pe .

, x
=0
for
in
T=null
(
H) 


,x
=P
=
x0
iff
x T−
1
column
(
PH) T
Sa calculam in sfarsit corectia
(ultima oara!)
Presupunand ca x este liniar in  = y − Hxˆ−

x=PH
T
K
Conditia y = H ( xˆ− + x)  Hx = y − Hxˆ− = 
Substituin d obtinem :

Hx==HPH
T
K
K=HPH ( )
T −
1

x=PH 
HPH T
( T−
1
)
Inca un observator de stare
(mai bun, speram …)
Putem sa generam un observator de stare mai bun urmand cei trei pasi dar in plus
va trebui sa calculam si matricea de covarianta P.
Vom incepe cu x(k|k) si P(k|k)
Step 1: Predictia
ˆ
x
(k+
1 )=
|k ˆ
F
x(
k |k
)+Gu
(
k)
Ce facem cu P? Din definitie:

P
(
k|
k)=
E(
x(
k−
)ˆ
x(
k|
k −
))(
x
(
k)ˆ
x(
(
k|
k T
)) )
si
+
P
(
k1|
k)=
E(
x
(+
k−
1
)ˆ
x
(
k+
1|
k +
))(
x
(
k1
)−
ˆ
x
(
k+
1(
|
k T
)) )
Tot pasul 1
Ca sa ne facem viata mai usoara, sa “siftam” un pic notatia:
−
P
+
k1=E
(x
k+−−
ˆ
x
1 k+ (
)(
x
1 k+−−T
ˆ
x
1 k+
1) )
(
=
E(
+
Fx
k +
Gu
kv
k−
(
F+
ˆ
x
k )
Gu
)
k(+
Fx
k +
Gu
kv
k−
(
F+
ˆ
x
k )
Gu
)
k )
T
=
E(
(
F(x−ˆ)
x+v)(
(Fx−ˆ)
x
k k+v)) k k k k
T
=
E(
F(
x −
ˆ
x )
(
x −
ˆ
x)F +2
F(
T
x
k kk k−ˆ
T
x )
v+v
v) kk
T T
k kk
=(
FE(
x ˆ)
−
x (
x ˆ))
−
x F +(
Evv
k k k k)
TT T
kk
=FP
k FT
+Q
P
(+
k1 )=
|k FP
(
k |k
)FT
+Q
Pasul 2: Calculul corectiei
De la pasul 1 avem xˆ (k + 1 | k ) si P(k + 1 | k ).

Acum putem sa calculam x :

=
xP
(
k+
1|
k)H+
HP
(
k1|
k)H(
y
(+
k−
1
)ˆ
H
x+
(
k1|
k))( −
T1
)
Vom nota (doar ca sa ne fie mai usor) cu W matricea care:
x = W
Pasul 3: Actualizare
ˆ
x
(k+
1 +
|k1)=ˆ
x
(k+
1|k
)+
W
P
+
k1=(
E
(x
k+−ˆ
x
1 k+ )
)(
x
1 k+−ˆ
x
1 k+
1
T
)
=
E(
x−
( ˆ−
x
+
k 
−
Wx−
)(
+
1k1 ˆ−
x 
W
)) +
k
−
+
1k1
T
(puteti sa ma credeti pe cuvant… ☺)
P
(
k+
1|
k +
1
)=
P(
k+
1|
k)− +
WHP
(k1
|
k)T
H
WT
In cazul in care nu ma credeti…
P
+
k1(
=E
(x
k+
1−ˆ
x
k+
1)(
x
k)
+
1−ˆ
x
k+
1
T
)
=
E(
x−
( +
k
−
ˆ−
x+
1k
W
1x−
)( ˆ−
x+
kW
−
)
+
1k)
1
T
=
E(



x−
( ˆ)
x
+
k
−
−
W
+
1k
)
1(
x−
( x −
ˆ)
+
kW
)
−
+
1k



1
T
=
E
(x
+
k
1(
−−
ˆ
x
+
k)(
1x
+
k−
1
−
ˆ
x
+
k
T
)
1−
2W
(
x
k−
+
1
−
ˆ
x
+
k
T
)
1+
W(
W)
T
 
)
−
=
P
+
k
1+
E−
2
WH
(
x
+
k−
1
−
ˆ
x
+
k
1(
k−
)(
x+
1
−
ˆ
x
+
k
1+
T
)WH
(
x
+
k−
1
−
ˆ
x
+
k
1 −
)(
x
k+
1
−
ˆ
x
+
k
1
T
)
HT
WT
)
− − − TT
=
P+
k1−2
WHP
+
k1+
WHP
+
k1HW
=
P
k−
+
1( )
− −
2P
+
k
1HT−
HP
+
k
1HT −
HP
+
k
1+−−
1T
WHP
kH
+
1WT
=
P−
2
P(
−−
H
+
k
1 )
HP
H+
k(
T−
HP
H
+
1 k
1)
(
T−
1
HP)
−
HT
HP
+
k
−
+T−
1
WHP
+
1 k
1HW− −
+
k +
1 k
1
TT
− − TT − TT
=
P+
k−
12WHP
+
kH
1 +
WWHP
+
kH
1 W
Sa rezumam ce am facut:
x
(k+
1)=Fx
(
k)+Gu
(
k)+v
(k
)
Sistemul:
y
(k
)=Hx
(
k)
1. Predictie ˆ
x
(k+
1 )=
|k ˆ
F
x(
k |k
)+Gu
(
k)
P
(+
k1 )=
|k FP
(
k |k
)FT
+Q
Observer
2. Corectie

x=
W(y
(k+
1−
) ˆ
H
x
(k+
1|k
))
3. Actualizare
ˆ
x
(k+
1 +
|k1)=ˆ
x
(k+
1|k
)+
W
P
(
k+
1|
k +
1
)=
P(
k+
1|
k)− +
WHP
(k1
|
k)T
H
WT
Cum gasim corectia ?
(cazul real, cu zgomot de iesire, chiar ca este
ultima oara ☺)
y = Hx+ w
Din moment ce nu avem un hiperplan la care sa tind

=x| Hx
=y , nu putem rezolva pe cale pur algebrica!
Va trebui rezolvata ca o problema de optimizare.
x̂− Exact ce a facul Kalman!

•
Si asta a optinut (pe cuvantul lui Rudolf!):

x=
PH +
HPH
R (
y−ˆ
H
x
−)T
( T
)−
1
Filtrul Kalman LTI – Linear, Time
Invariant
x
(k+
1)=Fx
(
k)+Gu
(
k)+v
(k
)
Sistemul:
y
(k
)=Hx
(
k)+w
(
k)
ˆ
x
(k+
1 )=
|k ˆ
F
x(
k |k
)+Gu
(
k)
1. Predictia
P
(+
k1 )=
|k FP
(
k |k
)FT
+Q
Filtrul Kalman
S= k+
HP
( 1
|k T
)H+R
2. Corectia

x=
W(y
(k+
1−
) ˆ
H
x
(k+
1|k
))
ˆ
x
(k+
1 +
|k1)=ˆ
x
(k+
1|k
)+
W
3. Actualizare
P
(
k+1+
|k1=
) P
(
k+1
|k
)− T
WSW
Un exemplu
Fie un vehicul care se poate misca pe o singura axa la
care putem controla acceleratia si la care nu face
modificari sau masuratori decat la un interval de timp T.
Ecuatia vitezei (scrisa in timp discret)
Iar pentru pozitia p avem:
Unde marimile notate cu ~ sunt zgomot.

Un exemplu
Vectorul de stare este:
Iar sistemul devine (cu notatiile evidente):

Covariante
Covarianta zgomotului de stare (numit si zgomot
de proces) este:
Covarianta zgomotului de iesire (numit si zgomot

de masura) este:
Ecuatiile filtrului Kalman
Unde am notat cu A, B, C matricile sistemului si cu

P covarianta erorii.
Sunt aceleasi ecuatii ☺!

Zgomotul
Sa presupunem ca masuram pozitia cu GPS-ul cu
o eroare (dispersie) de 10m.
Sa mai presupunem ca aplicam o acceleratie de
1m/s2.
Zgomotul de stare este 0.2m/s2 (damburi,
alunecari etc).
Intervalul de esantionare este T=0.1s.
Sistemul
Cu acestea sistemul devine
Matricile de covarianta
Cum nu masuram decat pozitia, covarianta
zgomotului de iesire devine un scalar si anume
Sz=(10)^2=100 m2
Pozitia depinde de acceleratie prin aT2/2 varianta
pozitie (functie de ea insasi) este (0.005)2*(0.2)2
Asemanator viteza este aT deci varianta vitezei
(functie de ea insasi) este (0.1)2*(0.2)2
In fine varianta pozitie functie de viteza (si invers)
este produsul (0.005*0.2)*(0.1*0.2)
Matricea de covarianta
MATLAB!
– Girolaser-ul
• Deducerea atitudinii folosind accelerometre si/sau senzori
magnetici
• Data fusion. Filtrul Kalman. Filtrul Kalman Extins
Linear Time Invariant
Cursul trecut am plecat de la un sistem linear dar variabil in timp:
x
(
k+=
1
)F(
k)
x(
k+
)G(
k)
u(
k+
)v(
k)
y
(=
kH
) (
k)
x(
k+
)w(
k)
Dar pe drum am pierdut variația in timp si am ajuns la un sistem LTI:
x
(k+
1)=Fx
(
k)+Gu
(
k)+v
(k
)
y
(k
)=Hx
(
k)+w
(
k)
De ce?
Simplificarea a fost necesara din cauza modului in care am considerat zgomotul.

Sa ne aducem aminte: Filtrul Kalman LTI
x
(k+
1)=Fx
(
k)+Gu
(
k)+v
(k
)
Sistemul:
y
(k
)=Hx
(
k)+w
(
k)
ˆ
x
(k+
1 )=
|k ˆ
F
x(
k |k
)+Gu
(
k)
1. Predictia
P
(+
k1 )=
|k FP
(
k |k
)FT
+Q
Filtrul Kalman
S= k+
HP
( 1
|k T
)H+R
2. Corectia

x=
W(y
(k+
1−
) ˆ
H
x
(k+
1|k
))
ˆ
x
(k+
1 +
|k1)=ˆ
x
(k+
1|k
)+
W
3. Actualizare
P
(
k+1+
|k1=
) P
(
k+1
|k
)− T
WSW
Cum folosim filtrul Kalman pentru AHRS?
x
(k+
1)=Fx
(
k)+Gu
(
k)+v
(k
)
y
(k
)=Hx
(
k)+w
(
k)
Trebuie să stabilim:
• Cine sunt stările? => F
• Cine sunt intrările? => G
• Cine sunt ieșirile? => H
• Zgomotul! => Q si R
Ecuația de stare este forma discreta a unui sistem diferențial.
Avem nevoie de ecuații diferențiale ordinare!
Stări, intrări, ieșiri
Stările: sunt ce ne dorim să aflam - atitudinea (în diverse
modalități de reprezentare)
Ieșirile (sau măsurătorile) sunt ce măsuram de la senzori -

accelerații g, camp magnetic m, viteze unghiulare ω).
Intrările ? Nu avem comenzi evidente… doua posibilități:

• Nu avem comenzi … bizar … mai funcționează?
• Folosim o parte din măsurători pe post de comenzi.
Consecințe:
• Ar trebui sa fie măsurători care afectează
derivatele direct => ω
• Ne rămân mai puține măsurători! Doar (a, m).
Candidați: Ecuațiile de propagare
Reprezentare unghiuri Euler Reprezentare DCM
Reprezentare cuaternionica
Avem nevoie de o legătură
liniară între stări și derivate
de genul:
Nu pare plauzibil să găsim…

Kalman: cazul neliniar
Este posibil sa extindem relativ ușor filtrul Kalman pentru
cazul neliniar … daca păstram zgomotul aditiv, adică:
Unde toate mărimile implicate au aceeași semnificație iar

funcțiile f() si h() sunt neliniare dar diferențiabile.
Zgomotul este aditiv si caracterizat ca si până acum de

matricele de covarianța Q si R.
Liniarizarea in jurul stării curente
Propagarea matricei de covarianta
f() și h() nu pot fi aplicate direct ecuației de propagare a covarianței
P si nu putem scrie (ușor) o funcție neliniara. => LINIARIZĂM!
Calculam o matrice de derivate parțiale (Jacobianul). La fiecare pas

de timp, Jacobianul este evaluat cu ce știm - stările prezise curente.
J(f()) poate fi utilizat în ecuațiile filtrului Kalman ca și F.
La fel procedam cu h().
Acest proces liniază în esență funcția neliniară în jurul estimării
curente …
Oare este OK? DA … oarecum (dar demonstrația este lungă …)
Filtrul Kalman Extins - EKF
1. Predicție
2. Actualizare
3. Corecție
Liniarizarea
Cine sunt F si H?
Jacobianul este calculat din mărimile disponibile (estimate).
Dimensiunile matricelor obținute sunt compatibile cu F si H din

cazul liniar dar calculul lor la fiecare pas le transforma in mărimi
dependente de timp (Fk, Hk).
Aceasta metoda se poate aplica și sistemelor LTV!

Dar …
Spre deosebire de cazul LTI, Filtrul Kalman Extins în general nu
este un estimator optim decât in cazuri particulare.
Dacă estimarea inițială a stării este greșită (sau dacă procesul

este modelat incorect), filtrul poate diverge rapid, datorită
liniarizării sale într-un punct fără legătură cu realitatea.
Matricea de covarianță estimată din Jacobian tinde să

subestimeze matricea reala de covarianță și devine
inconsistentă (degenerează) în sens statistic în absența
zgomotului.
În cazul in care senzorii nu au suficient zgomot se injectează

artificial zgomot.
Cum facem?
Să considerăm reprezentarea în unghiuri Euler. Avem:
Funcția neliniara care implementează dinamica sistemului este

Reprezentarea in unghiuri Euler
Funcția neliniara care implementează ieșirile este:
Matricele F si H sunt Jacobianul lui f respectiv h (matricele

derivatelor parțiale in raport cu φ, θ și ψ si au dimensiunile
F=[3x3] si H=[6x3].
*Termenii lipsa sunt coloanele din DCM

Reprezentarea cuaternionică:
Relații de normare
Reprezentarea în unghiuri Euler este desigur cea mai puțin avantajoasă
din cauza singularităților si a efortului de calcul necesar pentru evaluarea
funcțiilor trigonometrice.
Are însă un avantaj: cele trei variabile de stare sunt independente.
In cazul reprezentării cuaternionice avem o relație de normare iar in cazul

DCM avem 6 relații de ortonormare.
Relațiile de normare se introduc ca măsurători artificiale. De ex pentru

cuaternion se adaugă in h() o ecuație de genul:
A cărei valoare „măsurată” este întotdeauna* 1. Dimensiunea ieșirii pentru

reprezentarea cuaternionica va fi 7 iar cea a stării 4.
O alta modalitate este să forțăm normarea in funcția f() dar asta

complica forma Jacobianului …(si nu se poate aplica la DCM)
Reprezentarea DCM
Reprezentarea DCM necesita 3x3=9 variabile de stare

(componentele matricei de rotație).
Pentru ieșiri avem cele șase măsurători reale (trei accelerații si

trei câmp magnetic) la care se adaugă alte șase măsurători
artificiale rezultate din ecuațiile de normare ale coloanelor
(egale cu 1) si de perpendicularitate intre coloane (egale cu 0).
Dimensiunea lui y este deci 12.
Matricele Jacobian asociate au F=[9x9] și H=[12x9]
Metoda alternativa de a introduce relațiile de ortonormare in f()

este total impracticabilă.
Ofsetul senzorilor giroscopici
Principala sursa de erori pentru AHRS este ofsetul senzorilor
giroscopici.
Folosind filtrul Kalman exista o metoda de eliminare a
ofsetului introducând modelul ecuației de măsură in mod
explicit in dinamica sistemului.
Ofsetul senzorilor giroscopici
Q și R
Introducerea de relații suplimentare (fie ca variabile de stare
fie ca variabile de ieșire) creste automat si dimensiunea
matricelor de covarianța a zgomotului de sistem si a celui de
măsura.
La prima vedere covarianța zgomotului pentru o mărime

constanta (1 de ex in cazul ecuației de normare) ar trebui sa
fie zero. Cu toate acestea valoarea corespunzătoare in R
este diferita de zero deoarece covarianța reflecta diferența
intre valoarea estimata si cea măsurata (care este diferita de
zero!).
De altfel 0 pe diagonala lui R poate induce instabilitate
numerica la calculul lui S-1.
Concluzii
Ecuațiile de propagare necesita o forma neliniară de filtru
Kalman. În majoritatea cazurilor aceasta este filtrul EKF sau
UKF.
Pasul de eșantionare trebuie sa fie constant (valoarea numerica

a matricei P depinde de pasul de timp)
Modalitatea de definire a sistemului este foarte flexibilă.
Efortul de calcul este considerabil (mai ales pentru implementări

Euler (din cauza funcțiilor trigonometrice) sau DCM (din cauza
dimensiunilor)).
Nu exista o implementare „standard” ... În funcție de puterea de

calcul disponibilă putem adaugă funcții obiectiv adiționale
(minimizarea efectului ofsetului, a perturbațiilor magnetice, a
acceleraților cinematice, a drift-ul in temperatura, identificarea în
timp real defecțiunilor etc).

Toate3 Cursurile

Încărcat de

Informații document

Titlu original

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

Toate3 Cursurile

Încărcat de

Drepturi de autor:

Formate disponibile

AHRS

- Inerţial (i-frame): originea in centrul Pamantului

- Earth (e-frame): originea in centrul Pamantului si

Ziua siderala – Definita fata de echinoxul vernal (care se deplasează datorita

Viteza de rotaţie a Pamantului fata de un sistem inerţial a fost in 2005:

(7.2921150 ± 0.0000001) ×10−5 rad/s

- de navigaţie (n-frame): este legat de aeronava

- local orizontal (w-frame): wander azimuth

- aeronava (b-frame): body frame, are originea in

Ordinea de compunere contează!

⚫Matricea cosinusurilor directoare: o matrice 3x3 ale

⚫Cuaternioni: o reprezentare a trecerii de la un sistem

Matricea cosinusurilor de rotaţie

O vom nota Cnb (matricea de trecere de la

Elementul de pe rândul i si coloana j reprezintă

Matricea cosinusurilor de rotaţie

Propagarea matricei cosinusurilor directoare in

Unde cu A s-a notat matricea de trecere de la timpul t la t+δt.

Unde cu ψ s-au notat unghiurile de atitudine iar I este matricea

δψ, δθ si δφ sunt variaţiile mici ale unghiurilor cu

notaţii cu care ecuaţia de propagare devine:

Orice transformare de la un triedru la altul poate fi

In jurul lui z cu unghiul ψ

In jurul lui y cu unghiul θ

In jurul lui x cu unghiul φ

Iar transformarea inversa (din triedrul aeronava in cel de

Neglijând elementele mici de ordin superior avem:

Totuşi daca dispunem de o platforma inerţiala mecanica

Ecuaţia poate fi rearanjata ca:

Sau alternativ in format matricial:

Asociem o reprezentare cuaternionica avand

In aceste conditii reprezentarea cuaternionica in

Unde am folosit notatiile deja introduse:

Iar matricea C rezulta:

Daca suprimam zerourile formularea matriceala

Unde am notat pbnb cuaternionul asociat vitezei de

Pentru implementare efectiva scrierea pe

Sistemul de ecuatii de mai sus se poate folosi pentru

Daca unghiul θ nu este 90o (sau aproape 90o)

Pentru θ aproape de +90o

Pentru θ aproape de -90o

Momentul cinetic (momentul cantitatii de

Pentru un corp solid continuu suma se transforma in

Pentru rigidul cu punct fix O in originea sistemului de

J xy =  xy  dm J xz =  xz  dm etc

Daca sistemul de referinta este astfel ales incat Oxyz

Adică variaţia momentului cinetic este datorata momentelor

Daca vectorul K se roteşte cu viteza 

Înlocuind in teorema momentului cinetic obţinem ecuaţiile Euler

Schimbând sistemul de coordonate la triedrul Resal obţinem

J x cos  − 2 J y sin  + J z ( +  sin  )  = M x 3

Ecuatiile Euler sunt scrise in axele proprii

• Triedrul Resal (Ox3y3z3) este caracterizat prin

• Triedrul Ox1y1z1 este triedrul fix

• Avantajul principal este ca cele doua unghiuri se

• Ecuatiile isi pierd insa din generalitate si

Momentul cinetic al giroscopului in axele triedrului

Momentul fortelor exterioare in raport cu axele triedrului Resal

Din teorema momentului scrisa in triedrul Resal obţinem

J x cos  − 2 J y sin  + J z ( +  sin  )  = M x 3

 +  cos  +  sin  ) = M

Daca α si β sunt mici putem aproxima sin-usurile cu

Unde cu A,B si C sau notat marimi de natura momentelor de