Improving Occupancy Grid FastSLAM by Integrating Navigation Sensors

Transcription

1 Improving Occupancy Grid FasSLAM by Inegraing Navigaion Sensors Chrisopher Weyers Sensors Direcorae Air Force Research Laboraory Wrigh-Paerson AFB, OH Gilber Peerson Deparmen of Elecrical and Compuer Engineering Air Force Insiue of Technology Wrigh-Paerson AFB, OH Absrac When an auonomous vehicle operaes in an unknown environmen, i mus remember he locaions of environmenal objecs and use hose objec o mainain an accurae locaion of iself. This vehicle is faced wih Simulaneous Localizaion and Mapping (SLAM), a circularly defined roboics problem of map building wih no prior knowledge. The SLAM problem is a difficul bu criical componen of auonomous vehicle exploraion wih applicaions o search and rescue missions. This paper presens he firs SLAM soluion combining sereo cameras, inerial measuremens, and vehicle odomery ino a Muliple Inegraed Navigaion Sensor (MINS) pah. The FasSLAM algorihm, modified o make use of he MINS pah, observes and maps he environmen wih a LIDAR uni. The MINS FasSLAM algorihm closes a 140 meer loop wih a pah error ha remains wihin 1 meer of surveyed ruh. This pah reduces he error 79% from an odomery FasSLAM oupu and uses 30% of he paricles. I. INTRODUCTION One of he fundamenal properies of an auonomous vehicle is is percepion of he environmen, even when iniially unknown. This becomes increasingly difficul wihou he aid of exernal signals such as he Global Posiioning Sysem (GPS), which is ofen unavailable indoors. The challenge of navigaing hrough an unknown environmen while building a map is he Simulaneous Localizaion and Mapping (SLAM) problem. Is soluions include no only where a vehicle is, bu also a represenaion of where i has raveled. The SLAM problem is fundamenally difficul, as hese capabiliies are codependen by hemselves. However, an auonomous vehicle implemening a SLAM soluion has incredible poenial wihou relying on GPS or exernal communicaion. Nearly all SLAM soluions consruc he map using precise range measuremens, bu his precision is unhelpful if he pose esimae is uncerain. Vehicle odomery is ofen sufficien o obain an accurae esimae over a shor pah wih few urns. However, his becomes exceedingly difficul raveling a farher disance or exploring a larger environmen [2]. Odomery error is cumulaive and ofen grows quickly. An algorihm mus include more variance in is esimae o accoun for his, increasing compuaion and decreasing soluion cerainy. This research seeks o produce a more accurae vehicle pose esimae allowing for less variance and a more consisen soluion. Previous research has developed a navigaion sysem using an Exended Kalman Filer (EKF) o combine cameras and inerial measuremens [3]. I consiss of sereo cameras ighly coupled wih an Inerial Measuremen Uni (IMU) o produce an accurae posiion for differen plaforms in real-ime, bu is no designed for mapping [4]. This approach inegraes pose esimaes from sereo camera egomoion, IMU measuremen inegraion, and vehicle odomery in a linear Kalman filer (KF). This creaes a Muliple Inegraed Navigaion Sensor (MINS) pah. A paricle filer hen combines he MINS pah wih LIDAR ranges and creaes an occupancy grid map in a FasSLAM soluion [5]. Because he MINS sysem calculaes is posiion ouside he paricle filer, he SLAM implemenaion does no increase in complexiy. In real world ess, a vehicle operaing all necessary sensors ravels a 140 m loop around indoor hallways and collecs daa, which he MINS sysem and FasSLAM implemenaion process. Wih more sensors o esimae posiion, his MINS and FasSLAM sraegy produces a more accurae pah wih 79% less error han he FasSLAM implemenaion using only vehicle odomery. The following secion gives a background on relaed SLAM sraegies and sereo image navigaion. The nex secion discusses he mehods used in incorporaing IMU daa and image feaures wih odomery o achieve conrol inpu u, and specifics of he SLAM implemenaion. The final secions describe he esing procedure used and resuls obained, as well as propose furher opics o explore. II. BACKGROUND AND RELATED WORK This secion reviews exising SLAM and navigaion echniques uilized in and relaed o his work. A. SLAM Soluions The SLAM problem is ofen represened as a Dynamic Bayes Nework (DBN) in Fig. 1. Online soluions make he Markov assumpion, saving all useful informaion in he curren sae [6]. In he DBN srucure, observed variables (conrols u and measuremens z ) are inpus and hidden variables (poses s and map Θ) form he SLAM soluion. Since algorihms make localizaion he firs aspec of SLAM, he mos imporan aspec is o accuraely represen he vehicle posiion and orienaion in 2D wih coordinaes x, y, and heading θ. Added geomerically, hese are he elemens of vehicle pose s, depiced in (1) as a vecor. s = [ x y θ ] T (1)

