Monte Carlo Sampling of Non-Gaussian Proposal Distribution in Feature-Based RBPF-SLAM

Transcription

1 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. Mone Carlo Sampling of Non-Gaussian Proposal Disribuion in Feaure-Based RBPF-SLAM Nina Marhamai, Hamid D. Taghirad Advanced Roboics and Auomaed Sysems K. N. Toosi Universiy of Technology Tehran, Iran Kasra Khosoussi Cenre for Auonomous Sysems Faculy of Engineering and IT Universiy of Technology, Sydney Ausralia Absrac Paricle filers are widely used in mobile robo localizaion and mapping. I is well-known ha choosing an appropriae proposal disribuion plays a crucial role in he success of paricle filers. The proposal disribuion condiioned on he mos recen observaion, known as he opimal proposal disribuion (OPD), increases he number of effecive paricles and limis he degeneracy of filer. Convenionally, he OPD is approximaed by a Gaussian disribuion, which can lead o failure if he rue disribuion is highly non-gaussian. In his paper we propose wo novel soluions o he problem of feaure-based SLAM, hrough Mone Carlo approximaion of he OPD which show superior resuls in erms of mean squared error (MSE) and number of effecive samples. The proposed mehods are capable of describing non-gaussian OPD and dealing wih nonlinear models. Simulaion and experimenal resuls in large-scale environmens show ha he new algorihms ouperform he aforemenioned convenional mehods. 1 Inroducion Sequenial Mone Carlo (SMC) mehods, also known as paricle filers, have shown a grea success in localizing mobile robos and solving simulaneous localizaion and mapping (SLAM) problem due o heir capabiliy in capuring non-gaussian and muli-modal poserior disribuions and dealing wih nonlinear models. Murphy [Murphy, 2000] proposed o make use of Rao- Blackwellized paricle filer (RBPF) o solve he mapping problem hrough condiioning he map on robo rajecory. Hence, i is only required o sample from he robo pah, which reduces he dimension of samplespace, while he locaions of he feaures can be esimaed analyically and independen of each oher given he robo rajecory. The firs efficien SLAM algorihm based on RBPF, FasSLAM [Monemerlo e al., 2002], employed a paricle filer o esimae he robo pose, and used EKF in order o esimae he posiion of he landmarks for each paricle. Paricle filers suffer from he so-called degeneracy problem which occurs since he variance of imporance weighs grows wih ime [Douce, 1998]. The proposal disribuion ha minimizes he condiional variance of imporance weighs is called he opimal proposal disribuion (OPD) which is consruced by considering he mos recen observaion [Douce, 1998]. This disribuion canno be obained in closed form for he nonlinear models of SLAM. Local linearizaion [Douce, 1998] approximaes he OPD as a Gaussian by linearizing he observaion model using he firs-order Taylor expansion. Monemerlo e al. proposed FasSLAM 2.0 [Monemerlo e al., 2003], which uses local linearizaion in order o sample from he OPD. The resuls showed major improvemens compared o FasSLAM 1.0. However, Fas- SLAM 2.0 is subjeced o linearizaion errors which can lead o he filer divergence. Furhermore, is inabiliy o describe non-gaussian proposal disribuions, is anoher imporan disadvanage. Merwe e al. inroduced unscened paricle filer (UPF) [Van Der Merwe e al., 2001] in which insead

