DEPARTAMENTO DE INGENIER´
IA DE SISTEMAS Y AUTOM´
ATICA
ESCUELA SUPERIOR DE INGENIER´
IA
UNIVERSIDAD DE SEVILLA
Long-Te m Localiza ion o
Unmanned Ae ial Vehicles based on
3D En i onmen Pe cep ion
po
F ancisco Ja ie P´e ez G au
Ingenie o de Telecomunicaci´on
PROPUESTA DE TESIS DOCTORAL
PARA LA OBTENCI ´
ON DEL T´
ITULO DE
DOCTOR POR LA UNIVERSIDAD DE SEVILLA
SEVILLA, 2017
Di ec o es
D .-Ing. Fe nando Caballe o Ben´ı ez, P o eso Ti ula
D .-Ing. An´ıbal Olle o Ba u one, Ca ed ´a ico
ii
UNIVERSIDAD DE SEVILLA
Memo ia pa a op a al g ado de Doc o po la Uni e sidad de Se illa
Au o : F ancisco Ja ie P´e ez G au
T´ı ulo: Long-Te m Localiza ion o
Unmanned Ae ial Vehicles based on
3D En i onmen Pe cep ion
Depa amen o: Depa amen o de Ingenie ´ıa de Sis emas y
Au om´a ica
V◦B◦Di ec o :
Fe nando Caballe o Ben´ı ez
V◦B◦Di ec o :
An´ıbal Olle o Ba u one
El au o :
F ancisco Ja ie P´e ez G au
iii
i
A p oblem well pu is hal sol ed.
(John Dewey)

i
Acknowledgemen s
Fi s o all, I would like o since ely hank my supe iso s Fe nando Caballe o and
An´ıbal Olle o o all he suppo , ime, pa ience and eaching ha I ecei ed om
hem. I app ecia e a lo he amusing momen s while de eloping, es ing o w i ing
wi h Fe nando, as well as he e o and inspi ing con ibu ions om An´ıbal owa ds
his disse a ion.
I mus ex end my g a i ude o An idio Vigu ia, since his wo k would no ha e
been possible wi hou his guidance and cons an ad ice. He p o ided me wi h all
he necessa y esou ces o achie e my goals, and has always endea o ed o ensu e
he maximum o e lap be ween his disse a ion and he di e en esea ch p ojec s in
which I ha e pa icipa ed o e he pas yea s.
Du ing my ime as a PhD candida e, I had he oppo uni y o do a esea ch isi
a he Aus alian Cen e o Field Robo ics (ACFR) in Sydney. I wan o hank
Ali Hayda G¨ok oˇgan o his iendly hos ing du ing my s ay, aligning his esea ch
in e es s wi h hose o his disse a ion. I am happy o ha e sha ed hose mon hs wi h
Will Reid a The Ma s Lab, and mos no ably wi h Dan Wilson who emb aced me as
a b o he so a away om home.
Resea ch ac i i ies like he ones ca ied ou owa ds his disse a ion would no
ha e been possible wi hou he g ea eam o people a he Cen e o Ad anced
Ae ospace Technologies (CATEC). I would like o hank my colleagues o hei
kindness, help and encou agemen . The combina ion o excellen p o essional people in
a iendly en i onmen has led o many happy momen s. In pa icula , I am g a e ul
o
´
Angel Pe us and Rica do Ragel o spending coun less hou s es ing he ae ial
pla o m. I am also happy o ha e sha ed he indoo es bed wi h se e al ellows,
ii
especially Miguel
´
Angel T ujillo, who ga e highly app ecia ed cons uc i e eedback o
his wo k in o de o u he imp o e i , apa om his b igh ideas ega ding “s ings
enginee ing” (´
Angel oo!).
And las , bu ce ainly no leas , I would like o hank my pa en s o all hei lo e
and suppo . They ha e enabled me o add ess he subsequen challenges I ha e aced
h oughou my li e. Also, my closes iends who ha e been he e whene e needed,
in good imes and bad. And mos o all Ma ´ıa, my lo ing, encou aging and pa ien
li e pa ne , whose ai h ul suppo du ing he inal s ages o his wo k, su e ing my
uncoun able wo king hou s a nigh s o holidays, is so app ecia ed. Thank you.
Se ille, May 2017.
iii
Abs ac
Unmanned Ae ial Vehicles (UAVs) a e cu en ly used in coun less ci il and comme cial
applica ions, and he end is ising. Ou doo obs acle- ee ope a ion based on Global
Posi ioning Sys em (GPS) can be gene ally assumed hanks o he a ailabili y o
ma u e comme cial p oduc s. Howe e , some applica ions equi e hei use in con ined
spaces o indoo s, whe e GPS signals a e no a ailable. In o de o allow o he
sa e in oduc ion o au onomous ae ial obo s in GPS-denied a eas, he e is s ill a
need o eliabili y in se e al key echnologies o p ocu e a obus ope a ion, such as
localiza ion, obs acle a oidance and planning.
Exis ing app oaches o au onomous na iga ion in GPS-denied a eas a e no obus
enough when i comes o ae ial obo s, o ail in long- e m ope a ion. This disse a ion
handles he localiza ion p oblem, p oposing a me hodology sui able o ae ial obo s
mo ing in a Th ee Dimensional (3D) en i onmen using a combina ion o measu emen s
om a a ie y o on-boa d senso s. We ha e ocused on using h ee ypes o senso
da a: images and 3D poin clouds acqui ed om s e eo o s uc u ed ligh came as,
ine ial in o ma ion om an on-boa d Ine ial Measu emen Uni (IMU), and dis ance
measu emen s o se e al Ul a Wide-Band (UWB) adio beacons ins alled in he
en i onmen . The o e all app oach makes use o a 3D map o he en i onmen ,
o which a mapping me hod ha exploi s he syne gies be ween poin clouds and
adio-based sensing is also p esen ed, in o de o be able o use he whole me hodology
in any gi en scena io.
The main con ibu ions o his disse a ion ocus on a hough ul combina ion o
echnologies in o de o achie e obus , eliable and compu a ionally e icien long-
e m localiza ion o UAVs in indoo en i onmen s. This wo k has been alida ed
ix
x i Con en s
3 Robus Visual Odome y o UAVs 35
3.1 Regis a ion................................ 36
3.1.1 Dep h-only Regis a ion . . . . . . . . . . . . . . . . . . . . . 36
3.1.2 Colo -dep h Regis a ion . . . . . . . . . . . . . . . . . . . . . 45
3.2 Visual-Ine ial Odome y . . . . . . . . . . . . . . . . . . . . . . . . . 53
3.2.1 Fea u e De ec ion . . . . . . . . . . . . . . . . . . . . . . . . . 55
3.2.2 Fea u e Desc ip ion . . . . . . . . . . . . . . . . . . . . . . . . 55
3.2.3 F ameMa ching.......................... 56
3.2.4 A i ude Co ec ion . . . . . . . . . . . . . . . . . . . . . . . . 61
3.2.5 Key-F aming ........................... 61
3.2.6 Spa se Bundle Adjus men . . . . . . . . . . . . . . . . . . . . 62
3.2.7 G ound Plane Es ima ion . . . . . . . . . . . . . . . . . . . . 64
3.2.8 Expe imen al Resul s . . . . . . . . . . . . . . . . . . . . . . . 65
3.3 Conclusions ................................ 71
4 Mul i-Modal Senso Fusion o Long-Te m Localiza ion 73
4.1 S a eEs ima ion ............................. 74
4.2 Gaussian Fil e s o S a e Es ima ion . . . . . . . . . . . . . . . . . . 76
4.2.1 P edic ion............................. 80
4.2.2 Upda e............................... 80
4.2.3 Ou lie Rejec ion . . . . . . . . . . . . . . . . . . . . . . . . . 81
4.2.4 Expe imen al Resul s . . . . . . . . . . . . . . . . . . . . . . . 81
4.3 Pa icle Fil e s o S a e Es ima ion . . . . . . . . . . . . . . . . . . . 85
4.3.1 Ini ializa ion............................ 87
4.3.2 P edic ion............................. 89
4.3.3 Upda e............................... 90
4.3.4 Re-sampling............................ 93
4.3.5 Pose Compu a ion . . . . . . . . . . . . . . . . . . . . . . . . 93
4.3.6 Expe imen al Resul s . . . . . . . . . . . . . . . . . . . . . . . 94
4.4 Conclusions ................................ 105

Con en s x ii
5 Mul i-Modal Mapping 107
5.1 Range-only localiza ion and mapping (s ep 1) . . . . . . . . . . . . . 108
5.1.1 P oblem De ini ion . . . . . . . . . . . . . . . . . . . . . . . . 109
5.1.2 Op imiza ion ........................... 111
5.1.3 Ini ializa ion............................ 113
5.1.4 Weigh ing ............................. 113
5.2 3D Mapping and Pose Re inemen (s ep 2) . . . . . . . . . . . . . . . 114
5.3 Expe imen al Resul s . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
5.3.1 Mapping Expe imen . . . . . . . . . . . . . . . . . . . . . . . 117
5.3.2 Localiza ion Expe imen . . . . . . . . . . . . . . . . . . . . . 119
5.4 Conclusions ................................ 122
6 Sys em A chi ec u e and F amewo k 123
6.1 UAVPla o m............................... 127
6.2 Con olled es s.............................. 129
6.3 EuRoCBenchma king .......................... 130
6.4 EuRoCF ee-S yle............................. 133
6.5 EuRoCShowcase ............................. 137
6.5.1 Au onomous Deli e y Sys em . . . . . . . . . . . . . . . . . . 140
6.5.2 Missing I em De ec ion . . . . . . . . . . . . . . . . . . . . . . 141
6.6 Conclusions ................................ 143
7 Discussion and Conclusions 145
7.1 Conclusions o his Disse a ion . . . . . . . . . . . . . . . . . . . . . 145
7.2 LessonsLea ned.............................. 146
7.3 Fu u eWo k................................ 147
Re e ences 149
x iii Con en s
Lis o Figu es
1.1 GPS-based ae ial su ey in a e ine y. . . . . . . . . . . . . . . . . . . 4
1.2 Vicon-based indoo es bed a CATEC. . . . . . . . . . . . . . . . . . 7
1.3
3D localiza ion and mapping using a hand-held RGB-D came a and
RTAB-Map................................. 11
1.4 6D localiza ion o humanoid obo s based on AMCL. . . . . . . . . . 14
1.5 Thesisou line................................ 19
2.1 Sample colo image (le ) and i s associa ed dep h image ( igh ). . . . 23
2.2 S e eo ision p inciple. . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.3 Skybo ix’s VI-Senso Schnei h (2014). . . . . . . . . . . . . . . . . . . 26
2.4 RGB-D came a (ASUS’s X ion PRO LIVE) Asus (2017). . . . . . . . 29
2.5 O bbec’s As a O bbec (2017). . . . . . . . . . . . . . . . . . . . . . . 30
2.6 Nano on’s swa m ER Nano on (2017). . . . . . . . . . . . . . . . . 33
3.1 Scaled scena io o a landing si e o es ing dep h-only egis a ion. . 40
3.2 UAV used o es ing dep h-only egis a ion. . . . . . . . . . . . . . 41
3.3 UAV lying o e he as e oid scaled model and sample 3D da a. . . . 41
3.4
Pose es ima ion o he descen ajec o y compa ed o g ound- u h
da a..................................... 43
3.5
Pose es ima ion o he ho e ing ajec o y compa ed o g ound- u h
da a. (Le ) Posi ion plo s. (Righ ) O ien a ion plo s. . . . . . . . . . 44
3.6 Schema ic o e iew o he RGB-D egis a ion pipeline. . . . . . . . . 47
3.7 Pixel compa isons o de e mine he exis ence o a FAST key-poin . . 48
xix
xx Lis o Figu es
3.8 MAMMOTH o e wi h an RGB-D senso on op. . . . . . . . . . . . 50
3.9
Map gene a ed a e d i ing he MAMMOTH o e o e a sec ion o
heMa sYa d. .............................. 50
3.10 Sequence o images om he ial. . . . . . . . . . . . . . . . . . . . . 51
3.11 Posi ion and o ien a ion plo s o he MAMMOTH o e om he ial. 52
3.12 Schema ic o e iew o ou isual-ine ial odome y pipeline. . . . . . 54
3.13
Fea u es de ec ed om he o iginal image ( op) on a sample image
be o e (bo om le ) and a e (bo om igh ) applying he bucke ing
echnique. ................................. 56
3.14 Fea u e ma ching be ween le and igh images om he s e eo pai . 57
3.15 Bundle adjus men p ojec ion example. . . . . . . . . . . . . . . . . . 62
3.16 CATEC es bed wi h a mockup scena io. . . . . . . . . . . . . . . . . 65
3.17 The UAV wi h he RGB-D senso a he on . . . . . . . . . . . . . . 66
3.18 G ound- u h UAV ajec o y in XY du ing he expe imen . . . . . . 66
3.19 Es ima ed UAV posi ion and o ien a ion. . . . . . . . . . . . . . . . . 67
3.20 Localiza ion e o s in UAV es ima ion. . . . . . . . . . . . . . . . . . 69
3.21 Es ima ed UAV posi ion and o ien a ion using o he app oaches. . . . 70
4.1 Schema ic o e iew o he EKF app oach. . . . . . . . . . . . . . . . 79
4.2
UAV wi h he VI-senso a he on (le ) and g ound- u h ajec o y
in heXYplane( igh ). ......................... 82
4.3
Es ima ed UAV localiza ion (g ound- u h in ed, isual odome y in
g een, p oposed app oach in blue). . . . . . . . . . . . . . . . . . . . 83
4.4 Localiza ion e o s in posi ion and associa ed RMS e o . . . . . . . . 84
4.5
Au oma ic ini ializa ion o pa icles. F om le o igh and op o
bo om, ime e olu ion o pa icles a e au oma ic ini ializa ion. Black
a ows ep esen he pa icles. . . . . . . . . . . . . . . . . . . . . . . 88
4.6
CATEC indoo es bed ( op) and 3D map wi h app oxima e adio
beacons loca ions(bo om) used o ield expe imen s. . . . . . . . . . 95
4.7 UAV wi h an RGB-D senso a he on . . . . . . . . . . . . . . . . . 96
4.8
Ano he RGB-D senso wi h passi e ma ke s o p ecise 3D map building.
96
Lis o Figu es xxi
4.9
G ound- u h ajec o y ollowed by he ae ial obo du ing he expe i-
men ..................................... 97
4.10
Localiza ion esul s (posi ion and yaw) showing he g ound- u h, isual
odome y and he p oposed app oach. . . . . . . . . . . . . . . . . . . 98
4.11
Posi ion and o ien a ion e o s wi h espec o g ound- u h h ough
ime..................................... 99
4.12
Es ima ed UAV posi ion and o ien a ion using o he app oaches based
onRGB-Dsenso s. ............................ 100
4.13 Localiza ion e o s o di e en alues o α................ 102
4.14 Localiza ion e o s o di e en numbe o pa icles used. . . . . . . . 103
4.15
3D map o he a ea wi h di e en esolu ions: 0.2m (le ) and 0.4m
( igh ).................................... 104
4.16 Localiza ion e o s o di e en OcT ee esolu ions o he 3D map. . . 105
5.1 Mul iple hypo heses o he localiza ion o h ee UWB beacons. . . . 112
5.2
The UAV wi h RGB-D senso s a he on (le ) and he ea ( igh )
side, and a UWB senso on op. . . . . . . . . . . . . . . . . . . . . . 116
5.3
The indoo es bed a CATEC wi h a mock-up scena io o 3D map
building................................... 116
5.4 Resul s o he i s s ep: Range-only localiza ion and mapping. . . . . 117
5.5 Resul s o he second s ep: 3D Mapping and Pose Re inemen . . . . . 118
5.6 Top iew o he g ound- u h map (le ) and econs uc ed map ( igh ).119
5.7 Es ima ed UAV posi ion and yaw angle o he long- e m ligh . . . . 120
5.8 3D isualiza ion o senso da a, along wi h he pa icles ( ed cloud). . 121
5.9
E o s in he es ima ed UAV posi ion and yaw angle using he g ound-
u h map and he econs uc ed map. . . . . . . . . . . . . . . . . . 122
6.1 Schema ic o e iew o he p oposed a chi ec u e. . . . . . . . . . . . 124
6.2
The wo main senso s es ed on-boa d he UAV: VI-Senso (le ) and
As a( igh ). ............................... 128
6.3
A sample o 3D p in ed moun o he main ision-based senso on-boa d
heUAV. ................................. 129

