Sunteți pe pagina 1din 52

UNIVERSITATEA TEHNICĂ „Gheorghe Asachi” din IAȘI

FACULTATEA DE AUTOMATICĂ ȘI CALCULATOARE


DOMENIUL: Ingineria Sistemelor
SPECIALIZAREA: Informatică Aplicată

Recuperarea mişcării folosind senzori


inerţiali

Lucrare de Licenţă

Îndrumător:
Conf. Dr. Ing. Adrian Burlacu
Absolvent
Adrian Barcan

1
2
DECLARAȚIE DE ASUMARE A AUTENTICITĂȚII
LUCRĂRII DE LICENȚĂ

Subsemnatul Barcan Adrian, legitimat cu CI seria NT nr. 619899, CNP 1940116270825,


autorul lucrării Recuperarea mişcării folosind un senzor IMU elaborată în vederea susţinerii
examenului de finalizare a studiilor de licență organizat de către Facultatea de Automatică şi
Calculatoare din cadrul Universităţii Tehnice „Gheorghe Asachi” din Iaşi, sesiunea iulie 2017 a
anului universitar 2016-2017, luând în considerare conţinutul Art. 34 din Codul de etică
universitară al Universităţii Tehnice „Gheorghe Asachi” din Iaşi (Manualul Procedurilor,
UTI.POM.02 – Funcţionarea Comisiei de etică universitară), declar pe proprie răspundere, că
această lucrare este rezultatul propriei activităţi intelectuale, nu conţine porţiuni plagiate, iar
sursele bibliografice au fost folosite cu respectarea legislaţiei române (legea 8/1996) şi a
convenţiilor internaţionale privind drepturile de autor.

Data Semnătura

3
4
Cuprins

1. Introducere 8

2. Senzori inerţiali 11
2.1 Considerente Teoretice 11
2.2 Senzorul LPMS-B 13
3. Filtre de prelucrare a semnalelor 15
3.1 Filtru Kalman 18
3.2 Filtru Butterworth 24
4. Recuperarea mişcării folosind senzori inerţiali 31
4.1 Recuperarea Orientării 32
4.2 Recuperarea Poziţiei 35
4.3 Recuperarea Posturilor 3D 36
5. Rezultate experimentale 37
5.1 Utilizarea IMU în recuperarea mişcării persoanelor 37
5.2 Utilizarea IMU în recuperarea mişcării sistemelor automotive 41
6. Concluzii 45
7. Anexe 48

5
Lista de figuri
1. Senzorul folosit pentru experimente LPMS-B 13

2. Interfaţa folosită de senzor OpenMat 14

3. Exemplu de grafic pentru filtrare Butterworth 24

4. Exemplul de poli pentru filtrul Butterworth 25

5. Graficul Datelor Vitezei Liniare 27

6. Filtrare Trece-Jos 28

7. Filtrare Trece-Sus 28

8. Filtrare Trece-Banda 29

9. Filtru trece-jos de ordinul 3 format dintr-un circuit RLC 30

10. Imaginea unui Cuaternion într-un cadru de 6DoF 32

11. UCS în mediul 3D de Matlab 33

12. Exemplu de orientare arbitrară a Cuaterninilor 34

13. Reprezentarea celor 6 grade de libertate (6DoF) 36

14. Mediul 3D al afişării datelor 38

15. Graficul 3D al experimentului de alergare 38

16. Graficul experimentului simplu 39

17. Harta (Google Maps) a sectorului utilizat pentru experimentul automotive 41

18. Graficul 3D pentru experimentul automotive 42

19. Graficul 3D pentru experimentul automotive agresiv 43

20. Arhitectura Detectării condusului 44

6
Rezumat

Această lucrare descrie o metodă de recuperare a mişcării folosind senzorul de mişcare


inerţiala (IMU) mai exact senzorul LPMS-B prin înregistrarea şi folosirea datelor stocate de acesta
pentru în mediul Matlab pentru a recupera miscarea folosind următoarele seturi de date:
Acceleraţia, Giroscopul, Orientarea(Cuaternioni, Unghiuri Euler) şi Acceleraţia Liniară.
Algoritmul de recuperare a mişcării va genera un grafic 3D ce conţine traiectoria parcursa de
senzor precum si modificarile survenite in orientare.

Datele vor fi trecute prin diferite filtre, de exemplu filtrul Kalman, cunoscut sub numele de
estimator pătratic liniar, filtrul Kalman este un algoritm care utilizează o serie de măsurători
observate în timp, care conţin zgomot(variaţii aleatorii) sau alte inexactităţi produce estimări ale
variabilelor necunoscute, care tind să fie mai precise decât cele bazate pe o singură măsurare. Şi
mai avem filtrul Butterworth care sortează datele vectoriale prin transferul de trece sus/trece jos,
adică prin filtrarea frecvenţelor lăsând doar o parte din ele să treacă.

Creşterea rapidă a traficului rutier şi a transporturilor cauzate de urbanizare şi motorizare


a fost mereu acompaniata şi de accidente în trafic. Pentru reducerea acestor accidente nu e necesară
doar aducerea la cunoştinţa a populaţie, e nevoie şi de măsuri tehnologice de a reduce riscurile.
Lucrarea îşi propune să prezinte modul în care orice tip de mişcare poate fi analizată şi ulterior
recuperată iar aceste metode la rândul lor pot fi folosite în diverse domenii cum ar fi: Automotive
– Detectarea condusului agresiv, aici cu seturile de date de la un astfel de senzor, un GPS şi analiza
printr-o logică fuzzy se pot stabili stiluri diferite de condus; Medicina – Analiza mişcărilor
corpului, în cazul acesta, folosind un senzor IMU putem analiza în timp mişcările diferitelor părţi
ale corpului în cazul în care au o perioadă lungă de recuperare sau sunt înlocuite de proteze.

7
Capitolul 1
Introducere

Recuperarea mişcării reprezintă procesul de înregistrare şi recuperare a mişcării obiectelor


sau oamenilor. Este folosită în armată, divertisment, sporturi, aplicaţii medicale şi pentru viziunea
optică a calculatoarelor şi roboţilor. În divertisment adică în industria filmelor şi jocurilor video se
referă la înregistrarea acţiunilor unui actor uman şi apoi folosirea acelei informaţii pentru a anima
un model de caracter digital într-o animaţie computerizată 2D sau 3D.

Sistemele optice folosesc datele capturate din imagini pentru a triangula poziţia 3D a unui
subiect între două sau mai multe camere calibrate să suprapună proiecţii. Achiziţia datelor este
implementata folosind markeri speciali ataşaţi pe actor, dar cele mai recente sisteme sunt capabile
să genereze date exacte urmărind detaliile suprafeţelor identificate dynamic pentru fiecare subiect
în parte. Urmărirea unui număr mare de actori sau mărind planul de captură este realizată prin
montarea unor camere adiţionale.

Aceste sisteme produc date cu 3 grade de libertate pentru fiecare marker şi informaţia
rotaţiei trebuie dedusa din orientarea relativă a doi sau trei markeri, de exemplu, umărul, cotul şi
încheietura împreună dau unghiul cotului. Noile sisteme hibride combina senzorii inerţiali cu
sisteme optice pentru a reduce zgomotul, pentru a creşte numărul de utilizatori şi pentru a
îmbunătăţi abilitatea de a urmări fără a fi nevoie să cureţe datele manual. [1-4]

Sisteme Non-Optice:

Recuperarea mişcării inerţiale este bazată pe senzori inerţiali, modele biomecanice şi


algoritmi fuzzy. Datele mişcării ale senzorilor (sau sistemele de ghidare inerţiale în domeniul
automotive) sunt de obicei transmise wireless către un computer, unde mişcarea este înregistrată
sau vizionată. Majoritatea sistemelor inerţiale folosesc unităţi de măsură inerţiale (IMU) care
conţin o combinaţie de giroscop, magnetometru şi accelerometru pentru a măsura rata şi axele de
rotaţie. Aceste date sunt traduse într-un “schelet” în software.

8
Asemănători cu markerii optici, cu cât mai mulţi senzori IMU cu atât mai natural sunt
datele obţinute. Nu e nevoie de camere externe, emiţători sau markeri pentru mişcările relative, cu
toate că senzorii sunt nevoiţi să marcheze poziţia absolută dacă utilizatorul doreşte. Unităţile de
captură a mişcării inerţiale capturează toate cele 6 grade de libertate ale corpului uman în timp real
şi poate da informaţii a direcţiei limitate dacă includ şi un senzor magnetic cu toate că acestea au
o rezoluţie mai mică şi foarte susceptibile zgomotului magnetic. Avantajele folosirii sistemelor
inerţiale sunt: capturarea datelor intro varietate de medii, incluzând spaţiile strâmte, portabilitate
şi zone mari de captură. Dezavantajele ar fi o acurateţe scăzută a poziţiei şi mult zgomot poziţional
care se poate aduna în timp.

Aceste sisteme sunt asemănătoare cu controllerele Wii dar sunt mult mai sensibile, au o
arie mai mare de captură şi o rata mai rapidă de update. Ele pot măsura cu acurateţe direcţia către
pământ cu până la un grad.

Mişcare mecanică

Sistemele de captură a mişcării mecanice urmăresc direct unghiurile articulaţiilor corpului


şi sunt des numite sisteme de captură exo-schelet datorită modului în care sunt ataşaţi de corp. Un
actor ataşează structura “schelet” pe corp şi apoi se mişca astfel încât părţile mecanice articulate
măsoară mişcarea relativă a actorului. Sistemul de captură a mişcării mecanice transmit datele în
real-time, sunt relativ ieftine, au puţine erori şi sistemele wireless au volum nelimitat de captură.
De obicei sunt sisteme rigide cu metal, bare de plastic articulate şi legate împreună cu
potenţiometre care articulează articulaţiile şi încheieturile corpului.

