Navigation system for a micro-uav

Transcription

1 Nvigtion system for micro-uav JOHN-OLOF NILSSON Mster s Degree Project Stochom, Sweden 28 XR-EE-SB 28:1

2

3 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Astrct Recent progress in technoogy hs mde sm heicopter UAVs possie. A sm heicopter UAV hs numer of dvntges nd disdvntges in comprison to trdition rger UAVs. With crefu design it coud offer n inexpensive, porte, nd fexie UAV soution with some unique cpiities. he roe of UAV is often scouting. A simpe sis for this ts coud e to provide n opertor with visu informtion. he visu informtion one is esiy interpreted y the opertor ut for utomtic interprettion of visu dt, s we s other sensor dt, some informtion out oction is essenti. Hence nvigtion system woud e desire for ny UAV ptform. In this study, n inexpensive GPS-ided inerti nvigtion system, from rediy vie off-the-shef components, is deveoped for n existing sm heicopter UAV ptform. he GPS to inerti mesurement integrtion is sed on Kmn fiter. he dt strems re synchronised, y softwre mens, to ech other nd to video dt from the on-ord cmer system. he nvigtion itsef (fitering nd integrtion of inerti nd GPS dt) is performed on se sttion to which dt is wireessy stremed. An intern cirtion procedure is performed nd the system is shown to hve n orienttion ccurcy of out ±1 nd position ccurcy of out ±2-5 metre (depending on direction). Smmnfttning (Swedish strct) Senre tids teniutvecing hr gjort små heiopter-uaver möjig. En iten heiopter UAV hr åde förder och ncder jämfört med trditione större fygpns- och heiopter-uaver. Med en omsorgsfu design sue emeertid en iten heiopter-uav unn ge en iig, ärr och fexie UAV-ösning med uni egensper. UAVer nvänds oft för reognoseringsuppdrg. Bsen för dett är oft idinformtion som everers ti nvändren. I fet med en mänsig nvändre tos ätt informtionen men för utomtis toning v idinformtion och toning v nnn mätdt är någon informtion om UAVns position och orientering nödvändig. Såedes är ett nvigeringssystem önsvärt för en UAV. I denn studie utvecs ett iigt GPS-orrigert tröghetsnvigeringssystem, v ätt tigängig omponenter, ti en efintig UAV-ptform. Integreringen v GPS-oservtioner med tröghetsmätningr ser med ett Kmnfiter. Dt strömmrn är mjuvrumässigt sinsemen synroniserde och även synroniserde med videodt från UAVn. Sjäv nvigeringen (fitrering och integrering v tröghets- och GPS mätdt) ser på en ssttion ti vien dt överförs trådöst Systemet irers med egn dt och viss ge en ritningsnoggrnnhet v ungefär ±1 smt positionen inom 2-5 meter (eroende på ritning). NAVIGAION SYSEM FOR A MICRO-UAV 3

4 SCHOOL OF ELECRICAL ENGINEERING - KH Prefce he study which is the sis of this report, s we s the report itsef, hs een performed s mster thesis project during the utumn semester of 27 t SAAB Bofors Dynmics (SBD), Gothenurg. he mster thesis course hs een given y the Sign Processing Institution t Kung enis Högson (KH), Stochom, where presenttion of the wor hs een given. A suject supervisor, Isc Sog, hs een suppied y the Sign Processing Institution nd two engineering supervisors, Per Lindquist nd Hns Åerund, hs een present t SBD. I woud ie to thn them for their enggement nd support. Much of the dt fitering hs een inspired y previous wor y Isc Sog, [2]. he min hrdwre components of the system hve een piced out in pre-study y Per Lindquist. he study hs een finnced y SBD. he hrdwre nd the softwre cquired nd deveoped during the study re the property of SBD. John-Oof Nisson Gothenurg 28 4 JOHN-OLOF NILSSON

5 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Reding guide o fciitte reding rief expntion of sections nd some reding dvice re given here. See the e of content for pge numer references. Sections he Introduction section gives rief cground to the report nd the im nd purpose of the study. 3 pges. he Hrdwre nd softwre in short section give short description of especiy the hrdwre nd riefy discusses the softwre. 5 pges. he Nvigtion dt cquisition nd fitering section gives onger description nd derivtion of the nvigtion fiter nd the different coordinte systems. 23 pges. he Video dt cquisition nd integrtion section gives short description of the video dt to nvigtion dt integrtion nd synchronistion. 2 pges he System cirtion section expins the proem nd soution of cirting the system without n extern reference mesurement. 4 pges. he System fied dt ehviour nd performnce section gives the reder n ide out the re word ehviour nd performnce of the nvigtion system. 8 pges he Discussion nd Concusion section sum up the resut nd discuss wht concusions cn e drwn. 2 pges. he Further wor section gives n ide out wht contiguous deveopment might e. 3 pges. Further informtion out the hrdwre nd other technic detis cn e found in the Appendix. Reding dvice For swift comprehension of the study nd summry of the resut the reder cn red through the Introduction, Discussion nd Further wor sections of tot 9 pges. his does not contin ny technic detis nd no mthemtics. If deeper understnding of different system components is desired, the corresponding sections wi give extensive expntions. he Hrdwre nd softwre in short section gives short overview of the hrdwre without going into technic detis. Py speci ttention to this section if the physic system is not vie. More detis cn e found in the Appendix. he Nvigtion dt cquisition nd fitering section gives n understnding of the fiter nd wht the softwre does. he description is rther mthemtic nd the reder shoud e fmiir with sic iner ger. If specific system performnce is of interest the Fied tests nd experiences section presents grphs nd numer from sever tests. A rough understnding of the fiter is dvntgeous in interpreting the grphs. NAVIGAION SYSEM FOR A MICRO-UAV 5

6 SCHOOL OF ELECRICAL ENGINEERING - KH e of contents ILE PAGE... 1 ABSRAC... 3 SAMMANFANING (SWEDISH ABSRAC)... 3 PREFACE... 4 READING GUIDE... 5 Sections... 5 Reding dvice... 5 ABLE OF CONEN... 6 INRODUCION Why micro UAV nd why nvigtion system? Purpose of study/report Method Deimittion HARDWARE AND SOFWARE IN SHOR UAV Sensors GPS receiver IMU Video cmer Dt ins Digit ins Anogue video in Bse sttion System ssemy Dt ogging softwre Dt processing softwre NAVIGAION DAA ACQUISIION AND FILERING Nottion Units Coordinte systems JOHN-OLOF NILSSON

7 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Sensor coordinte systems... 2 Sensor coordinte system GPS... 2 Sensor coordinte system IMU Sensor coordinte system Video Integrtion/computtion coordinte system Loc nvigtion coordinte system Common sensor-fixed coordinte system Inerti nvigtion IMU to ody coordinte system trnsformtion Stte vector Sensor input Initi dt trnsformtion Initi prmeters nd grvity Inerti nvigtion equtions GPS nvigtion Sensor input Set-up prmeters Conversion of GPS dt Error equtions Error sttes... 3 Error equtions Error definition Position error Veocity error Orienttion error Inerti mesurement ises ot error mode Kmn fiter Kmn fiter Seprtion of inerti mode nd error mode Prediction phse Up-dte phse Feed-c phse Dt synchronistion Computer time stmps IMU time stmps GPS time stmp Red synchronistion ime stmp is eimintion VIDEO DAA ACQUISIION AND INEGRAION Video dt to nvigtion dt integrtion Dt synchronistion SYSEM CALIBRAION Cirtion procedure Mesuring performnce Mnu supervision of cirtion Verifiction of nvigtion dt NAVIGAION SYSEM FOR A MICRO-UAV 7

8 SCHOOL OF ELECRICAL ENGINEERING - KH Limittions SYSEM FIELD DAA BEHAVIOUR AND PERFORMANCE 48 Gener fiter ehviour Convergence phse Ste phse System performnce Corrections nd imittion of corrections DISCUSSION CONCLUSION FURHER WORK Differenti GPS receiver Bse sttion differenti GPS Fu qurterion impementtion Re-time dt processing On-ord dt processing Computtion od chrcteristion Automtic contro Bc correction Evution of other nvigtion dt sources Fourier trnsformtion of imge... 6 Improved synchronistion of dt... 6 Improved dt ins... 6 Extended error mode... 6 Low cost IMU... 6 Compss... 6 REFERENCES APPENDIX Sensor system connection nd dt ins Dt ins Logic eve converters PCB Buetooth modue Power suppy nd contcts Setting up Buetooth modue Buetooth modue commnd Buetooth stc GPS receiver Setting up GPS receiver communiction IMU Setting up IMU communiction Sew symmetric mtrices nd sm nge rottion Cmer to IMU cirtion... 7 Approximte trnsformtion... 7 rnsformtion correction horizont pne... 7 rnsformtion correction yw JOHN-OLOF NILSSON

9 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Limittions Cirtion nd cirtion prmeters Error stte vrinces Position error vrince Veocity error vrince Orienttion error vrince Acceerometer is vrince Gyro is vrince Synchronistion time is Oservtion vrinces Antenn position Initi vrinces Initi position vrince Initi veocity vrince Initi orienttion vrince Initi is vrinces Other prmeters Initi eve djustment Limittions Other possie error sttes... 8 Grvity mode Dt ogging softwre User interfce Progrm code components MicroUAV.cpp FrmeGrer.cpp IMU.cpp GPS.cpp SeriPort.cpp FrmeProcessor.cpp Dt processing softwre Nvigtion softwre MicroUAV.m NvDt.m SWEREF992R9.m NvInitStte.m NvKmn.m NvPot.m NvVideo.m GrphicFix.m GrphicNv.m GrphicDigits.m GrphicLine.m Cirtion nd nysis softwre SystemCirtion.m ime stmpanyser.m Video2IMUCirtion.m PCB circuit digrm WGS84 to R9 Conversion WGS84 to SWEREF SWEREF to R Eipsoid prmeters NAVIGAION SYSEM FOR A MICRO-UAV 9

10 SCHOOL OF ELECRICAL ENGINEERING - KH Eipsoid heper prmeters R9 prmeters Coordintes Conform titude nd differenti ongitude... 9 Conversion... 9 Reverse conversion... 9 GPS drift test Asoute position Initi position drift Drift in position with time Drift in position with oction Known proems Communiction fiure Communiction fiure Communiction fiure Poor communiction rnge Video in mfunction Poor video quity Mfunctioning dt Extreme dynmics Dropped frmes Frme grer video input nd output settings JOHN-OLOF NILSSON

11 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Introduction Why micro UAV nd why nvigtion system? Recent progress in contro technoogy, ttery weight-to-cpcity rtio, nd eectronics hs mde sm fying unmnned eri vehices (UAV), in the form of sm heicopters, possie. his cn e seen in UAV product ines of compnies such s Bertin echnoogies, Dieh BG Defence, Nov DEM, Honeywe, MicroPiot, DrgnFy Inc, nd Microdrones Gmh. A sm UAV with the spti freedom of heicopter fight is wht is referred to with the term micro UAV in the tite. With the spti freedom of heicopter fight, sm heicopter UAV coud give n opertor very fexie remote eye. Such remote eye woud e very vue in numer of situtions: A rger re or route hs to e serched or oserved. he ird s-eye view, together with the spti freedom owing for distnt views s we s cose-up serch, woud in such sitution e very dvntgeous. An re or pce where there is security concern hs to e monitored or serched. A UAV woud in this cse eiminte the need for personne to enter the re. An re or pce impossie or difficut to rech hs to e exmined. Such pce coud e t uiding or rdio mst. In such sitution UAV woud grety fciitte the opertion. A nown spot or re hs to e routiney or upon request (typicy rm set-off) serched. A UAV coud in this scenrio me the surveince more efficient nd sve personne resources. A these situtions woud typicy pper in miitry or security opertions, especiy within n urn environment. A sm UAV coud e very fesie pproch to sove prts of such opertions. Indeed there re sever oth rger nd smer UAVs vie for such opertions. However, most currenty used UAVs re either rge heicopters (e.g. Sedr from S Aerotech), rge eropnes (e.g. Neuron) or sm eropnes (e.g. Syr from Eit System Ltd.). A these systems hve some intrinsic imittions though: Aeropnes hve the disdvntge of the imittions of spti freedom tht comes with wings. It cnnot hover nor me shrp turns nd it cn ony trve in one direction s seen from the crft. Lrger heicopters hve spti imittions due to their mere size nd even more ecuse of their cost. he viiity of very costy system out on the units woud e scrce. Aso n opertor might e reuctnt to send in scrcey vie system for cose-up serch in potentiy hzrdous re. A rger vehice wi hve imited portiity in the fied nd hence so imited fexiiity in the fied. he over-the-hi reconnissnce is not prctic from distnt irfied. he rger size hs cost s regrds steth. NAVIGAION SYSEM FOR A MICRO-UAV 11

12 SCHOOL OF ELECRICAL ENGINEERING - KH he size nd the type of the vehice is very much resut of the equipment it needs to crry nd the opertion rnge needed. A rger vehice in gener cn crry more equipment nd winged vehice especiy cn crry more equipment, nd hs onger opertion rnge, ut with the cost of the imittions mentioned ove. In contrst sm heicopter UAV coud not crry s much equipment nd woud hve imited rnge, ut with crefu seection oth considering size, numer, nd price it coud sti offer considere functionity nd provide n inexpensive, porte, nd fexie soution with some unique cpiities. echnoogic progress is iey to continue to deveop, ming ight, etter nd ess expensive sensors possie. Aso the computtion power is iey to ecome ighter nd ess expensive nd commerci forces pushes the deveopment of sm nd efficient comustion nd eectric engines ming susequent upgrde, to onger opertion rnge, possie. With ddition sensors nd dvnced sign processing, s we s ddition equipment, it is esy to imgine tht sm UAV coud e much more thn just remote eye. With more sensors together with sign processing it coud wor s remote fuy fedged scouting mchine. With greter on-ord processing cpiities nd cever gorithms it coud evove to ecome fuy utonomous scouting mchine or with other ddition equipment it coud sove other tss thn just scouting. Ntury some sensors woud e more desire thn others. Visu informtion is proy the first tht comes to mind. It is esiy interpreted y n opertor nd provides the sic informtion out the oject of interest. Further thn tht, one cn imgine mny other sensors. However for most ny sensory dt to e use, even visu dt, s we s for n utomtic contro system, some informtion out oction nd orienttion must e nown. For visu dt, interpreted y humn opertor, the dt itsef cn provide rough position informtion, ut for most dt, interpreted nd processed utomticy, numeric position vue woud e desire. For imge processing, nd other orienttion-dependent informtion processing, direction informtion woud so e needed. Hence the second thing to dd to UAV ptform woud e nvigtion system. A sm heicopter UAV with video nd nvigtion system woud give the sic scouting functionity nd ptform for further deveopment. here re sever possie impementtions of nvigtion system. Recent progress in micro eectro-mechnic systems (MEMS) hs mde sm, inexpensive inerti mesurement units (IMU) vie. Athough sefcontined, hving high short-term ccurcy, nd high up-dte rte, pure inerti nvigtion system hs the imittion of unound error growth. A go positioning system (GPS) nvigtion system on the other hnd is not sef contined nd hs ow up-dte rte, ut ounded errors. hey thus hve compementry nture nd y comining them, GPS-ided inerti nvigtion system cn give n esiy impemente sef-contined nvigtion system with high up-dte rte nd good ccurcy.[6] he ese of impementtion, the rediy vie components, nd the possie ow weight me it ntur choice for simpe nvigtion system for sm heicopter UAV ptform. Purpose of study/report he rod im of this study ws to oo into the technic possiiities of sm UAV in the context of urn wrfre. his hs een nrrowed down to 12 JOHN-OLOF NILSSON

