Fixed-lag Sampling Strategies for Particle Filtering SLAM

Transcription

1 To appear in he 7 IEEE Inernaional Conference on Roboics & Auomaion (ICRA 7) Fixed-lag Sampling Sraegies for Paricle Filering SLAM Krisopher R. Beevers and Wesley H. Huang Absrac We describe wo new sampling sraegies for Rao-Blackwellized paricle filering SLAM. The sraegies, called fixed-lag roughening and he block proposal disribuion, boh exploi fuure informaion, when i becomes available, o improve he filer s esimaion for previous ime seps. Fixed-lag roughening perurbs rajecory samples over a fixed lag ime according o a Markov Chain Mone Carlo kernel. The block proposal disribuion direcly samples poses over a fixed lag from heir fully join disribuion condiioned on all he available daa. Our experimenal resuls indicae ha he proposed sraegies, especially he block proposal, yield significan improvemens in filer consisency and a reducion in paricle degeneracies compared o sandard sampling echniques such as he improved proposal disribuion of FasSLAM. I. INTRODUCTION Simulaneous localizaion and mapping (SLAM) algorihms based on paricle filers have gained excepional populariy in he las few years due o heir relaive simpliciy, compuaional properies, and experimenal successes. However, recen work such as ha by Bailey e al. [] and ohers has shown ha paricle filering SLAM algorihms are suscepible o subsanial esimaion inconsisencies because hey generally significanly underesimae heir own error. In large par his is due o degeneracies in he paricle filer sampling process. A paricle filer for SLAM represens he poserior disribuion of he robo s rajecory using a se of samples, or paricles. Condiioned on each paricle is a map, esimaed using a series of small exended Kalman filers for each landmark. A each ime sep, paricles are exended according o a moion model and maps are updaed based on sensor observaions. The paricles are weighed according o he likelihood of he observaions given he sampled poses and previous observaions. Finally, paricles are resampled (wih replacemen) according o o heir weighs in order o give more presence o highly-weighed rajecories. Paricle degeneracies occur when he weighs of paricles in he filer are highly skewed. In his case, he resampling sep selecs many copies of a few highly weighed paricles. Since resampling is repeaed ofen, he sampled poses represening pas porions of he robo s rajecory end o become degenerae (i.e., all or K. Beevers is wih he Deparmen of Compuer Science, Rensselaer Polyechnic Insiue, 8h Sree, Troy, NY 8, U.S.A., beevek@cs.rpi.edu W. Huang is wih Applied Percepion, Inc., Execuive Drive, Suie 4, Cranberry Township, PA 666, U.S.A., wes@appliedpercepion.com mosly all idenical) so ha hey insufficienly encode uncerainy in he esimaion. These degeneracies can have significan consequences if he robo revisis he poorly esimaed region, such as when closing a loop. In his paper we describe wo new sampling sraegies for paricle filering SLAM, inspired in par by he racking lieraure [7], [6], which improve he consisency of he filer s esimaion and he diversiy of he rajecory samples. The firs approach, ermed fixed-lag roughening, incorporaes a Markov Chain Mone Carlo (MCMC) move sep, perurbing pose samples over a fixed lag ime according o an MCMC kernel o comba paricle degeneracy. The second echnique employs a block proposal disribuion which direcly samples poses over a fixed lag ime from heir fully join disribuion condiioned on all of he available daa. The main idea behind boh mehods is o exploi fuure informaion o improve he esimaes of pas porions of he rajecory, in he sense ha he informaion becomes available only afer iniial esimaion of he poses. The new sampling echniques lead o significan reducions in esimaion error over previous approaches. For example, in our experimens, esimaion error using fixed-lag roughening was as lile as 3% ha of FasSLAM on average, and error using he block proposal was as lile as %, boh a he expense of a consan facor increase in compuaion ime. Furhermore, rajecory samples from he block proposal exhibi beer diversiy han hose from FasSLAM, wih he filer mainaining muliple samples over mos of he pose hisory for reasonably long rajecories. The primary complicaion of boh approaches is in designing and drawing from he sampling disribuions i.e., he MCMC kernel for fixed-lag roughening, and he fully join pose disribuion for he block proposal. This paper describes he main highlighs, bu some of he deails of he disribuion derivaions are provided in a companion echnical repor [4] o conserve space. In he nex secion we formally inroduce paricle filering SLAM and he consisency issue, and describe previous work on improving consisency and paricle diversiy. We inroduce fixed-lag roughening in Secion III and he block proposal disribuion in Secion IV. Secion V presens he resuls of experimens comparing our echniques o previous approaches. II. PARTICLE FILTERING SLAM The simulaneous localizaion and mapping (SLAM) problem is for a robo o concurrenly esimae boh

