The IMU/UWB Fusion Positioning Algorithm Based on a Particle Filter

Transcription

1 Inernaional Journal Geo-Informaion Aricle The IMU/UWB Fusion Posiioning Algorihm Based on a Paricle Filer Yan Wang and Xin Li * School Compuer Science and Technology, China Universiy Mining and Technology, Xuzhou , China; wysephen@cum.edu.cn * Correspondence: xinli@cum.edu.cn Received: 9 June 2017; Acceped: 3 Augus 2017; Published: 7 Augus 2017 Absrac: This paper inegraes UWB (ulra-wideband) and IMU (Inerial Measuremen Uni) daa o realize pedesrian posiioning hrough a paricle filer in a non-line--sigh (NLOS) environmen. Afer acceleraion and angular velociy are inegraed by ZUPT-based algorihm, velociy and orienaion fee are obained, and n velociy and orienaion whole body are esimaed by a virual odomeer mehod. This informaion will be adoped as prior informaion for paricle filer, and observaion value UWB will ac as basis for weigh updaing. According o experimenal resuls, prior informaion provided by an IMU can be used o resrain observaion error UWB under an NLOS condiion, and posiioning precision can be improved from posiioning error 1.6 m obained using pure UWB-based algorihm o approximaely 0.7 m. Moreover, wih high compuaional efficiency, his algorihm can achieve real-ime compuing performance on ordinary embedded devices. Keywords: UWB/INS; ZUPT; paricle filer; indoor locaion 1. Inroducion Wih emergence small low-cos IMU (Inerial Measuremen Uni) chips, IMU-based indoor posiioning echnology has developed quickly [1 4]. However, IMU-based mehod has an accumulaion error ha is proporional o hird power ime [5]. To reduce speed error accumulaion, one common approaches is o use ZUPT (Zero Velociy Updae) algorihm [6] o consrain posiioning drif. This is because i can no only uilize fac ha velociy is zero a momen when foo ouches ground bu also consrain posiioning drif when zero velociy is aken as observaion EKF (exended Kalman filer) algorihm. The applicaion his echnology has improved posiioning precision and algorihm sabiliy. However, since IMU-based algorihm can only acquire relaion beween relaive posiion and iniial sae, posiioning error will sill grow linearly over ime, despie adopion Zero Velociy Updae algorihm. More imporanly, alhough PDR (pedesrian dead reckoning) algorihm has good posiioning precision in case recilinear movemen, i will resul in big angular deviaion overall due o accumulaion sligh angular deviaions a corners. Afer ha, rajecory will deviae even more seriously. Therefore, performance PDR algorihm will be degraded by presence many corners. In view his, one mehod is o use a magneomeer for consrain [7,8]. However, as magneomeer can be easily affeced by many facors, such as large meal pieces, pedesrians, and walls, consrain effec is no ideal. Then, o acquire a sable and precise posiioning resul afer a long ime, i s necessary o inroduce daa from or sensors o impose a consrain on posiioning resul for conrol error growh. To resrain error accumulaion an IMU-based algorihm, algorihm can be combined wih or posiioning mehods wihou error accumulaion o achieve beer resuls. In my opinion, ISPRS In. J. Geo-Inf. 2017, 6, 235; doi: /ijgi

2 ISPRS In. J. Geo-Inf. 2017, 6, se mehods can be grouped ino wo major ypes: laser or visual posiioning sysem based on a given map and posiioning sysem based on wireless sensors. The former is mainly characerized by cenimere-level precision [9 11]. However, such a visual or laser-based posiioning sysem requires consumpion compuing resources, so generally, real-ime calculaions can only be performed on devices wih powerful compuing abiliies. Since i uses a complex process o build a global map in early sage, i s no applicable o rapidly changing scenes, as accuracy global map grealy influences posiioning precision, and any change in map will also affec posiioning resul. For laer, Blueooh and WIFI-based posiioning mehod achieves a slighly lower accuracy, which only reaches approximaely 3 m [12,13], while UWB-based algorihm achieves a beer posiioning resul wih decimere-level precision [14,15]. Neverless, since a wireless signal-based posiioning sysem wih cerain robusness in a dynamic environmen does no have a high requiremen for compuing resources during posiioning calculaion, i s highly applicable o consrucion a low-cos, indoor posiioning sysem wih inegraion IMU. Based on a similar idea, some researchers have already inegraed odomeer (which is also an incremenal posiioning sysem) wih UWB (ulra-wideband) echnology for posiioning a wheeled robo [16]. For a wheeled robo, as UWB sensor can be easily conneced wih odomeer, such a consrain will faciliae. However, in case pedesrian posiioning, in inegraion IMU-based posiioning sysem wih UWB-based posiioning sysem, following facors mus be aken ino accoun: mehod for placing boh sensors and esablishmen a relaionship beween m. For purpose zero-speed deecion, IMU mus be mouned on a foo. However, UWB sensor mus be mouned on head or shoulder o reduce blockage UWB signal by human body due o is grea influence when i s mouned on a foo. Therefore, his paper adops a mehod by mouning UWB on head and IMU on a foo. Based on choice IMU as core, many researchers ake UWB daa as posiional observed values and add m ino EKF algorihm, which is based on zero velociy, o realize [5,17]. Neverless, as EKF is, in essence, linear approximaion observaion equaion, i can hardly achieve a good approximaion UWB sensor [16] based on a highly nonlinear observaion model. Therefore, his paper adops a paricle filer algorihm o fuse daa from boh sensors. I akes IMU-based calculaion resul as prior informaion for paricle filer, and uses UWB observaion as observed value paricle. Secion II inroduces general framework for posiioning based on UWB and IMU, and describes UWB and IMU separaely. Secion III provides relevan experimenal resuls and compares posiioning precisions obained based on algorihm and pure applicaion daa from each sensor, in addiion o analysis error source during posiioning. The las secion is conclusion his paper. 2. UWB and IMU Fusion Algorihm 2.1. Problem Descripion Aiming a sae esimaion problem, one common approaches is Bayesian filer echnology, which provides a general framework for esimaing sysem sae based on is observed value. I uses a sae and corresponding confidence coefficien (in form a covariance marix) his sae o esimae sysem sae. For linear problem, Kalman filer uses a Gaussian disribuion o describe sae disribuion. The EKF algorihm also uses Gaussian disribuion o describe sae disribuion for linear approximaion a nonlinear sysem. This is because a Gaussian disribuion is obeyed only in a linear sysem. In conras, paricle filer can address all non-linear or non-gaussian problems when many paricles disribued in sae space approximae sae probabiliy disribuion. As ZUPT-based PDR algorihm can be linearized perfecly, assume ha observed value is zero velociy obained from zero-velociy deecion. Then, observaion equaion is