Sisteme magnetice

Acestea calculează poziţia şi orientarea din datele fluxului magnetic relativ sau din cele
trei bobine ortogonale la capetele transmiţătorului şi a receptorului. Intensitatea relativă a
curentului sau a tensiunii din cele trei bobine permit acestui sistem să calculeze atât distanţă cât şi
orientarea mapând meticulos volumul de urmărire. Datele de ieşire a senzorului sunt cele 6 grade
de libertate a mişcării (6DoF – 6 Degrees of Freedom) [5] care dau date folositoare folosind două
treimi din markerii utilizaţi în sistemele optice, de exemplu unul pe partea superioară a braţului iar
celălalt pentru poziţia cotului şi unghi. Markerii nu au interferenţe cu obiectele non-metalice dar
sunt susceptibili zgomotului magnetic, interferentele electrice a obiectelor metalice din zonă,

9
cabluri, computere. Răspunsul senzorului este non-linear în special către marginile zonei de
captură. [1-4]

Între sistemele de captură magnetice, este o diferenţă între curentul alternativ şi curentul
continuu, unul foloseşte pulsuri pătratice iar celălalt sinusoide.

În capitolul 2 vom prezenta senzorul LPMS-B, cel folosit pentru a înregistra datele
experimentelor, inclusiv modul de achiziţionare al acestor date. [5]Vom continua cu capitolul 3
unde avem în detaliu filtrările folosite pentru a procesa datele experimentale, filtrul Kalman[5-10]
pentru predicţie şi filtrul Butterworth [17-18] pentru netezire. După achiziţionarea şi filtrarea
datelor Capitolul 4 prezintă recuperarea orientării, mişcărilor şi posturilor. [20-21]

Iar în capitolul 5 vom avea rezultatele experimentelor, mai exact recuperările traseurilor
parcurse cu senzorul în cazurile simple cum ar fi mişcări ale persoanelor cât şi cazuri mai complexe
însemnând trasee cu sisteme automotive. [19]

10
Capitolul 2
Senzori inerţiali
2.1 Considerente Teoretice

Senzorii utilizaţi în domeniul navigaţiei inerţiale, întâlniţi în literatură şi sub denumirea


generică de senzori inerţiali, sunt reprezentaţi cel mai des de accelerometre şi giroscoape.
Accelerometrele măsoară forţa specifică iar giroscoapele măsoară viteză unghiulară fără a avea
o referinţă externă. Instrumentele care măsoară viteza, acceleraţia sau viteză unghiulară
raportate la alte elemente din mediul înconjurător nu intră în categoria senzorilor inerţiali.
O unitate inerţială de măsură (IMU) este un dispozitiv electronic care măsoară şi raportează
forţa, unghiul şi uneori câmpul magnetic ce înconjoară un corp folosind o combinaţie de
accelerometri şi giroscop respectiv magnetometri. IMU-urile sunt folosite în general pentru
manevrarea aeronavelor inclusiv cele fără pilot, printre altele şi nave spaţiale cum ar fi sateliţii
sau navele de aterizare, aselenizare. Noile lucruri care au apărut în folosirea senzorilor IMU
au fost introducerea dispozitivelor GPS, un IMU permite semnalului GPS să lucreze chiar şi
în zone fără semnal cum ar fi tuneluri, clădiri, sau zone cu interferenţe electronice. [6-7]
În sistemele de navigaţie, datele înregistrate de IMU sunt introduse intrun procesor ce
calculează orientarea, viteza şi poziţia. Datele unghiulare de la giroscop sunt integrate pentru
a se calcula poziţia unghiulară. Apoi este inserată cu vectorul de gravitaţie măsurat de
accelerometri într-un filtru Kalman pentru a estima altitudinea. Altitudinea estimată este
folosită pentru a transforma măsurătorile accelerometrului într-un tipar de referinţă inerţială
unde sunt integraţi din nou pentru a se afla viteza liniara şi încă o dată pentru poziţia liniara.

Ansamblul de senzori utilizaţi pentru determinarea forţei specifice (accelerometre) şi a


vitezelor de rotaţie (giroscoape), poartă denumirea de Unitate Inerţială de Măsurare. Pentru
aplicaţii ce necesită şi un calcul al poziţiei şi orientării vehiculului, este necesar ca IMU să fie
completat cu un microprocesor ce rezolvă ecuaţiile de orientare şi de navigaţie.

De exemplu, dacă un IMU instalat pe o aeronavă merge pe un vector cu o direcţie exactă


şi măsoară acceleraţia la 5m/s^2 pentru 1 secundă atunci computerul de ghidare va deduce că
avionul călătoreşte cu 5m/s şi că e la 2.5m distanţa faţă de poziţia iniţială (presupunând că v=0,

11
x0,y0,z0). Combinat cu o hartă digitală sau mecanică, sistemul de ghidare poate folosi această
metodă pentru a-i arăta unui pilot unde este aeronava localizată geografic într-un anumit
moment, la fel ca un sistem GPS, doar că fără niciun fel de comunicare externă, iar cum poziţia
sistemelor inerţiale are o reactualizare de frecvenţă mare, localizarea mişcării pe harta poate fi
percepută cât mai lin. [20]

Una dintre cele mai vechi unităţi de acest tip a fost proiectată şi construită de către Ford
Instrument Company pentru a ajuta avioanele să navigheze în zbor fără niciun ajutor exterior
aeronavei. Numit şi Ground-Position Indicator, o dată ce pilotul introduce datele latitudinii şi
longitudinii la decolare, dispozitivul va arăta pilotului latitudinea şi longitudinea aeronavei în
referinţă cu Pământul.

Un mare dezavantaj al acestor sisteme ar fi faptul că suferă de erori acumulate în timp.


Deoarece sistemul de ghidare integrează continuu acceleraţia şi respectiv timpul de calcul al
vitezei şi poziţiei, orice eroare de măsurare, deşi relativ mică, sunt acumulate în timp, acest
lucru duce la zgomot, o diferenţă în creştere între unde crede sistemul că se afla şi unde se afla
el de fapt. Datorită integrării unei erori constante în acceleraţie va rezulta o eroare în viteză
liniara şi o eroare pătratica în poziţia liniara. O eroare constantă în giroscop rezulta o eroare
pătratica în viteză şi o eroare cubică în poziţie. [20]
Pentru calculul matricei de rotaţie a unui IMU cel mai bun element folosit este
cuaternionul, deoarece acesta are cea mai mică perturbaţie în cazul zgomotului care apare la
folosirea acestui senzor. [21]

12
2.2 Senzorul LPMS-B

Senzorul folosit (Fig. 1) pentru aceasta aplicaţie este LP-Research Motion Sensor
Bluetooth (LPMS-B) [5], este un senzor de dimensiuni mici, wireless de măsurători inerţiale şi cu
sistemul de referinţă de orientare (AHRS).

(Fig. 1 - Senzorul folosit pentru experimente LPMS-B)

Este foarte versatil, precis, orientare rapidă şi măsurători ale erorii de poziţie. Foloseşte 3
senzori diferiţi MEMS – Accelerometru cu 3 axe, Giroscop cu 3 axe, Magnetometru cu 3 axe.
Temperatura opţională şi presiunea barometrică vor da măsurători corecte şi pentru altitudine.
Senzorul comunica cu o conexiune clasică Bluetooth 2.1. Senzori multipli LPMS-B pot fi folosiţi
simultan. Senzorul e capabil de o transmisiune de date cu o rată de până la 400Hz. Interfaţa acestuia
pentru sistemul Windows/Linux se numeşte OpenMat (figura 2) iar aici putem observa datele în
timp real şi orientarea. Tot în această interfaţă putem reseta senzorul la specificaţiile iniţiale.

13
(Fig. 2 - Interfaţa folosită de senzor OpenMat)
Datele înregistrate sunt salvate pe disk fără niciun fel de extensie, pentru a le folosi în
matlab, acestor date le vom da extensia “.csv” care le va transforma intrun fişier Excel cu tabelele
separate pentru fiecare tip de date (Acceleraţie X, Acceleraţie Y, etc.) pentru că ulterior să fie
folosite fiecare în afişarea graficelor şi calculul orientării cât şi a traiectoriei parcurse de senzor.

14
Capitolul 3
Filtre de prelucrare a semnalelor

În procesarea semnalelor, un filtru este un dispozitiv sau un proces care elimină unele
componente nedorite sau unele caracteristici a unui semnal. Filtrarea este o clasă de procesare a
semnalelor definiţia de bază a unui filtru fiind cea de suprimare completă sau parţială a unor
aspecte a semnalului. Cel mai des asta înseamnă eliminarea unor frecvente sau a unor benzi de
frecvenţe. [22-24]

Sunt mai multe metode de clasificare a filtrelor, ele pot fi:

- Liniare sau non-liniare

- Variabile sau invariabile în timp, cunoscut ca şi schimbarea invariatiei. Dacă filtrul


lucrează intrun domeniu spaţial atunci caracterizarea este invariatie spaţială.

- Cauzale sau non-cauzale. Un filtru este non-cauzal dacă rezultatul lui actual depinde de
rezultatul care urmează. Procesarea în semnalelor în domeniul timp şi în timp real sunt
strict cauzale, dar nu şi filtrele care acţionează în domeniul spaţial.

- Digitale sau analogice

- Discrete în timp sau continue în timp

- Pasive sau active – pentru cele continue în timp

- Cu răspuns la impuls infinit sau cu răspuns finit – pentru cele discrete

Filtrele liniare continue în timp e probabil cel mai folosit filtru de prelucrare a
semnalelor. Circuitele liniar continue în timp sunt în general proiectate să elimine anumite
frecvente şi să permită altora să treacă. Circuitele care fac acest lucru sunt în general liniare în
răspunsul lor, sau cel puţin aproximativ. Orice non-liniaritate ar avea un rezultat potenţial cu
frecvenţe ce nu sunt în semnalul iniţial.