xxii Lis o Figu es
6.4 One o he es ing scena ios in CATEC’s indoo es bed. . . . . . . . 130
6.5 ETH’s lying a ena used in he Benchma king and F ee-S yle ounds. 131
6.6
The UAV pe o ming obs acle de ec ion and a oidance o a human
wo ke (le ) and ano he smalle UAV ( igh ). . . . . . . . . . . . . . 134
6.7
Obs acle de ec ion and ajec o y eplanning esul s in he mannequin
( op) and mul i-UAV (bo om) expe imen s. . . . . . . . . . . . . . . 134
6.8
Es ima ed UAV posi ion and o ien a ion in he mannequin ( op) and
mul i-UAV (bo om) expe imen s. . . . . . . . . . . . . . . . . . . . . 136
6.9
Ai bus D&S manu ac u ing plan ( op) and eplica ed en i onmen a
CATEC(bo om). ............................ 138
6.10 RGB-D senso s on-boa d he UAV and signaling ligh s. . . . . . . . . 138
6.11 UWB beacons ins alled in he indoo es bed. . . . . . . . . . . . . . 139
6.12
Small ca go bay on he UAV (le ) and hoppe o au onomous deli e y
( igh ).................................... 140
6.13
Es ima ed UAV posi ion and o ien a ion in he au onomous deli e y
expe imen . ................................ 141
6.14
Es ima ed UAV posi ion and o ien a ion in he missing i em de ec ion
expe imen . ................................ 142
Chap e 1
In oduc ion
1.1 Mo i a ion
1.1.1 The Localiza ion P oblem
Imagine you a e a home pleasan ly wa ching a mo ie on a Sa u day nigh , and
suddenly he powe goes ou . You cell phone is no a hand so you need o go check
you uses in comple e da kness. E en hough you know exac ly how o each ha
place in you house, and you hink ha he s eps you make a e aking you he e
p ope ly, you will mo e likely bump in o nea by obs acles such as ables o doo s.
Mos o us ely on ou eyes o help us ind ou way. By using he in o ma ion om
ou eyes and o he senses, we ge an idea o whe e we a e in he wo ld, and hus can
co ec he inhe en impe ec ions o ou mo emen s. Mobile obo s su e om he
same issues when a e sing he wo ld. I hey only mo e a ound wi hou looking a
whe e hei mo emen s a e aking hem, impe ec ions in hei mo ing mechanisms
will ge hem los . Jus like humans, sensing he en i onmen can help hem de ec
hese impe ec ions and ge a be e idea o whe e hey a e.
Besides mobili y, au onomy is ano he key ea u e ha enables obo s o ope a e
e ec i ely in complex en i onmen s and pe o m asks wi hou explici human in e -
en ion. Sensing he en i onmen allows an au onomous obo o decide he execu ion
o speci ic ac ions acco ding o he da a ha is being ga he ed. Bu be o e doing
1
2In oduc ion
so, he obo needs o be capable o sel -localizing in i s en i onmen . Localiza ion
is one o he basic pilla s o au onomous mobile obo ics. The localiza ion p oblem
is essen ial o building a mobile obo ic sys em, since accu a e pose es ima ion is
equi ed o e en he mos basic asks, e en holding he posi ion. I is commonly
e e ed o as “ he mos undamen al p oblem o p o iding a mobile obo wi h
au onomous capabili ies” Cox (1991). Once his is achie ed, he es o echnologies
such as na iga ion and guidance can be implemen ed in o de o de e mine whe e he
goals a e and how o each hem.
1.1.2 Ae ial Robo s: De ini ions and Ca ego ies
Nowadays, he ae ial ehicles popula ly known as “d ones” a e no un amilia o any
o us. Though o en associa ed wi h mili a y ac i i y, he e is also keen in e es in
many ci ilian applica ions. Appa en ly, his is he las example o mili a y echnology
ans e ed and made a ailable o ci ilian use, such as he In e ne o Global Posi-
ioning Sys em (GPS). These pla o ms ha e b ough a e olu ion in almos e e y
ield, la gely due o he dec easing cos o echnology and he ac ha hey ha e
dis inc unc ional ad an ages wi h espec o manned a ia ion. Indus y expe s
indica e ha his echnology has been he mos dynamic g ow h sec o o he ae ospace
indus y in he pas decade Ca oukian (2012). They a e o en p esen in he news on
ele ision, newspape s, adio o social ne wo ks, used by domes ic law en o cemen ,
he p i a e sec o o ama eu en husias s. Fac o s like he abili y o apidly explo ing
la ge and/o inaccessible a eas, he educ ion o ma e ial cos s as well as pe sonnel
cos s, he p ocess au oma ion and he educ ion o wo king imes make hem sui able
o many uses in indus ial, go e nmen al and academic ields. They a e al eady
being used in a a ie y o applica ions, and many mo e a eas will bene i by hei
use Jenkins and Vasigh (2013). Among hese a eas, we can ind wild i e mapping
Me ino e al. (2012), ag icul u al moni o ing Saa i e al. (2011), disas e managemen
Maza e al. (2011), powe line su eys Wang e al. (2010), law en o cemen Pu i
(2005), elecommunica ion Zhan e al. (2011), wea he moni o ing Re e comb e al.
(1996), ae ial imaging/mapping Nex and Remondino (2014), ele ision news co e age
1.1 Mo i a ion 3
CNN (2016), spo ing e en s Pede sen and Cooke (2006), mo ie-making Lin and
Yang (2014), en i onmen al moni o ing Ace edo e al. (2013), oil and gas explo a ion
Hausamann e al. (2005), e c.
Common ac onyms o en used a e UAV, UAS, RPA o RPAS; howe e , hey do no
mean he same. “D one” is he popula denomina ion in he media, and s a ed ou
as a mili a y e m; ne e heless, i is no commonly applied by specialized pe sonnel
in esea ch ac i i ies. Fo ease o cla i y, we will b ie ly discuss he meanings o he
main ac onyms in o de o p ope ly in oduce he speci ic ca ego y used h oughou
his hesis. Acco ding o he In e na ional Ci il A ia ion Au ho i y (ICAO) Ca y
(2011), he main e minology is explained as ollows:
•
UAV (Unmanned Ae ial Vehicle): an ai c a which is in ended o ope a e wi h
no pilo on boa d.
•
UAS (Unmanned Ai c a Sys em): i is he UAV and i s associa ed elemen s
which a e ope a ed wi h no pilo on boa d, i.e. communica ion link, g ound
con ol s a ion, e c.
•
RPA (Remo ely Pilo ed Ai c a ): an ai c a whe e he lying pilo is no on
boa d (i is a subca ego y o UAV).
•
RPAS (Remo ely Pilo ed Ai c a Sys em): i is he RPA and i s associa ed
emo e pilo s a ion, he equi ed command and con ol links and any o he
sys em elemen s as may be equi ed, a any poin du ing ligh ope a ion.
To summa ize, he UAV (o UAS) is any ai c a (o sys em) in which he pilo is
no physically on boa d he pla o m. In he case o RPA (o RPAS), exp ess e e ence
is made o he exis ence o a pilo who emo ely ope a es he ai c a ; whe eas he
de ini ion o UAV (o UAS) lea es he op ion o ca ying ou he ligh , o pa s o i ,
as a ully au onomous ope a ion. Hence all RPAS a e UAS, bu no all UAS a e RPAS.
Tha said, since his wo k ocuses on he au onomous ope a ion o ae ial ehicles, we
will e e o hem as UAV o UAS, depending on he e e ence o he ae ial pla o m
o he comple e sys em. In he scope o his disse a ion, we will also e e o UAVs
as ae ial obo s.
10 In oduc ion
Many o he algo i hms de eloped o s e eo- ision odome y and SLAM can
be applied o 3D came as based on pa e n p ojec ion, i.e. Red,G een,Blue-Dep h
(RGB-D) came as End es e al. (2012); Ke l e al. (2013). These senso s ha e ecen ly
become a e y popula op ion due o hei low weigh , low cos and he amoun o
in o ma ion p o ided; apa om RGB images hey di ec ly p o ide dep h images o
he scene in on o he senso , sa ing he bu den o 3D econs uc ion compu a ion
(as in s e eo- ision sys ems). Besides, hey exhibi ano he impo an ad an age wi h
espec o classic s e eo-based app oaches: dep h es ima ion does no depend on he
p esence o dis inc isual ea u es in he scene in o de o es ima e he dep h.
The a ailabili y o RGB-D came as has made dense 3D poin clouds a ailable,
which we e p e iously only accessible using much mo e expensi e senso s like Time o
Fligh came as o scanning 3D lase ange inde s. The e a e se e al s a e-o - he-a
open-sou ce algo i hms ha p o ide localiza ion es ima ions based on RGB-D came as,
such as RGBD-SLAM End es e al. (2012), Ci y College o New Yo k (CCNY) RGB-D
D yano ski e al. (2013) o Real-Time Appea ance-Based Mapping (RTAB-Map)
Labbe and Michaud (2014) (see Figu e 1.3). Ne e heless, hey a e a ely employed
o on-line compu a ion du ing he UAV ligh , since hese senso s ypically gene a e
oluminous da a ha ypically canno be p ocessed in i s en i e y in eal ime (e.g.
he Mic oso Kinec senso p oduces o e 9 million 3D poin s pe second). The ac
ha he ci ed SLAM algo i hms a e con inuously building hei maps on-line g ea ly
inc eases hei compu a ional equi emen s, which limi hei usabili y and obus ness
gi en he usual on-boa d es ic ions in e ms o payload capaci y and compu a ional
esou ces when wo king wi h UAVs.
Poin cloud based odome y and localiza ion is ano he signi ican esea ch a ea.
Al hough compu a ionally expensi e, ecen ad ances in
kd
- ees Nuch e e al. (2007)
and App oxima e Nea es Neighbou s (ANN) Ma den and Gui an (2012) enable
as e compu a ion imes in he p esence o medium and dense poin clouds. The
p oblem o aligning a pai o poin clouds is commonly known as egis a ion. I s
ou pu is usually a ans o ma ion ma ix ep esen ing he o a ion and ansla ion
ha would ha e o be applied on one o he clouds in o de o be pe ec ly aligned
wi h he o he . In o de o do his, se e al echniques use a landma k-based app oach,

1.2 Localiza ion o UAS in GPS-denied A eas 11
Figu e 1.3: 3D localiza ion and mapping using a hand-held RGB-D came a and
RTAB-Map.
geome ically compu ing an es ima e o he ans o ma ion based on he ecogni ion
o dis inc ea u es in he en i onmen , occu ing na u ally o a i icially placed Rusu
e al. (2009). The ac o s con ibu ing o he success ul pe o mance and in eg i y
o hese me hods is he eliable acquisi ion and ex ac ion o ea u es om senso
da a, and he abili y o e icien ly ecognize and associa e such ea u es. This can be
challenging depending on he spa ial esolu ion and noise p esen in he dep h senso
measu emen , apa om he compu a ional cos o analyzing dense 3D da a. Ano he
amily o app oaches is based on da a co ela ion, a emp ing o u ilize wha e e
senso da a a e a ailable o compu e he ans o ma ion. This elimina es he need
o decide wha cons i u es a ea u e, and uses a maximum likelihood alignmen o
ind he bes i be ween wo se s o da a poin s. I e a i e Closes Poin (ICP) is a
me hod capable o p o iding a compu a ionally e icien pose es ima ions in complex,
uns uc u ed en i onmen s. ICP has he ad an ages o locally sol ing he p oblem o
ma ching and localiza ion, being gene ic and po en ially used o eal- ime applica ions
Pome leau e al. (2013).
12 In oduc ion
In gene al, ision-based odome y and localiza ion sys ems o ae ial obo s a e no
eliable enough in he long e m due no only o cumula i e d i , bu also ex e nal
ac o s such as poo illumina ion, lack o ex u e, occlusions o mo ing objec s. All
hese ha e a signi ican impac on he obus ness and eliabili y o mos s a e-o -
he-a algo i hms. In any case, he a o emen ioned app oaches demons a e ai ly
good esul s in he sho e m; howe e , hey could quickly di e ge depending on he
en i onmen .
Au onomous sys ems in ended o ope a e o e long pe iods o ime usually pe o m
some so o loop closing in o de o ecognize e isi ed places. This allows educing he
localiza ion unce ain y a he cos o adding compu a ional complexi y S¨unde hau
and P o zel (2011); Low y e al. (2016). Howe e , he p oblem o loop closing in
o de o dis inguish e isi ed places is usually amed as a classi ica ion ask, a he
han a obo localiza ion ask Angeli e al. (2008). Reliable place ecogni ion can be
challenging in la ge-scale en i onmen s o si es wi h epe i i e s uc u es ha migh
exhibi simila scenes in di e en a eas. This is a e y delica e issue since a single
w ong loop closu e can esul in a de as a ing ailu e o he localiza ion sys em, which
could lead o an undesi ed ligh e mina ion.
1.2.2 Map-based app oaches
The o he la ge amily o localiza ion echniques elies on p e iously buil maps o he
en i onmen . Some UAS applica ions a e usually ca ied ou in known en i onmen s,
e.g. logis ic se ices o pos -disas e assessmen s Ezequiel e al. (2014). In hese cases,
he ae ial obo is equi ed o ca y ou speci ic ajec o ies o each ce ain places.
This could be also a equi emen o pa h planning in o de o speci y goals in a
p ede ined coo dina e sys em.
These app oaches use ea u es such as he lines o planes ha desc ibe walls in
hallways o o ices o build a map o he en i onmen . Senso da a can be ma ched
wi h such ea u es in o de o de e mine he UAV loca ion. P obably he mos common
map ep esen a ions a e occupancy g ids, which we e i s in oduced se e al decades
ago Mo a ec and El es (1985). These g ids a e a p obabilis ic app oach o ep esen
1.2 Localiza ion o UAS in GPS-denied A eas 13
he en i onmen in disc e e cells ha indica e hei p obabili y o being occupied
by an obs acle. A g ea ad an age is ha hey do no ely on speci ic p ede ined
ea u es, and a e able o ep esen unknown a eas. Howe e , an impo an d awback
o his app oach is i s la ge memo y equi emen , bu ecen de elopmen s such as
Oc oMap Ho nung e al. (2013) p o ide e icien da a s uc u es pa icula ly sui ed
o obo ics applica ions in he cons ained equipmen usually ound on-boa d UAVs
Nieuwenhuisen e al. (2014); D oeschel e al. (2016).
One o he app oaches based on a p ede ined model o he en i onmen is commonly
known as each- eplay Chen and Bi ch ield (2006); Roye e al. (2007), which is
accomplished in wo s ages: i s he obo is manually pilo ed along he desi ed
pa h as in a eaching phase, and an accu a e 3D map o he en i onmen is buil ,
along wi h he obo mo ion om his lea ning pa h; a e wa ds, his map is used o
loca e he obo when i epea edly isi s he same pa h. Howe e , his app oach is
somewha simplis ic and limi ed, since i only enables a obo o ollow a p ede e mined
ajec o y.
Mon e Ca lo Localiza ion (MCL) is ano he app oach ha makes use o a known
map o he en i onmen , and is one o he mos popula algo i hms used o obo
na iga ion in indoo en i onmen s Th un e al. (2001). I is a p obabilis ic localiza ion
algo i hm ha makes use o a pa icle il e o es ima e he pose o he obo wi hin
he map, based on senso measu emen s. Impo an bene i s include he possibili y o
accommoda ing a bi a y senso cha ac e is ics, mo ion dynamics and noise dis ibu-
ions. In o de o ob ain a eliable localiza ion esul , a ce ain numbe o pa icles
will be needed. The la ge he en i onmen is, he mo e pa icles a e needed. Ac ually,
each pa icle can be seen as a pseudo- obo , which pe cei es he en i onmen using a
p obabilis ic measu emen model. A each i e a ion, he i ual measu emen akes
la ge compu a ional cos s i he e a e hund eds o pa icles. Fo ha eason, he e is
a a ian o MCL called Adap i e MCL (AMCL) Fox (2001). The e m “adap i e”
comes om he ac ha he numbe o pa icles is adjus ed dynamically: i he e is
high unce ain y abou he obo pose, he numbe o pa icles inc eases; i he pose is
well known, such numbe dec eases. Expe imen al app oaches o ei he MCL o AMCL
exhibi some limi a ions when i comes o ae ial obo s. Due o he a o emen ioned
14 In oduc ion
Figu e 1.4: 6D localiza ion o humanoid obo s based on AMCL.
compu a ional equi emen s, mos o he exis ing app oaches a e mean o wheeled
obo s mo ing in a 2D en i onmen , equi ing a 2D lase scanne o map building and
localiza ion. O he au ho s p esen ed an ex ension o his app oach o 6D localiza ion
based on 2D lase scanne Ho nung e al. (2010), bu i is mean o 2D mo ion o
humanoid obo s in a 3D en i onmen (see Figu e 1.4), which makes i no sui able
o UAVs.
1.3 Con ibu ions
The a o emen ioned app oaches a e p omising in ha hey can all p o ide solu ions o
he localiza ion p oblem, bu impo an d awbacks a e p esen using each app oach
alone. In as uc u e-based posi ioning sys ems achie e eliable pe o mances, bu
hey can be expensi e and migh equi e labo ious se up and calib a ion p ocesses.
Vision-based app oaches based on na u al landma ks demons a e good esul s elying
only on on-boa d equipmen , bu a e no obus enough when applied o UAVs and
especially o long- e m ope a ion (i.e. long ligh ime). Map-based me hods a e
obus solu ions o long- e m localiza ion, bu o en demand a high compu a ional
cos and hey need o p e iously build an accu a e ep esen a ion o he en i onmen .
1.3 Con ibu ions 15
The main con ibu ion o his wo k ocuses on he combina ion o echnologies in
o de o achie e long- e m au onomous ope a ion o UAVs in indoo en i onmen s,
aking ad an age o hei espec i e bene i s o o e come hei main limi a ions. This is
accomplished by using da a om di e en senso s in o de o imp o e he pe o mance
o he o e all sys em. In pa icula , a isual odome y algo i hm based on s e eo o
RGB-D came as and a localiza ion algo i hm based on UWB senso beacons ha e
been me ged in o an enhanced MCL algo i hm which elies on a p e iously buil
mul i-modal map ha includes 3D occupancy da a and he loca ion o he UWB
beacons. Speci ic con ibu ions in each ield a e lis ed below, along wi h ele an
ela ed publica ions.
•
Resea ch and de elopmen o a obus isual odome y app oach sui able o 3D
senso s, which p o ides a eliable sho - e m pose es ima ion. Valida ion o he
app oaches using bo h ae ial and g ound obo s.
–
F.J. Pe ez, J. Gil, G. Bine and A. Vigu ia, “Valida ion o 3D En i onmen
Pe cep ion o Landing on Small Bodies using UAV Pla o ms”, 13 h
Symposium on Ad anced Space Technologies in Robo ics and Au oma ion
(ASTRA 2015), ESA/ESTEC, Noo dwijk, The Ne he lands, 2015. Link.
–
W. Reid, F. J. Pe ez-G au, A. H. G¨ok oˇgan and S. Sukka ieh, “Ac i ely
a icula ed suspension o a wheel-on-leg o e ope a ing on a Ma ian
analog su ace”, 2016 IEEE In e na ional Con e ence on Robo ics and
Au oma ion (ICRA), S ockholm, Sweden, 2016, pp. 5596-5602. doi: 10.1109
/ ICRA.2016.7487777
–
F. J. Pe ez-G au, R. Ragel, F. Caballe o, A. Vigu ia and A. Olle o, “Semi-
Au onomous Teleope a ion o UAVs in Sea ch and Rescue Scena ios”, 2017
In e na ional Con e ence on Unmanned Ai c a Sys ems (ICUAS), Miami,
FL, USA, 2017. Accep ed o publica ion.
•
Resea ch and de elopmen o mul i-modal senso usion me hods ha combine
he a o emen ioned isual odome y wi h o he sou ces o 3D measu emen s,
namely adio- ange sensing and poin clouds, o long- e m localiza ion. The