3 ISPRS In. J. Geo-Inf. 2017, 6, ISPRS In. J. Geo-Inf. 2017, 6, linear. Therefore, EKF algorihm has minimum linear error among se ypes algorihms. Meanwhile, EKF algorihm as can frequency reduce consumpion oupuing daa compuing from an IMU resources. is very high, Hence, i s adopion feasible o adop simple EKF algorihm can in reduce process where consumpion IMU daa compuing are used resources. o calculae Hence, velociy i s feasible and odirecion adop EKF pedesrian algorihm movemen. in process The key where o his process IMU daa mainly are used lies in o calculae linear represenaion velociy and direcion sae ransiion pedesrian equaion movemen. and observaion The key oequaion his process wih mainly specific lies inprocedure linear provided represenaion in Secion 1.3. sae ransiion Wih equaion regard o and observaion inegraed filering, equaionor wih in or specific words, procedure filering provided in IMU-based Secion 2.3. resul and WihUWB regard observaion, o inegraed PF filering, (paricle or filer) in or framework words, is adoped fileringas indicaed IMU-based in Figure resul 1. and The main UWB reason observaion, is ha UWB PF (paricle observaion filer) model framework is a highly is adoped non-linear as indicaed model inand Figure canno 1. The be main well reason described is ha by a linear UWBmodel. observaion Meanwhile, model re is a highly is a large non-linear error when model andgaussian canno be disribuion well described is used by ao linear approximae model. Meanwhile, observaion re is error a large probabiliy error when disribuion. Gaussian Through disribuion paricle is usedfiler o approximae algorihm, a large observaion number error paricles probabiliy can perform disribuion. very well Through in approximaing paricle filer sysem algorihm, sae aunder large number non-linear paricles condiions. can perform very well in approximaing sysem sae under non-linear condiions. Figure The algorihm flowchar. In paricle filer algorihm, assume ha number given beacons is N and In paricle filer algorihm, assume ha number given beacons is N and posiions posiions all beacons in hree-dimensional space are known and can be expressed by [B k N ] k = 1. The all beacons in hree-dimensional space are known and can be expressed by [B k] N. The variables variables known a ime include collecion M paricles a ime 1 and k = weigh 1 every known a ime include collecion M paricles a ime 1 and weigh M every paricle a [i] M [i] [i] paricle a ime 1, which can be expressed by [S 1 ] i = 1 and [weigh 1 ], respecively. s 1 ime 1, which can be expressed by [S [i] i = 1 1 ]M and [weigh[i] i = 1 [i] 1 ]M, respecively. s[i] i = 1 1 represens represens sae ih paricle a ime 1 and weigh 1 is corresponding weigh sae ih paricle a ime 1 and weigh [i] ih paricle a ime 1. Assume ha curren sysem 1 is corresponding weigh ih paricle sae only depends on sysem sae a a ime 1. Assume ha curren sysem sae only depends on sysem sae a previous previous momen and curren inpu. Then, sysem sae can be updaed according o momen and curren inpu. Then, sysem sae can be updaed according o following seps, following seps, which are illusraed by flowchar in Figure 1: which are illusraed by flowchar in Figure 1: 1. As indicaed by Sample in corresponding flowchar, perform sampling according o 1. As indicaed by Sample in corresponding flowchar, perform sampling according o disribuion sae ransiion equaion. disribuion sae ransiion equaion. 2. As indicaed by Evaluae in corresponding flowchar, updae weigh every paricle 2. according As indicaed o by Evaluae observaion in model corresponding and observaion flowchar, values. updae weigh every paricle 3. As according indicaed o by Resample observaion in model corresponding and observaion flowchar, values. use resampling mehod o resrain 3. As indicaed paricle aenuaion. by Resample in corresponding flowchar, use resampling mehod o resrain paricle aenuaion. As indicaed by Ge Resul, a sub-process is uilized o oupu average posiion afer observaion values UWB sensor and IMU sensor are considered; in corresponding flowchar, posiioning resul is oupued by paricle filer before resampling. I can be used as posiioning resul whole posiioning algorihm afer average weighing paricle

4 ISPRS In. J. Geo-Inf. 2017, 6, As indicaed by Ge Resul, a sub-process is uilized o oupu average posiion afer observaion values UWB sensor and IMU sensor are considered; in corresponding flowchar, posiioning resul is oupued by paricle filer before resampling. I can be used as posiioning resul whole posiioning algorihm afer average weighing paricle sae. I's worh noing ha his weigh is, in effec, resul normalizing all weighs. The key o paricle filer sep is selecion sae ransiion equaion and observaion model. For former, i requires oupu from virual odomeer sysem o esimae change in moion sae. For laer, i requires UWB observaion daa o correc sae esimaion. They funcion oger o obain poserior disribuion sae represened by a paricle and corresponding weigh, which will be discussed in Secion 2.4. Figure 1 shows overall algorihm flowchar. If IMU daa are received, sae ransiion model is used o updae curren sysem sae and esimae if i s in a zero-velociy sae. If velociy is zero, n zero velociy is updaed o correc sae and oupu from visual odomeer is calculaed based on updaed sae. Neverless, if UWB daa are received, UWB daa are used as observaion values o obain poserior disribuion curren posiion and sae esimae is oupu afer updaing paricle based on informaion from visual odomeer. Finally, re-sampling is performed o alleviae paricle degeneracy problem The Velociy and Direcion Virual Odomeer Mehod Generally, oupu frequency a UWB ag is beween 2 and 4 Hz, while oupu frequency an IMU is beween 100 and 1000 Hz. Since re is a big difference beween daa oupu frequencies sensors, i is necessary o align m on ime axis before inegraing m. The algorihm in his secion consrucs a ransformaion beween se wo sensors wihou a rigid connecion. Firs, his paper direcly uses IMU daa o perform a calculaion based on PDR algorihm. Then, i akes resul PDR algorihm as a virual odomeer o obain a prior esimae on whole movemen. Finally, i uses UWB daa as observaion o consrain predicion resul and obain poserior esimae on overall movemen sae o avoid a large difference beween oupu frequencies wo daa sources. Meanwhile, consrain ha human body as a whole moves in same direcion esablishes a connecion beween sensors. Thus, ir can be realized. In real scenarios, zero-velociy phenomenon can inermienly occur when people are walking. In normal cases, human head moves roughly in same direcion and a same speed as human body. To esimae movemen sae head based on foo movemen, following relaion can be uilized. Assume ha v f is velociy foo movemen, θ f is direcion foo movemen, v b is velociy body movemen and θ b is angle body movemen. Alhough i s very difficul o express relaionship beween insananeous velociy and angle foo movemen wih velociy and angle body movemen, wihou loss generaliy, we can assume ha average velociy v f from momen foo sars o move o momen i sops moving is equal o velociy body movemen v b. Then, according o foo and body movemen rule, which will be explained laer, we obain following equaion: ˆv b = ˆv f 1 0 ( 2 0 ), (1) where 0, 1, and 2 represen momens when foo sars o move, momen i sops moving, and momen i begins o move again, respecively. I is clearly shown in Figure 2 ha righ foo is only moved in period beween 0 and 1, and whole body is moved hroughou enire period (from 0 o 2 ). Since whole body is conneced wih righ foo, displacemens se wo pars are equal; hus, average velociy righ foo beween 0 and 2, which is on righ-hand side equaion (1), mus equal o average velociy whole body, which is on

