Localization of a Wireless Sensor Network with Unattended Ground Sensors and Some Mobile Robots

Transcription

1 Localzaton of a Wreless Sensor Networ wth Unattended Ground Sensors and Some Moble Robots Koushl Sreenath, Fran L. Lews, Dan O. Popa Automaton & Robotcs Research Insttute (ARRI) Unversty of Texas at Arlngton Arlngton, USA oushls, popa@arr.uta.edu, lews@uta.edu Abstract A range-free approach for adaptve localzaton of un-localzed sensor nodes employng a moble robot wth GPS s detaled. A moble robot navgates through the sensor deployment area broadcastng ts postonal estmate and the uncertanty n ts estmate. Dstrbuted computatonallynexpensve, dscrete-tme Kalman Flters, mplemented on each statc sensor node, fuse nformaton obtaned over tme from the robot to decrease the uncertanty n each node s locaton estmate. On the other hand, due to dead reconng and other systematc errors, the robot loses postonal accuracy over tme. Updates from GPS and from the localzed sensor nodes serve n mprovng the localzaton uncertanty of the robot. A Contnuous-Dscrete Extended Kalman Flter (CD EKF) runnng on the moble robot fuses nformaton from multple dstnct sources (GPS, varous sensors nodes) for robot navgaton. Ths two-part procedure acheves smultaneous localzaton of the sensor nodes and the moble robot. Keywords Adaptve Localzaton, Contnuous-Dscrete Extended Kalman Flter (CD EKF), Smultaneous Localzaton, Sensor Networs. I. INTRODUCTION Locaton nformaton s mperatve for applcatons n both wreless sensor networs and moble robotcs. Many sensor networ applcatons, such as tracng targets, envronmental montorng, geo-spatal pacet routng, requre that the sensor nodes now ther locatons. The large scale of deployment n sensor networs maes careful placement or unform dstrbuton of sensor nodes mpractcal. The requrement of the sensors to be small, un-tethered, low energy consumng, cheap, etc., mae the sensors resource-constraned [1]. Localzaton s a challengng problem and yet crucal for many applcatons. Approaches to the problem of localzaton are vared. A detaled ntroducton to localzaton n sensor networs s presented n [2]. GPS [3] solves the problem trvally, but equppng the sensors wth the requred hardware may be mpractcal. A small secton of actve beacons can be placed n the sensor networ and other sensors can derve ther locaton from these anchor nodes [4], [5]. Cooperatve localzaton methods have been developed for relatve localzaton [6], [7]. Ths wor was supported n part by the Army Research Offce grants W91NF , M CI-RIP and the Natonal Scence Foundaton grants IIS , CNS Other approaches nvolve RSSI [8], TOA [9], [1], AOA [11], and Sgnal Pattern Matchng [3]. For localzaton wth no addtonal hardware on the sensor node, the geometrc constrants of rado connectvty are exploted. Some authors suggest usng a moble robot (whose poston s nown) to localze the sensors. However, the poston of the moble robot may be hard to determne. LaSLAT [12] uses a Bayesan flter to localze the sensor networ and trac the moble robot. In [13], a partcle flter s employed to localze elements of the networ based on observaton of other elements of the networ. In [14], a moble robotc sensor localzed the networ based on smple ntersectons of boundng boxes. In [15], geometrc constrants based on both rado connectvty and sensng of a movng beacon localze the sensor networ. The Kalman flter has been used n dead-reconng for moble robots but ts full potental n localzaton of WSN has not heretofore been fully explored. In [16], an extended Kalman flter s used for localzaton and tracng of a target. The Kalman flter was used n [17] for actve beacon and moble AUV localzaton and n [18] for schedulng of sensors for target tracng. SLAM [19] and CML [2] employ Kalman flters for concurrent mappng and moble robot localzaton, whch can be consdered smlar to our wor wheren the geometrc constrants ntroduced due to rado connectvty of the statc sensors play the role of features. In ths paper we use the full capabltes of the Kalman flter n the general WSN localzaton problem. Our wor explots geometrc constrans based on rado connectvty such that range nformaton s not needed. A moble robot ntally sweeps the networ, and broadcasts from the robot are used to localze the sensor nodes. Computatonally nexpensve Kalman flters mplemented on the sensors fuse the nformaton. On the other hand, as tme passes, the moble robot gradually loses ts own localzaton nformaton. We present an algorthm that uses updates from the better localzed sensors along wth GPS updates, when they occur, to correct ths problem. A contnuous-dscrete extended Kalman flter runnng on the robot estmates the robot state contnuously and fuses the dscrete measurement updates avalable from the more localzed sensors and nfrequent GPS. II. SENSOR LOCALIZATION USING MOBILE ROBOT In ths secton we provde an algorthm that runs on each Unattended Ground Sensor (UGS) node that allows t to update /6/$2. 26 IEEE RAM 26