16 In oduc ion
noise and ou lie s om adio measu emen s a e il e ed hanks o he odom-
e y es ima ions, while he odome y d i is bounded hanks o adio-based
measu emen s and poin cloud ma ching.
–
F. J. Pe ez-G au, F. R. Fab esse, F. Caballe o, A. Vigu ia and A. Olle o,
“Long- e m ae ial obo localiza ion based on isual odome y and adio-
based anging”, 2016 In e na ional Con e ence on Unmanned Ai c a
Sys ems (ICUAS), A ling on, VA, USA, 2016, pp. 608-614. doi: 10.1109 /
ICUAS.2016.7502653
–
F. J. Pe ez-G au, F. Caballe o, A. Vigu ia and A. Olle o, “Mul i-Senso 3D
Mon e Ca lo Localiza ion (MCL) o Long-Te m Ae ial Robo Na iga ion”,
In e na ional Jou nal o Ad anced Robo ics Sys ems (IJARS). Accep ed
o publica ion.
•
Resea ch and de elopmen o a mul i-modal map building algo i hm ha exploi s
he syne gies be ween adio-based dis ance es ima ions and poin clouds om
3D imaging senso s, which equi es a minimum se up.
–
F. J. Pe ez-G au, F. Caballe o, L. Me ino and A. Vigu ia, “Mul i-Modal
Mapping and Localiza ion o Unmanned Ae ial Robo s based on Ul a-
Wideband and RGB-D sensing”, 2017 IEEE/RSJ In e na ional Con e ence
on In elligen Robo s and Sys ems (IROS). Unde e iew.
•
De elopmen o a modula and ex ensible so wa e a chi ec u e o sa e and
eliable au onomous na iga ion o ae ial obo s in GPS-denied en i onmen s,
alida ed du ing ex ensi e ield es ing h oughou di e en expe imen cam-
paigns and demons a ions in he con ex o na ional and Eu opean Union (EU)
unded esea ch p ojec s.
–
F. J. Pe ez-G au, R. Ragel, F. Caballe o, A. Vigu ia and A. Olle o, “An
A chi ec u e o Robus UAV Na iga ion in GPS-denied A eas”, Jou -
nal o Field Robo ics (JFR), Special Issue on High Speed Vision-Based
Au onomous UAVs. Accep ed o publica ion.
1.4 Thesis F amewo k 17
•
Highly e icien implemen a ion o all he algo i hms in o de o make hem
sui able o eal- ime on-line localiza ion in he usually cons ained equipmen
ha can be moun ed on-boa d a UAV.
–
Video showing on-line localiza ion o he au onomous ope a ion o a UAV
using he p oposed a chi ec u e and algo i hms. EuRoC Challenge 3 –
Team GRVC-CATEC – S age IIb (Showcase).
I is impo an o poin ou he s ong expe imen al ocus o his disse a ion,
whose main con ibu ions ha e been ocused no only on ad ancing in he s a e-o -
he-a , bu also on implemen ing and alida ing di e en app oaches in eal-wo ld
se ups. P oo o he po en ial impac o his wo k ega ding i s cu en echnology
de elopmen and u u e ans e o he indus y is i s ecen ecogni ion wi hin he
1s EU D one Awa ds, o ganized by he Eu opean Young Inno a o s Fo um a he
Eu opean Pa liamen in Janua y 2017. A special inno a i e p ize in he ca ego y
“Bes D one-based Solu ion” was awa ded o he applica ion o indoo localiza ion o
UAVs o logis ic ope a ions in ai c a manu ac u ing plan s EYIF (2017), whe e he
sys em de eloped wi hin his disse a ion is used.
1.4 Thesis F amewo k
The esea ch leading o hese esul s has ecei ed suppo om he Spanish Cen o
pa a el Desa ollo Tecnol´ogico Indus ial (CDTI) INNPRONTA 2011-2014 p og am
wi hin Pe igeo p ojec , and he Eu opean Communi y’s Se en h F amewo k P og am
(FP7) p ojec EuRoC (FP7-ICT-608849) in he pe iod 2014-2017.
This disse a ion has been ex ensi ely alida ed wi hin he con ex o he Eu opean
Robo ics Challenges (EuRoC)
1
, an EU FP7 p ojec whose main mo i a ion is o b ing
inno a i e echnologies om esea ch labs o indus ial end-use s. In o de o do
ha , a se ies o challenges in a public compe i ion o ma we e p esen ed, and one o
hem (Challenge 3) is ela ed o he demons a ion o high-le el semi-au onomous
ope a ion o a UAV o an inspec ion ask. The goal is o enable unskilled wo ke s
1h p://www.eu oc-p ojec .eu
18 In oduc ion
o pe o m complex inspec ion missions wi h he aid o UAVs. This is di icul o
achie e as he complexi y o UAVs equi es expe pilo ing skills. In his con ex , a
amewo k has been de eloped in o de o pe o m localiza ion and s a e es ima ion
o he UAV wi hou ex e nal posi ioning sys ems such as GPS o mo ion cap u ing
sys ems, as well as au onomous local obs acle a oidance, local pa h planning, ollowing
o s uc u es o homing o he ae ial obo .
The au ho is pa o he challenge eam GRVC-CATEC, which is cu en ly com-
pe ing among op Eu opean esea ch ins i u ions, and has success ully demons a ed
accu a e localiza ion o he UAV pla o m wi hin he p ojec .
1.5 Thesis Ou line
The di e en chap e s o his documen desc ibe he e olu ion o he de eloped sys em
along he yea s o wo k owa ds his disse a ion. The e was an inc emen al s a egy
based on he accomplishmen o in e media e de elopmen objec i es, all o hem
aiming a he o e all goal o achie ing long- e m localiza ion o UAVs in GPS-denied
a eas, keeping in mind sa e y and obus ness as lagship ea u es o he success ul
widesp ead use o au onomous ae ial obo s. Following his inc emen al app oach, he
chap e s a e s uc u ed as ollows:
•
Chap e 2 discusses se e al sensing modali ies ha can be used h oughou he
di e en de elopmen s.
•
Chap e 3 de ails he inc emen al wo ks owa ds a ision-based odome y algo-
i hm using images, 3D poin clouds and ine ial measu emen s.
•
Chap e 4 p esen s he e olu ion o ou app oach o combine isual odome y
wi h adio-based measu emen s om UWB beacons and a 3D map o he
en i onmen .
•
Chap e 5 desc ibes a 3D map building me hod in o de o be able o use he
p e iously desc ibed localiza ion sys em in any en i onmen .
1.5 Thesis Ou line 19
•
Chap e 6 in oduces an o e iew o he whole amewo k in which his wo k
has been es ed and alida ed o au onomous na iga ion o UAVs in EuRoC
p ojec .
•Chap e 7 includes conclusions, lessons lea ned and u u e lines o wo k.
Figu e 1.5 shows a pic o ial ep esen a ion o he hesis ou line, p esen ing how
chap e s a e ela ed wi h each o he .
Chap e 1
In oduc ion
Chap e 2
3D Pe cep ion
o Localiza ion
Chap e 3
Robus Visual Odome y
o UAVs
Chap e 4
Mul i-Modal Senso
Fusion o Long-Te m
Localiza ion
Chap e 5
Mul i-Modal Mapping
Chap e 6
Sys em A chi ec u e
and F amewo k
Chap e 7
Discussion and
Conclusions
Expe imen al Resul s
6DoF Pose
3D MapOdome y
Senso s
On-line
localiza ion
Figu e 1.5: Thesis ou line.
26 3D Pe cep ion o Localiza ion
Figu e 2.3: Skybo ix’s VI-Senso Schnei h (2014).
he came as). Gi en he es ic ions in came a sizes ha can be moun ed on-boa d a
small UAV, applicable baselines can handle a ela i ely limi ed dep h ange.
The s e eo- ision senso used in some s ages o his wo k is he Visual-Ine ial
(VI-) Senso , shown in Figu e 2.3, which p o ides ully ime-synch onized and ac o y
calib a ed IMU- and s e eo-came a da a s eams. In pa icula , s e eo- ision has been
used as he main odome y sou ce in some o he expe imen s included in Chap e s 3
and 6.
2.1.2 Ac i e Imaging
LIDAR
P obably he mos common senso s based on ac i e op ical me hods o ob aining
dep h in o ma ion a e LIgh De ec ion And Ranging (LIDAR) ins umen s. They
a e based on he well-known ime o ligh p inciple, i.e. he dis ance o an objec
is measu ed om he ime i akes a ligh pulse o a el om he emi e un il i
e u ns e lec ed om he objec . Hence an ac i e ligh sou ce is equi ed, o en in he
IR equency ange. Since all elec omagne ic ene gy a els a he speed o ligh
c
, in
ee space, he ela ionship be ween he dis ance
Z
and he ound- ip a el ime
is
gi en by
Z=c·
2(2.2)
LIDAR sys ems p o ide high p ecision measu emen s a high da a a es; howe e ,
hei high cos and ela i ely high weigh , o he non-simul aneous poin s acquisi ion

2.1 Op ical Sensing 27
(poin s a e g abbed one a e ano he , no simul aneously), limi hei widesp ead use
in some applica ions in ol ing UAV pla o ms. Fo hese easons, his disse a ion
has no ocused on his ype o ac i e imaging echnique.
ToF came as
Ligh pulses a e no he only me hod o measu e ime o ligh . By using a modula ed
signal, dis ances can be calcula ed by es ima ing he phase di e ence be ween he
emi ed and he e lec ed signal. The dis ance Zis hen ob ained as ollows:
Z=c
2· m·φ
2π(2.3)
whe e
c
is again he speed o ligh ,
m
is he modula ion equency o he emi ed
signal and φis he es ima ed phase shi be ween he emi ed and ecei ed signal.
Due o he pe iodici y o he modula ion signal, Equa ion 2.3 is only alid o
dis ances smalle han
c
2· m
. The modula ion equency
m
o he emi ed signal
de e mines he “ambigui y- ee” dis ance ange o he senso .
The las decade has seen an inc easing end in he de elopmen o 3D came as.
Time o Fligh (ToF) came as a e a ela i ely new ype o senso ha deli e s 3D
imaging a a high ame a e, simul aneously p o iding in ensi y da a and ange
in o ma ion o e e y pixel. Nea -IR modula ed ligh wa es a e emi ed by se e al
Ligh -Emi ing Diodes (LEDs) and e lec ed by he objec s on he scene back o he
image . Con en ional imaging senso s consis o mul iple pho o-diodes a anged in
a ma ix. No mally, hese diodes p o ide a g ay-scale o colo image o he scene.
In con as o no mal came as, a Pho on Mixing De ice (PMD) senso addi ionally
acqui es a dis ance alue o each pixel simul aneously o he in ensi y (g ay) alue.
Despi e his ema kable p og ess, i is s ill made wi h s anda d Complemen a y Me al-
Oxide-Semiconduc o (CMOS) echnology. The e o e, he pixels in hese came as a e
o en called “sma pixels” Xu e al. (1998).
Compa ed o o he echnologies ha ob ain 3D in o ma ion, ToF came as allow
he acquisi ion o dep h images wi hou any scanning mechanism and om jus one
poin o iew. They egis e dense dep h along wi h in ensi y images a a high ame
28 3D Pe cep ion o Localiza ion
a e. They a e small, low-weigh and compac , since no mobile pa s a e needed. They
ha e low cos s compa ed o LIDAR sys ems, and a lowe powe consump ion wi h
espec o classical lase scanne s. Ano he impo an cha ac e is ic o hese de ices
is hei abili y o ope a e in en i onmen s whe e o he sys ems would be unable o do
so. Fo example in un ex u ed scena ios, whe e he ope a ion o s e eo- ision sys ems
would be e y di icul due o he lack o ep esen a i e ea u es on he images o sol e
he co esponding p oblem.
Impo an disad an ages o hese sys ems a e hei high sensi i i y o noise, espe-
cially in ou doo scena ios due o in e e ence wi h di ec sunligh , and hei low-dep h
measu emen accu acy. This is due mainly o he way in which ToF came as g ab
da a, which consis s in he scene being bomba ded wi h nea -IR ligh , cap u ing a
whole su ace included in o he emi ed ligh cone in one single sho . This di e s om
lase ange inde s, which acqui e poin s sequen ially wi h e y high accu acy. These
de ices also p o ide e y low esolu ion (no mo e han a ew housands o ens o
pixels) compa ed o cu en s anda d came as.
Ne e heless, when i comes o moun ing a senso on-boa d a UAV pla o m,
addi ional conside a ions and equi emen s mus be aken in o accoun . Especially
impo an a e he weigh , size and powe consump ion o he senso , since he payload
capaci y o small UAVs is e y limi ed. Due o hese easons, he p e e ed sensing
modali y o ob aining dense 3D dep h images is based on ano he ype o ac i e
imaging echnique known as s uc u ed ligh .
S uc u ed ligh
Senso s based on s uc u ed ligh simpli y he solu ion o he co espondence p oblem
in oduced in s e eo- ision echniques. They eplace he second came a in he s e eo
se up by a ligh sou ce which p ojec s a known pa e n o ligh on he scene. I he
scene is simply a plana su ace, he pa e n acqui ed by he i s came a will be
simila o he p ojec ed pa e n. When he su ace is non-plana , he geome ic shape
o he su ace dis o s he p ojec ed pa e n. The shape o his su ace can hen be
ob ained based on he in o ma ion om he dis o ion o he p ojec ed pa e n. S ill,
some co espondences be ween bo h pa e ns mus be sol ed. Di e en p ojec ion
2.1 Op ical Sensing 29
Figu e 2.4: RGB-D came a (ASUS’s X ion PRO LIVE) Asus (2017).
pa e ns ha e been p oposed such as bina y codes o colo -coded s ipes Geng (2011).
The mos usual pa e n is he p ojec ion o a g id, in which an easie co espondence
p oblem has o be sol ed. In his case, we only need o iden i y, o each poin o he
cap u ed pa e n, he co esponding poin o he p ojec ed pa e n.
Apa om ToF came as, a new ype o senso s commonly e e ed o as RGB-D
came as can p o ide bo h isual ex u e in o ma ion and pe -pixel dep h in o ma ion
simul aneously. They ha e wo came as: he i s is usually a con en ional webcam
ha eco ds colo ideo, and he second is an IR came a ha eco ds a non- isible
s uc u ed ligh pa e n gene a ed by he IR p ojec o (see Figu e 2.4).
Mic oso ’s Kinec was p obably he i s a o dable RGB-D came a widely used in
obo ics esea ch Boudji e al. (2008). The pe -pixel dep h-sensing echnology ha is
used in consume RGB-D came as was de eloped and pa en ed by P imeSense Ga cia
and Zale sky (2008). The dep h acquisi ion echnology is named Ligh Coding; i has
an IR ligh sou ce o p ojec a complex pa e n o do s in o he scene. This pa e n is
pe cei ed by an IR came a and he dis ance o each do is compu ed by iangula ion
o build a 3D model o he scene. The colo in o ma ion o his model is ga he ed by
an RGB came a.
Rega dless o ex u e and illumina ion condi ion, an RGB-D came a can di ec ly
ob ain 3D in o ma ion, unlike s e eo- ision due o he lack o ep esen a i e ea u es
in he images. Abo e all, hey a e small, low-weigh and compac , since no mobile
30 3D Pe cep ion o Localiza ion
Figu e 2.5: O bbec’s As a O bbec (2017).
pa s a e in ol ed. Besides, hey ha e much lowe cos and lowe powe consump ion
when compa ed o LIDAR sys ems and ToF came as.
One impo an limi a ion o RGB-D came as is ha hey can only ope a e eliably
indoo s, since he p ojec ed pa e n is o e whelmed by ex e io ligh ing condi ions.
Ano he d awback is he measu emen noise when compa ed o LIDAR senso s, and
hei limi ed wo king ange (up o 10m). Ne e heless, RGB-D senso s ha e seen
widesp ead adop ion in obo ics esea ch due o hei abili y o gene a e eliable 3D
da a a a as ame a e a low cos Han e al. (2013).
Rega ding on-boa d equi emen s o he UAV pla o m, subsequen models o
RGB-D came as om o he endo s exhibi g ea ad an ages wi h espec o Mic oso
Kinec : hey a e signi ican ly smalle , easie o in eg a e and do no equi e an ex e nal
powe supply. These aspec s make i much mo e po able and sui able o small ae ial
ehicles. Examples o hese o he came as a e ASUS’s X ion PRO LIVE (shown in
Figu e 2.4), and mo e ecen ly O bbec’s As a (see Figu e 2.5) which exhibi s a longe
dep h ange (up o 10m ins ead o 4m). These wo came as ha e been used in he
expe imen s o his disse a ion, he X ion PRO LIVE in Chap e 3 and he As a in
Chap e s 4, 5 and 6.
Table 2.1 summa izes he main speci ica ions o he p e iously discussed RGB-D
came as.
2.2 Non-op ical Sensing 31
Table 2.1: RGB-D came as compa ison
Speci ica ions Kinec X ion PRO LIVE As a
Size (mm) 305 x 63 x 76 180 x 35 x 50 165 x 30 x 40
Weigh (g) 1320 540 300
Range (m) 0.8 - 4 0.8 - 3.5 0.6 - 8
Dep h Image Size 640 x 480 640 x 480 640 x 480
RGB Image Size 640 x 480 640 x 480 640 x 480
F ames pe second 30 30 30
Field o View (◦) 57 x 43 58 x 45 60 x 49.5
Powe Ex e nal USB USB
2.2 Non-op ical Sensing
As s a ed ea lie in his chap e , non-op ical sensing e e s o he use o pulses o
wa es no included in he isible o he IR spec um o ob aining 3D measu emen s.
This sec ion ocuses on mic owa es, in pa icula on UWB, which is he echnology
ha has been used in his wo k.
UWB is a high da a a e, low powe sho - ange wi eless echnology ha is
gene a ing a lo o in e es in he esea ch communi y and he indus y, as a high-
speed al e na i e o exis ing wi eless echnologies such as IEEE 802.11 WLAN, Home
Radio F equency (RF) and Hipe LANs Lad (2004). E en hough UWB has been
a ound o mo e han 40 yea s, a subs an ial change occu ed in 2002, when he
Fede al Communica ion Commission (FCC) issued a epo allowing he comme cial
and unlicensed deploymen o UWB wi h a gi en spec al mask o bo h indoo and
ou doo applica ions in Uni ed S a es. This wide equency alloca ion ini ia ed a lo
o esea ch ac i i ies om bo h indus y and academia, ocusing in ecen yea s on
consume elec onics and wi eless communica ions.
UWB ansmi s bina y da a, using low ene gy and ex emely sho du a ion
impulses o bu s s (in he o de o picoseconds) o e a wide spec um o equencies. I
deli e s da a o e 15 o 100 me e s and does no equi e a dedica ed adio equency,

32 3D Pe cep ion o Localiza ion
so is also known as ca ie - ee, impulse o base-band adio. UWB sys ems use ca ie -
ee, meaning ha da a is no modula ed on a con inuous wa e o m wi h a speci ic
ca ie equency, as in na owband and wideband echnologies.
UWB echnology has he ollowing signi ican cha ac e is ics Lad (2004):
•High da a a e
: i can handle mo e bandwid h-in ensi e applica ions like
s eaming ideo, han ei he 802.11 o Blue oo h, eaching da a a es o oughly
100 Megabi s pe second (Mbps), wi h speeds up o 500 Mbps. The maximum
speed o 802.11a is 54 Mbps, while o Blue oo h i is abou 1 Mbps.
•Low powe consump ion: i cons an ly ansmi s sho impulses, ins ead o
ansmi ing modula ed wa es like mos na owband sys ems do, and hence does
no need con e sion be ween equencies, local oscilla o s, mixe s, and o he
il e s.
•In e e ence immuni y
: due o low powe and high equency ansmission,
UWB’s agg ega e in e e ence is aguely de ec ed by na owband ecei e s. This
makes i sui able o coexis ence wi h na owband adio sys ems ope a ing in
he same spec um wi hou causing undue in e e ence.
•High secu i y
: since UWB sys ems’ noise is e y low, hey a e inhe en ly
co e and ex emely di icul o unin ended use s o de ec .
•Low complexi y, low cos
: he mos a ac i e o UWB’s ad an ages a e i s
low sys em complexi y and cos . T adi ional ca ie based echnologies modula e
and demodula e complex analog ca ie wa e o ms. Due o he absence o ca ie
in UWB, he anscei e s uc u e can be e y simple. Besides, ecen ad ances
in silicon p ocess and swi ching speeds make UWB sys ems also low-cos .
•Resis ance o jamming
: UWB spec um co e s a huge ange o equencies.
Tha is why i s signals a e ela i ely esis an o jamming, because i is e y di -
icul o jam e e y equency in he UWB spec um a he same ime. The e o e,
he e is a wide equency ange a ailable e en in he case ha some equencies
a e jammed.
2.2 Non-op ical Sensing 33
Figu e 2.6: Nano on’s swa m ER Nano on (2017).
•Scalabili y
: UWB sys ems a e e y lexible because hei common a chi ec u e
is so wa e e-de inable, so ha i can dynamically ade-o high-da a h oughpu
o ange Chong e al. (2006).
UWB signaling is especially sui able o posi ion and anging applica ions due o i s
low ene gy, high bandwid h and ine empo al esolu ion p ope ies. I allows accu acies
o a ew cen ime e s in anging, as well as low-powe and low-cos implemen a ion o
communica ion sys ems Gezici e al. (2005). The p ocess in ol es exchange o signals
be ween nodes, and measu emen o pa ame e s o es ima e dis ances.
Dis ance measu emen s be ween wo adio-based senso s a e usually based on
he ene gy (signal s eng h) o a el imes ( ime o a i al) o signals be ween he
de ices. To de e mine he dis ance om ene gy measu emen s, he cha ac e is ics o
he channel mus be known; he e o e his echnique is e y sensi i e o he es ima ion
o hose pa ame e s. In ha sense, he measu emen s o a el imes is p e e ed. I
he wo senso s ha e a common clock, he senso ecei ing he signal can de e mine he
ime o a i al o he incoming signal ha is ime-s amped by he emi e senso . In
he absence o a common clock be ween he senso s, ound- ip imes can be measu ed
in one o he senso s o es ima e he dis ance be ween he wo.
The UWB senso s used in his wo k a e manu ac u ed by Nano on Technologies
wi hin hei swa m p oduc amily, which p o ide accu a e and ela i ely low-cos
loca ion capabili ies. These UWB senso s p o ide dis ance measu emen s wi h an
accu acy o 10 cm. A sample boa d is shown in Figu e 2.6.
34 3D Pe cep ion o Localiza ion
2.3 Conclusions
This chap e summa izes di e en 3D sensing modali ies ha a e conside ed ele an
o he pu pose o localiza ion o ae ial obo s in GPS-denied a eas. They ha e
been classi ied acco ding o how hey ob ain dep h in o ma ion, whe he i is a
simple poin - o-poin dis ance es ima ion o a ull dep h image. Di e en senso s
ha e been discussed, highligh ing hei main ad an ages and limi a ions ega ding
hei applicabili y o small UAVs. Taking in o accoun hese ac o s and on-boa d
equi emen s, he mos in e es ing selec ed de ices o be es ed a e s e eo came as,
RGB-D came as and UWB senso s.
Chap e 3
Robus Visual Odome y o UAVs
The i s s ep o he wo k ca ied ou owa ds his disse a ion was ob aining localiza ion
es ima ions h ough he analysis o da a p o ided by a 3D imaging senso . Du ing he
pas decades, di e en echniques ha e been p oposed in o de o ackle his es ima ion
p oblem, gene ally by p ocessing senso da a acqui ed a subsequen ime ins an s.
Vision-based odome y add esses he p oblem o es ima ing he mo ion o a obo by
only using in o ma ion om ision senso s Sca amuzza and F aundo e (2011).
One o he mos popula echniques is known as egis a ion, which is he p ocess
by which wo da a se s a e b ough in o alignmen . In pa icula , when dealing wi h
3D imaging senso s, i in ol es aligning 3D poin clouds. Bo h passi e and ac i e
op ical senso s, such as s e eo came as o RGB-D came as, can p o ide dense 3D poin
clouds a a high equency. Then, subsequen poin clouds can be ma ched in o de o
deduce he ans o ma ion be ween hem and, consequen ly, he 6 Deg ees-o -F eedom
(DoF) mo ion o he senso .
Apa om ha , many s a e-o - he-a app oaches a e based on ex ac ing in e es
poin s om RGB images, and ma ching hem wi h hose ex ac ed in p e ious
ames. By using he 3D in o ma ion associa ed wi h such poin s, he ans o ma ion
be ween ames can be es ima ed, and hence he mo ion o he obo F aundo e and
Sca amuzza (2012).
In he p esen ed app oach, egis a ion was i s implemen ed, using only 3D poin
clouds o es ima e he obo localiza ion. Then, his p ocessing pipeline was op imized
35
42 Robus Visual Odome y o UAVs
G ound- u h localiza ion da a o compa ison we e acqui ed by he mo ion cap u e
sys em o CATEC’s indoo es bed. In bo h expe imen s, he UAV au oma ically
ollowed a p ede ined lis o waypoin s using he mo ion cap u e sys em o close he
con ol loop, and hen he poin clouds we e p ocessed o -line in o de o ob ain he
localiza ion es ima ions shown in he igu es. Only he ini ial g ound- u h pose o
he UAV was used in o de o p ope ly ini ialize he egis a ion algo i hm.
Figu e 3.4 shows he esul s o he descen ajec o y in posi ion (
x, y, z
) and
o ien a ion ( oll, pi ch, yaw), including bo h he g ound u h alues (g X, g Y, g Z,
g ROLL, g PITCH and g YAW) and he egis a ion algo i hm ou pu s (es X, es Y,
es Z, es ROLL, es PITCH and es YAW). E o s in posi ion a e ela i ely small
(
<
10cm), as well as in o ien a ion (
<
0
.
2
◦
) du ing he whole ajec o y. In he case o
descen ajec o ies, ini ial e o s in he poin cloud ma ching p ocess end o con e ge.
The o e all map ha he algo i hm is con inuously building usually ep esen s he
same su ace o he small body. As he UAV ge s close o he as e oid model, he
poin clouds desc ibe his su ace wi h mo e de ail, leading o be e esul s in he
ma ching p ocess.
E en hough he esul s a e al eady p omising, he e is s ill signi ican oom o
imp o emen . The o e all map ha he algo i hm con inuously builds is ini ialized a
he beginning o he ajec o y, when he ae ial obo is a he u hes dis ance om
he scaled as e oid su ace. A such dis ance (a ound 3m), possible UAV ib a ions
and 3D senso noise g ea ly a ec he ini ial poin cloud compa isons, hus making
he ma ching p ocess much mo e challenging han when he UAV is close o objec s
in gene al.
Figu e 3.5 shows he esul s o he ho e ing ajec o y also in posi ion and o ien a-
ion. E o s in posi ion a e simila han hose in he case o he descen ajec o y.
In his case, he UAV ligh heigh was app oxima ely 1m abo e he scaled as e oid,
which allowed he 3D senso o p o ide dep h measu emen s wi hou much noise.
Howe e , a ce ain momen s, he su ace egion acqui ed by he senso appea s mainly
la ; hence he poin cloud ma ching can be ambiguous since he e a e no enough
dep h de ails ha could help he ma ching p ocess.