5 ISPRS In. J. Geo-Inf. 2017, 6, lef-hand side equaion. In addiion, ˆv f can be obained by dividing foo displacemen in period beween 0 and 1 by ime difference 2 0. Figure 2. Saes foo corresponding o differen ime poins. Similarly, re is no firm correlaion beween direcion body movemen and direcion foo movemen. However, re is a relaionship beween mean moving direcion foo ˆθ f and mean moving direcion body ˆθ b : ˆθ b = ˆθ f (2) where ˆθ f is direcion vecor from locaion foo a 0 o locaion foo a 1. So far, we have esablished formulable relaion beween IMU sensor and UWB sensor The ZUPT-Based Algorihm in IMU The Zero Velociy Updae (ZUPT)-based algorihm in IMU mainly serves o provide necessary informaion for esimaion human movemen sae by a virual odomeer. A ime K, sae IMU can be represened by X k R 9, which includes coordinaes IMU in iniial coordinae sysem, velociy, and aiude a ime : X k = f (X k 1, c k, w k ), (3) where f is sae ransiion funcion, c k R 6 represens oupu from an IMU sensor, including curren angular velociy ω k R 3 and acceleraion α k R 3 IMU, while w k R 3 is sysem process noise. In calculaion process, assume ha w k N(0, Q k ), or in or words, assume ha sysem noise is whie Gaussian noise, where Q k is covariance marix sysem noise. To ieraively calculae sae covariance marix P k, linearize sae ransiion funcion a X k o obain following approximae expression: X k = F k X k 1 + G k w k, (4) where F k and G k represen sae ransiion marix and sysem noise gain marix, respecively. In addiion, sysem sae covariance marix P k can be updaed according o following formula: P k = F k P k 1 F T k + G k QG T k. (5) Obviously, P k R 9 9 represens confidence level curren sysem sae esimae. To consrain error accumulaion based on ime sequence, i s necessary o add an observaion o sae updae as a consrain o increase posiioning precision. In pure IMU-based PDR posiioning, generally a zero-velociy deecor is adoped o obain a zero-velociy observaion o consrain curren resul. As such a consrain can effecively reduce speed error accumulaion, his paper adops acceleraion-magniude deecor [18] as zero-velociy deecion algorihm

6 ISPRS In. J. Geo-Inf. 2017, 6, based on generalized likelihood raio es (GLRT). Through zero-velociy deecion, a virual movemen velociy zero will be obained as observaion value, ha is o say, Y k = [0, 0, 0] T, which represens ha movemen velociies on all hree axes are zero. A his momen, observaion equaion is linear, which can be expressed as follows: Y k = H k X k + v k, (6) where v k represens curren observaion noise wih v k N(0, R k ) and H k = [0 I 0]. All elemens in 0 R 3 3 are 0 and I R 3 3 is an ideniy marix. In general, R k represens confidence level curren observaions. To simplify algorihm, i s feasible o choose a consan value. According o Kalman filer model, i s necessary o updae prediced value based on observaion value. When perurbaion sysem sae a momen k is defined as δx k R 9, updaed perurbaion can be calculaed according o following formula: δx = K k (Y k H k X k ). (7) When relaionship beween real sysem sae X K, soluion ˆX k navigaion algorihm, and sysem sae perurbaion can only be represened by a nonlinear funcion, denoed a Γ, i can be expressed as follows: X k = Γ( ˆX k, δx k ) (8) In fac, such funcions play an imporan role in correcing sysem sae afer δx and X k are calculaed. To secure robusness and sabiliy covariance updae, covariance updae equaion can be expressed as below in Joseph sabiliy form: P k = (I K k H k )P k (I K k H k ) T + K k R k K T k (9) So far, we have calculaed movemen rajecory foo-mouned IMU and correced movemen rajecory based on zero-velociy sae. The pseudo code for whole process is provided in Algorihm 1. Algorihm 1 Pseudo code for ZUPT 1: k := 0 2: Iniial(X 0, P 0 ); 3: While 4: k := k + 1 5: Xˆ k := f(x k 1, c k, ω k ) 6: P k := F k P k 1 Fk T + G kqgk T 7: if(zerovelociy(c k ) = True) 8: K k := P k Hk T ( Hk P k Hk T + R 1 k) 9: δx k := K k (Y k H k Xˆ k ) 10: X k := Γ( Xˆ k, δx k ) 11: VirualOdomeer() // The virual odomeer mehod provided in Secion : P k := (I K k H k )P k (I K k H k ) T + K k R k Kk T 13: end if 14: end while 2.4. The Fusion UWB and IMU Based on Paricle Filer Afer paricle sampling according o previous paricle sae and oupu u virual odomeer a ime, prior disribuion paricles is obained. The paricle updae mus conform

7 ISPRS In. J. Geo-Inf. 2017, 6, o disribuion p(s [i] f (s [i] 1, u )), where f is sae ransiion equaion. Generally, pedesrian movemen can be described perfecly as uniform moion or uniformly variable moion. Considering ha direc applicaion movemen velociy and angle requires alignmen boh coordinae sysems and relaionship beween boh sensors is no currenly clear, i s feasible o ake raes change in velociy and angle as connecion beween se wo coordinae sysems o simplify operaion and modelling. Therefore, paricle sae [ includes ] coordinae, linear velociy, and angle. A his poin, paricle sae vecor s = x b, yb, vb, θb. Hence, inpu paricle filer algorihm from corresponding virual odomeer is u = equaion is nonlinear, updaed vecors can be expressed as below: [ δv b, δθb ]. As sae ransiion v b[i] = v b[i] 1 + δvb, (10.1) x b[i] y b[i] θ b[i] = x b[i] 1 + vb[i] = y b[i] 1 + vb[i] Then, prior probabiliy follows Gaussian disribuion: = θ b[i] 1 + δθb[i] (10.2) cos (θ b[i] ), (10.3) sin (θ b[i] ). (10.4) p(s [i], s[i] 1, u ) N(s [i] f (s [i] 1, u ), W k ) (11) A his poin, W k is sill a second-order diagonal marix. Assume ha δv b and δθ b are independenly and idenically disribued. Then we have: W k = [ σ 2 δv 0 0 σ 2 δθ where σ δv and σ δθ represen sandard deviaions velociy and angle variaions, respecively. Through above formulas, approximaion prior disribuion a ime can be achieved. Then, we need o updae paricle weigh according o observaion o obain poserior disribuion paricle. In addiion, we updae paricle weigh based on weigh afer paricle sampling according o daa from virual odomeer and curren UWB observaion. In or words, we muliply prior probabiliy by likelihood probabiliy o obain poserior probabiliy wih updae formula provided below: weigh [i] ] (12) weigh [i] 1 p( z s [i] ), (13) where z = [ d 1, d 2,..., d j ] is vecor disances from every beacon o ag, and dj is disance beween jh beacon o curren ag a ime. As weigh, in essence, represens poserior probabiliy relevan paricle sae, i s possible o normalize all weighs. Wihou loss generaliy, likelihood probabiliy can be expressed in following scalar form: p( z s [i] ) = N p(z j s [i] ). (14) j = 1 In addiion, likelihood funcion can be expressed as below according o UWB observaion model: ( ) ( s [i] g p z j N (z j dis(s [i] ) ), j), σ d (15)