13 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB studying nd gthering experience out possie nvigtion of UAV nd integrtion of such dt with imge dt, to fciitte further imge processing. he im of this report is to ring together nd descrie the documenttion, resuts, nd experiences gined y this study to me further wor, which cn e crried out y others, rther thn the uthor, esier. Consequenty, the report is rther extensive nd detied. Method Within the frme of the study, simpe ow cost (f 27 ess thn 35$ excuding se sttion computer) nvigtion system incuding GPS receiver, n inerti mesurement unit (IMU), video cmer, rdio ins, se sttion, nd softwre hs een deveoped for n existing sm heicopter UAV ptform. Deveopment hs miny focused on the integrtion of off-the-shef components, nvigtion fiter impementtion, nd the processing of output dt. he nvigtion system hs een cirted nd tested nd its performnce hs een studied. An engineering pproch hs een ten for the deveopment. No extensive theoretic verifiction of the vidity of the nvigtion modes, error modes, fiter nd cirtion hs een done. he correct (verified) output of the system hs een ten s verifiction of the vidity of the system itsef. Deimittion he study hs not incuded ny deveopment or modifiction of the UAV ptform. Neither hs ny imge processing een done, except for videofrme-to-nvigtion-dt synchronistion nd the integrtion of nvigtion dt into video frmes. NAVIGAION SYSEM FOR A MICRO-UAV 13

14 SCHOOL OF ELECRICAL ENGINEERING - KH Hrdwre nd softwre in short Further technic informtion, detis nd set-up procedure cn e found in the Appendix. Product documenttion cn e found t compny homepges. An iustrtion of the fu system cn e seen in the end of the section in Figure 6. UAV he UAV, used s ptform for the nvigtion system, is the DF-SAVS from DrgnFy Innovtions Inc. Cnd. he DF-SAVS cn e seen in Figure 1. he DF-SAVS is ttery powered, four-rotor, symmetric heicopter nd cn therefore, depending on piot sis, e fown in ny direction. Its crrying cpcity is out 85g nd opertion time out 1 minutes with the nvigtion system on-ord. Figure 1. Picture of the UAV ptform used in the study. he dimension of the UAV is (tip of rotor) 8x8x15 mm nd the weight 54 g. Sensors GPS receiver he GPS receiver is n EM-411 from Go St, iwn, (seen in Figure 2), n inexpensive off-the-shef GPS-receiver with SiRF str III chip set. he receiver uses 3-wire Seri interfce with L eve ogics. o me the communictions s hrdwre independent s possie, the GPS receiver hs een set to use NMEA-183 protoco for communiction. Specificy the system uses the GGA output messge with 1Hz (mximum). 14 JOHN-OLOF NILSSON

15 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Figure 2. Picture of the EM-411 GPS modue. he modue is 3x3x1 mm nd weight 16g. IMU he IMU is 3DM-GX2 (without mgnetometer) from MicroStrin Inc, US. he IMU is of MEMS type. his mens inexpensive, very compct nd ight weight ut ow performnce nd with short error-corretion time. he IMU use 3-wire RS232 connection for extern communiction. he IMU cn e seen with nd without the csing in Figure 3. Figure 3. Picture of the 3DM-GX2 IMU with nd without the csing. For system ssemy the csing hs een ten wy. he IMU is 41x44x24 mm with csing nd 32x36x15 mm without csing. he IMU weight 16 g without csing. Video cmer he video cmer is n SC-P63L compct coour CCD chip cmer from Senech Americ Inc (seen in Figure 4). he cmer hs PAL composite video output nd fuy utomtic white nce. It uses 4mm ens with mximum horizont fied of view of 62 nd vertic view of NAVIGAION SYSEM FOR A MICRO-UAV 15

16 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 4. Picture of the SC-P63L chip cmer. he cmer PCB is 32x32 mm nd the weight 12 g. Dt ins Digit ins For digit wireess communiction stndrd Buetooth communiction is used. he Buetooth ins re primriy used s seri ce repcements. Within the frmewor of Windows they re piced up s COM-ojects within the ogging progrm just s ny device connected to seri port. he on-ord Buetooth modues re two BISM II PA modues from Ezurio (seen in Figure 5), one for ech sensor. he modues re of css 1 with trnsmission power of 65mW. he modues use L eve ogic for communictions. For Buetooth communiction on the se sttion (ptop computer) side, stndrd Buetooth USB stic hs een used. he L eve ogic communiction of the Buetooth modues mde conversion of the RS232 eve signs of the IMU necessry. Proems with the od/source impednce of the connection GPS receiver to Buetooth modue connection so mde n mpifiction of the GPS sign necessry. his, s we s the surfce mounting of the Buetooth modue contct, hs een soved on sm printed circuit ord (PCB), with pir of MAX23E trnsceivers. Detis of the dt in set-up nd connection nd communiction with the sensors cn e found in Appendix. Figure 5. Picture of the BISM II PA Buetooth modue. he dimension of the modue is 23x34x8 mm nd the weight 8 g. Anogue video in o get roust trnsmission of the video sign it is trnsmitted s n nogue sign. he nogue video sign is sent vi 2.6 GHz 5mW 16 JOHN-OLOF NILSSON

17 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB video in. On the receiver side there is two porized ntenns for est possie reception. After decoding to nogue video sign crried in coxi ce, USB frme grer is used to cpture individu frmes for digit video sequence of 25Hz. Bse sttion For fied tests simpe windows sed ptop computer hs een used. Any windows sed PC computer with two USB ports nd decent performnce woud do. System ssemy he sensors, the video trnsmitter, nd the Buetooth modues re mounted on the UAV ptform nd the video in receiver vi the USB frme grer nd the Buetooth USB stic is connected to the se sttion computer. Further few sign eve converters re needed for the sensor-to-buetoothin communictions. he fu system ssemy cn e seen in Figure 6. Figure 6. Iustrtion of the fu system ssemy. Lines indicte physic connection nd yeow fshes indicte wireess connection. he sensors (cmer, IMU, nd GPS) re mounted on the UAV ptform ut prt from the power suppy they re not connected to the ptform in ny ind of wy. Dt ogging softwre he dt ogging softwre hs een written in Microsoft Visu C++ (MSVC++). Astrctions of ech sensor run in seprte threds. For seri communiction, stndrd Windows system cs re utiised. As for video frme gring the DirectShow frmewor is used. Hence the progrm is restricted to Windows system. he seri dt strems re interpreted nd ny informtion necessry for further dt processing is written to ASCII text fies. Cptured video frmes re sved s seprte itmps. Auxiiry informtion (time stmps) to cptured video frmes is so written to n ASCII text fie. ime stmps re given from intern cocs of ech sensor ut dt rriv (dt red from input uffer) time stmps re so given from the computer for inter sensor dt integrtion. NAVIGAION SYSEM FOR A MICRO-UAV 17

18 SCHOOL OF ELECRICAL ENGINEERING - KH Dt processing softwre Remining dt processing hs een done in Mt. Essentiy there re four prts to the further dt processing. First of, dt hs to e imported nd pre-processed to form suite for integrtion. Second, the dt strems hve to e synchronised in time with the hep of the time stmps of the ogging softwre. herefter GPS nd IMU dt is integrted with Kmn fiter resuting in nvigtion informtion. Finy the nvigtion informtion is dded to the cptured video frmes providing video output. Video frmes re so to spe stmped with nvigtion informtion. 18 JOHN-OLOF NILSSON

19 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Nvigtion dt cquisition nd fitering Nottion Vectors wi e written with ower cse od etters s x nd mtrices with upper cse od etters s. he coordinte system in which vectors nd mtrices re given in wi e designted y hyper script s foow: oc (right hnded R9), f ritrry Erth fix, s sensor, nd ody. Sm nge rottions nd other sew symmetric mtrices wi e designted x where x is the vector from which the sew symmetric mtrix is formed. Further informtion out sm nge rottion nd sew symmetric mtrices cn e found in the Appendix. rnsformtion or retions etween coordinte systems wi e designted y super script nd su script indicting to wht coordinte system nd to from wht coordinte system it refers to, from. For scr components of rottion suscript wi indicte which xis it refers to. he suscript designtes the index of the dt. Further ~ designtes mesured vue nd ^ designtes estimted vues respectivey. I nd wi indicte 3x3 identity mtrix nd 3x3 zero mtrix respectivey. If other mtrix sizes re used it wi e expicity written. he importnt equtions wi e numered nd referred to with od numer in prenthesis, e.g. (1). Units A dt is given in SI units. Rdins re used in the fiter. However, if nges re given in the report they wi in gener e given in degrees which wi e indicted y degree sign,. GPS dt is given in the NMEA183 stndrd ut wi e treted s if it gve position informtion in R9. Coordinte systems A sensor dt (Inerti mesurements, GPS, nd video) re dependent on spti position nd/or spti orienttion. he mesurements re, so to spe, done in sensor coordinte system, which depend on the position nd the orienttion of the sensor. In order to comine these dt nd for processing in common fiter, they wi hve to e trnsformed into other suite coordinte systems. Mthemticy, trnsformtion from one coordinte system to nother is determined y trnsformtion mtrix nd trnstion vector. Assuming coordinte systems to e Crtesin nd orthogon puts sustnti constrints on the trnsformtion mtrix, of which the determinnt must e equ to one. he choice of coordinte systems, other thn the oc sensor coordinte systems, is free even though some wi e mthemticy more convenient nd more intuitive to user. here re numerous things to consider. First of, the coordinte system of choice ought to gree with the diy perception of directions: Depending on the use of the coordinte system, the directions of up, down, right, eft, nd forwrd nd cwrd, or positions nd the NAVIGAION SYSEM FOR A MICRO-UAV 19

20 SCHOOL OF ELECRICAL ENGINEERING - KH directions of, up, down, nd the four crdin points ought to e esiy identifie. Further, since there wi e dt processing over time, it wi e convenient if some of the time derivtives of these coordinte systems re zero. In other words, it woud e convenient to wor in inerti systems for the simpicity of the motion equtions. his is normy not possie since the Erth nd the sor system is rotting. However, from diy ife experience it is ovious tht the effect of the Erth s rottion is sm for moderte dynmics over shorter distnces. From sic mechnics one finds tht the reevnt quntity is the rottion mutipied y the veocity, so referred to s the Coriois force. Judging from imgine dynmics of the system it cn sfey e negected. Lsty, there re stndrds of coordinte systems to consider. For exmpe mps hve certin coordinte systems nd, in fight dynmics, specific coordinte systems re normy used. Sensor coordinte systems In the system, we hve three coordinte systems, introduced with the dt from ech sensor. Figure 7. Iustrtion of the WGS84 coordinte system. WGS84 is the wenown coordinte system used to descrie oction on Erth in ongitude, titude nd titude. Sensor coordinte system GPS Word geodetic System 84 (WGS84) is the coordinte system of the Go Positioning System (GPS). his system is wht we woud normy refer to when speing of ongitude, titude, nd titude. he coordinte system, iustrted in Figure 7, ought to e fmiir to most peope. WGS84 is quite different from the other coordinte systems in tht it is neither Crtesin nor orthogon. his woud normy resut in compicted nvigtion mode. Wht is done to overcome this proem is, s wi e seen ter, to me oc trnsformtion/pproximtion into Crtesin nd orthogon system, R9.[3] he coordinte provided y the GPS receiver is given t the position of the ntenn which is moving in the coordinte system. he sensor is therefore not fixed in its own coordinte system. However the sensor is fixed in retion to the other sensor coordinte systems. 2 JOHN-OLOF NILSSON

21 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Figure 8. Iustrtion of the sensor coordinte system of the IMU. he coordinte system is Crtesin nd orthogon nd fixed to the physic sensor. Note tht the unit shown on the picture consist of oth the IMU nd the cmer, which re mounted together. Sensor coordinte system IMU he sensor coordinte system in which the inerti mesurements re done is fixed to the physic sensors. We wi not dig deeper into the pcement of the individu sensors within the sensor pcge ut ssume they re pced in singe point nd perfecty orthogony igned resuting in right hnd orthogon Crtesin coordinte system centred in the IMU (iustrted in Figure 8). Since the coordinte system is fixed to the sensor unit, its retions to the other sensors re so fixed, ut wi depend on the mounting of the sensor. Figure 9. Iustrtion of the video cmer coordinte system. he system ony orrows the directions from the imge ut prt from tht the imge is not descried in the coordinte system. NAVIGAION SYSEM FOR A MICRO-UAV 21

22 SCHOOL OF ELECRICAL ENGINEERING - KH Sensor coordinte system Video he coordinte system of the video dt strem is reted to the foc point of the system, presume t the photo sensor rry, nd the orienttion of the rry. Once gin the coordinte system is fixed in retion to the other sensor coordinte systems. his coordinte system does not descrie the pixes of the imge ut rther provides direction informtion out the imge, ming pssive integrtion with the rest of the dt possie. At this point we cn thin of the coordinte system s just n ritrry orthogon Crtesin coordinte system which hs one xis pointing to the centre of the cmer imge nd the two others spnning the picture pne igned with the vertic nd horizont directions in the imge. Iustrtion of this cn e seen in Figure 9. Integrtion/computtion coordinte system For integrtion of dt we need common coordinte system. he sensors re fixed in retion to ech other prt from the sm motions in the ntivirtion cmer rm. o descrie sensory dt we use common sensorfixed system. o descrie the nvigtion prmeters of the system we wi use n Erth-fixed oc Crtesin coordinte system. Figure 1. he R9 coordinte system is oc Crtesin nd orthogon pproximtion of the go WGS84 coordinte system. he R9 coordinte system is used for nvigtion. Loc nvigtion coordinte system As noted ove, the WGS84 coordinte system hs the drwc of neither eing Crtesin nor orthogon. Using the WGS84 for inerti nvigtion woud grety compicte the nvigtion mode. Fortuntey, since this system hs very imited opertion rnge s compred to the size of Erth the devitions from orthogonity wi e negigie. However WGS84 wi not sce eveny in different directions prt from t the equtor nd we need dt in metric units, rther thn degrees, in order to integrte it with other dt. Further, re mps might hve WGS84 informtion incuded ut they wi e drwn using other oc coordinte systems. Since we currenty operte in Sweden this eds us to the oc metric Crtesin coordinte 22 JOHN-OLOF NILSSON