3.1 Regis a ion 43
Figu e 3.4: Pose es ima ion o he descen ajec o y compa ed o g ound- u h da a.
Table 3.1 summa izes he oo -mean-squa e (RMS) e o s o pose es ima ions o
bo h ajec o ies, in posi ion and o ien a ion.
44 Robus Visual Odome y o UAVs
Figu e 3.5: Pose es ima ion o he ho e ing ajec o y compa ed o g ound- u h
da a. (Le ) Posi ion plo s. (Righ ) O ien a ion plo s.
The esul s show he good pe o mance o he p oposed algo i hm, bu a numbe
o sho comings p e en his se up om cons i u ing a gene ic solu ion o long- e m
UAV localiza ion:
3.1 Regis a ion 45
Table 3.1: RMS localiza ion e o s
x(m) y(m) z(m) oll (◦)pi ch (◦)yaw (◦)
Descen ajec o y 0.024 0.042 0.063 0.007 0.002 0.111
Ho e ing ajec o y 0.027 0.023 0.009 0.004 0.02 0.003
•
The dis ance ha he UAV a eled in he expe imen s was ela i ely sho
(<5m).
•
The eloci y a which he UAV pe o med he expe imen s was low (0
.
05m/s)
in o de o ec ea e scaled-down space explo a ion ajec o ies.
•
Senso da a was unde used. Apa om 3D poin clouds, he RGB-D senso
p o ided colo images ha we e no used in o de o ai h ully emula e he Flash
LIDAR senso unde es . Besides, o ien a ion da a om he on-boa d IMU
we e no used ei he .
Ne e heless, hese expe imen s helped o s a he de elopmen o he so wa e
amewo k o he es o echnologies owa ds his disse a ion, as well as gaining
ield expe ience in he se up o indoo ligh es ing.
3.1.2 Colo -dep h Regis a ion
3D imaging senso s usually p o ide no only a poin cloud o he scene, bu also hei
associa ed colo images. I seems easonable o make use o such images, since adding
RGB in o ma ion o he so wa e p ocessing pipeline can imp o e he eliabili y o he
egis a ion p ocess. In his way, we can ake ull ad an age o all he in o ma ion ha
he senso p o ides (ei he a s e eo o RGB-D came a), bo h isual and dep h da a.
O e he las decades, di e en ICP a ian s ha e been in oduced Rusinkiewicz
and Le oy (2001) p oposing imp o emen s in any o mo e o he s ages o he algo i hm,
namely:
•Selec ing some se o poin s in one o bo h poin clouds.
46 Robus Visual Odome y o UAVs
•Ma ching he poin s om one cloud o samples in he o he poin clouds.
•Weighing he co esponding pai s app op ia ely.
•
Rejec ing ce ain pai s based on looking a each pai indi idually o conside ing
he en i e se o pai s.
•Assigning an e o me ic based on he poin pai s.
•Sol ing he op imiza ion p oblem.
In o de o educe he compu a ional cos o ou p ocessing pipeline, we ha e
ocused on op imizing he i s s age o he ICP algo i hm, i.e. selec ing in e es poin s
on bo h he eading and he e e ence poin clouds. A each ame, a se o isual
ea u es is ex ac ed om he colo image and mapped o hei 3D loca ions using
he associa ed poin cloud. The se o 3D poin s is hen ma ched ac oss consecu i e
ames o es ima e senso pose inc emen s since he las p ocessed ame. Figu e 3.6
shows an o e iew o he me hod.
The in e es poin s a e ex ac ed using he Fea u es om he Accele a ed Segmen
Tes (FAST) algo i hm Ros en and D ummond (2006), which is a compu a ionally
e icien me hod o co ne de ec ion. Acco ding o his algo i hm, a pixel is de ined
as a key-poin i in a ci cle su ounding he pixel,
N
o mo e con iguous pixels a e all
signi ican ly b igh e han, o all signi ican ly da ke han he cen e pixel, as depic ed
in Figu e 3.7. The ex ac ed key-poin s a e pixels which con ain local in o ma ion
ha ideally makes hem epea able ac oss consecu i e ames.
Depending on he en i onmen , he scene in on o he senso could be ea u e-less,
o he e migh be s ong-de ailed egions whe e all he key-poin s a e de ec ed. Bo h
cases pose p oblems in he subsequen s ages o he egis a ion me hod and may lead
o poo ans o ma ion es ima ions. In o de o o e come such issues, a bucke ing
echnique has been adop ed. The image is di ided in o se e al egions, and a ixed
numbe o key-poin s is equi ed o be ex ac ed om each one. Ou app oach is
based on subdi iding he image in o six bucke s ( wo columns and h ee ows), hence
p o iding a ela i ely homogeneous dis ibu ion o key-poin s in he image.
3.1 Regis a ion 47
Figu e 3.6: Schema ic o e iew o he RGB-D egis a ion pipeline.
The p oposed app oach is no pu ely based on isual ea u e ma ching hough, in
he sense ha i does no y o ind he same key-poin s ac oss consecu i e RGB
images. Ins ead, he se o 2D key-poin s based on FAST ea u es is enhanced wi h
dep h in o ma ion o u n i in o a se o 3D key-poin s. This is pe o med by di ec ly
using hei co esponding dep h alues om he associa ed poin cloud (in he case
o s e eo came as, om he dispa i y map compu ed om he pai o images). Then,
he s a egy is o align consecu i e se s o 3D key-poin s, assuming ha he selec ed
FAST ea u es a e epe i i e enough o be ound in consecu i e ames.
3D imaging senso s p o ide dep h in o ma ion o mos o he image pixels, bu
no all o hem. This is a common si ua ion in bo h s e eo and RGB-D came as.

48 Robus Visual Odome y o UAVs
Figu e 3.7: Pixel compa isons o de e mine he exis ence o a FAST key-poin .
Depending on he scene and he physical p ope ies o he ma e ials in he en i onmen ,
a 2D key-poin migh no ha e a co esponding dep h alue. Mo eo e , a 2D key-poin
migh co espond o a e y a loca ion, hus he noise o he dep h measu emen
can be signi ican . In hese cases, ou app oach ejec s he 2D key-poin s om he
ea u e se . The ac ha we migh be h owing away possibly “good” key-poin s
does no signi ican ly impac on he pe o mance o he algo i hm, since he size o
he key-poin s se s is ela i ely la ge (se e al hund eds o poin s).
Subsequen ames a e analyzed in o de o ob ain se s o 3D key-poin s. Once a
se o 3D key-poin s has been il e ed, an alignmen p ocess is ca ied ou o ind he
bes i be ween wo se s. As in dep h-only egis a ion, he compa ed se s do no
co espond wi h consecu i e ames in o de o mi iga e he d i e ec commonly
p esen in odome y app oaches. The se s o align a e he cu en ame and he
o e all map ha is con inuously being buil using he aligned 3D poin s. The o e all
goal is s ill o apply a ans o ma ion o one se o b ing i as close as possible o he
o he , and ICP is used again o ind such ans o ma ion. In his case, due o he
spa se dis ibu ion o he 3D key-poin s, he poin - o-poin e o me ic is used in he
i e a i e p ocess:
E(T) =
N
X
i=1 kTui−zik(3.1)
3.1 Regis a ion 49
whe e
N
is he size o he cu en se o 3D key-poin s
ui
,
zi
is he co esponding poin
in he map poin cloud, and
T
is he ans o ma ion ma ix composed o a o a ion
and ansla ion.
ICP’s compu a ional load is g ea ly dec eased wi h espec o ou p e ious app oach
by educing he size o he poin clouds when only using he 3D key-poin s om bo h
da a se s. O he main d awbacks o ICP include i s inabili y o deal wi h noise and
ou lie s in he poin clouds, o he absence o enough o e lap be ween he clouds. Bo h
cases a e co e ed in he p oposed app oach, since he 3D key-poin ex ac ion p ocess
ocuses on emo ing noise om dis an poin s and possible ou lie s. Addi ionally, he
high ame a e o 3D imaging senso s acili a es high o e lap be ween he cu en
poin cloud and he o e all map ha is being buil .
The senso pose upda e is inally ans o med o he obo body ame in o de o
ob ain a localiza ion pose upda e.
Expe imen al Resul s
Colo -dep h egis a ion esul s we e ob ained in he con ex o a esea ch s ay
pe o med a he Aus alian Cen e o Field Robo ics (ACFR) a he Uni e si y o
Sydney. RGB images and 3D poin clouds a e acqui ed om an RGB-D senso acing
sligh ly down on-boa d he Ma s Analog Mul i-Mode T a e se Hyb id (MAMMOTH)
o e Reid e al. (2014), shown in Figu e 3.8. E en hough he obo ic pla o m is no
a UAV, he accu acy o he localiza ion algo i hm was success ully alida ed h ough
an ac i e a icula ion s a egy o he o e , which elied on he co ec on-boa d
on-line obo localiza ion and a spa se map building in o de o es ima e each wheel
con ac poin in he e ain. The o e can hen a icula e i s limb join s in o de o
ac i ely con o m o he e ain while a e sing ough a eas.
G ound- u h localiza ion da a o di ec compa ison we e a ailable only o
o ien a ion hanks o an on-boa d IMU; ne e heless, he ollowing esul s desc ibe he
success ul o e all pe o mance o he app oach in bo h posi ion and o ien a ion. An
example e ain poin cloud gene a ed om consecu i e ames is shown in Figu e 3.9,
o which accu a e localiza ion is i s needed in o de o pe o m p ope egis a ion
o he 3D poin clouds.
50 Robus Visual Odome y o UAVs
Figu e 3.8: MAMMOTH o e wi h an RGB-D senso on op.
Figu e 3.9: Map gene a ed a e d i ing he MAMMOTH o e o e a sec ion o he
Ma s Ya d.
To alida e he ac i ely a icula ed suspension echnique based on he RGB-D
senso based localiza ion desc ibed be o e, he MAMMOTH o e is d i en ac oss a
s ep obs acle and ins uc ed o main ain a cons an o ien a ion and linea eloci y
while d i ing o wa d (along he
x
-axis ela i e o he wo ld ame). The e ain ha
he MAMMOTH o e a e ses is a he Sydney Powe house Museum’s Ma s Ya d,
3.1 Regis a ion 51
Figu e 3.10: Sequence o images om he ial.
a 7x17 m space designed o esemble a sec ion o he Columbia Hills egion isi ed
by he “Spi i ” Ma s Explo a ion Ro e om he Na ional Ae onau ics and Space
Adminis a ion (NASA).
The MAMMOTH o e mo es o e a bed o ocks ha su ounds a 0
.
15m all
cinde block, shown in Figu e 3.10. In all ials, he o e ini ially d i es o e a 4m
sec ion o ela i ely la e ain so as o allow each o i s wheels o be wi hin he
mapped e ain egion. Once all wheels a e wi hin his egion, he ac i e a icula ion
con olle is ac i a ed by an ope a o . The desi ed a es o be kep o each ial a e
ze o ansla ional eloci y along he
y
and
z
axes, and ze o o a ional eloci y o
oll, pi ch and yaw angles. In he ial shown in he plo s, he commanded speed o
he o e is
x
= 0
.
05
m/s
. The wo ld ame is de ined a he base o he o e a
he beginning o i s a e se. Ini ially, he
x
and
y
posi ions o he wo ld ame a e
coinciden wi h body ame’s
x
and
y
posi ions. The
z
posi ion o he wo ld ame is
a he a e age ini ial heigh o each o he wheel con ac ames ela i e o he body
ame.
58 Robus Visual Odome y o UAVs
i.e. he es ima ion o a came a pose (6 DoF: o a ion and ansla ion) wi h espec o
a coo dina e ame in which 3D poin s and hei 2D p ojec ions in he came a a e
p o ided, gi en he came a calib a ion pa ame e s. The 3D poin s a e he coo dina es
o he key-poin s om he p e ious ame, while he 2D p ojec ions a e hei ma ched
co espondences in he cu en ame. Hence he esul ing pose p o ides an es ima ion
o he came a mo ion be ween bo h ames.
Gi en a se o
n
3D poin s in a wo ld e e ence ame and hei co esponding 2D
image p ojec ions, as well as he calib a ed in insic came a pa ame e s, he pose o
he came a wi h espec o he wo ld ame is calcula ed as ollows. The pe spec i e
model o he came a is:
s pc=K[R| ]pw(3.2)
whe e
pw
= [
x y z
1]
T
is he homogeneous wo ld poin ,
pc
= [
u
1]
T
is he co esponding
homogeneous image poin ,
K
is he ma ix o in insic came a pa ame e s (which
a e
x
and
y
o he scaled ocal leng hs,
γ
o he skew and (
u0, 0
) is he p incipal
poin ),
s
is a scale ac o o he image poin , and
R
and
a e he desi ed 3D o a ion
and 3D ansla ion o he came a (ex insic pa ame e s) ha a e o be es ima ed.
This leads o he ollowing equa ion o he model:
s



u
1




=



xγ u0
0 y 0
0 0 1







11 12 13 1
21 22 23 2
31 32 33 3










x
y
z
1






(3.3)
The assump ion made in mos solu ions is ha he came a is al eady calib a ed.
Fo each solu ion o P
n
P, he chosen poin co espondences canno be coplana .
In addi ion, P
n
P can ha e mul iple solu ions, and choosing a pa icula solu ion
would equi e pos -p ocessing o he solu ion se . Fu he mo e, using mo e poin
co espondences can educe he impac o noisy da a when sol ing he p oblem.
A commonly used solu ion o he P
n
P p oblem exis s o
n
= 3, which is called
Pe spec i e-Th ee-Poin (P3P) Gao e al. (2003). Howe e , wi h only 3 co espon-
dences, P3P yields many solu ions, so a ou h co espondence is used in p ac ice