8 In his paper, experimens have been conduced in an area wih wo bearing pillars wihin a hall ha occupies an area approximaely m 2. The cross-secional area bearing pillar is m 2. Four UWB beacons are se up in experimenal scene, where disribuion beacons and bearing pillars can be found in various rajecory chars. All beacon anennas are locaed a a disance 1.12 m above ground and exposed o air. In experimen, UWB and IMU daa have been colleced along wo pahs, where walking velociy has been conrolled wihin 1.5~2.5 m/s. Throughou calculaion process, all ISPRS In. J. Geo-Inf. 2017, 6, where σ d is sandard deviaion UWB observaion value and g is error model UWB signal. This paper assumes ha sensor measuremen value is a real value and mean σ d is Gaussian disribuion sandard deviaion; dis(s [i], j) represens curren disance beween ih paricle and jh beacon. 3. Experimens and Resuls In his secion, wo experimens are conduced o verify ha algorihm (denoed as Fusing) ouperforms paricle filer algorihm using only UWB daa (denoed as Only-UWB) and zero-velociy-updae-based EKF algorihm (denoed as ZUPT) using only IMU daa in erms posiioning precision in an NLOS (No Line Sigh) environmen. Since general conrol-poin-based posiioning precision calculaion mehod canno reflec posiioning error very well a every ime poin in process, his paper adops landmark-based approach o calculae real rajecory. Based on high-precision posiion a each momen, reference disances from ag o each beacon are calculaed and play an imporan role in explaining benefis algorihm in Secion This secion is organized as follows. Firs, i inroduces experimenal scene and seps. Then, i presens mehods used o obain real rajecory. Finally, i presens and discusses ISPRS In. J. Geo-Inf. 2017, 6, experimenal resuls. This secion is organized as follows. Firs, i inroduces experimenal scene and seps Experimenal Then, i presens Scene and Mehod mehods used o obain real rajecory. Finally, i presens and discusses experimenal resuls. Figure 3 shows how sensors are mouned. The UWB sensor was developed based on DW1000 and has a oreical 3.1. Experimenal precision Scene and Mehod approximaely 30 cm in indoor posiioning. The frequency a which UWB sensor Figure reurns 3 shows daa how is approximaely sensors are mouned. 2 Hz. The To UWB avoid sensor was shielding developed effec based on DW1000 human body on UWB and sensor, has a oreical UWB precision mus be approximaely mouned on 30 cm in op indoor posiioning. helme. The However, frequency a which IMU sensor will be mouned UWB on sensor a foo; reurns hisdaa sensor is approximaely has a daa2 oupu Hz. To avoid frequency shielding approximaely effec human 128body Hz. The IMU on UWB sensor, UWB mus be mouned on op helme. However, IMU sensor is calibraed before experimens, which means ha gyro bias and acceleraion bias are considered will be mouned on a foo; his sensor has a daa oupu frequency approximaely 128 Hz. The IMU during is preprocessing calibraed before procedure experimens, inwhich IMU means sensor. ha gyro To bias ensure and acceleraion accuracy bias are considered zero-velociy deecion, during IMU preprocessing mus be secured procedure a in fron IMU sensor. foo, To ensure and camera accuracy used zero-velociy o acquire image for calculaion deecion, IMU real mus rajecory be secured a mus fron be secured foo, a and fron camera used o helme; acquire image camera has a for calculaion real rajecory mus be secured a fron helme; camera has a resoluion 1080*1920 and a frame rae 30 FPS. Before being used, camera should be calibraed resoluion 1080*1920 and a frame rae 30 FPS. Before being used, camera should be calibraed and is inernal and is parameers inernal parameers should should be be deermined. To Toensure ensure sabiliy sabiliy acquired acquired daa, daa, experimen experimen should begin should five begin minues five minues afer afer saring up sensors. Figure Figure 3. A3. phoo A phoo aa person wearing devices devices.

