A thesis submitted in fulfilment of the requirements for the degree of Doctor of Philosophy in the Research Group Name Department or School Name 11… [602044]

UNIVERSITY NAME
DOCTORAL THESIS
Thesis Title
Author:
John S MITHSupervisor:
Dr. James S MITH
A thesis submitted in fulfilment of the requirements
for the degree of Doctor of Philosophy
in the
Research Group Name
Department or School Name
11 iunie 2015

Declaration of Authorship
I, John S MITH , declare that this thesis titled, ’Thesis Title’ and the work presented in it are my
own. I confirm that:
This work was done wholly or mainly while in candidat: [anonimizat]
Where any part of this thesis has previously been submitted for a degree or any other
qualification at this University or any other institution, this has been clearly stated.
Where I have consulted the published work of others, this is always clearly attributed.
Where I have quoted from the work of others, the source is always given. With the excep-
tion of such quotations, this thesis is entirely my own work.
I have acknowledged all main sources of help.
Where the thesis is based on work done by myself jointly with others, I have made clear
exactly what was done by others and what I have contributed myself.
Signed:
Date:
i

“Thanks to my solid academic training, today I can write hundreds of words on virtually any
topic without possessing a shred of information, which is how I got a good job in journalism."
Dave Barry

UNIVERSITY NAME (IN BLOCK CAPITALS)
Abstract
Faculty Name
Department or School Name
Doctor of Philosophy
Thesis Title
by John S MITH
The Thesis Abstract is written here (and usually kept to just this page). The page is kept centered
vertically so can expand into the blank space above the title too. . .

Acknowledgements
The acknowledgements and the people to thank go here, don’t forget to include your project
advisor. . .
iv

Cuprins
Declaration of Authorship i
Abstract iii
Acknowledgements iv
Contents v
List of Figures vii
List of Tables viii
Abbreviations ix
Physical Constants x
Symbols xi
1 Introducere 1
2 Componente Hardware 2
2.1 Placa de dezvoltare Arduino Uno . . . . . . . . . . . . . . . . . . . . . . . . . 2
2.2 AltIMU-10 v4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.3 Conectarea AltIMU-10 v4 la Arduino Uno . . . . . . . . . . . . . . . . . . . . 5
3 Software 7
3.1 Unity 3D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.1.1 Editorul Unity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.1.2 Crearea unei animat ,ii în Unity3D . . . . . . . . . . . . . . . . . . . . 9
4 Calibrarea senzorilor 10
5 Convent ,iile orient ˘arii 11
5.1 Unghiurile lui Euler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
5.2 Cuaternionii . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
5.2.1 Norma unui cuaternion . . . . . . . . . . . . . . . . . . . . . . . . . . 13
v

Contents vi
5.2.2 Înmult ,irea . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
5.2.3 Vectorul de rotat ,ie . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
5.2.4 Derivata unui cuaternion . . . . . . . . . . . . . . . . . . . . . . . . . 15
5.2.5 Determinarea unghiurilor lui Euler pe baza cuaternionilor . . . . . . . 15
6 Filtrarea datelor 16
6.1 Filtrul Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
6.1.1 Predict ,ia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
6.1.2 M ˘asurarea . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
6.1.3 Asocierea datelor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
6.1.4 Corect ,ie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
6.1.5 Vectorul de stare . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
6.1.6 Predict ,ia modelului s ,i m˘asur˘atorile modelului AHRS . . . . . . . . . 18
6.2 Dezavantajele folosirii filtrului Kalman liniar . . . . . . . . . . . . . . . . . . 19
6.3 Filtrul Kalman extins . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
6.3.1 Vectorul de stare . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
6.3.2 Formularea modelului predict ,iei . . . . . . . . . . . . . . . . . . . . . 20
6.3.3 Formularea modelului m ˘asur˘atorilor . . . . . . . . . . . . . . . . . . . 21
6.3.3.1 Maparea parametrilor accelerometrului . . . . . . . . . . . . 21
6.3.3.2 Maparea parametrilor magnetometrului . . . . . . . . . . . . 22
6.3.4 Matricele de covariant ,˘a . . . . . . . . . . . . . . . . . . . . . . . . . . 23
6.3.5 Ecuat ,iile filtrului Kalman extins . . . . . . . . . . . . . . . . . . . . . 24
A Appendix Title Here 25
Bibliografie 26

List˘a de figuri
2.1 Explicarea pinilor pl ˘acii Arduino Uno . . . . . . . . . . . . . . . . . . . . . . 3
2.2 Altimu-10 v4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.3 Conectarea AltIMU-10 v4 la Arduino Uno . . . . . . . . . . . . . . . . . . . . 5
3.1 Animat ,ie Unity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.2 Editorul Unity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.3 Unelta Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
5.1 Reprezentarea unghiurilor Tait-Bryan. Unghiul corespunde rotat ,iei în jurul
axei Z. Unghiul corespunde rotat ,iei în jurul axei X. Unghiul corespunde
rotat ,iei în jurul axei Y . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
5.2 Orientarea vectoruluiA^ rce se rotes ,te în cu unghiul în sistemul de coordonate
A raportat la sistemul de coordonate B . . . . . . . . . . . . . . . . . . . . . . 13
vii

List˘a de tabele
viii

Abbreviations
LAH L istAbbreviations Here
ix

Physical Constants
Speed of Light c= 2:997 924 58108mss(exact)
x

Symbols
a distance m
P power W (Js1)
! angular frequency rads1
xi

For/Dedicated to/To my. . .
xii

Capitolul 1
Introducere
1