3.2 Visual-Ine ial Odome y 59
o emo e ambigui y. Le
P
be he cen e o p ojec ion o he came a,
A
,
B
and
C
he 3D wo ld poin s wi h co esponding image poin s
u
,
and
w
. Le
X
=
|PA|
,
Y
=
|PB|
,
Z
=
|PC|
,
α
=
∠BPC
,
β
=
∠APC
,
γ
=
∠APB
,
p
= 2
cosα
,
q
= 2
cosβ
,
= 2
cosγ
,
a0
=
|AB|
,
b0
=
|BC|
and
c0
=
|AC|
. This o ms iangles
PBC
,
PAC
and
PAB om which we ob ain he equa ion sys em o P3P:
Y2+Z2−Y Zp −b02= 0 (3.4)
Z2+X2−XZq −c02= 0 (3.5)
X2+Y2−XY p −a02= 0 (3.6)
I is common o no malize he image poin s be o e sol ing P3P. Sol ing he P3P
sys em esul s in ou possible solu ions o
R
and
T
. The a o emen ioned ou h
wo ld poin
D
and i s co esponding image poin
z
a e hen used o ind he bes
solu ion among he ou .
As p e iously men ioned, using mo e poin co espondences helps o educe he
impac o noisy da a, which will gene ally be ou case when using 3D imaging senso s
on-boa d UAVs. In pa icula , he E icien P
n
P (EP
n
P) algo i hm Lepe i e al.
(2008) has been used in ou app oach, which p o ides an e icien implemen a ion o
sol ing he P
n
P p oblem o
n≥
3. This me hod is based on he no ion ha each o
he n poin s (which a e called e e ence poin s) can be exp essed as a weigh ed sum o
ou i ual con ol poin s. Thus, he coo dina es o hese con ol poin s become he
unknowns o he p oblem. I is om hese con ol poin s ha he inal pose o he
came a is sol ed.
As an o e iew o he p ocess, i s no e ha each o he
n
e e ence poin s in he
wo ld ame,
pw
i
, and hei co esponding image poin s,
pc
i
, a e weigh ed sums o he
ou con ols poin s,
cw
j
and
cc
j
(
j
= 1
...
4) espec i ely, and he weigh s a e no malized
60 Robus Visual Odome y o UAVs
pe e e ence poin as shown below. All poin s a e exp essed in homogeneous o m.
pw
i=
4
X
j=1
αijcw
j(3.7)
pc
i=
4
X
j=1
αijcc
j(3.8)
4
X
j=1
αij = 1 (3.9)
F om his, he de i a ion o he image e e ence poin s becomes
sipc
i=K
4
X
j=1
αijcc
i(3.10)
The homogeneous image con ol poin has he o m
cc
j
=
hxc
jyc
jzc
jiT
. Re-
a anging he image e e ence poin equa ion yields he ollowing wo linea equa ions
o each e e ence poin :
4
X
j=1
αij xxc
j+αij(u0−ui)zc
j= 0 (3.11)
4
X
j=1
αij yyc
j+αij( 0− i)zc
j= 0 (3.12)
Using hese wo equa ions o each o he
n
e e ence poin s, he sys em
Mx
= 0
can be o med whe e
x
=
hccT
1ccT
2ccT
3ccT
4iT
. The solu ion o he con ol poin s
exis s in he null space o Mand is exp essed as
x=
N
X
i=1
βi i(3.13)
whe e
N
is he numbe o null singula alues in
M
and each
i
is he co esponding
igh singula ec o o
M
.
N
can ange om 1 o 4. A e calcula ing he ini ial
3.2 Visual-Ine ial Odome y 61
coe icien s
βi
, he Gauss-New on algo i hm is used o e ine hem. The
R
and
T
ma ices ha minimize he ep ojec ion e o o he wo ld e e ence poin s,
pw
i
, and
hei co esponding ac ual image poin s pc
i, a e hen calcula ed.
This solu ion has
O
(
n
) complexi y and wo ks in he gene al case o P
n
P o bo h
plana and non-plana con ol poin s.
3.2.4 A i ude Co ec ion
The P
n
P p oblem es ima es a 6 DoF mo ion be ween ames, including ansla ion
and o a ion. Since he UAV coun s wi h an on-boa d IMU, ou odome y sys em is
able o ead i s da a and in eg a e he oll and pi ch angles wi h he pu ely isual
es ima ion be o e and a e he P
n
P algo i hm. They a e i s in oduced in he
algo i hm o p o ide an ini ial es ima ion o he o a ion ha ook place be ween
he ames unde compa ison. A e wa ds, hey a e used again o compensa e o he
inal angles ha he algo i hm ou pu s.
3.2.5 Key-F aming
In o de o pa ially mi iga e he e ec o cumula i e e o s usually ound in odome y
app oaches, a key- aming app oach has been adop ed. Thus, new key- ames a e
p oduced when he ea u e acking low exceeds a gi en h eshold. This is a measu e
o how much he image shi ed since he las c ea ion o a key- ame, by compa ing
he pixel posi ions o he key-poin s.
A ha momen , he cu en image key-poin s desc ip o s, hei 3D es ima ed
posi ion and he obo posi ion/o ien a ion a e s o ed. Subsequen ames will compu e
he ans o ma ion wi h espec o he las key- ame (and i s pose) ins ead o he
immedia ely p eceding image, so ha e o s a e only accumula ed wi h he in oduc ion
o new key- ames, ins ead o wi h each ame.
62 Robus Visual Odome y o UAVs
Figu e 3.15: Bundle adjus men p ojec ion example.
3.2.6 Spa se Bundle Adjus men
In gene al, Bundle Adjus men (BA) is he p oblem o e ining a isual econs uc ion,
and is almos in a iably used as he las s ep o e e y ea u e-based mul iple- iew
econs uc ion ision algo i hm o ob ain join ly op imal 3D s uc u e and iewing
pa ame e (came a pose and/o calib a ion) es ima es. I is op imal because he
pa ame e es ima es a e ound by minimizing a cos unc ion ha quan i ies he model
i ing e o , and i is join ly op imal because he solu ion is simul aneously op imal
wi h espec o bo h s uc u e and came a a ia ions T iggs e al. (1999). I s name
e e s o he bundles o ligh ays o igina ing om each 3D ea u e and con e ging on
each came a’s op ical cen e , which a e adjus ed op imally wi h espec o bo h he
s uc u e and iewing pa ame e s. Figu e 3.15 shows an example using h ee iews.
BA boils down o minimizing he ep ojec ion e o be ween he image loca ions
o obse ed and p edic ed image poin s, which is exp essed as he sum o squa es o a
la ge numbe o nonlinea , eal- alued unc ions. Thus, he minimiza ion is achie ed
using nonlinea leas -squa es algo i hms. O hese, Le enbe g–Ma qua d (LM) has
p o en o be one o he mos success ul due o i s ease o implemen a ion and i s use
3.2 Visual-Ine ial Odome y 63
o an e ec i e damping s a egy ha lends i he abili y o con e ge quickly om a
wide ange o ini ial guesses.
BA amoun s o join ly e ining a se o ini ial came a and s uc u e pa ame e
es ima es o inding he se o pa ame e s ha mos accu a ely p edic he loca ions
o he obse ed poin s in he se o a ailable images. Mo e o mally, assume ha
N
3D poin s
Xi
a e seen in
M
iews
Pj
, and le
xij
be he p ojec ion o he
i
- h poin on
image
j
. Le
ij
deno e he bina y a iables ha equal 1 i poin
i
is isible in image
j
, and 0 o he wise. Assume also ha each came a
Pj
is pa ame e ized by a ec o
aj
, and each 3D poin
Xi
by a ec o
bi
. Bundle adjus men minimizes he o al
ep ojec ion e o wi h espec o all 3D poin and came a pa ame e s, speci ically
min
aj,bi
N
X
i=1
M
X
j=1
ij d(Q(aj,bi),xij)2(3.14)
whe e
Q
(
aj,bi
) is he p edic ed p ojec ion o poin
i
on image
j
, and
d
(
x,y
) deno es
he Euclidean dis ance be ween he image poin s ep esen ed by ec o s
x
and
y
.
Clea ly, BA is by de ini ion ole an o missing image p ojec ions and minimizes a
physically meaning ul c i e ion.
Howe e , due o he la ge numbe o unknowns con ibu ing o he minimized
ep ojec ion e o , a gene al pu pose implemen a ion o he LM algo i hm (such as
MINPACK’s lmde Mo ´e e al. (1980)) incu s high compu a ional cos s when applied
o he minimiza ion p oblem de ined in he con ex o BA.
Fo una ely, he lack o in e ac ion among pa ame e s o di e en 3D poin s and
came as esul s in he unde lying no mal equa ions exhibi ing a spa se block s uc u e.
Ou app oach makes use o an e icien solu ion called Spa se Bundle Adjus men
(SBA) Lou akis and A gy os (2009), which exploi s his spa seness by employing a
ailo ed spa se a ian o he LM algo i hm ha leads o conside able compu a ional
gains. SBA is gene ic in he sense ha i g an s he use ull con ol o e he de ini ion
o he pa ame e s desc ibing came as and 3D s uc u e. The e o e, i can suppo
i ually any mani es a ion o pa ame e iza ion o he mul iple iew econs uc ion
p oblem, such as a bi a y p ojec i e came as, pa ially o ully in insically calib a ed

64 Robus Visual Odome y o UAVs
came as, ex e io o ien a ion (i.e. pose) es ima ion om ixed 3D poin s, e inemen
o in insic pa ame e s, e c.
In ou case, le us conside a se ies o key- ames, each o hem wi h hei associa ed
came a poses and key-poin s wi h 3D coo dina es and 2D image p ojec ions. The idea is
o imp o e he compu a ion o he came a mo ion be ween key- ames acco ding o he
se s o obse a ions. SBA is able o op imize no only he mo ion es ima ions, bu also
educe he e o s ha we e in oduced in o he 3D coo dina es o he key-poin s. Ou
app oach only conside s he las
K
p oduced key- ames (
K
=10 in he expe imen s),
and applies wo subsequen me hods in o de o allow on-line compu a ion o he pose
e inemen :
•
I only conside s he key-poin s ha a e seen and ma ched ac oss a leas
K/
2
consecu i e key- ames.
•
I applies a bucke ing echnique o he su i ing key-poin s, keeping he bes 3
key-poin s om 28 image bucke s ( ou ows and se en columns).
3.2.7 G ound Plane Es ima ion
Ano he ea u e ha was conside ed in his wo k is he es ima ion o he g ound plane,
gi en he pa icula i ies o he senso a angemen and he es ing en i onmen . The
3D imaging senso is moun ed sligh ly poin ing downwa ds, hence he poin cloud can
easily be used o es ima e he ligh heigh o he UAV, as an al ime e does, assuming
ha enough g ound scene is wi hin he ield o iew o he came a. The poin cloud is
i s downsampled in o de o keep he compu a ional e iciency o he whole pipeline.
A simple plane segmen a ion has been pe o med, i.e. ind all he poin s wi hin he
poin cloud ha suppo a plane model. The de ec ed planes a e il e ed by hei
no mal ec o , since we a e looking o ho izon al planes up o a maximum no mal
angle (10
◦
in ou app oach). The es ima ed heigh o he UAV is an inpu o he
pose co ec ion ega ding
Z
, which is a e aged wi h he pu ely isual heigh using a
con igu able weigh ac o (0.5 in ou expe imen s).
3.2 Visual-Ine ial Odome y 65
Figu e 3.16: CATEC es bed wi h a mockup scena io.
The inal came a pose es ima ion is ans o med om he senso ame o he ae ial
obo body ame, and used o calcula e he new ela i e posi ion and o ien a ion
es ima ions wi h espec o he ini ial pose.
3.2.8 Expe imen al Resul s
In o de o alida e ou app oach using he odome y app oach desc ibed in his
sec ion, a UAV has pe o med a ligh in CATEC’s indoo es bed wi hin a scena io
ec ea ing an indus ial acili y, as shown in Figu e 3.16. The isual odome y has
been used as he localiza ion es ima ion inpu in o de o close he con ol loop, and
he UAV was eleope a ed h ough a joys ick o command small inc emen s in posi ion
(
x
,
y
o
z
) and o ien a ion (only
yaw
). The main objec i e o he expe imen was o
demons a e he sui abili y and accu acy o he app oach in eal- ime.
This sec ion p o ides expe imen al esul s om he es ima ed localiza ion compa ed
wi h g ound- u h da a ob ained om he es bed acking sys em. The UAV used o
demons a e ou app oach is shown in Figu e 3.17. The main senso on-boa d he
pla o m is he RGB-D came a O bbec As a, which is acing o wa d and sligh ly
il ed down (25
◦
). The ajec o y ollowed by he UAV acco ding o g ound- u h
da a is shown in Figu e 3.18.
66 Robus Visual Odome y o UAVs
Figu e 3.17: The UAV wi h he RGB-D senso a he on .
x (m)
-4 -3.5 -3 -2.5 -2 -1.5 -1
y (m)
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
Figu e 3.18: G ound- u h UAV ajec o y in XY du ing he expe imen .
The es ima ed UAV localiza ion p o ided by he isual odome y algo i hm du ing
he expe imen can be seen in Figu e 3.19. The es ima ed posi ion and yaw angle
3.2 Visual-Ine ial Odome y 67
ime (s)
0 20 40 60 80 100 120 140 160
x (m)
-2
0
2G ound T u h
Odome y
ime (s)
0 20 40 60 80 100 120 140 160
y (m)
-2
0
2
ime (s)
0 20 40 60 80 100 120 140 160
z (m)
0
1
2
ime (s)
0 20 40 60 80 100 120 140 160
yaw ( ad)
-1
0
1
Figu e 3.19: Es ima ed UAV posi ion and o ien a ion.
du ing he expe imen a e shown: he ed dashed line co esponds o he UAV g ound-
u h cap u ed by he mo ion cap u e sys em, while he g een do ed line is he
es ima ed UAV posi ion and o ien a ion using he p oposed isual odome y. Roll
and pi ch angles we e acqui ed di ec ly om he on-boa d IMU and hence a e no
shown in he plo s, since hey a e di ec ly in eg a ed in o he algo i hm.
As expec ed, he odome y exhibi s some d i , hough i p o ides a smoo h es i-
ma ion which emendously helps he con ol algo i hms achie e a s able pe o mance.
Figu e 3.19 also shows how he odome y is consis en h oughou he whole ligh
ajec o y, aking in o accoun he ela i ely ad e se condi ions o he ligh wi hin
he indoo es bed o he algo i hm, since he e a e almos no isual ea u es in he
loo o he mockup scena io, and some imes he nea es objec s a e mo e han i e
74 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
does no p o ide enough in o ma ion o cons i u e a ull localiza ion sys em, since he
da a p o ided does no include bea ing in o ma ion.
By using bo h sensing modali ies, he eliable sho - e m posi ion es ima ion
based on odome y is combined wi h dis ances o ixed UWB senso s o co ec i s
cumula i e d i . Bo h bene i om each o he when in eg a ed in o de o ob ain as
and e o -bounded localiza ion es ima ions. Fu he mo e, he high e iciency o he
implemen ed algo i hms makes hem sui able o eal- ime localiza ion in he usually
cons ained equipmen ha can be moun ed on-boa d an ae ial ehicle. This pa icula
combina ion is he main s eng h o he app oach, in which wo di e en senso usion
s a egies ha e been de eloped and es ed using UAVs; hey a e explained in de ail
la e in his chap e . Fi s , an o e iew o how he ae ial obo s a e es ima ion is
handled om a p obabilis ic poin o iew is p esen ed.
4.1 S a e Es ima ion
Gene ally speaking, he in e ac ion o a obo and i s en i onmen can be modeled as
a dynamic sys em, in which he obo can manipula e i s en i onmen by choosing
con ols, and in which i can pe cei e i s en i onmen h ough senso measu emen s. In
p obabilis ic obo ics, he dynamics o he obo and i s en i onmen a e cha ac e ized
in he o m o wo p obabilis ic laws:
•
he s a e ansi ion dis ibu ion, which cha ac e izes how s a e changes o e
ime, no mally as he e ec o a obo con ol (e.g. a mo emen ), and
•
he measu emen dis ibu ion, which cha ac e izes how measu emen s a e go -
e ned by s a es.
Bo h laws a e p obabilis ic, accoun ing o he inhe en unce ain y in s a e e olu ion
and sensing.
The es ima ion o he s a e o a mobile obo is usually achie ed by he Bayes
il e Th un e al. (2005). The localiza ion p oblem consis s in inding he obo s a e
a ime
,
x
, gi en he las measu emen
z
and a p io s a e
x −1
. The Ma ko

4.1 S a e Es ima ion 75
assump ion holds, i.e. a s a e is a comple e summa y o he pas . Hence he Bayes
il e is ecu si e, since x is calcula ed om x −1.
The Bayes il e algo i hm possesses wo essen ial s eps. I i s calcula es a belie
o e he s a e
x
based on he p io belie o e s a e
x −1
and a con ol inpu
u
. This
is usually called he p edic ion s ep. The second s ep is called he upda e, in which
he algo i hm mul iplies he belie
x
by he p obabili y ha he measu emen
z
may
ha e been obse ed. The esul is no malized since he esul ing p oduc is gene ally
no a p obabili y.
In o de o compu e
x
ecu si ely, he algo i hm equi es an ini ial belie a ime
= 0. I one knows he alue o
x0
wi h ce ain y, i should be ini ialized wi h a poin
mass dis ibu ion ha cen e s all p obabili y mass on he co ec alue, assigning ze o
p obabili y anywhe e else. I one is en i ely igno an abou he ini ial alue
x0
, i may
be ini ialized using a uni o m dis ibu ion o e he domain o x0.
Bayesian il e s a e implemen ed in se e al di e en ways. Each o hem elies on
di e en assump ions ega ding he s a e ansi ion, measu emen p obabili ies and
he ini ial belie . In gene al, exac echniques o s a e calcula ion a e no a ailable
and hence he s a e has o be app oxima ed. Finding a sui able app oxima ion is
usually a challenging p oblem. When choosing an implemen a ion, se e al p ope ies
ha e o be conside ed.
•
Compu a ional e iciency: some app oxima ions allow compu ing s a e belie s in
ime polynomial in he dimension o he s a e space, o example linea Gaussian
app oxima ions. Pa icle-based echniques ha e an any- ime cha ac e is ic,
enabling hem o ade-o accu acy wi h compu a ional e iciency.
•
Accu acy o he app oxima ion: some echniques can app oxima e a wide
ange o dis ibu ions mo e igh ly han o he s. Fo example, linea Gaussian
app oxima ions a e limi ed o unimodal dis ibu ions. Pa icle ep esen a ions
can app oxima e a wide ange o dis ibu ions, bu he numbe o pa icles
needed o a ain he desi ed accu acy can be la ge.
•
Ease o implemen a ion: i depends on a a ie y o ac o s, such as he o m
o he measu emen p obabili y and he s a e ansi ion p obabili y. Pa icle
76 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
ep esen a ions o en yield su p isingly simple implemen a ions o complex
non-linea sys ems, which is one o he easons o hei ecen popula i y.
The nex wo sec ions desc ibe he wo echniques ha ha e been used in he
con ex o his disse a ion, which ep esen examples o p obably he wo mos popula
amilies o ecu si e s a e es ima ion echniques, bo h de i ed om he Bayes il e :
Gaussian il e s and pa icle il e s.
4.2 Gaussian Fil e s o S a e Es ima ion
Gaussian echniques sha e he basic idea ha belie s o he obo s a e a e ep esen ed
by mul i- a ia e no mal dis ibu ions. The densi y o e he a iable
x
is cha ac e ized
by wo se s o pa ame e s:
•
The mean
µ
, which is a ec o ha possesses he same dimensionali y as he
s a e x.
•
The co a iance Σ, which is a quad a ic ma ix ha is symme ic and posi i e-
semide ini e. I s dimension is he dimensionali y o he s a e
x
squa ed (i.e. he
numbe o elemen s depends quad a ically on he numbe o elemen s in he
s a e ec o ).
Rep esen ing he belie by a Gaussian dis ibu ion has impo an implica ions.
Mos impo an ly, Gaussians a e unimodal, ha is, hey possess a single maximum.
This is cha ac e is ic o many acking p oblems in obo ics, in which he belie o he
s a e is ocused a ound he ue s a e wi h a small ma gin o unce ain y. Howe e ,
hey a e a poo ma ch o many global es ima ion p oblems in which many dis inc
hypo heses exis .
P obably he bes s udied Gaussian echnique o implemen ing Bayes il e s is he
Kalman il e (KF) Kalman e al. (1960), which has been widely used o il e ing and
p edic ion in linea sys ems. A ime
, he belie is ep esen ed by he mean
µ
and
he co a iance Σ
. Apa om he Ma ko assump ion, he ollowing h ee p ope ies
mus hold:
4.2 Gaussian Fil e s o S a e Es ima ion 77
•
The s a e ansi ion p obabili y
p
(
x |u ,x −1
) mus be a linea unc ion in i s
a gumen s wi h added Gaussian noise:
x =A x −1+B u + (4.1)
•
The measu emen p obabili y
p
(
z |x
) mus also be linea in i s a gumen s wi h
added Gaussian noise:
z =C x +δ (4.2)
•The ini ial belie in = 0 mus be no mal dis ibu ed.
The assump ions o linea s a e ansi ions and linea measu emen s wi h added
Gaussian noise a e a ely ul illed in p ac ice. Fo example, a obo ha mo es wi h
cons an ansla ional and o a ional eloci y ypically mo es on a ci cula ajec o y,
which canno be desc ibed by linea s a e ansi ions. This obse a ion, along wi h
he assump ion o unimodal belie s, ende s plain Kalman il e s inapplicable o all
bu he mos i ial obo ic p oblems.
The ex ended Kalman il e (EKF) o e comes one o hese assump ions: linea i y.
He e he assump ion is ha he s a e ansi ion and he measu emen p obabili ies
a e go e ned by non-linea unc ions:
x =g(x −1, u ) +  (4.3)
z =h(x ) + δ (4.4)
A Gaussian p ojec ed h ough he non-linea s a e ansi ion unc ion
g
is ypically
non-Gaussian. Linea iza ion app oxima es
g
by a linea unc ion ha is angen
o
g
a he mean o he Gaussian. By p ojec ing he Gaussian h ough his linea
app oxima ion, he esul is also a Gaussian. The same applies o he mul iplica ion o
Gaussians when he measu emen unc ion
h
is in ol ed. Tangen s a e linea , making
he il e applicable. The e exis many echniques o linea izing non-linea unc ions.
EKFs u ilize Taylo expansion, which in ol es calcula ing he i s de i a i e o he
78 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
a ge unc ion, and e alua ing i a a speci ic poin . The esul o his ope a ion is a
ma ix known as he Jacobian.
The EKF is one o he mos popula ools o s a e es ima ion in obo ics Mao e al.
(2007); Pu is e al. (2008); Rullan-La a e al. (2011). I s s eng h lies in i s simplici y
and compu a ional e iciency. The EKF owes i s compu a ional e iciency o he ac
ha i ep esen s he belie by a mul i- a ia e Gaussian dis ibu ion. A Gaussian is a
unimodal dis ibu ion, which can be hough o as a single guess, anno a ed wi h an
unce ain y ellipse. In many p ac ical p oblems, Gaussians a e obus es ima o s.
An impo an limi a ion o he EKF a ises om he ac ha i app oxima es
he s a e ansi ion and measu emen unc ions using linea Taylo expansions. In
i ually all obo ic p oblems, hese unc ions a e non-linea . The accu acy o his
app oxima ion depends on wo main ac o s: he deg ee o non-linea i y o he unc ions
ha a e being app oxima ed, and he deg ee o unce ain y which is di ec ly ela ed
o he wid h o he p obabili y dis ibu ion ( he la ge he unce ain y, he highe he
e o in oduced by he linea iza ion).
In ou inc emen al app oach o achie ing obus long- e m localiza ion o UAVs,
an EKF has been i s used o use he in o ma ion p o ided by he isual odome y
algo i hm and UWB-based dis ance measu emen s. Al hough localiza ion based on
isual odome y will e en ually di e ge due o cumula i e e o s, i can be used as
a sho - ime p edic o o he UAV mo ion (p edic ion). The UWB senso eadings
can hen cons ain he odome y d i (upda e). Thus, using he odome y as mo ion
p io helps il e ing UWB noise and de ec ing/ emo ing measu emen ou lie s. A
he same ime, he UWB-based upda e helps o emo e he cumula i e e o s in he
odome y, building an accu a e and s able localiza ion sys em bo h oge he . Figu e
4.1 shows an o e iew o he me hod.
The UWB senso on-boa d he UAV compu es he dis ance o all he senso s a a
lowe ope a ing equency han ha o he isual odome y sys em, and in eg a es
he in o ma ion in o an EKF desc ibed below. As he obo mo es, he UWB senso
pe iodically sends ou a que y, and any o he senso wi hin ange esponds by sending
a eply. Since each senso ansmi s a unique ID numbe , dis ance eadings a e
au oma ically associa ed wi h he app op ia e ags, so he da a associa ion p oblem is
4.2 Gaussian Fil e s o S a e Es ima ion 79
Figu e 4.1: Schema ic o e iew o he EKF app oach.
i ially sol ed. Ano he key ad an age is ha he obo can es ima e he dis ance
o each esponding senso e en when hey a e no wi hin line o sigh . This is e y
use ul in si ua ions whe e isual-based me hods usually ail, such as poo ly illumina ed
scenes o highly dynamic en i onmen s.
The ange measu emen s and odome y p io s a e all in eg a ed in o a simple EKF
ha allows es ima ing he obo ’s posi ion and eloci y wi h he ollowing s a e ec o :
x="p
#=