2 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. of linearizing nonlinear models, unscened Kalman filer (UKF) is used in order o obain he mean and covariance of OPD and o approximae i by a Gaussian disribuion. UPF yields more accurae esimaes compared o local linearizaion. Kim e al. have recenly applied UPF o he problem of feaure-based SLAM [Kim e al., 2008]. Their mehod, Unscened FasSLAM (UFasSLAM), has shown superior resuls compared o FasSLAM 2.0 in erms of he MSE and consisency. More recenly, Saha e al. have proposed a more accurae Gaussian approximaion algorihm for he OPD hrough exac momen maching (EMM) [Saha e al., 2009]. Through saisical analyses and experimens, Sachniss e al. [Sachniss e al., 2007] have shown ha Gaussian approximaion fails o esimae he map accuraely in cerain cases. Their work shows ha he abiliy o describe muli-modal proposal disribuions is necessary o preven such failures. In oher words, Gaussian approximaion of OPD in SLAM is doomed o failure whenever he OPD is highly non-gaussian and muli-modal, no maer how accurae he approximaion mehod is. Mone Carlo (MC) sampling mehods are capable of sampling from arbirary disribuions. Recenly, Blanco e al. [Blanco e al., 2010] proposed a mehod o sample from he OPD using rejecion sampling (RS), for he case of non-parameric observaion models and grid-based maps. The basic idea of heir mehod was presened in [Douce, 1998] and [Liu and Chen, 1998]. Their algorihm addressed he grid-based form of he SLAM problem and i yielded beer resuls compared o [Grisei e al., 2007] a grid-based RBPF-SLAM algorihm which employs Gaussian approximaion for he OPD. Neverheless, heir mehod has a sochasic ime-complexiy which can be considered as a major disadvanage. In his work we propose o approximae he OPD using local MC (LMC) sampling mehods. We propose LMC- 1 and LMC-2, wo novel approaches o he problem of feaure-based SLAM based on his idea. The proposed algorihms employ RS and imporance sampling (IS) in order o generae samples according o he OPD. Hence, our algorihms are perfecly capable of sampling from he non-gaussian and muli-modal OPD of SLAM. Unlike [Blanco e al., 2010], he proposed algorihms are focused on he feaure-based form of he SLAM problem. Moreover, hey do no suffer from he sochasic ime-complexiy. 2 Problem Formulaion In his secion we presen a formulaion of he feaurebased SLAM problem. The sae variable a ime N is represened by x R nx which consiss of robo pose s R ns and he map Θ. The map is represened by a se of saic landmarks {θ i ; i = 1,..., L} where θ i R n θ. The robo pose process is modelled as a Markov process wih ransiion equaion p(s s 1 ) (known as he moion model). The observaions z R nz are condiionally independen given he sae process wih marginal disribuion p(z x ) known as he observaion model. We denoe he se of any variable like c i up o ime by c 1: {c 1,..., c }. This model is associaed wih he following sae space equaions s = h(s 1, u ) + v (1) z = g(s, θ n ) + r (2) where h(, ) and g(, ) are nonlinear funcions describing he robo moion and measuremen model respecively, v and r are zero mean whie Gaussian noises wih covariances Q and R, u is he conrol inpu, and θ n is he observed feaure a ime. Hence we have p(s s 1, u ) = N (s ; h(s 1, u ), Q ) (3) p(z s, θ n ) = N (z ; g(s, θ n ), R ) (4) In his work, we consider he daa associaion parameer n as known. Also from now on, we omi he conrol inpu u for noaional convenience. Our goal is o esimae he robo pose and he map for any ime sep using he observaions and conrol inpus available up o ha ime. More precisely, we are ineresed in esimaing he filering disribuion p(x z 1: ) and he expecaion E[x z 1: ] = x p(x z 1: )dx (5) as he minimum mean square error (MMSE) esimae, sequenially in ime. 1 3 Sequenial Mone Carlo Mehods in SLAM RBPF soluions o he SLAM problem use he following facorizaion of he full SLAM poserior [Murphy, 2000] p(s 0:, Θ z 1: ) = p(s 0: z 1: ) L p(θ i s 0:, z 1: ) where p(θ i s 0:, z 1: ) can be compued analyically by a low dimensional (n θ n θ ) EKF. Therefore we can employ SMC mehods only o esimae p(s 0: z 1: ), which will reduce he variance of filer, along wih he compuaional coss. If we were able o sample N independen and idenically disribued (i.i.d.) samples {s [i] 0: ; i = 1,..., N} 1 In fac in paricle filering, he filering disribuion is obained indirecly by marginalizing he disribuion p(x 0: z 1:) which is known as he full SLAM poserior.