Capitolul 2
Componente Hardware
Componetele dispozitivului hardware sunt compuse dintr-o placa de dezvoltare Arduino Uno, ce
are ca scop citirea parametrilor furnizat ,i de pachetul de senzori AltImu-10 v4 ce cont ,ine senzori
de accelerometru, giroscop s ,i magnetometru.
2.1 Placa de dezvoltare Arduino Uno
Arduino UNO este o platform ˘a de procesare open-source, bazat ˘a pe software s ,i hardware flexibil
s,i simplu de folosit. Const ˘a intr-o platform ˘a de mici dimensiuni (6.8 cm / 5.3 cm – în cea mai des
întalnit ˘a variant ˘a) construit ˘a în jurul unui procesor de semnal s ,i este capabil ˘a de a prelua date din
mediul inconjurator printr-o serie de senzori s ,i de a efectua act ,iuni asupra mediului prin interme-
diul luminilor, motoarelor, servomotoare, s ,i alte tipuri de dispozitive mecanice. Procesorul este
capabil s ˘a ruleze cod scris într-un limbaj de programare care este foarte similar cu limbajul C++.
Specificat ,ii :Microcontroler: ATmega328
Tensiune de lucru:5V
Tensiune de intrare (recomandat): 7-12V
Tensiune de intrare (limita): 6-20V
Pini digitali: 14 (6 PWM output)
Pini analogici: 6
Intensitate de iesire: 40 mA
Intensitate de iesire pe 3.3V: 50 mA
Flash Memory: 32 KB (ATmega328) 0.5 KB pentru bootloader
SRAM: KB (ATmega328)
EEPROM: 1 KB (ATmega328)
Clock Speed: 16 MHz
2

Capitolul 2. Componente Hardware 3
FIGURA 2.1: Explicarea pinilor pl ˘acii Arduino Uno
1-Conector de USB de Tip B(Mam ˘a), pentru alimentarea cu 5V s ,i transmisii de date.
2 – Alimentare plac ˘a Arduino . Se recomand ˘a ca tensiunea de alimentare s ˘a fie între 7V-12V .
Observat ,ii:
* În cazul în care se alimenteaz ˘a Arduino Uno prin conectorul USB din alt ˘a surs ˘a înafar ˘a de
PC(de exemplu un astfel de înc ˘arc˘ator), trebuie m ˘asurat ˘a tensiunea de alimentare deoarece ali-
mentarea de la conectorul USB nu trece prin stabilizatorul de 5V al pl ˘acii!
* În momentul când placa Arduino Uno este conectat ˘a la PC pentru programare sau comunicare
prin intermediul monitoriz ˘arii seriale este recomandat deconectarea aliment ˘arii externe din co-
nectorul nr. 2.
3 – Buton de reset , reseteaz ˘a placa FAR ˘A îns ˘a a se pierde programul scris în aceasta. Progra-
mul reîncepe de la funct ,iavoid setup() .
4 – S ,ir pini digitali , cont ,ine 14 intr ˘ari/ies ,iri din care 6 pot fi folosite ca ies ,iri PWM (3,5,6,9,10,11).
* Pinii 0(rx), 1(tx) sunt folosit ,i pentru a primi (rx) s ,i transmite (tx) date. În cazul în care progra-
mul foloses ,te funct ,ia de comunicare serial ˘a (se poate indentifica în program prin „Serial.begin„)
aceste ies ,iri nu pot fi folosite.
* Pinul 13 are conectat un led (L pe plac ˘a) în serie cu o rezistent ˘a. LED-ul ne indic ˘a starea pinu-
lui: aprins pentru HIGH, s ,i stins pentru LOW. (Semnalele Logice, Primul program pe Arduino
Uno).
* GND este conectat la mas ˘a.
* AREF – Analogic Reference
5 – S ,ir pini alimentare:
* IOREF – Se poate m ˘asura tensiunea la care opereaz ˘a microcontrolerul (3v3 sau 5V); * RESET
– Reseteaz ˘a placa când acesta este conectat la mas ˘a (GND); * 3.3V – Furnizeaz ˘a o tensiune de

Capitolul 2. Componente Hardware 4
3,3V;
* 5V – Furnizeaz ˘a o teniune de 5V;
* GND – Este conectat la mas ˘a
* Vin – Prin intermediul acestui pin se poate alimenta placa Arduino Uno respectându-se tensiu-
nea recomandat ˘a de alimentare (în cazul în care aliment ˘am placa prin acest pin, tensiunea trece
prin stabilizator);
6 – S ,ir pini analogici ,A0. . . A5. Valoarea acestori pini se poate citi cu funct ,iaanalogRead() ,
din citire rezult ˘a numere între 0-1023 pentru 0V – 5V .
7 – Pini ICSP (In-Circuit Serial Programming);
2.2 AltIMU-10 v4
AltIMU-10 v4 este un pachet de senzori inert ,iali(IMU) ce este compus dintr-un giroscop tri-
axial L3GD20H, un accelerometru tri-axial LSM303D, un magnetometru tri-axial s ,i un baro-
metru digital LPS25H.Primii trei parametrii ai acestui pachet de senzori, reprezint ˘a componen-
tele inert ,iale ce pot determina orientarea s ,i pozit ,ia sistemului(AHRS), iar citirea presiuni poate
fi convertit ˘a în altidudine.Printr-un algoritm matematic s ,i un microcontroler sau microprocesor
pe baza m ˘asur˘atorilor f ˘acute de Altimu-10 v4 se poate determina orientarea s ,i altitudinea unui
corp.
FIGURA 2.2: Altimu-10 v4
Specificat ,ii:
* Dimensiuni:25mm x 13mm x 3mm
* Greutate(f ˘ar˘a pini):0.8g
* Tensiunea de operare:2.5V -5.5V
* Intensitate de iesire:6mA