2 ts poston estmate, and the uncertanty n that estmate, as a moble robot wth nown poston moves through the networ. The algorthm s range-free n that only the communcaton range need be nown, not the range from the node to the moble robot. It s assumed n ths secton that the moble robot s poston s exactly nown. A. Scenaro A deployed wreless sensor networ comprsed of statc unattended ground sensors s to be absolutely localzed by a moble robot. The robot broadcasts consst of ts own poston and ts poston uncertanty estmates. Broadcasts can only be heard wthn the robot s communcaton range. The statc sensors, on recevng these broadcasts, combne the new nformaton to update ther current locaton estmate. A smple dscrete-tme Kalman flter runnng on each statc sensor node serves to fuse nformaton and update ts locaton and uncertanty estmates. Ths s a formalzed rgorous approach employng Kalman flters for localzaton, n contrast to boundng boxes [14], [15], whch are harder to update and eep trac of. The developed algorthm s smple and can effcently be mplemented on the sensor nodes wth a small computng power. The Kalman flter s smply an optmal recursve data processng algorthm [21] and has been subject of extensve research and applcatons, partcularly n the area of autonomous navgaton. B. Robot Control A classcal three-wheeled trcycle robot model s employed n all smulatons. Ths confguraton uses a controlled steerng angle and drve speed to navgate to a desred poston as llustrated n Fg. 1. wth ( x, y) the poston of the robot, α the steerng angle, and φ the headng angle. The control nputs are the speed v t and the steerng rate ω α. A smple Proportonal-Dervatve goal-based controller wth a temporally varyng goal s mplemented to navgate the robot along a desred trajectory. For more detals, see [22]. Ths dynamcal setup allows more accurate smulatons than the smple movng-pont model usually assumed n sensor networ localzaton papers. C. Sensor Node Kalman Flter Each statc sensor node mantans ts own poston and uncertanty estmates. The moble robot broadcasts contan the robot s poston estmate and uncertanty estmate. The broadcasts can only be heard wthn the robot s communcaton range. A dscrete-tme Kalman flter runnng on each sensor node combnes ths nformaton to optmally update the node s poston estmate and ts uncertanty. For more detals on the dervaton of the Kalman flter equatons, nterested readers are referred to [26]. The Kalman flter s a set of mathematcal equatons runnng n a software algorthm that provde an effcent computatonal means to estmate the state of a process. The state of sensor at dscrete tme nstant s [ x y ] T x (3) The sensor state s governed by the lnear stochastc dfference equaton wth measurements gven by +1 A x + B u G w (4) x + z H x + v (5) Fgure 1. Trcycle Robot Confguraton. The states and nematcs of the robot are gven by, [ x y φ α ] T X (1) x v t cos α cos φ ( ) y v cos α sn φ X t a x, t φ v (2) t sn α L α ω α The random varables w and measurement noses gven by where ( P) v represent process and ( x, P ), w (, Q ), v ( R ) x x, (6) m, denotes a Gaussan nose process wth mean m and covarance P. For statonary nodes, the system matrces are gven by A, B, G, H (7) 1 1 1