2 a map of he environmen and he robo s pose wih respec o he map. We consider he map o consis of a se of parameerized landmarks and rea SLAM as a sae esimaion problem, wih he goal of recovering he map x m = [x m... x m n ] T and he robo s imedependen rajecory x: r hrough he environmen. A each pose he robo execues a conrol command (i.e., moion) according o he conrol inpu u. I hen acquires an observaion (or se of observaions) z from is sensors and compues correspondence variables n mapping observaions o landmarks in he map. The goal of SLAM is herefore o esimae he PDF: p(x r :, xm u :, z :, n : ) () In Rao-Blackwellized paricle filering (RBPF), he poserior () is facored under cerain independence assumpions [] o obain: p(x: r u :, z :, n : ) }{{} rajecory poserior n p(x m i x r :, z :, n : ) }{{} i= landmark i poserior The poserior over rajecories is esimaed nonparamerically using N samples ( paricles ). Condiioned on he sampled values, landmarks are independen, and each is esimaed separaely, ypically by a small consan-size exended Kalman filer (EKF). The esimaion of he rajecory poserior by samples is done using sequenial imporance sampling wih resampling (SISR), or paricle filering. { The basic idea is o employ for each paricle φ i = x r,i :, xm,i} a moion model p(x r xr,i, u ) as a proposal disribuion o projec he robo sae forward by sampling, i.e.: x r,i () p(x r x r,i, u ) (3) Once every paricle is projeced forward, he poserior p(x r : u :, z :, n : ) is represened by he paricles. Nex, he samples are weighed according o he sensor measuremen likelihood, i.e.: ω i = ω i p(z x r,i, n, x m,i n ) (4) Finally, he paricles are resampled according o he weighs o obain a represenaion of (). An imporan issue in paricle filering SLAM is he consisency of he SLAM filer. A filer is inconsisen if i significanly underesimaes is own error, which can lead o divergence of he filer esimae from he ruh. Bailey e al. [] have shown experimenally ha in general, curren paricle filering SLAM algorihms are inconsisen. This is in large par due o degeneracies caused by frequen resampling such ha mos samples become idenical for much of he robo s rajecory. A. Relaed work Several researchers have addressed consisency in he conex of RBPF SLAM; we describe wo well-known approaches and a hird recen developmen. ) Improved proposal disribuion: For robos wih very accurae sensors such as scanning laser rangefinders, he measuremen likelihood in (4) is highly peaked. Thus, he proposal disribuion (3) samples many robo poses ha are assigned low weighs, so only a few samples survive he resampling sep. This can quickly lead o paricle degeneracies. An alernaive is o incorporae he curren sensor measuremen z ino he proposal disribuion, i.e.: x r,i p(x r x r,i :, u :, z :, n : ) (5) Using his approach, many more samples are drawn for robo poses ha mach well wih he curren sensor measuremen and paricles are more evenly weighed, so more paricles are likely o survive resampling. This improved proposal disribuion has been used in boh landmark-based SLAM [] (FasSLAM ) and in occupancy grid scan-maching SLAM [8]. ) Effecive sample size: The basic paricle filering algorihm resamples paricles according o heir weighs a every ieraion. I can be shown ha if he weighs of paricles are approximaely he same, resampling only decreases he efficiency of he sampled represenaion [9]. The effecive sample size is a useful meric o deermine wheher resampling is necessary []. I can be approximaed as: ˆN eff = / i= N ( ) ω i. If he effecive sample size is large, say, ˆN eff > N/, resampling is undesirable since he PDF over robo rajecories is well represened. This echnique was firs applied o SLAM by Grisei e al. [8]. 3) Recovering diversiy hrough sored sae: The preceding mehods focus on prevening loss of paricle diversiy. Anoher approach is o aemp o recover diversiy. Sachniss e al. [] sore he sae of he paricle filer upon deecing he robo s enry ino a loop. Afer repeaedly raversing he loop o improve he map (a process normally resuling in loss of diversiy), he filer sae is resored by splicing he loop rajecory esimae ono each saved paricle, effecively resoring he diversiy of he filer prior o loop closing. III. FIXED-LAG ROUGHENING Resampling leads o degeneracies because muliple copies of he same highly-weighed paricles survive. In his secion we describe a modificaion of he paricle filer sampling process ha incorporaes roughening of he sampled rajecories over a fixed lag so ha he poserior over rajecories is beer esimaed. A similar approach ermed resample-move has been described in he saisical lieraure in he conex of arge racking [7], [6], and has been menioned (bu no pursued) in he conex of SLAM by Bailey []. The basic idea is o incorporae a pos-sisr Markov Chain Mone Carlo (MCMC) sep o move he rajecory of each paricle over a fixed lag ime L afer he usual RBPF updae is complee. Specifically, for