9 ISPRS In. J. Geo-Inf. 2017, 6, In his paper, experimens have been conduced in an area wih wo bearing pillars wihin a hall ha occupies an area approximaely m 2. The cross-secional area bearing pillar is m 2. Four UWB beacons are se up in experimenal scene, where disribuion beacons and bearing pillars can be found in various rajecory chars. All beacon anennas are locaed a a disance 1.12 m above ground and exposed o air. In experimen, UWB and IMU daa have been colleced along wo pahs, where walking velociy has been conrolled wihin 1.5~2.5 m/s. Throughou calculaion process, all iniial values for algorihms are given by reference rajecory. Wih ZUPT-based algorihm, iniial movemen direcion is calculaed based on real rajecory in firs hree seconds. Pah I is relaively simple, along which eser walks around whole experimenal field wih a moving rajecory similar o a recangle. Pah II is more complicaed. Par pah has a large UWB observaion error based on Pah I. I s creaed o reflec beer robusness algorihm in an NLOS environmen. The deailed pahs will be described separaely in following secions The Acquisiion Real Trajecory This experimen uses landmark-based visual posiioning sysem o obain real rajecory curren sysem sae. This approach uses square fiducial marker wih a given dimension ( edge lengh) as ag o calculae relaive posiion beween camera and fiducial marker [19]. Before use his sysem, i s necessary o deploy cameras o ake picures all landmarks in enire experimenal field. If re are wo or more wo fiducial markers in same frame an image, consrain relaion (which can be expressed by 6-DOF roaion or ranslaion) beween se fiducial markers can be esablished. To ensure ha sysem sae in a unified coordinae sysem can be recovered based on picure a random fiducial marker, his paper adops graph opimizaion [20] mehod o deermine fiducial marker and pose camera when picures muliple fiducial markers are aken as verices a graph for opimizaion. In addiion, when roaion and ranslaion from fiducial marker o camera a each momen is aken as edge, relaive relaion beween all fiducial markers can be obained globally afer ieraive compuaion minimum errors. In posiioning process, picure all fiducial markers in an image frame is used o esimae curren pose camera and exclude all esimaion resuls wih apparen errors, o obain rue value curren sysem sae. Based on his mehod, indoor posiioning precision can reach 7 cm [21]. Since graph opimizaion mehod is adoped in his process, accumulaed error will be limied by closed-loop deeced in whole experimen. The pose has similar scale o covariance hroughou whole process. Since pose is necessary for calculaing a relaively accurae posiion, his feaure plays an imporan role in error analysis UWB observaion values Experimenal Resuls and Comparison Comparison Various Algorihms in Pah I During experimen, IMU and UWB daa are colleced simulaneously o be used for posiioning based separaely on ZUPT, Only-UWB, and algorihms, in addiion o comparison posiioning resuls. To beer reflec effec algorihm on improving precision, Only-UWB-based posiioning algorihm does no direcly use rilaeral posiioning resul ha is obained based on daa from UWB sensor. Insead, i uses same paricle filer ha is adoped in algorihm for posiioning. The difference beween algorihms lies in sampling procedure, where Only-UWB-based algorihm assumes ha boh moving velociy and angle are kep unchanged, based on which new paricles are generaed. In or words, Only-UWB-based algorihm enables paricles o obey Gaussian disribuion wih mean equal o mos recen sae in sampling process. In experimen, Only-UWB-based algorihm uses 4000 paricles in calculaion, where σ δv = 0.1 m s, σ δθ = 0.2 rad, and σ d = 0.5 m.

10 ISPRS In. J. Geo-Inf. 2017, 6, In algorihm, all or parameers are same as hose used in Only-UWB-based algorihm, excep ha σ δv = 0.05 m/s and σ δθ = 0.1 rad. This is because algorihm can use direcion informaion from virual odomeer o perform sampling on a small scale. However, wih Only-UWB-based algorihm, sampling mus be performed on a larger scale due o lack relevan prior informaion. Hence, σ δθ and σ δv can be assigned smaller values in algorihm. The moving rajecory is relaively simpler along Pah I, where eser direcly walks around hall hree imes in clockwise direcion. Figure 4 shows reference rajecory along Pah I and differen posiioning resuls obained based on various algorihms. The red line indicaes reference rajecory along pah, purple line represens pah calculaion resul based on Only-UWB-based algorihm and blue line is calculaion resul based on ZUPT-based ISPRS In. J. Geo-Inf. 2017, 6, algorihm. Meanwhile, green line is posiioning resul based on algorihm, black squares obvious sand ha boh for pillar Only-UWB-based obsacles andalgorihm red diamonds and represen algorihm beacon significanly posiions. ouperform I s very obvious ZUPT-based ha bohalgorihm Only-UWB-based in erms algorihm posiioning and precision. algorihm Wha s more, significanly ouperform algorihm can ZUPT-based beer reflec algorihm real movemen in erms rajecory posiioning locally precision. han Wha s Only-UWB-based more, algorihm. algorihm This canis beer because reflec alhough real movemen Only-UWB-based rajecoryalgorihm locally han can achieve Only-UWB-based a smoor algorihm. esimae This is rajecory, because alhough which o some Only-UWB-based exen migh improve algorihm can posiioning achieve aprocess smoor afer esimae addiion rajecory, velociy whichand o some direcion exen migh movemen, improve assumpion posiioning uniform process afer linear moion addiion mus be added velociy ino and direcion algorihm movemen, due o lack assumpion or daa uniform sources linear on moion changes mus in be added reference ino velociy algorihm and direcion due o lack movemen. or If his daa assumpion sources on has a high changes confidence in reference coefficien velociy (a smaller and sampling direcion range), movemen. alhough If his i guaranees assumpion ha has a high Only-UWB-based confidence coefficien algorihm (a smaller will have sampling a srong range), ani-jamming alhough capabiliy i guaranees for ha movemen Only-UWB-based along a sraigh algorihm line, leads willo have a slow a srong response ani-jamming o inflecion capabiliy poin. forin movemen his case, along i akes a sraigh quie a line, long i ime leads for o a slow posiioning responseresul o based inflecion poin. correc In his UWB case, measuremen i akes quieo a long reurn ime o for posiion posiioning near resul real based coordinaes on correc afer UWB inflecion measuremen poin or oa reurn deviaion o from posiion real near rajecory. real coordinaes However, if afer his assumpion inflecion has poin a or low a deviaion confidence from coefficien, real rajecory. posiioning However, precision if his migh assumpion be easily has affeced a lowby confidence observaion coefficien, noise posiioning UWB sensor. precision In conras, migh be easily affeced algorihm by won observaion lead o noise large error UWB a sensor. inflecion In conras, poin due o availabiliy algorihm won lead reference o largeprovided error a by inflecion IMU poin daa. due Moreover, o availabiliy i is highly robus reference agains provided incorrec by UWB IMU daa daa. when Moreover, movemen i is highly is in robus a sraigh agains line. The incorrec deails will UWB be inroduced daa when in Secion movemen is in a sraigh line. The deails will be inroduced in Secion Figure 4. The comparison rajecories along Pah I. I. Figure 5 demonsraes cumulaive error probabiliy disribuion Pah I. I. I I clearly reveals ha algorihm has a big big advanage in in posiioning precision over Only-UWB-based algorihm and ZUPT-based algorihm. Table 1 provides operaion ime, mean posiioning error, and sandard deviaion errors for each algorihm. These resuls reveal ha algorihm can achieve a smaller mean error han Only-UWB-based algorihm wih a lower sandard deviaion errors. This means ha if compuing ime is same order magniude, algorihm can provide a higher precision wih beer sabiliy and similar algorihmic complexiy. Table 1. Algorihm error and compuing ime in Pah I.