3 The a pror poston estmates pror to measurement updates at tme + 1 are gven by the tme update equatons, whch gve the effects of tme on sensor localzaton: +1 P Q (8) P + + xˆ (9) xˆ 1 In these equatons, xˆ represents the poston estmate of node at tme, whle the covarance matrx P gves the correspondng uncertanty n the poston estmate. The a posteror estmates gven a poston measurement z are gven by the measurement update equatons, whch gves the effect of the robot broadcast on sensor localzaton: 1 1 T P H + 1 R+ 1 H + 1 P (1) T 1 + xˆ + 1 xˆ + 1 P + 1H + 1 R + 1 z + 1 H + 1xˆ + 1 (11) The covarance matrces Q and R are desgn parameters chosen by the engneer. Wth a zero Q, the uncertanty n locaton of the sensor remans constant wth tme. Wth an extremely small Q, the localzaton uncertanty slowly drfts wth tme. Ths means that the current measurements from the moble robot are gven more weght than the current node poston estmate, whch avods the node s becomng too certan of a poston that may be ncorrect. When the robot s n range and the sensor hears the broadcast poston of the robot, the measurement update equaton s used to combne the new nformaton to mprove sensor node poston and uncertanty estmates. In ths secton, the robot s assumed to be perfectly localzed. Thus when a sensor hears a broadcast, t could only be wthn the communcaton range of the robot whose poston s broadcast. The measurement uncertanty matrx R reflects ths, and s chosen as σ x, σ (12) σ y R σ const const y Range Range x σ x, σ y (13) σ σ where σ s the uncertanty ntroduced due to Range, the communcaton range of the robot. We assume the desgn parameter σ const 3, to nclude 7% of the communcaton range, Range, of the robot. (Gaussan uncertantes are assumed.) Through ths selecton of R the Kalman flter automatcally taes care of the range of the robot wthn whch t hears broadcasts. Table I shows the poston update algorthm that runs on each node, whch s very smple and easy to mplement. It conssts of four equatons, two for the tme update, and two for the measurement update. Ths algorthm automatcally provdes uncertanty estmates through the computaton of the error covarance P, whch s equvalent to the boundng box nformaton provded by the algorthm n [14]. TABLE I. STATIC SENSOR NODE LOCALIZATION ALGORITHM 1. At each dscrete tme nstant, 2. f robot broadcast receved by sensor 3. then 4. Update sensor state and uncertanty estmates usng KF measurement equatons (1,11). 5. else 6. Propagate estmates usng tme update equatons (8,9). 7. end f D. Smulaton Results Extensve smulatons have been performed to verfy the effectveness of the proposed algorthm. We also studed the effects of ntal sweep paths and the robot broadcast nterval on sensor localzaton. The moble robot s navgated along the desred sweep path and perodc locaton nformaton s broadcast. On recevng a broadcast, sensors update ther locaton and uncertanty estmates. Ths s a range-free procedure that reles on the lmted communcaton range of the robot, and as such, the sensor locatons are updated based on the poston of the robot. That s, the updated sensor poston estmate s a weghted combnaton of ts current locaton estmate and the current locaton of the robot. Thus sensors hearng only one broadcast wll have an estmated locaton that s projected onto the path of the robot. Fg. 2 shows the ntal snusodal sweep path and the poston and range of the broadcast wth a broadcast nterval of 5 dscrete tme nstants. The x represent the actual postons of the statc sensors that are to be localzed. The sensor nodes do not ntally now ther actual postons. The nodes all have ntal poston estmates beng the centrod of the deployment area, and an ntal uncertanty of nfnty, correspondng to complete lac of nowledge of ther postons. Fg. 3 llustrates the localzed sensors after the robot has made ts sweep through the networ. The represent the fnal poston estmates of the nodes. To reman consstent wth earler wor nvolvng boundng boxes (e.g. [14]), the uncertanty of the sensors n ther poston estmates has been depcted as rectangles representng 3 σ of the uncertanty dstrbuton, assumng Gaussan uncertantes. Note that the sensors always outsde the communcaton range of the moble