3 each paricle φ i, i =... N, we sample xr,i L+: q(x r L+: ), where q is an MCMC kernel wih invarian disribuion p(x r L+: u :, z :, n : ). Afer he move, he paricles are sill disribued according o he desired poserior bu degeneracies over he lag ime L have been avered. Furhermore, some fuure informaion is used in drawing new values for previously sampled poses. The samples are already approximaely disribued according o he desired poserior before he move, so he usual burn-in ime of MCMC samplers can be avoided. The MCMC move can be repeaed o obain beer samples, alhough in our implemenaion we only perform a single move a each ime sep. There are wo main difficulies in implemening he approach. Firs, an appropriae kernel q and mehod for sampling from i mus be devised. Second, care mus be aken o avoid bias from couning he same measuremen wice, leading o a need for a simple mechanism o manage incremenal versions of he map. A. Fixed-lag Gibbs sampler for SLAM An effecive approach for sampling from he join MCMC kernel q(x r L+: ) is o employ Gibbs sampling, which samples each componen of x r L+: in urn from is condiional disribuion given he values of oher componens. A a paricular lag ime k, he singlecomponen disribuion is of he form: x r,i k p(x r k xr,i :k,k+:, u :, z :, n : ) (6) This disribuion can be manipulaed o obain: p(x r k xr,i :k,k+:, u :, z :, n : ) p(z k x r,i k, n k, x m n k )p(x m n k x r,i :k,k+:, z :k,k+:, n : ) dx m n k p(x r k xr,i k, u k) p(x r k xr,i k+, u k+) (7) The derivaion is given in [4]. We employ he usual Gaussian approximaions of he erms in (7) o sample each componen of he rajecory in urn (again, see [4]). The process is repeaed for every paricle o obain N samples {x r,i L+: }, and hen he maps of he paricles are updaed condiioned on he new rajecories. B. Incremenal map managemen To avoid bias he inermediae map esimae p(x m n k x r,i :k,k+:, z :k,k+:, n : ) used in (7) should incorporae all available informaion excep he measuremen z k from he ime sep being moved. Thus, we sore incremenal versions of he map over he lag ime so ha he inermediae map disribuions can be compued. To avoid soring muliple complee copies of he map of each paricle, he binary ree daa srucure of log N FasSLAM [] can be used o sore only he differences beween maps from each ime sep. In our implemenaion, we sore he map from ime L and he measuremens z L+:. The inermediae map disribuions are compued on he fly by applying EKF updaes o he map using he observaions from all bu he kh ime sep. C. Discussion Noe ha (7) is nearly idenical o he resul of similar manipulaions of he improved proposal disribuion (5) as described by Monemerlo []. The main difference is he backward model p(x r k xr,i k+, u k+) since we are sampling a pose in he mids of he rajecory raher han simply he mos recen pose. Noe also ha we do no reweigh he paricles afer performing he MCMC roughening sep. This is because he paricles before he move are asympoically drawn from he same disribuion as hose afer he move. IV. BLOCK PROPOSAL DISTRIBUTION An alernaive approach is o draw new samples for he las L poses direcly from he join opimal block proposal disribuion, i.e.: p(x r L+: u :, z :, n :, x r,i L ) (8) The basic idea is o sample from (8) a each ime sep, replacing he mos recen L poses of each paricle wih he newly sampled ones. The resul is a paricle filer ha is curren in ha is samples are always disribued according o he desired poserior () and can be used for, e.g., planning, bu which yields much beer samples since fuure informaion is direcly exploied by he join proposal. Thus, degeneracies in he weighs of paricles are much less likely o occur. A relaed echnique was recenly described by Douce e al. [6] in he general paricle filering conex. One can hink of he sandard improved proposal disribuion (5) as a -opimal version of he block proposal. The main difficuly in employing he block proposal is in drawing samples from he join disribuion (8). Our approach relies on he facorizaion due o Chib [5]: p(x r L+: u :, z :, n :, x r,i L ) = p(x r u :, z :, n :, x r,i L ) p(x r u :, z :, n :, x r,i L, xr,i ) p(x r L+ u :, z :, n :, x r,i L, xr,i L+: ) (9) Here, he ypical erm is: p(x r k u :, z :, n :, x r,i L, xr,i k+: ) p(x r k u :k, z :k, n :k, x r,i L ) p(xr k+ xr,i k, u k+) () (See [4] for deails.) The idea is o firs filer forward over he robo s rajecory by compuing he disribuions {p(x r k u :k, z :k, n :k, x r,i L )} using alernaing predicion/updae seps (e.g., wih an EKF), and hen sample backward, firs drawing x r,i p(x r u :, z :, n :, x r,i L ), and hen sampling he poses from he preceding ime seps in reverse order using he disribuions ha arise from subsiuing he sampled values ino (). This process is repeaed for every paricle, and he maps are updaed condiioned on he sampled rajecories.