3 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. from he pah poserior, he poserior could be approximaed by ˆp (s 0: z 1: ) = 1 N N δ [i] s (s 0: ) (6) 0: in which δ [i] s (s 0: ) denoes he Dirac dela mass locaed 0: a s [i] 0:. Using (6), SLAM poserior can be obained as 1 N N L [ j=1 p(θ j s [i] 0:, z 1:)]δ [i] s (s 0: ) 0: which can be inerpreed as a se of N paricles describing he robo pah, each accompanied by L Gaussian disribuions represening he map poserior for ha paricle [Monemerlo e al., 2002]. The srong law of large numbers implies ha any expecaion compued wih respec o (6) converges almos surely o is rue value as N goes o infiniy. Neverheless, he pah poserior is only known up o a consan, and herefore, samples canno be generaed direcly from p(s 0: z 1: ). Insead, we can sample according o an easy o sample disribuion such as π(s 0: z 1: ), called imporance funcion, and assign weighs o hese in order o approximae (6). Sampling according o π(s 0: z 1: ) from scrach, implies a growing compuaional complexiy over ime. By choosing imporance funcions of he form generaed samples proporional o p(s0: z1:) π(s 0: z 1:) π(s 0: z 1: ) = π(s 0: 1 z 1: 1 )π(s s 0: 1, z 1: ) (7) one is able o reuse he previous generaed samples s [i] 0: 1 and consruc s [i] 0: as where s [i] s [i] 0: = s[i] 0: 1, s[i] is sampled from π(s s [i] 0: 1, z 1:). Using (7), imporance weighs can be compued up o a normalizing consan as 2 w [i] 0: = p(z s [i] w[i] )p(s[i] s[i] 1 ) 0: 1 π(s [i] s[i] 0: 1, z 1:) p(s[i] 0: z 1:) π(s [i] 0: z 1:) Finally, pah poserior can be approximaed by where ˆp(s 0: z 1: ) = w [i] N 0: = w [i] 0: w [i] 0: δ (s s [i] 0: ) 0: N j=1 w[j] 0: (8) 2 Wih a sligh abuse of noaion, we use w [i] 0: o emphasize ha his weigh is associaed wih he pah sample s [i] 0:. In his way, MMSE esimae (5) is obained by compuing he weighed mean of hese samples. This mehod is known as sequenial imporance sampling (SIS). I is proved ha he variance of imporance weighs grows wih ime [Kong e al., 1994] which leads o he so-called degeneracy problem in which afer a few seps, all bu one of he paricles will have very insignifican normalized weighs. This problem was addressed in boosrap filer [Gordon e al., 1993] by inroducing resampling seps a he end of each SIS ieraion, a process in which paricles wih low normalized weighs are eliminaed and hose wih high normalized weighs are copied. See [Douc and Cappé, 2005] for a review on differen resampling schemes. Effecive sample size (ESS) [Kong e al., 1994], which is defined as N eff = 1 N w[i]2 0: (9) is a measure of he degeneracy of SIS ha can be used o avoid unnecessary resampling seps. Resampling is performed only if N eff is below a hreshold N hr, ypically N hr = N/2. Selecing an appropriae proposal disribuion π(s s 0: 1, z 1: ) is of umos imporance o he success of SIS. Various proposal disribuions have been proposed and evaluaed for many applicaions [Douce e al., 2001]. FasSLAM 1.0 uses he simples and mos common choice, he ransiion densiy p(s s 1 ), which according o (3) is a known Gaussian. As a consequence, (8) will be simplified as w [i] 0: = w[i] 0: 1 p(z s [i] ) (10) in which p(z s [i] ) can be obained in closed form by linearizing g(, ) wih respec o he observed feaure. In FasSLAM 1.0, new samples are generaed wihou any knowledge of he mos recen observaion z, and herefore a grea number of paricles may be wased in he less imporan regions of sae-space. Consequenly, hose paricles will receive small weighs and become discarded in he resampling sage and only a few number of paricles would be duplicaed for many imes, which is obviously undesirable. This phenomenal has a more disasrous effec in a case when he observaion model being used is more accurae han he moion model (e.g. accurae laser range finders and noisy encoder). I is proved ha by choosing p(s s 1, z ) as he proposal disribuion, he variance of unnormalized imporance weighs condiional upon s [i] 0: 1 and z 0: is minimized [Douce, 1998]. This disribuion is called he opimal proposal disribuion (OPD) and can be used in order o limi he degeneracy of he filer. In his case, imporance weighs (8) can be obained as w [i] 0: = w[i] 0: 1 p(z s [i] 1 ) (11)