Capitolul 2. Componente Hardware 5
* Formatul ies ,irii(I2C):
-Ies,irea giroscopului:citirea unei axe se face pe 16 bit ,i.
-Ies,irea accelerometrului:citirea unei axe se face pe 16 bit ,i.
-Ies,irea magnetometrului:citirea unei axe se face pe 16 bit ,i.
-Ies,irea barometrului:citirea presiunii se face pe 24 bit ,i.
* Domeniul sensibilitat ˘at,ii:
-Giroscop:245,500=s
-Accelerometru:2,4,6,8g
-Magnetometrul:2,4,8gauss
-Barometrul:260 mbar-1260 mbar
Conectarea :
Pentru folosirea acestui pachet de senzori trebuie folosite cel put ,in patru conexiunilegate la
pinii:VIN ,GND ,SCL s,iSDA .VIN va fi conectat la un pin ce are o tensiune între 2:5
5:5V,GND va fi conectat la 0V, iar piniiSCL s,iSDA fi conectat ,ii la magistrala I2Cce
opereaz ˘a pe nivelul logic al pinului VIN .
2.3 Conectarea AltIMU-10 v4 la Arduino Uno
Din caracteristicile de conexiune ai pachetului de senzori Altimu-10 v4 se observ ˘a c˘a acesta
trebuie conectat la o magistral ˘aI2C.
FIGURA 2.3: Conectarea AltIMU-10 v4 la Arduino Uno
MagistralaI2Ca fost creata in ani ’80 de c ˘atre Philips semiconductors. Ea îs ,i propunea sa fie
o cale usoara de a conecta microcontroller-ul cu alte circuitele integrate.Magistrala I2Cconsta
fizic in doua linii active ( SDA siSCL ) si una de masa( GND ) prin care comunic ˘a dou ˘a sau
mai multe componente dupa niste reguli precise.Liniile active SDA (Serial DAta line) si SCL

Capitolul 2. Componente Hardware 6
(Serial CLock line) sunt bidirectionale, componentele conectate la magistrala pot functiona fie
ca receptor fie ca emitator (la momente de timp distincte).
Controlul magistralei va apartine (la un moment dat) unui singur dispozitiv Master, care va
adresa un singur dispozitiv Slave . Masterul este dispozitivul care va initia transferul (printr-o
conditie de START), va genera impulsurile de ceas pe linia SCL si tot el va incheia transfe-
rul generand conditia de STOP.Viteza de transfer pe magistrala poate fi de 100kbit=sec sau
400kbit=sec , iar adresa dispozitivelor I2Ceste format ˘a de regul ˘a din 7 bit ,i.

Capitolul 3
Software
3.1 Unity 3D
La începutul anilor 2000, trei tineri programatori, f ˘ar˘a mult ,i bani, au început s ˘a codeze s ,i s˘a
implementeze ce avea s ˘a devin ˘a una din cele mai populare platforme software din industria
jocurilor.Aces ,ti trei programatori sunt David Helgson,Joachim Ante s ,i Nicholas Francis, iar
proiectul lor a fost inspirat din platforma Apple Final Cut Pro.Final Cut Pro oferea realizatorilor
amatori de filme instrumente profesionale la un pret ,redus, pe aceleas ,i coordonate s-a bazat s ,i
Unity, viz ˘and dezvoltatorii amatori de jocuri video.
O versiune primitiv ˘a de Unity a fost lansat ˘a în 2005, versiune ce era compatibil ˘a s,i pe sistemul
Windows, nu doar sistemul de operare Mac pentru care s-a dezvoltat init ,ial.Din 2008 bazându-
se pe un real succes, platforma a devenit mult mai complex ˘a, iar vânz ˘arile softului le-a permis
s˘a angajeze s ,i o duzin ˘a de programatori.
O alt ˘a lovitur ˘a dat ˘a de în 2009 a fost folosirea platformei Unity3D de c ˘atre Cartoon Network
pentru crearea FusionFall, un MMORPG(Massively Multiplayer Online Role Playing Game)
pentru copii ce a fost jucat de 8 milioane de persoane. Electronic Arts a folosit în Unity3D în
2009 pentru crearea Tiger Woods PGA Tour Online, chiar Microsoft s ,i Ubisoft au devenit client ,i
pentru Unity.
Ast˘azi Unity are peste 300 de angajat ,i în toat ˘a lumea s ,i dezvolt ˘a software pentru iOS, Android,
Windows, Mac, Linux, Web browsers, PS3, Xbox 360. Unity pl ˘anuies ,te suport s ,i pentru Sony’s
PlayStation Vita, oferind în acest moment suport pentru Windwos Phone s ,i BlackBerry. Peste
1.8 milioane de programatori folosesc Unity, plug-in-ul pentru browser a fost insatlat de peste
200 de milioane de ori. Dead Trigger s ,i Dead Trigger 2, unele din cele mai complexe grafici
pentru jocuri au fost dezvoltate pe baza a Unity3D.
Chiar dac ˘a mari nume din industria graficii folosesc Unity, micii dezvoltatori sunt principala
7

Capitolul 3. Software 8
mândrie ai dezvoltatorilor acestei platforme, fapt ce se observ ˘a din deviza spus ˘a de David Helg-
son, CEO s ,i co-fondatorul Unity: ”Dorint ,a noastr˘ a este s˘ a oferim aceleas ,i instrumente s ,i micilor
developeri precum granzilor”
FIGURA 3.1: Animat ,ie Unity
3.1.1 Editorul Unity
Editorul Unity are împ ˘art,it˘a fereastra principal ˘a mai multe sect ,iuni vizibile urm ˘atoarea imagine,
ce descrie sect ,iunile importante:
FIGURA 3.2: Editorul Unity