4 Once new samples have been drawn for {x r,i L+: }, he paricles are reweighed wih he usual echnique: ω i = ω i arge disribuion proposal disribuion The opimal weigh updae can be shown [4] o be: () ω i = ω i p(z x r,i : L, u :, z :, n : ) () i.e., he updae is proporional o he likelihood of he curren measuremen using he forward-filered pose and map disribuion. A. Pracical implemenaion As wih fixed-lag roughening, we implemen he necessary models as Gaussians in pracice. Forward filering employs an EKF o compue he inermediae disribuions {p(x r k u :k, z :k, n :k, x r,i L )}, and mus be performed separaely for each paricle φ i, iniialized wih a zero-covariance disribuion cenered a x r,i L. During forward filering, a emporary version of he map mus be updaed wih he measuremens from each ime sep o obain he correc forward-filered disribuions. The ideal approach is o apply he EKF o he full sae vecor [x r x m ] T over he lag ime. An alernaive which we have used (see [4]) is o assume he landmarks are independen and apply he usual RBPF updaes o he landmarks during forward filering, inflaed by he uncerainy of he inermediae pose disribuions compued by he EKF. Afer forward filering, samples are drawn for each inermediae pose x r k, for k =... L+. The firs sample is drawn direcly from he (approximaely) opimal forward-filered disribuion. The remaining samples are condiioned on he poses drawn for succeeding ime seps by applying he same backward model as for fixed-lag roughening. Again, see [4] for deails. B. Discussion A firs i may appear ha he samples from he block proposal are no differen from hose obained wih fixed-lag roughening. In fac, while he samples are asympoically from he same disribuion (he desired poserior), hose obained from he block proposal are generally beer, because poses over he lag ime are drawn direcly from he join disribuion ha incorporaes fuure informaion. In fixed-lag roughening, poses are originally drawn using pas and presen informaion only, hen are gradually moved as fuure informaion becomes available. Only by applying many MCMC moves a each ime sep would he samples obained by fixed-lag roughening be as good as hose from he block proposal. V. RESULTS Our experimens compared FasSLAM, he fixedlag roughening (FLR) algorihm from Secion III, and he block proposal (BP) disribuion from Secion IV (a) Sparse environmen (b) Dense environmen Fig.. Simulaed environmens used o es he algorihms, consising of poin landmarks placed uniformly a random. Solid dark lines are ground ruh rajecories, kep he same for all simulaions. Ligh gray lines depic ypical uncorreced odomery esimaes. For he roughening and block proposal approaches we esed he algorihms wih several values for he lag ime L. All experimens used N = 5 paricles and resampling was performed only when ˆN eff < N/. Our experimens were in simulaion since comparing he esimaion error of he filers requires ground ruh. We assumed known daa associaions o preven poor correspondence-finding from influencing he comparison beween filering algorihms. Noise was inroduced by perurbing odomery and range-bearing measuremens. The observaion model used σ r = 5 cm and σ b =.3 wih a sensing radius of m, and he moion model used σ x =.d cos θ, σ y =.d sin θ and σ θ =.d +.4φ for ranslaion d and roaion φ. Experimens were performed in a variey of simulaed environmens consising of poin feaures. We presen resuls from wo represenaive cases wih randomly placed landmarks: a sparse map wih a simple 7 sec. rajecory (no loops) and a dense map wih a 63 sec. loop rajecory. The environmens, ground ruh rajecories, and ypical raw odomery esimaes are shown in Figure. All resuls were obained by averaging 5 Mone Carlo rials of each simulaion. A. NEES comparison We begin by comparing he normalized esimaion error squared (NEES) [3], [] of he rajecory esimaes produced by each algorihm. The NEES is a useful measure of filer consisency since i esimaes he saisical disance of he filer esimae from he ground ruh, i.e., i akes ino accoun he filer s esimae of is own error. For a ground ruh pose x r and an esimae ˆx r wih covariance ˆP r (compued from he weighed paricles assuming hey are approximaely Gaussian), he NEES is (x r ˆxr )( ˆP r ) (x r ˆxr )T. The recen paper by Bailey e al. [] gives more deails abou using NEES o measure RBPF SLAM consisency. We compued he NEES a each ime sep using he curren paricle se. Figure shows he resuling errors from each of he algorihms. (To conserve space, we presen reduced versions of he plos here. Larger

5 Alg. NEES alg NEES alg / NEES FS Alg. ˆN eff FS FS 5 FLR() FLR() 5 5 FLR() FLR() 5 BP() BP() 5 5 BP() (a) Sparse environmen BP() 5 (a) Sparse environmen FS FS 5 FLR() FLR() 5 5 FLR() FLR() 5 BP() BP() 5 5 BP() (b) Dense environmen BP() 5 (b) Dense environmen Fig.. NEES (lef column) and he raio of NEES o ha of FasSLAM (righ column) for each of he algorihms, versus he simulaion ime on he x-axis. We use he abbreviaions FS, FLR(L), and BP(L) o indicae FasSLAM, fixed-lag roughening wih lag L, and block proposal wih lag L respecively. versions can be seen in [4].) In he sparse environmen, NEES grows seadily for FS and FLR, and for BP wih small lag imes. Increasing he lag ime for FLR has relaively lile effec on NEES because fuure informaion is exploied slowly (see Secion IV-B). FLR s NEES is approximaely 33% ha of FS on average for L = 5 and L =. On he oher hand, increasing he lag ime for BP dramaically reduces NEES. The NEES of BP() is roughly 73% ha of FS on average; for, %; and for BP(), %. For he dense case he resuls are similar. Noe ha FLR avoids degeneracies (manifesed as spikes in he NEES plos) by moving paricles afer resampling. Ineresingly, increasing L in a dense environmen appears o slighly increase he NEES of FLR, a subjec warraning furher invesigaion. Noe ha he range of he NEES plos is quie large none of he filers is ruly consisen. (A consisen filer over 5 Mone Carlo rials should have NEES less han 3.7 wih 95% probabiliy [].) While he esimaion error using fixed-lag roughening and he Fig. 3. ˆN eff for each of he algorihms, versus he simulaion ime on he x-axis. block proposal is significanly reduced, hese sraegies alone do no guaranee a consisen filer, a leas wih reasonably small lag imes. In fac i is likely ha guaraneeing consisen SLAM esimaion (wih high probabiliy) while represening he rajecory poserior by samples requires drawing he full dimensionaliy of he samples from a disribuion condiioned on all he measuremens, e.g., wih MCMC, since paricle filering is always suscepible o resampling degeneracies depending on he environmen and rajecory. B. ˆN eff comparison The effecive sample size ˆN eff is also a useful saisic in examining filer consisency. If ˆN eff is high, he weighs of paricles are relaively unskewed, i.e., all paricles are conribuing o he esimae of he rajecory poserior. Furhermore, since ˆN eff dicaes when resampling occurs, high values of ˆN eff indicae less chance for degeneracies in pas porions of he rajecory esimae because resampling occurs infrequenly. Figure 3 shows ˆN eff as compued a each ime sep in he simulaions. In he sparse case, FLR exhibis no