4 robot do not become localzed (.e. they have no boundng box, whch denotes nfnte poston uncertanty). The sensors that receve more than one broadcast from the moble robot end up beng better localzed, snce each poston update reduces the poston uncertanty. The effectveness of the algorthm s demonstrated by the fact that n every case, the actual locaton (mared by an x ) s wthn the uncertanty bound of the estmated poston (mared by a ). The localzaton error of the sensors, computed as the Eucldean dstance between true and estmated postons, s depcted n the vertcal axs of Fg. 4. Sensors near the path of the moble robot that have receved multple broadcasts have smaller errors. The same smulaton was rerun wth dfferent moble robot broadcast ntervals, and the effect of broadcast nterval on the average localzaton error of the networ s depcted n Fg. 5. Generally, as broadcast nterval decreases, the average error decreases. the deployment area. Uncertanty rectangles have been llustrated to depct the uncertanty of the sensor n ts poston estmate. Fgure 4. Localzaton error, computed as the Eucldean dstance between real and estmated postons, of sensors after ntal sweep of the deployment area. Fgure 2. Intal snusodal sweep path wth broadcast locatons and range of broadcast. Fgure 5. Effect of broadcast nterval on average localzaton error. III. SIMULTANEOUS MOBILE ROBOT AND SENSOR LOCALIZATION In ths secton we consder the realstc case where the moble robot s poston s not exactly nown. We provde an algorthm whch runs on the moble robot that fuses poston nformaton from GPS, when t s avalable, and from the already-localzed sensor nodes. Ths allows the robot to update ts poston estmate as well as the uncertanty estmate. When ths algorthm s run smultaneously wth the algorthm of the prevous secton runnng on each sensor node, the result s smultaneous moble robot and sensor localzaton. A procedure s gven to avod detrmental recursve feedbac between the two algorthms. Fgure 3. Localzed sensors, real postons (denoted by x ) and estmated postons (denoted by ), are llustrated after ntal moble robot sweep of