15
Metoda modernă pentru metodologia filtrelor liniare se numeşte sinteza reţelelor. Unele
familii importante de filtre ar fi:
- FIltrul Chebyshev, are cea mai bună aproximare a răspunsului ideal
- Filtrul Butterworth are răspunsul maxim cel mai neted al frecventei.
- Filtrul Bessel, are un răspuns maxim neted cu întârziere de faza.
- Filtrul Elliptic, are cea mai abruptă tăiere dintre filtre pentru ordine şi răspuns.
Diferenţa dintre aceste familii de filtre este ca toate folosesc funcţii polinomiale diferite
pentru a aproxima răspunsul ideal, însemnând că fiecare are funcţia de transfer diferită
O altă metodă mai veche şi mai puţin folosită este cea a parametrilor imaginii. Filtrele
proiectate de aceasta metodologie se mai numesc şi filtre de undă. Câteva filtre importante din
această categorie sunt:
- Filtrul de constanta k, cel mai simplu şi original filtru pentru formele de undă
- Filtru de derivaţie m, o modificare a constantei k improvizat pentru filtrările abrupte şi
pentru asocierea impedanţelor.

Răspunsurile în frecvenţa pot fi clasificate în diferite forme de banda ce descriu care


frecvenţa îşi schimba banda (cea de trecere) şi care este eliminată (cea de stop):
- Filtru trece-jos, frecvenţele joase trec, frecvenţele înalte sunt atenuate
- Filtru trece-sus, frecvenţele înalte trec, frecvenţele joase sunt atenuate
- Filtru trece-banda, doar frecvenţele dintr-o anumită banda trec
- Filtrul stop-banda, doar frecvenţele dintr-o anumită banda sunt atenuate
- Filtrul Notch, elimina doar o frecvenţă specifică, un filtru stop-banda extrem
- Filtrul Comb, are multiple porţiuni de trece-banda care face formă de undă să pară în forma
unui pieptene
- Filtrul trece-tot, toate frecvenţele trec, dar faza ieşirii este modificată

O importanţă aplicare a filtrelor este în telecomunicaţii, unde multe din aceste sisteme
folosesc multiplexarea diviziunilor de frecvenţă, unde proiectanţii împart o frecvenţă largă în mai
multe frecvente înguste numite sloturi, sau canale, iar fiecare conţine câte o bucată de informaţie.

16
Filtrele pot fi construite cu tehnologii diferite, adică aceasi funcţie de transfer poate fi
realizată în mai multe moduri diferite, iar proprietăţile matematice ale filtrului sunt la fel dar
proprietăţile fizice sunt diferite.
Ele pot fi:
- Filtre Electronice, iniţial erau complet pasive, construite din rezistenţă, inductanţa şi
capacitate
- Filtre Digitale, operează pe semnale în formă digitală. Esenţa unui filtru digital este în
implementarea unui algoritm matematic, corespunzător funcţiei de transfer a filtrului dorit
în programare.
- Filtre Mecanice, sunt construite din elemente mecanice, în majoritatea cazurilor sunt
folosite pentru a procesa un semnal electronic şi apoi convertit în vibraţii mecanice.
- Filtre cu Elemente distribuite, sunt construite din componente construite din piese mici de
linie de transmisie sau elemente distribuite. Sunt structuri care corespund direct cu
elementele catalogate ale filtrelor electronice sau altele unice acestei tehnologii
- Filtre cu Cristal, foloseşte cristalul de quartz ca rezonator, sau alte elemente piezoelectrice
- Filtre Acustice
- Filtre Optice, iniţial construite pentru luminozitate şi fotografie, dar cu creşterea
tehnologiei fibrei optice aceste filtre au o creştere în aplicarea lor pentru filtrarea
semnalelor ale aplicaţiilor de procesare

17
3.1 Filtrul Kalman

Teoretic, un filtru Kalman [9-14] este un estimator pentru problema liniar pătratică.
Aceasta se defineşte ca fiind o problemă în estimarea unei stări instantanee al unui sistem dinamic
liniar perturbat de zgomot alb, folosind măsurători legate liniar de stare, dar perturbate de
zgomot alb. Estimatorul rezultant este unul optim din punct de vedere statistic.

Avantaje relative ale filtrelor Kalman si Wiener:


1. Implementarea filtrului Wiener în circuite electronice analogice poate determina la o
eficientă mult mai mare decât filtrul digital Kalman.
2. Filtrul Kalman este implementat în forma unui algoritm pentru calculatoarele digitale,
care inlocuieşte circuitele analogice pentru estimare şi conducere în vremea în care filtrul
Kalman a fost introdus.
Aceasta implementare poate fi mai înceata, dar este capabilă de o precizie mult mai mare
decât poate fi atinsă de un filtru analogic.
3. Filtrul Wiener nu are nevoie de modele deprocese stohastice finite dimensional
pentrusemnal si zgomot.
4. Filtrul Kalman este compatibil cu formularea controlerelor optimale în spaţiul stărilor
alesistemelor dinamice, iar Kalman a reuşit să dovedească utilitatea celor două
proprietăţiale estimării şi conducerii pentru aceste sisteme.

Algoritmul celor mai mici patrate generalizate.


Una dintre abordările cele mai des utilizate în teoria filtrării adaptive este cea bazata pe
formularea unei astfel de aplicaţii sub forma unei probleme de optimizare. În mod concret, se
defineşte un criteriu de performanta (denumit de regula funcţie de eroare) care depinde de setul de
coeficienţi ai filtrului si de proprietăţile statistice ale semnalelor de la intrarea si ieşirea doritaa
acestuia, care asociază fiecărui vector de coeficienţi o valoare scalara. Aspectul geometric al
suprafeţei multidimensionale rezultate poate fi foarte complicat, însa în cazul particular al unui
filtru discret liniar de tip FIR si al erorii pătratice medii aceasta se prezintă ca o
suprafaţa parabolica de forma convexa, având o valoare minima unica. Mai mult, vectorul de coe
ficienţi corespunzător acestei valori minime este chiar setul optim definit de ecuaţiile Wiener-

18
Hopf! În principiu, determinarea acestui sistem de ecuaţii s-ar putea efectua “dintr-un singur foc”
prin calcul algebric, însa în multe situaţii practice apar dificultăţi datorate dimensiunilor mari ale
matricelor implicate sau probleme de stabilitate ale metodelor numerice folosite în inversarea
matricei de autocorelaţie R.

3.1.1 Filtrul Kalman discret


Aceasta secţiune conţine descrierea filtrului în formularea initială realizată în 1960 de
catre Kalman, în care masurările au loc şi stările sunt estimate la momente de timp discrete.
a)Procesul ce se doreşte a fi estimat
Filtrul Kalman se adreseaza in mod special incercarii de a estima starea 𝑥 𝑛 a unui proces
controlat discret-in-timp, care este guvernat de catre ecuatia diferentiala aleatoare:
xK  AxK 1  BuK  wK 1 , (1.1)

unde avem valoarea masurată:


z K  HxK  vK . (1.2)

Variabilele aleatoare wk si vK reprezinta zgomotul procesului respectiv zgomotul masurat. Ele se


presupun a fi independente(fiecare), pure si cu distributia de probabilitate normala:
p(w) ~ N (0, Q) , (1.3)
p(v) ~ N (0, R) . (1.4)
În practică, matricile covarianţei de zgomot ale procesului Q şi covarianţei de zgomot
măsurată R respectiv, se pot schimba la fiecare pas de timp sau al măsurătorii, însă cu toate acestea
ne vom asuma faptul de a-l considera constant.
Matricea A de dimensiuni n x n relaţionează starea de la pasul anterior de timp k-1 cu starea
de la pasul de timp curent k, în lipsa unei funcţii de conducere sau a unui zgomot in cadrul
procesului. Notabil însă, în practică A se poate schimba la fiecare pas, insă aici noi il vom asuma
ca fiind constant. Matricea B de dimensiuni 𝑛 ∗ 𝑙 relaţionează intrarea de control opţională
𝑢 𝜖  𝑛 faţă de starea x. Matricea 𝑚 ∗ 𝑛 𝐻 relaţionează starea cu masurătoarea z K . În practică H

se poate schimba la fiecare pas de timp sau la fiecare nouă măsurătoare, insă aici il vom considera
ca şi constant.

19
b)Originile calculului filtrului
Vom defini xˆ K   n (evidenţiind prin minusul de deasupra) ca fiind estimarea a

priori a starii la pasul k având certitudinea procesului de dinaintea pasului k, si xˆ K   n ca fiind

starea a posteriori faţă de pasul k, dat de masurătoarea z K . Putem atunci sa definim erorile de

estimare a priori şi a posteriori ca fiind:


eK  xK  xˆK , si (1.5.1)

eK  xK  xˆK . (1.5.2)

Eroarea de estimare a covarianţei exprimată a priori devine:


PK  E[eK eKT ] , (1.6)

în timp ce eroarea de estimare a covarianţei exprimate a posteriori este:


PK  E[eK eKT ] . (1.7)

În derivarea ecuaţiilor pentru filtrul Kalman vom începe cu căutarea unei ecuaţii care
calculează o estimare a posteriori a stării - xˆ K ca si combinaţie liniară a unei estimări a priori xˆK

şi diferenţa ponderată dintre masurătoarea actuală z K şi o masurătoare prezisă H xˆK după cum se
poate vedea în ecuaţia de mai jos:
xˆK  xˆK  K ( z K  HxˆK ) . (1.8)

Diferenţa ( z K  Hxˆ K ) din ecuaţia (1.8) se numeşte masurarea inovaţie sau reziduală.

Rezidualul reflectă discrepanţa dintre masuratoarea prezisă şi măsurătoarea actuală z K . Un