4 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. In general, neiher p(s s 1, z ) nor p(z s 1 ) can be compued in closed form. Neverheless, if he observaion equaion (2) was linear, hese disribuions would become available as Gaussian disribuions. The socalled local linearizaion mehod approximaes hese disribuions by linearizing g(, ) using he firs-order Taylor expansion [Douce, 1998]. FasSLAM 2.0 employs local linearizaion mehod o generae samples according o he OPD as follows. The OPD can be expanded using he Bayes rule as p(s s [i] 1, z ) p(z s )p(s s [i] 1 ) (12) The disribuion p(z s ) can be wrien in erms of he SLAM observaion model as p(z s, θ n )p(θ n s [i] 0: 1, z 0: 1)dθ n (13) in which p(θ n s [i] 0: 1, z 0: 1) represens he landmark esimae a ime 1 which is disribued according o N (θ n ; µ [i] n,, 1 Σ[i] n, 1 ). A firs-order Taylor expansion of g(, ) yields o [Monemerlo e al., 2003] g(s, θ n ) ẑ [i] where and g s ŝ [i] = g s + g θ n (θ n µ [i] n, 1 ) + g s (s ŝ [i] ) (14) = h(s [i] 1, u ), ẑ [i] = g(ŝ [i], µ [i] n ), 1 (ŝ [i], µ[i] θ n n, 1 ), g θn = g (ŝ [i], µ[i] n, 1 ) Using (12)-(14), FasSLAM 2.0 approximaes he OPD as N (s ; µ [i] s, Σ [i] s ) where Σ [i] s = [ gs T (R + g θn Σ [i] n, 1 gt θ n ) 1 g s + Q 1 µ [i] s = Σ [i] s gs T (R + g θn Σ [i] n, 1 gt θ n ) 1 (z ẑ [i] ) + ŝ [i] FasSLAM performs resampling a he end of each sep. Therefore (11) is reduced o p(z s [i] 1 ) which can be approximaed using he same linearizaion used above as ] 1 w [i] 0: N (z ; ẑ [i], g s Q g T s + g θn Σ [i] n, 1 gt θ n +R ) As i was menioned earlier, FasSLAM 2.0 suffers from wo major drawbacks regarding he approximaion of he OPD. The Gaussian approximaion is unable o describe cases were he rue OPD is muli-modal (see [Sachniss e al., 2007] for more deails). Moreover, he linearizaion error can lead o filer divergence and inconsisen esimaes, as repored in [Bailey e al., 2006]. In he following secions we presen wo alernaive mehods in order o address hese issues. 4 MC approximaion of he opimal proposal disribuion The basic idea of his work is o consruc he OPD using MC sampling mehods. Therefore, he OPD will be described by a se of random samples which is in conras o he exising mehods such as [Monemerlo e al., 2003], [Kim e al., 2008], [Van Der Merwe e al., 2001], [Grisei e al., 2007] and [Saha e al., 2009] in which an analyic approximaion (i.e. Gaussian) is provided for he OPD. The OPD in SLAM can be highly non- Gaussian and muli-modal [Sachniss e al., 2007]. As a consequence, Gaussian approximaion, even in perfec condiions, fails o describe he OPD adequaely. The abiliy o draw samples from he various modes of he OPD is crucial o he success of he paricle filer because evenually hese samples will be used in order o esimae he arge disribuion. Gaussian approximaion is, by definiion, incapable of describing he various modes of he OPD. In his work, he OPD p(s s [i] 1, z ), is considered as a local arge disribuion and is esimaed using M samples, iniially drawn from anoher disribuion such as q(s s [i] 1, z ) he so-called local proposal disribuion. An overview of he procedure will be as follows. Firs, emporary samples are iniially generaed according o his local proposal disribuion. Nex, his se of emporary samples will be ransformed by means of MC sampling mehods ino anoher se of samples disribued according o he OPD. A his sage, we would have a se of paricles which describes he OPD. Finally, in order o reach he main arge disribuion (i.e. robo pah poserior), imporance weighs will be assigned o hese samples according o SIS (11) and resampling will be performed if i is necessary. I is of grea imporance o noe ha unlike SIS, hese local MC mehods are employed on a fixed low dimensional space R ns and herefore are immune o such difficulies menioned for SIS. The mos imporan advanage of our mehod over FasSLAM 2.0, UFasSLAM and EMM is is abiliy o cope wih and sample from non-gaussian (e.g. mulimodal) OPD in nonlinear problems such as SLAM. Furhermore, i is clear ha by increasing M, he OPD is obained more accuraely. 3 Therefore, by conrolling he number of local samples, M, one can reach he desired accuracy based on he available compuaional resources a he momen which is of grea imporance in online applicaions. This is in conras o FasSLAM 2.0 where he linearizaion error can be neiher avoided nor conrolled. 3 Here we are inerpreing hese local MC seps as MC sampling raher han MC inegraion mehods. Neverheless, samples generaed in hese seps are furher being used o compue he desired expecaion (5).