23 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB system used for Swedish mps, R9. Any other oc metric orthogon coordinte coud e used with equtions unchnged. Detis of the R9 coordinte system s we s of the conversion etween the WGS84 nd the R9 coordinte system cn e found in the ppendix. R9 is somewht speci coordinte system since it is eft hnd system with the x-xis pointing north, the y-xis est, nd the z-xis pointing up. his is mthemticy cumersome. A simpe wy of overcoming this proem is to simpy turn it into right-hnd system. Hence we hve to either interchnge two xes or to chnge the direction of one of them. wo things suggest we chnge the direction of the z-xis. First of most of our mp dt wi e using the x- nd the y-xis nd therefore we do not wnt to chnge them. Secondy, our ody coordinte system hs the z-xis pointing down. If we chnge the direction of the z-xis in the R9 system, vectors given in one coordinte system cn esiy nd intuitivey e converted to the other. Chnging the direction of the z-xis resut in right-hnd oc orthogon metric system which cn suity e used for nvigtion. We ony hve to rememer tht the titude informtion wi e pointing downwrds nd thus pose sign error to our intuitive directions. his right-hnd R9 coordinte system wi e referred to s the oc coordinte system. Figure 11. Iustrtion of the video cmer coordinte system nd the IMU coordinte system. he xes of the coordinte system is cose to pre. When the ody coordinte system is creted the fix direction of the cmer coordinte system together with eve in the imge is used. Further, the sm distnce, etween the coordinte system origins, is ignored nd the ssumed common origin is ten s the origin for the ody coordinte system, see Figure 12. Common sensor-fixed coordinte system he two sensors with oc sensor-fixed coordinte systems, video nd IMU, re mounted in cose proximity to ech other. he sensory dt of the GPS cn esiy e trnsted; the sensory dt of the IMU cn esiy e rotted; whie the video dt is difficut to trnste. Further the video coordinte system is in our cse igned with the sensor ptform, in rediy detecte wy, giving us perception of direction in dt. he video dt is so used NAVIGAION SYSEM FOR A MICRO-UAV 23

24 SCHOOL OF ELECRICAL ENGINEERING - KH for direction cirtion nd thence, the ntur choice of common coordinte system is the video cmer coordinte system with suite rottion, even though the video dt is not ctivey used. Since this common coordinte system is the ody-fixed coordinte system used for nvigtion, it wi e ced the ody coordinte system. he coordinte systems re iustrted together in Figure 11 nd the fin ody coordinte system is iustrted in Figure 12. Now there re sever different wys y which this coordinte system cn e chosen. his is ecuse video dt does not hve n intrinsic coordinte system ut ony intrinsic direction. he orienttion of the nvigtion system wi e descried s the retion etween the ody coordinte system nd the oc nvigtion coordinte system. For intuitive dt presenttion, we wnt to descrie this orienttion with stndrd fight dynmics terminoogy, ro, pitch, nd yw. his gives the stndrd fight dynmics coordinte system with the x-xis forwrd, the y-xis strord, nd the z-xis down. his gives ro, pitch, nd yw s positive rottion in the x-, y-, nd z-xes respectivey. his is iustrted in Figure 12. Figure 12. Iustrtion of the ody coordinte system nd the directions nd rottions of fight dynmics. Inerti nvigtion In inerti nvigtion the nvigtion sttes re estimted y integrtion of inerti mesurements. It gives good short term ccurcy ut sm errors wi ccumute giving rise to nvigtion stte errors which swifty grow without ound, ming it unreie for nvigtion over onger periods.[1,3] Further inerti nvigtion does not fee soute position or veocity. Hence the system is ony cpe of giving sttes retive to the initi stte which hs to e externy set.[1] hereof we t out estimtes nd stte estimtion. 24 JOHN-OLOF NILSSON

25 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB IMU to ody coordinte system trnsformtion Since the IMU nd the cmer re mounted in cose proximity to ech other, the trnsformtion from the IMU coordinte system to the ody coordinte system is pproximted with fixed-rottion. he cmer nd the IMU re mounted (see iustrtion) so tht rough trnsformtion is given y: IMU o this we dd sm (cose to identity mtrix) rottion in the form of cirtion mtrix, d. IMU IMU d d wi hve to e chnged if the mounting of the IMU, in retion to the cmer, is chnged. For procedure see Appendix. Stte vector he prmeters tht determine our inerti nvigtion mode is position x nd veocity v given in the oc coordinte system nd the orienttion θ given in etween oc nd the ody coordinte system. hese prmeters wi form the stte vector z. θ v x z where z y x x w v u v z y x ψ φ θ θ We wi ter see tht the orienttion in form of ro (θ ), pitch ( φ ), nd yw/heding ( ψ ) is, though good for presenttion, somewht cumersome to wor with. Hence in ddition to them there wi so e rottion mtrix representing the sme informtion.,, θ his rottion mtrix wi e used in ccution whie the orienttion vector wi ony e used for dt representtion. Sensor input he input we hve in cse of IMU dt is n cceertion nd n ngur rte ω vector given in the sensor coordinte system of the IMU. hese inputs wi form the input vector u. s s s, ω u where s z s y s x s s z s y s x s s ω ω ω, ω NAVIGAION SYSEM FOR A MICRO-UAV 25

26 SCHOOL OF ELECRICAL ENGINEERING - KH Since we re using strp down system the grvittion cceertion g wi e incuded in our cceertion mesurement. As for the ngur rte the Erth s rottion is incuded ut due to the imited ccurcy of the sensor it wi e negigie nd wi not e compensted for. ogether with every dt smpe, time stmp t is given. From djcent time stmps, t nd t + 1, time difference etween dt smpes, dt t +1 t, is ccuted. Initi dt trnsformtion For intuitive dt presenttion nd simpicity, we wnt to express prmeters nd dt in either R9 coordintes or ody coordintes. Hence input dt wi e trnsformed into ody coordintes with the set-up dependent IMU-sensor-to-ody-coordinte rottion., see ove: ω IMU IMU IMU, IMUω IMU, Initi prmeters nd grvity As noted ove the inerti nvigtion system cn ony give sttes retive to the initi sttes, ut hve no wy of determining them. However with some resone ssumptions few things cn e found. he system is initiy ssumed to e resting. he initi position wi e set to some pproximte vue (First GPS oservtion) in the oc coordinte system. v x x y z he initi resting system does not experience ny true cceertion ut grvittion cceertion ought to e the soe contriution to our cceertion mesurement. he grvity is ssumed to e nown excty nd is given y grvity mode. grvity mode g Grvity eing nown nd the ony contriutor to the cceertion mesurement mens tht we cn find eve (ro (θ ), pitch ( φ )). he yw/heding ( ψ ) wi hve to e externy set though. Leve nd heding provide n initi orienttion nd rottion mtrix. θ,,, For detis of grvity nd initi djustment of eve, see Appendix. Inerti nvigtion equtions Approximting the oc coordinte system, to e sttionry (inerti), the sic equtions for position nd veocity of inerti nvigtion cn esiy, ignoring the Coriois terms, e derived from sic mechnics nd the ssumption of sm nge rottion. he equtions wi simpy e given here. 26 JOHN-OLOF NILSSON

27 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB x& v v & g (1) &, Rememer tht the grvity g wi e component of the mesured cceertion since the whoe oc coordinte frme is suject to grvittion cceertion. For the equtions (1) to e usefu in n impementtion they need to e discretized. For the two first equtions this cn esiy e done y writing them in stte spce form. With given initi vue t time t the soution t time t+dt, ssuming nd constnt over time dt, cn e written s [2] x v I I dt I x v 1 I + 2 I dt 2 ( dt ) [ g ] he rottion mtrix is somewht tricier. Anyticy it cn e soved s t () t e, where, is n initi vue. Assuming, to e, constnt over dt yieds, (2), dt + 1,. Expnding the exponenti,, e nd ignoring higher order term the orthogonity of wi e compromised over sever updtes. his is due to the sm nge pproximtion which gives sight distortion from orthogonity. o ensure orthogonity second order Pdé pproximtion cn e used to pproximte the mtrix exponenti.[2] ( 2I + dt )( I dt ) 1, + 1,, 2, Sti, imited numeric ccurcy might compromise the orthogonity of over ong time periods (mny updtes). If necessry this cn e soved with occsion renormiztions. GPS nvigtion GPS nvigtion hs very different chrcteristics from inerti nvigtion. he GPS receiver gives position tht cn e directy reted to the nvigtion stte, n oservtion. his oservtion of the nvigtion stte hs poor short term ccurcy (there is noise to every oservtion), the nvigtion sttes re not fuy oserve from GPS dt from singe GPS receiver (e.g. system orienttion cnnot e derived from GPS oservtions), ut the error is ounded over time.[1,3] Note tht there is difference etween n eropne nd heicopter in the sense of oserviity of orienttion from GPS dt. An eropne wi prefery move in one ody direction which wi e tngent to the motion trjectory. Aso the other directions re, vi the fight dynmic, sustntiy restrined y the trjectory. his sitution is drsticy different for heicopter nd especiy the symmetric UAV ptform. Even though the orienttion is somewht restrined y the trjectory, the fct tht the heicopter cn move in ny ody direction, mes it impossie to deduce the orienttion from GPS oservtions. (3) NAVIGAION SYSEM FOR A MICRO-UAV 27

28 SCHOOL OF ELECRICAL ENGINEERING - KH No direct GPS nvigtion wi e used in the fin system. Hence no such fu frmewor is discussed here. Ony reted dt nd dt conversions re treted. Sensor input he GPS receiver gives position in the form of WSG84 coordintes. hese wi e converted to the coordintes of the oc coordinte system (righthnd R9). For more informtion see Coordinte system section nd the Appendix. Here the GPS receiver wi e treted s if it gve positions in the oc coordinte system. he GPS receiver gives position x of the GPS ntenn. x GPS x y z Set-up prmeters he GPS receiver gives position of the GPS ntenn ut wht we wnt is position of the ody coordinte system origin. For this we need the position of the GPS ntenn x in the ody coordinte system which is given y the set-up. x ntenn x y z ntenn ntenn ntenn Figure 13. Iustrtion of the retion of the ody coordinte system nd the GPS receiver. Conversion of GPS dt he GPS dt give oction of point (GPS ntenn) tht is fixed in the ody coordinte system ut the ody coordinte system is not fixed in the 28 JOHN-OLOF NILSSON

29 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB R9 coordinte system. he position of the GPS ntenn, in the ody coordinte system, is nown from the system set-up, nd, in R9 coordinte system, from GPS input dt. he retions re iustrted in Figure 13. With the nowedge of the orienttion of the ody system in the nvigtion coordinte system we cn trnsform the GPS dt to the origin of the ody coordinte system. Given vector x GPS ntenn representing the position of the ntenn in the ody coordinte system nd rottion vector from ody to oc coordinte system we wi get the GPS position t the origin of the ody system s: x x x GPS ntenn + GPS ntenn However is not oserve from GPS dt. With ony GPS dt we woud e stuc here nd the tter prt woud just dd to the uncertinty of the true nvigtion stte oservtion. Error equtions he GPS receiver nd the inerti nvigtion system provide n oservtion nd n estimtion respectivey of the current position. Aprt from tht, the inerti nvigtion system so provides estimtes for the remining nvigtion sttes. It is esy to imgine how we coud use the GPS position oservtion to improve our inerti nvigtion position estimtion. From the difference in error chrcteristics nd the two positions we coud estimte n error in our position estimtion. his is iustrted in Figure 14. However the estimted error in position woud so e reted to the errors in the remining nvigtion sttes. A mode for how the errors of the nvigtion sttes rete to ech other woud et us use the oservtion of the position together with the position estimtion to estimte errors for nvigtion sttes.[1] Furthermore the IMU wi hve systemtic errors which wi contriute to the estimted sttes nd hence so to the estimted errors. hese errors of MEMS sensors hve short corretion times.[4] hus we wnt to incude them in the mode too. he error mode eow is derived essentiy in ine with the error mode of reference [2]. NAVIGAION SYSEM FOR A MICRO-UAV 29

30 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 14. Correction of position estimte with position error mode nd GPS oservtion. Assume there is no error in the orienttion ut tht the ony error contriution is the noise in cceertion mesurement nd GPS oservtions. A simpe correction procedure coud then oo ie: 1. Strt in point given y GPS oservtion (red point). 2. Estimte position y integrting cceerometer mesurements (ue ine). 3. As new GPS oservtion rrives there wi e sm discrepncy in etween the inerti estimtion nd the oservtion. 4. From the error chrcteristics nd the error mode of the inerti estimtion nd the GPS oservtion we cn estimte proiity distriution (green) for the position. 5. Correct the position in ccordnce with the proiity distriution. 6. Continue estimting position. Error sttes For ech nvigtion stte there wi e corresponding error stte. x x y z v u v θ w θ x φ y ψ z 3 JOHN-OLOF NILSSON

31 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB In ddition to this there wi e n error stte corresponding to the systemtic errors in mesurement dt of the IMU. z y x z y x ω ω ω ω hese error sttes wi form tot error vector e. ω θ v x e Error equtions From the inerti nvigtion section we hve n eqution system (1) descriing the retions mong nvigtion sttes. hese equtions hod true for oth our estimted sttes nd our true sttes. (4), g v v x & & &, ~ ˆ ~ ˆ ˆ ˆ ˆ ˆ g v v x & & & (5) (6) Error definition We define the errors s the sm perturtions which dd to the estimted or mesured vue to get the true vue. For the rottion mtrix we use sm nge rottion of ngur errors. (7) ( ) ( ),,,,,, ~ ~ ~ ˆ ˆ ˆ ˆ ˆ ˆ ω ω ω I I v v v v v v x x x x x x θ θ θ & & & & & & & & & (8) (9) (1) (11) Position error Insertion of (7) into the first inerti nvigtion eqution, (4), gives the trivi resut. v x v v x x v x & & & & ˆ ˆ (12) NAVIGAION SYSEM FOR A MICRO-UAV 31

32 SCHOOL OF ELECRICAL ENGINEERING - KH 32 JOHN-OLOF NILSSON Veocity error For the second eqution, (5), inserting (8), expnding nd cnceing true terms yieds ( ) ( ) ( ) ( ) v g v v g I v v g v v θ θ θ & & & & & & & ~ ˆ ˆ However we do not hve ccess to ut ony ˆ. Agin using the sm nge rottion we cn rotte c to ˆ s ( ) I ˆ θ +. Note tht this is the opposite of wht we hd efore, ( ) I θ ˆ, in (9). ( θ I + ) is the inverse of ( θ I ) ignoring higher order terms of the individu rottions. Using this, inserting the mesured cceertion, (1), nd expnding yieds ( ) ( ) ( ) ( ) & & & 2 ˆ ~ ˆ ˆ ~ ˆ ˆ ~ ˆ v I I v v θ θ θ θ θ θ θ he two st terms re of second order in error nd cn e discrded. Writing ~ ˆ θ s ˆ θ the definition of the sew symmetric mtrix gives θ θ θ θ ˆ ˆ ˆ ˆ. hus we rrive t the resut θ v ˆ ˆ + & (13) Orienttion error In simir mnner, inserting (9) nd (11), expnding, nd cnceing true terms, in the third eqution, (6), give ( ) ( ),,,,, ˆ ˆ I I θ ε θ θ θ + & & & & & As erier, inserting ( ) I ˆ θ + nd ignoring higher order terms, yieds:

33 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB ( ) ( ) z z z z z z I I ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ,,,,,, ε ε ε ε & & & 4243 & & & In more convenient vector form, the resuting eqution reds [2] ω θ ˆ & (14) Inerti mesurement ises Finy we ssume the systemtic errors to e independent from the other stted nd the time vritions re descried y sm rndom noise, w. (15) ω w ω w & & (16) ot error mode he tot error eqution system of the error mode is given y eqution (12-16). + ω w w ω θ v x I ω θ v x e ˆ ˆ ˆ & & & & & & Discretizing the eqution in the sme wy s the nvigtion equtions we rrive t [2] w e E w e I I I I I I e dt dt dt dt,,, 1 ˆ ˆ ˆ (17) Kmn fiter Being suppied with n estimtion of the nvigtion stte from the inerti nvigtion system nd n oservtion of the position from the GPS receiver we wnt wy of finding n optim updte of our estimtion given the error chrcteristics of the estimtion nd the oservtion. Ming the ssumption out inerity, normy distriuted white noise, nd out few ess NAVIGAION SYSEM FOR A MICRO-UAV 33

34 SCHOOL OF ELECRICAL ENGINEERING - KH importnt detis, the Kmn fiter cn e shown to provide the optim updte.[3] Unfortuntey our mode is not competey iner. he mode is differentie though nd ming oc iner ssumption, the prti derivtives cn e used to derive n extended Kmn fiter for our mode.[2] However, with the sme oc iner ssumption nd sm tric more intuitive derivtion of norm Kmn fiter of our mode cn e done, yieding the sme resut s the extended Kmn fiter. his pproch hs the dvntge of the Kmn fiter derivtion not eing dependent on the specific error nd nvigtion mode. Bsed on the error mode we wi extend the simpe correction procedure given in Figure 9 to incude other nvigtion sttes, not just position. In short the fiter wi essentiy contin predict phse in which the inerti mesurements re used to predict nvigtion sttes, nd up-dte phse in which the GPS oservtions re used to estimte the errors which rise in the predict phse, nd sty feed-c phse in which the estimted errors re fed c into the nvigtion sttes. Kmn fiter he mening of system stte, index nd system input of this section is not the sme s tht of the impemented fiter in ter sections. his section is ony short review of iner Kmn fiter nd is y no mens exhustive. A compete description of Kmn fiters cn e found in stndrd suject reference iterture.[1] he Kmn fiter ssumes system mode of the form: x x +1 F + u w (18) Where x is the system stte, F is the stte trnsition mode, u is some contro/input prmeters, B is the contro/input mode, nd w is some normy distriuted noise dded to the system. Further there re stte oservtions s: y H x + v (19) Where y is the prti oservtion of the system sttes s descried y the oservtion mtrix, H, nd noise dded to the oservtion v. he fiter first tries to predict the system trjectory with the system mode ignoring the unnown noise (since the noise is white, the est we cn do is to ssume it to e zero). Upon oservtion it uses the discrepncy of the oservtion nd the predicted stte nd error chrcteristics (noise chrcteristics) to updte the stte. he predict phse then oos ie: xˆ x ˆ + 1 F u ~ P F P F + Q + 1 (2) (21) 34 JOHN-OLOF NILSSON

35 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Where P is the predicted stte covrince mtrix of system sttes x nd Q is the covrince chrcteristics of the extern input w. he covrince of the vrie itsef wi e the vrince nd hence we find the vrinces in the digons of the mtrices. he updte phse wi then oo ie. K P H ( HP H + R) (22) x P, new, new x P + K K ( z Hx ) HP (23) (24) R is the covrince chrcteristics of the stte oservtions, v. An updte gin is ccuted from the predicted covrince for every stte in P. he updte of the sttes is performed from the oservtion z, nd the covrince is updted. Note tht these steps re done with different frequencies so K nd z wi ony exist for some vues of n. Seprtion of inerti mode nd error mode he tot system we wnt to estimte is the nvigtion sttes nd the error sttes. If we ssume we re ony moving sm distnce round n ngur vue the ngur updte cn e inerized owing us to wor directy with the Euer nges. hen from (18) nd (2) the inerti nvigtion system coud e written s. z + 1 Where F F z u (25) is given y directy descretized version of (1). ogether with the error mode, (17), the tot system woud e written s: z e F E z u e he oservtion mde y the GPS woud e x x + x + v GPS. his fu system is rther cumersome to use since the error terms s we s the nvigtion sttes wi grow without ound even if their sum woud e finite. However, the error sttes in position, veocity nd orienttion cn e fed into the corresponding nvigtion sttes nd inerti mesurements nd set to zero. z z + e, new,1:9 e, new,1:9 9x 1 nd his cn e done every time GPS oservtion rrives ( for which GPS oservtion exists) updting e,1:9 to non-zero vue. On the other hnd the is vues, e,1:15, cnnot e set to zero ut wi hve to e continuousy u fed into the mesurement/input vries,. NAVIGAION SYSEM FOR A MICRO-UAV 35

36 SCHOOL OF ELECRICAL ENGINEERING - KH u u + e for every., new,1:15 his provides ony zeros or non-chnging vues in the error mode in the predict phse. his ets us seprte the error mode from the predict phse y dding feed-c phse which performs ddition of the error to the nvigtion sttes nd incuding the continuous feedc into the predict phse. Furthermore, compring the estimted position nd the GPS oservtion, the GPS oservtion cn e treted s n oservtion of the x insted of x. his wi e mthemticy equivent since x nd x re dependent on x x + x + v ech other s GPS. Hence n oservtion of x wi give the sme informtion s n oservtion of x, however, impying tht x is fed c to x. his wi et us seprte the inerti nvigtion mode from the updte step. he error sttes ony hve to e fed into the nvigtion sttes efore entering the prediction fce. Wht we hve done essentiy is to eiminte rge chuns of zeros nd res mutipied y zeros from the tot system mtrices, y ccepting the error feed-c into the nvigtion sttes. Note tht the tot system is iner even though some of the nvigtion sttes pper in the E mtrix. his is ecuse these sttes re not dependent on ny of the error sttes. here re ony cross terms so the system is sti inery dependent on vries independenty, yet not inery dependent on comintions of them. Hence the stte spce coud e trnsformed giving truy iner mode in which the fitering coud e done fter which the sttes coud e trnsformed c giving the origin sttes. he seprtion is fortunte in the sense tht we do not hve to do this. Prediction phse Now tht we hve dodged the stte trnsformtion into true iner form we wi so ignore the ineriztion of the orienttion tretment in the inerti nvigtion mode, (25). hus we write the prediction s function given y (2) nd (3), nd not s iner retion. First we form the corrected mesurement vector from the corrected inerti mesurement: u u + e, new,1: 15 he inerti nvigtion stte estimtion cn then e written s: z + 1 F( z, u ) Where the function F( ) is given y (2) nd (3). Regrding the error prt of the mode, there wi e no prediction since the mode e + 1 E e + w together with the feed-c ( e,1:9 ) provide zero or non-chnging vues for which is then so true for the fiter except for updte instnces. However, the covrinces of the true sttes need to e predicted. Let P e the covrince mtrix of the error sttes t instnce, then the covrince in (21) wi e predicted s: P E P E + Q JOHN-OLOF NILSSON

37 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Here Q is the covrince of the noise of the error mode. Q cn e estimted from the input signs nd then tuned to optimise the ehviour of the fiter. Hence the fin vue of Q wi e suject to fiter cirtion. P is determined from E nd Q given n initi vue of P. his initi vue is mesurement of how much we trust our initi error sttes e. Assuming the initi error vues nd the noise of the error sttes to e independent, Q nd the initi vue of P wi ony hve digon eements. Up-dte phse he updte phse wi e concerned with the error sttes. First, the updte gin (so ced Kmn gin), (22), is ccuted: K P H ( HP H + R) Here R is the covrince of the noise ssocited with the GPS position oservtion. Just s Q, R cn e estimted from the oservtions. However, the fin vue is suject to fiter cirtion. K cn e seen s mesurement of trust of the GPS oservtion. Next the updte of the error stte is done from the GPS position oservtion: e, new e + K, GPS (( x x ) He ) his oos sighty different from wht we might expect. Wht is done is tht the position oservtion is trnsformed into n oservtion of the position error since idey xgps x x. he oservtion mtrix hs to pic out the position error x nd wi therefore oo ie: H [ I ] he position error x though is set to zero in the feed-c phse, so the term He wi so e zero nd the updte, (23), finy reds: e ( x x ), new e + K, GPS Finy the covrinces of the error sttes, (24), re updted: P, new P K Feed-c phse HP Before we return to the prediction phse the error sttes hve to e fed c to the inerti nvigtion sttes. his is done once for the position x, the veocity v, nd the orienttion θ, ut continuousy for the input vues cceertion nd ngur veocity ω, which is integrted into the prediction phse. he sttic nture of the feedc of the corresponding sttes to the nvigtion stte sets them to zero whie the continuous nture of the input vector feed-c does not: z e, new,1:9,1:9 z 9x1,1:9 + e,1:9 NAVIGAION SYSEM FOR A MICRO-UAV 37

38 SCHOOL OF ELECRICAL ENGINEERING - KH Dt synchronistion he fiter ove ssumes perfecty set time stmps of dt nd does not te into ccount the short time period etween the st inerti mesurement efore GPS oservtion. A dt time synchronistion error gives rise to incorrect stte djustments since the GPS (stte) oservtion wi e compred to n estimtion of coming or pst time. he impct of the timing error wi e dependent on the dynmics of the system. Simpe tris show tht if the error is sm compred to the time of significnt stte chnges the impct wi e ow whie, if the error is compre to such time, the fiter wi hve ow performnce. No theoretic tretment of synchronistion error hs een crried out. Computer time stmps he ogging progrm wi suppy computer time stmp s it reds incoming dt. his time stmping hs poor performnce due to intrinsic ow performnce of system coc. his is iustrted in Figure 15. With nown chrcteristics of dt, the ccurcy cn e estimted to e within few hundredths of second. However, the IMU dt rrives t 1Hz which mens tht few hundredths of second is quite significnt. Figure 15. Iustrtion of computer time stmps of IMU dt. Bue represent true mesurement time nd red the computer time stmp. he figure is ony for iustrtion nd not representtive of re dt. IMU time stmps he IMU hs n intern coc which strts up upon power-up. his is n exct mesurement of the coc cyce, during which the dt were mesured. It is cimed to hve n ccurcy of.1%. ests hve shown tht the time discrepncy etween the IMU nd the computer coc is ess thn few hundredths of second over period of sever minutes. he IMU time stmps re very regur. Compre the computer time stmps in Figure 15 with the sensor time stmps in Figure 16. Figure 16. Iustrtion of sensor time stmps. Both the IMU nd GPS time stmps hve regur intervs. GPS time stmp he GPS dt come with time stmp sed on the steite cocs. his time stmp is n soute time nd it is very ccurte. However, time 38 JOHN-OLOF NILSSON

39 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB discrepncy (dey) in the order of seconds hs een noted in comprison to the cpture time. his is proy due to ong computtion time in the GPS receiver. Unfortuntey the soute time is of itte use since the computer time stmp time is not directy rete to it. he GPS time stmps re, just s the IMU time stmps, very regur. his is iustrted in Figure 16. Figure 17. Iustrtion of the retion etween (computer) time stmps of GPS nd IMU dt. he GPS time stmps hve very ong (.8s) hrdwre dey nd their ow frequency (1Hz) mes the softwre dey devition negigie. Red synchronistion he time stmps of the computer re too errtic/irregur to use s time stmps within the fiter. In ddition, the time mesurements of the sensor time stmps re not directy rete to ech other, or to the computer time mesurements. However the computer time stmps of the different sensors cn e directy reted to ech other s seen in Figure 17. In Figure 18 there is n iustrtion of the components of computer time stmp. Knowing the frequency of dt nd verging over sever smpes, the point indicted with dotted red ine cn e found. Its retion to the true mesurement time wi e unnown ut fixed. Setting the dotted red ine to e the origin of the intern sensor cocs' time stmps, their retion wi so e unnown ut fixed. his is iustrted in Figure 19. he resuting synchronistion is iustrted in Figure 2. NAVIGAION SYSEM FOR A MICRO-UAV 39

40 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 18. Components of computer time stmp. he distnce etween the ue dotted ine nd the red dotted ine wi e fixed. his cn e used for synchronistion of sensor times. Figure 19. Setting the origin of sensor time stmps with the first computer time stmp nd the ccuted verge softwre dey. Bue - true mesurement time, red computer timestmp, green intern sensor coc time stmps. Aso, see Figure JOHN-OLOF NILSSON

41 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Figure 2. Resuting red synchronistion. Bue - true mesurement time, red computer timestmp, green intern sensor coc time stmps. ime stmp is eimintion Even though fixed retionship etween the two sensor time stmps hs een estished, their retion to the true mesurement time is not nown. his is not needed though, since for now there is no feedc from the system. he constnt retive time error/time stmp is needs to e found for the fiter to wor correcty. Lter on the synchronistion etween intern sensor cocs wi e done t strt-up. he hrdwre dey originting from intern sensor ccution is ssumed to e constnt s we s the verge softwre dey for specific se sttion computer. he Buetooth in hrdwre dey might vry with reception. However, the reevnt dey is tht occurring from system strt-up nd coupe of seconds forth, when the red synchronistion is performed. Assuming we wys strt the system cose to the se sttion the time Buetooth in time dey cn e treted s constnt nd the fu retionship etween sensors' time stmps fixed. his mens tht the time error/time stmp is cn e treted s prt of the cirtion. he IMU computer synchronised time stmps hs een chosen s the fiter time. A fixed time set-off, corresponding to the est performnce, is sutrcted from the GPS time stmps. he resuting synchronistion is iustrted in Figure 21. See so the System Cirtion section, nd Figure 24. Figure 21. Synchronistion fter fix time error eimintion. NAVIGAION SYSEM FOR A MICRO-UAV 41

