On line Mapping and Global Positioning for autonomous driving in urban environment based on Evidential SLAM

Transcription

1 On line Mapping and Global Posiioning for auonomous driving in urban environmen based on Evidenial SLAM Guillaume Trehard, Evangeline Pollard, Benazouz Bradai, Fawzi Nashashibi To cie his version: Guillaume Trehard, Evangeline Pollard, Benazouz Bradai, Fawzi Nashashibi. On line Mapping and Global Posiioning for auonomous driving in urban environmen based on Evidenial SLAM. Inelligen Vehicles Symposium - IV 215, Jun 215, Seoul, Souh Korea. <hal > HAL Id: hal hps://hal.inria.fr/hal Submied on 7 May 215 HAL is a muli-disciplinary open access archive for he deposi and disseminaion of scienific research documens, wheher hey are published or no. The documens may come from eaching and research insiuions in France or abroad, or from public or privae research ceners. L archive ouvere pluridisciplinaire HAL, es desinée au dépô e à la diffusion de documens scienifiques de niveau recherche, publiés ou non, émanan des éablissemens d enseignemen e de recherche français ou érangers, des laboraoires publics ou privés.

2 On line Mapping and Global Posiioning for auonomous driving in urban environmen based on Evidenial SLAM Guillaume Trehard 1, Evangeline Pollard 1, Benazouz Bradai 2 and Fawzi Nashashibi 1 Absrac Locae a vehicle in an urban environmen remains a challenge for he auonomous driving communiy. By fusing informaion from a LIDAR, a Global Navigaion by Saellie Sysem (GNSS) and he vehicle odomery, his aricle proposes a soluion based on evidenial grids and a paricle filer o map he saic environmen and simulaneously esimae he posiion in a global reference a a high rae and wihou any prior knowledge. I. INTRODUCTION In he remaining challenges of auonomous driving in urban areas, vehicle accurae posiioning in is environmen is one of he mos difficul asks o ackle. A ciy offers indeed a large amoun of siuaions in which some sensors are occluded (e.g. in urban canyons, raffic jams or crowded srees) or he road have been modified (e.g. road works, evens) and a he same ime, hese siuaions require a high level of inerpreaion and knowledge o be correcly managed. Curren bes answers o his problem are based on high resoluion daa map (i.e. pre-recorded map close o sensors represenaion) mosly coming from a 3D laser scanner [1] bu also from cheaper 2D LIDAR [2]. Even wih low-cos embedded sensors such as a camera [3], his map suppor indeed enables o reach a cenimere accuracy. However, hese soluions could rapidly fail if heir map is no updaed quickly enough, if he road users are oo numerous and occlude sensors or if he area is simply no mapped. Anoher approach known as on-line Simulaneous Localizaion And Mapping (SLAM) [4], [5] enables o build a map of he crossed area while locaing he vehicle in i. The resuls a a local scale can be very accurae bu suffer a drif when considering long erm driving or large maps. On-line mapping suppored by a GNSS informaion have hen been proposed, mosly based on landmarks maps which are well adaped o visual SLAM [6], [7]. This GNSS suppor indeed leads o compensae he naural drif of SLAM soluions wihou any prior knowledge so ha he esimae of boh he pose and he surrounding map can be correced wih a GPS measure. If hese landmark maps are ineresing for he localizaion, heir descripion of he environmen is quie poor because limied o he landmarks hemselves. This informaion, wihou using high level 3D laser scan, can however be exraced from a basic 2D LIDAR sensor. A laser impac indeed provides boh he informaion of he free 1 RITS Team, INRIA, Domaine de Voluceau, 7815 Rocquencour, FRANCE hps://eam.inria.fr/ris 2 Valeo Driving Assisance Research France, 34 rue Sain Andre, 9312 BOBIGNY Cedex, FRANCE area crossed and abou he impaced obsacle. Using such a sensor in urban environmens becomes highly relevan when i comes o map he surroundings of a vehicle in erms of drivable areas and occupied zones. If heir mapping qualiy is no o be demonsraed any more, he occupancy grids mosly used as a discreized map of he surrounding area suffers from a drif which canno be easily correced and which lead o a growing error in he vehicle posiioning. Moreover, his grid, applied o oudoor applicaions can lead o significan compuaion coss which are no suiable in an embedded sysem. Based on he indoor localizaion soluions proposed in [8] and in [9], his aricle hen inroduces a soluion for on-line mapping and global posiionning using daa from a 2D LIDAR sensor and a basic GNSS receiver. I enables o preserve a quasi-insignifican drif in a shor range mapping and assure a consisen global posiioning wih a road level precision. Combining he mechanisms of an Evidenial SLAM [1] and of a Mone Carlo Localizaion close o he FasSLAM algorihm [8], his soluion uses a srong approximaion on he grid map consrucion which enables o scale he soluion up o a vehicle size wih a fas execuion. Afer a brief inroducion o he Evidenial SLAM conceps, his paper inroduces he proposed Mone Carlo algorihm wih he common grid map assumpion. Some resuls and a discussion on he consisency of he filer are finally proposed in he las secion. II. EVIDENTIAL SLAM Using evidenial heory in a Simulaneous Localizaion And Mapping algorihm has been proposed by he auhors in [1] and validaed in [11]. The conribuion was o propose o swich from he classic probabilisic framework o he Transferable Belief Model (TBM) framework enables o bypass he saic world assumpion in mos of he curren SLAM processes. The developed algorihm was an adapaion of a Maximum-Likelihood SLAM (ML-SLAM) for evidenial grids and wih daa coming from a LIDAR sensor. The oupu of he sysem was a 2D evidenial grid which is used as he map of he environmen and he posiion of he vehicle in his same map. An overview of he algorihm is proposed in Fig. 1 and he crucial poins of he process are discussed in his secion. A. Evidenial grids The main advanage of TBM over probabiliies is o explicily model he no-known and he incoheren informaion.