Capitolul 3. Software 9
Editorul Unity ofer ˘a un mediu drag and drop .Pentru dezvoltarea unui joc nu e nevoie obligato-
riu de scrierea uni cod într-un limbaj de programare, dar majoritatea proiectelor necesit ˘a deprin-
deri de programare.Unity ofer ˘a o diversitate de limbaje de codare precum C# , JavaScript, sau
Boo, pentru dezvoltatorii ce folosesc sintaxa Python.Mediul de dezvoltare este rulat în Mono,
o versiune gratis de .NET Framework.Platforma Unity în sine este scris ˘a în C++,pentru c ˘a, din
spusele lui Helgson "Animat ,ia trebuie s ˘a ruleze rapid precum un program C++, dar controlul
animat ,iei trebui facut printr-un limbaj accesibil, oferit de platforma .NET"
3.1.2 Crearea unei animat ,ii în Unity3D
Un joc în Unity este împ ˘art,it în mai multe obiecte ale jocului, obiecte ce sunt principalele entit ˘at,i
ale animat ,iei s ,i au propriet ˘at,i speciale, acesta putând fi un personaj, mediu înconjur ˘ator sau un
efect special.Astfel anumite obiecte dintr-un joc sunt foarte diferite,fiind necesar ˘a o repartizare
a acestora în containere ce înmagazineaz ˘a obiecte de acelas ,i tip.
Folosirea obiectelor
Un obiect dintr-un joc are automat o pozit ,ie, rotat ,ie s,i scalare, componente definite de unelta
Transform , ce determin ˘a locat ,ia obiectului în spat ,iul real(3D).
Prin unelta Rigidbody , îi putem ad ˘auga obiectului creat anumite caracteristici ce influent ,eaz˘a
comportaea inert ,ial˘a din lumea real ˘a, parametrii precum masa sau gravitat ,ia, aces ,tia influnt ,ând
comportarea animat ,iei spre o comportare realist ˘a
FIGURA 3.3: Uneltele Transform s,iRigidbody

Capitolul 4
Calibrarea senzorilor
10

Capitolul 5
Convent ,iile orient ˘arii
Exist ˘a multe metode ce descriu orientarea unui obiect în raport cu un sistem de referint ,˘a. În acest
proiect sunt folosite metoda unghiurilor lui Euler s ,i cuaternionii relativi la orizontal ˘a s,i la câmpul
magnetic.O alt ˘a metod ˘a folosit ˘a des este determinarea direct ,iei prin matricea cosinusului(DCM),
dar aceast ˘a metod ˘a este inferioar ˘a metodei cuaternionilor.
5.1 Unghiurile lui Euler
Reprezentarea sub forma unghiurilor lui Euler foloses ,te rotat ,ia pe cele trei axe ale reprezent ˘arii
3D. Rotat ,iile realizate în funct ,ie de cele trei axe se poate organiza în funct ,ie de reprezenta-
rea Tait-Bryan a unghiurilor sau în funct ,ie de reprezentarea clasic ˘a a unghiurilor Euler.În acest
proiect e folosit ˘a reprezentarea în funct ,ie de Z-X’-Z” a convent ,ei Tait-Bryan, în care se re-
prezint ˘a rotat ,ia în sensul acelor de ceasornic funct ,ie de axa Z(yaw), rotat ,ia în funct ,ie de axa
Y(pitch),rotat ,ia în fuct ,ie de axa X(roll).
Aceast ˘a metod ˘a de reprezentare a orient ˘arii are o serie de dezavantaje. Cel mai important dez-
avantaj este reprezentat de blocarea gimbal.(Sistemul gimbal este reprezentat de trei inele pivo-
tante ce exemplific ˘a mis ,c˘arile de rotat ,ie ale unui corp în sistemul 3D). Principiul bloc ˘arii gimbal
este reprezentat de cazul în care dou ˘a inele axiale se plaseaz ˘a pe acelas ,i plan, moment în care
sistemul 3D se transforma într-un model 2D.În exemplul din imaginea …. dac ˘a avionul reali-
zeaz˘a o miscare de rotat ,ie cu 0 radiani fat ,˘a de axa Z, 0.5 radiani în raport cu axa Y s ,i 0 radiani
fat,˘a de axa Z. Axa Z s ,i axa X se afla pe planul xy, astfel avionul nu se mai poate rotii pe planul
yz.
11

Capitolul 5. Convent ,iile orient˘ arii 12
FIGURA 5.1: Reprezentarea
unghiurilor Tait-Bryan. Un-
ghiul corespunde rotat ,iei
în jurul axei Z. Unghiul 
corespunde rotat ,iei în jurul
axei X. Unghiul cores-
punde rotat ,iei în jurul axei Y
5.2 Cuaternionii
Un cuaternion reprezint ˘a o extensie a numerelor complexe fiind o ereprezentre patru-dimensional ˘a
a unui num ˘ar complex folosit pentru coordona o reprezentare tri-dimensional ˘a sau pentru a re-
prezenta orientarea unui corp în spat ,iu.Rotat ,ia unui corp cu unghiul Theta în jurul axei Ar din
sistemul de coordonate A, dar aceast ˘a rotat ,ie poate avea o reprezentare în raport cu un alt sistem
de coordonate B, ce este relativ la sistemul A.
A
B^ q=h
q0q1q2q3i
=h
cos(
2)rxsin(
2)rysin(
2)rzsin(
2)i
(5.1)
Astfel în figura se poate observa c ˘a vectorii ^xA,^yA,^zAs,i vectorii ^xB,^yB,^zBreprezint ˘a cele trei
axe de coordonate ale sistemelor A respectiv B. În ecuat ,ia 1.1 cuaternionul ^qce descrie orienta-
rea corpului,unde rx,ry,rzdefinesc componentele vectoruluiA^ rin sistemul de coordonate A.