x
y
z
x
y
z












(4.5)
whe e
p
and
e e o he obo posi ion and eloci y espec i ely. O ien a ion da a
is no conside ed in he EKF since, as s a ed be o e, he UWB-based measu emen s
used in he upda e s ep do no p o ide bea ing in o ma ion. Hence, isual odome y
is he only sou ce o in o ma ion o es ima ing he ae ial obo o ien a ion. As a
eminde , oll and pi ch angles a e in eg a ed di ec ly om he on-boa d IMU, while
yaw is es ima ed pu ely om isual in o ma ion.

80 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
4.2.1 P edic ion
The isual odome y sys em p o ides he es ima ed eloci y o he obo pe iodically,
o
= [
ox, oy, oz
]
. This in o ma ion is used in he p edic ion s age o he EKF o
cons ain he obo localiza ion acco ding o he ollowing exp ession:
p =p −1+ ∆ · −1(4.6)
= o (4.7)
E en ually, he odome y sys em migh no be able o p o ide an es ima ion, o
ins ance, due o he lack o ex u e in he scene. In such a case, he s a e is p edic ed
based on andom walk.
4.2.2 Upda e
Gi en he dis ance
di
measu ed om he obo o a beacon wi h known posi ion
bi
,
he ollowing cons ain can be applied o he obo posi ion:
di=||p −bi|| (4.8)
This cons ain can be easily in eg a ed in o he upda e s age o he EKF, ollowing
he equa ion:
z =h(x( )) (4.9)
whe e z =diand h(x( )) = ||p −bi||.
This il e upda e is applied o all he UWB measu emen s ecei ed wi hin he las
pe iod o ime acco ding o he il e ope a ing equency (50ms in ou expe imen s). I
mo e han one measu emen o a speci ic beacon a e a ailable, he dis ance is a e aged.
No e ha he il e does no need dis ances o se e al beacons in o de o pe o m
a il e upda e; he dis ance o a single senso is enough. This is possible hanks o
he s a e p io p o ided by he p edic ion s age, which is expec ed o be accu a e
and s able a sho - e m. This is a g ea ad an age wi h espec o s a e-o - he-a
4.2 Gaussian Fil e s o S a e Es ima ion 81
UWB-based localiza ion sys ems, which usually wai un il he dis ances o h ee o
mo e senso s a e measu ed.
4.2.3 Ou lie Rejec ion
Dis ance measu emen s based on adio senso s a e subjec o equen ou lie s when
pe o med indoo s. These ou lie s a e caused by mul i-pa h and signal e lec ion in
walls and s uc u es, and in oduce signi ican e o s in he localiza ion.
The odome y p io (EKF p edic ion) p o ides a sho - e m accu a e and eliable
es ima ion o he obo ’s posi ion, so he EKF measu emen esidual
y
=
z −h
(
x
)
can be used o decide whe he a ange measu emen is an ou lie o no . Thus, i
he esidual o a ange measu emen is abo e a h eshold
h
, he measu emen is
de e mined as an ou lie wi h high p obabili y and he il e will ejec he da a. This
h eshold has been se o h = 2min ou expe imen s.
4.2.4 Expe imen al Resul s
In o de o alida e he a o emen ioned app oach, which makes use o isual odome y
and UWB-based measu emen s, an ae ial obo has pe o med a ela i ely long ligh
(a ound 7 minu es). The ligh has aken place in an indoo con olled scena io whe e
se e al UWB de ices (six senso s) ha e been ins alled. The main objec i e o he
expe imen is o demons a e he sui abili y o he app oach du ing egula ope a ions.
The expe imen s ook place a he indoo es bed o CATEC, whose mo ion cap u e
sys em p o ided g ound- u h localiza ion da a.
P io o he discussion o he expe imen al esul s, he se up desc ibing all he
elemen s in ol ed in he expe imen is p esen ed:
•
Six UWB senso s used as beacons, ins alled ac oss he indoo es bed a di e en
loca ions, homogeneously dis ibu ed also a di e en heigh s. These loca ions
a e p e iously known by he sys em.
82 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
Figu e 4.2: UAV wi h he VI-senso a he on (le ) and g ound- u h ajec o y in
he XY plane ( igh ).
•
An ae ial obo wi h he ollowing on-boa d senso s: a VI-senso (s e eo came a),
an IMU and a UWB senso in o de o compu e poin - o-poin dis ances (see
Figu e 4.2).
The ae ial obo was manually pilo ed h ough he es bed a ea a di e en heigh s.
The g ound- u h ajec o y ollowed by he ehicle is also p esen ed in Figu e 4.2.
This is a 75 me e s long ajec o y in which he obo was pilo ed wi hin he indoo
es bed, lying a di e en al i udes.
The es ima ed UAV posi ion du ing he expe imen can be seen in Figu e 4.3, which
includes he es ima ed used localiza ion (blue solid line) oge he wi h he aw isual
odome y ou pu (g een do ed line), bo h compa ed o he g ound- u h posi ion ( ed
dashed line). As expec ed, he odome y sys em slowly di e ges h ough ime, while
he p oposed me hod in eg a ing adio-based measu emen s ollows he g ound- u h
wi h small de ia ions. Only posi ion esul s a e analyzed since adio-based sensing
does no p o ide o ien a ion measu emen s.
I can be also seen in Figu e 4.3 how he odome y is consis en mos o he ime,
aking in o accoun he ela i ely ad e se condi ions o he ligh wi hin he indoo
es bed: low ex u e ( he e a e almos no isual ea u es in he loo ) and high dis ance
o 3D scenes (some imes up o en me e s a ahead).
4.2 Gaussian Fil e s o S a e Es ima ion 83
Figu e 4.3: Es ima ed UAV localiza ion (g ound- u h in ed, isual odome y in
g een, p oposed app oach in blue).
Figu e 4.4 shows he es ima ion e o o he p oposed app oach in X, Y and Z
sepa a ely. The RMS e o o each localiza ion componen is also p esen ed. I can
be seen how he RMS e o is a ound 0.25m in Y and Z, and 0.4m o X.
As i was p e iously obse ed, he ligh condi ions a e a om ideal ega ding he
isual odome y app oach. The scena io loo exhibi s a e y homogeneous scene, and
he in oduced objec s o ec ea e a 3D en i onmen a e some imes e y a om he
ae ial ehicle (up o en me e s). Hence, dispa i y compu a ion esul s in highe e o s
ha lead o possible w ong ma ches ac oss ames. Ne e heless, he esul s p esen ed
in his sec ion show ha hese issues, when combined wi h adio-based measu emen s,
can be o e come esul ing in a mo e obus solu ion, sui able o long- e m ope a ion
o au onomous ehicles.
90 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
4.3.3 Upda e
In his phase, we use a measu emen model o inco po a e in o ma ion om he
senso s. We ha e de ined h esholds in posi ion and o ien a ion such ha i he isual
odome y es ima ion exceeds any o hose, a il e upda e is pe o med using he
las 3D poin cloud ecei ed om he s e eo/RGB-D came a, and all he dis ance
measu emen s o he UWB beacons ecei ed since he las il e upda e. Then, each
pa icle
x[i]
e alua es i s ela i e impo ance by checking how likely i would ecei e
such senso eadings a i s cu en pose, hence compu ing a new weigh alue
w[i]
.
Then, pa icles can la e be e-sampled conside ing hese weigh s, hus ob aining a
new es ima e o he cu en s a e gi en he las measu emen z .
In his wo k,
z
co esponds o he poin clouds om he 3D imaging senso and
he dis ance measu emen s o he UWB senso s. The upda e is pe o med h ough
he use o a 3D map o he en i onmen in he o m o an Oc oMap Ho nung e al.
(2013), augmen ed wi h he posi ion o he ixed loca ions o UWB beacons. Gi en
he dis inc na u e o he wo echnologies in ol ed, we calcula e sepa a e weigh s
o each sensing modali y, i.e.
w[i],map
and
w[i],uwb
. A simple weigh ed a e age is la e
used o ob ain he inal weigh o each pa icle:
w[i]
=α∗w[i],map
+ (1 −α)∗w[i],uwb
(4.18)
whe e
α
is chosen depending on he pa icula i ies o he indoo en i onmen whe e
he UAV is going o ope a e. I he map used in he il e does no con ain he ull
en i onmen , o i s accu acy is no enough o us he map ma ching,
α
should be
lowe han 0
.
5. Whe eas i he e a e ew UWB senso s deployed in he scena io, o
hei loca ion is no accu a e, αshould be highe .
Compu a ion o w[i],map
The acqui ed 3D poin cloud is ans o med o each pa icle’s pose in o de o ind
co espondences be ween such cloud and wha he map should look like om ha
pa icle’s pose. Since his is e y expensi e compu a ionally, we i s compu e a 3D
p obabili y g id as in Ho nung e al. (2010), in which each posi ion s o es a alue o

4.3 Pa icle Fil e s o S a e Es ima ion 91
how likely i is ha such posi ion alls wi hin an occupied poin o he map, ins ead
o s o ing bina y in o ma ion abou occupancy as in he p o ided map. Each 3D
posi ion
pi
o he g id is hen illed wi h p obabili y alues acco ding o a speci ic
Gaussian dis ibu ion cen e ed in he closes occupied poin in he map om
pi
,
mapi
,
and whose a iance σ2depends on he senso noise used in he app oach.
g id(pi) = 1
√2πσ2e−||pi−mapi||2/2σ2(4.19)
Such p obabili y g id only needs o be compu ed once, is no equi ed o be
upda ed o a gi en en i onmen , and elie es om pe o ming nume ous dis ance
compu a ions be ween each cloud poin o each pa icle and i s closes occupied poin
in he map. Besides, each poin cloud is i s ans o med acco ding o he cu en
oll and pi ch p o ided by he on-boa d IMU. This ans o ma ion is done jus once
pe upda e, educing he compu a ional equi emen s as well.
Then, o e e y poin o he ans o med cloud, we access i s co esponding alue
in he 3D p obabili y g id. Such alue would be an indica o o how likely is ha
poin o be pa o he map. By doing his wi h e e y poin o he cloud and adding
all he p obabili y alues, we ob ain a igu e o how well ha pa icle i s he ue
loca ion o he ae ial obo acco ding o he map.
Finally, he weigh
wi
o each pa icle
pi
is compu ed. Assuming ha he poin
cloud is composed by
M
3D poin s
cj
, he weigh is compu ed by adding all he
associa ed p obabili y g id alues as ollows:
wmap
i=1
M
M
X
j=1
g id(pi(cj)) (4.20)
whe e
pi
(
cj
) s ands o he ans o ma ion o he poin o he pa icle’s s a e, and
g id(pi(cj)) is he e alua ion o he p obabili y g id in such ans o med posi ion.
Compu a ion o w[i],uwb
On he o he hand, dis ance measu emen s be ween UWB senso s a e used o compu e
ano he weigh alue o each pa icle acco ding o how well hei s a e i s o he
92 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
dis ibu ion o ixed adio beacons. Since he adio beacons do no p o ide bea ing
in o ma ion, we i s de ine new s a es wi hou
ψ
(yaw angle), i.e.
x0[i]
= [
x, y, z
]
T
,
and UWB beacon s a es using hei es ima ed posi ions,
bj
= [
xj, yj, zj
]
T
. Gi en a
measu ed dis ance
dj
om he UWB senso on-boa d he UAV o he
j
- h beacon,
he ollowing cons ain can be applied o each pa icle s a e:
dj=||x0[i]
−bj|| (4.21)
This can be easily applied o he pa icle weigh calcula ion acco ding o he ac ual
Euclidean dis ance
ij
be ween he s a e
x0[i]
o he
i
- h pa icle and
bj
, and how close
his is o
dj
. The p oduc is used o agg ega e he alues om he measu emen s o
di e en beacons, since hey a e independen p obabilis ic p ocesses. The weigh o
he i- h pa icle associa ed o UWB sensing is calcula ed as ollows:
w[i],uwb
=
B
Y
j=1
1
σ√2πe−(dj− ij )2/2σ2(4.22)
whe e
B
is he numbe o beacons in he scena io. The dis ance measu emen s o
he senso s used in ou app oach ha e a s anda d de ia ion
σ
o oughly 0
.
1m, a e
il e ing po en ial ou lie s.
A g ea ad an age o his app oach is ha he dis ance o a single senso is enough
o upda e he weigh s o he pa icles, i does no need o wai un il dis ances o h ee
o mo e senso s a e ob ained by he on-boa d adio- ag, which usually happens in
o he s a e-o - he-a ange-based localiza ion sys ems.
Weigh combina ion
Be o e combining he weigh s o bo h sensing app oaches, all he weigh s mus i s be
no malized wi hin hei ca ego ies in o de o e i y Equa ion 4.12, hence ep esen ing
a alid p obabili y dis ibu ion. This also p e en s combining weigh s o dis inc
na u e which usually a e in di e en o de s o magni ude. Equa ion 4.18 is used o
ob ain he inal weigh o each pa icle, and he new weigh s a e no malized again in
o de o ep esen a alid p obabili y dis ibu ion.
4.3 Pa icle Fil e s o S a e Es ima ion 93
We ha e also e alua ed he op ion o d awing new pa icles in o he il e wi h each
new adio-based measu emen , aking in o accoun he lack o bea ing in o ma ion.
Thus, wi h each dis ance measu emen , a se o pa icles would be d awn in a sphe e
su ounding he on-boa d senso posi ion in o he map. Mos o hese pa icles will
be la e on des oyed in he nex e-sampling a e upda e hanks o he weigh
con ibu ion o he poin cloud ma ching wi h he 3D map. Howe e , his app oach
was disca ded because i equi es a la ge numbe o pa icles o a p ope ep esen a ion
o he ange hypo heses, which hinde s he compu a ional e iciency.
4.3.4 Re-sampling
The nex s ep o he me hod in o de o comple e one i e a ion o he algo i hm
in ol es he gene a ion o a new se o pa icles om he cu en one, by choosing
each pa icle
x[i]
acco ding o i s weigh
w[i]
. Hence, he new poses o he pa icles
will be mo e likely o be accu a e. The algo i hm employed o e-sampling is he low
a iance sample Th un e al. (2005). In his case, a single andom numbe is used as
a s a ing poin o ob ain Npa icles acco ding o hei ela i e weigh s.
4.3.5 Pose Compu a ion
The pa icle il e me hod is an i e a i e p ocess ha in ol es mo ing, sensing and
e-sampling mul iple localiza ion hypo heses. While he il e p oduces a se o
localiza ion hypo heses, he na iga ion needs o decide which hypo hesis is he co ec
one. The upda ed s a e ec o o he ae ial obo is hen calcula ed as he weigh ed
sum o all he pa icles, since when he il e con e ges o a single hypo hesis, ha
is a good es ima ion o he ue posi ion o he obo . In his case, all pa icles a e
ound o be clus e ed a ound a speci ic egion.
Due o he mixed sensing app oach, he ou lie s commonly p esen in indoo
UWB measu emen s a e ejec ed hanks o he pa icle weigh ing. I an ou lie in he
dis ance measu emen om a adio-based senso is ecei ed, ollowing Equa ion 4.22,
his would esul in e y low alues o
w[i],uwb
; in his case he e-sampling would
only depend on he associa ed weigh s calcula ed om he poin cloud ma ching, and
94 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
he ela i e sco ing among he se o pa icles would emain una ec ed. On he o he
hand, i he map ma ching be ween using he 3D poin clouds is no accu a e, i s
associa ed weigh would no in luence he o e all pa icle’s weigh , which will only
depend on he con ibu ion om he UWB side.
4.3.6 Expe imen al Resul s
An expe imen al se up has been concei ed o alida e he p esen ed app oach. The
ield es ook place a an indoo es bed o CATEC; pa o i can be seen in Figu e
4.6.
The ae ial obo in Figu e 4.7 pe o med a ligh based on he es bed’s mo ion
cap u e sys em o localiza ion, and na iga ed using joys ick commands ha sen
nea by posi ion and o ien a ion waypoin s in he obo coo dina e ame. The esul s
p esen ed in his sec ion we e ob ained o -line. In o de o alida e he long- e m
cha ac e o ou app oach, he ligh ook oughly 9 minu es so he isual odome y
could d i enough o e i y ha he wo sensing measu emen s used in he pa icle
il e help p e en ing localiza ion e o g ow h.
The comple e expe imen al se up is composed o he ollowing elemen s:
•
An ae ial obo wi h he ollowing on-boa d senso s: an RGB-D senso (O bbec’s
As a), an IMU and a UWB senso (see Figu e 4.7).
•
A 3D map o he wo king a ea (see Figu e 4.6) ob ained using an RGB-D senso
(ASUS’s X ion PRO LIVE) and mo ion cap u e da a.
•Th ee UWB beacons ins alled ac oss he indoo es bed a known posi ions.
The 3D map o he a ea was acqui ed using a di e en RGB-D senso om he
one on-boa d he UAV, whose housing was augmen ed wi h se e al passi e ma ke s
(see Figu e 4.8) in o de o ge i s pose wi h high accu acy om he es bed’s mo ion
cap u e sys em. Such senso was manually ca ied a ound he es bed while connec ed
o a lap op, and he acqui ed poin clouds we e p ojec ed using he co esponding
senso pose and me ged in o an accu a e OcT ee. This map can be seen in Figu e 4.6.
4.3 Pa icle Fil e s o S a e Es ima ion 95
Figu e 4.6: CATEC indoo es bed ( op) and 3D map wi h app oxima e adio beacons
loca ions(bo om) used o ield expe imen s.
A small in as uc u e o UWB beacons ( h ee senso s) has been ins alled in he
indoo scena io whe e he ligh has aken place. The adio-based senso s ha e been
ins alled a h ee di e en loca ions homogeneously dis ibu ed wi hin he indoo
es bed. Thei ue loca ions we e acqui ed using he mo ion cap u e sys em and