42 SCHOOL OF ELECRICAL ENGINEERING - KH Video dt cquisition nd integrtion Video frmes re coected nd sved s itmps t rte of 25Hz. here is no imge processing in the system. However, simpe processing such s deintercing nd rdi distortion correction coud e impemented without chnges in the rest of the system. Video dt to nvigtion dt integrtion For demonstrtion nd dt inspection, in time corresponding nvigtion dt is dded to the video frmes s sces going cross the imge, indicting ro, pitch nd yw. Position dt is potted on reference grid together with uxiiry informtion on the side of the video frme. his is ony for demonstrtion nd dt inspection of the couping of video frmes nd nvigtion dt. For further deveopment the importnt resut is the couping itsef. An exmpe of this integrtion cn e seen in Figure 22. Figure 22. Exmpe imge of video-dt-to-nvigtion-dt integrtion. he numers indicte orienttion in degrees. Green rs indicte tens of degrees. o the eft the position coordintes in the R9 coordinte system is visuised, together with numeric vues of the orienttion (ro, pitch, yw). Dt synchronistion he video to nvigtion dt synchronistion is somewht difficut since there is no esiy nd utomticy recognise fetures of the video dt tht cn e used. he frme grer gives time stmp to every frme, strting with zero for the first frme, nd the ogging softwre gives time stmp s it sves every frme. Just s with the IMU nd GPS dt the computer time stmps cn e used for rough utomtic synchronistion. However, ny constnt time dey wi hve to e mnuy identified y inspection of the comintion of video dt nd nvigtion dt. As for the cse with the IMU nd GPS dt, the computer time stmps re too irregur to e used for synchronistion of individu frmes with nvigtion dt. Further, the time stmps of the frme grer cnnot e used in the sme wy s for the sensor cocs of the IMU nd the GPS receiver. his is ecuse the time stmp given y the frme grer is presenttion time for viewing the video strem. his men tht if video 42 JOHN-OLOF NILSSON

43 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB frme is dropped it wi e ignored nd the time stmp wi e given to the next frme to ensure frme rte, given y the time stmps, of 25Hz. his woud give n incresing time dey etween video dt strem nd nvigtion dt if the frme grer time stmp is used s the soe timing informtion. he soution to this is to use the nown frme frequency of incoming frmes (25Hz) nd the computer time stmps to detect dropped frmes, nd with this informtion to correct the time dey etween frme grer time stmps nd nvigtion dt time. Compring the numers of rrived frmes with the numer of frmes ccuted from the time stmps nd the nown frequency give the ue curve in Figure 23. A dropped frme wi pper s permnent rise of this curve. he pes re proy due to uffering in the system. he reson why the system occsiony fis to uffer frmes nd insted drop them is unnown. ypicy the frme drops wi pper in custers with inter spcing time with no frme drops. his is proy due to the computer's ttention to other system processes. o get the right time dey for ech frme minimum numer of dropped frmes over short time frme is used. he resuting estimtion of dropped frmes cn e seen in the red curve. he time dey is stright forwrd to ccute from dt in Figure 23. Figure 23. Bue is the ccuted continuous numer of dropped frmes s directy judged y ech individu computer time stmps. Red is the discrete numer of missing frmes s determined from time stmps from sm time frme. NAVIGAION SYSEM FOR A MICRO-UAV 43

44 SCHOOL OF ELECRICAL ENGINEERING - KH System cirtion Cirtion procedure he cirtion procedure ims t finding prmeters which optimise the performnce of the system. his wi e done with one or more representtive sets of dt. hence, the estimted cirtion prmeters, wi e connected oth to the system set-up nd the dynmics tht the dt sets re representtive of. If either the set-up of the system or the dynmics of the system is significnty chnged the cirtion hs to e redone. o me the system roust ginst dynmic chnges the representtive dt ought to e chosen so tht it incudes rod rnge of dynmics. Cirtion shoud e vidted ginst dt not incuded in the cirtion run. Mesuring performnce A proem with this ind of compex system with outputs of different units nd mgnitude is tht there is no soute mesure of performnce. here might e trde-offs etween performnces of different outputs. Wht mes things even worse is tht we do not hve ny esiy ccessie extern independent mesurement of the system. Wht we do hve is the imge dt which cn provide exct informtion out the orienttion of the ptform. Unfortuntey it is hrd to tech the computer to interpret the imge dt nd such cirtion is thus difficut to utomte. herefore such n interprettion hs to e done mnuy which mes it very time consuming. Yet, it cn e used to verify the cirtion. Even though the output of the system is not independent of the GPS oservtions, they cn sti e used to mesure performnce of the system y the corrections they give rise to. GPS oservtions re prt of the input of the fiter tht is cirted. It coud therefore e sid to e n intern cirtion. he error of the GPS oservtion is ounded nd the oservtion is fctor in the stte corrections. hus, if the mode is correct nd the fiter converges then the corrections shoud e ounded s we. In the ide cse (perfect cirtion nd the mode perfecty descriing reity) the correction shoud e equ to oservtion noise pus mesurement noise infuence on inerti nvigtion. Any non-ide prmeter vue shoud give n ddition to this correction. he verge correction shoud thus give mesurement of the fiter performnce with oc minimum t the est vues. his is not wys the cse since there wi e trde-offs etween different corrections. However, ooing t the corrections dependencies of cirtion prmeter for sever corrections comined correction minimum cn mnuy e found. here re 15 different corrections nd even though we cn form rootmen-squre of the individu corrections of the sme units (the three components of the position correction vector, veocity correction vector, etc.) nd sum together over correction instnces, there re sti five mesurements of the performnce. An error in the is stte nd the veocity stte wi dd to the position stte nd the orienttion stte ut not the other wy round. Hereof the summed corrections of the position stte nd the orienttion stte re used to mesure performnce. 44 JOHN-OLOF NILSSON

45 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Mnu supervision of cirtion he oc or comined minimum nture of the summed correction round the correct prmeter vues nd the fct tht there re sti two mesurements of performnce me mnu supervision of the cirtion procedure necessry. By ming n initi guess of the prmeter vues the system cn e run with set of dt vrying one prmeter t the time. Potting the summed corrections ginst the vried prmeter shoud yied two curve (summed position correction nd orienttion correction respectivey) with hopefuy coinciding minim. As noted ove this is not wys the cse ut normy the curves provide enough informtion to mnuy improve the initi guess of the prmeter vue. Especiy fine tuning cn e difficut. Exmpes of cirtion curves re shown in Figure Figure 24. Cirtion of time is. he minim in the grphs cn esiy e identified. he time is is round.8s. he time is cirtion curves re exceptiony esy to interpret. Even though the minim do not competey coincide the performnce trde-off etween position nd orienttion is minim. NAVIGAION SYSEM FOR A MICRO-UAV 45

46 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 25. Cirtion of veocity vrince (cceerometer noise). Here the trde-off etween position correction nd orienttion correction cn esiy e seen. here is no minim. A high degree of position correction eimintes the need for orienttion correction nd vice vers. Here sensie seection of prmeter vue hs to e mde with nce etween the corrections of orienttion nd position. Note tht there is ogrithmic sce on the x-xis. he oc minimum nture cn me the cirtion sensitive to initi prmeter vues. Cre hs to e ten since wht might e descried s correction resonnce cn sometimes give rge correction sums cose to the correct prmeter vues. Aso minim correction or no correction woud give sm correction sums for set of dt, over short time period, ut terrie performnce. Occsion inspection of resuting nvigtion dt during cirtion cn e dvntgeous. By running over the prmeters sever times one cn me sure tht their vues hve converged. Finy these vues ought to e checed ginst some other set of dt. More informtion out the cirtion prmeters cn e found in the Appendix. Lte in the wor it hs een found tht the corrections re not optim to use for cirtion. Looing t the corrections x n, new x n + K n ( z Hx ) n he terms used for cirtion re descried y ( z Hx ) K. n n n A etter mesurement of performnce woud e ony ( z n Hx n ). n his gives ony position discrepncy to minimize nd does not diute this origin discrepncy used to ccute the corrections. 46 JOHN-OLOF NILSSON

47 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Verifiction of nvigtion dt An intrinsic proem of this nvigtion system is the difficuty of verifying the nvigtion system. A rough verifiction of the system cn e done y direct inspection of the resuting dt. However, testing of performnce is more difficut since, s noted ove, there is no extern mesurement of position nd direction. Video dt cn hep primriy with the orienttion performnce verifiction. Imge dt cn e seen s n soute mesurement, of the surroundings, from within the system. A distnt oject wi pproximtey hve constnt ering. he imge itsef does not give ny direction informtion out the oject ut the comintion nvigtion dt nd the imge does. Identifiction of the ering (pitch proy cose to zero) of distnt oject in mutipe views of n oject with interspcing cceertions nd trnstions wi give mesurement of the retive ccurcy of the system (IMU to video cirtion negected). Idey the system shoud indicte tht the oject hods the sme direction in every view. For reference the ering of distnt oject cn either e mesured with compss or on mp. ogether with the horizon we hve soute mesurements of directions in the imge. Compring these to the couped nvigtion dt give mesurement of the soute performnce of the system. Limittions With horizont fied of view of 62 nd horizont resoution of 32 pixes (the PAL formt ow for higher resoution ut due to se sttion imittions ony 32 pixes hs een used), one pixe wi correspond to For non-sttistic tretment of the dt this wi e imit of the ccurcy in the imge. For retive orienttion ccurcy performnce test one shoud e e to verify the ccurcy to within.1. For n soute performnce of ering of re word oject, this wi proy e significnty worse. With simpe instruments such s sighting compss it is difficut to mesure the ering of n oject to coser thn to within one degree nd without cer view of the ocen or very high titude the horizon wi ony e n imgine ine, ehind distnt ostces, mnuy interpreted. he horizon wi proy not e nown to coser thn within one degree. Hence re word performnce cnnot e verified to more thn within out 1. NAVIGAION SYSEM FOR A MICRO-UAV 47

48 SCHOOL OF ELECRICAL ENGINEERING - KH System fied dt ehviour nd performnce Re word dt is the utimte verifiction of the vidity of the system. Here coupe of sets of re word dt nd fiter output re presented. Note tht the time is given from ogging progrm strt-up nd not from the strt-up of inerti mesurement/gps oservtions. his expins why time does not strt t zero in the grphs. Gener fiter ehviour Upon strt-up of the system, it egins to deiver dt. From here on the system strt to converge nd to estimte its nvigtion stte. he ength of the convergence phse wi vry for different nvigtion sttes nd different dynmics ut tot system convergence is normy reched within few minutes. After this there wi e ste opertion phse. his mens tht the corrections (errors) re sm nd tht the is vues re ste. A typic nvigtion stte trjectory cn e seen in Figure Figure 26. R9 coordintes - Position trjectory (ue), Corrections (green), GPS oservtion (c), nd IMU-to-GPS-ntenn seprtion (green). Note tht this seprtion is not in forwrd direction ut rther out 45 in forwrdstrord direction. he position is given in retion to the strt position. 48 JOHN-OLOF NILSSON

49 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Figure 27. Veocity - North (ue), est (red), height (green), Veocity corrections seen in different coours. he oscitions in veocity is proy n effect of motion of the rm on which the IMU nd cmer is mounted. Figure 28. Orienttion - Pitch (ue), ro (red), yw (green). Corrections re too sm to e seen. Convergence phse At the point of strt-up the system is proy stnding sti. his mens tht the nvigtion informtion is not compete. Stnding sti the nvigtion system cnnot find the heding nd thus nor the gyroscope is in the corresponding rottion, yw. Stnding sti, there is informtion out the position nd veocity in the GPS oservtions nd out the ro nd pitch from the grvittion cceertion. NAVIGAION SYSEM FOR A MICRO-UAV 49

50 SCHOOL OF ELECRICAL ENGINEERING - KH As for the cceerometer ises there is informtion even though it is imited y the fct tht such ises wi prtiy e hidden y the grvittion cceertion whie the system is stnding sti nd the convergence wi e sow. As dynmics re ppied to the system (UAV strts to fy) the nvigtion system wi sowy coect informtion out the non-directy oserve sttes. Acceertions give informtion out the cceerometer is nd the comintion cceertion nd rottion give informtion out the gyroscope is. Figure 29. Estimted mgnitude of errors - x-xis/forwrd/pitch (ue), y- xis/strord/ro (red), z-xis/down/yw (green). 5 JOHN-OLOF NILSSON

51 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB he convergence phse cn cery e seen in the estimted errors in Figure 29. One cn cery see the stepwise convergence of the orienttion error nd the gyroscope is error. Ech step represents turn. Aso the cceerometer is error show stepwise chrcteristics corresponding to cceertions. One cn so see tht the position nd veocity swifty converge upon strt-up. he stedy stte vues of the predicted errors represent the intrinsic uncertinty imittion of inerti mesurements nd GPS position, the system performnce. However, one shoud e crefu in interpreting the vues of Figure 29. hese stedy sttes vues re dependent on the cirtion prmeters nd s noted in the System Cirtion section there wi e system component to these. However, the vues do give n ide out the performnce. he performnce though is etter thn the vues indicte, s wi e shown eow. Figure 3. Bis vues - x-xis/forwrd/pitch (ue), y-xis/strord/ro (red), z-xis/down/yw (green). With Figure 29 in mind, the chrcteristics of the is convergence in Figure 3 shoud come s no surprise. We see tht the pitch nd ro swifty ttin ste vues due to the grvittion cceertion present in the cceerometer mesurements. It tes sighty onger to ttin ste vues for the cceerometer ises. he time periods of rge chnges cn e referred to instnces of rge dynmics. Ste phse Once the system hs ttined ste opertion considering predicted errors nd corrections it wi operte t fu performnce. At this point the corrections shoud e eveny distriuted in directions. he fiter shoud just gurd ginst errors rising from the noise in inerti mesurements nd GPS oservtions, nd the noise shoud hve no is to ny direction. NAVIGAION SYSEM FOR A MICRO-UAV 51

52 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 31. Cumutivey summed corrections - x-xis/forwrd/pitch (ue), y- xis/strord/ro (red), z-xis/down/yw (green). he ste chrcteristic cn e seen inspecting the cumutivey summed corrections s in Figure 31. Initi heding is ssumed to e, in the fiter, ut ws oviousy pproximtey 3. he ehviour of the fiter is somewht unpredicte if the initi error in yw is rge. he fiter does converge ut sometimes the convergence time is significnty onger if the initi error in orienttion is rge. An esy wy to void this is to hve the hit of pointing the nvigtion system to the north upon strt-up. he reson tht correspondingy ste stte cnnot e seen in position nd veocity s in orienttion is tht the initi vues re nown to high ccurcy. Hence, there is no jump to correct vue s for the heding. Further, since the corrections re resut of the rndom noise, their sums wi e ie rndom w process giving sow divergence. Aso the GPS drift wi ffect the summed orienttion nd this effect wi e seen s vritions of some meter over sever minutes. 52 JOHN-OLOF NILSSON