2 Fig. 1. SLAM as a DBN. Conrols u and measuremens z are observed a each ime, as vehicle poses s and a single map Θ are hidden [7]. To solve he probabilisic DBN, vehicle localizaion algorihms redefine he SLAM soluion o be he probabiliy ha a vehicle occupies any given locaion. To localize in an environmen, he Bayesian filering problem execues a moion model, hen represens an esimae of he poserior probabiliy in (2) using he Bayes heorem [8]. p(s z 0: ) = p(z s ) p(s z 0: 1 ) p(s z 0: 1 ) SLAM soluions ierae by firs esimaing a pose wihin an exising map, hen updaing he map o incorporae new measuremens [9]. The SLAM poserior is similar o he localizaion poserior wih more variables. SLAM requires he curren pose s, all measuremens z 0: and conrols u 1: o mainain he map Θ. The poserior is calculaed as an inegral in (3), where η is a normalizaion consan. p(s, Θ z 0:, u 1: ) = η p(z s, Θ) p(s s 1, u ) p(s 1, Θ z 0: 1, u 1: 1 ) ds 1 (3) Due o is occupancy grid map sorage, his research does no include a erm for daa associaion. Raher, i considers daa associaion par of he measuremen model. Because evaluaing his inegral is compuaionally prohibiive, SLAM soluions use saisical esimaing echniques o approximae he Bayesian filer problem [10]. A common approach o approximaing his poserior is o use a paricle filer o mainain he error disribuion hrough M samples, each one soring a pose esimae s [m] and map Θ [m] [10]. FasSLAM operaes a Rao-Blackwellized Paricle Filer (RBPF), which samples from a Gaussian proposal disribuion [11]. I hen calculaes an imporance weigh for each paricle in (4), as he raio of disribuions a each sample locaion. w [m] w [m] = arge disribuion proposal disribuion = p(s[m] z, u ) p(s [m] z 1, u ) Wih he weighing deermined, FasSLAM uses a Sampling Imporance Resampling (SIR) algorihm o selec a new se of samples based on each w [m]. This replaces low weighed paricles wih higher weighed ones much like he operaion of a Geneic Algorihm. (2) (4) B. SLAM Improvemens To reduce he chances of replacing good paricles, a SLAM soluion can implemen a selecive resampling process o only resample when necessary [12]. This reduces he risk of removing accurae paricles by only resampling he paricle se once he weighing indicaes a need, insead of every ime. This sraegy is also implemened in his research, as boh he RBPF and meric map echniques are similar. Anoher FasSLAM enhancemen uses scan maching, which leverages he accuracy of LIDAR unis; his reduces he number of paricles required, cuing memory requiremens and compuaion ime [12]. Using he principle of scan maching, he mos recen measuremen is considered in addiion o vehicle movemen, resuling in a paricle disribuion wih less variance. The RBPF hen preceeds wih weighing he paricles by comparing z o each Θ [m]. A common map soring sraegy is meric represenaion, where each sores objecs by heir absolue posiion. Soring a global meric map such as his carries a significan memory requiremen which can affec compuaion more han he algorihm complexiy [5]. Oher work mainains several local meric maps roughly he size of he sensor range [7]. This work also mainains an overall opological map (soring locaions relaive o each oher), and places he local meric maps wihin he global opological one. This resuls in less of an overall sorage requiremen and helps o keep local areas correcly placed relaive o each oher. C. Machine Vision SLAM Many SLAM implemenaions have presened resuls using odomery and LIDAR, bu ohers presen he use of oher inpu sensors. Exracing SIFT feaures from cameras has been used as an alernaive SLAM inpu, removing he requiremen for oher sensors [13]. Laer work used a RBPF implemenaion wih feaures as inpu and resuled in a more accurae SLAM map han using pose esimaes from odomery measuremens [14]. Cos is a significan facor in hese siuaions, as cameras are smaller, ligher, cheaper, and require less power han LIDAR unis. Addiionally, cameras are passive sensors, as hey do no projec energy ino he environmen. This is safer and less deecable han using acive sensors like ranging devices. Oher work uses cameras alongside oher sensors, as used exensively for rover navigaion when paired wih odomery [15]. The process of using sequenial images for navigaion is called egomoion (also visual odomery or opical flow). Egomoion relies on epipolar geomery o calculae focal poins from sereo cameras [16]. This mahemaical soluion provides he basis for he feaure processing used in his research. The algorihms mach image feaures over ime, hen deermine heir posiion using sereopsis, he abiliy o approximae disance. Egomoion ranslaes feaure movemen ino vehicle moion, and has been used for SLAM applicaions wih differen map represenaion on simulaed daa [1].