11 ISPRS In. J. Geo-Inf. 2017, 6, ISPRS In. J. Geo-Inf. 2017, 6, Figure5. The cumulaive error probabiliy in in Pah I. I Table Comparison 1 provides Various operaion Algorihms ime, mean in Pah posiioning II error, and sandard deviaion errors Figure for each 6 shows algorihm. comparison These resuls reveal relevan ha rajecories algorihm along Pah can achieve II. The a direcion smaller mean error movemen han along Only-UWB-based Pah II is algorihm same as wih ha a along lower Pah sandard I. However, deviaion for errors. a long This ime, means re ha is a if significan compuing signal ime error is due o same shielding order magniude, beacons by pillar algorihm in exended can provide secion a higher precision pah. Due wih o beer presence sabiliy many and similar corners algorihmic in movemen complexiy. process, ZUPT-based algorihm can provide high precision, which is refleced by a significan error in rajecory direcion calculaed Table 1. Algorihm error and compuing ime in Pah I. based on ZUPT-based algorihm afer some corners. However, i can sill achieve reasonable accuracy in displacemen disance. Tha is o say, ZUPT-based algorihm is sill high Algorihm Mean Error (m) Sandard Deviaion Errors (m) Time Offline Calculaion (s) reference value in esimaion moving velociy. Meanwhile, as ZUPT-based algorihm can ZUPT ensure ha angle calculaion resul for a single corner does no vary much from real value, Only-UWB i s sill Fusing significan effeciveness when acing as inpu algorihm. 3.22The Only-UWBbased rajecory demonsraes similar characerisics pahs. In or words, i can provide high Comparison accuracy under LOS Various condiions. Algorihms However, in Pah grea II deviaions will arise when re is a barrier nearby. This is mainly because, a his momen, re is a large error in UWB observaion value, which Figure can be 6 shows observed in comparison Figure 7, where relevan red line rajecories represens along Pah disance II. The beween direcion beacon movemen and ag along afer Pah calculaion II is same based ha on along reference Pah I. rajecory. However, for The arms long ime, errors re isbeacons a significan over signal whole errorprocess due o are 0.456, shielding 0.626, 0.737, beacons and by pillar The blue in line exended sands for secion measuremen pah. value Due o presence UWB sensor. many corners in movemen process, ZUPT-based algorihm can provide high precision, which is refleced by a significan error in rajecory direcion calculaed based on ZUPT-based algorihm afer some corners. However, i can sill achieve reasonable accuracy in displacemen disance. Tha is o say, ZUPT-based algorihm is sill high reference value in esimaion moving velociy. Meanwhile, as ZUPT-based algorihm can ensure ha angle calculaion resul for a single corner does no vary much from real value, i s sill significan effeciveness when acing as inpu algorihm. The Only-UWB-based rajecory demonsraes similar characerisics pahs. In or words, i can provide high accuracy under LOS condiions. However, grea deviaions will arise when re is a barrier nearby. This is mainly because, a his momen, re is a large error in UWB observaion value, which can be observed in Figure 7, where red line represens disance beween beacon and ag afer calculaion based on reference rajecory. The RMS errors beacons over whole process are 0.456, 0.626, 0.737, and The blue line sands for measuremen value UWB sensor. Figure 6. The comparison rajecories along Pah II.

12 nearby. This is mainly because, a his momen, re is a large error in UWB observaion value, which can be observed in Figure 7, where red line represens disance beween beacon and ag afer calculaion based on reference rajecory. The RMS errors beacons over whole process are 0.456, 0.626, 0.737, and The blue line sands for measuremen value UWB sensor. ISPRS In. J. Geo-Inf. 2017, 6, ISPRS In. J. Geo-Inf. 2017, 6, 235 Figure Figure The The comparison comparison rajecories rajecories along along Pah Pah II. II Figure7. 7. The comparison beween UWB measuremen values and reference values in Pah II. Compared wih Only-UWB-based algorihm, algorihm algorihm can provide can provide beer precision beer precision under NLOS under condiions. NLOS condiions. This is mainly This because is mainly because prior informaion prior informaion abou direcion abou andirecion velociy and body velociy movemen body obained movemen from obained ZUPT-based from ZUPT-based algorihm can algorihm ac as a consrain. can ac as a The consrain. disribuion The disribuion likelihood funcion likelihood around funcion around real posiion real in an posiion LOS environmen in an LOS environmen is providedis inprovided Figure 8, in where Figure 8, cenral where red cenral poin represens red poin represens curren posiion. curren The posiion. ligherthe ligher colour, colour, larger larger value value likelihood likelihood funcion funcion will be. will In his be. In case, his case, peak peak likelihood likelihood funcion funcion is very is very close close o o real real coordinae. This This indicaes ha ha high high posiioning precision can can be be achieved even if if re is is no no prior informaion from ZUPT-based algorihm. Figure 9 shows disribuion likelihood funcion around real coordinae in an NLOS environmen. However, due o presence barrier beween beacon and ag, re is a large error in UWB observaion value and peak likelihood funcion has deviaed considerably from real posiion. If his only occurs for a momen, Only-UWB-based algorihm can sill guaranee a cerain precision as paricle sae also includes informaion abou direcion and velociy movemen in Only-UWB-based algorihm. The The signal diagram reveals ha ha alhough re re is an is an obvious obvious change change in in measuremen value value in in LOS LOS condiion, posiioning resul doesno no deviae very much from ha Only-UWB-based algorihm. However, if if deviaion UWB measuremen value lass for a long ime, Only-UWB-based posiioning algorihm canno guaranee accuracy posiioning resul. As menioned above, o guaranee correc movemen pah and direcion in such a condiion hrough Only-UWB-based algorihm, assumpion uniform recilinear moion mus have a high confidence coefficien; however, his will resul in a slow response o inflecion and difficuly correcing any errors. However, as algorihm can uilize prior