53 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Figure 32. Fiter corrections - - x-xis/forwrd/pitch (ue), y- xis/strord/ro (red), z-xis/down/yw (green). Figure 32 shows the corrections mde y the fiter nd the verge corrections over epsed time. his gives feeing for the infuence of the inerti mesurement noise in etween corrections. he corrections in position re in the order of dm nd the corrections in veocity re in the order of dm/s. Note tht this is the effect over just one second. If the inerti nvigtion is eft uncorrected it wi swifty diverge. System performnce he soute position performnce is poor s seen in the GPS drift test section of the Appendix. he inerti nvigtion cnnot compenste for this since its position error, over typic period of GPS position drift, is much rger. he retive position on the other hnd is much etter nd hence so the direction output. Crrying round the system nd noting the directions, rough ide out the direction performnce, cn e chieved y simpe inspection of the direction dt. Wnting to trc down the direction performnce even further we suffer from not hving n esiy ccessie extern mesurement of the system. his mens tht such mesurement hs to e done mnuy. Regury pcing the system in predefined direction with inter spcing periods of dynmics we ought to get pproximtey the sme direction informtion showing up repetedy. o improve the ccurcy the imge informtion is used. By identifying the heding, s given y the system in the imge dt, to distnt oject, for ech occsion the system hs een id down in the predefined direction, we get set of orienttion estimtions, of fix orienttion, given y the nvigtion system. his wi give mesure of retive direction performnce. It is retive in the sense tht it is not dependent on IMU-tocmer cirtion. With sm fixed off-set (fw in IMU to cmer cirtion) it represents re heding though. he resut of such direction performnce test is shown in Figure 33. After the is terms of the system hd converged, the heding of distnt NAVIGAION SYSEM FOR A MICRO-UAV 53

54 SCHOOL OF ELECRICAL ENGINEERING - KH oject ws identified mnuy in the frmes with nvigtion dt dded. he oservtions of the system were ten pproximtey 6 seconds prt with significnt dynmics in etween. he system ws essentiy stnding sti when the hedings were identified. As for ro nd pitch this wi give sighty etter orienttion informtion thn for mesurement in motion. his is since ro nd pitch wi continuousy e updted with the grvittion cceertion. Since not oserve the yw informtion wi rther drift wy if stnding sti. However, the period the system is stnding sti is short enough for this to e negected. However mesurement in motion woud not give worse performnce. hough time off-set in the time stmps in etween the nvigtion dt nd the imge dt woud give incorrect pirings of frmes nd nvigtion dt. o void such possie sitution is mtter of cirtion. Figure 33. Devition in orienttion,given y the nvigtion system, when compred to soute nd mnuy identified orienttion in video frmes. Pitch (ue), ro (red), nd yw (green). he identified orienttion in the video frme ws distnt t uiding towrds which the system ws pointed with regur intervs with significnt interspcing dynmics. In Figure 33 the difference etween the system-estimted orienttion nd the orienttion mnuy identified from imge dt is shown. he mesurements re centred round n verge orienttion estimtion mde y the system. he estimtions re ten out 6 second prt nd the stndrd devition is 1.2. Mesuring the soute heding on mp gve fixed offset of out 2. However, the ccurcy of this numer ws hrd to estimte nd no cirtion hs een done. he ccurcy of the ro nd the pitch is much etter. his is due to the grvittion cceertion component (of the cceerometer mesurements) which is used y the fiter to correct the ro nd the pitch. he numer of orienttion estimtions re too few to get definite numer of the system performnce. However, it does give n order of mgnitude of such. 54 JOHN-OLOF NILSSON

55 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Corrections nd imittion of corrections An orienttion error pus n cceertion give rise to n error in the position estimte. An error in ro nd pitch wi me the grvittion cceertion dd to the true cceertion. Becuse the grvittion cceertion is so rge this wi give rge error nd n efficient correction. Further the grvittion cceertion is wys present which me the correction possiiity wys present. Yw, on the other hnd is rottion orthogon to the grvittion vector. herefore there hs to e true cceertion nd dispcement for the system correct yw. he cceertions re often order of mgnitude smer thn the grvittion cceertion nd not wys present which me the performnce of the yw so much worse thn tht of the ro nd the pitch. here re three prts to the orienttion correction/performnce: n cceertion hs to e mesured y the IMU, dispcement or c of dispcement (for grvittion cceertion) hs to e oserved y the GPS, nd the gyro eep trc of the orienttion ming correction possie over sever position oservtions. As cn e seen in the GPS drift test section of the Appendix the GPS imittion of yw direction informtion with dynmics in the horizont direction of 1 m/s is out.8. Stnding sti the system wi rememer the yw for whie which et us identify the true heding nd we cn hence concude tht the ddition error comes from the imited performnce of the gyro nd imperfections of the mode. he sitution is sighty different for pitch nd yw. Even though there is typicy much ower dynmics in the vertic direction the grvittion dynmics me it possie to use the c of dynmics insted. he c of dynmics given y the grvittion cceertion is pproximtey 5 m/s 2 ( 1 2 gt ). his woud give imittion of direction informtion for ro nd pitch to out.2. his wi e sighty improved stnding sti since the discretiztion of GPS position wi essen the impct of the GPS drift. Every time the system moves over to nother equiposition ox there wi e n error in orienttion ut for the remining oservtions from within the equiposition ox the orienttion informtion wi e etter thn for the system in motion. NAVIGAION SYSEM FOR A MICRO-UAV 55

56 SCHOOL OF ELECRICAL ENGINEERING - KH Discussion he resut of the wor presented is n inexpensive wireess nvigtion system with video informtion uit y very simpe mens. he nvigtion system is ight enough (71g) to e crried y sm heicopter UAV ptform nd is synchronised with on-ord video dt. here is no on-ord dt processing, nd thus the system is rnge-imited to the se sttion. he min contriution of the wor is the physic system, the performnce nysis, nd the experience dded to S Bofors Dynmics. he principes of nvigtion used cn e found in stndrd textoo on the suject nd indeed there re simir on-ord systems on the mret, even though their price is not compre to tht of this system. he performnce evution gives ower ound on the performnce of the nvigtion system with the given sensors. However, this ound cn most iey e improved. Ides regrding this is found in the Further wor section. he direction informtion (performnce) is given with mrgin of error (stndrd devition) of out 1. he soute position informtion is given to within 2-5 metres. he soute position coud e significnty improved y using differenti GPS, ut it woud not significnty ffect the retive position or the direction informtion. he use of the nvigtion system is imited to pces with GPS reception. Without the GPS oservtions the fiter cnnot eep the position error t resone eves for more thn few seconds. he orienttion estimtion is more roust ginst GPS outges. Hence, for further deveopment, compementry informtion to the GPS oservtion woud e desire for improved spti fexiiity in n urn environment. he nvigtion fiter is simpe Kmn fiter. he fiter might e considered to e n extended Kmn fiter, ut in this report it hs een derived s norm Kmn fiter. his hs the dvntge of ming the fiter esier to understnd nd independent of nvigtion mode nd error mode derivtion. his coud e of use if the resut of contro system is dded in the mode. If the uxiiry error equtions re then hrd to derive nyticy, (not differentie) n d hoc error mode coud e used without coming into confict with the fiter. he fiter is not run in re time. his is ecuse the fiter hs een impemented in Mt nd the ogging progrm in C/C++. However, re time impementtion is mosty mtter of porting code from Mt to C/C++. It is worth noting tht the nvigtion system is stnd-one system nd coud e ported to ny other ppiction where the sme dt output is desire. As rgued in the introduction, the extended ptform (the UAV ptform pus the nvigtion system) coud e the sis for fuy function sm UAV scouting heicopter. However, s the dt ins re rrnged tody, it is not yet suite s foundtion for dding more sensors. he reson for this is tht it cnnot esiy e extended to trnsmit more dt strems. he nvigtion system gives n ide of the performnce nd usiity of simpe nvigtion system uit from inexpensive off-the-shef components. It serves s sis for sign processing of other possie sensor dt. 56 JOHN-OLOF NILSSON

57 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB However, for fied use, mnu contro of the UAV is wwrd. It requires wees of trining for n opertor nd tht the UAV is within sight. he opertor wi so hve to focus competey on the contro of the UAV. Hence, n utomtic contro system is necessry. he nvigtion system provides position, veocity, cceertion, orienttion, nd ngur veocity. his is so the sic informtion needed for contro system nd for this reson the nvigtion system is so foundtion for contro system. For n utomtic contro system/utopiot to e roust it hs to e impemented on-ord nd hence so the nvigtion system. he UAV ptform woud e reieved from the rdio contro communiction equipment nd one Buetooth dt in, ut sti the weight constrint is severe. A high performnce micro controer or direct hrdwre impementtion in progrmme circuits woud proy e needed. he woud sti e needed though to send commnds to the UAV nd it woud so provide extr computtion cpiity to for exmpe further imge processing. Concusion A ight inexpensive nvigtion system, with video informtion, crrie y sm heicopter UAV, though computtiony impemented t se sttion vi wireess dt ins, is impemente y simpe mens. It cn give direction informtion to within ±1 nd position to within 2-5 metres. he UAV is possie sis for further deveopment of sm scouting heicopter UAV. he UAV system so provides the possiiity of imge processing impementtion t the se sttion. NAVIGAION SYSEM FOR A MICRO-UAV 57

58 SCHOOL OF ELECRICAL ENGINEERING - KH Further wor Here some possie further deveopment wor is suggested. Differenti GPS receiver As it is now, the direction informtion is good whie the soute position is poor. his coud proy esiy e soved y repcing the GPS receiver with receiver supporting steite sed ugmenttion system (in Europe, the EGNOS system). his repcement coud proy e done with minim chnges to the nvigtion system. Bse sttion differenti GPS he steite-sed ugmenttion system is imited to res of the Erth which hve such coverge. Coverge is more or ess imited to the industriised prts of the word. Hence, more fexie wy of deing with it woud e to dd GPS receiver t the se sttion. Since the GPS error is strongy correted in spce nd the se sttion is sttionry, this woud provide informtion out the GPS drift. Aso, since the sttion is fixed, one coud get much improved soute position informtion fter some time of opertion, through verging of the se sttion position nd comprison of such with nvigtion system position output (position differentition). Even if, during fied opertion, porte se sttion is needed, it is sti iey tht its dynmics wi e much ower thn tht of the UAV. One coud imgine simpe cceerometer which woud determine when the sttion ws stnding sti. his wy, the se sttion coud strt verging its position s soon s it is sttic. Aso the higher ccurcy of the retive GPS position woud me shorter reoctions possie without osing souteposition informtion. Since position ccution from different steites wi give different position nd different errors prefery pseudo rnges woud hve to e used to impement ses sttion differenti GPS. he proximity of the UAV nd the verging point shoud give very good position ccurcy. here shoud e no GPS drift ut ony sow convergence towrd the soute position. Correction of od nvigtion dt woud so e possie. Inexpensive simpe USB GPS receivers re rediy vie so se sttion GPS coud esiy e impemented. Fu qurterion impementtion he use of rottion mtrices is nyticy simpe ut numericy proemtic nd computtiony hevy. On the other hnd, qurterions re nyticy more difficut to use in the fiter ut numericy ste nd computtiony cheper. Hence fu qurterion impementtion woud e desire. Re-time dt processing A first step to on-ord dt processing is to impement the nvigtion fiter in re-time. his woud incude porting the nvigtion fiter from Mt code to C/C++ code nd to merge it with the ogging progrm. 58 JOHN-OLOF NILSSON

59 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB On-ord dt processing o improve fexiiity of the ptform one woud ie to process dt independent of the se sttion. his on-ord dt processing woud require impementtion of the fiter on microcontroer or progrmme circuit, nd re-routing of the sensor dt strems to it. here re ight-weight commerci ppictions on the mret which coud e used (MicroPiote Inc.). he possiiity of true hrdwre impementtion (progrmme circuit) woud so e interesting to study. he dt ins woud now e used ony for ogging of nvigtion dt, nd the se sttion woud e used to perform other dt processing, such s imge processing with the hep of the nvigtion dt. Computtion od chrcteristion In n on-ord dt processing impementtion, where computtion resources re very imited, it woud e etter to find out how different prmeters (e.g. inerti nd error mode, numeric ccurcy, etc.) ffect the computtion od of the fiter nd how these ffects the system performnce. his nowedge coud hep to provide n efficient ow eve impementtion of the fiter. Automtic contro For resone fied opertion, n utomtic contro system woud hve to e deveoped. For dequte system roustness it woud hve to e impemented on-ord. he deveopment of the contro system woud proy first require simution of the dynmics of the UAV ptform nd deveopment of the contro system in the simution environment with susequent impementtion in microcontroer, nd sty repcement of the rdio contro components. Automtic contro systems re commerciy vie (e.g. Micro Piot Inc.) yet expensive ($8), so deveopment of n utomtic contro system woud miny e motivted y the experience it woud give. Bc correction In re time nvigtion system, ony the instntneous system stte is of interest, nd ccordingy stte corrections ony ffect the sttes t the correction instnt nd therefter. his gives the est re time stte estimtion ut jgged oo of the system sttes. For mny other dt processing procedures (e.g. geometric modeing from imge nd nvigtion dt) we cn ow post processing or t est deyed processing. his woud give us time for c correction, correction of the nvigtion sttes of pst time. his enes smooth system stte trjectories which woud proy e enefici for most dt processing ppictions utiising the nvigtion informtion. Evution of other nvigtion dt sources he usiity of GPS is imited to pces with GPS reception. his excudes indoor environments which re common in urn wrfre environments. Wht other sources for nvigtion informtion re there (imge, compss, ir pressure, etc.)? Coud ny of them e impemented in the current frmewor nd ptform? NAVIGAION SYSEM FOR A MICRO-UAV 59

60 SCHOOL OF ELECRICAL ENGINEERING - KH Fourier trnsformtion of imge A simpe Fourier trnsformtion of the imge shoud provide informtion out shift in the imge originting in either system rottion or system trnstion. his coud e used to improve nvigtion. Other informtion coud possiy e extrcted s we. Improved synchronistion of dt Currenty the sensor dt is ony synchronised y se sttion time stmping nd cirtion. One woud ie to hve on-ord hrdwre synchronistion. he P63L chip cmer is vie in vrint with extern synchronistion, P63CL/CCS. Further, the GPS receiver hs one puse-persecond output pin synchronised to the GPS time. his 1Hz sign coud proy rther esiy e converted into suite synchronistion sign for the cmer. he IMU dt nd the GPS dt coud then e synchronised y the mens of sm fiter extension s suggested in reference [5]. Aterntivey, the OEM-version of the IMU is so vie with n extern synchronistion from GPS. his woud render dt strems synchronised. Improved dt ins he current dt in system hs mjor drwc in tht the seprte dt strems use seprte ins insted of hving common dt trnsmission in. his mes the dt in system infexie. Aso the currenty used Buetooth ins hve proved to e somewht proemtic, oth considering rnge nd reiiity. Hence new, common, nd from sensors seprted, dt in system woud e desire. Extended error mode he error mode used is y no mens exhustive. Sever other error terms such s scing errors, sensor orienttion errors, etc., coud e dded. It is hrd to sy how gret n impct such n extension woud hve on ccurcy considering the performnce of the sensors in use. However, n extended error mode might e worth testing efore extensive wor is done on n onord impementtion of the nvigtion system. Possiy n extended error mode coud ow for use of ess expensive ess we cirted inerti sensors. However, there is sti the imittion of the GPS dt which sets the imit of the performnce within rge rnge of sensors. Low cost IMU One of the min dvntges of micro UAV is the ow cost. However, the IMU is n order of mgnitude more expensive thn the GPS receiver. It woud thus e interesting to repce the IMU with truy ow cost cceerometer nd gyroscope to see how this woud ffect the nvigtion system performnce. Compss Upon strt-up nd fter n extended period of rest the heding informtion in the system is poor. his coud e prevented y using mgnetometer (compss). Even though the mgnetometer does not hve etter dynmic 6 JOHN-OLOF NILSSON