3 Fig. 2. The KF akes inpu from IMU inegraion, sereo image egomoion, and odomery, hen provides u o a RBPF wih LIDAR ranges z. III. METHODOLOGY Where previous research used separae sensors o creae separae pahs, his work combines sereo image egomoion, inegraed IMU measuremens, and odomery using a linear KF o generae a single MINS pah. A FasSLAM implemenaion uses his MINS pah as u and LIDAR ranges as z. I carries ou he necessary seps o consruc occupancy grid maps from hese inpus. Fig. 2 shows an overall sysem diagram of MINS and FasSLAM. I has four inpus from he four sensors, and mainains pose s and map Θ as oupu. A. Sereo Egomoion To ake advanage of sereopsis, MINS begins by maching feaures beween he lef and righ camera images using SIFT [17]. MINS calculaes a meric similar o Euclidean disance from of each pair of lef and righ camera descripors d in (5) o find he disance beween each descripor pair. arccos(d lef d righ ) (5) If he closes mach is less han 0.6 imes he second closes, MINS maches he feaure and saves is pixel coordinaes. To reduce he number of false maches, MINS discards feaure maches in he oher image ha measure greaer han 1.5 from horizonal. Before he coordinaes can be accuraely used, MINS removes lens disorion effecs hrough he CalTech disorion model [3]. MINS hen uses epipolar geomery o measure he physical locaion of each feaure aking advanage of sereopsis [16]. I uses Direcion Cosine Maricies (DCMs) o conver pixels o physical locaions in he correc reference frame relaive o he vehicle. To implemen egomoion, MINS assumes ha he only hing moving in he environmen is he vehicle. This makes i possible o exrac a vehicle pose change, because he algorihm inerpres feaure moion as a resul of vehicle moion. MINS removes oulying feaure maches by eliminaing hose ha ravel more han 0.75 sandard deviaions from he mean disance raveled. MINS hen uses a polar represenaion o measure movemen by subracing he curren locaions from he previous locaions in polar form o obain posiion differences. This is opposie ypical moion, as he algorihm is ranslaing movemen in one direcion as vehicle movemen in he oher. Finally, MINS measures egomoion pose difference δs ego in (6) by convering he mean of he differences θ and r back o recangular form. δs ego = [ r cos( θ) r sin( θ) θ] T (6) B. Inerial Inegraion Obaining δs ego from he images is merely one of he hree measured pahs. Vehicle odomery measures a second pah δs odom ha MINS does no need o process. MINS compues an IMU pose difference δs imu for he hird pah, as he IMU measures linear acceleraions and roaional velociies. These measuremens, shown in (7) mus be inegraed wice and once, respecively [18]. [ẍ ÿ θ ] T (7) Due o specific IMU hardware, MINS does no compensae for coning and sculling effecs. However, o limi he effec of drifing, MINS racks and removes a bias in θ when he vehicle is saionary [3]. MINS firs inegraes ẍ and ÿ o curren velociy v in (8). To miigae growing inegraed errors, MINS limis v o vehicle maximum 1 m/s. ẋ 1 + (d)ẍ v = ẏ 1 + (d)ÿ (8) θ MINS performs numeric inegraion on he measured values in (7) o deermine curren IMU pose difference δs imu in (9), where d is he ime inerval since he previous measuremen [18]. δs imu = d ) (v 1 + v (9) 2 C. MINS Pah MINS combines δs imu from he IMU, δs ego from he cam- from odomery o produce he combined eras, and δs odom pose difference δs. I does his by implemening a linear KF, advanageous as i handles asynchronous updaes and exends easily o include differen sensors and uncerainies. KFs are ofen described as a series of predicions and observaions ha mainain sae x and covariance Σ [19]. as a predicion wih δs ego and as separae observaions. MINS ses he associaed MINS provides each δs imu δs odom variance of each from hardware specificaions and esed pah accuracy. MINS calculaes he KF funcions hrough he Bayes++ filering library [20]. The resul gives MINS a pose difference δs from KF sae x. D. FasSLAM Implemenaion MINS provides δs as conrol inpu u o FasSLAM as a 2D pah. MINS also sends FasSLAM he KF covariance Σ shown in (10) o represen is cerainy. Σ = σ xx σ xy 0 σ xy σ yy 0 (10) 0 0 σ θθ

4 1) Moion Model: The firs FasSLAM sep uses a moion model o propagae each paricle o an updaed pose using u. Four parameers deermine he amoun of he Gaussian noise applied. They represen he moion uncerainy and allow he RBPF o model specific errors in u [2]. Uncerainy in ranslaion by ranslaion α 3 and roaion by roaion α 1 correspond respecively wih he upper and lower blocks of Σ. Therefore, FasSLAM obains is moion model in (11) by modifying hese parameers wihou changing is uncerainy in roaion by ranslaion α 2 or uncerainy in ranslaion by roaion α 4 from configured consans. In his way, he MINS covariance affecs moion uncerainy in he FasSLAM implemenaion. ( ) σθθ α1 (σxx α 2 ) + σ xy + σ yy α3 (11) α 4 2) Measuremen Model: The measuremen model saisfies he second FasSLAM sep o apply z o each Θ [m]. Necessarily differen for each map represenaion and sensor, he measuremen model compares z o each Θ [m], accumulaing line errors e line ino scan errors e scan ha measure inconsisencies. This research uses a LIDAR uni and mainains a meric occupancy grid, soring probabiliies ha locaions are open or occupied [21]. Algorihm 1 gives an overview of his process, making calculaions similar o he map building process. Each LIDAR scan has a valid range r min o r max, and he occupancy grid has 0.1 m resoluion and a 256 value range. Addiionally, he model decays each previous map error e [m] 1 by discoun facor 0.99 o weigh recen observaions higher in deermining curren map error e [m] [6]. 3) Paricle Weighing: Afer execuing he measuremen model, FasSLAM assigns a weigh w [m] o each paricle from is map error e [m]. I firs subracs he minimum error value from each e [m], hen imposes a lower limi of 2 o preven a spike in he paricle disribuion. FasSLAM calculaes weigh w [m] in (12) by aking he reciprocal of is map error e [m] and normalizing he resul. ( w [m] = 1 M ) 1 1 (12) e [m] m=1 e [m] 4) Resampling: Before resampling, FasSLAM performs a es o deermine if resampling is necessary [12]. This reduces boh compuaion and he chances of a good paricle being replaced by resampling oo ofen. The es calculaes he number of effecive paricles by deermining a sum of he squared weighs. FasSLAM only resamples if his number is less han M/2, half he number of paricles. IV. TESTING AND RESULTS The es daa is colleced from he Pioneer P2-AT8 vehicle shown in Fig. 3. The P2-AT8 provides inernal odomery on skid seering wheels, and a SICK LMS 200 LIDAR uni. The vehicle also carries wo PixeLINK PL-A741 cameras Algorihm 1 LIDAR Grid Measuremen Model for all paricles [m] do e scan = 0 for all scans wih range r s a angle θ s do if r min < r s < r max hen e line = 0 x q = r s cos(θ s ) y q = r s sin(θ s ) q = s [m] + [x q y q 0] T ρ = r s /0.1 x d = (x q x [m] )/ρ y d = (y q y [m] )/ρ for i = 0 o ρ + 1 do x p = x [m] + (i)x d + (i)y d if i ρ and Θ[x p ][y p ] [m] > 0 hen e line += Θ[x p ][y p ] [m] (ρ + 1 i)(0.1/256) break else if i > ρ and Θ[x p ][y p ] [m] < 0 hen e line = Θ[x p ][y p ] [m] (0.1/256) end if end for e scan = e scan + e line end if end for e [m] end for Fig. 3. y p = y [m] = 0.99e [m] 1 + e scan The P2-AT8 vehicle wih cameras and IMU above he LIDAR. wih a 90 field of view and resoluion, and a MicroRoboics MIDG II consumer grade IMU wih 50 Hz sampling rae, boh conneced o an exernal PC. This PC connecs o he inernal one via Eherne cable o record colleced daa. A manually driven vehicle es run provides a reusable daa se. The vehicle begins in he norheas corner before raveling lef around he meer recangular loop. Afer reurning o is saring locaion, he vehicle makes a righ urn ino a room and sops. The implemenaion firs compues he feaure exracion, sereopsis, and egomoion using a SIFT feaure execuable