rezidual de 0 arată faptul că cele doua sunt în perfectă conformitate.


Matricea K n x m este aleasă să fie câştigul sau factorul de golire care minimizează ecuaţia (1.7) a
erorii de covarianţă a posteriori. Aceasta minimizare poate fi îndeplinită de către prima ecuaţie
substitutivă (1.8) în definiţia de mai sus pentru eK , substituind aceasta în ecuaţia (1.8), realizând

separările, derivând apoi faţă de K, egalând cu zero şi apoi rezolvând pentru K. O formă a lui K
rezultat care minimizează ecuaţia (1.7) este:
PK H T
K K  PK H T ( HPK H T  R ) 1 = (1.9) 1
( HPK H T  R)1

1
Toate ecuaţiile care descriu filtrul Kalman pot fi scrise algebric in diverse forme. Ecuaţia (1.9) reprezintă calculul
Kalman intr-o formă populară.

20
Privind ecuaţia (1.9) se observa că la fel cum eroarea de covarianţă masurată R se apropie
de zero, câştigul K măreşte rezidualul mult mai mult. Mai precis:
𝑅 lim 𝐾𝑘 = 𝐻 −1 (2)
𝑘→0

Pe de altă parte, precum estimarea a priori a erorii de covarianţă PK se apropie de zero, câştigul
K creste mult mai puţin pierderile reziduale. Mai precis:
𝑃𝐾− lim 𝐾𝑘 = 0 (2.1)
𝑘→0

Un altfel de a gandi creşterea cu K este că la fel precum eroarea de covarianţă masurată se


apropie de zero, măsurătoarea actuală z K este luată în considerare din ce în ce mai mult, în timp

ce valoarea prezisă Hxˆ K este luată in considerare din ce în ce mai puţin. Pe de altă parte, aşa cum

eroarea de covarianţă exprimată a priori PK se apropie de zero, masurarea actuală z K este luata în

considerare din ce în ce mai puţin, în timp ce Hxˆ K este luat în considerare din ce în ce mai puţin.
c)Originile probabiliste ale filtrului
Justificarea pentru ecuaţia (1.8) este stabilit in probabilitatea estimării Hxˆ K conditionată

de toate masurătorile z K precedente (regula lui Bayes). Deocamdată este suficient să pontăm faptul

că filtrele Kalman menţin primele doua momente ale distribuţiei de stări,


E[ xK ]  xˆK

E[( xK  xˆK )( xK  xˆK )T ]  PK . (2.2)

Ecuaţia de estimare a stării a posteriori (1.8) reflectă media (primul moment) a distribuţiei
de stări – aceasta are o distribuţie normală dacă condiţia ecuaţiei (1.3) si ecuaţia (1.4) sunt
indeplinite. Ecuaţia ce reprezintă estimarea erorii de covariaţie a posteriori (1.7) reflectă variaţia
distribuţiei de stări (al doilea moment ne-central). Cu alte cuvinte:
p ( xK z K ) ~ N ( E[ xK ], E[( xK  xˆK )( xK  xˆK )T ])  N ( xˆ K , PK ) . (2.3)

c)Algoritmul filtrului Kalman discret


Filtrul Kalman estimează un proces folosind o undă-raspuns de control : filtrul estimează
starea procesului la un anumit moment de timp şi apoi obtine o undă-răspuns (de zgomot)
masurată. Ca atare, ecuaţiile filtrului Kalman se despart în două tipuri: ecuaţii actualizate în timp
şi ecuaţii actualizate la măsurare. Ecuaţiile actualizate în timp sunt responsabile pentru proiecţia

21
în viitor a stării curente şi a erorii de covariaţie estimată pentru a obţine estimările a priori pentru
pasul următor. Ecuaţiile actualizate la măsurare sunt responsabile pentru răspuns – de exemplu
pentru încorporarea unei noi măsurători în estimarea a priori pentru a obtine o estimare a posteriori
îmbunătăţită.

3.1.2 Filtrul Kalman extins (EKF)


a)Procesul ce se doreşte a fi estimat
La fel ca în descrierea de mai sus, filtrul Kalman adresează problema generală a încercării de a
estima starea x n a unui proces controlat în timp discret, guvernat de catre o ecuaţie diferenţială
liniară aleatoare. Dar ce se întâmplă dacă procesul spre a fi estimat şi (sau) relaţia procesului cu
masurătorile este non-liniară? Una dintre cele mai interesante şi încununate de succes aplicaţii a
filtrării Kalman au fost astfel de situaţii. Un filtru Kalman care liniarizează faţă de media curentă
şi covariaţie este denumit filtrul Kalman extins sau EKF.
În unele situaţii înrudite cu seriile Tylor, putem liniariza estimarea în jurul estimării curente
folosind derivate parţiale ale procesului şi funcţii de măsurare pentru a calcula estimările în cazul
relaţionărilor neliniare. Pentru aceasta, trebuie să începem modificând unele calcule realizate în
Sectiunea 3.1.1. Să considerăm din nou că procesul nostru are un vector al stărilor x n , însă
acum procesul este guvernat de ecuaţia diferenţială neliniară aleatoare:
xK  f ( xK 1 , uK , wK 1 ), (2.4)

împreună cu o măsurătoare z m care este:


zK  h( xK , yK ) . (2.5)

în care variabilele aleatoare wK şi vK reprezintă din nou zgomotul procesului şi cel măsurat la fel

ca în ecuaţiile (1.3) şi (1.4). În acest caz funcţia neliniară f din ecuaţia (2.4) relaţionează starea de
la pasul anterior de timp k – 1 cu starea de la pasul current de timp, k. Aceasta include ca şi
parametrii orice funcţie de control u K şi zgomotul de medie zero al procesului wK . Funcţia

neliniară h din ecuaţia masurătorii (2.5) relaţionează starea xK de masurătoarea z K .

În practică desigur nu se va putea şti valorile precise ale zgomotului wK şi vK la fiecare

pas de timp. Totuşi, putem să aprozimăm starea şi vectorul măsurătorilor fără ele astfel:
x  f ( xˆK 1 , u K , 0) (2.6)

22
şi
zˆK  h( xK ,0) (2.7)

unde xˆ K este o estimare a posteriori a starii (de la timpul aferent pasului anterior pasului k).

Este important de notat că defectul fundamental al filtrului EKF este acela că distribuţiile
(sau continuităţile în cazurile continui) a numeroase variabile aleatoare nu mai sunt distribuite
normal dupa parcursul transformărilor lor neliniare. EFK este simplu un estimator de stări ad-hoc
care aprozimează optimalul regulei lui Bayes prin liniarizare.O lucrare interesantă a fost realizată
de către Julier et al. În dezvoltarea unei variante a EKF, folosind metode care conservă distribuţia
normală la trecerea prin transformările neliniare.

3.1.3 Aplicaţii ale filtrului Kalman


Am stabilit mai devreme că un filtru Kalman este un estimator liniar, adică
deduce parametrii de interes din surse de observare indirecte, imprecise sau nesigure. Am stabilit
că prezintă un algoritm recursiv, astfel încât noile măsurători pot fi procesate atunci când ajung.
Faptul de a fi considerat optimal se referă la faptul că acesta minimizează eroarea medie pătratică
a parametrilor estimaţi. [9]
În cazul unui zgomot de formă negausiană, filtrul Kalman având doar media şi deviaţia
standard a zgomotului, este cel mai bun estimator liniar. În acest caz estimatorii neliniari se
pretează mai mult a fi folositi. [11]
Popularitatea filtrului Kalman se datorează mai multor clauze: fiindcă este optimal şi
structurat; se pretează procesării în timp real a datelor; este uşor de înţeles şi implementat pe baza
unei întelegeri de bază; ecuaţiile de masurare nu trebuiesc inversate.
Dezvoltat iniţial pentru a fi folosit în navigaţia spaţială, filtrul Kalman se dovedeşte a fi util
în multe alte aplicaţii: determinarea parametrilor orbitei planetei din observaţii limitate ale
pământului; ţintirea pistelor – de exemplu în cazul avioanelor, proiectilelor, folosind radarul;
localizarea roboţilor şi construirea de hărţi prin senzori de distanţă sau semnalizări; aplicări în
economie; multe aplcaţii pe staţii computerizate: - stabilizarea măsurătorilor de adâncime,
urmărire a funcţiilor, urmărirea de grupuri (clustere), combinarea datelor primite de la radare,
scanere cu laser sau/şi camere stereo pentru masurători de adâncime şi de viteză, management al
traficului de date.

23
3.2 Filtrul Butterworth
Filtrul Butterworth este un filtru de procesare al semnalelor proiectat sa aiba un raspuns
in frecventa cat mai neted. A fost proiectat in anul 1930 de inginerul britanic Stephen Butterworth
in lucrarea sa cu titlul “On the Theory of Filter Amplifiers” (Teoria Filtrelor Amplificatoare)
Filtrul nu a fost folosit pentru mai bine de 30 de ani de la publicarea sa. Citatul celebru a
lui Stephen Butterworth despre acest filtru a fost: “Un filtru electric ideal nu ar trebui doar sa
elimine frecventele nedorite ci ar trebui sa si netezeasca sensibilitatea uniforma a frecventelor
dorite”.
Filtrul Butterworth (analogic) este definit de ordinul sau N si de o frecventa de taiere Ωt.
Raspunsul in frecventa al filtrului este:
1
|𝐻(𝑗𝜔)2 | = 𝐻(𝑗𝜔)𝐻(−𝑗𝜔) = 𝜔 2𝑁 (2.8)
1+( )
𝜔𝑡

Amplitudinea raspunsului in frecventa a filtrului Butterworth este descrescatoare:


1
|𝐻(𝑗𝜔𝑡 )| = , H(0)=1 (2.9)
√2