61 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB performnce nd is sensitive to disturnces it wi sti give error ound upon strt-up nd sttic conditions. NAVIGAION SYSEM FOR A MICRO-UAV 61

62 SCHOOL OF ELECRICAL ENGINEERING - KH References 1. J. Frre nd M. Brth, he Go Positioning System nd Inerti Nvigtion. McGrw-Hi, New Yor NY, I. Sog nd P. Hände, A Low-Cost GPS Aided Inerti Nvigtion System for Vehice Appictions. in Proc. EUSIPCO 25, (Anty, urey), Sept S. Mohinder et. Go Position Systems, Inerti Nvigtion nd Integrtion. John Wiey&Sons, New Jersey, Person communiction with Frns Hofmnn, nvigtion expert, SAAB Bofors Dynmics. 23 oct I. Sog nd P.Hände, ime synchroniztion errors in GPS-ided inerti nvigtion systems, sumitted to IEEE rnsctions on Inteigent rnsporttion Systems, S. Hong, F. Hrshim, S. Kwon, S.B Choi, M.H Lee nd H. Lee, Estimtion of errors in inerti nvigtion systems with GPS mesurements. in Proc. of ISIE 21, IEEE Interntion Symposium 21, vo. 3, JOHN-OLOF NILSSON

63 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Appendix Sensor system connection nd dt ins Dt ins Here the dt ins refer to the fu system tht mes the communiction etween sensor nd se sttion possie. It thus incudes power suppy nd contcts, Buetooth modues, PCB, nd ogic eve converter. Logic eve converters he IMU gives RS232 eve output which needs to e converted to L eves to me communiction with the Buetooth modue possie. Since the IMU uses 3-wire (receive ce (RX), trnsmit ce (X), nd ground (GND)) RS232 communiction ony two conversions re needed. he conversion is done y MAX23E chip. he circuit hs two conversion possiiities in ech direction (two drivers nd two receivers), two L sttes, SO2 csing, ESD resistnt design, 5 Vot suppy votge, nd no extern cpcitnces. For detied description see product documenttion. he GPS receiver gives L eve output. However direct connection to the Buetooth modue is not possie. his is possiy ecuse of n inputoutput impednce mismtch. For simpicity this is soved y dding n extr eve converter. he sign is then first converted to RS232 eve nd then converted c to L eve. his is sight miss-use of the MAX23E chip functionity ut it wors. PCB he contct of the Buetooth modues ws ony vie in surfce mounted version. ogether with the need for eve converter integrtion nd ightness of the system it required system specific PCB to e deveoped. For detis on connection see PCB circuit digrm section. In short the PCB is composed of pin-contcts, 2 MAX23E chips, nd two Buetooth modue contcts. 5V suppy votge nd ground is suppied to components. he IMU input nd output pin is connected vi MAX23E to one Buetooth modue. he input nd output pin of the GPS is ed vi oth MAX23E chips to the other Buetooth modue. A fu 9-pin seri connection from ech Buetooth modue is pued out to pin contcts. he synchronistion pin of the GPS receiver is pued out to nother pin if needed in future system deveopment. Stiizing cpcitnces is dded to suppy votge nd pority sensitive resistor to protect components from miste of pority switch. he PCB so gives mechnic support for the Buetooth modues. One mounting spcer is used for ech modue to secure it to the PCB. A the component ut the pin contcts re surfce mounted. he PCB with mounted components cn e seen in Figure 34. NAVIGAION SYSEM FOR A MICRO-UAV 63

64 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 34. PCB with contcts nd ogic converter circuits. Buetooth modue he Buetooth modues re mounted on top of the PCB. However they cn esiy e removed for reconfigurtion. he mounted Buetooth modues cn e seen in Figure 35. he modue is cpe of 28K/s dt trnsfer rte. his wi decrese with distnce. he intern fsh memory wi hod oth the contro register nd dt trnsfer uffer. he size of the memory/uffer hs not een found in product documenttion ut hs een estimted to out 8 its. he modues re used s simpe seri ces repcements. However, they hve much rger functionity which is not used. For further informtion see product documenttion. Figure 35. PCB with Buetooth modues mounted. Power suppy nd contcts he suppy votge nd ground is pued from the intern 5V power suppy of the UAV. he contcts re mde from socet connectors nd epoxy poymers. he ce is stndrd.25mm sign ce. 64 JOHN-OLOF NILSSON

65 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB he fu ssemy of PCB, Buetooth modues, nd contcts is shown in Figure 36. Figure 36. PCB with Buetooth modues nd contcts. he eft contct is for the IMU, the midde for the GPS receiver, nd the right the suppy votge nd ground. Setting up Buetooth modue he Buetooth modues re y defut set to e not connectie, not discovere, nd not remote cpture A prser. his men tht if Buetooth modue is rest to defut, for reconfigurtion, it wi hve to e connected to deveopment ord or one hs to mnuy connect it with RS232 connection to the pin contct of the PCB. With norm settings, configurtions cn e done wireess (see eow). If one hs to connect to the Buetooth modue directy deveopment ord is prefere. he fu seri connection of the PCB pin contct ist uses L eve ogic. If direct RS232 connect is desire (Computer seri port) connection hs to e mde to the IMU pin set nd the Buetooth modues moved to ccess oth. See detied hrdwre description. Buetooth modue commnd When connection is estished with the Buetooth modue (it is recognised s com-port in the OS) the settings cn e chnged with ny termin progrm (Ezurio ermin nd Windows Hyper ermin hs een tested). he foowing commnds wi set-up the Buetooth modue for communiction within the system. Commnd Response Description!!! OK Cpture the A prser in wireess mode. Ony if one hs connected wireessy to the modue. Not possie on defut A OK Checs the connection with Buetooth modue A&F* OK Reset modue to defut. his sh not e done if one wnts to eep NAVIGAION SYSEM FOR A MICRO-UAV 65

66 SCHOOL OF ELECRICAL ENGINEERING - KH setting or is woring wireess since then it wi cut the connection. AS5124 OK Me the strt-up mode, of the modue, discovere nd connecte. AS-1 OK Set modue to nswer immeditey on incoming cs. A+BK pin code OK Set pin code for piring (further informtion see eow). AS52XXX OK Chnge the communiction speed to XXX ud rte. he termin ud rte wi hve to e chnged for further communiction. AS5361 OK Me the A prser cpture vi wireess communiction (me the!!! commnd possie) A&W OK Sve settings to EEPROM. Now the modue cn e turned off eeping the settings. For detied communiction reference, see product documenttion. Note, tht in order to ene wireess A prser cpture the string!!! cnnot e sent over the in. here is inter sign time spcing imit of the!!! commnd tht cn e set with AS12X, where X cn e from 4 to 5 giving time with grnurity of 2ms. Pin code is not needed if encrypted communiction is not used. However it seems ie Windows require one initi pring with pin code to ow nonencrypted communiction. Hence, it needs to e set nywy. he ud rte settings re typicy 576 for the GPS nd 1152 for the IMU. he AS5124 determines the strt-up mode of the modue. For this setting to te effect the modue needs to e restrted. Hence, wireess connection is not possie efore the modue hs een restrted. If the Buetooth modue hs incoming communiction it cnnot e connected to. his sitution might rise if the IMU hs een set to continuousy output dt nd we wnt to reconnect the Buetooth in. he reson for this is unnown. Buetooth stc For the OS to hnde Buetooth connection Buetooth stc is needed. Most Buetooth stcs offer out the sme functionity. An Buetooth stc from WIDCOMM Inc. distriuted y Ezurio hs een used. he Buetooth modues hve unique nmes (written on e on the RFcsing) nd if powered up nd set-up correcty they wi e found when scnning the re for Buetooth devices. Prefery one crete short-cut (right-cic nd choose Crete Short-cut ) to ech device. his short-cut enes permnent settings for their respective connection. he short-cut wi pper in the Buetooth stc min pge. hese short-cuts wi e specific to 66 JOHN-OLOF NILSSON

67 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Buetooth donge nd n USB-port nd wi not show-up uness it is inserted. On defut ony one seri port is ened in the Buetooth stc. Since two is needed one hs to e dded. Under Buetooth->Advnced Configurtion- >Cient Progrm in the Buetooth stc choose the Add COM-port. Right cic on the Buetooth modue short-cut creted nd seect Preferences. Under the COM-port drop-down ist there wi now e two seri ports. Seect one for ech modue. Further it is prefere to unseect the ox sfe connection. he dt sent wi not e encrypted ut this wy the pin code does not hve to e entered upon connection. Mnuy connecting once wi me it possie for the ogging progrm to utomticy connect to the modue. When the ogging progrm enquire for the com-port the stc wi recognise the modue s the st device eing connected to this com-port nd utomticy connect nd the progrm wi receive hnde to the com-port. GPS receiver he Go Positioning System (GPS) receiver is n EM-411 from Go St, n inexpensive off-the-shef GPS-receiver with SiRF str III chip set. he receiver uses 3-wire Seri interfce with L eve ogics. he contct of the GPS receiver is 1mm spced 6-pin socet connector. Numering from the right, PCB fcing down. Pin 1 Ground (GND) Pin 2 Suppy votge, 5V (Vcc) Pin 3 rnsmit (X) Pin 4 Receive (RX) Pin 5 Ground (GND) Pin 6 Synchronistion puse, 1Hz (not connected) he GPS receiver cn use NMEA183, SiRF inry, nd User1 protoco. For further informtion see product documenttion. Setting up GPS receiver communiction he GPS receiver is y defut set to 96 ud. o minimize the time for sending informtion from the GPS receiver to the Buetooth modue nd to mximise the time ccessie for the modue to send the informtion further the ud rte wi e set to 576. his setting cn e done with either the SiRF-demo progrm or with ny termin. he commnd sent from termin is. $PSRF1,1,576,8,1,*36\r\n In the commnd comms re used s deimiter. he first prt is the commnd heder. he one tes the receiver to use NMEA protoco for communiction. hen there is the ud rte foowed y yte size, stop it, nd prity. Lst we hve hexdecim checsum, crrige return, nd ine feed. Not tht \r nd \n re non-printing ASCII contro chrcters nd there might e different wys of writing them in different termins. For fu commnd references see product documenttion. NAVIGAION SYSEM FOR A MICRO-UAV 67

68 SCHOOL OF ELECRICAL ENGINEERING - KH Note tht, in the product commnd documenttion, the ud 576 is not mentioned s possie. his is incorrect. IMU he Inerti mesurement unit (IMU) is 3DM-GX2 without mgnetometer from MicroStrin. he IMU is of MEMS type. he IMU use 3-wire RS232 connection for extern communiction. he contct is Micro-D me connector. For seri communiction ony four pins re used. With stndrd DE-9 numering. Pin 3 Suppy votge, 5V (Vcc) Pin 4 rnsmit (X) Pin 5 Receive (RX) Pin 8 Ground (GND) Setting up IMU communiction he IMU is y defut set to poing mode nd 1152 ud. his is wht wi e used in upon strt-up of the ogging progrm. Uness these settings hve een chnged no configurtion hs to e done. If new strt-up settings hs een set in the memory nd need to e chnged c communiction protocos cn e found in the product documenttion. 68 JOHN-OLOF NILSSON

69 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Sew symmetric mtrices nd sm nge rottion When the ngur rte is integrted or when there is correction of the orienttion, the nges re normy sm. dθ dθ x dφ y dψ z his mens tht we cn simpify the mtrix representtion y ignoring higher order terms. Representing the rottion round different xes s seprte rottion mtrices, mutipying together, nd ignoring higher order terms it is found tht independenty of the order of mutipiction of the mtrices the resuting mtrix is the sme. 1 dψ z dφ y dψ z 1 dθ x I + d y dθ φ x 1 d θ his is the sm nge mtrix representtion of nge representtion of the reverse rottion is. I he mtrix dθ dθ. Further, the sm dθ is so the sew symmetric representtion of the vector dθ. Such representtion hs the properties x q xq. Using the permuttion rues of the cross product it is so found tht xq qx. NAVIGAION SYSEM FOR A MICRO-UAV 69

70 SCHOOL OF ELECRICAL ENGINEERING - KH Cmer to IMU cirtion he video cmer nd the IMU re mounted in cose proximity nd hence the origin of their coordinte system is thought to coincide. However their orienttion does not coincide. he coordinte system of the video cmer is not ment to give ny informtion in the imge ut is rther frme to give direction informtion of the imge. It shoud thus not e mixed up with the coordinte system used to drw grphics in the imge. Leve nd vertic in the imge wi though coincide with coordinte pnes of the video cmer coordinte system. he cirtion of IMU to cmer wi ony ffect the retion of nvigtion dt to the video dt nd not the nvigtion dt itsef. he ody system coud e chosen ritrry. It is just convenient to et it coincide with the cmer coordinte system. For iustrtions of the coordinte systems, see Figure 8,9,11,12. Approximte trnsformtion he video cmer coordinte system is so defined s the ody coordinte system. Hence we use the stndrd directions of fight dynmics, see Figure 12. he IMU nd the video cmer is mounted in such wy tht they hve coordinte xes tht re pproximtey pre, see Figure 11. he nowedge of the pproximte direction of the IMU coordinte xis cn e found y simpe turn-nd-wtch-the-grvity-vector test. For our specific mounting we wi get n pproximte trnsformtion from IMU sensor coordintes to ody coordintes s. s rnsformtion correction horizont pne From here we wi descrie the true, s, trnsformtion with this pproximte trnsformtion with sm correction, s d. his division is not necessry ut the fu trnsformtion cn e found from the cirtion procedures. However, the trnsformtion ought to e esier to understnd using egie pproximte trnsformtion nd sm correction thn from fu trnsformtion mtrix. he video output (or prefery cptured frmes) is used to eve the cmer. his cn e done y using te or ny other ft eve re tht cn e wtched from the side. Let the ine, which the side of te form, spns the mjor prt of the imge nd mesure horizont from one end to the other. Otherwise rdi distortion might compromise the resut. e cptured frme nd drw ine from one point of the te edge t one side of the imge to nother point of the te edge of the other side of the imge (using Pint for exmpe). Adjust the cmer so tht this ine gives stright ine of pixes. Now djust the pcement of the cmer so tht the te top is not visie ut the sightest oject on the te top wi e visie. We wnt the system to pprehend this s eve. 7 JOHN-OLOF NILSSON