5 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. In he following secions, we presen wo novel soluions o he feaure-based SLAM problem, namely LMC- 1 and LMC-2. LMC-1 employs rejecion sampling (RS) [Neumann, 1951], while LMC-2 uses imporance sampling in order o sample from he OPD. In boh of hese approaches, he process of updaing landmark esimaes is idenical o FasSLAM [Monemerlo e al., 2002], and herefore is no presened here. 4.1 LMC-1 Rejecion sampling (RS) can be used in order o obain samples from he OPD by acceping or rejecing samples generaed according o q(s s [i] 1, z ). Suppose here is a consan C i such ha s we have C i q(s s [i] 1, z ) p(s s [i] 1, z ), hen each RS sep may consis of [Neumann, 1951] Generae s [i,j] Generae u U[0, 1] Compue i,j : i,j = according o q(s s [i] 1, z ) p(s[i,j] s [i] 1, z ) C i q(s [i,j] s [i] 1, z ) ( 1) (15) Accep s [i,j] if u < i,j Therefore, he acceped samples are disribued according o p(s s [i] 1, z ). A proper choice for q(s s [i] 1, z ) is he SLAM moion model p(s s [i] 1 ). Therefore, according o (12) and (15) i,j can be obained as i,j = 1 C i p(z s [i,j] ) To mee he condiion depiced earlier, C i can be chosen as max p(z s [i,j] ). In pracice we use a modified version of RS, in which Ĉi = max p(z s [i,j] ) is being used insead j of C i [Caffo e al., 2002]. Consequenly, we need o evaluae p(z s [i,j] ), which according o (13) can be done using he firs-order Taylor expansion of g(, ) wih respec o he observed feaure as 4 N (z ; ẑ [i], g θ n Σ [i] n, 1 gt θ n + R ) (16) Hence an ieraion of he algorihm proceeds as follows: for each paricle s [i] 1 (i = 1,..., N), M samples are drawn from p(s s [i] 1 ). This can be done by sampling M i.i.d. samples from p(v ) and passing hem along wih s [i] 1 and u hrough he moion equaion (1). Then by 4 An alernaive approach would consis of generaing T i.i.d. samples according o p(θ n s [i] 0: 1, z0: 1) and hen approximaing p(z s [i,j] ) as 1 T T l=1 p(z s[i,j], θ n [l] ). applying he procedure described above, some of hese NM generaed samples are acceped. Thus we have ˆp(s s [i] 1, z ) = 1 K i K i j=1 δ [i,j] s (s ) where {s [i,j] ; j = 1,..., K i } represens he se of acceped samples corresponding o he i h paricle in ime 1. The arge disribuion is esimaed by assigning weighs o hese samples according o (11). By applying oal probabiliy heorem, (11) can be rewrien as w [i,j] 0: = w [i] 0: 1 p(z s )p(s s [i] 1 )ds Finally, using p(s s [i] 1 ) 1 M M j=1 δ (s s [i,j] ), imporance weighs can be obained as w [i,j] 0: w [i] 0: 1 [ 1 M M j=1 p(z s [i,j] )] (17) These weighs are normalized laer in order o obain w [i,j] 0:. Unlike FasSLAM, in order o avoid he error and depleion caused by unnecessary resampling seps, we only resample whenever N eff ges below a hreshold as i was discussed in secion LMC-2 Anoher MC sampling mehod ha can be used o generae samples from he OPD is imporance sampling (IS). In his way, samples drawn from he local imporance funcion q(s s [i] 1, z ) receive local weighs w (IS),[i,j] p(s s [i] 1,z) in order o approximae p(s s [i] q(s s [i] 1,z) 1, z ) as in which ˆp(s s [i] 1, z ) = M j=1 w (IS),[i,j] w (IS),[i,j] w (IS),[i,j] = M j=1 w(is),[i,j] δ [i,j] s (s ) (18) A sraighforward choice is o selec he SLAM moion model p(s s [i] 1 ) as he local imporance funcion q(s s [i] 1, z ) which implies w (IS),[i,j] = p(z s [i,j] ) Similar o LMC-1, p(z s [i,j] ) is evaluaed as (16). I can be easily shown ha local weighs w (IS),[i,j] mus be muliplied by he SIS weighs (11), in order o obain he pah poserior. We denoe by π op (s 0: z 1: ) and