Capitolul 5. Convent ,iile orient˘ arii 13
FIGURA 5.2: Orientarea vectoruluiA^ rce se rotes ,te în cu unghiul în sistemul de coordonate
A raportat la sistemul de coordonate B
În momentul în care avem o unitate a magnitudinii, cuaternionii pot fi folosit ,i pentru a reprezenta
rotat ,ia.Pentru reprezentarea rotat ,iilor se folosesc cuaternionii de rotat ,ie, iar pentru reprezentarea
orient ˘arii relative la un sistem de referint ,a se folosesc cuaternionii de pozit ,ie.Spre deosebire de
metoda unghiurilor lui Euler, metoda cuaternionilor poate m ˘asura rotat ,iile în sistemul 3D în
fiecare orice situat ,ie, evitând blocarea gimbal. Aceast ˘a caracteristic ˘a a cuaternionilor duce la o
reprezentare matematic ˘a a orint ˘arii mult mai robust ˘a s,i fiabil ˘a.
5.2.1 Norma unui cuaternion
Norma unui cuaternion este definit ˘a ca :
jqj=q
q2
0+q2
1+q2
2+q2
3 (5.2)
Norma cuaternionului este folosit ˘a la normalizarea expresiei acestuia dupa fiecare iterat ,ie. Ex-
presia dupa cere se realizeaz ˘a acest lucru este:
q=q
jqj(5.3)

Capitolul 5. Convent ,iile orient˘ arii 14
5.2.2 Înmult ,irea
Înmult ,irea quaternionilor pNqeste definita ca o combinat ,ie secvent ,ial˘a a doua rotat ,ii. Înmult ,irea
cuaternionilor este o operat ,ie necomutativ ˘a definit ˘a ca:
p
q=Q(p)q=Q(q)p (5.4)
unde matricea cuaternionilor este definit ˘a astfel:
Q(p) =2
666664q0q1q2q3
q1q0q3q2
q2q3q0q1
q3q2q1q03
777775(5.5)
iar conjugata acesteia este:
Q(p) =2
666664q0q1q2q3
q1q0q3q2
q2q3q0q1
q3q2q1q03
777775(5.6)
5.2.3 Vectorul de rotat ,ie
Un vector x poate fi rotit fat ,˘a de sistemul de referint ,˘a, proces ce e determinat de cuaternionul q
ce produce expresia vectorului x0folosind expresia:
"
0
x0#
=q
"
0
x#

q(5.7)
Aceste dou ˘a înmult ,iri de cuaterninoni epoate fi combinat ˘a sub forma unei matrice R(q)unde
x0=R(q)x (5.8)
R=2
664q2
0+q2
1q2
2q2
32(q1q2q0q3) 2(q1q3q0q2)
2(q1q2q0q3)q2
0q2
1+q2
2q2
32(q2q3q0q1)
2(q1q2q0q3) 2(q1q3q0q2)q2
0q2
1q2
2+q2
33
775(5.9)

Capitolul 5. Convent ,iile orient˘ arii 15
5.2.4 Derivata unui cuaternion
Derivata unui cuaternion poate fi f ˘acut˘a cu ajutorul a dou ˘a ecuat ,ii, prima ecuat ,ie ia în considerare
viteza unghiular ˘a a vectorului de referint ,a în raport cu sistemul de referint ,˘a fixat, iar a doua
ecuat ,ie analizeaz ˘a viteza unghiular ˘a fat ,˘a de sistemul de referint ,a al corpului.În acest proiect s-a
folosit a doua ecuat ,ie:
_ qw(q;w) =1
22
666664q0q1q2q3
q1q0q3q2
q2q3q0q1
q3q2q1q03
777775
2
6666640
wx
wy
wz3
777775(5.10)
5.2.5 Determinarea unghiurilor lui Euler pe baza cuaternionilor
Pe baza expresiei orint ˘arii pe baza cuaternionilor se poate determina expresia unghiurilor lui
Euler în forma Tait-Bryan YPR(yaw=rotat ,ia pe axa z, pitch=rotat ,ia pe axa y, roll=rotat ,ia pe axa
x):
yaw=atan2(2
(q0q3+q1q2);12
(q2
2+q2
3)) (5.11)
pitch =atan2(2
(q0q1+q2q3);12
(q2
1+q2
2)) (5.12)
roll=asin(2
(q0q2+q3q1)) (5.13)
Cu ajutorul acestor ecuat ,ii se poate determina s ,i folosii o expresie a unghiurilor lui Euler nea-
fectat ˘a de efectul bloc ˘arii gimbal, generând performant ,e mult mai bune în aplicat ,iile folosite.

Capitolul 6
Filtrarea datelor
6.1 Filtrul Kalman
Filtrul Kalman e un filtru linear s ,i recursiv ce e folosit pentru urm ˘arirea unui proces cu funct ,ie
de probabilitate Gaussian ˘a .
6.1.1 Predict ,ia
Predict ,ia infer ˘a starea din cadrul i, din informat ,iile din cadrul anterior i1¸ si din modelul
dinamic. Modelul dinamic liniar este aplicat estim ˘arii anterioare, ¸ si se obt ,ine valoarea prezis ˘a
pentru vectorul de stare curent. Adit ,ional, valori ale unor m ˘arimi cunoscute pot forma un vector
de intrare ui care poate contribui la predict ,ie.
Pe lâng ˘a modelul dinamic, exprimat de transformarea liniar ˘aFi, ¸ si de modelul intr ˘arii pe care îl
exprim ˘am ca transformarea liniar ˘aBi, exist ˘a o incertitudine wi. Aceast ˘a incertitudine (zgomot)
exprim ˘a devierea unui sistem real fat ,˘a de modelul dinamic ¸ si de intrare, care nu pot t ,ine cont de
orice evolut ,ie. Acest zgomot are media zero, deci nu va influent ,a predict ,ia, care este exprimat ˘a
de urm ˘atoarea ecuat ,ie:
Xi=FiXi1+Biui (6.1)
Pentru a obt ,ine matricea de covariant ,˘a pentru predict ,ie, vom aplica transform ˘arile modelului
dinamic matricei de covariant ,˘a a st ˘arii anterioare, modelul de transformare a intr ˘arii pe matricea
de covariant ,˘a a intr ˘arii, si vom ad ˘auga matricea de covariant ,˘a a incertitudinii. Not ˘am cu Ti
matricea de covariant ,˘a a intr ˘arii, si cu Qi matricea de covariant ,˘a a incertitudinii tranzit ,iei. Atunci
16