6 significan improvemen over FS, bu in he dense environmen and FLR() have abou 7% higher ˆN eff han FS on average. Again, BP exhibis sronger resuls, wih BP() more han % beer han FS in he dense case, and 34% beer in he sparse case. This can be aribued o direc use of fuure informaion by he block proposal, which leads o beer samples for essenially he same reason FasSLAM s samples are beer han hose of FasSLAM. C. Paricle diversiy Finally, we examine paricle diversiy for each of he filers. Figure 4 shows he variance of he pose hisories of all he paricles, compued a he end of SLAM, along wih he number of unique paricles represening each pose. For all of he algorihms, he end of he rajecory is beer represened han earlier porions. FLR exends he represenaion over he lag ime bu he ypical quick dropoff remains. BP avoids he loss of diversiy in he sparse case, mainaining non-zero variance over mos of he rajecory, as one would expec since lile resampling occurs due o he high effecive sample size. In a denser environmen a significan amoun of resampling sill occurs, reducing he benefi somewha. VI. CONCLUSIONS We have described wo new sampling sraegies for paricle filering SLAM. The firs mehod, fixed-lag roughening, applies an MCMC move o he rajecory samples over a fixed lag a each ime sep. The second approach, he block proposal disribuion, draws new samples for all poses in a fixed-lag porion of he rajecory from heir join disribuion. Boh echniques exploi fuure informaion o improve he esimaion of pas poses. We have given overviews of he echnical deails behind each approach; full deails are in [4]. Our resuls show ha he new algorihms lead o subsanial improvemens in SLAM esimaion. Fixedlag roughening and he block proposal yield samples wih lower saisical esimaion error han hose of FasSLAM. Furhermore, samples drawn from he block proposal end o have much more uniform imporance weighs, leading o less need for resampling and consequenly, improved paricle diversiy. REFERENCES [] T. Bailey. Mobile robo localisaion and mapping in exensive oudoor environmens. PhD hesis, Ausralian Cener for Field Roboics, Universiy of Sydney, Augus. [] T. Bailey, J. Nieo, and E. Nebo. Consisency of he FasSLAM algorihm. In IEEE Inl. Conf. on Roboics and Auomaion, pages 44 47, 6. [3] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan. Esimaion wih applicaions o racking and navigaion. Wiley, New York,. [4] K. Beevers. Sampling sraegies for paricle filering SLAM. Technical Repor 6-, Deparmen of Compuer Science, Rensselaer Polyechnic Insiue, Troy, NY, 6. [5] S. Chib. Calculaing poserior disribuions and modal esimaes in Markov mixure models. Journal of Economerics, 75():79 97, 996. Alg. Sample variance Unique samples FS FLR() FLR() BP() BP() FS FLR() FLR() BP() BP() x 4 x 4 x 4 x 4 x 4 x 4 x 4 (a) Sparse environmen (b) Dense environmen Fig. 4. Comparison of paricle diversiy. The plos shown here are compued using he rajecory samples a he end of SLAM, i.e., {x r,i : u :, z :, n : }. On he lef is he sample variance for each pose in he rajecory, compued as race( ˆP r ). On he righ is he number of unique samples represening each pose. [6] A. Douce, M. Briers, and S. Sénécal. Efficien block sampling sraegies for sequenial Mone Carlo mehods. Journal of Compuaional and Graphical Saisics, o appear, 6. [7] W. Gilks and C. Berzuini. Following a moving arge Mone Carlo inference for dynamic Bayesian models. Journal of he Royal Saisical Sociey B, 63():7 46,. [8] G. Grisei, C. Sachniss, and W. Burgard. Improving gridbased SLAM wih Rao-Blackwellized paricle filers by adapive proposals and selecive resampling. In Proc. of he IEEE Inl. Conf. on Roboics and Auomaion, pages , 5. [9] J. Liu. Mone Carlo sraegies in scienific compuing. Springer, New York,. [] J. Liu and R. Chen. Blind deconvoluion via sequenial impuaions. Journal of he American Saisical Associaion, 9(43): , June 995. [] M. Monemerlo. FasSLAM: a facored soluion o he simulaneous localizaion and mapping problem wih unknown daa associaion. PhD hesis, Carnegie Mellon Universiy, Pisburgh, PA, 3. [] C. Sachniss, G. Grisei, and W. Burgard. Recovering paricle diversiy in a Rao-Blackwellized paricle filer for SLAM afer acively closing loops. In Proc. of he IEEE Inl. Conf. on Roboics and Auomaion, pages ,