Avantajul pentru proiectare: raspunsul in frecventa are o forma analitica simpla, prezentata
in figura 3 unde avem un exemplu de schimbare a frecventei.

(Fig. 3- Exemplu de grafic pentru filtrare Butterworth)

24
Functia de transfer a filtrului Butterworth se determina punand s = j 𝜔.

1
𝐻(𝑠)𝐻(−𝑠) = 𝑠 2𝑁 (3)
1+( )
𝑗𝜔𝑡

Cei 2 N poli ai functiei H ( s ) H ( − s ) sunt definiti de (−1)1/2 N j𝜔t, deci au forma:

sk  t exp( j  / 2 N (2k 1  N )), k   : 2 N 1 (3.1)

Polii sunt plasati echidistant pe un cerc de raza Ω t centrat in origine, simetric fata de axa

imaginara. Penru H ( s ) se iau polii cu parte reala negativa astfel incat H ( s ) sa fie o functie de
transfer stabila.

(Fig. 4 - Exemplul de poli pentru filtrul Butterworth)

25
Filtrele Butterworth (au caracteristica plata in banda de trecere, banda de tranzitie nu
foarte ingusta, iar atenuarea in banda de oprire este mare) au in continuare explicatiile necesare
folosirii lor in matlab.
Functia folosita este butter( ),
[b, a] = butter(ord, wn, optiuni), unde:
ord reprezinta ordinul filtrului, avand valori uzuale in gama 10.15,
wn reprezinta frecventa de taiere normalizata, f / (fe/2), cu valori intre 0 si 1; pentru filtrele
de tip trece jos si trece sus, wn este un scalar, iar pentru cele de tip trece banda si opreste
banda wn este un vector linie cu 2 elemente, reprezentand banda de trecere, respectiv cea de taiere;
toate frecventele de taiere sunt date pentru o atenuare de 1/Ö2 (-3dB, daca caracteristica este
considerata in coordonate semilogaritmice), in caracteristica magnitudine-frecventa.
optiuni:
-daca acest camp nu contine nici un caracter, filtrul este trece jos sau trece banda, functie de
forma argumentului wn;
-daca contine caracterele 'high', filtrul este trece sus,
-daca contine caracterele 'stop', filtrul este opreste banda.
Se precizeaza ca pentru filtrele trece banda si opreste banda, filtrul obtinut este de ordinul
2ord, adica vectorii b si a au 2ord +1 elemente.
Pentru obtinerea ordinului minim necesar ca un filtru sa aibe caracteristici impuse referitoare la
banda (benzile) de tranzitie se foloseste functia buttord(), astfel:
[ord, wn] = buttord(wp, ws, Rp, Rs), unde
Rp reprezinta atenuarea, in banda de trecere, la wp, in dB,
Rs reprezinta atenuarea, in banda de oprire, la ws, in dB,
iar ord si wn, rezultatele furnizate de aceasta functie, au aceeasi semnificatie ca si la
functia butter.
Exemple:
1. Pentru un filtru trece jos, este necesar ca wp<ws.
2. Pentru un filtru trece sus, este necesar ca wp>ws.
3. Pentru un filtru trece banda este necesar ca elementele vectorului wp sa fie intercalate
intre cele ale vectorului ws.

26
4.Pentru un filtru opreste banda este necesar ca elementele vectorului ws sa fie intercalate
intre cele ale vectorului wp.
In experimentele efectuate cu senzorul LPMS-B, filtrul Butterworth s-a folosit pentru
filtrarea datelor inregistrate ale Vitezei Liniare.
In figura 5 avem graficul initial al vitezei liniare dinaintea a orice fel de filtrare.

(Fig. 5 – Grafic Viteza Liniara)

Aceste date au fost filtrate cu filtrul Butterworth in cele 3 cazuri (Trece-Jos, Trece-Sus si
Trece-Banda).Primul caz de filtrare Trece-Jos este prezentat in figura 6, unde se poate observa o
netezire foarte clara a datelor, in experimentele efectuate (capitolul 5) aceasta filtrare nu a fost
necesara ci doar testata.

27
(Fig. 6 – Filtrare Trece-Jos)

Urmatorul caz de filtrare, cea Trece-Sus din figura 7 a fost folosita in experimentele
efectuate cu scopul de a utiliza doar frecventele mari inregistrate si integrarea lor pentru a obtine
pozitia liniara, cea utilizata pentru trasarea miscarilor in graficul 3D. Aici axa X si Y au trecut intr-
o frecventa superioara

(Fig. 7 – Filtrare Trece-Sus)

28
Iar ultima filtrare folosita (figura 8) este cea de Trece-Banda, efectuata ca si in cazul filtrarii
Trece-Jos, doar pentru a fi testata. Aceasta filtrare este foarte apropiata de setul de date original
dar cu o netezire mai exacta decat filtrarea Trece-Jos.

(Fig. 8 - Filtrare Trece-Banda)

În figura 8 avem prezentat un filtru trece-jos de ordinul 3 format dintr-un circuit RLC.
Filtrul devine Butterworth cu o frecvenţă de “cutoff” ωc = 1 atunci când (de exemplu) C2 = 4/3
farad, R4 = 1 ohm, L1 = 3/2 henry și L3 = 1/2 henry.

(Fig. 9 - Filtru trece-jos de ordinul 3 format dintr-un circuit RLC)

29
Aproximarea Butterworth la un filtru ideal trece jos se bazează pe presupunerea că un
răspuns plat la frecventa zero este mai important decât răspunsul la alte frecvente. O funcţie de
transfer normalizată este de tipul numai cu poli şi care are rădăcini ale polinomului de la numitor
care sunt situate toate un cerc unitar în planul complex. Atenuarea este de 3 dB la frecvenţa de 1
rad/s.
Erorile de aproximare şi ordinul de complexitate al funcţiei aproximante reprezintă
indicii de calitate ai procesului de aproximare, fiind de dorit ca acestea să fie cât mai mici.
Intervalul de aproximare reprezintă mulţimea punctelor de pe axa frecventelor fizice 𝜔 pentru care
se realizează aproximarea funcţiei de transfer ideale. În cazul general acest interval coincide cu
axa frecventelor. Uneori acesta se reduce doar la banda de trecere sau la cea de blocare. Funcţie
de repartiţia erorii în intervalul de aproximare exista mai multe criterii de aproximare. Criteriul
aplatizării maxime numit şi criteriul maximplat determina parametrii funcţiei aproximate prin
dezvoltarea erorii 𝜀𝐻(𝜔) în serie Taylor în jurul unui punct 𝜔0 .

30
Capitolul 4
Recuperarea mişcării folosind senzori inerţiali

Unitățile de măsură inerțială(IMU) sunt compuse din accelerometre 3D și giroscoape 3D


și uneori senzori magnetici 3D. Acești senzori pot detecta mișcarea chiar și în cazurile în care
encoderele nu pot furniza corect datele. IMU-urile sunt folosite pentru a calcula viteza, poziția și
atitudinea vehiculelor, navelor și avioanelor. Pentru a se obține viteza semnalul de la accelerometru
este integrat, iar pentru a afla poziția accelerația este dublu integrată. Prin integrarea datelor de la
giroscop se obține unghiul de ruliu, unghiul de tangaj și orientarea. Semnalele primite de la IMU
conțin erori care se acumulează în urma integrării datelor. Din acest motiv IMU reprezintă un
senzor bun pentru perioade de timp scurte, dar acumulează erori din ce în ce mai mari dacă timpul
de funcționare este mai lung. Aceste erori trebuie identificate și corectate pentru a obține date
corecte. Corectarea datelor de la IMU se realizează utilizând filtrul Kalman.
Un IMU oferă date precise despre atitudine și poziție pentru vehicule doar în cazul
perioadelor scurte de timp, în cazul intervalelor de timp lungi integrarea micilor erori ale IMU
conduc la erori mari. Soluția pentru o funcționare corectă este identificarea și eliminarea erorilor
care afectează senzorul. O metodă pentru identificarea erorilor este metoda Allan.
Metoda Allan (AVAR ) este o tehnică de analiză în domeniul timp, care a fost inițial folosită pentru
analiza stabilității oscilatoarelor, dar apoi a fost folosită pentru analiza senzorilor inerțiali. Formula
acestei metode este:
1 1
𝜎𝑦2 (𝜏) = 2 (𝑦̅ 𝑛+1 − 𝑦̅ 𝑛 ) = 2𝜏2 (𝑥𝑛+2 − 2𝑥𝑛+1 + 𝑥𝑛 )2 (3.2)

Această metodă presupune că incertidudinea este cauzată de diferite caracteristici ale


componentelor zgomotului. Covarianța fiecărei componente este evaluată folosind datele din
analiza Allan .

31
4.1 Recuperarea Orientării

Elementul cel mai important în recuperarea orientării este cuaternionul. Cuaternionul


este un element complex format dintrun scalar şi un vector cu ajutorul căruia se poate calcula
rotaţia unui obiect şi orientarea acestuia.
Matricea Cuaternionilor este notată cu H (Hamilton, numele celui care a pus bazele calculelor cu
Cuaternioni). Ei sunt formaţi dintr-o parte scalara s ∈ ℝ şi o parte vectorială v =(x,y,z) ∈ ℝ3
Elementele au formă: i2=j2=k2 =ijk = -1, ij=k s ji=-k atunci q ∈ H poate fi scrisă ca:
q ≡ [s,v] , s ∈ ℝ, v ∈ ℝ3
≡ [s,(x,y,z)], s,x,y,z ∈ ℝ
≡ s+ix+jy+kz, s,x,y,z ∈ ℝ (3.3)

Produsul încrucişat al vectorilor defineşte reprezentarea axelor şi unghiurilor, şi oferă o