13 ISPRS In. J. Geo-Inf. 2017, 6, for a long ime, Only-UWB-based posiioning algorihm canno guaranee accuracy posiioning resul. As menioned above, o guaranee correc movemen pah and direcion in such a condiion hrough Only-UWB-based algorihm, assumpion uniform recilinear moion mus have a high confidence coefficien; however, his will resul in a slow response o inflecion and difficuly correcing any errors. However, as algorihm can uilize prior informaion oupued from ZUPT-based algorihm, sampling range can be limied o a small scale in sampling process. In his way, mos paricles will be disribued in correc direcion movemen in sampling process o compensae error arising from likelihood probabiliy disribuion. Therefore, i can sill achieve a reliable posiioning precision in conex poor UWB signals. Meanwhile, wih use prior informaion in sampling process, algorihm can sill guaranee a high posiioning precision a inflecion poin. The Only-UWB-based algorihm has a posiioning error up o 1.50 m a = 438 in posiioning process, while algorihm can achieve a posiioning precision m. However, a = 711, posiioning error achieved by Only-UWB-based algorihm is only m, which is very close o posiioning error m based on algorihm. ISPRS In. J. Geo-Inf. 2017, 6, ISPRS In. J. Geo-Inf. 2017, 6, Figure 8. The disribuion map likelihood funcion around acual posiion afer walking 711 seps (under an LOS condiion). Figure 8. The disribuion map likelihood funcion around acual posiion afer walking 711 Figure 8. The disribuion map likelihood funcion around acual posiion afer walking 711 seps (under an LOS condiion). seps (under an LOS condiion). Figure Figure The The disribuion map map likelihood funcion around acual posiionafer aferwalking walking seps seps (under (under an annlos condiion). Figure 9. The disribuion map likelihood funcion around acual posiion afer walking 428 Alhough seps (under an NLOS condiion). algorihm shows beer robusness for shor-erm posiioning under an NLOS condiion, i can hardly correc large deviaion curren moving direcion caused by a series Alhough severe UWB errors algorihm when shows oupu beer from robusness virual for shor-erm odomeer posiioning is used as under prior an informaion NLOS condiion, o consrain i can hardly direcion correc and large velociy deviaion movemen. curren I migh moving perform direcion even worse caused han by Only-UWB-based algorihm in erms local error, and his is refleced by rajecory from

14 ISPRS In. J. Geo-Inf. 2017, 6, Alhough algorihm shows beer robusness for shor-erm posiioning under an NLOS condiion, i can hardly correc large deviaion curren moving direcion caused by a series severe UWB errors when oupu from virual odomeer is used as prior informaion o consrain Table direcion 2. Algorihm and velociy error and compuing movemen. ime I in migh Pah perform II. even worse han Only-UWB-based algorihm in erms local error, and his is refleced by rajecory from (0, 5) o (0, 5) in coordinae sysem. Sandard Deviaion Time Offline Algorihm Mean error (m) Table 2 provides posiioning error, sandard deviaion Errors (m) posiioning Calculaion error, (s) and ime consumpion ZUPT various algorihms 3.20 for Pah II. The phrase 2.50 fline means ha compuing process needn Only-UWB wai for daa from 1.76 sensors, and value is proporional o 3.80 algorihmic complexiy. On Fusing whole, algorihm has an advanage over Only-UWB-based 4.12 algorihm. Meanwhile, wih a smaller sampling range, algorihm can achieve same effec wih use athe smaller influence number paricles. Number This Paricles will beon discussed Posiioning in Secion Resul ISPRS In. J. Geo-Inf. 2017, 6, This secion mainly discusses and compares posiioning precisions and compuing imes Table 2. Algorihm error and compuing ime in Pah II. achieved by boh algorihms on Pah II wih differen numbers paricles. All or parameers are kep same as hose provided in previous wo secions. Algorihm Mean Error (m) Sandard Deviaion Errors (m) Time Offline Calculaion (s) Figure 10 demonsraes influence number paricles on posiioning precision. I ZUPT reveals ha posiioning precision ends o become sable when 5000 paricles are used in boh Only-UWB algorihms. Fusing However, posiioning precision reaches maximum when paricles are used in algorihm. Afer ha, an increase in number paricles does no have any significan effec. This is because prior informaion oupued from ZUPT-based algorihm enables The influence Number Paricles on Posiioning Resul paricles o be disribued closer o mean poserior probabiliy disribuion. Therefore, poserior This secion probabiliy mainly disribuion discussescan andbe compares described very posiioning well wih precisions a small number and compuing paricles. imes achieved Figure by boh 11 demonsraes algorihms influence on Pah II wih number differen paricles numberson paricles. compuing All imes or boh parameers algorihms. are The kep calculaion sameis as performed hose provided on a machine previous insalled wo wih secions. CPU E v2. Boh algorihm Figure and 10 demonsraes Only-UWB-based influence algorihm have number been wrien paricles in C++, on whereas posiioning paricle precision. filer Ialgorihm reveals ha makes posiioning use eigh precision hreads o ends perform o become parallel sable compuaions when 5000 hroughou paricles are used seps. ini boh akes algorihms. only 4.1 s However, o calculae daa posiioning ses ha precision migh consume reaches up maximum o 107 s if when re 1000 are paricles 5000 paricles. are usedwih in performance algorihm. far beyond Afer ha, real-ime an increase requiremen, in number i can be paricles easily does ransplaned no haveino any significan Android effec. device This or a ishigh-performance because priorscm. informaion oupued from ZUPT-based algorihm enables paricles o be disribued closer o mean poserior probabiliy disribuion. Therefore, poserior probabiliy disribuion can be described very well wih a small number paricles. Figure Figure The The comparison comparison average average posiioning posiioning errors errors wih wih differen differen numbers numbers paricles. paricles.

15 ISPRS In. J. Geo-Inf. 2017, 6, Figure 11 demonsraes influence number paricles on compuing imes boh algorihms. The calculaion is performed on a machine insalled wih CPU E v2. Boh algorihm and Only-UWB-based algorihm have been wrien in C++, whereas paricle filer algorihm makes use eigh hreads o perform parallel compuaions hroughou seps. I akes only 4.1 s o calculae daa ses ha migh consume up o 107 s if re are 5000 paricles. Wih performance far beyond real-ime requiremen, i can be easily ransplaned ino an Android device or a high-performance SCM. ISPRS In. J. Geo-Inf. 2017, 6, Figure 11. The comparison average compuing imes wih differen numbers paricles. Figure 11. The comparison average compuing imes wih differen numbers paricles. I s I s very very clear clear ha ha algorihm can can achieve achieve a sable a sable posiioning resul resul based based on on a smaller a smaller number number paricles, as as i i can can make make good good use use ZUPT-based resul resul as as prior prior informaion for for sampling process. process. Compared wih wih Only-UWB-based algorihm, algorihm only only requires an an addiional compuaion based based on on ZUPT-based algorihm. However, ZUPT-based algorihm, in in essence, is is an an EKF-based algorihm wih wih far far lower lower ime ime consumpion han han paricle paricle filer filer algorihm, algorihm, bu bu i does i does no no have have a biga influence big influence on on oal compuing oal compuing ime ime a paricle a paricle filer-based filerbased algorihm. All se All prove se ha prove ha algorihm canalgorihm achieve a higher can achieve posiioning a higher precision posiioning wih algorihm. lower precision ime consumpion. wih lower ime consumpion Conclusions Conclusions Aiming Aiming a a posiioning posiioning based based on on UWB UWB and and IMU IMU daa, daa, his his paper paper proposes proposes an an approach approach o o fuse fuse resuls resuls based based on on a a paricle paricle filer filer wih wih calculaion calculaion virual virual odomeer odomeer which, which, in urn, in urn, is based is based on on IMU IMU daa. daa. In In addiion, addiion, effeciveness effeciveness algorihm algorihm is is verified verified and and illusraed illusraed hrough hrough an an experimen. experimen. The The posiioning posiioning can can achieve achieve high high precision precision in in an an NLOS NLOS environmen environmen when when daa daa on on movemen movemen rend rend are are obained obained hrough hrough calculaion calculaion IMU IMU daa. daa. Meanwhile, Meanwhile, wih wih addiion addiion prior prior informaion informaion from from IMU, IMU, sampling sampling accuracy accuracy in in every every sep sep is is improved, improved, which which makes makes i i possible possible o o approximae approximae real real poserior poserior probabiliy probabiliy disribuion disribuion wih wih a smaller a smaller number number paricles. paricles. Therefore, Therefore, algorihm algorihm can can achieve achieve high high precision precision wih wih fewer fewer paricles. paricles. Acknowledgmens: This work was suppored by Fundamenal Research Funds for Cenral Universiies under gran number 2017XKQY020. Auhor Conribuions: Yan Wang and Xin Li conceived, designed and performed experimens; Yan Wang analyzed daa and wroe paper. Conflics Ineres: The auhors declare no conflic ineres. The founding sponsors had no role in design sudy; in collecion, analyses, or inerpreaion daa; in wriing manuscrip, and in decision o publish resuls. References 1. Nilsson, J.O.; Skog, I.; Händel, P.; Hari, K.V.S. Foo-mouned INS for everybody An open-source

16 ISPRS In. J. Geo-Inf. 2017, 6, Acknowledgmens: This work was suppored by Fundamenal Research Funds for Cenral Universiies under gran number 2017XKQY020. Auhor Conribuions: Yan Wang and Xin Li conceived, designed and performed experimens; Yan Wang analyzed daa and wroe paper. Conflics Ineres: The auhors declare no conflic ineres. The founding sponsors had no role in design sudy; in collecion, analyses, or inerpreaion daa; in wriing manuscrip, and in decision o publish resuls. References 1. Nilsson, J.O.; Skog, I.; Händel, P.; Hari, K.V.S. Foo-mouned INS for everybody An open-source embedded implemenaion. In Proceedings IEEE/ION Posiion, Locaion and Navigaion Symposium (PLANS), Myrle Beach, SC, USA, April 2012; pp Skog, I.; Nilsson, J.O.; Händel, P. Pedesrian racking using an IMU array. In Proceedings 2014 IEEE Inernaional Conference on Elecronics, Compuing and Communicaion Technologies (CONECCT), Bangalore, India, 6 7 January Huang, C.; Liao, Z.; Zhao, L. Synergism INS and PDR in self-conained pedesrian racking wih a miniaure sensor module. IEEE Sens. J. 2010, 10, [CrossRef] 4. Mezensev, O.; Lachapelle, G.; Collin, J. Pedesrian dead reckoning A soluion o navigaion in GPS signal degraded areas? Geomaica 2005, 59, Zampella, F.; De Angelis, A.; Skog, I.; Zachariah, D.; Jimenez, A. A consrain approach for UWB and PDR. In Proceedings 2012 Inernaional Conference on Indoor Posiioning and Indoor Navigaion, IPIN 2012 Conference Proceedings, Sydney, Ausralia, November Li, X.; Xie, L.; Chen, J.; Han, Y.; Song, C. A ZUPT Mehod Based on SVM Regression Curve Fiing for SINS; IEEE: Piscaaway, NJ, USA, 2014; pp Hellmers, H.; Eichhorn, A. IMU/Magneomeer Based 3D Indoor Posiioning for Wheeled Plaforms in NLoS Scenarios; IEEE: Piscaaway, NJ, USA, Kang, W.; Nam, S.; Han, Y.; Lee, S. Improved heading esimaion for smarphone-based indoor posiioning sysems. In Proceedings IEEE Inernaional Symposium on Personal, Indoor and Mobile Radio Communicaions PIMRC, Sydney, Ausralia, 9 12 Sepember 2012; pp Mur-Aral, R.; Moniel, J.M.M.; Tardos, J.D. ORB-SLAM: A Versaile and Accurae Monocular SLAM Sysem. IEEE Trans. Robo. 2015, 31, [CrossRef] 10. Zhang, J.; Singh, S. Visual-lidar Odomery and Mapping: Low-drif, Robus, and Fas. In Proceedings IEEE Inernaional Conference on Roboics and Auomaion (ICRA), Seale, WA, USA, May 2015; pp Mur-Aral, R.; Tardos, J.D. Probabilisic Semi-Dense Mapping from Highly Accurae Feaure-Based Monocular SLAM. In Proceedings Roboics Science and Sysems (RSS), Rome, Ialy, July Wagle, N.; Frew, E.W. A Paricle Filer Approach o WiFi Targe Localizaion. In Proceedings AIAA Guidance, Navigaion, and Conrol Conference, Torono, ON, Canada, 2 5 Augus 2010; pp Bekkelien, A. Blueooh Indoor Posiioning. Maser s Thesis, Universiy Geneva, Geneva, Swizerland, Yu, K.; Monille, J.; Rabbachin, A.; Cheong, P.; Oppermann, I. UWB locaion and racking for wireless embedded neworks. Signal Process. 2006, 86, [CrossRef] 15. Zhou, Y.; Law, C.L.; Guan, Y.L.; Chin, F. Indoor ellipical localizaion based on asynchronous UWB range measuremen. IEEE Trans. Insrum. Meas. 2011, 60, [CrossRef] 16. Blanco, J.L.; Galindo, C.; Fern, J.A.; Moreno, F.A.; Mar, J.L. Mobile Robo Localizaion based on Ulra-Wide-Band Ranging: A Paricle Filer Approach. Robo. Auon. Sys. 2009, 57, Zwirello, L.; Ascher, C.; Trommer, G.F.; Zwick, T. Sudy on UWB/INS inegraion echniques. In Proceedings 8h Workshop on Posiioning Navigaion and Communicaion 2011, WPNC, Dresden, Germany, 7 8 April 2011; pp Skog, I.; Händel, P.; Nilsson, J.-O.; Ranakokko, J. Zero-velociy deecion An algorihm evaluaion. IEEE Trans. Biomed. Eng. 2010, 57, [CrossRef] [PubMed]

17 ISPRS In. J. Geo-Inf. 2017, 6, Garrido-Jurado, S.; Muñoz-Salinas, R.; Madrid-Cuevas, F.J.; Marín-Jiménez, M.J. Auomaic generaion and deecion highly reliable fiducial markers under occlusion. Paern Recogni. 2014, 47, [CrossRef] 20. Kümmerle, R.; Rainer, K.; Grisei, G.; Konolige, K. G2o:A general framework for graph opimizaion. In Proceedings IEEE Inernaional Conference on Roboics and Auomaion Shanghai Inernaional Conference Cener, Shanghai, China, 9 13 May 2011; pp Bacik, J.; Durovsky, F.; Fedor, P.; Perdukova, D. Auonomous flying wih quadrocoper using fuzzy conrol and ArUco markers. Inell. Serv. Robo. 2017, 10, [CrossRef] 2017 by auhors. Licensee MDPI, Basel, Swizerland. This aricle is an open access aricle disribued under erms and condiions Creaive Commons Aribuion (CC BY) license (hp://creaivecommons.org/licenses/by/4.0/).