5 A. Moble Robot Localzaton When localzng the sensor nodes n the prevous secton, the robot was assumed to now ts poston exactly at all nstants of tme. However, as the robot navgates by dead reconng, or due to steerng naccuraces, ts localzaton ncreasngly deterorates as tme passes. Locaton updates from the GPS, when they occur, and from statonary sensor nodes that have already been localzed can be used to mprove the localzaton estmate of the robot. Some sensor nodes are localzed more fnely due to more numerous updates they have prevously receved from the moble robot. These sensors can be employed to localze the robot when ts poston nformaton deterorates. Ths s accomplshed by havng each sensor node mae a transmsson that contans the node s poston estmate and uncertanty. Ths s receved by the robot when t s n range. The sensors transmt at fxed ntervals, wth each sensor havng a dfferent random nterval. Ths ensures that the updates between moble robot and sensor nodes are staggered n tme and that no recursve feedbac occurs. A contnuous-dscrete extended Kalman flter runnng on the moble robot s used to smulate the robot and update the states usng measurements from the GPS system and the betterlocalzed UGSs. Extended Kalman flters have been used for local and nfrequent global senor data fuson [23], for moble robot localzaton [24], and n navgaton of autonomous vehcles [25]. For nformaton about the Extended Kalman flter see [26]. The contnuous-tme system model of the robot s gven by (2) as a( X, u, t) G( t)w (14) X + The sampled dscrete-tme measurement model of the robot s gven by Z Z h h [ X ( t ), ] + v [ X ( t ), ] + v (15) In the extended Kalman flter, the effect of tme on the robot states s gven by the tme update equaton Xˆ a P A, u, t) T T, t) P + PA, t) + GQG (19) In [27], the deleterous effects of tme passng are shown n terms of ncreasng poston uncertanty and decreasng belef. These effects are formally captured n a rgorous manner by the tme-update equatons (18)-(19), whch shows how uncertanty ncreases due to dead reconng and steerng uncertantes. The effects of the GPS navgaton updates, when they are receved, are gven by the measurement update equaton K P ( t ) H T ) H Xˆ ( ) ( ) [ I K H )] P ( t ) Xˆ K [ Z h +, ) ] P t Xˆ P ( t ) H T ) + R 1 (2) The effects of the updates based on localzed sensor nodes, when they are receved, are gven by the UGS measurement update equaton K P ( t ) H T ) H Xˆ ( ) ( ) [ I K H )] P ( t ) Xˆ K [ Z h +, ) ] P t Xˆ P ( t ) H T ) + R 1 (21) R The measurement uncertanty matrces R and represent the uncertanty n the GPS and the uncertanty n the update offered by UGS respectvely. The uncertanty n the sensor update, R, s a combnaton of the uncertanty of the sensor poston and the uncertanty due to the communcaton range of the sensor. These uncertantes combne n quadrature as where ( ) ( X, P ), w() t (, Q), v (, R ), v ( R ) X, (16) R P 2 const x Range σ x σ 2 σ x + σ, σ const y Range, σ y σ σ y (22) a x y φ α vt cosα cosφ v cosα sn φ vt sn L α ω α ( ) t X, t, G() t h 1 1 [ X t ), ], h [ X ( t ), ] (17) x x ( (18) y y where σ s the uncertanty ntroduced due to communcaton range of sensor. Range, the Smlarly, the measurement nose covarance of the sensor, (12), has to be modfed to nclude the uncertanty n the robot s poston. The robot s no longer absolutely localzed wth zero uncertanty. The uncertanty n robot localzaton and the uncertanty due to robot communcaton range combne n quadrature, modfyng (12) as

6 XY 2 2 R + P XY σ (23) P s the partal error covarance of the robot whch effects only the poston of the robot, and σ s as defned earler. The Jacobans of the nonlnear system, determned from (2), are gven by the followng system matrces: A H H ( X, t) a( X, t) X ( X ) ( X ) h h ( X, ) X ( X, ) X 1 1 v 1 1 cosα sn φ v cosα cosφ t t vt sn α cosφ vt sn α sn φ vt cosα L (24) Wth these equatons n place and programmed as a software algorthm on the moble robot, and the sensor nodes runnng the algorthm presented n the prevous secton, the moble robot and the statc sensors automatcally mutually update ther estmates wth ncomng updates. There s no addtonal decson-mang logc to be mplemented as n other range-free wor dscussed earler. There s no need to compute boundng boxes, as the error covarance matrces are automatcally updated as measurements are receved. The algorthm to be mplemented on the moble robot that updates ts poston estmate and uncertanty based on GPS measurements and on the localzed sensor nodes s gven as Table II. Ths algorthm s effcent to mplement snce the bul of t s mathematcal equatons. TABLE II. MOBILE ROBOT LOCALIZATION ALGORITHM 1. Navgate robot along desred path. 2. Broadcast locaton nformaton at dscrete ntervals. 3. f broadcast from GPS receved 4. Update robot state and uncertanty estmates usng measurement equaton (2). 5. end f 6. f broadcast from sensor receved 7. Update robot state and uncertanty estmates usng measurement equaton (21). 8. end f When algorthm II s run on the robot smultaneously along wth algorthm I on each sensor node, smultaneous moble robot and sensor localzaton occurs. Algorthm II on the moble robot. Infrequent GPS updates and temporally staggered sensor updates help localze the robot. Fg. 6 shows the robot s sweep path wth GPS and UGS updates dsabled. A systematc dead reconng error, [28], has been njected nto the moble robot to gve gradually deteroratng poston nformaton. The localzaton of the robot deterorates wth tme as can be seen n the devaton of the robot s estmated path (hyphenated green lne) from the robot s true path (contnuous green lne.) Fg. 7 llustrates the robot s sweep path whch s corrected n tme by GPS and UGS updates usng Algorthm II. As s evdent, the robot s localzaton has mproved and the postons of where the robot thns t s (the estmated poston), and where the robot actually s (the true poston) are much closer, snce the estmates are contnuously corrected usng Algorthm II as poston nformaton arrves, ether from GPS or from sensor node broadcasts. Fgure 6. Intal sweep path of the robot wth GPS and UGS updates dsabled. Robot s localzaton deterorates wth tme as evdent n the devaton n the estmated path (hyphenated green lne) and the true path (contnuous red lne.) Robot broadcasts occur along the true path of the robot and consst of the robot s estmated poston (slghtly dfferent from the robot s true poston where the broadcast occurs) and uncertanty. Sensors wthn range receve the broadcast and update ther postonal nformaton based on the robot s estmates. Fg. 8 llustrates the localzed sensors after the ntal sweep. True sensor postons are ndcated by an x and estmated postons by a. Now, some true sensor postons are outsde the 3σ boxes due to the added uncertanty n the robot poston, though they are generally close to these boxes. Fg. 9 depcts the fnal localzaton error of each sensor. B. Smulaton Results The smulatons descrbed n Secton II have been rerun wth GPS updates and sensor updates mplemented as

7 IV. CONCLUSION Rgorous mathematcal algorthms for adaptve smultaneous localzaton of the statc unattended ground sensors and the moble robot have been demonstrated. The frst algorthm localzes the statc sensors and the second algorthm localzes the moble robot. These algorthms together allow smultaneous localzaton of the statc sensor and the moble robot. A thrd adaptve localzaton algorthm ensures that the regon of the deployment area wth the largest uncertanty s localzed wth mnmal robot movement. Fgure 7. Intal sweep path of the moble wth GPS and UGS updates enabled as Algorthm II. The robot s localzaton has mproved and the true poston and the estmated poston of the robot along the path are much closer. Fgure 8. Localzed sensors after ntal sweep of the deployment area. True sensor postons are ndcated by a x and estmated postons by a. Fgure 9. Localzaton error of sensors computed as the Eucldean dstance between true and estmated postons. REFERENCES [1] N. Bulusu, J. Hedemann and D. Estrn, "Gps-less low cost outdoor localzaton for very small devces," IEEE Personal Communcatons Magazne, vol. 7, pp , 25. [2] J. Bachrach and C. Taylor, "Localzaton n sensor networs," n Handboo of Sensor Networs : Algorthms and Archtectures, 1st ed., vol. 1, I. Stojmenov Ed. USA: Wley, 25,. [3] B. Hofmann-Wellenhof, H. Lchtenegger and J. Collns, Global Postonng System : Theory and Practce, New Yor: Sprnger, 24. [4] R.L. Moses, D. Krshnamurthy and R.M. Patterson, "A self-localzaton method for wreless sensor networs," EURASIP Journal on Appled Sgnal Processng, 22. [5] N. Bulusu, J. Hedemann and D. Estrn, "Adaptve Beacon Placement," ICDCS '1: Proceedngs of the the 21st Internatonal Conference on Dstrbuted Computng Systems, pp. 489, 21. [6] C. Savarese, J.M. Rabaey and J. Beutel, "Locatonng n dstrbuted adhoc wreless sensor networs, IEEE Internatonal Conference on Acoustcs, Speech, and Sgnal Processng, 21. [7] N. Patwar, J.N. Ash, S. Kyperountas, A.O. Hero III, R.L. Moses and N.S. Correal, "Locatng the nodes: cooperatve localzaton n wreless sensor networs," Sgnal Processng Magazne, IEEE, vol. 22, pp , July 25. [8] P. Bahl and V.N. Padmanabhan, "RADAR: An In-Buldng RF-Based User Locaton and Tracng System," Proceedngs of the IEEE Infocom 2, Tel-Avv, Israel, pp , Mar. 2 [9] N.B. Pryantha, A. Charaborty and H. Balarshnan, "The crcet locaton-support system," Sxth Annual ACM Internatonal Conference on Moble Computng and Networng (MOBICOM), Boston, Massachusetts, August 2. [1] A. Savvdes, C. Han and M.B. Strvastava, "Dynamc fne-graned localzaton n Ad-Hoc networs of sensors," MobCom '1: Proceedngs of the 7th annual nternatonal conference on Moble computng and networng, Rome, Italy, pp , 21. [11] D. Nculescu and B. Nath, "Ad Hoc Postonng System (APS) usng AoA," Proceedngs of INFOCOM, San Francsco, CA, 23 [12] C. Taylor, A. Rahm, J. Bachrach and H. Shrobe, "Smultaneous Localzaton and Tracng n an Ad Hoc Sensor Networ," Informaton Processon n Sensor Networs, 25. [13] A.M. Broos, S. Wllams and A. Maareno, "Automatc Onlne Localzaton of Nodes n an Actve Sensor Networ," Internatonal Conference on Robotcs and Automaton, vol. 5, pp , 24. [14] S. Shenoy and J. Tan, "Smultaneous Localzaton and Moble Robot Navgaton n a Hybrd Sensor Networ," IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems, August 25. [15] A. Galstyan, B. Krshnamachar, K. Lerman and S. Pattem, "Dstrbuted onlne localzaton n sensor networs usng a movng target," IPSN'4: Proceedngs of the Thrd Internatonal Symposum on Informaton Processng n Sensor Networs, pp. 61-7, 24. [16] V. Cevher and J.H. McClellan, "Sensor array calbraton va tracng wth the extended Kalman flter," IEEE Internatonal Conference on Acoustcs, Speech, and Sgnal Processng, vol. 5, pp , 21. [17] E. Olson, J. Leonard and S. Teller, "Robust Range-Only Beacon Localzaton," n IEEE/OES Autonomous Underwater Vehcles, 24, pp

8 [18] W. Xao, J.K. Wu and L. Xe, "Sensor schedulng for target tracng n networs of actve sensors," n IEEE Internatonal Worshop on Sensor Networs and Applcatons, 25. [19] Dssanayae, Gamn M. W. M., P. Newman, S. Clar and H.F. Durrant- Whyte, "A Soluton to the Smultaneous Localzaton and Map Buldng (SLAM) Problem," vol. 17, pp , June [2] J.W. Fenwc, P.M. Newman and J.J. Leonard, "Cooperatve Concurrent Mappng and Localzaton," n Proceedngs of the 22 IEEE Internatonal Conference on Robotcs and Automaton, May 22, pp [21] P.S. Maybec, Stochastc models, estmaton, and control, New Yor: Academc Press, [22] W.S. Levne Ed., The Control Handboo, New Yor: CRC Press, [23] S.I. Roumelots and G.A. Beey, "An extended Kalman flter for frequent local and nfrequent global sensor data fuson," n SPIE Internatonal Symposum on Intellgent Systems and Advanced Manufacturng, pp [24] E. Kry and M. Buehler, "Three-state Extended Kalman Flter for Moble Robot Localzaton," McGll Unversty., Montreal, Canada, Tech. Rep. TR-CIM 5.6, Aprl, 22. [25] A. Kelly, "A 3D State Space Formulaton of a Navgaton Kalman Flter for Autonomous Vehcles," Carnege Mellon Unversty., Pttsburgh, PA, Tech. Rep. CMU-RI-TR-94-19, May, [26] F.L. Lews, Optmal Estmaton, New Yor: John Wley & Sons, [27] D. Fox, W. Burgard, F. Dellaert and S. Thrun, "Monte Carlo Localzaton: Effcent Poston Estmaton for Moble Robots," Proceedngs of the Sxteenth Natonal Conference on Artfcal Intellgence (AAAI'99), July, [28] J. Borensten and L. Feng, "Measurement and correcton of systematc odometry errors n moble robots," IEEE Transactons on Robotcs and Automaton, vol. 12, pp , 1996.