orientare în spaţiu: spaţiul vectorial în 3 dimensiuni. Cei trei vectori ai ecuaţiei a x b = c vor forma
o orientare în spaţiul vectorial. În formă cuaternionica alegerea orientării în spaţiu corespunde cu
înmulţirea ij=k dar ji= -k ceea ce înseamnă o orientare inversată atunci formula devine p -> q-1 , q
fiind înlocuit cu cuaternionul conjugat, dar va rezulta acelaşi comportament în axele vectorilor.
Reprezentarea rotaţiei sub formă de cuaternioni (4 numere) e mai compactă decât
reprezentarea ca o matrice ortogonală (9 numere). În plus pentru o axă şi un unghi dat, se poate
construi foarte uşor cuaternionul corespunzător dar şi invers, pentru un cuaternion dat, se pot
determina axele şi unghiurile, lucru care este mult mai greu cu matrici sau unghiuri Euler.

(Fig. 10 - Imaginea unui Cuaternion într-un cadru de 6DoF)

32
În figură 10 avem un exemplu de Cuaternion folosit pentru căzut 6DoF (6 Degrees of
Freedom) sau 6 grade de libertate unde se poate analiza mişcarea unui obiect pe cele 3 axe de
rotaţii pe care le putem considera că mişcări de stânga-dreapta, sus-jos, sau pe diagonală. Cu
ajutorul calcului pentru rotatiile matriciale se poate stabili o orientare, care, adaugata la setul
datelor de pozitie liniara va duce la crearea unui traseu 3D.

(Fig. 11 - UCS în mediul 3D de Matlab)

În figură 11 orientarea se recuperează cu ajutorul unui sistem de coordonate în matlab într-


un grafic 3d, axa albastră pe Z, cea verde pe Y şi cea roşie pe X. În mişcările înregistrate de senzor
vom putea observa orientarea cu ajutorul acestor coordonate. Proprietăţile Cuaternionilor sunt
folositoare în reprezentarea orientării corpurilor sau coordonatelor intrun spaţiu tri-dimensional.
Cuaternionii folosiţi pentru reprezentarea rotaţiei includ şi axele şi unghiurile acestora.

33
(Fig. 12 - Exemplu de orientare arbitrară a Cuaterninilor)

Figura 12 este un exemplu de reprezentare a cuaternionilor. O rotaţie arbitrară de la cadrul


B la cadrul A poate fi reprezentată ca o rotaţie de unghi 0 în jurul axei r în cadrul A. Cuaternionul
𝐴
𝐵𝑞 descrie o rotaţie:

𝐴
𝐵𝑞 =(q0 q1 q2 q3)
=[cos /2 x (sin /2)r]
=(cos /2 -rxsin/2 -rysin/2 -rzsin/2) (3.4)

rx,ry,rz reprezintă componentele vectorului r în axele X,Y,Z ale cadrului A.

34
4.2 Recuperarea Poziţiei

Pentru recuperarea poziţiei unui obiect, semnalul acceleraţiei a(t)=(ax(t),ay(t),az(t))T


obţinut de la accelerometru este proiecta în cadrul referinţei:
𝑎𝑔 (𝑡) = 𝐶(𝑡)𝑎(𝑡) (3.5)
Acceleraţia rămasă după ce sustragem gravitaţia, este integrată din nou pentru a obţine viteza şi
din nou o integrare pentru a obţine deplasarea:

𝑡
𝑣𝑔 (𝑡) = 𝑣𝑔 (0) + ∫0 𝑎𝑔 (𝑡) − 𝑔𝑔 𝑑𝑡 (3.6)
𝑡
𝑠𝑔 (𝑡) = 𝑠𝑔 (0) + ∫0 𝑣𝑔 (𝑡) 𝑑𝑡 (3.7)

Unde avem 𝑣𝑔 (0)viteza iniţială a obiectului, 𝑠𝑔 (0) deplasarea iniţială şi 𝑔𝑔 este

Acceleraţia în concordanţă cu gravitaţia referinţei.


Urmărirea poziţiei este bazată pe măsurătorile pure ale acceleraţiei, dar poate fi şi o
problemă. Pentru a avea rezultate exacte ale poziţiei, acceleraţia liniara (datele de la accelerometru
minus gravitaţia) trebuie integrată de două ori, cum a fost precizat şi mai sus, o integrare pentru
viteză şi o integrare pentru poziţie. Dacă există chiar şi o minimă interferenţă pe datele uneia dintre
axele urmărite, valorile poziţiei rezultate pot avea mult zgomot (drift).
Deşi este posibil să mărim performanţele acestor informaţii poziţionale prin fuziunea
senzorului cu sisteme externe de referinţă (sisteme optice, barometrice etc.) în majoritatea
cazurilor tot va fi nevoie pentru calculul poziţiei utilizând datele acceleraţiei liniare.

35
4.3 Recuperarea posturilor 3D

În recuperarea posturilor 3D cea mai folosită metodă este urmărirea mişcărilor 6DoF – 6
Grade de libertate. Aceasta metodă se referă la numarul specific de axe pe care un corp rigid este
liber sa se miste. Defineste numarul independent de parametri ce definesc configuratia unui sistem
mecanic. Specific, corpul se poate misca in 3 dimensiuni, axele X, Y, Z cat si schimbarile de
orientare.

(Fig. 13 - Cele 6 grade de libertate (6DoF) )

În figură 13 avem prezentate cele 6 grade de libertate: sus/jos, stanga/dreapta,


înainte/înapoi si schimbarile de orientare: rotaţia(axa longitudinală), giraţia(axa normală) şi
pasul(axa laterală).
Folosim aceste grade de libertate pentru a stabili mediul 3D în care urmărim obiectul şi
mişcările acestuia, calculate anterior cu orientare şi mişcare.Iar cu toate aceste informaţii adunate,
în mediul 3D se va putea reconstitui un traseu sau o mişcare înregistrată cu senzorul IMU – LPMS-
B.

36
Capitolul 5
Rezultate Experimentale
Au fost efectuate experimente cu senzorul IMU atât cât pentru simple mişcări de poziţie
orientare, traseu sau urcatul unor scări cât şi trasee cu un automobil înregistrând date pentru două
cazuri de condus, condus prudent şi cu o viteză mică şi condus agresiv cu o viteză mare şi mişcări
bruşte.
Toate experimentele înregistrate au avut ca scop recuperarea traseelor şi vizualizarea
diferenţelor de nivel, acceleraţie sau rotaţie a purtătorului de senzor. Vizualizarea pas cu pas a
acestor experimente s-a făcut într-un mediu 3D de animaţie al Matlabului unde mişcările
înregistrate sunt recuperate şi reprezentate pas cu pas de la origine până la punctul de final.

5.1 Utilizarea IMU în recuperarea mişcării persoanelor

Unul dintre experimente a fost unul de tip bucla. Aici înregistrarea datelor s-a facut
utilizand senzorul în mâna unei persoane. Aceasta a alergat pe un culoar, unde apoi s-a întors brusc
pe lângă locul de unde a plecat şi a continuat direcţia câteva secunde după care din nou o mişcare
bruscă la dreapta şi apoi stânga pe nişte scări un nivel mai jos, revenind apoi la o distanţă apropiată
de cea a plecării într-un ritm lejer spre deosebire de cel alergător cu care pleacase.

În figura 14 avem traseul plotat în graficul 3d în Matlab unde se poate observa punctul de
plecare, diferenţele de viteză la alergare şi la mers normal, diferenţele de nivel pe axa Z. Graficul
este un tipar de 6 grade de libertate fiind format din cele 3 elemente X,Y,Z ale pozitiei liniare si
celelalte 3 elemente fiind matricea de rotatie rezultata din datele cuaternionilor, mai exact rotatia,
giratia si pasul.

37
(Fig. 14 - Mediul 3D al afişării datelor)

O explicaţie adiţională cu o legendă ataşată prezintă mai exact traseul parcurs în cadrul
acestui experiment este prezentată în figură 15. Se poate observa pozitia liniara in diferentele de
acceleratie cat si in cele de nivel cat si rotatiile date de cuaternioni in schimbarile de traseu si in
schimbarea traiectoriilor.

(Fig. 15 - Graficul 3D al experimentului de alergare)

38
Pentru a putea observa diferenţele de distanţă sau acceleraţie avem prezentat în figura 16
un simplu experiment cu senzorul ţinut simplu în mâna şi parcurgând o distanţă centimetrica în
dreapta respectiv în stânga. De asemenea se poate observa că mişcarea nu este liniara deoarece
parcurgând astfel de distanţe mici pentru dubla integrare în rezultarea poziţiei se vor aduna erori
pe care filtrul Butterworth nu le poate elimina complet.
Cea mai importantă diferenţa între cele două experimente este diferenţa spaţiului în care s-
au efectuat dar şi diferenţa de acceleraţie, unde se poate vedea în primul experiment pentru cazul
alergării punctele din grafic au diferenţe semnificative între ele spre deosebire de cazul mersului
normal unde punctele sunt foarte apropiate.
Acest experiment cu senzorul în simple mişcări umane sau chiar mai complexe poate avea
multe întrebuinţări dar cel mai important domeniu ar fi cel medical unde se pot analiza multe
probleme cu ajutorul unui astfel de senzor. Un astfel de sistem cu giroscop poate fi de asemenenea
benefic pentru ajutorul obiectelor folosite de persoane ce sufera de boala Parkinson.

(Fig. 16 - Graficul experimentului simplu)