5 TABLE I PATH ERROR (METERS) Odomery MINS Odomery MINS FasSLAM FasSLAM Fig. 5 lef 5 righ 6 op 6 boom Sar NE NW SW SE NE Finish mean and MATLAB R scrip files. This porion requires hours of compuaion if calculaed direcly; improvemens have been discussed in previous research [4]. The main implemenaion includes he remainder of MINS and all of FasSLAM as an applicaion buil on he Bayes++ filering library. The remaining discussion concerns only his main implemenaion and no he image calculaions. As he rue vehicle pah was unknown a all imes, pah errors can be approximaed using a se of surveyed poins in he environmen corresponding o sar, finish, and each of he corner locaions. To compare he error of each pah, his research measures he disance beween he curren pose and he survey poin locaion a ha ime. Table I displays each error for odomery, he MINS sysem, and FasSLAM using odomery and FasSLAM using MINS for conrol inpu u. Afer aking he firs corner, vehicle odomery drifs away and compounds error much faser han he MINS sysem, which is only six meers away upon reurning o he saring locaion. The FasSLAM implemenaion grealy improves on each of hese pahs by incorporaing LIDAR scans. When using he more accurae MINS pah as inpu, FasSLAM remains wihin a meer a all poins. Table I does no facor errors in heading or inaccuracies a oher poins along he pah, so Fig. 4 displays vehicle odomery and he MINS pah alongside approximae ruh from he surveyed poins. MINS is suiable for navigaion requiremens in local environmens wihou using GPS. I displays accurae corners wih a smooh pah and sraigh hallways. MINS does no suffer problems from loss of image feaures, as consisen odomery provides an addiional KF inpu. The goal of SLAM is more abou building maps han producing a pah, so applying LIDAR scans produces a beer visual resul. The occupancy grids indicae open space as whie and occupied space as black; unobserved areas are grey. Fig. 5 displays maps from he odomery and MINS pahs, revealing more deails abou heir accuracy. The odomery map (Fig. 5 lef) conrass he severiy of is drif wih he accuracy of is measured disances. MINS (Fig. 5 righ) combines he accuracy of each sensor o produce a pah close o closing he loop. These pahs do no use he LIDAR scans o improve pose informaion. Since he FasSLAM implemenaion seeks o improve on hese pahs, Fig. 6 displays he maps generaed by he bes Fig. 4. Approximae ruh, odomery, and MINS over a building floor plan. Fig. 5. Maps produced from LIDAR scans using odomery (lef) and using he MINS pah (righ) FasSLAM paricle. Odomery FasSLAM wih 100 paricles (Fig. 6 op) improves he pah remarkably, bu is unable o remove all drif and does no close he loop; MINS FasSLAM (Fig. 6 boom) needs only 30 paricles o produce is map. Because he implemenaion calculaes he MINS pah ouside he FasSLAM algorihm, is compuaion ime is also reduced linearly wih number of paricles o 30%. Furhermore, he main MINS implemenaion (consising of IMU, pah, and KF calculaions) adds no measurable compuaion ime o he FasSLAM algorihm. This keeps overall compuaion linear in boh analysis and in resuls. The end resul of his research is he MINS sysem conneced o he FasSLAM implemenaion. Improving on he MINS pah and odomery FasSLAM resuls, using MINS wih FasSLAM successfully closes he 140 m loop. I displays sraigh hallways wih minimal inaccuracies from LIDAR scans, which are also presen in he oher maps. V. CONCLUSIONS AND FUTURE WORK This research presens a SLAM soluion for exploring indoor 2D environmens. I inegraes muliple navigaion