Capitolul 6. Filtrarea datelor 17
covariant ,a predict ,iei este:
Pi=FiPi1XiFT
i+BiTiBT
i+Qi (6.2)
6.1.2 M ˘asurarea
Proces extern filtrului Kalman, mãsurãtoarea are ca rezultat unul sau mai mult ,i vectori de mãsurã
Yk
ifiecare cu o matrice de covariant ,ãRk
icare codificã eroarea de mãsurare estimatã (imprecizia
senzorului). Relat ,ia dintre vectorul de stare si vectorul de mãsurã este modelul de mãsurare,
iar dacã acesta este o transformare liniarã aceasta este descrisã de matricea Hi. Folosind acest
model, obt ,inem predict ,ia mãsurãtorii, Yi.
Yi=HiXi (6.3)
Matricea de covariant ,˘a pe care o vom asocia lui Yiva fi notat ˘aSi.
Si=HiPiHT
i+Ri (6.4)
MatriceaSinu este matricea de covariant ,ã a mãsurãtorii prezise, ci matricea de covariant ,ã a
diferent ,ei dintre mãsurãtoarea prezisã si o posibilã mãsurãtoare realã (matricea de covariant ,ã a
inovarii , sau arezidualului ) – adicã exact matricea necesarã pentru a definii o zonã de cãutare
în jurul mãsurãtorii prezise Yi.
6.1.3 Asocierea datelor
Filtrul Kalman este foarte vulnerabil la asocierea datelor (asocierea mãsurãtorilor) [Bar- Sha-
lom1988], din cauza naturii unimodale a funct ,iei de probabilitate în fiecare din fazele de lucru
ale filtrului. Acest lucru înseamnã cã vectorul de stare, odatã deplasat spre un indiciu fals, va
deveni criteriul de select ,ie pentru mãsurãtorile viitoare, asta însemnând mai multe date gres ,ite
incluse în estimare, pânã la devierea totalã de la t ,intã.
Singura m ˘asur˘a obiectiv ˘a a utilit ˘at,ii m˘asur˘atoriiYieste verosimilitatea ei, dându-se starea unui
obiect urm ˘arit. Aceast ˘a verosimilitate este dat ˘a de funct ,ia de probabilitate Gaussian ˘a, centrat ˘a
în predict ,ia m˘asur˘atorii Yi, si având matricea de covariant ,˘aSi.
p(Yk
i) =1p
(2)njSije(Yk
iYi)TS1
i(Yk
iYi)
2(6.5)

Capitolul 6. Filtrarea datelor 18
6.1.4 Corect ,ie
În acest moment toate datele necesare sunt disponibile, si se va calcula noul vector de stare Xi
si matricea sa de covariant ,˘aPi.
Prima dat ˘a se calculeaz ˘a matricea de amplificare Kalman, Ki:
Ki=PiHT
i(HiPiHT
i+Ri)1(6.6)
Ecuat ,ia este echivalent ˘a cu
Ki=PiHT
iS1
i (6.7)
Pasul final este calcularea matricei de covariant ,ãPi. Majoritatea documentat ,iilor disponibile
dau ecuat ,ia urm ˘atoare pentru calculul Pi.
Pi= (IKiHI)Pi (6.8)
6.1.5 Vectorul de stare
Acest filtru este folosit pentru estimarea rotat ,iei în jurul axei Y(pitch )s,i a rotat ,iei în jurul axei
X(roll),luând în considerare s ,i erorile de m ˘asurare ale giroscopului pe aceste axe, wxbs,iwyb:
x=2
666664pitch
roll
wxb
wyb3
777775
i(6.9)
6.1.6 Predict ,ia modelului s ,i m˘asur ˘atorile modelului AHRS
Expresiile modelului predict ,ies,imodelului m˘ asur˘ atorilor pot fi exprimate prin ecuat ,iile matri-
ceale:
2
666664pitch
roll
wxb
wyb3
777775
i=2
6666641 0dt 0
0 1 0dt
0 0 1 0
0 0 0 13
777775
i
2
666664pitch
roll
wxb
wyb3
777775
i1+2
666664dt0 0 0
0dt0 0
0 0 0 0
0 0 0 03
777775
i
2
666664wx
wy
0
03
777775
i(6.10)

Capitolul 6. Filtrarea datelor 19
6.2 Dezavantajele folosirii filtrului Kalman liniar
Implementarea unui filtru Kalman linear are o serie de dezavantaje. Primul dezavantaj se
întâlnes ,te în momentul rotirii unei axe iar concomitent alt ˘a ax˘a este deviat ˘a de la orizontal ˘a,
unghiul trigonometric estimat de accelerometru s ,i corespondenta vitez ˘a unghiular ˘a m˘asurat ˘a de
giroscop nu vor mai fi în acelas ,i plan.Acest aspect nu reprezint ˘a un defect în momentul în care
se opereaz ˘a foarte aproape de orizontal ˘a, dar în momentul în care se opereaz ˘a o deciat ,ie mare
fat,˘a de orizontal ˘a rezult ˘a o surs ˘a important ˘a de erori.
Pentru a rezolva aceste dezavantaje se foloses ,te implementarea cuaternionilor pe baza compor-
t˘arii modelului AHRS.Acest lucru impune folosirea predict ,iei neliniarit ˘at,i modelului m ˘asurat.
Acest lucru poate fi f ˘acut prin implementarea unui filtru Kalman extins.
6.3 Filtrul Kalman extins
Filtrul Kalman extins este versiunea neliniar ˘a a filtrului Kalman regulat.Aceast ˘a variant ˘a de
filtru Kalman foloses ,te matricea Jacobian pentru predict ,ie, iar funct ,iile m ˘asur˘atorilor pentru a
liniariza estimarea st ˘arii curente s ,i a covariant ,ei.Aceast ˘a implentare permite unui nou set de
predict ,i s,i de m ˘asur˘atori, pentru a fi folosite în cadrul estim ˘arii.
6.3.1 Vectorul de stare
Vectorul de stare reprezint ˘a reprezint ˘a variabilele ce se doresc a fi estimate recursiv cu ajutorul
filtrului.În acest proiect vectorul de stare e compus dintr-un cuaternion ce determi ˘a orintarea s ,i
erorile vitezelor unghiulare m ˘asurate de giroscop.
x=h
q0q1q2q3q3wxbwybwzbiT
(6.11)
La momentul init ,ial vectorul de stare este compus din pozit ,ia int ,ial˘aq=[ 1 0 0 0] s ,i din eroarea
viteze unghiulare care este necunoscut ˘a init ,ial.