39
Pentru a putea observa diferenţele de distanţă sau acceleraţie avem prezentat în figura 16
un simplu experiment cu senzorul ţinut simplu în mâna şi parcurgând o distanţă centimetrica în
dreapta respectiv în stânga. De asemenea se poate observa că mişcarea nu este liniara deoarece
parcurgând astfel de distanţe mici pentru dubla integrare în rezultarea poziţiei se vor aduna erori
pe care filtrul Butterworth nu le poate elimina complet.
Cea mai importantă diferenţa între cele două experimente este diferenţa spaţiului în care s-
au efectuat dar şi diferenţa de acceleraţie, unde se poate vedea în primul experiment pentru cazul
alergării punctele din grafic au diferenţe semnificative între ele spre deosebire de cazul mersului
normal unde punctele sunt foarte apropiate. Acest experiment cu senzorul în simple mişcări umane
sau chiar mai complexe poate avea multe întrebuinţări dar cel mai important domeniu ar fi cel
medical unde se pot analiza multe probleme cu ajutorul unui astfel de senzor.

40
5.2 Utilizarea IMU în recuperarea sistemelor automotive

Acest experiment a fost efectuat punând senzorul într-un automobil acesta parcurgând o
distanţă relativ mică, în două cazuri diferite, primul fiind un mers încet şi simplu, iar în al2lea caz
un mers agresiv cu acceleraţii bruşte şi variaţii de direcţie.
În fig. 17 avem prezentat traseul efectuat cu maşina pentru primul experiment. Punctul de
plecare fiind o parcare din fata Facultaţii de Automatică si Calculatoare, iar traseul parcurs pe
străzile din faţa acesteia. Experimentul fiind de tip buclă, punctul de intoarcere este acelasi cu cel
de plecare.

(Fig. 17 - Harta (Google Maps) a sectorului utilizat pentru experimentul automotive)

După cum putem vedea în datele recuperate în mediul 3d traseul este neted fără variaţii pe
axa Z, iar cele mai semnificative diferenţe de distanţa se pot vedea pe axa Y. Având în vedere
acumularea erorilor în calcularea acceleraţiei liniare, poziţia finală este puţin perturbată. Acest
zgomot (drift) se acumulează în timp şi produce erori de la experiment la experiment.

41
În figură 18 avem şi graficul acestui experiment unde se observă că mişcarea este destul de
netedă şi liniara. Traseul parcurs are acelasi punct de pornire si de stop. Graficul este neted iar
curbele parcurse sunt rotunde deoarece viteza a fost mică, de aceea aspectul este unul destul de
rotund comparat cu fig. 17, unde străzile sunt pătratice.

(Fig. 18 - Graficul 3D pentru experimentul automotive)

Spre deosebire de experimentul anterior în recuperarea mişcării persoanelor, filtrarea


Butter orth nu a fost atât de fină, datele înregistrate de senzor fiind destul de liniare şi cu foarte
mici variaţii pe axa Z. Datele acceleraţiilor şi giroscopului având valori mai ridicate au fost mai
uşor de netezit spre deosebire de distanţe centimetrice şi variaţii mici şi dese ca în experimentul
anterior.

42
După efectuarea experimentului final, însemnând parcurgerea aceluiaşi traseu dar cu
acceleraţii şi orientări bruşte, variate şi dinamice, se pot observa diferenţele majore ale
înregistrărilor în fig. 19 spre deosebire de figura 18 unde automobilul a fost condus calm şi cu o
viteză destul de redusă.

(Fig. 19 - Graficul 3D pentru experimentul automotive agresiv)

Aceste experimente în sistemele automotive au ca scop formarea unei idei pentru


detectarea condusului agresiv, lucru ce prezintă o problemă majoră în zilele noastre. În figura 20
este prezentată arhitectura pentru detectarea condusului. Putem observa că folosind datele
senzorului IMU, împreună cu câteva alte date se poate crea un tipar inovativ de detectare a stilulilor
de condus.

43
(Fig. 20 - Arhitectura Detectării condusului)

Întreg sistemul de identificare a mişcării poate fi acompaniat şi de sistemul GPS iar


multitudinea acestor informaţii poate fi filtrată şi clasificată pentru reducerea accidentelor. Iar
folosind o logică şi un set de reguli Fuzzy o identificare exactă ar fi posibilă.

44
Capitolul 6
Concluzii

Scopul lucrării a constat in studiul, implementarea şi testarea metodelor de recuperare a


mişcării tridimensionale folosind informatii de la senzori inerţiali. Aceste informaţii au fost
alcătuite din vectorii datelor inregistrate de senzorul inertial (IMU) LPMS-B prin comunicatia
acestuia cu calculatorul prin interfaţa OpenMat.
Parametrizarea cinematică a miscării a fost constituită din două entitaţi: cuaternionii
(pentru orientare) şi vectorii de poziţie. Cuaternionii au fost transformaţi intr-o matrice de rotatie
R folosind produsul Hamilton, iar poziţia a fost obtinuta prin dubla integrare a vectorilor
acceleraţiei.
O etapă foarte importantă in utilizarea datelor de la senzorii inerţiali este etapa de filtrare.
Au fost studiate şi implementate doua metode de filtrare: Kalman şi Butterworth. Filtrul Kalman
a fost utilizat pentru estimarea şi predictia poziţiilor iar filtrul Butterworth a fost utilizat pentru
netezirea datelor vectoriale inregistrate, dar şi pentru prelucrarea frecvenţelor unora dintre ele.
Aplicaţiile dezvoltate de aceasta lucrare au fost constituite in 2 categorii: recuperarea
mişcării persoanelor şi recuperarea miscării automobilelor. Primul experiment al recuperarii
mişcării persoanelor a fost unul de tip bucla, unde persoana avand senzorul in mană, a alergat
dintr-un punct A pe un coridor, apoi pe niste scări şi s-a intors in punctul A. Al2lea experiment al
recuperării mişcării persoanelor a fost mai simplu, adica mişcarea senzorului cu mana in dreapta
şi apoi stanga pentru a observa miscarea. Experimentul recuperării miscării automobilelor a fost
compus din parcurgerea unui traseu simplu (tip dreptunghi), tip buclă si acesta cu plecarea dintr-
un punct şi intoarcerea in acelasi loc. Au fost inregisrate doua parcurgeri al acestui traseu, diferenţa
majora intre ele fiind stilul condusului, mai exact primul traseu a fost parcurs calm şi cu o viteza
redusă in timp ce al2lea experiment a fost agresiv cu acceleratii mari şi opriri bruşte. In cazul
acestui ultim experiment, diferenţele in traseul recuperat au fost foarte vizibile.
Directiile viitoare vor consta in utilizarea acestor date spre stabilirea unui tipar de detectare
a stilului de condus (agresiv sau nu). Ideea de bază este să extragem aceste informaţii din senzorii
de mişcare, incluzând poziţia, orientarea, viteza şi apoi o clasificare a diferitelor stiluri de condus
acompaniate de mesaje de avertizare.

45
Bibliografie:

1. Goebl, W.; Palmer, C. (2013). Balasubramaniam, Ramesh, ed. "Temporal Control and
Hand Movement Efficiency in Skilled Music Performance".
2. David Noonan, Peter Mountney, Daniel Elson, Ara Darzi and Guang-Zhong Yang
(2010). A Stereoscopic Fibroscope for Camera Motion and 3D Depth Recovery During
Minimally Invasive Surgery.
3. NY Castings, Joe Gatt, (2014) Motion Capture Actors: Body Movement Tells the Story.
4. Andrew Harris Salomon, Feb. 22, 2013, Backstage Magazine, Growth In Performance
Capture Helping Gaming Actors Weather Slump.
5. LPMS User’s Guide 1.3.4
6. R. Colin Johnson (2013) - GPS system with IMUs tracks first responders.
7. A. D. KING - Inertial Navigation – Forty Years of Evolution.
8. Richard Markell 1994 “Better than Bessel” Linear Phase Filters for Data
Communications.
9. Paul Zarchan; Howard Musoff (2000). Fundamentals of Kalman Filtering: A Practical
Approach. American Institute of Aeronautics and Astronautics, Incorporated.
10. Kalman, R. E. (1960). "A New Approach to Linear Filtering and Prediction
Problems". Journal of Basic Engineering.
11. Gaylor, David; Lightsey, E. Glenn (2003). "GPS/INS Kalman Filter Design for
Spacecraft Operating in the Proximity of International Space Station". AIAA Guidance,
Navigation, and Control Conference and Exhibit
12. Stratonovich, R. L. (1959). On the theory of optimal non-linear filtering of random
functions. Theory of Probability and its Applications, 4, pp. 223–225.
13. Greg Welch, Gary Bishop, 2001,An introduction to Kalman Filter University of North
California at Chapel Hill,
14. Lindsay Kleeman, , 2011 Understanding and Aplzing Kalman Filtering, Departament of
Electrical and Computer Systems Engineering, Monash University.
15. Bogdan Dumitrescu, Prelucrarea Semnalelor, Capitolul 4 Proiectarea filtrelor
16. T.Delyiannis, Yichuang Sun, J.K.Fidler, 1999,”Continous-Time Active Filter Design”,
CRC Press.

46
17. S. Butterworth, vol. 7, 1930, In Wireless Engineer (also called Experimental Wireless
and the Wireless Engineer), pp. 536–541 - "On the Theory of Filter Amplifiers"-
18. Giovanni Bianchi and Roberto Sorrentino (2007). Electronic filter simulation & design.
19. Steven T. Karis, 2007 „Signals and Systems with MATLAB Computing and SIMULINK
Modeling”,Orchard Publications, ,
20. D. Titterton and J. Weston, 2004 Strapdown Inertial Navigation Technology. The
American Institute of Aeronautics and Astronautics, second edition,.
21. Craig, John J. 1986, Introduction to Robotics: Mechanics and Control, Addison-Wesley,
22. Miroslav D. Lutovac, Dejan V. Tošić, Brian Lawrence Evans, Filter Design for Signal
Processing Using MATLAB and Mathematica
23. B. A. Shenoi, Introduction to Digital Signal Processing and Filter Design, John Wiley &
Sons
24. L. D. Paarmann, Design and Analysis of Analog Filters: A Signal Processing Perspective,
Springer