6 Fig. 6. The bes paricle maps from 100 paricle odomery FasSLAM (op) and 30 paricle MINS FasSLAM (boom). sensors o provide an improved soluion for FasSLAM. By combining sensors in MINS and using is pah in a FasSLAM implemenaion using LIDAR and occupancy grid maps, his research achieved a more accurae pah han odomery in real-world ess around a large loop. The mos ime inensive aspec of he implemenaion involves processing he sereo image feaures for egomoion, which is also he leas consisen pah. In comparison, he odomery and IMU pahs do no require advanced calculaion. To provide feasible real-ime navigaion, simple sensors like hese should be used wih accurae ranges. LIDAR scan maching appears o be he mos likely sraegy o improve his research, improving he moion disribuion and paricle accuracy [12]. Alernaively, MINS provides an opporuniy o apply new sensors and combinaions o SLAM soluions, bu i may be more complicaed han necessary. Implemening a simple KF, i can likely be replaced wih a weighed average sysem wihou losing much effeciveness, raher han incorporaing SLAM ino a more complex KF. ACKNOWLEDGMENTS The auhors would like o hank he Advanced Navigaion Technology Cener saff for all heir supporing effors. This work was suppored in par hrough AFRL/RYMN Lab Task 06SN02COR from he Air Force Office of Scienific Research. The views expressed in his aricle are hose of he auhors and do no reflec he official policy or posiion of he Unied Sae Air Force, Deparmen of Defense, or he U.S. Governmen. REFERENCES [1] G. Bleser and G. Hendeby, Using opical flow as lighweigh slam alernaive, in IEEE Inernaional Symposium on Mixed and Augmened Realiy, 2009, pp [2] S. Thrun, W. Burgard, and D. Fox, Probabilisic Roboics. The MIT Press, [3] M. Veh, Fusion of image and inerial sensors for navigaion, Ph.D. disseraion, Air Force Insiue of Technology, [4] J. Flecher, Real-ime gps-alernaive navigaion using commodiy hardware, Maser s hesis, Air Force Insiue of Technology, [5] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei, Fasslam: A facored soluion o he simulaneous localizaion and mapping problem, in AAAI Naional Conference on Arificial Inelligence, 2002, pp [6] S. Russell and P. Norvig, Arificial Inelligence: A Modern Approach, 3rd ed. Prenice Hall, [7] J.-L. Blanco, J.-A. Fernández-Madrigal, and J. Gonzalez, Towards a unified bayesian approach o hybrid meric-opological slam, IEEE Transacions on Roboics, vol. 24, no. 2, pp , [8] F. Dellaer, D. Fox, W. Burgard, and S. Thrun, Mone carlo localizaion for mobile robos, in IEEE Inernaional Conference on Roboics and Auomaion, vol. 2, 1999, pp [9] M. G. Dissanayake, P. Newman, S. Clark, H. Durran-Whye, and M. Csorba, A soluion o he simulaneous localizaion and map building (slam) problem, IEEE Transacions on Roboics and Auomaion, vol. 17, no. 3, pp , [10] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei, Fasslam 2.0: An improved paricle filering algorihm for simulaneous localizaion and mapping ha provably converges, in Inernaional Join Conference on Arificial Inelligence, 2003, pp [11] A. Douce, N. de Freias, K. Murphy, and S. Russell, Raoblackwellised paricle filering for dynamic bayesian neworks, in Conference on Uncerainy in Arificial Inelligence, 2000, pp [12] G. Grisei, C. Sachniss, and W. Burgard, Improved echniques for grid mapping wih rao-blackwellized paricle filers, IEEE Transacions on Roboics, vol. 23, pp , [13] S. Se, D. Lowe, and J. Lile, Mobile robo localizaion and mapping wih uncerainy using scale-invarian visual landmarks, Inernaional Journal of Roboics Research, vol. 21, no. 8, pp , [14] R. Sim, P. Elinas, and J. Lile, A sudy of he rao-blackwellised paricle filer for efficien and accurae vision-based slam, Inernaional Journal of Compuer Vision, vol. 74, no. 3, pp , [15] C. Olson, L. Mahies, M. Schoppers, and M. Maimone, Rover navigaion using sereo ego-moion, Roboics and Auonomous Sysems, vol. 43, no. 4, pp , [16] X. Armangué, H. Araújo, and J. Salvi, A review on egomoion by means of differenial epipolar geomery applied o he movemen of a mobile robo, Paern Recogniion, vol. 36, no. 12, pp , [17] D. Lowe, Disincive image feaures from scale-invarian keypoins, Inernaional Journal of Compuer Vision, vol. 2, no. 60, pp , [18] D. Tieron and J. Weson, Srapdown Inerial Navigaion Technology, 2nd ed. The Insiuion of Elecrical Engineers, [19] P. Maybeck, Sochasic Models, Esimaion, and Conrol. Academic Press, 1979, vol. 1 and 2. [20] M. Sevens, Bayes++ bayesian filering library, Ausralian Cenre for Field Roboics, [21] A. Howard, Muli-robo simulaneous localizaion and mapping using paricle filers, Inernaional Journal of Roboics Research, vol. 25, no. 12, pp , 2006.