An algorthm for real-tme restructurng of a rangng-base localzaton network Saman Fahanezh-Saa an Mark W. Mueller arxv:84.9833v [cs.ro] 5 Apr 8 Abstract Ths paper presents a metho to mprove the localzaton accuracy of robots operatng n a range-base localzaton network. The metho s favorable especally when the robots operate n harsh envronments where the access to a robust an relable localzaton system s lmte. A state estmator s use for a sx egree of freeom object usng nertal sensors as well as an Ultra-weban (UWB) range measurement sensor. The estmator s ncorporate nto an aaptve algorthm, mprovng the localzaton qualty of an agent by usng a moble UWB rangng sensor, where the moble anchor moves to mprove localzaton qualty. The algorthm reconstructs localzaton network n real-tme to mnmze the etermnant of the covarance matrx n the sense of least square error. Fnally, the propose algorthm s expermentally valate n a network consstng of one moble an four fxe anchors. I. INTRODUCTION An accurate, robust, an accessble localzaton system s crucal for robot operaton n the case of, for example, emergency servces, bulng fault etecton n saster areas, or rescue mssons. We assume that n these stuatons the pre-nstalle nfrastructure s estroye or lmte (.e. no access to GPS sgnal), an therefore a local an stanalone alternatve localzaton system s vtal. Also, the harsh envronment of these stuaton requres to use relable sensory evces. For example, the sensors shoul functon n smoke, ust, an n foggy or heavy ran contons where optcal systems cannot be use relably. An nfrastructure (e.g. rao/auo beacons) for the local postonng system s mportant to be place n the envronment such that t maxmzes the localzaton accuracy, but t s most lkely mpossble ue to lmte accessblty of such envronment. The propose metho n ths paper wll aress above ssues by usng a relable range measurement sensor wth moble nfrastructure settng. The recently popular ultra-weban rao rangng s a flexble, relatvely low-cost, an relable localzaton technology mostly for but not lmte to noor envronments see e.g. [] [5]. The nfrastructure components (.e rao-beacon) of the system are easy to setup n the envronment. The rao-beacon (here calle anchor ) communcates va rao messages wth an agent whch s equppe wth UWB sensor. The result s a range measurement, the stance between the agent an each nvual anchors. The UWB sensors are use n a we range of localzaton methos. For example, n conjuncton wth the The authors are wth the Department of Mechancal Engneerng, at the Unversty of Calforna, Berkeley. {samanfahanej,mwm}@berkeley.eu SLAM (smultaneous localzaton an mappng) problem, a partcular UWB transmtter-recever confguraton on an agent s use n [6]. The authors explan that t s crucal to use UWB rangng specally n emergency stuaton, when other technologes lke camera- or laser-base sensors fal to operate n such harsh envronment. The UWB technology also has mecal applcatons. For example, authors n [7] evelope an algorthm to mprove the localzaton accuracy of surgcal evces usng UWB sensor n hghly reflectve an ense noor envronments such as operatng rooms where multpath an no-lne-of-sght contons are an ssue. In [8], authors evelope a Kalman flter estmator usng fuson of nertal measurement unt wth UWB rangng sensor for localzaton n crowe an ynamc envronments lke magng rooms, where techncal evces lke C-arm magng systems or operatng tables are movng. The range measurement base localzaton s also use n mult-agent cooperatve postonng an wreless sensor network [9] [3]. The man ea s to localze several agents (or wreless sensor noes) wth the capablty of exchangng ther nformaton (.e. relatve stance, poston, orentaton, etc). Our propose metho consers usng several fxe UWB anchors as well as moble anchors, the ones that aapt ther postons n orer to mprove the poston accuracy of the agent. In other wors, the cooperaton occurs between an agent an a set of moble anchors. Snce the localzaton accuracy s senstve to the anchor arrangement, t s har to acheve reasonable accuracy wth restrcte anchor s arrangement wthout careful plannng, whch may be ffcult n harsh contons. In ths paper, the poston of moble anchors s assume to be known (.e. they have access to a separate, nepenent localzaton technology), whereas the agents rely only on the moble an fxe anchors for localzaton, snce they on t have access to any localzaton nfrastructure. Future work wll look at generalzng ths by removng the separate localzaton technology whch moble anchors currently rely on. A schematc of our propose system s shown n Fg. In ths paper, we use a state estmator wth no assumpton on force-torque, an we evelop an aaptve localzaton algorthm on top of the estmator for movng a known rangng measurement pont (.e. a moble anchor), so that an agent may mnmzes ts poston uncertanty. The state estmaton conssts of a general sx egrees of freeom (6DOF) moel wth measurement moel as agent s stance to known postons n the worl. The aaptve algorthm maxmzes the agent s localzaton qualty n the least-square sense. The results are valate n a seres of experments,
A A M A4 A3 Fg.. A schematc of the propose systems: an agent (here a quacopter) operates n a space prepare wth combnaton of one moble (nexe as M) an four fxe rao anchors (nexe A-A4). At the left, the ellpse representng the poston uncertanty s extene n the recton whch there s no anchors present. At the rght, the moble anchor moves to the area wth no anchors an reuces the uncertanty aroun the agent. where the estmator s mplemente on a quacopter system. A A II. SYSTEM MODEL We conser moel of a generc 6 egree of freeom (3 n translaton an 3 n rotaton) rg boy for the purpose of estmatng the states of an agent. The agent s equppe wth nertal measurement unt (accelerometer an rate gyroscope) an a range measurement sensor that allows to measure the stance to any of fxe or moble sets of anchors (wth known locaton) n ts envronment. In the followng subsectons we wll brefly escrbe the unerlyng moels of the estmator. A. Equatons of moton We wll use the conventon of usng bol-face symbols for vector/matrx quanttes, an regular font for scalars. In the moel, the poston of the rg boy (agent) s enote as x, ts velocty as v, an acceleraton as a, all expresse n the nertal frame fxe to the groun. The rotaton matrx an the angular velocty are gven respectvely by R an ω, where the rotaton matrx represents the orentaton of the agent. The multplcaton by the roaton matrx wll result a coornaton transformaton from the boy-fxe frame to the nertal frame. The tme ervatves of these quanttes are gven as t x = v () t v = a () R = RS(ω) (3) t note that S(ω) s the skew-symmetrc matrx verson of cross prouct, where S(x)y = x y. B. Inertal measurements The nertal measurement unt outputs the accelerometer an rate gyroscope measurements, α an γ. The accelerometer measures the proper acceleraton n the boy-fxe frame whch we assume t s corrupte by atve nose ν α. α m = R (a g) + ν α (4) M A4 A3 The gravtatonal acceleraton s n the nertal frame fxe to the groun, an has magntue g = (,, 9.8) m/s. The rate gyroscope measures angular velocty of the agent n the boy-fxe frame. The measurement s moele as γ = ω + ν γ (5) here the the measurement s corrupte by ν γ. Both ν α an ν γ are assume to be zero mean, base on the fact that the sensors are well calbrate an scale/bas-free. C. UWB Range measurement system The UWB rao mounte on the agent communcates wth other raos n the envronment. At each tme nstant, the agent measures the stance from ts poston at x to one of fxe or moble poston at p (anchor s poston) n the worl. Ths measurement ρ to anchor s moelle as the Euclean norm corrupte wth atve scalar nose ν ρ wth zero mean. ρ = x p + ν ρ (6) The UWB rao uses two-way rangng tme-of-flght base algorthm to calculate the stance (see [4] for two-way rangng scheme). The agent can communcate only wth one anchor at a tme, meanng that only a sngle range measurement can be taken at any nstant n tme. III. STATE ESTIMATOR For the state estmaton, an extene Kalman flter (EKF) [5] presente n ths secton estmates the -element state of the agent consst of poston, atttue an ther ervatves. The knematc moel makes the Kalman flter an estmator for a generc 6DOF rg boy wth no assumpton on forces or torques actng on the agent. The EKF uses the technque of [6] to encoe an atttue n the state wth correct-to-frstorer statstcs. The estmator oes not nclue the angular velocty as a state, nstea uses the output of rate gyroscope measurement; assumng that the moern sensors output hgh-qualty measurements whch s a stanar approach n atttue estmaton for satelltes [7]. Thus, the estmator s stochastc state ξ s a 9 mensonal vector: ( ˆξ = ˆx, ˆv, ˆδ ) (7) wth the hat enotng estmate quanttes, an where ˆδ represents atttue error measure, assume to be small. The estmator uses a reunant atttue representaton, wth a reference atttue R ref an the atttue error ˆδ combne yelng the estmator s atttue estmate ˆR ) ˆR = R ref (I + S(ˆδ) (8) wth I the entty matrx. Ths representaton allows for a sngularty-free atttue estmaton usng only a threemensonal representaton of the atttue error a complete scusson of ths approach s gven n [6].
The EKF uses the output of accelerometer an rate gyroscope for the precton step, so the state fferental s gven by t v = Rα m + g + Rν α (9) t δ = γ ν γ () In the measurement upate step, the estmator uses the output of the UWB rangng raos. The lnearzaton of measurement equaton (6) s as follows H := ρ ( ξ = ρ x, ρ v, ρ ) δ () ρ x = x p x p =: e () ρ v = ρ δ = (3) Note that the measurement senstvty wth respect to the agent s poston e s the unt vector n the recton of anchor from the agent. The estmate covarance matrx Σ compute by EKF reles on the partal ervatves of lnearzaton process an usng the approach of [6]. Ths s the parttone matrx as below Σ xx Σ xv Σ xδ Σ = Σ xv Σ vv Σ vδ R 9 9 (4) Σ T xδ Σ T vδ Σ δδ wth e.g. Σ xδ the 3 3 cross-covarance between the poston an atttue states. The very ntutve property of measurement senstvty wll be use n the followng secton to etermne how the anchors can move n orer to maxmze the localzaton qualty of the agent. IV. MOBILE ANCHOR ALGORITHM The measurement moel as escrbe n () s senstve to the locaton of the anchors p. Ths means, t s possble to affect the varance of the state estmates by movng the anchors. Ths allows to create an optmzaton problem n orer to move the anchors n the recton whch mnmzes the estmaton error of the agent s poston. The covarance matrx s use as the metrc for estmaton qualty. In the followng subsectons we wll scuss ths approach n etal. A. Least squares approach The least-square approach s use as an easy-to-analyze approxmaton of the EKF use to estmate the agent s locaton. Ths s motvate by the least squares nterpretaton of the Kalman flter. All the ervatons n ths secton are assume to be for a sngle moble anchor wth N fxe anchors. It s smple to generalze the ervaton for multple moble anchors, snce the esre movng recton of each anchor ecouples. All the quanttes wthout number subscrpton are ntrouce for the sngle moble anchor. Notable s that we have two sets of ecson varables n ths secton. The poston estmate of the agent x s the ecson varable for the least squares problem. The moble anchor s poston p s the ecson varable for mnmzng the varance of the agent s poston estmate. We are usng these two varables throughout ths secton. We conser agan the rangng measurement moel n (6). Note, here we look at a batche verson of ths equaton (.e. the equaton s concatenaton of scalar measurements at each screte tme step for N anchors, fxe or moble anchors), whch s an approxmaton of EKF ρ ρ. ρ N ρ = h(x, p) + ν ρ (5) x p ν ρ x p ν ρ =. x p N +. ν ρn, where ρ s the measurement vector, ν ρ s the atve nose vector wth zero mean, an h(x, p) s the nonlnear measurement moel (vector-value equaton) mappng the poston of an agent n R 3 to the batch of anchors measurements n R N. The partal ervatve of the nonlnear moel wth respect to the agent s poston wll result a N 3 Jacoban matrx A(ˆx, p) = h(x, p) x =. N RN 3 (6) where th row of the matrx s the unt vector pontng from the agent to anchor. Assumng nose n (5) to be zero mean wth sotropc covarance Var(ν ρ ) = qi, the poston estmate can be foun by mnmzng the norm square of nose ˆx = arg mn x ν ρ As state before, for the nonlnear measurement moel ths wll be one teratvely by lnearzng the moel at each tme step, an movng n the recton of the graent escent. B. Statstcal propertes Snce we use the lnearze verson of the equaton, the mean of the estmate poston s zero (.e. unbase estmator) but the varance s Var(ˆx) ((A T A) A T )qi((a T A) A T ) T = q(a T A) (7) For brevty, we use A nstea of A(ˆx, p) from now on. Our goal s to mnmze the effect of nose on the state estmates (.e. to mnmze the varance of the state estmates by varyng the anchor locaton). Specfcally, we choose to mnmze the largest egenvalue of the covarance matrx, whch s equvalent to maxmzng the smallest egenvalue of ts nverse: max p mn λ ((A T A)) (8)
By expanng matrx A T A, the max-mn problem can be converte to a smple form. Substtutng (6) n matrx A, the result s the sum of N rank-one matrces A T A = [ ] e e... e N. N = e + e N (9) Snce all e an e are unt vectors (the recton to the anchors), the trace of A T A s always constant an equal to the number of anchors: N tr(a T A) = tr( e) + tr( e ) = N () The trace of a matrx s also equal to the sum of ts egenvalues; that means the sum of egenvalues of the covarance matrx n ths problem s constant. 3 tr(a T A) = λ = N () Usng ths fact, the max-mn optmzaton problem can be transforme to t max λ,t subject to T λ = N t λ λ () where t s a slack varable, an λ s a vector representng the egenvalues of matrx A T A. Ths problem can be solve by ntroucng ts ual problem. Snce () shows that the sum of egenvalues s constant, we can conclue that () s equvalent to the followng problem max p et(a T A) = max 3 λ (3) It can be shown that for both problems the maxmum occurs, when all the egenvalues are equal. Specfcally, (8) has nterpretaton for robust esgn (t mnmzes the worst varance recton) as oppose to (3) whch mnmzes the volume of ellpso assocate wth the covarance matrx [8]. Ths shows that ths metho satsfes both esgn approaches (.e. mnmzng the largest varance recton wll result n mnmzng the ellpso volume). C. Moble anchor Snce the matrx conssts of the moble anchor s poston, ts etermnant epens on the poston of the moble anchor at each tme step. A T (ˆx p)(ˆx p)t A = ˆx p N (ˆx p )(ˆx p ) T + ˆx p R 3 3 By takng partal ervatve wth respect to the moble anchor s poston p = (x, y, z) we can fn a recton that the moble anchor can move n orer to mnmze the etermnant of the covarance matrx et(a T A) p x et(at A) = y et(at A) (4) z et(at A) We rewrte the nverse of the covarance matrx n a compact form as m m m 3 A T A = m m 4 m 5 (5) m 3 m 5 m 6 where m are functons of moble anchor s poston. As an example, m an m 6 have the form (note that ˆx, ˆx, ˆx 3 are the three frst states of the estmator representng the agent s poston): m = (ˆx x)(ˆx y) ˆx p, m 6 = (ˆx 3 z) ˆx p Representng the nverse of the covarance matrx n the compact form of (5) allows to erve the etermnant of the matrx an ts partal ervatve n terms of functons m, an then substtute back the moble anchor s poston. At the en, (4) wll be a set of three close form equatons n terms of the moble anchor s poston as well as the agent s poston, whch can be easly mplemente. We choose the control acton as the velocty comman for the moble anchor whch s the Jacoban of the etermnant of the covarance matrx multplyng by a gan value s > as follows v cm = s et(at A) p Ths s exactly the same as movng on the recton of graent ascent teratvely untl the moble anchor reaches to the top (max pont of etermnant) an stays there. For sequence of steps the moble anchors move as follows p(k + ) = p(k) + ( t)v cm (6) where k s tme steps an t s the screte tme nterval. In the followng secton we wll present the real-tme expermental results of ths approach. V. EXPERIMENTAL VALIDATION The approach s valate n experment, where two quacopters are use, one as autonomous agent an one as moble anchor. A frst set of experments just uses the fxe anchors as the only UWB measurement settng. The secon experment uses one moble anchor n aton to the fxe anchors. Results wll be presente by comparng the agent s varance compute by the onboar EKF for the two fferent experment settngs.
X [m].8. poston.8 velocty 4 3 atttue wth moble anchor wthout moble anchor yaw ψ [eg.] Y [m]..5.3. ptch θ [eg.]. Z [m].5..75.5.5 5 5 5. 5 5 5. 5 5 5 roll φ [eg.] Fg.. Close loop control of quacopter usng UWB rangng sensor wth an wthout use of moble anchor. The experment llustrates the square roots of the agonals of EKF covarance matrx (.e. one stanar evaton). As seen usng moble anchor, ecreases uncertanty (poston an velocty) ramatcally n the x recton, snce there are enough fxe anchors along y recton but nothng n the x recton (see Fg. 3). Note that the estmator oes not compute the estmate n terms of yaw, ptch, an roll angles; the estmator output s smply transforme nto ths format as t s easer to parse n a fgure. X [m]..5..5..5..5. 3 Y [m] Fg. 3. Top vew. Re ots represent the poston of fxe anchors n both sets of experments. The black ot s the ffth fxe anchor n the frst an the ntal poston of moble anchor n the secon set of experments. The set of four anchors are all locate at z =. The blue path s the projecton of the moble anchor trajectory on xy plane. A. Expermental Setup The testbe we use to examne our algorthm s a Crazyfle. quacopter (shown n Fg. 4), wth approxmate mass of 3g, an motor-to-motor stance of 5mm. We use two quacopters as the agent an the moble anchor n our experments. The quacopter s equppe wth an STM3F4 mcrocontroller, an Invensense MPU95 nertal measurement unt, an a Decawave DW rao moule 3 for the ultra-weban rangng measurements. Bese the moble anchor whch share the same harware, all the fxe anchors also have the same computatonal an sensng harware. The state estmaton (EKF) for the agent was performe on the mcrocontroller, but the moble anchor use moton capture system for ts own localzaton, an was commane by the trajectory accorng to (6) compute off-boar on a computer. Measurements from the accelerometer an rate gyroscope were taken at 5Hz, an range measurements were taken at approxmately 8Hz. The effectveness of our approach on the estmator performance s quantfe by usng the moton capture system, whose measurements are taken as groun truth. The top vew of the anchor arrangement can be seen n Fg. 3. Notable s that the anchors are place such that ther measurements carry suffcent nformaton n the y recton, but not very much n the x recton. Ths was chosen so as to hghlght the mportance of moble anchor an our propose algorthm. B. Experment wth moble anchor Two sets of smlar experments were conucte to verfy the avantage of usng moble anchor aganst only fxe anchors to mprove the localzaton accuracy. In the frst set of experments, we use fve fxe anchors place on the groun as pcture n Fg. 3. We ran ths test several tmes whle the quacopter hovers at the same poston at orgn
et. of nv. cov. matrx 3.5 3..5..5..5. 5. 7.5..5 5. 7.5. The quacopter use n our experments as the agent an moble TABLE I C OMPARISON OF ESTIMATION ERROR FROM EXPERIMENTS poston [m] velocty [m/s] atttue [eg.] RMSE Average 4 fxe, moble 5 fxe anchors.56.8.374 9 5.4 5.35 fference (%) -4.3% -8.6% -3.3% an the EKF escrbe n Secton III runs on the quacopter s mcrocontroller. In the secon set of experments, we use four fxe anchors n aton to one moble anchor. The moble anchor starts from ts ntal poston (black ot n Fg. 3) an moves accorng to the algorthm escrbe n Secton IV. Table I compares the average root mean square error (RMSE) on state estmates of the quacopter (poston, velocty, atttue) from several trals for both sets of experments. As seen the average RMSE s mprove by about 4%, 9%, an 3% for poston, velocty an atttue respectvely. The results show larger mprovement on poston rather than other states. Ths represent the fact that the propose algorthm have rect mpact on the poston estmate qualty but not on velocty an atttue. Fg. 5 shows the etermnant of nverse covarance matrx n the propose algorthm et(at A). Ths verfes that the moble anchor has move to ecrease the poston uncertanty aroun the agent. Fg. shows the expermental ata of a tral of both experments, whch s the comparson of one stanar evaton of state estmates. Comparng the monotonc ecrease n the uncertanty of poston n x recton (blue lne) wth no mprovement n y shows that our ntuton about anchor s arrangement was correct; absence of any anchor along the axs of symmetry between the fxe anchors causes nsuffcent avalable nformaton along that axs, but when the moble anchor starts to move towars the axs the uncertanty also ecreases accorngly. As the agent navgates n the envronment to reach the target poston, the localzaton network (anchors) s capable to reconstruct tself n real tme accorngly. Although the propose algorthm for the moble anchor was erve assumng that the agent s poston s fxe (.e. no ynamc moel was ntrouce), the algorthm works relably even Fg. 5. Determnant of the nverse of the covarance matrx. The plot shows the ncrease of the etermnant as expecte when the moble anchor movng n the recton of graent ascent; that means the state estmate varance s ecreasng. poston z [m] poston y [m] poston x [m] Fg. 4. anchor. comman estmaton groun truth one stanar evaton 5 5 5 3 Fg. 6. Close loop control of the quacopter usng one moble anchor. The plot shows that the quacopter s trackng a horzontal trajectory (ashe blue) along the y axs. Despte the fact that the metho was evelope for a fxe agent, the results show that t can be use for movng agents as well. for the movng agent. As an example, Fg. 6 shows an experment when the quacopter tracks a trajectory, an the moble anchor aapts ts poston base on the propose approach reucng the uncertanty of the quacopter s poston estmate. VI. C ONCLUSION The paper presente a EKF estmator for estmatng a 6DOF states of an object usng accelerometer, rate gyroscope, an UWB rangng sensor. Specfcally, the paper was focuse on evelopng a metho to use UWB moble anchor an mprovng the localzaton qualty of an agent. Ths was one by mnmzng the etermnant of the covarance matrx. The mnmzaton wll result a set of close form equatons representng a recton that the moble anchor moves n that recton at each tme nstant. By usng an UWB moble anchor an the presente metho, the quacopter was able to relably fly (hoverng an trackng a trajectory) whle the fxe anchors was set n specfc locatons to cause a large uncertanty n one recton whch makes the flght very har. The expermental results
presente n ths paper have shown a mprovement on the poston estmate (about 4%), whch ha also an nrect mpact on the mprovement of the velocty an atttue estmates. In aton, the result of a trajectory trackng scenaro showe that the propose metho can be apple for the movng agents, an wll result to reucng the overall uncertanty of the state estmates. VII. ACKNOWLEDGEMENT Ths materal s base upon work supporte by the Natonal Scence Founaton Grauate Research Fellowshp uner Grant No. DGE 64. REFERENCES [] J Sachs, Hanbook of ultra-weban short-range sensng: Theory, sensors, applcatons, p. 5, Dec.. [] T. Dessler an J. Thelecke, Feature base noor mappng usng a bat-type uwb raar, n 9 IEEE Internatonal Conference on Ultra-Weban, Sep. 9, pp. 475 479. [3] Z. Sahnoglu, S. Gezc, an I. Gvenc, Ultra-weban postonng systems: Theoretcal lmts, rangng algorthms, an protocols. New York, NY, USA: Cambrge Unversty Press,. [4] S. Gezc, Z. Tan, G. B. Gannaks, H. Kobayash, A. F. Molsch, H. V. Poor, an Z. Sahnoglu, Localzaton va ultra-weban raos: A look at postonng aspects for future sensor networks, IEEE Sgnal Processng Magazne, vol., no. 4, pp. 7 84, Jun. 5. [5] M. Mahfouz, C. Zhang, B. Merkl, M. Kuhn, an A. Fathy, Investgaton of hgh-accuracy noor 3-D postonng usng UWB technology, IEEE Transactons on Mcrowavheory an Technques, vol. 56, no. 6, pp. 36 33, 8. [6] T. Deßler an J. Thelecke, Uwb slam wth raoblackwellze monte carlo ata assocaton, n Internatonal Conference on Inoor Postonng an Inoor Navgaton, Sep., pp. 5. [7] D. Brese, H. Kunze, an G. Rose, Uwb localzaton usng aaptve covarance kalman flter base on sensor fuson, n 7 IEEE 7th Internatonal Conference on Ubqutous Wreless Broaban (ICUWB), Sep. 7, pp. 7. [8] M. J. Kuhn, J. Turnmre, M. R. Mahfouz, an A. E. Fathy, Aaptve leang-ege etecton n uwb noor localzaton, n IEEE Rao an Wreless Symposum (RWS), Jan., pp. 68 7. [9] F. Olsson, J. Rantakokko, an J. Nygårs, Cooperatve localzaton usng a foot-mounte nertal navgaton system an ultraweban rangng, n 4 Internatonal Conference on Inoor Postonng an Inoor Navgaton (IPIN), Oct. 4, pp. 3. [] S. I. Roumelots an G. A. Bekey, Dstrbute multrobot localzaton, IEEE Transactons on Robotcs an Automaton, vol. 8, no. 5, pp. 78 795, Oct.. [] J. A. Costa, N. Patwar, an A. O. Hero III, Dstrbute weghte-multmensonal scalng for noe localzaton n sensor networks, ACM Trans. Sen. Netw., vol., no., pp. 39 64, Feb. 6. [] Y. Wang, Lnear least squares localzaton n sensor networks, EURASIP Journal on Wreless Communcatons an Networkng, vol. 5, no., p. 5, Mar. 5. [3] I. M. Reklets, G. Duek, an E. E. Mlos, Mult-robot exploraton of an unknown envronment, effcently reucng the oometry error, n Proceengs of the Ffteenth Internatonal Jont Conference on Artfcal Intellgence - Volume, ser. IJCAI 97, Nagoya, Japan: Morgan Kaufmann Publshers Inc., 997, pp. 34 345. [4] M. W. Mueller, M. Hamer, an R. D Anrea, Fusng ultraweban range measurements wth accelerometers an rate gyroscopes for quarocopter state estmaton, n IEEE Internatonal Conference on Robotcs an Automaton (ICRA), IEEE, 5, pp. 73 736. [5] D. Smon, Optmal state estmaton: Kalman, h nfnty, an nonlnear approaches. John Wley & Sons, 6. [6] M. Mueller, M. Hehn, an R. D Anrea, Covarance correcton step for Kalman flterng wth an atttue, Journal of Guance, Control, an Dynamcs, pp. 7, 6. [7] F. L. Markley, Atttue error representatons for Kalman flterng, Journal of guance, control, an ynamcs, vol. 6, no., pp. 3 37, 3. [8] S. Boy an L. Vanenberghe, Convex optmzaton. New York, NY, USA: Cambrge Unversty Press, 4, p. 387.