6 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. π prior (s 0: z 1: ) respecively, he opimal and prior imporance funcions. Ieraing (7) yields π op (s 0: z 1: ) = p(s 0 ) π prior (s 0: z 1: ) = p(s 0 ) p(s i s i 1, z i ) p(s i s i 1 ) Finally, oal weigh is obained as p(s 0: z 0: ) π op (s 0: z 1: ) p(s i s i 1, z i ) p(s i s i 1 ) where according o (10) we have = p(s 0: z 0: ) π prior (s 0: z 1: ) 0: z 0:) w [i,j] 0: = w [i] 0: 1 p(z s [i,j] ) p(s[i,j] π prior (s [i,j] 0: z 1:) Alhough LMC-2 is derived based on he idea of approximaing he OPD, he resul can also be inerpreed as a modified FasSLAM 1.0 (i.e. prior proposal disribuion) in which, insead of sampling one paricle, M samples are generaed according o p(s s [i] 1 ). I is of umos imporance o noe ha in his case, he mos recen observaion is playing par hrough w (IS),[i,j]. In pracice, we use w (IS),[i,j] in order o filer samples s [i,j] wih insignifican local weighs. Moreover, we propagae only he samples wih highes local weighs whenever resampling is no performed. Therefore, for each i (i = 1,..., N) we have s [i] = arg max s [i,j] p(z s [i,j] ) whenever N eff is above he hreshold. 5 Resuls 5.1 Simulaion Resuls To evaluae he performance of he proposed SLAM algorihms and o sysemaically compare differen mehods of approximaing he OPD, we use exensive simulaions. We used he FasSLAM 2.0 implemenaion of Bailey e al. [Bailey e al., 2006] and implemened our algorihms (LMC-1 and LMC-2) for he same environmen. A Dual-Core 1.6 GHz PC wih 2 GB RAM was used o perform he simulaions. The vehicle moion and observaion models and simulaion parameers are similar o [Bailey e al., 2006] and are no brough here for breviy. The simulaed large scale environmen ( 200m 230m), he robo rajecory and he esimaed map are shown in Fig. 1. In each experimen, robo closes he loop once. In order o rule ou he effec of daa associaion errors Table 1: Average number of resampling seps over 50 MC simulaions N FasSLAM 2.0 LMC-1 LMC-2 (M = 50) (M = 3) meer meer Figure 1: Simulaed environmen. The sars denoe he landmark locaions, he line represens he robo rajecory and red dos indicae he landmark esimaes of paricles. on he performance of OPD approximaion, rue associaion is used in he simulaions. For each experimen, 50 MC runs were performed. Therefore, mean squared error (MSE) is compued according o b=1 (sb ŝ b ) 2 where s b and ŝ b, respecively, denoe he rue and he esimaed robo pose in ime of he b-h MC simulaion. The esimaed robo pose, E[s z 1: ], is obained by compuing he weighed average of paricles before resampling. I is of crucial imporance o noe ha resampling will inroduce addiional random variaion o he esimaed robo pose and herefore, i is necessary o compue he desired expeced value using he se of weighed paricles before resampling [Liu and Chen, 1998]. The original FasSLAM 2.0 performs resampling whenever an observaion is available. Here, an improved version of FasSLAM 2.0 is used in which resampling is performed based on he ESS (9). For all of he experimens, N hr = 3 4N is used. The main funcion of he OPD is o limi he degeneracy problem. I is crucial o noe ha in pracice, by approximaing he OPD we will only be able o reduce he rae of paricle degeneraion. Resampling addresses his issue parially. However, as i was menioned earlier, resampling iself will lead o he oher major issue in paricle filering, he so-called depleion problem in

7 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. Table 2: Average Runime over 50 MC simulaions LMC-1 LMC-2 N FasSLAM 2.0 (M = 50) (M = 3) (sec) (sec) (sec) Table 3: Average MSE over he pah LMC-1 LMC-2 N FasSLAM 2.0 (M = 50) (M = 3) (m 2 ) (m 2 ) (m 2 ) which all he paricles s [i] 0: a ime sep share a common hisory of robo poses s [i] 0: k and map for some k. Mainaining he diversiy of paricles in RBPF-SLAM is crucial o he efficien loop-closure performance. ESS measures he degeneracy in each ime sep and can be employed in order o deermine if resampling is needed. Therefore, by using ESS, number of resamplings reflecs boh he rae of paricle degeneraion and depleion. For his reason, i is common o monior and repor he value of N eff hroughou he simulaion. Neverheless, due o he grea deal of variaion of N eff among differen MC rials for fixed ime seps, we chose o repor direcly he average number of resampling seps among 50 MC runs. Table 1 clearly shows ha LMC-1 and LMC-2 ouperform FasSLAM 2.0 in he number resampling seps performed in each experimen. LMC-1 is slighly beer han LMC-2 in he number of resampling seps. Smaller number of resampling seps suggess slower rae of degeneracy and consequenly, closer approximaion of he OPD. As i was discussed above, his decrease in he number of resampling seps reduces he rae of loss of paricle diversiy and depleion. Reducing he rae of diversiy loss significanly affecs he SLAM algorihm performance as i is suggesed by [Bailey e al., 2006]. The average runime of LMC-1, LMC-2 and Fas- SLAM 2.0 in he aforemenioned scenario is repored in Table 2. FasSLAM 2.0 is slighly faser han LMC-2 (M = 3) while LMC-1 (M = 50) requires approximaely 10 imes more compuaional ime o process a loop. I is imporan o noe ha LMC-1 uses local samples o approximae he inegral in (17) in order o calculae he opimal weighs. This MC inegraion requires a fairly large number of local samples, M, in order o obain accurae resuls. On he conrary, in LMC-2, opimal weighs are eliminaed hus good esimaes can be obained using much smaller M. The performance of hese algorihms are also compared by varying he number of paricles and compuing he average MSE over ime. The resul is averaged over 50 MC rials and is repored in Table 3. According o Table 3, LMC-2 (M = 3) exhibis he lowes MSE for small number of paricles (N = 10), followed closely by LMC- 1 (M = 50). On he conrary, FasSLAM 2.0 shows quie a large MSE compared o LMC-2 and LMC-1 for 10 paricles. The accuracy of FasSLAM 2.0 and LMC-2 (M = 3) becomes more similar as he number of paricles increases. This behavior should no come as a surprise, because i is well-known ha for large enough number of paricles he difference beween sampling from differen proposal disribuions diminishes. Neverheless, LMC-1 (M = 50) yields even more accurae esimaes as he number of paricles increases. Therefore, LMC-1 (M = 50) ouperforms he oher wo algorihms in erms of accuracy for large number of paricles, while LMC-2 (M = 3) has shown he lowes MSE for small number of paricles. Finally, he MSE of esimaed robo posiion hroughou he loop for differen algorihms wih fixed number of paricles (N = 80) is shown in Fig. 2. FasSLAM 2.0 and LMC-1 (M = 20) have failed o obain good esimaes. The poor performance of LMC-1 (M = 20) suggess ha M = 20 is no enough o approximae he inegral in opimal weighs (17). Moreover, he fac ha LMC-2 is able o achieve much more accurae esimaes wih he same and lower (M = 3) number of local samples suppors his claim. According o Fig. 2, LMC-1 (M = 50) has he lowes MSE a he expense of is huge compuaional ime. Noe ha FasSLAM 2.0 has shown he larges MSE a he end of pah. This behavior is consisen wih he FasSLAM 2.0 s larger number of resamplings which adversely affecs is loop-closing performance. 5.2 Vicoria Park Daase Resuls Due o he sochasic naure of SMC mehods, MC simulaions are necessary for evaluaion of he proposed algorihms. However, in order o evaluae he performance of he proposed algorihms in real-world siuaions, hey were also esed on he well-known Vicoria Park daase from he Universiy of Sydney. The daase was gahered by using a vehicle equipped wih a SICK laser range finder (LRF), driven in a large-scale environmen ( 250m 250m). The lengh of he raversed pah is approximaely 3.5 km. The rees in he park were exraced from he LRF scans and are used as he poin feaures in feaure-based SLAM algorihms. Neverhe-

8 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand meer meer (a) LMC-1 (M = 30) meer (b) LMC-2 (M = 3) meer (c) LMC-1 (M = 30) Figure 3: The esimaed rajecory (red) and GPS daa (blue) for 20 paricles. The zoomed version of he boxed region in Fig. 3a for each algorihm is shown in Fig. 3b and 3c. MSE (m 2 ) FasSLAM2.0 LMC 1 (M=50) LMC 1 (M=20) LMC 2 (M=20) LMC 2 (M=3) Table 4: Number of Resamplings in Vicoria Park Daase LMC-1 LMC-2 N FasSLAM 2.0 (M = 30) (M = 3) ime (sec) Figure 2: Average MSE of esimaed robo posiion over 50 Mone Carlo simulaions. less, he daase is noorious for spurious observaions which makes he daa associaion difficul. GPS daa is available parially and is used as he ground ruh, bu here is no ground ruh available for he landmarks posiions. Similar o he simulaions, daa associaion is assumed known here. We used he se of associaions provided by I-SLSJF algorihm [Huang and Wang, 2008]. Since here is no ground ruh available for he landmarks posiions, only he esimaed robo rajecories are ploed here. Number of resamplings for each algorihm wih 20 paricles is shown in Table 4. According o Table 4, LMC-1 (M = 30) has he lowes number of resampling seps, followed by LMC-2 (M = 3). Compared o FasSLAM 2.0, LMC-1 and LMC-2 have reduced he number of resamplings by 27% and 20% respecively. These resuls are consisen wih hose obained from he simulaions. The accuracy of LMC-1 (M = 30), LMC-2 (M = 3) and FasSLAM 2.0 wih 20 paricles were quie similar in he firs par of he rajecory. The esimaed rajecory for LMC-1 is shown in Fig. 3a. The zoomed version of he boxed region in Fig. 3a for each algorihm is depiced in Fig. 3b and 3c. According o Fig. 3 LMC-1 (M = 30) has shown he bes performance, followed by LMC-2 (M = 3). Such resuls were expeced according o Table 4 and simulaions resuls. The error can be reduced furher by increasing M and/or N. 6 Conclusion In his paper we reviewed differen approaches o approximae he OPD in he feaure-based RBPF-SLAM. Furhermore, wo novel feaure-based SLAM algorihms based on MC approximaion of he OPD have been proposed. The proposed algorihms employ MC sampling mehods o draw samples from he non-gaussian OPD of SLAM by considering i as a local arge. Boh of hese algorihms benefi from he capabiliy of MC sampling mehods o represen and sample from non-gaussian and muli-modal disribuions, and consequenly ouperform Gaussian approximaion approaches such as FasSLAM 2.0. To he bes of our knowledge, he proposed algorihms are he firs soluions of he feaure-based SLAM wih such a capabiliy. Finally, he proposed algorihms were evaluaed using boh simulaions and real-world daa in large scale environmens. Resuls have shown improvemens compared o FasSLAM 2.0. The accuracy of LMC-1 comes a he expense of huge compuaional ime which makes i ideal for offline applicaions. On he oher hand, while LMC-2 is as fas as FasSLAM 2.0, i yields much more accurae esimaes han Fas-

9 Proceedings of Ausralasian Conference on Roboics and Auomaion, 3-5 Dec 2012, Vicoria Universiy of Wellingon, New Zealand. SLAM (especially for small number of paricles). References [Bailey e al., 2006] T. Bailey, J. Nieo, and E. Nebo. Consisency of he fasslam algorihm. In Proc. IEEE In. Conf. Roboics and Auomaion ICRA 2006, pages , [Blanco e al., 2010] J. Blanco, J. Gonzalez, and J.A. Fernandez-Madrigal. Opimal Filering for Nonparameric Observaion Models: Applicaions o Localizaion and SLAM. The Inernaional Journal of Roboics Research, [Caffo e al., 2002] B.S.Caffo, J.G. Booh, and AC Davison. Empirical supremum rejecion sampling. Biomerika, 89(4):745, [Douc and Cappé, 2005] R. Douc and O. Cappé. Comparison of resampling schemes for paricle filering. In Image and Signal Processing and Analysis, ISPA Proceedings of he 4h Inernaional Symposium on, pages IEEE, [Douce e al., 2001] A. Douce, N. De Freias, and N. Gordon. Sequenial Mone Carlo mehods in pracice. Springer Verlag, [Douce, 1998] Arnaud Douce. On sequenial simulaion-based mehods for bayesian filering. Technical Repor CUED/F-INFENG/TR. 310, Cambridge Universiy Deparmen of Engineering, [Gordon e al., 1993] N.J. Gordon, D.J. Salmond, and A.F.M. Smih. Novel approach o nonlinear/non- Gaussian Bayesian sae esimaion. In IEE Proceedings, volume 140, pages , [Grisei e al., 2007] G. Grisei, C. Sachniss, and W. Burgard. Improved echniques for grid mapping wih rao-blackwellized paricle filers. Roboics, IEEE Transacions on, 23(1):34 46, [Huang and Wang, 2008] S. Huang and Z Wang. I-slsjf implemenaion, hp:// [Kim e al., 2008] C. Kim, R. Sakhivel, and W.K. Chung. Unscened fasslam: A robus and efficien soluion o he slam problem. Roboics, IEEE Transacions on, 24(4): , [Kong e al., 1994] A. Kong, J.S. Liu, and W.H. Wong. Sequenial impuaions and Bayesian missing daa problems. Journal of he American Saisical Associaion, 89(425): , [Liu and Chen, 1998] Jun S. Liu and Rong Chen. Sequenial mone carlo mehods for dynamic sysems. Journal of he American Saisical Associaion, 93: , [Monemerlo e al., 2002] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei. FasSLAM: A facored soluion o he simulaneous localizaion and mapping problem. In Proceedings of he Naional conference on Arificial Inelligence, pages Menlo Park, CA; Cambridge, MA; London; AAAI Press; MIT Press; 1999, [Monemerlo e al., 2003] 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 Proceedings of he Sixeenh Inernaional Join Conference on Arificial Inelligence (IJCAI), Acapulco, Mexico, IJCAI. [Murphy, 2000] K. Murphy. Bayesian map learning in dynamic environmens. Advances in Neural Informaion Processing Sysems, 12: , [Neumann, 1951] J. Neumann. Various echniques used in connecion wih random digis. Mone Carlo mehods. Naional Bureau of Sandards AMS, 12, [Saha e al., 2009] S. Saha, PK Mandal, Y. Boers, H. Driessen, and A. Bagchi. Gaussian proposal densiy using momen maching in smc mehods. Saisics and Compuing, 19(2): , [Sachniss e al., 2007] C.Sachniss, G.Grisei, W. Burgard, and N. Roy. Analyzing gaussian proposal disribuions for mapping wih rao-blackwellized paricle filers. In Inelligen Robos and Sysems, IROS IEEE/RSJ Inernaional Conference on, pages IEEE, [Van Der Merwe e al., 2001] R.VanDer Merwe, A. Douce, N. De Freias, and E. Wan. The unscened paricle filer. Advances in Neural Informaion Processing Sysems, pages , 2001.