Capitolul 6. Filtrarea datelor 20
x=h
1 0 0 0 0 0 0 0iT
(6.12)
6.3.2 Formularea modelului predict ,iei
În aceast ˘a aplicat ,ie filtrul Kalman extins foloses ,te un model al predict ,ie ce seste reprezentat
printr-o funct ,ie ce descrie urm ˘atoarea orientare estimat ˘a pe baza precedentei estim ˘ari s ,i a vec-
torului vitezei unghiulare m ˘asurate de giroscop.
qk=qk1+dt_qk (6.13)
unde
_ qw(q;w) =1
22
666664q1q2q3
q0q3q2
q3q0q1
q2q1q03
777775
2
664wxwxb
wywyb
wzwzb3
775(6.14)
Astfel urm ˘atoarea predict ,ie poate fi definit ˘a, pornind de la expresia predict ,iei filtrului Kalman
liniar xi=Fixi1+Biui, printr-o funct ,ief(xi1;ui)ce va ar ˘ata astfel:
xi=f(xi1;ui) =2
66666666666664q0+dt
2
(q1(wxwxb)q2(wywyb)q3(wzwzb))
q1+dt
2
(q0(wxwxb) +q3(wywyb)q2(wzwzb))
q2+dt
2
(q3(wxwxb) +q0(wywyb) +q1(wzwzb))
q3+dt
2
(q2(wxwxb)q1(wywyb) +q0(wzwzb))
wxb
wyb
wzb3
77777777777775(6.15)
Expresia Jacobianului poate fi calculat ˘a prin derivarea funct ,ieifîn funct ,ie de pozit ,ie, rezultând
matriceaF