96 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
Figu e 4.7: UAV wi h an RGB-D senso a he on .
Figu e 4.8: Ano he RGB-D senso wi h passi e ma ke s o p ecise 3D map building.
p o ided o ou il e . The e is an addi ional UWB senso ins alled on he ae ial obo
in o de o compu e poin - o-poin dis ances.
The g ound- u h ajec o y ollowed by he ehicle is p esen ed in Figu e 4.9.
This is a oughly 200 me e s long ajec o y in which he obo pe o med a ajec o y
o a whole ba e y du a ion.
The main objec i e o he expe imen is o demons a e he sui abili y o he
app oach in eal- ime du ing egula ope a ions. The es ima ed UAV posi ion and yaw
du ing he expe imen can be seen in Figu e 4.10, bo h om he aw isual odome y
and he pa icle il e . Roll and pi ch angles a e no included in he plo s because,
as p e iously s a ed, hey a e obse able om he on-boa d IMU and a e di ec ly
4.3 Pa icle Fil e s o S a e Es ima ion 97
x (m)
-4 -3 -2 -1 0 1 2 3 4 5 6
y (m)
-4
-3
-2
-1
0
1
2
3
4
x (m)
-2 -1 0 1 2 3 4 5
z (m)
-1
0
1
2
3
4
5
Figu e 4.9: G ound- u h ajec o y ollowed by he ae ial obo du ing he expe imen .
in eg a ed in o ou localiza ion app oach. The IMU used in ou app oach p o ides
accu a e and il e ed es ima ions o such angles.
These esul s (Figu es 4.10 and 4.11) we e ob ained a e con igu ing he pa icle
il e wi h he pa ame e s shown in Table 4.1.
Table 4.1: MCL pa ame e s
Pa ame e Value
Numbe o pa icles 500
α(Eq. 4.18) 0.5
OcT ee esolu ion 0.1m
RGB-D senso σ0.05m
UWB senso σ0.1m
Upda e h eshold (pos) 0.1m
Upda e h eshold ( o ) 0.1 ad
The plo also shows he g ound- u h da a p o ided by he es bed’s mo ion cap u e
sys em. I can be seen how he isual odome y slowly accumula es e o s o e ime,
while he es ima ions om ou comple e app oach closely ollow he g ound- u h
du ing all he expe imen . I is also wo h o men ion ha his es ima ion does no
d i wi h ime and he e o s a e app oxima ely bounded.
98 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
ime (s)
0 50 100 150 200 250 300 350 400 450 500
x (m)
-2
0
2
4
6
8
G ound T u h
Odome y
MCL
ime (s)
0 50 100 150 200 250 300 350 400 450 500
y (m)
-5
0
5
ime (s)
0 50 100 150 200 250 300 350 400 450 500
z (m)
0
2
4
ime (s)
0 50 100 150 200 250 300 350 400 450 500
yaw ( ad)
-4
-2
0
2
4
Figu e 4.10: Localiza ion esul s (posi ion and yaw) showing he g ound- u h, isual
odome y and he p oposed app oach.
Figu e 4.11 shows he e o o he es ima ion du ing he ajec o y execu ion. The
e o is compu ed as he Euclidean dis ance be ween he es ima ion and he g ound-
u h a e e y ime s ep. In he igu e, le plo s co espond o he odome y app oach,
while igh plo s show esul s om he comple e p oposed app oach including he
il e . Besides, he plo depic s he compu ed RMS e o s o each axis, which a e also
shown in Table 4.2.
I can be seen how he RMS e o s a e app oxima ely 0
.
4m in
x
and
y
, while in
z
is less han 0
.
2m hanks o he g ound es ima ion module. RMS e o in
yaw
angle is
a ound 10 deg ees. E o s in
x
and
y
a e highe due o he la ge dis ance a eled
4.3 Pa icle Fil e s o S a e Es ima ion 99
ime (s)
0 100 200 300 400 500
x e o (m)
0
5
Odome y E o s
ime (s)
0 100 200 300 400 500
y e o (m)
0
5
ime (s)
0 100 200 300 400 500
z e o (m)
0
0.5
ime (s)
0 100 200 300 400 500
yaw e o ( ad)
0
1
2
ime (s)
0 100 200 300 400 500
x e o (m)
0
0.5
1
1.5 MCL E o s
ime (s)
0 100 200 300 400 500
y e o (m)
0
0.5
1
1.5
ime (s)
0 100 200 300 400 500
z e o (m)
0
0.5
ime (s)
0 100 200 300 400 500
yaw e o ( ad)
0
0.5
1
Figu e 4.11: Posi ion and o ien a ion e o s wi h espec o g ound- u h h ough
ime.
h ough hese axes. Ne e heless, he global RMS e o in posi ion is 0
.
32m which
can be conside ed accep able o obo na iga ion.
Table 4.2: RMS localiza ion e o s
x(m) y(m) z(m) yaw ( ad)
Odome y 2.47 2.53 0.19 0.88
MCL 0.36 0.39 0.17 0.18
In o de o quan i a i ely assess he pe o mance o ou localiza ion app oach, we
ha e es ed he da a om he alida ion expe imen agains o he popula app oaches
106 Mul i-Modal Senso Fusion o Long-Te m Localiza ion
om ou comple e app oach closely ollow he g ound- u h du ing all he expe imen .
I is also wo h o men ion ha his es ima ion does no d i wi h ime and he e o s
a e app oxima ely bounded.
E en hough good accu acy h ough he whole ligh du a ion we e achie ed, he
localiza ion app oach elied on a 3D map ha was buil using a mo ion cap u e
sys em. In o de o be able o ex end he p oposed sys em o a cus om indoo
scena io o au onomous UAV ope a ion, we mus complemen his me hodology wi h
mapping capabili ies elying also only on 3D imaging senso s and UWB sensing. This
is explained in de ail in Chap e 5.

Chap e 5
Mul i-Modal Mapping
P io o obo localiza ion, a 3D map o he en i onmen needs o be buil in o de
o make use o he pa icle il e p oposed in Chap e 4. A ailable senso da a o
achie e his a e he 3D poin clouds om s e eo o RGB-D came as and he dis ance
measu emen s be ween UWB senso s (one o hem is on-boa d he UAV and he es
a e ixed in he en i onmen ). Nei he he UAV ajec o y no he posi ion o he
UWB senso s in he en i onmen a e known a p io i.
The objec i es o his app oach a e o map he en i onmen based on he 3D
poin clouds and o accu a ely localize he se o ixed UWB senso s, o beacons.
App oaching hese wo asks sepa a ely may seem logical. Fi s ly, he scene could
be mapped based exclusi ely on he poin clouds while a he same ime he ae ial
obo ajec o y is calcula ed. This is usually pe o med h ough he use o a SLAM
app oach based on 3D poin clouds such as End es e al. (2012); Labbe and Michaud
(2014). La e , he ajec o y in o ma ion could be used o accu a ely loca e he UWB
beacons in o he map, acco ding o he co esponding dis ance measu emen s om
each UAV pose o each beacon. Howe e , his me hod does no ake ad an age o
UWB sensing o mapping, nei he localiza ion.
The me hod p oposed in his chap e ollows an in eg a ed app oach in which we
ake ad an age o bo h sensing modali ies. The posi ion o he UWB beacons a e
i s ly app oxima ed, and la e on e ined oge he wi h he 3D map o he en i onmen .
The e a e wo main s eps in ou app oach. The i s s ep conside s da a om he
107
108 Mul i-Modal Mapping
UWB senso s o compu e a globally consis en 3D ajec o y o he UAV, and hen
o au oma ically pe o m loop closing, which is he de ec ion o a p e iously isi ed
loca ion by some means. Bo h he ini ially compu ed ajec o y and he loop closu es
will be used in a second s ep o op imize he 3D posi ion o he UAV, and also
he posi ions o he UWB beacons. Wi h his in o ma ion, a 3D map can be buil
acco ding o he es ima ed UAV ajec o y and p ojec ing he 3D poin cloud a each
loca ion. Fu he de ails abou he implemen a ion o bo h s eps a e explained in he
ollowing sec ions.
5.1 Range-only localiza ion and mapping (s ep 1)
The objec i e o he i s s ep o he algo i hm is o compu e an ini ial guess o he
posi ions o he UWB beacons based on he measu ed dis ances o he senso on-boa d
he UAV. Then we can use his in o ma ion o ob ain a globally consis en ajec o y
o he ae ial obo . This is gene ally e e ed o as a Range-Only Simul aneous
Localiza ion and Mapping (RO-SLAM) Kan o and Singh (2002); Newman and
Leona d (2003). In gene al, RO-SLAM aims o localize a mobile sys em and a he
same ime map he posi ion o a se o ange senso s. In con as wi h o he SLAM
app oaches based on came as o LIDAR, RO-SLAM has he ad an age o no equi ing
di ec line o sigh be ween each pai o senso s when adio-based ange sensing is used.
Besides, he p oblem o da a associa ion is i ially sol ed; his could pose a signi ican
obs acle o he algo i hm, since ange senso s ypically p o ide dis ance measu emen s
o some objec wi hou iden i ying such objec . In he case o adio-based senso s,
hey usually ansmi hei unique ID along wi h he ange in o ma ion, as i is he
case o he UWB senso s used in he p oposed app oach.
Mos o he app oaches o RO-SLAM in he li e a u e a e based on ime il e ing
and p obabilis ic amewo ks such as EKF-SLAM, Unscen ed Kalman Fil e (UKF)-
SLAM, Fas SLAM, and o he s. Compa isons o hese amewo ks can be ound in
Ku -Ya uz and Ya uz (2012) and Li e al. (2012). Ku -Ya uz and Ya uz (2012)
p esen how he unscen ed Fas SLAM exhibi s be e pe o mance o e o he classical
me hods based on EKF o UKF. Howe e , Fas SLAM solu ions do no p ese e he
5.1 Range-only localiza ion and mapping (s ep 1) 109
co ela ion be ween di e en beacons o he map in hose applica ions in which i
migh exis . Thus, o example, in Blanco e al. (2008); Wang e al. (2009); Yang
(2012), a Fas SLAM solu ion is p oposed using a pa icle il e o obo localiza ion
and ano he one o each beacon (i.e. o each adio emi e ). In Yang (2012), an
op imiza ion on he beacons’ pa icle il e is p oposed by educing he numbe o
pa icles, hence dec easing he compu a ional bu den o he il e . On he o he hand,
Wang e al. (2009) op imize he p oblem using an adap i e e-sampling me hod which
dynamically educes he numbe o pa icles equi ed o each beacon. A di e en
beacon-based SLAM algo i hm is conside ed in Hai e al. (2010), whe e he au ho s
use a pa icle il e o ini ialize he EKF il e o each new beacon o he Fas SLAM.
The main d awback o he p e ious app oaches lies in he delayed ini ializa ion o
he beacons in o he il e s, which signi ican ly educes he op imiza ion o he obo
localiza ion un il he posi ion solu ion o he ange senso s has con e ged.
This p oblem is sol ed in Boo s and Go don (2013), whe e a ba ch solu ion is
p oposed o es ima e he posi ion o he mobile obo and he beacons by using a
singula alue decomposi ion (SVD) o he obse a ion ma ix. A ba ch p ocessing
solu ion is also p esen ed in Kehagias e al. (2006) based on op imiza ion. Howe e ,
hese me hods assume measu emen s om all he adio beacons a e e y obo posi ion;
o he wise, he measu emen mus be in e pola ed. This is a ha d cons ain in ealis ic
implemen a ions whe e he isibili y o all he ange senso s canno be gua an eed a
all imes.
This sec ion p oposes a new op imiza ion app oach o he RO-SLAM p oblem ha
ollows he ba ch p ocessing ideas bu gene alizes o a mo e common si ua ion in which
he ange measu emen s om he ange senso s migh a i e a he obo independen ly,
o e en a speci ic obo pose does no ha e associa ed ange measu emen s.
5.1.1 P oblem De ini ion
The UAV ajec o y is ep esen ed as a se ies o
N
obo poses
X
=
{x1,x2, ..., xN}
,
whe e each pose is de ined as
xi
= [
xi, yi, zi, ψi
]
T
, being
ψ
he yaw angle o he ae ial
obo . The e is also a se o
M
UWB beacons
B
=
{b1,b2, ..., bM}
, each one loca ed a
110 Mul i-Modal Mapping
a posi ion de ined by
bj
= [
xbj, ybj, zbj
]
T
. The objec i e o he RO-SLAM op imiza ion
p oblem is o compu e he bes UAV ajec o y
X
and loca ions o UWB beacons
B
ha minimizes he e o s wi h espec o he obse a ions o he UWB measu emen s
om each UAV pose.
No e how he obo oll and pi ch angles a e no included in o he obo pose
de ini ion. We assume hese angles a e a ailable and accu a e enough in an UAV
h ough he use o i s on-boa d IMU. They a e ully obse able and hei alues a e
usually accu a e in ae ial obo s because hey a e he mos basic con ol a iables
( oge he wi h he o a ion a es) o he sys em s abili y. While i is ue ha oll and
pi ch es ima ion based on accele ome e and gy oscope in eg a ions migh be biased
unde cons an accele a ions (e.g. loi e ing in ixed-wing UAVs), hese scena ios a e
e y di icul o achie e indoo s and hence a e no conside ed in his app oach.
The UWB-based ange obse a ions a e he dis ances be ween he UAV posi ion
and each beacon posi ion. Thus, he obse a ions a pose
xi
a e modeled as he se o
measu emen s
Di
=
{di1, di2, ..., diM }
wi h a bi a y leng h om 0 (no measu emen s)
o
M
(measu emen s o all UWB senso s), whe e
dij
is he Euclidean dis ance be ween
pose xiand beacon posi ion bj:
dij =q(xi−xbj)2+ (yi−ybj)2+ (zi−zbj)2(5.1)
Thus, he esul ing ae ial obo ajec o y and UWB beacon posi ions will be ha
which minimizes he ollowing exp ession:
a g min
{X,B}"N
X
i=1
M
X
j=1
cij (kxi−bjk−dij)#(5.2)
whe e
cij
is a a iable ha akes alue 1 i he e is a measu emen om pose
xi
o
UWB beacon bj, and 0 o he wise.
Howe e , due o he na u e o he p oblem add essed, he e exis s he possibili y
ha a numbe o poses do no ha e associa ed measu emen s ( he UAV is ou o ange
o all UWB beacons) and, mo e equen ly, ha he numbe o ange measu emen s
in a pose is below ou (minimum numbe o compu e he obo posi ion in 3D).
5.1 Range-only localiza ion and mapping (s ep 1) 111
These limi a ions a e o e come by including odome ic cons ain s in o Equa ion 5.2,
ob aining he inal exp ession o minimize:
a g min
{X,B}"N
X
i=1 Ei+
M
X
j=1
cij(kxi−bjk−dij )!# (5.3)
whe e
Ei
s ands o he e o be ween pose
xi
and
xi−1
acco ding o odome y
in o ma ion. This unc ion ans o ms he pose
xi−1
acco ding o he es ima ed obo
odome y and compu es he e o wi h espec o
xi
in each pose dimension. We make
use o he isual-ine ial odome y algo i hm p esen ed in Chap e 3.
5.1.2 Op imiza ion
Sol ing Equa ion 5.3 is s aigh - o wa d i good guesses abou he ae ial obo poses
and he posi ions o UWB beacons a e a ailable. Howe e , RO-SLAM does no
necessa ily ha e p io in o ma ion abou he posi ion o he beacons. An op ion would
be o ini ialize
B
o andom posi ions and le he op imiza ion p ocess es ima e hem,
bu he algo i hm will mos likely ge s uck in a local minimum.
In o de o o e come his p oblem, his app oach p oposes wo ac ions. Fi s , he
zb
pa ame e o e e y UWB beacon in
B
could be es ima ed by jus measu ing hei
dis ances o he loo . This is an easy p ocess ha can be implemen ed accu a ely.
This assump ion allows educing he numbe o unknown pa ame e s o each UWB
senso posi ion o wo (
xb
and
yb
), bu s ill, mo e in o ma ion is needed in o de o
ini ialize each UWB beacon posi ion o he op imiza ion me hod.
The second ac ion consis s in e-pa ame e izing he beacon posi ion so ha we can
eed se e al es ima ion hypo heses in o he op imize , as p oposed in Fab esse e al.
(2013). Thus, when he obo is a pose
xi
and ecei es he i s ange measu emen
dij
o senso
j
, he beacon
bj
would be in a ci cum e ence a ound he cu en obo
pose and a al i ude
zbj
. The p oposed app oach samples his ci cum e ence wi h
mul iple posi ion hypo heses a di e en angles, allowing he op imize o choose he
bes one. Figu e 5.1 shows an example including a UAV 3D ajec o y seen om an
o hogonal iew o easy isualiza ion ( iangles a e obo poses and ci cles a e UWB

112 Mul i-Modal Mapping
Figu e 5.1: Mul iple hypo heses o he localiza ion o h ee UWB beacons.
beacon posi ion hypo heses). Thus, assuming
H
di e en posi ion hypo heses along
he ci cum e ence, he UWB beacon posi ion will be pa ame e ized as ollows:
bj= [bj1,bj2, ..., bjHj] (5.4)
whe e each posi ion hypo hesis is ep esen ed as
bjk
= [
xbjk , ybjk , zbj
]
T
. No e how
zbj
is he same o all hypo heses because i is al eady known.
Wi h his pa ame e iza ion in mind, Equa ion 5.3 can be e o mula ed as ollows:
a g min
{X,B}

N
X
i=1 
Ei+
M
X
j=1
1
Hj
Hj
X
k=1
cij (kxi−bjkk−dij)