71 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Coect cceerometric dt from this position. Be sure to eve the IMU on for whie efore coecting this dt since there might e initi strt-up ises. When dt hs een coected the procedure is the sme s for the initi eve djustment (see Initi eve djustment section of Appendix). he pproximte trnsformtion of the dt is done nd from the new dt orienttion is ccuted ssuming the ony component of the dt is grvity. From the orienttion correction mtrix is constructed just s the norm ody-to-oc coordinte system trnsformtion mtrices. ogether the pproximte trnsformtion mtrix nd the correction mtrix forms the sensor-to-ody coordinte system trnsformtion. s d s hese mtrices re susequenty used to trnsform mesurement dt of the IMU to the ody coordinte system. ω s s s, s ω s, Beow we cn see cirtion run in Figure 36. We see the initi xis switching (pproximte trnsformtion) of cceerometric dt in su-pot one (sensor coordinte system) nd three (pproximte ody coordinte system). In su-pot four we see the resut of the corrected trnsformtion. he x-xis dt (ue) hs een over pinted y the y-xis dt (red). Not surprisingy one cn see tht the cceerometer mesurement is pointing competey in the z-direction in the new (ody) coordinte system. Figure 37. IMU to cmer cirtion. op-eft supot, rw cceerometer mesurement from eve position. Lower-eft supot, pproximte trnsformtion of mesurement dt. Lower-right supot, corrected trnsformtion of mesurement dt. NAVIGAION SYSEM FOR A MICRO-UAV 71

72 SCHOOL OF ELECRICAL ENGINEERING - KH rnsformtion correction yw he Yw wi hve to e cirted s we. Forwrd in the ody coordinte system shoud correspond to the centre of the picture. No such utomtic cirtion hs een done. Possiy this coud e done y fcing the cmer stright down. Now the grvity vector cn e ssumed to y in the xy-pne of the coordinte system tht hs een trnsformed with oth the pproximte trnsformtion nd the correction trnsformtion of ro nd pitch. his woud give the yw s. ψ ( ) tn 2 y, x Forming nother correction mtrix ssuming ro nd pitch zero nd dd to the trnsformtion mtrix woud correct for the yw misignment etween the IMU nd the cmer. his hs not een done. he primry reson for this is difficuties of igning the cmer stright down. his woud require some ind of djuste support to eep it in tht orienttion. A simper yet not s ccurte wy is to mnuy cirte it y repetedy ooing t fr ojects of nown ering nd djust the yw to give good performnce. Limittions here re some imittions to the cirtion procedure. First of imittion mentioned for the eve djustment so ppy here giving n uncertinty of out ±. 3. Further there is n imperfection of the eveing with the cmer. he eve cn ony e djusted to within one pixe. As of worst cse scenrio (eve eing one hf pixe wrong in ech end), ssuming n resoution of 32x24, gives n uncertinty of ± rctn ( 1 32) ±. 2. Atogether the uncertinty of the cirtion of the horizont pne cn e ssumed to e eow.5. he mnu djustment of yw wi proy e much worse. Judging from ering performnce tests it wi e in the order of degrees. 72 JOHN-OLOF NILSSON

73 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Cirtion nd cirtion prmeters here is numer of prmeters of the system tht cn e chnged to tune the nvigtion system performnce. he erier mentioned cmer to IMU cirtion does not ffect the nvigtion system. he prmeters cn e divided up in those tht wi ffect the system throughout its runtime nd those tht wi ony ffect the system during initi convergence. A these prmeters hve re word origin. However since our mode is imperfect there wi e system component to the prmeters s we. Further, since the fiter view of over performnce might not e excty the sme s our view of performnce the prmeters re etter thought of s cirtion prmeters rther thn true physic prmeters. he physic origin wi however give us rough ide of pproprite prmeter vues. Beow the prmeters re presented with typic cirtion curves. For n expntion of the curves see the Cirtion section. Error stte vrinces he updte of the sttes is determined y the predicted covrinces nd the discrepncies etween the estimted sttes nd our oservtion of sttes. hose covrinces re in turn determined y erier (initi) covrinces nd the intrinsic stte vrinces of Q. hese intrinsic covrinces hd een ssumed to e independent of ech other nd consequenty the off digon eements of Q wi e set to zero. his so men tht we ssume the individu components (corresponding to different xes) of the sttes independent of ech other. his is motivted y the fct tht there re seprte sensors mesuring the sttes. Ech digon entry wi correspond to vrince of n error stte. In the fiter it ws ssumed tht the mesurement noise of the inerti mesurement ws trnsferred to the corresponding error stte. he vrinces thus hve re word origin in the inerti mesurements. his is importnt since the error sttes, except the ises, does not hve ny intrinsic noise. hey re rther resut of the noise. he vrinces might e esier to understnd if thought of in terms of stndrd devitions (squre root of the vrinces). hen they cn e thought of s the stndrd devition of the differences etween the stte vries mesurements ten one second prt. Position error vrince Looing t the origin discretized inerti nvigtion mode we see tht the position wi ony e ffected y the ffected y the mesurement noise through the summtion of veocities t different instnces. Hence there is no first order dependence noise nd the position error vrince wi e sm NAVIGAION SYSEM FOR A MICRO-UAV 73

74 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 38. Position vrince cirtion Veocity error vrince he veocity is direct resut of the cceertion. Looing t the discretized inerti nvigtion mode we see tht there wi e term dt s the cceertion noise is trnsferred to the veocity. his term wi e impemented seprtey nd not prt of the prmeter. his is to ow for different vues of dt. dt cnnot vry much without chnging the vue eow though, since the rndom w process tht the integrtion of noise cn e modeed s does not sce inery with time. We cn so see tht the vrince of the cceerometer is woud dd. However it is order of mgnitude smer thn the cceerometer noise itsef nd cn e negected. Figure 39. Veocity vrince cirtion. Orienttion error vrince Just s the cceertion noise trnsfer to the veocity the ngur rte noise wi trnsfer to the orienttion. Here it is given in degrees. 74 JOHN-OLOF NILSSON

75 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Figure 4. Orienttion vrince cirtion. Acceerometer is vrince he cceertion is wi vry sowy with time. As mtter of fct the whoe time vrition is modeed s resut of noise. Figure 41. Acceerometer is vrince cirtion. Gyro is vrince he gyro is wi sowy vry with time. Just s the cceerometer is its vrition is modeed s soey resut of noise. NAVIGAION SYSEM FOR A MICRO-UAV 75

76 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 42. Gyro is vrince cirtion. Synchronistion time is As seen in the Synchronistion section constnt, ut unnown, time retion/is cn e found etween the dt strems. his cn e treted s cirtion prmeter. Figure 43. ime is cirtion. Oservtion vrinces Aso the oservtions wi hve noise nd vrince. he updte is resut of the oservtion vrince nd the stte vrinces (nd the discrepncies etween estimted stte nd oservtion). It cn e seen s mesurement of how much to eieve the different vues. he drift of the GPS oservtion wi not e component of this vue. his vue wi rther e resut of short term noise of the oservtion nd quntistion effects due to imited numeric ccurcy of the oservtion. As we cn see in the GPS drift test the noise nd the quntistion wi e different in different directions. he different performnce in different 76 JOHN-OLOF NILSSON

77 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB direction cn prtiy e expined y the fct tht retive positions of steites, in use, give different performnce in different direction. Most noty is the fct tht the ow nge, under which the steites re seen, gives good horizont ccurcy ut ow vertic ccurcy. he different quntiztion effects re due to the fct tht the titude nd ongitude coordintes sce differenty when converted to the orthogon coordintes, yet they re given with the sme numer of decims. 1 titude corresponds to frther distnce thn 1 ongitude when converted to meter in the R9 system. Figure 44. GPS oservtion vrince cirtion. Antenn position he pproximte ntenn position cn esiy e mesured within the ody coordinte system. he exct position, s the system sees it, is more difficut since we do not hve re word reference to the orienttion of the ody system nd the ntenn hs non-zero extent. Locing t the position nd orienttion correction did not give stisfctory resuts. he ntenn position ws insted cirted y direct position trjectory inspection during rottion. Initi vrinces he initi system sttes wi hve n uncertinty. his corresponds to the initi vues of the covrince mtrix P. Once gin we ssume tht these errors re uncorreted nd set off-digon entries to zero. hese initi vues wi ffect the updtes efore stedy stte. Simpy they re mesurement of the uncertinty of the initi sttes. Initi position vrince he initi position is set from the first GPS oservtion. he initi stte error vrince shoud thus e out the sme s the susequent error vrince of GPS mesurement. As rgued ove the vue wi differ in different directions NAVIGAION SYSEM FOR A MICRO-UAV 77

78 SCHOOL OF ELECRICAL ENGINEERING - KH Initi veocity vrince As ong s we do not de with on-the-fy strt-up of the system the initi veocity wi e zero nd the vrince cose to zero. By other words, we re very sure of tht the UAV is stnding sti to strt with. Initi orienttion vrince As expined in the Initi eve djustment section the initi vue of ro nd pitch is determined y smping few initi cceertion mesurements, nd hs n ccurcy of frction of degree. As for the yw it hs to e mnuy set. For simpicity we wi normy set it to zero (system pointing to the north) nd then et the fiter find the vue from the dynmics. his gives high uncertinty of the yw orienttion. Initi is vrinces By verging cceerometer nd gyro mesurements from the system stnding sti we get n ide of the pproximte size of such. With initi vue of zero we then get the uncertinty. Other prmeters he time over which to smpe initi dt to determine orienttion for exmpe hs to e mnuy set. he system ought to e stnding sti during this time. It is normy sufficient with coupe of seconds. 78 JOHN-OLOF NILSSON

79 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Initi eve djustment Independent of initi error of orienttion the system ought to find the correct orienttion s it strts to move. However we cn grety simpify the wor of the fiter y me good guess of strting orienttion. As for the yw we cn ony mnuy set it or et the fiter do the jo. he sitution with the ro nd the pitch is etter. he wy the system defines yw is motion in the horizont pne, mening tht such rottion vector is pre with the grvity vector. Stnding sti grvity ought to e our ony component of the cceerometric mesurements. Ony using ro nd pitch rottion we shoud e e to confine the cceertion mesurement to the z-xis. Upon system strt-up we ssume we re stnding sti for short time over which we cn verge the grvittion vector. his improves the eve prediction even though it is not necessry. he grvittion cceertion is fr greter thn ny other ong term cceertion, which mens tht if we verged over n interv of order of seconds, the grvittion cceertion woud most iey e the dominnt term. Let e the verge of the cceerometric mesurement over some time period. hen g. g is not nown though, ut wht is nown is g g Assuming strict equity this eds us to g g. Since the yw shoud not ffect this retion we cn set it to zero. Expressing in terms of ro (θ), pitch (φ ) nd yw (ψ), ssuming 2 x 2 y 2 z g + +, nd soving for ro nd pitch yieds. θ tn 2 (, ) 2 φ tn 2 x, y + y z 2 z Setting yw to suite vue we cn then ccute Limittions. Possie is terms woud to some degree compromise the eve estimtion. Experience with the current IMU though shows tht under norm opertion conditions such terms re we eow.5m/s 2. ogether with grvity component of out 9.81m/s 2, this gives n uncertinty of out ±rctn(.5/9.81) ±.3º. A other possie error terms reting to grvity mentioned in the other possie error terms of the Appendix section might so contriute to esser degree. NAVIGAION SYSEM FOR A MICRO-UAV 79

80 SCHOOL OF ELECRICAL ENGINEERING - KH Other possie error sttes he error of the inerti dt is not imited to ises. here is numer of other possie error sttes. Among other there might e scing errors of the inerti mesurements. he individu sensor eements sensing cceertion nd ngur rte in different directions might not e perfecty orthogon. If the sensor eements re mounted very cose to ech other there might e cross corretion etween their output. One might so incude the Coriois terms in the inerti nvigtion mode nd the effects of A/D-converter quntistion. he impct of the ddition error terms wi e dependent on sensor dt quity. Proy their impct wi e ow ut for system sensor fexiiity for exmpe scing nd orthogonity errors might e interesting to impement if ess expensive sensors is tried out. 8 JOHN-OLOF NILSSON

81 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Grvity mode he grvittion cceertion tht enters the cceertion mesurements re not constnt in spce. It vries with position. However, the vritions re sm over the opertion rnge of the system. A grvittion vector is ccuted from the initi position t strt-up ut wi fter tht not e chnged. he grvittion mode used ssumes Erth to e homogene spheroid resuting from compressed sphere. he grvity is given s function of titude (φ) nd titude (z). g g g 2 2 ( sin ( φ).58 sin ( 2φ )) z t NAVIGAION SYSEM FOR A MICRO-UAV 81

82 SCHOOL OF ELECRICAL ENGINEERING - KH Dt ogging softwre he dt ogging softwre is written in MSVC++. he progrm is simpe consoe ppiction. he progrm is written in n oject oriented wy with strctions of the system components. he code is composed of 6 csses which re riefy descried eow. For more detis, see code nd comments in code. A the csses descried in the cpp-fies so hve heder-fies. Except from the cpp-fies nd heder-fies there is so one fie for precompied heder prts nd one for resource references. User interfce he user interfce is text sed consoe window. he threds of the ogging progrm dispy ogging informtion nd requests in time order. he initi strt-up procedure for the ogging progrm cn e seen in Figure 45. he strt-up procedure is expined in the cption. he ogging procedure is shown nd expined in Figure 46. Especiy note tht susequent ogging instnces re sved in seprte foder in the foder from where the progrm is run. 82 JOHN-OLOF NILSSON

83 MASER OF SCIENCE HESIS SAAB BOFORS DYNAMICS AB Figure 45. Strt-up procedure: he progrm first identify the frme grer. his identifiction is utomtic. Next the progrm ss out the GPS receiver nd the IMU. Here the user hs to specify the com-port they re setup/connected to, nd if the stndrd seri communiction settings ought to e used. When the connections re estished the progrm smpes gyro mesurement for 1 seconds to cpture the gyro ises. After the strt-up procedure the progrm dispys the commnds to run nd stop the cpturing nd to quit the progrm. he commnds re not cse sensitive. NAVIGAION SYSEM FOR A MICRO-UAV 83

84 SCHOOL OF ELECRICAL ENGINEERING - KH Figure 46. Logging procedure: When [R] is pressed the progrm strts ogging dt from sensors. When [S] is pressed the progrm stops ogging dt nd coses the ogging fies. Ech ogging instnce [R]->[S] is sved in seprte foder. he progrm cn e quit whie running s we s whie stopped. 84 JOHN-OLOF NILSSON