47
Anexe
clear;
close all;
clc;
addpath('ximu_matlab_library'); % includem libraria x-IMU MATLAB
addpath('quaternion_library'); % includem libraria cuaternion

acc=csvread('skoda2.csv',1,3,[1 3 3675 5]); %citire date acceleratie


gyro=csvread('skoda2.csv',1,6,[1 6 3675 8]); %citire date giroscop
mag=csvread('skoda2.csv',1,9,[1 9 3675 11]); %citire date magnetice
euler=csvread('skoda2.csv',1,12,[1 12 3675 14]); %citire unghi Euler
quat=csvread('skoda2.csv',1,15,[1 15 3675 18]); %citire cuatenioni
linAcc=csvread('skoda2.csv',1,19,[1 19 3675 21]); %citire acceleratie
liniara
samplePeriod = 0.005;
%facc=convacc(acc,'G''s','m/s^2'); %conversie acceleratie

figure('NumberTitle', 'off', 'Name', 'Accelerometru'); %Grafic Accelerometru


AccY=acc(:,2);
AccZ=acc(:,3);
hold on;
plot(AccX, 'r');
plot(AccY, 'g');
plot(AccZ, 'b');
xlabel('sample');
ylabel('g');
title('Accelerometru');
legend('X', 'Y', 'Z');

smAccX= smooth (acc(:,1),'moving');


smAccY= smooth (acc(:,2),'moving');
smAccZ= smooth (acc(:,3),'moving');
figure('NumberTitle','off','Name','SmoothAcc') %Netezire Accelerometru
hold on;
plot(smAccX,'r');
plot(smAccY,'g');
plot(smAccZ,'b');
xlabel('sample');
ylabel('m/s^2');
title('SmoothAcc');
legend('X', 'Y', 'Z');

smAcc=horzcat(smAccX,smAccY,smAccZ); %Concatenare in matrice

gyr=deg2rad(gyro); %Conversie grade in radiani

figure('NumberTitle', 'off', 'Name', 'Giroscop'); %Grafic Giroscop


GyroX=gyro(:,1);
GyroY=gyro(:,2);
GyroZ=gyro(:,3);
hold on;

48
plot(GyroX, 'r');
plot(GyroY, 'g');
plot(GyroZ, 'b');
xlabel('sample');
ylabel('deg');
title('Giroscop');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', 'QuatData');


QuatW=quat(:,1);
QuatX=quat(:,2);
QuatY=quat(:,3);
QuatZ=quat(:,4);
hold on;
plot(QuatW, 'm');
plot(QuatX, 'r');
plot(QuatY, 'g');
plot(QuatZ, 'b');
xlabel('sample');
ylabel('dps');
title('QuatData');
legend('W', 'X', 'Y', 'Z');

q=quat;
R=quat2rotm(q); %Includerea cuaternionilor in matricea de rotatie

figure('NumberTitle', 'off', 'Name', 'Date Magnetice'); %Grafic Magnetometru


MagX=mag(:,1);
MagY=mag(:,2);
MagZ=mag(:,3);
hold on;
plot(MagX, 'r');
plot(MagY, 'g');
plot(MagZ, 'b');
xlabel('sample');
ylabel('uT');
title('Date Magnetice');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', 'Unghiuri Euler'); %Grafic unghiuri


Euler
EulerX=euler(:,1);
EulerY=euler(:,2);
EulerZ=euler(:,3);
hold on;
plot(EulerX, 'r');
plot(EulerY, 'g');
plot(EulerZ, 'b');
xlabel('sample');
ylabel('deg');
title('Unghiuri Euler');
legend('X', 'Y', 'Z');

49
figure('NumberTitle', 'off', 'Name', 'Graficul Datelor Acceleratiei
Liniare'); %Grafic Acceleratie liniara
LinAccX=linAcc(:,1);
LinAccY=linAcc(:,2);
LinAccZ=linAcc(:,3);
hold on;
plot(LinAccX, 'r');
plot(LinAccY, 'g');
plot(LinAccZ, 'b');
xlabel('sample');
ylabel('m/s^2');
title('Graficul Datelor Acceleratiei Liniare');
legend('X', 'Y', 'Z');

smLinAccX= smooth (LinAccX,'moving');


smLinAccY= smooth (LinAccY,'moving');
smLinAccZ= smooth (LinAccZ,'moving');
figure('NumberTitle','off','Name','SmoothLinAcc') %Netezire Acceleratie
Liniara
hold on;
plot(smLinAccX,'r');
plot(smLinAccY,'g');
plot(smLinAccZ,'b');
xlabel('sample');
ylabel('m/s^2');
title('SmoothLinearAcc');
legend('X', 'Y', 'Z');

smlinAcc=horzcat(smLinAccX,smLinAccY,smLinAccZ); %Concatenare
linVel = zeros(size(linAcc)); %Integrare acceleratie liniara pentru obtinere
viteza liniara
for i = 2:length(linAcc)
linVel(i,:) = linVel(i-1,:) + linAcc(i,:) * samplePeriod;
end

% Plot
figure('NumberTitle', 'off', 'Name', 'Graficul Datelor Vitezei Liniare');
%Grafic Viteza liniara
hold on;
plot(linVel(:,1), 'r');
plot(linVel(:,2), 'g');
plot(linVel(:,3), 'b');
xlabel('sample');
ylabel('g');
title('Graficul Datelor Vitezei Liniare');
legend('X', 'Y', 'Z');

order = 2;
filtCutOff1 = 0.05;
[b, a] = butter(order, (2*filtCutOff1)/(1/samplePeriod), 'low');
linAccHP0 = filtfilt(b, a, linVel); %Filtru Butterworth trece-jos

figure('NumberTitle', 'off', 'Name', 'Filtrare Trece Jos - Viteza Liniara');


hold on;
plot(linAccHP0(:,1), 'r');

50
plot(linAccHP0(:,2), 'g');
plot(linAccHP0(:,3), 'b');
xlabel('sample');
ylabel('g');
title('Filtrare Trece Jos - Viteza Liniara');
legend('X', 'Y', 'Z');

order = 1;
filtCutOff2 = 0.005;
[b, a] = butter(order, (2*filtCutOff2)/(1/samplePeriod), 'high');
linVelHP = filtfilt(b, a, linVel); %Filtru Butterworth trece-sus

% Plot
figure('NumberTitle', 'off', 'Name', 'Filtrare Trece Sus - Viteza Liniara');
hold on;
plot(linVelHP(:,1), 'r');
plot(linVelHP(:,2), 'g');
plot(linVelHP(:,3), 'b');
xlabel('sample');
ylabel('g');
title('Filtrare Trece Sus - Viteza Liniara');
legend('X', 'Y', 'Z');

order = 2;
filtCutOff2 = [0.1 0.4];
[b, a] = butter(order, (2*filtCutOff2)/(1/samplePeriod), 'stop');
linVelSP = filtfilt(b, a, linVel); %Filtru Butterworth Stop-Banda

% Plot
figure('NumberTitle', 'off', 'Name', 'Filtrare Trece Banda - Viteza
Liniara');
hold on;
plot(linVelSP(:,1), 'r');
plot(linVelSP(:,2), 'g');
plot(linVelSP(:,3), 'b');
xlabel('sample');
ylabel('g');
title('Filtrare Trece Banda - Viteza Liniara');
legend('X', 'Y', 'Z');

smLinVelHPX= smooth (linVel(:,1),'rloess',7);


smLinVelHPY= smooth (linVel(:,2),'rloess',7);
smLinVelHPZ= smooth (linVel(:,3),'rloess',7);
figure('NumberTitle','off','Name','SmoothLinVel') %Netezire viteza liniara
hold on;
plot(smLinVelHPX,'r');
plot(smLinVelHPY,'g');
plot(smLinVelHPZ,'b');
xlabel('sample');
ylabel('m/s^2');
title('SmoothLinearVe');
legend('X', 'Y', 'Z');

51
smlinVelHP=horzcat(smLinVelHPX,smLinVelHPY,smLinVelHPZ);

linPos = zeros(size(linVel)); %Integrare viteza liniara pentru obtinerea


pozitiei liniare

for i = 2:length(linVel)
linPos(i,:) = linPos(i-1,:) + linVel(i,:) * samplePeriod;
end

% Plot
figure('NumberTitle', 'off', 'Name', 'Pozitie Liniara'); %Graficul
Pozitiei Liniare
hold on;
plot(linPos(:,1), 'r');
plot(linPos(:,2), 'g');
plot(linPos(:,3), 'b');
xlabel('sample');
ylabel('g');
title('Pozitie Liniara');
legend('X', 'Y', 'Z');

order = 3;
filtCutOff = 0.0125;
[b, a] = butter(order, (2*filtCutOff)/(1/samplePeriod), 'high');
linPosHP =filtfilt(b, a, linPos); %Filtru Butterworth trece-sus

% Plot
figure('NumberTitle', 'off', 'Name', 'High-pass filtered Linear Position');
hold on;
plot(linPosHP(:,1), 'r');
plot(linPosHP(:,2), 'g');
plot(linPosHP(:,3), 'b');
xlabel('sample');
ylabel('g');
title('High-pass filtered linear position');
legend('X', 'Y', 'Z');

%% Play animation

SamplePlotFreq = 15;

SixDOFanimation(linPosHP, R, ... %Folosirea pozitiei liniare si a matricei


de rotatie pentru obtinerea graficului 3D
'SamplePlotFreq', SamplePlotFreq, 'Trail', 'DotsOnly', ...
'Position', [9 39 1280 720], ...
'AxisLength', 0.1, 'ShowArrowHead', false, ...
'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)',
'ShowLegend', false, 'Title', 'Unfiltered',...
'CreateAVI', true, 'AVIfilenameEnum', false, 'AVIfps',
((1/samplePeriod) / SamplePlotFreq));

52