(5.5)
No e how he con ibu ion o a single UWB senso
j
is scaled by 1
/Hj
o e e y
pose
xi
, so ha we do no double-coun he in o ma ion p o ided by a single ange
measu emen .
5.1 Range-only localiza ion and mapping (s ep 1) 113
5.1.3 Ini ializa ion
In summa y, he pa ame e s o he op imiza ion p ocess a e he posi ion o he obo
in each pose
xi
, which is ini ialized acco ding o he odome y alues, and he di e en
posi ion hypo heses o e e y UWB beacon.
When a ange measu emen
dij
is ecei ed om he
j
- h beacon o he i s ime,
he cu en obo posi ion es ima ion
xi
is used o ini ialize he
Hj
posi ion hypo heses
acco ding o he ollowing equa ions:
xbjk =xi+dij ·cos(2π(k−1)/Hj) (5.6)
ybjk =yi+dij ·sin(2π(k−1)/Hj) (5.7)
zbjk =bzj(5.8)
The alue o
Hj
is ini ialized o
Hj
= 10
·dij
, so ha he numbe o hypo heses adap s
o he senso dis ance.
Thus, he ou come o his i s s ep is a globally consis en ajec o y and an ini ial
posi ion es ima ion o he UWB beacons. I he obo mo ion was enough o le he
op imize disambigua e he ho izon al lip ambigui y Fab esse e al. (2016), hen he
algo i hm con e ges o a single solu ion and all he hypo heses will be localized in a
speci ic posi ion.
5.1.4 Weigh ing
I is impo an o ake in o accoun ha he di e en sou ces o in o ma ion in
Equa ion 5.5 ha e di e en le els o accu acy. I is e y usual o make use o he
cons ain ’s associa ed in o ma ion ma ix o ine une he minimiza ion p ocess. In
his way, e y di e en sou ces o in o ma ion such as isual odome y and UWB
senso s can be easily conside ed.
Howe e , p ac ical expe ience shows ha he UWB measu emen s end o inco -
po a e ou lie s equen ly due o mul i-pa h e ec s o signal a enua ion. This is he
eason behind weigh ing he di e en componen s in o Equa ion 5.5 di e en ly. We
es ablish a weigh ing ac o assuming ha he odome y is eliable in he sho e m,
114 Mul i-Modal Mapping
while UWB migh p o ide ou lie s depending on he en i onmen and no ma e he
ime. Thus, in he p oposed implemen a ion, UWB cons ain s a e mul iplied by a
ac o anging om 0
.
5 o 0
.
1 depending on he quali y o he in o ma ion, while
odome y will be always weigh ed by a ac o o 1
.
0. Tha is, we us odome y
cons ain s mo e han UWB a sho - e m.
5.2 3D Mapping and Pose Re inemen (s ep 2)
The esul s o he i s s ep a e a cohe en UAV ajec o y and ini ial posi ion
es ima ions o he UWB beacons. The second s ep in ou me hodology in ol es he
au oma ic de ec ion o loop closu es, which is usually based on di e en app oaches
such as isual place ecogni ion o scan ma ching. In his case, a massi e scan ma ching
p ocess is ca ied ou among all he poses ha all wi hin a gi en sea ch adius, based
on he 3D poin clouds acqui ed by he on-boa d 3D imaging senso (a s e eo o
RGB-D came a).
This loop-closu e de ec ion will add new cons ain s o he obo poses G ise i
e al. (2010). Howe e , his app oach goes one s ep o wa d and akes in o accoun a
new ype o cons ain apa om he usual ans o m be ween poses, so ha we can
also op imize he alignmen be ween he 3D poin clouds di ec ly in o he op imize .
Gi en he senso poin cloud
pci
a pose
xi
, and he poin cloud
pcj
a pose
xj
, he
scan ma ching p ocess es ablishes he ans o m ha bes aligns bo h poin clouds
(using ICP as in Chap e 3). In he li e a u e, his ans o m is commonly used as a
cons ain be ween bo h poses, and i s associa ed in o ma ion ma ix allows uning
he impo ance o such cons ain in o he non-linea op imiza ion p ocess. Ins ead,
we p opose o include he alignmen e o in o he op imiza ion p ocess.
To his end, each poin cloud is ans o med o he global e e ence ame acco ding
o i s associa ed pose (
pcg
i
) and hen he alignmen e o be ween poin clouds is
compu ed as he a e aged Euclidean dis ance be ween hei indi idual 3D poin s.
Equa ion 5.3 is used ins ead o Equa ion 5.5, since now a single hypo heses is a ailable
o each UWB posi ion. This equa ion can be enla ged wi h his new cons ain as
5.3 Expe imen al Resul s 115
ollows:
a g min
{X,B}"N
X
i=1 Ei+
M
X
j=1
cij(kxi−bjk−dij ) +
Pi
X
l=1
D(pcg
i,pcg
l)!# (5.9)
whe e
Pi
is he numbe o loop closu es in which pose
i
is in ol ed, and he unc ion
D
(
pcg
i,pcg
l
) compu es he a e age Euclidean dis ance be ween he gi en poin clouds
in he global ame.
Ob aining
D
(
pcg
i,pcg
l
) could ha e a signi ican compu a ional cos i he poin
clouds a e la ge, because we need o calcula e he closes 3D poin o each cloud in o
he o he , hence slowing down he op imiza ion p ocess. Howe e , assuming ha he
poses o be op imized a e no a om he inal es ima es hanks o he RO-SLAM
s ep, hese associa ions be ween he poin clouds can be p e-compu ed. In his way,
he unc ion
D
(
pcg
i,pcg
l
) only needs o compu e he a e age dis ance be ween 3D
poin s because he associa ions a e al eady known.
5.3 Expe imen al Resul s
An expe imen al se up has been concei ed in o de o es he p oposed app oach. A
UAV has been equipped wi h wo RGB-D senso s (O bbec’s As a), one in he on
and ano he one in he ea side o he obo . Bo h a e il ed down sligh ly (25
◦
). A
UWB senso has been also ins alled on op o he ae ial obo , and a se o h ee UWB
beacons ha e been placed in he scena io. The UWB dis ance measu emen s ha e a
s anda d de ia ion o app oxima ely 20cm, bu hey a e subjec o u he dis o ions
due o e lexions o sigma a enua ion. The UAV used o es ing is depic ed in Figu e
5.2.
The expe imen s ha e been ca ied ou once again a CATEC’s indoo es bed,
in which a scena io ec ea ing an ai c a manu ac u ing plan has been ins alled as
shown in Figu e 5.3. As in p e ious expe imen s, accu a e g ound- u h localiza ion
da a o he UAV we e acqui ed, as well as he posi ions o he h ee UWB beacons
ins alled in he en i onmen , using he es bed’s mo ion cap u e sys em.
122 Mul i-Modal Mapping
ime (s)
0 50 100 150 200 250 300 350 400
x e o (m)
0
0.5
1
E o s using g ound- u h map
ime (s)
0 50 100 150 200 250 300 350 400
y e o (m)
0
0.5
1
ime (s)
0 50 100 150 200 250 300 350 400
z e o (m)
0
0.5
ime (s)
0 50 100 150 200 250 300 350 400
yaw e o ( ad)
0
0.5
ime (s)
0 50 100 150 200 250 300 350 400
x e o (m)
0
0.5
1
E o s using ou buil map
ime (s)
0 50 100 150 200 250 300 350 400
y e o (m)
0
0.5
1
ime (s)
0 50 100 150 200 250 300 350 400
z e o (m)
0
0.5
ime (s)
0 50 100 150 200 250 300 350 400
yaw e o ( ad)
0
0.5
Figu e 5.9: E o s in he es ima ed UAV posi ion and yaw angle using he g ound- u h
map and he econs uc ed map.
5.4 Conclusions
The expe imen al esul s show how a mul i-senso sui e can be used o pe o m 3D
mapping and long- e m localiza ion, in eg a ing senso eadings om UWB beacons
and RGB-D came as o build a obus app oach. The mapping algo i hm exploi s he
syne gies be ween UWB and RGB-D o build an accu a e 3D map o he en i onmen
and o localize he UWB senso s in o such map. The localiza ion app oach success ully
in eg a es bo h senso ypes o o e come he limi a ions o each senso modali y by
i s own.
This mapping app oach is o ien ed o be used wi h he pa icle il e p esen ed
in Chap e 4, hence p o iding a ull me hodology o applying his sa e, obus and
long- e m localiza ion app oach o any cus om scena io in which small UAVs need o
be deployed.

Chap e 6
Sys em A chi ec u e and
F amewo k
In o de o simpli y he accomplishmen o all he asks ela ed o he au onomous
na iga ion o UAVs, a amewo k o combining and execu ing he e ogeneous so wa e
modules has been de eloped. The e a e se e al open-sou ce p ojec s ha a ge com-
ple e so wa e a chi ec u es Lim e al. (2012), such as A duPilo
1
o PX4
2
. Howe e ,
hese app oaches usually lack lexibili y o pe o m and suppo high-le el unc ionali-
ies, which a e o en demanded by use s ha wish o de elop inno a i e app oaches
owa ds UAV au onomy.
In ecen yea s, se e al ini ia i es ha e a isen om some esea ch g oups. The
Au onomous Sys ems Lab om Eidgen¨ossische Technische Hochschule (ETH) Zu ich
de eloped asc ec ma amewo k
3
, howe e i is dependen on he UAV manu ac u e .
The Papa azzi p ojec
4
om
´
Ecole Na ionale de l’A ia ion Ci ile (ENAC) is a comple e
solu ion ha also includes speci ic ha dwa e, he Papa azzi au opilo . Ano he example
is hec o quad o o
5
om Technische Uni e si ¨a Da ms ad , which is hea ily ocused
on simula ion en i onmen s and has limi ed applicabili y in eal sys ems pe o ming
1h p://a dupilo .o g
2h p://px4.io
3h p://wiki. os.o g/asc ec ma amewo k
4h p://wiki.papa azziua .o g/wiki/Main Page
5h p://wiki. os.o g/hec o quad o o
123
124 Sys em A chi ec u e and F amewo k
Figu e 6.1: Schema ic o e iew o he p oposed a chi ec u e.
expe imen al ligh s. E en hough hese app oaches ha e p oduced majo ad ances in
his line o esea ch, he e a e s ill impo an challenges ela ed o he achie ed le el
o au onomy and lexibili y o he sys em o be able o adap i o di e en ae ial
obo s o applica ions.
Figu e 6.1 shows he so wa e a chi ec u e ha has been buil and e ined h ough-
ou he di e en wo ks owa ds his disse a ion. The sys em is decomposed in o
se e al modules which a e in cha ge o di e en asks. One o he i s hings o con-
side o an au onomous obo is he es ima ion o he cu en s a e o he pla o m in
a speci ic coo dina e sys em. Fo his, senso da a is i s p ocessed and used o ob ain
a localiza ion es ima ion wi h espec o i s en i onmen . This is commonly ca ied ou
by an odome y calcula ion in Senso -based Odome y and a localiza ion es ima ion
in Robo Localiza ion o p oduce a eliable 6 DoF pose (posi ion and o ien a ion).
Senso da a is commonly used in bo h modules in o de o educe unce ain ies and
imp o e he obus ness o he es ima ions, especially o long- e m ope a ion o he
125
ae ial obo . These wo modules ha e been he speci ic a ge o he wo k desc ibed
in his disse a ion.
Once eliable obo localiza ion is a ailable, subsequen modules can conside
ackling mo e complex asks, such as whe e o go o how o each a speci ic loca ion.
The gene a ion o sa e ajec o ies is ano he essen ial capabili y o au onomous
na iga ion, especially in clu e ed a eas whe e se e al obs acles o e en o he obo s
can be ound. Realis ic ope a ing en i onmen s o ae ial obo s may exhibi ixed
o mobile elemen s and simple o complex obs acles, all o hem sha ing he same
a ea. The e o e, he ae ial obo mus con inuously conduc a local ecalcula ion o
he ajec o y in eal- ime in o de o a oid collisions wi h di e en ypes o elemen s
as he on-boa d senso s gain in o ma ion abou i s su oundings. Addi ionally, he
UAV migh also need o adop di e en collision a oidance s a egies depending on
he na u e o he obs acles; he obo should a oid pe sons di e en ly han s a ic
objec s o ins ance. Senso da a is also acqui ed by he T ajec o y Planne o
eac i e collision a oidance, along wi h he localiza ion ou pu in o de o compu e
sa e ajec o ies in he ae ial obo en i onmen . The T ajec o y T acke ensu es he
UAV eaches he desi ed waypoin s a he co ec imes, and hus sends he Con olle
he e e ences ha i needs in o de o calcula e he necessa y ac ions o he UAV.
Special ocus has been gi en o sa e y in he ope a ion o he ae ial obo , hence
he key componen s in ou app oach a e he pe cep ion o he en i onmen o bo h
localiza ion and mo ion planning. This will allow he UAV o selec an app op ia e
beha io acco ding o he awa eness o he cu en si ua ion.
The co e idea behind he a chi ec u e is he combina ion o simple concep s and
componen s o build a eliable sys em. Each module uns as an independen p ocess
on-boa d he UAV. The di e en modules can implemen a wide a ie y o algo i hms
and echniques o pe o ming hei ask, acco ding o he equipmen o he ae ial
obo . I is impo an o poin ou ha wo king wi h UAVs in ol es addi ional
cons ain s in e ms o compu a ion and payload capaci y which mus be aken in o
accoun o he de elopmen o applicable sys ems. Tha said, he di e en algo i hms
can be de eloped independen ly and swi ch o eplace hem wi hin any o he modules
o sol e he ask ha is in ended o pe o m. This does no a ec he es o he
126 Sys em A chi ec u e and F amewo k
a chi ec u e p o ided ha he new algo i hm complies wi h he applicable inpu and
ou pu in e aces o he speci ic module.
Besides, he a chi ec u e is ocused on he so wa e componen s, emaining inde-
penden om he ha dwa e used unde nea h. The co e algo i hms o he sys em do
no depend on he speci ic manu ac u e o he UAV pla o m o he speci ic senso s
ha a e used, and hence i is possible o eplace hem wi h a di e en model o e en
wi h an upg aded e sion o he de ice, implying only mino changes o he sys em.
Apa om ha , i is possible o complemen he sys em wi h addi ional senso s o
enhanced p ocessing capabili ies wi hou al e ing he o e all scheme. Mo eo e , his
a chi ec u e is able o accommoda e applica ions in ol ing a single o mul iple UAVs,
as well as highly au onomous ope a ions o eleope a ed missions.
Ou app oach uses as middlewa e he Robo Ope a ing Sys em (ROS) Quigley
e al. (2009), which was de eloped by Willow Ga age and S an o d Uni e si y as
pa o he STan o d A i icial In elligence Robo (STAIR) p ojec as a ee and
open-sou ce obo ic middlewa e o he la ge-scale de elopmen o complex obo ic
sys ems. ROS ac s as a me a-ope a ing sys em o obo s as i p o ides ha dwa e
abs ac ion, low-le el de ice con ol, in e -p ocesses message-passing and package
managemen . I also p o ides ools and lib a ies o ob aining, building, w i ing, and
unning code ac oss mul iple compu e s.
Cu en ly, ROS is widely used in esea ch ac i i ies and is becoming a de ac o
s anda d o obo ic applica ions. One o he main ad an ages o ROS is ha i allows
manipula ing senso da a as a labeled abs ac da a s eam, called opic, wi hou
ha ing o deal wi h ha dwa e d i e s. The in e aces be ween ou modules a e based
on s anda d ROS da a ypes o ans o ma ions, pose es ima ions o ajec o y
commands. This g ea ly imp o es he lexibili y o he a chi ec u e o allow es ing
cus om high-le el algo i hms o au onomous ope a ion, while a he same ime
enables he use o UAVs om di e en endo s and a wide a ie y o senso s o which
suppo ed d i e s a e al eady a ailable. We ha e made ex ensi e use o he ools
a ailable in ROS o help o debug and de ec issues in bo h ha dwa e and so wa e.
This includes simula ions un in Gazebo, logging and playing back da a om di e en
expe imen s o isualiza ion and inspec ion ools. This is especially use ul when an
6.1 UAV Pla o m 127
inno a i e app oach o algo i hm is unde de elopmen , and he e is a po en ial isk
o c ashing he pla o m. Thanks o he use o hese ools, i is possible o es he
algo i hms and modules o -line using eal ligh da a and debug in e nal s a es o he
UAV o p esen ele an da a in a g aphical in e ace.
As s a ed in Chap e 1, he main con ex in which he p oposed a chi ec u e has
been de eloped and es ed is EuRoC, in which high-le el semi-au onomous ope a ion
o a UAV o inspec ion asks has been demons a ed. The a o emen ioned amewo k
has been designed and implemen ed in o de o pe o m UAV localiza ion wi hou
ex e nal posi ioning sys ems, among o he asks. Ou eam GRVC-CATEC success ully
comple ed S age I o he p ojec (Quali ying Simula ion Con es ), being among he op
15 challenge eams. In o de o gain access o S age II o EuRoC p ojec , an end-use
had o be selec ed in o de o ac ually demons a e he sys em capabili ies, and in
ou case, he end-use is Ai bus De ence&Space (D&S), a wo ld leade in ai c a
manu ac u ing, in e es ed in he au oma ion o logis ic p ocesses in hei manu ac u ing
plan s. The e alua ion o he p oposals was ca ied ou by he Challenge Ad iso y
Boa d (ETH Zu ich among o he ins i u ions) wi h he help o ex e nal e iewe s and
enowned independen expe s, and i was based on no el y o he applica ion, le el
o di icul y o he use case, po en ial ma ke o i , s eng h o he eam, u he o
he ank in he simula ion con es . Ou eam was g an ed access o S age II wi h
he second highes sco e, and we a e cu en ly compe ing wi h o he ou Eu opean
challenge eams o gain access o he inal S age III. The p oposed a chi ec u e has
been de eloped in he con ex o his p ojec , and he UAV has been ex ensi ely es ed
in o de o succeed in he di e en ounds o expe imen s.
6.1 UAV Pla o m
The UAV used in EuRoC’s S age II is a esea ch p o o ype om Ascending Technologies
called AscTec Neo.I is a hexacop e wi h 9” p opelle s which can li up o 2Kg, bu
he maximum nominal ligh ime o 20 minu es (wi hou payload) will be educed
acco dingly. This is a pla o m de eloped and op imized wi hin he amewo k o
di e en Eu opean esea ch p ojec s.

128 Sys em A chi ec u e and F amewo k
Figu e 6.2: The wo main senso s es ed on-boa d he UAV: VI-Senso (le ) and
As a ( igh ).
The de aul main senso on-boa d he pla o m is Skybo ix’s VI-Senso , a s e eo
came a wi h a calib a ed and synch onized IMU. Ne e heless, he o iginal moun
has been modi ied in o de o accommoda e di e en senso s. Ou a chi ec u e has
been alida ed no only wi h he VI-Senso , bu also using an RGB-D came a as
he main senso , in pa icula O bbec’s As a, as i can be seen in Figu e 6.2. The
senso is moun ed acing o wa d in he di ec ion o he ligh in o de o maximize
he in o ma ion ga he ed ega ding possible nea by obs acles. The o ien a ion o he
senso is cus omizable as we ha e manu ac u ed di e en moun s using a 3D p in e .
Figu e 6.3 shows one o he es ed moun s. The selec ed moun will depend on he
expe imen s scena io, he ligh heigh o he ype o obs acles ha he UAV migh
encoun e in i s en i onmen . Depending on he main isual senso moun ed on-boa d
he UAV, he so wa e modules in ol ed wi h senso da a we e adap ed acco dingly in
o de o wo k wi h he ype o da a ha each senso deli e s.
The on-boa d embedded compu e (In el Nex Uni o Compu ing (NUC) wi h
Co e i7) is in cha ge o acqui ing senso da a, es ima ing he ae ial obo cu en
s a e, gene a ing he app op ia e ajec o ies acco ding o a gi en na iga ion goal and
unning he au opilo o con olling he UAV. Gi en all hese equi emen s, special
ca e has been pu o e all he unning modules in o de o cu down he compu a ional
needs when possible.
6.2 Con olled es s 129
Figu e 6.3: A sample o 3D p in ed moun o he main ision-based senso on-boa d
he UAV.
I is impo an o poin ou ha all he esul s shown in his chap e co espond
o di e en ligh s in which he con ol loop is closed using he localiza ion es ima ions
based on on-boa d senso s, which a e compu ed on-line, unlike p e ious chap e s
whe e da a we e p ocessed o -line.
6.2 Con olled es s
To succeed in achie ing obus au onomous na iga ion, he ae ial obo mus demon-
s a e bo h eliabili y and good pe o mance. Ex ensi e ield es ing has aken place
a he indoo es bed o CATEC, shown in Figu e 6.4, whose mo ion cap u e sys em
emendously helped us o benchma k he de eloped algo i hms agains a e y p ecise
g ound- u h o obo localiza ion, mo ion planning o obs acle a oidance wi h sa e y
dis ance assu ance.
The goal o he con olled es s pe o med a his indoo es bed was o moni o
ou de elopmen p og ess and measu e lying skills o he ae ial obo . We also used
he es s o e alua e he e ec s o changes in ha dwa e and so wa e on he UAV.
The lying skills which we ha e sys ema ically es ed o assess ou p og ess we e he
ollowing:
•Abili y o he UAV o au onomously ake-o and pe o m s able ho e ing.
•Abili y o he UAV o accu a ely ollow a p e-planned obs acle- ee ajec o y.
130 Sys em A chi ec u e and F amewo k
Figu e 6.4: One o he es ing scena ios in CATEC’s indoo es bed.
•
Abili y o he UAV o gene a e an obs acle- ee ajec o y in o de o sa ely
each a speci ic waypoin .
•
Abili y o he UAV o dynamically modi y a gene a ed ajec o y owa ds a
waypoin o a oid pe cei ed obs acles along he way.
As i is appa en in Figu e 6.4, he es bed s uc u e allowed us o secu e he
UAV om he op ia a sa e y ope o p e en an undesi ed ligh e mina ion. This
has been e y use ul du ing he de elopmen p ocess, as es ing new ea u es in he
algo i hms may some imes esul in unexpec ed beha io s.
6.3 EuRoC Benchma king
The i s pa o S age II consis ed o pe o ming a se ies o asks de ined by he
Challenge Hos (ETH Zu ich), which we e he same o all he eams in he same
lying a ena wi h he same UAV pla o m. This sec ion b ie ly desc ibes ou expe ience
6.3 EuRoC Benchma king 131
Figu e 6.5: ETH’s lying a ena used in he Benchma king and F ee-S yle ounds.
and esul s ob ained om he expe imen s ca ied ou du ing he summe o 2016
ega ding he Benchma king ound, ha ook place in ETH Zu ich (see Figu e 6.5).
The UAV unning he p oposed a chi ec u e has been ex ensi ely es ed in o de
o ul ill he p oposed asks, which we e mean o guide ou de elopmen owa ds
high-le el au onomous asks. Ou sys em elied on a Senso -based Odome y based on
he isual-ine ial odome y algo i hm desc ibed in Chap e 3, ga he ing senso da a
om he on-boa d VI-Senso s e eo came a. The Robo Localiza ion module used in
his expe imen ound was based on S e eo Pa allel T acking and Mapping (S-PTAM)
Pi e e al. (2015), a s e eo SLAM sys em which we adap ed in o de o make i wo k
wi hin ou a chi ec u e and using ou odome y es ima ions along wi h he acqui ed
s e eo images.
A se ies o Benchma king expe imen s we e p oposed, which consis ed o se e al
asks ha he ae ial obo should pe o m, and me ics o e alua e hem.
•Task 1 - Sys em Se up
: his ask simply benchma ks he ime ha eams
need o se up he pla o m ou o he anspo a ion box un il s able ho e ing
a 1 me e o al i ude abo e he ake-o loca ion.
•Task 2 - S a e Es ima ion
: he pu pose o his ask is o assess he d i
o he isual localiza ion sys em and s a e es ima o . Collision- ee andom