3 Polar grid map Conversion o Caresian coordinaes Maching m Ω i,j, X Merging m Ω i,j, 1 Normalizaion Relaive posiion and heading Occupancy grid Fig. 1: Overview of he Evidenial SLAM algorihm Applied o occupancy grids, his propery enables o describe he sae of each cell of he grid wih a se of four masses: {Free, Occupied, No-known, Conflic} also denoed {F, O, Ω, }. This se is defined as he Basic Belief Assignmen (BBA) of he cell and is updaed hrough he ime wih informaion coming from a LIDAR and according o he sensor model proposed by Moras e al. in [12]. This sensor model enhances boh he area crossed by he laser beam and he laser impac iself. As illusraed in Fig. 2, a polar grid is used o model and discreize he laser scan. For each cell of he polar grid map, defined by is angle θ and radius r, he measured BBA, denoed m Ω r,θ,, is hen filled as follows: { m Ω r,θ, (A) = λ { O if impaced m Ω wih A = r,θ, (Ω) = 1 λ F if crossed wih λ he confidence accorded o he LIDAR sensor. By convering hose polar grids in a Caresian reference and searching for he bes mach wih he previously buil evidenial grid map, he SLAM process updaes each cell saus in he surrounding environmen of he vehicle. Filling he polar grid map Impaced cells Laser beam (1) m Ω r,θ, (Ω) = 1 m Ω r,θ, (F) = 1 m Ω r,θ, (O) = 1 Fig. 2: Filling he polar grid map wih a new laser scan B. Mapping quasi saic environmen The Conflic represened in he TBM framework sands for he pieces of informaion which have been incoheren among differen sources of informaion. In he curren sysem, he evidenial grid map is updaed a each new laser scan so ha he only source of informaion is he laser sensor. These incoherences can hen occur in cases of differences beween he measured saes of a cell and he corresponding grid map cell. In an urban scenario and wih a correc ego-localizaion, he wo evens which can change a cell sae deeced by he LIDAR are he noise of he sensor iself (i.e. false alarms) or a moving obsacle, passing from one cell o anoher. If he reliabiliy of he laser sensor is good enough, one can assume ha mos of Conflic siuaions are he resul of he mobile obsacles in he surrounding of he vehicle. In is maching and merging operaion, he proposed algorihm hen balances he impac of hese mobile objecs in he SLAM process wihou any addiive racking sysem. The obained evidenial grid map is hen assumed o represen he quasi saic par of he environmen. The adjecive quasi saic refers here o a period of ime depending on he vehicle dynamics (e.g. High speed vehicles could see a large par of slow obsacles as saic). In his aricle, erms saic and quasi saic will be used indisincly o ease he read. C. Maching operaor The key par of he Evidenial SLAM lays in is maching sep. I aims a finding he opimum mach beween a new laser scan M, discreized as discussed in Sec. II-A, and he previously buil evidenial grid map M 1. The idea of he Evidenial SLAM algorihm was o propose a se of candidaes around an a priori displacemen which corresponds o he possible maches beween he polar grid (i.e. he new measure) and he evidenial grid (i.e. he pas measuremen). Each candidae C is represened by a ransformed version of he evidenial grid M C. I is hen scored o selec he mos likely candidae as he esimaed vehicle displacemen. In [1], he a priori displacemen was compued wih a basic Consan Velociy model and a discussion on he choice of he maching operaor Op led o he following one: Op( M C 1, M) = cells f( m Ω,C i,j, 1, m Ω i,j, ) (2) where m Ω,C i,j, 1 is he BBA of a cell (i,j) in he occupancy grid reference and wih he displacemen corresponding o C and m Ω i,j, is he corresponding BBA in he polar grid map. This has he effec o sum scores of all he couples of cells from he measured polar grid map and he sored one. The funcion f is hen defined: f( m Ω,C i,j, 1, m Ω i,j,) = ( mω,c i,j, 1 m Ω i,j,)(o) 1 ( m Ω,C i,j, 1 m Ω i,j,)( ) where denoe he disjuncive rule and he conjuncive rule of he TBM [13]. This operaor favouries he cells wih a BBA concenraed on he Occupied mass bu balances heir impac according o he conflic hey creae. Conflic siuaions (i.e. false alarms or mobile obsacles) will hen be ignored or heir impac will be limied in comparison o he saic environmen (cf Sec. II-B). In he following secions of his aricle, anoher way o build a se of candidaes will be proposed bu he same operaor Op will be used. (3)

4 III. SYSTEM ARCHITECTURE AND DETAILS The sysem inroduced in his publicaion aims a linking a local mapping provided by an Evidenial SLAM (cf. Sec II) and a global posiioning coming from a GNSS receiver. This fusion mus be operaed on-line so ha a vehicle or a mobile robo can be locaed in real-ime. As commened in [14], Mone Carlo Localisaion (MCL) has several advanages o ackle he vehicle global posiioning problem. MCL indeed performs well in non-linear and non-gaussian siuaions, i does no require a complex iniializaion and i is paricularly easy and fas o implemen. Moreover, i eases he fusion of informaion from differen kinds of sources and a differen raes because is mahemaical mechanism is more flexible wih regards o differen sensor models. This secion inroduces he heoreical background of he MCL and he archiecure proposed o fuse global localizaion and local mapping. A. Mone Carlo Localizaion The MCL algorihm is based on Bayes filering [9] and applied o mobile robo localizaion. I aims a esimaing he belief denoed Bel (i.e. he poserior densiy) of a dynamical sae x a ime, knowing all he pas measuremen daa. Using he same noaion as proposed in [14] he Bayes recursive filer heory hen provides he following updae equaion: Bel(x ) = ηp(o x ) p(x x 1,a 1 )Bel(x 1 )dx 1 (4) whih η a normalizaion facor, o he observaion daa and a he acion daa a ime. In his equaion, he densiies p(o x ) and p(x x 1,a 1 ) are respecively known as he percepual model and moion model which are boh assumed as ime invarian. Their noaion are hen simplified in [14] by p(o x) and p(x x, a). The key idea of he MCL is o assume ha a se of N weighed paricles could sample he belief Bel(x ). Each paricle is defined as a couple of a sae x i - a sample of x - and a weigh w i. The algorihm is hen divided in hree main seps: A se of N paricles is compued according o Bel(x 1 ) approximaed by he se {x i,w i } i=1...n, 1. A new se is hen proposed by following he moion model p(x x, a) for each paricle i. Each paricle is hen weighed according o is imporance regarding he percepual model p(o x). The so formed new se of paricles {x i,w i } i=1...n, represens he poserior densiy Bel(x ). In pracice, a resampling sep is required when he se of paricles is no efficien anymore o describe Bel(x ). To do so, he Sequenial Imporance Resampling algorihm [15] is used in he proposed sysem. B. Evidenial SLAM and MCL coupled Since he proposed soluion aims a esimaing he global posiion of a mobile robo while building he map of is surrounding, he dynamical sae x seen in Sec. III-A heoreically represens boh he complee pose (posiion X, Y and heading Θ in Caresian reference and displacemen r and Θ in curren vehicle polar reference) of he robo iself and he evidenial grid map of is surroundings M. X Y x = θ r (5) θ M Applying he MCL algorihm o his dynamic sae hen leads o a se of N paricles conaining a sample x i of Bel(x ) and an imporance facor w i. I is imporan o noice ha his sample hen represens a realisaion of boh he vehicle pose and he grid map of is surrounding. The moion model applied o each paricle is chosen as he following non-linear evoluion: ) i X i x i = Y i θ i a i M i ( r where a i =, he acion sensor informaion, is a Θ sample of he odomery measure densiy, M i is he grid map ransformed wih he displacemen represened by a i and he evoluion of he pose is proposed according o a bicycle model: i X Y = X Y θ θ i 1 + r cos(θ 1 + θ ) r sin(θ 1 + θ ) θ Considering only he laser scanner as an observaion sensor in his secion, he percepual model p(o x) of he MCL (cf. Sec. III-A) can hen be seen as he likelihood of he new laser scan knowing he a priori sae, i.e. he a priori posiion and grid map corresponding o he vehicle a ime. The MCL is based on a se of samples so his percepual model is equivalen o score each paricle according o is sae and he new laser scan. The proposiion is hen o use he maching operaor seen in II-C o updae he imporance weigh of each paricle. This weigh is updaed following his equaion: i (6) (7) w i = Op(M i, M) (8) A normalizaion sep hen occurs o assure ha he sum of w i over he N paricles equals one.

5 C. Common grid map assumpion If he MCL heory leads each paricle o represen he pose of he vehicle along wih he grid map of is environmen, he amoun of memory and compuaion power required o manage N grid maps can quickly increase and overpass reasonable resources. To bypass his limiaion and obain a fas algorihm, he proposiion is o assume ha he grid map obained by merging each new scan( according ) o he r poserior esimaion of he displacemen is a good Θ approximaion, for each paricle, of is own grid map M i. The same grid map can hen be used o es each paricle displacemen using he same new laser scan: M i M i 1 (9) where M i 1 is he previous esimaed grid map M 1 ransformed wih he displacemen corresponding o paricle i. I enables o sore a single grid map in memory which decreases he required compuer resources. The imporance weigh of each paricle is hen updaed using he following equaion: w i = Op( M i 1, M) (1) If his can be seen as a srong assumpion, resuls in Sec. IV show ha he map iself is no more affeced bu only drifs as if here was no fusion. Since he algorihm aims a providing a correc map of he direc surrounding along wih a global posiion, his drif is really sof in he concerned window so he common grid assumpion sofly affecs he global posiioning. D. GNSS updaing In addiion o he laser scanner a GNSS receiver is assumed o be available as an observaion sensor. The qualiy of his receiver is considered o be he same as a sandard GPS. Is horizonal precision wihou any map-maching is hen around 1 m and is rae is supposed o be 1 Hz. This rae is differen from he laser one (i.e. approximaely 1 imes slower) so ha i is assumed ha a GNSS measure never occurs a he exac same ime of a laser scan. Following he same process as in [14], he algorihm is hen ran each ime a new measure is coming, using eiher he laser scan or he GNSS measure. Considering his GNSS measure o Sa, he percepual model proposed in Sec. III-B is no adaped any more since GNSS informaion only conains a global posiion. Assuming ha his posiion is affeced by a whie noise wih a covariance of σ Sa, proporional o he Horizonal Diluion Of Posiion (HDOP), he imporance weigh of each paricle is hen updaed following his equaion: ( ) i w i X N(,o Y Sa,σ Sa) (11) ( ) i X i.e. he probabiliy of he paricle posiion from Y he Gaussian disribuion wih he average o Sa, he GNSS measure, and he sandard deviaion σ Sa. The Gaussian disribuion used o represen he GNSS noise cerainly appears as a srong simplificaion regarding oher models bu his sysem aims a highlighing he validiy of he common grid map assumpion (cf. Sec. III-C) via simulaed GNSS measure. A more advanced model will be used in fuure works. A. Resuls IV. VALIDATION In all he following resuls, he KITTI daabase is used as raw daa inpu [16]. The following sensors are hen simulaed: A one layer, 36 deg LIDAR daa is exraced from KITTI s Velodyne daa. An odomery measure is creaed by adding a whie noise wih sandard deviaions σ v =.3 m/s and σ w =.5 rad/s o he rue velociy and roaion speed. A GNSS measure is generaed using he MATLAB GPS oolbox [17] wih a sandard deviaion σ Sa = 8 m o simulae a classic GNSS signal. As an example of resul, he Roo Mean Squared Error (RMSE) of he global posiion on a 2.2 km sequence of he KITTI daabase has been compued over 5 runs of he algorihm and ploed on Fig. 3. In addiion, a example of an obained resul on anoher 2.5 km sequence can be found in Fig. 4. RMSE (m) GNSS sandard deviaion Compued RMS Errors ieraions Fig. 3: RMS Errors of 5 runs of he algorihm on he same KITTI sequence These resuls shows ha he proposed filer converges o an RMSE approximaely half of he sandard deviaion of he GNSS signal used. If his is sill a 3 m error, i is worh noicing ha i is achieved wih low cos odomery and GNSS sysem suppored by a reasonable cos LIDAR. Moreover, hese resuls are obained wihou any prior knowledge or map suppor so i shows he poenial of his algorihm if hose informaion were added. In addiion, he ime of execuion of he algorihm was beween 6 ms and 7 ms (depending on he laser impac number) wih 5 paricles and a local map of 1 m 1 m

6 and a resoluion of.2 m. The processor used was an Inel i5 wihou gpu suppor. Y [m] Measured posiion Ground ruh X [m] Fig. 4: Example of global posiioning on a KITTI sequences B. Consisency To measure he consisency of he proposed filer, he Normalized Esimaion Error Squared (NEES) is used [18], [19]. I is used o check wheher or no a filer can be considered as consisen by measuring, knowing he rue sae x, he NEES coefficien ǫ : ǫ = (x x )P 1 (x x ) T (12) where P 1 denoe he inverse covariance marix associaed o he esimaed sae x. This coefficien is averaged over M Mone Carlo runs of he algorihm. When M approaches he infinie, he proof of consisency is validaed if ǫ ends o he dimension of he considered sae for each sep of he algorihm. A simulaion has hen been run overm = 5 Mone Carlo ess on he same sequence of he KITTI daabase as he one used o compue he RMSE (cf. Sec. IV-A). The resuls are ploed in Fig. 5. NEES NEES ieraions ieraions Fig. 5: up: NEES corresponding o he complee pose down: NEES wih a resriced sae o he global posiion and heading X Y The firs plo refers o he NEES of he pose Θ r Θ inroduced in Sec. III-B. I shows fas augmenaions of he NEES coefficien largely over is supposed limi (i.e. he sae dimension: 5 here) which signifies ha he filer is opimisic. This resuls migh be linked wih he one obained by T. Bailey e al. in [18]. A paricle filer is indeed used o perform a SLAM algorihm and he opimisic characer of he filer is linked o he resampling sep which leads o loose rack of some displacemen hypoheses of he SLAM iself. The common grid map approximaion explained in Sec. III- C is moreover srongly inconsisen so ha he displacemen relaed par of he sae migh suffers his approximaion oo. The second plo confirms hese hypoheses. The NEES refers here o he global posiion par of he sae X Y Θ and he plo shows a much more reasonable resul of a filer slighly conservaive. I enables o conclude ha he global posiioning of he proposed filer is a safe approximaion of he rue posiion bu ha he drif which affecs he mapping par remains. C. SLAM drif As explained in Sec III-C, a common grid map is used for each paricle of he MCL algorihm o approximae he correc map. This hypohesis is assumed o be valid since only he direc surrounding of he environmen is considered and he corresponding grid map hen no suffer a srong drif. To validae his approximaion, he algorihm was esed over 11 differen sequences of he KITTI daabase and he drif affecing he grid map was compued according o he mehod proposed in [16]. Roaion error [deg/m] Translaion error [%] 8 6 Translaion errors (score: 5.74 %) Pah lengh [m] Roaion errors (score:.112 deg/m) Pah lengh [m] Fig. 6: Drif of he map on 11 sequences of he KITTI daabase The resuls ploed on Fig. 6 show he drif which affecs he map and illusrae ha i is under 5 % in displacemen and,3 deg/m in roaion. Those resuls are similar o he ones obained wih he Evidenial SLAM in erms of local posiioning [11], [1]. An example of he esed series is ploed Fig. 7.

7 Y [m] Measured posiion Ground ruh X [m] Fig. 7: Example of local posiioning on a KITTI sequence The common grid map assumpion can hen be validaed as an ineresing alernaive for on-line mapping of he direc environmens of he vehicle. D. Discussion One can noice ha he SLAM algorihm could be seen as an odomery measure in he proposed sysem. Through he observaion model p(o x) seen in Sec. III-B, he SLAM process indeed balances he displacemen of each paricle. As a consequence, he SLAM score infers on he global process only via he esimaion of he vehicle displacemen, exacly like an odomery. On anoher hand, he GNSS signal only provides informaion abou he global posiioning par of he algorihm. Is impac on he global process is hen resriced o he he global pose on he vehicle. This way of fusing a GNSS and a local mapping hen enables o provide a good local navigaion while searching for is coordinaes in global references. If hese global coordinaes are only a a road level of accuracy, he measured displacemen is accurae around he decimere so can suppor a planning or conrol sysem. In addiion, he global navigaion qualiy is sufficien o be mached wih a map daabase so can provide a direc link beween a local and a global descripion of he scene. V. CONCLUSION A soluion o fuse a GNSS localizaion wih an Evidenial SLAM using a paricle filer have been proposed and esed in his aricle. This soluion enables o boh locae a vehicle in a global reference and map is surrounding. The map of he surrounding suffers a drif which can be ignored when considering only he direc direc environmen so ha i can be used for conrol purpose. The filer is slighly conservaive so ha he oupu posiion provided is a safe esimaion which can hen be used in a map-maching algorihm. If he global performances sill have o be improved, his sysem is a fas soluion which can easily be implemened on an embedded sysem. I finally opens los of perspecives for urban driving such as on-line mapping using boh he informaion of a global map daabase and of he embedded sensors. REFERENCES [1] J. Levinson and S. Thrun, Robus vehicle localizaion in urban environmens using probabilisic maps, pp , 21. [2] J. Xie, F. Nashashibi, M. Paren, and O. G. Favro, A real-ime robus global localizaion for auonomous mobile robos in large environmens, 11h Inernaional Conference on Conrol Auomaion Roboics & Vision (ICARCV 21), pp , 21. [3] R. W. Wolco and R. M. Eusice, Visual localizaion wihin LIDAR maps for auomaed urban driving, pp , 214. [4] H. F. Durran-Whye and T. Bailey, Simulaneous localizaion and mapping: par I, Roboics & Auomaion Magazine, IEEE, vol. 13, no. 2, pp , 26. [5] T. Bailey and H. F. Durran-Whye, Simulaneous localizaion and mapping (SLAM): Par II, Roboics & Auomaion Magazine, IEEE, vol. 13, no. 3, pp , 26. [6] J. Carlson, Mapping Large, Urban Environmens wih GPS-Aided SLAM, Ph.D. disseraion, Carnegie Mellon Universiy, 21. [7] T. Bailey, Mobile robo localisaion and mapping in exensive oudoor environmens, 22. [8] D. H. D. Fox, W. Burgard, and S. Thrun, A highly efficien FasSLAM algorihm for generaing cyclic maps of large-scale environmens from raw laser range measuremens, in Proceedings IEEE/RSJ Inernaional Conference on Inelligen Robos and Sysems, 23. [9] F. Dellaer, D. Fox, W. Burgard, and S. Thrun, Mone Carlo localizaion for mobile robos, pp vol.2, [1] G. Trehard, Z. Alsayed, E. Pollard, B. Bradai, and F. Nashashibi, Credibilis Simulaneous Localizaion and Mapping wih a LIDAR, in Inernaional Conference on Inelligen Robos and Sysems (IROS), 214. [11] G. Trehard, E. Pollard, B. Bradai, and F. Nashashibi, Credibilis SLAM Performances wih Differen Laser Se-ups, in ICARCV h Inernaional Conference on Conrol, Auomaion, Roboics and Vision, 214. [12] J. Moras, V. Cherfaoui, and P. Bonnifai, Credibilis occupancy grids for vehicle percepion in dynamic environmens, IEEE Inernaional Conference on Roboics and Auomaion (ICRA 211), pp , 211. [13] G. Shafer, A mahemaical heory of evidence. Princeon universiy press Princeon, 1976, vol. 1. [14] S. Thrun, D. Fox, W. Burgard, and F. Dellaer, Robus Mone Carlo localizaion for mobile robos, Arificial Inelligence, vol. 128, no. 12, pp , May 21. [15] N. J. Gordon, D. J. Salmond, and A. F. M. Smih, Novel approach o nonlinear/non-gaussian Bayesian sae esimaion, pp , [16] A. Geiger, P. Lenz, and R. Urasun, Are we ready for auonomous driving? The KITTI vision benchmark suie, IEEE Conference on Compuer Vision and Paern Recogniion (CVPR 212), pp , 212. [17] A. K. Teewsky and A. Solz, COLUMNS-INNOVATION: GPS MATLAB TOOLBOX REVIEW, GPS World, vol. 9, no. 1, pp. 5 57, [18] T. Bailey, J. Nieo, and E. Nebo, Consisency of he FasSLAM algorihm, pp , 26. [19] T. Bailey, J. Nieo, J. Guivan, M. Sevens, and E. Nebo, Consisency of he EKF-SLAM Algorihm, pp , 26.