Capitolul 6. Filtrarea datelor 21
@f
@x=F=2
666666666666641dt
2(wxwxb)dt
2(wywyb)dt
2(wzwzb)dt
2q1dt
2q2dt
2q3
dt
2(wxwxb) 1 dt
2(wzwzb)dt
2(wywyb)dt
2q0dt
2q3dt
2q2
dt
2(wywyb)dt
2(wzwzb) 1 dt
2(wxwxb)dt
2q3dt
2q0dt
2q1
dt
2(wzwzb)dt
2(wywyb)dt
2(wxwxb 1dt
2q2dt
2q1dt
2q0
0 0 0 0 1 0 0
0 0 0 0 0 1 0
0 0 0 0 0 0 13
77777777777775
(6.16)
6.3.3 Formularea modelului m ˘asur ˘atorilor
Modelul m ˘asur˘atorilor definit în cadrul filtrului Kalman extins ca yi=Hixi, putând fi exprimat
ca o funct ,ie de orientarea estimat ˘a, astfelh(xk).Aceast ˘a funct ,ie poate fi folosit ˘a pentru a mapa
starea estimat ˘a pe vectorul m ˘asur˘atorilorz, practic se realizeaz ˘a o comparare a predict ,ie cu
m˘asur˘atorile reale.În acest caz m ˘asur˘atorile realizate ce implic ˘a parametrii accelerometrului s ,i
ai magnetometrului ce vor fi folosit ,i pentru predict ,ia curent ˘a.Vectorul m ˘asur˘atorilor poate fi
definit astfel:
z=h
axayazmxmymziT
(6.17)
6.3.3.1 Maparea parametrilor accelerometrului
Vectorul gravitat ,iei din sistemul fixat este rotit în cadrul sistemului de coordonate al corpu-
lui, fapt reprezentat prin cuaternionul q,mapat in cadrul vectorului accelerometrului. Aceast ˘a
aplicat ,ie t,ine cont doar de direct ,ia vectorului gravitat ,iei s,i nu de cea a câmpului magnetic, vec-
torul este normalizat înnainte de operarea asupra acestuia.
2
664ax
ay
az3
775= (h(xi)) =R(q) !g=R(q)
2
6640
0
13
775=2
6642(q1q3q0q2)
2(q2q3q0q1)
q2
0+q2
1+q2
2q2
33
775(6.18)

Capitolul 6. Filtrarea datelor 22
6.3.3.2 Maparea parametrilor magnetometrului
Maparea componentelor câmpului magnetic se face similar cu maparea accelerat ,iei, rotat ,ile
vectorului magnetometrului fiind reprezentate de cuaternionul determinat.
2
664mx
my
mz3
775= (h(xi)) =R( !q)
2
664bx
by
bz3
775=2
664bx(q2
0+q2
1q2
2q2
3) + 2by(q1q2+q0q3) +bz(q1q3q0q2)
2bx(q1q2+q0q3) +by(q2
0q2
1+q2
2q2
3) +bz(q2q3q0q1)
2bx(q1q3+q0q2) +by(q2q3q0q1) +bz(q2
0q2
1q2
2+q2
3)3
775
(6.19)
Folosirea acestui model de citire a valorilor magnetometrului influent ,eaz˘a estim ˘arile expresilor
rotat ,iilor în jurul axelor x(roll)s,iy(pitch )determinând erori ale orient ˘arii.De asemenea este
nevoie de specificarea direct ,iei câmpului magnetic al p ˘amântului, care variaz ˘a considerabil în
jurul p ˘amântului.Acast ˘a problem ˘a a fost rezolvat ˘a folosind solut ,ia prezentat ˘a de S.Madgwick ,
unde direct ,ia câmpului magnetic este calculat ˘a pe baza aceias ,i înclin ˘ari asupra câmpului m ˘asu-
rat.
Acest procedeu este facut prin rotirea vectorului câmpului magnetic m ˘asurat al sistemului de
coordonate al corpului în raport cu sisteul de coordonate fix, asfel rezultând expresia m0
2
664m0
x
m0
y
m0
z3
775=R( !q)
2
664mx
my
mz3
775=2
664mx(q2
0+q2
1q2
2q2
3) + 2my(q1q2+q0q3) +mz(q1q3q0q2)
2mx(q1q2+q0q3) +my(q2
0q2
1+q2
2q2
3) +mz(q2q3q0q1)
2mx(q1q3+q0q2) +my(q2q3q0q1) +mz(q2
0q2
1q2
2+q2
3)3
775
(6.20)
Noul câmp de referint ,˘a e calculat în scopul obt ,inerii aceias ,i înclin ˘ari în cadrul vectorului m ˘asu-
r˘atorilor.
b=2
664bx
by
bz3
775=2
664q
m0x2+m0y2
0
m0
z3
775(6.21)
Aceast ˘a nou ˘a valoare de referint ,˘a este substituit ˘a în maparea original ˘a, unde termenul by= 0
2
664mx
my
mz3
775= (h(xi)) =R( !q)
2
664bx
by
bz3
775=2
664bx(q2
0+q2
1q2
2q2
3) +bz(q1q3q0q2)
2bx(q1q2+q0q3) +bz(q2q3q0q1)
2bx(q1q3+q0q2) +bz(q2
0q2
1q2
2+q2
3)3
775(6.22)

Capitolul 6. Filtrarea datelor 23
Aceste dou ˘a map ˘ari pot fi combinate într-o funct ,ie extins ˘a a m ˘asur˘atorilorhs,i a expresiei Jaco-
bianuluiH
h(xi) =2
666666666642(q1q2q0q2)
2(q2q3q0q1)
q2
0+q2
1+q2
2q2
3
bx(q2
0+q2
1q2
2q2
3) +bz(q1q3q0q2)
2bx(q1q2+q0q3) +bz(q2q3q0q1)
2bx(q1q3+q0q2) +bz(q2
0q2
1q2
2+q2
3)3
77777777775(6.23)
H=@h
@x=2
666666666666642q22q3 2q02q1 0 0 0
2q12q02q32q2 0 0 0
2q0 2q12q22q3 0 0 0
2q0 2q12q22q3 0 0 0
2(q0bxq2bz) 2(q1bx+q3bz) 2(q2bxq0bz) 2(q3bx+q1bz) 0 0 0
2(q3bx+q1bz) 2(q2bx+q0bz) 2(q1bx+q3bz) 2(q0bx+q2bz) 0 0 0
2(q2bx+q0bz) 2(q3bxq1bz) 2(q0bxq2bz) 2(q1bx+q3bz) 0 0 03
77777777777775
(6.24)
6.3.4 Matricele de covariant ,˘a
Rezultatele filtrului Kalman estins sunt afectate s ,i de specificat ,iile matrielor de covariant ,˘aQs,i
R.Acestea sunt matrice diagonal ˘a reprezentând variat ,ia as ,teptat ˘a a zgomotului în faza predict ,iei
s,i în vectorul m ˘asur˘atorilorz.În aceast ˘a aplicat ,ie matricele de covariant ,˘a au fost alese astfel:
Q=2
666666666666640 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 :2 0 0
0 0 0 0 0 0 :2 0
0 0 0 0 0 0 0 :23
77777777777775(6.25)

Capitolul 6. Filtrarea datelor 24
R=2
666666666666641
e6 0 0 0 0 0 0
0 1
e6 0 0 0 0 0
0 0 1
e6 0 0 0 0
0 0 0 1
e6 0 0 0
0 0 0 0 1
e6 0 0
0 0 0 0 0 1
e6 0
0 0 0 0 0 0 1
e63
77777777777775(6.26)
6.3.5 Ecuat ,iile filtrului Kalman extins
Ecuat ,iile filtrului Kalman extins se pot clasifica în dou ˘a sect ,iuni specifice etapelor filtr ˘arii.
Ecuat ,iile predict ,iei
Predict ,ia orinet ˘ari:xi=f(xi1;ui)
Covariant ,a predict ,iei:P=FPF0+Q
Ecuat ,iile corect ,iei
Predict ,ia m˘asur˘atorii:y=zh(xi)
Covariant ,a m˘asur˘atorii:S=HPH0+R
Matricea de amplificare Kalman: K=PH0S1
Corect ,ia orinet ˘arii prezise:xi=xi+Ky
Corect ,ia covariant ,ei prezise:P= (IKH)P
CuF=@f
@xs,iH=@h
@x.Ireprezint ˘a matricea unitate.
Expresia cuaternionului rezultat poate fi convertit ˘a în expresia sub forma unghiurilor lui Euler.
2
664pitch
roll
yaw3
775(q) =2
664atan2(2(q0q1+q2q3);12(q2
1+q2
2))
asin(2
(q0q2+q3q1))
atan2(2
(q0q3+q1q2);12
(q2
2+q2
3))3
775(6.27)

Anexa A
Appendix Title Here
Write your Appendix content here.
25

Bibliografie
26