Observability-based Rules for Designing Consistent EKF SLAM Estimators

Transcription

1 1 Observability-based Rules fr Designing Cnsistent EKF SLAM Estimatrs Guquan P Huang, Anastasis I Murikis, and Stergis I Rumelitis Dept f Cmputer Science and Engineering, University f Minnesta, Minneaplis, MN {ghuang stergis}@csumnedu Dept f Electrical Engineering, University f Califrnia, Riverside, CA murikis@eeucredu Abstract In this wrk, we study the incnsistency prblem f EKF-based SLAM frm the perspective f bservability We analytically prve that when the Jacbians f the prcess and measurement mdels are evaluated at the latest state estimates during every time step, the linearized errr-state system emplyed in the EKF has bservable subspace f dimensin higher than that f the actual, nnlinear, SLAM system As a result, the cvariance estimates f the EKF underg reductin in directins f the state space where n infrmatin is available, which is a primary cause f the incnsistency Based n these theretical results, we prpse a general framewrk fr imprving the cnsistency f EKF-based SLAM In this framewrk, the EKF linearizatin pints are selected in a way that ensures that the resulting linearized system mdel has an bservable subspace f apprpriate dimensin We describe tw algrithms that are instances f this paradigm In the first, termed Observability Cnstrained EKF (OC-EKF), the linearizatin pints are selected s as t minimize their expected errrs (ie, the difference between the linearizatin pint and the true state) under the bservability cnstraints In the secnd, the filter Jacbians are calculated using the first-ever available estimates fr all state variables This latter apprach is termed First-Estimates Jacbian (FEJ)-EKF The prpsed algrithms have been tested bth in simulatin and experimentally, and are shwn t significantly utperfrm the standard EKF bth in terms f accuracy and cnsistency I INTRODUCTION Simultaneus lcalizatin and mapping (SLAM) is the prcess f building a map f an envirnment and cncurrently generating an estimate f the rbt pse (psitin and rientatin) frm the sensr readings Fr autnmus vehicles explring unknwn envirnments, the ability t perfrm SLAM is essential Since Smith and Cheeseman, 1987 first intrduced a stchastic-mapping slutin t the SLAM prblem, rapid and exciting prgress has been made, resulting in several cmpeting slutins Recent interest in SLAM has fcused n the design f estimatin algrithms Mntemerl, 23, Paskin, 22, data assciatin techniques Neira and Tards, 21, and sensr data prcessing Se et al, 22 Amng the numerus algrithms develped thus far fr the SLAM prblem, the extended Kalman filter (EKF) remains ne f the mst ppular nes, and has been used in several applicatins (eg, Newman, 1999, Williams et al, 2, Kim and Sukkarieh, 23) Hwever, in spite f its widespread adptin, the fundamental issue f the cnsistency f the EKF- SLAM algrithm has nt yet been sufficiently investigated As defined in Bar-Shalm et al, 21, a state estimatr is cnsistent if the estimatin errrs are zer-mean and have cvariance matrix smaller r equal t the ne calculated by the filter Cnsistency is ne f the primary criteria fr evaluating the perfrmance f any estimatr; if an estimatr is incnsistent, then the accuracy f the prduced state estimates is unknwn, which in turn makes the estimatr unreliable Since SLAM is a nnlinear estimatin prblem, n prvably cnsistent estimatr can be cnstructed fr it The cnsistency f every estimatr has t be evaluated experimentally In particular fr the standard EKF-SLAM algrithm, there exists significant empirical evidence shwing that the cmputed state estimates tend t be incnsistent (cf Sectin II) In this paper, we investigate in depth ne fundamental cause f the incnsistency f the standard EKF-SLAM algrithm In particular, we revisit this prblem frm a new perspective, ie, by analyzing the bservability prperties f the filter s system mdel Our key cnjecture in this paper is that the bservability prperties f the EKF linearized system mdel prfundly affect the perfrmance f the filter, and are a significant factr in determining its cnsistency Specifically, the majr cntributins f this wrk are the fllwing: Thrugh an bservability analysis, we prve that the standard EKF-SLAM emplys an errr-state system mdel that has an unbservable subspace f dimensin tw, even thugh the underlying nnlinear system mdel has three unbservable degrees f freedm (crrespnding t the psitin and rientatin f the glbal reference frame) As a result, the filter gains spurius infrmatin alng directins f the state space where n infrmatin is actually available This leads t an unjustified reductin f the cvariance estimates, and is a primary cause f filter incnsistency Mtivated by this analysis, we prpse a new methdlgy fr imprving the cnsistency f EKF-based SLAM Specifically, we prpse selecting the linearizatin pints f the EKF in a way that ensures that the unbservable subspace f the EKF system mdel is f apprpriate dimensin In ur previus wrk Huang et al, 28a, Huang et al, 28b, we prved that this can be achieved by cmputing the EKF Jacbians using the first-ever available estimates fr each f the state variables The resulting algrithm is termed First Estimates Jacbian

2 2 (FEJ)-EKF In this wrk, we prpse an alternative apprach, named Observability Cnstrained (OC)-EKF, which falls under the same general framewrk In this nvel filter, the EKF linearizatin pints are selected s as nt nly t guarantee the desired bservability prperties but als t minimize the expected errrs f the linearizatin pints (ie, the difference between the linearizatin pint and the true state) This can be frmulated as a cnstrained minimizatin prblem, whse slutin renders the linearizatin pints used fr cmputing the filter Jacbians Thrugh extensive simulatins and real-wrld experiments, we verify that bth the FEJ-EKF and the OC- EKF utperfrm the standard EKF, even thugh they use less accurate linearizatin pints in cmputing the filter Jacbians (since the linearizatin pints used in the FEJ-EKF and OC-EKF are, in general, different frm the latest, and thus best, state estimates) This result supprts ur cnjecture that the bservability prperties f the EKF system mdel play a fundamental rle in determining cnsistency The remainder f the paper is rganized as fllws After an verview f related wrk in the next sectin, the standard EKF-SLAM frmulatin with generalized system and measurement mdels is described in Sectin III In Sectin IV, the bservability analysis f SLAM is presented and is emplyed t prve that the standard EKF-SLAM always has incrrect bservability prperties Sectin V describes the prpsed appraches fr imprving the cnsistency f EKF-SLAM, and in Sectins VI and VII the perfrmance f the FEJ-EKF and OC-EKF is demnstrated thrugh Mnte-Carl simulatins and experiments Finally, Sectin VIII utlines the main cnclusins f this wrk II RELATED WORK The incnsistency prblem f the standard EKF-SLAM algrithm has recently attracted cnsiderable interest Castellans et al, 24, Castellans et al, 27, Julier and Uhlmann, 21, Bailey et al, 26, Huang and Dissanayake, 26, Huang and Dissanayake, 27, Huang et al, 28a, Huang et al, 28b The first wrk t draw attentin t this issue was that f Julier and Uhlmann, 21, wh bserved that when a statinary rbt measures the relative psitin f a new landmark multiple times, the estimated variance f the rbt s rientatin becmes smaller Since the bservatin f a previusly unseen feature des nt prvide any infrmatin abut the rbt s state, this reductin is artificial, and leads t incnsistency Additinally, a cnditin that the filter Jacbians need t satisfy in rder t permit cnsistent estimatin, was described We shw that this cnditin, derived in Julier and Uhlmann, 21 fr the case f a statinary rbt, is a special case f an bservabilitybased cnditin derived in ur wrk fr the general case f a mving rbt (cf Lemma 51) Mre recently, the wrk f Huang and Dissanayake, 27 extended the analysis f Julier and Uhlmann, 21 t the case f a rbt that bserves a landmark frm tw psitins (ie, the rbt bserves a landmark, mves and then re-bserves the landmark) A cnstraint that the filter Jacbians need t fulfill in this case s as t allw fr cnsistent estimatin, was prpsed In Huang and Dissanayake, 27, it was shwn that this cnditin is generally vilated, due t the fact that the filter Jacbians at different time instants are evaluated using different estimates fr the same state variables Interestingly, in Sectin V-C it is shwn that this cnditin can als be derived as a special case f ur generalized analysis Bailey et al, 26 examined several symptms f the incnsistency f the standard EKF-SLAM algrithm, and argued, based n Mnte Carl simulatin results, that the uncertainty in the rbt rientatin is the main cause f the incnsistency f EKF-SLAM Hwever, n theretical results were prvided The wrk f Huang and Dissanayake, 26 further cnfirmed the empirical findings in Bailey et al, 26, and argued by example that in EKF-SLAM the incnsistency is always in the frm f vercnfident estimates (ie, the cmputed cvariances are smaller than the actual nes) The afrementined wrks have described several symptms f incnsistency that appear in the standard EKF-SLAM, and have analytically studied nly a few special cases, such as that f a statinary rbt Julier and Uhlmann, 21, and that f ne-step mtin Huang and Dissanayake, 27 Hwever, n theretical analysis int the cause f incnsistency fr the general case f a mving rbt was cnducted T the best f ur knwledge, the first such analysis appeared in ur previus publicatins Huang et al, 28a, Huang et al, 28b Therein, the mismatch in the dimensins f the bservable subspaces between the standard EKF and the underlying nnlinear SLAM system was identified as a fundamental cause f incnsistency, and the FEJ-EKF was prpsed as a means f imprving the cnsistency f the estimates In this paper, we present the theretical analysis f Huang et al, 28a, Huang et al, 28b in mre detail, and prpse a general framewrk fr imprving the cnsistency f EKF- SLAM It is shwn that the FEJ-EKF is ne f several pssible estimatrs, which rely n the bservability analysis fr the selectin f EKF linearizatin pints Mrever, we prpse an alternative EKF estimatr, the OC-EKF, whse perfrmance is an imprvement ver the FEJ-EKF The OC-EKF selects the ptimal linearizatin pints in a way that minimizes the the linearizatin errrs, while ensuring that the bservable subspace f the EKF linearized system mdel has crrect dimensins The fllwing sectins describe the theretical develpment f the algrithms in detail III STANDARD EKF-SLAM FORMULATION In this sectin, we present the equatins f the standard EKF-SLAM frmulatin with generalized system and measurement mdels T preserve the clarity f the presentatin, we first fcus n the case where a single landmark is included in the state vectr, while the case f multiple landmarks is addressed later n In the standard frmulatin f SLAM, the state vectr cmprises the rbt pse and the landmark psitin in the glbal frame f reference Thus, at time-step k

3 3 the state vectr is given by: x k = p T φ Rk p T L T = x T Rk p T L T (1) where x Rk = p T φ Rk T dentes the rbt pse, and p L is the landmark psitin EKF-SLAM recursively evlves in tw steps: prpagatin and update, based n the discrete-time prcess and measurement mdels, respectively A EKF Prpagatin In the prpagatin step, the rbt s dmetry measurements are prcessed t btain an estimate f the pse change between tw cnsecutive time steps, and then emplyed in the EKF t prpagate the rbt state estimate On the ther hand, since the landmark is static, its state estimate des nt change with the incrpratin f a new dmetry measurement The EKF prpagatin equatins are given by: 1 ˆp Rk+1 k = ˆp Rk k + C( ˆφ Rk k ) Rk ˆp Rk+1 (2) ˆφ Rk+1 k = ˆφ Rk k + ˆφRk+1 (3) ˆp Lk+1 k = ˆp Lk k (4) where C( ) dentes the 2 2 rtatin matrix, and ˆxRk+1 = ˆp T Rk+1 ˆφRk+1 T is the dmetry-based estimate f the rbt s mtin between time-steps k and k + 1 This estimate is crrupted by zer-mean, white Gaussian nise w k = x Rk+1 ˆxRk+1, with cvariance matrix Q k This prcess mdel is nnlinear, and can be described by the fllwing generic nnlinear functin: x k+1 = f(x k, Rkˆx Rk+1 + w k ) (5) In additin t the state prpagatin equatins, the linearized errr-state prpagatin equatin is necessary fr the EKF This is given by: ΦRk x k+1 k = 3 2 xrk k GRk + w 2 3 p Lk k k 2 2 Φ k x k k + G k w k (6) where Φ Rk and G Rk are btained frm the state prpagatin equatins (2)-(3): I2 JC( Φ Rk = ˆφ ) Rk k ˆpRk+1 (7) I2 J (ˆp ˆp Rk+1 k k) (8) C( ˆφRk k G Rk = ) 2 1 (9) with J 1 It is imprtant t pint ut that the frm f the prpagatin equatins presented abve is general, and hlds fr any rbt 1 Thrughut this paper the subscript l j refers t the estimate f a quantity at time-step l, after all measurements up t time-step j have been prcessed ˆx is used t dente the estimate f a randm variable x, while x = x ˆx is the errr in this estimate m n and 1 m n dente m n matrices f zers and nes, respectively, while I n is the n n identity matrix Finally, we use the cncatenated frms sφ and cφ t dente the sin φ and cs φ functins, respectively kinematic mdel (eg, unicycle, bicycle, r Ackerman mdel) In Appendix A, we derive the expressins fr (2)-(4), as well as the state and nise Jacbians, fr the cmmn case where the unicycle mdel is used B EKF Update During SLAM, the measurement used fr updates in the EKF is a functin f the relative psitin f the landmark with respect t the rbt: z k = h(x k ) + v k = h ( p L ) + vk (1) where p L = C T (φ Rk )(p L p Rk ) is the psitin f the landmark with respect t the rbt at time-step k, and v k is zer-mean Gaussian measurement nise with cvariance In this wrk, we allw h t be any measurement functin Fr instance, z k can be a direct measurement f relative psitin, a pair f range and bearing measurements, bearing-nly measurements frm mncular cameras, etc Generally, the measurement functin is nnlinear, and hence it is linearized fr use in the EKF The linearized measurement errr equatin is given by: z k H Rk H Lk x Rk k 1 p Lk k 1 + v k H k x k k 1 + v k (11) where H Rk and H Lk are the Jacbians f h with respect t the rbt pse and the landmark psitin, respectively, evaluated at the state estimate ˆx k k 1 Using the chain rule f differentiatin, these are cmputed as: H Rk = ( h k )C T ( ˆφ Rk k 1 ) J(ˆp Lk k 1 ˆp Rk k 1 ) (12) H Lk = ( h k )C T ( ˆφ Rk k 1 ) (13) where h k dentes the Jacbian f h with respect t the rbt-relative landmark psitin (ie, with respect t the vectr p L ), evaluated at the state estimate ˆx k k 1 IV SLAM OBSERVABILITY ANALYSIS In this sectin, we perfrm an bservability analysis fr the generalized EKF-SLAM frmulatin derived in the previus sectin, and cmpare its prperties with thse f the underlying nnlinear system Based n this analysis, we draw cnclusins abut the cnsistency f the filter We nte that, t keep the presentatin clear, sme intermediate steps f the derivatins have been mitted The interested reader is referred t Huang et al, 28c fr details It shuld be pinted ut that the bservability prperties f SLAM have been studied in nly a few cases in the literature In particular, Andrade-Cett and Sanfeliu, 24, Andrade- Cett and Sanfeliu, 25 investigated the bservability f a simple linear time-invariant (LTI) SLAM system, and shwed that it is unbservable The wrk f Vidal-Calleja et al, 27 apprximated the SLAM system by a piecewise cnstant linear (PWCL) ne, and applied the technique f Gshen- Meskin and Bar-Itzhack, 1992 t study the bservability prperties f bearing-nly SLAM On the ther hand, in Lee

4 4 et al, 26, Huang et al, 28a the bservability prperties f the nnlinear SLAM system were studied using the nnlinear bservability rank cnditin intrduced by Hermann and Krener, 1977 These wrks prved that the nnlinear SLAM system is unbservable, with three unbservable degrees f freedm All the afrementined appraches examine the bservability prperties f the nnlinear SLAM system, r f linear apprximatins t it Hwever, t the best f ur knwledge, an analysis f the bservability prperties f the EKF linearized errr-state system mdel had nt been carried ut prir t ur wrk Huang et al, 28a, Huang et al, 28b Since this mdel is the ne used in any actual EKF implementatin, a lack f understanding f its bservability prperties appears t be a significant limitatin In fact, as shwn in this paper, these prperties play a significant rle in determining the cnsistency f the filter, and frm the basis f ur apprach fr imprving the perfrmance f the estimatr A Nnlinear Observability Analysis fr SLAM We start by carrying ut the bservability analysis fr the cntinuus-time nnlinear SLAM system This analysis is based n the bservability rank cnditin intrduced in Hermann and Krener, 1977 Specifically, Therem 311 therein states that if a nnlinear system is lcally weakly bservable, the bservability rank cnditin is satisfied generically We here shw that the SLAM system des nt satisfy the bservability rank cnditin, and thus it is nt lcally weakly bservable nr lcally bservable In particular, we cnduct the analysis fr a general measurement mdel, instead f nly relative-psitin r distance-bearing measurements as in Huang et al, 28a, Lee et al, 26 Fr the cntinuus-time analysis, we emply a unicycle kinematic mdel, althugh similar cnclusins can be drawn if different mdels are used Lee et al, 26 The prcess mdel in cntinuus-time frm is given by: ẋ R (t) cφ R (t) ẏ R (t) φ R (t) = sφ R (t) v(t) + 1 ω(t) ẋ L (t) ẏ L (t) ẋ(t) = f 1 v(t) + f 2 ω(t) (14) where u v ω T is the cntrl input, cnsisting f linear and rtatinal velcity Since any type f measurement during SLAM is a functin f the relative psitin f the landmark with respect t the rbt, we can write the measurement mdel in the fllwing generic frm: z(t) = h(ρ, ψ) (15) ρ = p L p R (16) ψ = atan2(y L y R, x L x R ) φ R (17) where ρ and ψ are the rbt-t-landmark relative distance and bearing angle, respectively Nte that parameterizing the measurement with respect t ρ and ψ is equivalent t parameterizing it with respect t the relative landmark psitin expressed in the rbt frame, R p L The relatin between these cψ quantities is R p L = ρ The analysis will be based n the sψ fllwing lemma: Lemma 41: All the Lie derivatives f the nnlinear SLAM system (cf (14) and (15)) are functins f ρ and ψ nly Prf: See Appendix B We will nw emply this result fr the nnlinear bservability analysis In particular, assume that a number f different measurements are available, z i = h i (ρ, ψ), i = 1, 2,, n Then, since all the Lie derivatives fr all measurements are functins f ρ and ψ nly, we can prve the fllwing: Lemma 42: The space spanned by all the k-th rder Lie derivatives L k f j h i ( k N, j = 1, 2, i = 1, 2,, n) is dented by G, and the space dg spanned by the gradients f the elements f G is given by: sφr cφ dg = span R cφ R δx sφ R δy sφ R cφ R rw cφ R sφ R sφ R δx cφ R δy cφ R sφ R where δx x L x R and δy y L y R Prf: See Appendix C The matrix shwn abve is the bservability matrix fr the nnlinear SLAM system under cnsideratin Clearly, this is nt a full-rank matrix, and the system is unbservable Intuitively, this is a cnsequence f the fact that we cannt gain abslute, but rather nly relative state infrmatin frm the available measurements Even thugh the ntin f an unbservable subspace cannt be strictly defined fr this system, the physical interpretatin f the basis f dg will give us useful infrmatin fr ur analysis in Sectin IV-B By inspectin, we see that ne pssible basis fr the space dg is given by: dg = span cl 1 y R 1 x R 1 1 y L span n 1 n 2 n 3 1 x L (18) Frm the structure f the vectrs n 1 and n 2 we see that a change in the state by x = αn 1 + βn 2, α, β R crrespnds t a shifting f the x y plane by α units alng x, and by β units alng y Thus, if the rbt and landmark psitins are shifted equally, the states x and x + x will be indistinguishable given the measurements T understand the physical meaning f n 3, we cnsider the case where the x y plane is rtated by a small angle δφ Rtating the crdinate system transfrms any pint p = x y T t a pint p = x y T, given by: x x y = C(δφ) y 1 δφ x x = + δφ δφ 1 y y y x where we have emplyed the small angle apprximatins c(δφ) 1 and s(δφ) δφ Using this result, we see that if the plane cntaining the rbt and landmarks is rtated by

5 5 δφ, the SLAM state vectr will change t: x = x R y R φ R x L y L x R y R φ R x L y L y R + δφ x R 1 y L = x + δφn 3 (19) which indicates that the vectr n 3 crrespnds t a rtatin f the x y plane Since n 3 dg, this result shws that any such rtatin is unbservable, and will cause n change t the measurements The preceding analysis fr the meaning f the basis vectrs f dg agrees with intuitin, which dictates that the glbal crdinates f the state vectr in SLAM (rtatin and translatin) are unbservable x L B EKF-SLAM Observability Analysis In the previus sectin, it was shwn that the underlying physical system in SLAM has three unbservable degrees f freedm Thus, when the EKF is used fr state estimatin in SLAM, we wuld expect that the system mdel emplyed by the EKF als shares this prperty Hwever, in this sectin we shw that this is nt the case, since the unbservable subspace f the linearized errr-state mdel f the standard EKF is generally f dimensin nly 2 First recall that in general the Jacbian matrices Φ k, G k, and H k used in the EKF-SLAM linearized errr-state mdel (cf (6) and (11)), are defined as: Φ k = xk f G k = wk f H k = xk h {x {x k k,x k+1 k,} (2) k k,} (21) x k k 1 (22) In these expressins, x l l 1 and x l l (l = k, k + 1) dente the linearizatin pints fr the state x l, used fr evaluating the Jacbians befre and after the EKF update at time-step l, respectively A linearizatin pint equal t the zer vectr is chsen fr the nise The EKF emplys the abve linearized system mdel fr prpagating and updating the estimates f the state vectr and cvariance matrix, and thus the bservability prperties f this mdel affect the perfrmance f the estimatr T the best f ur knwledge, a study f these prperties has nt been carried ut in the past, and is ne f the main cntributins f this wrk Since the linearized errr-state mdel fr EKF-SLAM is time-varying, we emply the lcal bservability matrix Chen et al, 199 t perfrm the bservability analysis Specifically, the lcal bservability matrix fr the time interval between time-steps k and k + m is defined as: H k H k +1Φ k M H k+mφ k+m 1 Φ k H Rk H Lk H Lk+1 (23) H Rk+1 Φ Rk = (24) H Φ Rk +m R Φ k +m 1 H Lk +m = M(x k k 1, x k k,, x k +m k +m 1, x k +m k +m ) (25) where (24) is btained by substituting the matrices Φ k and H k (cf (6) and (11), respectively) int (23) The last expressin, (25), makes explicit the fact that the bservability matrix is a functin f the linearizatin pints used in cmputing all the Jacbians within the time interval k, k + m In turn, this implies that the chice f linearizatin pints affects the bservability prperties f the linearized errr-state system f the EKF This key fact is the basis f ur analysis In the fllwing, we discuss different pssible chices fr linearizatin, and the bservability prperties f the crrespnding linearized systems 1) Ideal EKF-SLAM: Befre cnsidering the rank f the matrix M, which is cnstructed using the estimated values f the state in the filter Jacbians, it is interesting t study the bservability prperties f the racle, r ideal EKF (ie, the filter whse Jacbians are evaluated using the true values f the state variables, in ther wrds, x k k 1 = x k k = x k, fr all k) In the fllwing, all matrices evaluated using the true state values are dented by the symbl We start by nting that (cf (8)): I2 J ( p Φ Rk+1 ΦRk = p ) Rk Based n this prperty, it is easy t shw by inductin that: Φ Rk +l 1 (26) Φ Φ = I2 J ( ) p Rk+l p Rk Rk +l 2 Rk which hlds fr all l > Using this result, and substituting fr the measurement Jacbians frm (12) and (13), we can prve the fllwing useful identity: H Rk Φ Φ +l Rk +l 1 Rk = ( h k +l)c T (φ Rk+l ) J(p L p Rk ) = H Lk+l I2 J(p L p Rk ) (27) which hlds fr all l > The bservability matrix M can

6 6 nw be written as: ( M = Diag HLk, H,, H Lk+1 Lk+m) }{{} J(p L p ) I Rk 2 J(p L p Rk 2 } J(p L p ) Rk {{ } N D (28) Lemma 43: The rank f the bservability matrix, M, f the ideal EKF is 2 Prf: The rank f the prduct f the matrices D and N is given by (cf (451) in Meyer, 21): rank( D N) = rank( N) dim(n ( D) R( N)) (29) Since N cmprises m + 1 repetitins f the same 2 5 blck rw, it is clear that rank( N) = 2, and the range f N, R( N), is spanned by the vectrs u 1 and u 2, defined as fllws: 2 u1 u 2 = I (3) We nw bserve that in general Du i, fr i = 1, 2 Mrever, nte that any vectr y R( N) \ can be written as y = α 1 u 1 + α 2 u 2 fr sme α 1, α 2 R, where α 1 and α 2 are nt simultaneusly equal t zer Thus, we see that in general Dy = α 1 Du 1 + α 2 Du 2, which implies that y des nt belng t the nullspace N ( D) f D Therefre, dim(n ( D) R( N)) =, and, finally, rank( M) = rank( N) dim(n ( D) R( N)) = rank( N) = 2 Mst imprtantly, it can be easily verified that a basis fr the right nullspace f N (and thus fr the right nullspace f M) is given by the vectrs shwn in (18) Thus, the unbservable subspace f the ideal EKF system mdel is identical t the space dg, which cntains the unbservable directins f the nnlinear SLAM system We therefre see that if it was pssible t evaluate the Jacbians using the true state values, the linearized errr-state mdel emplyed in the EKF wuld have bservability prperties similar t thse f the actual, nnlinear SLAM system The preceding analysis was carried ut fr the case where a single landmark is included in the state vectr We nw examine the mre general case where M > 1 landmarks are included in the state Suppse the M landmarks are bserved at time-step k + l (l > ), then the measurement matrix H k +l is given by: 2 H k +l = H (1) +l H (1) L k +l H (M) H (M) +l L k +l (31) 2 We here assume that all M landmarks are bserved at every time step in the time interval k, k + m This is dne nly t simplify the ntatin, and is nt a necessary assumptin in the analysis where H (i) R and k H (i) +l L k (i = 1, 2,, M), are btained +l by (12) and (13) using the true values f the states, respectively The bservability matrix M nw becmes: M = (32) H (1) H (1) L k H (M) H (M) L k H (1) +1 Φ Rk H (1) L k+1 H (M) Φ Rk H (M) +1 L k +1 H (1) Φ Rk+m 1 Φ Rk H (1) +m L k +m H (M) +m Φ Rk Φ +m 1 Rk H (M) L k+m Using the identity (27), substitutin f the Jacbian matrices in (32) yields: ( M = Diag H (1) L k,, H (M) }{{} D L k+m ) J(p L1 p ) I Rk J(p LM p ) Rk 2 2 J(p L1 p ) I Rk J(p LM p Rk ) 2 2 J(p L1 p Rk } J(p LM p Rk ) {{ 2 2 } N (33) Clearly, the matrix N nw cnsists f m+1 repetitins f the M blck rws: I2 J(p Li p Rk ) 2 2 I2 }{{} 2 2 ith landmark fr i = 1, 2,, M Therefre, rank( M) = 2M Furthermre, by inspectin, a pssible basis fr the right nullspace f M is given by N ( M) = span cl Jp Rk Jp L1 Jp LM (34)

7 7 Nte the similarity f this result with that f (18) Clearly, the physical interpretatin f this result is analgus t that f the single-landmark case: the glbal translatin and rientatin f the state vectr are unbservable 2) Standard EKF-SLAM: We nw study the bservability prperties f the standard EKF-SLAM, in which the Jacbians are evaluated at the latest state estimates (ie, x k k 1 = ˆx k k 1 and x k k = ˆx k k, fr all k) Once again, we begin by examining the single-landmark case By deriving an expressin analgus t that f (26), we btain (cf Sectin IV-B1): ) Φ Φ Rk +1 R = I2 J (ˆp ˆp Rk+2 k+1 Rk k p Rk+1 k where p Rk +1 ˆp +1 k+1 ˆp Rk+1 k is the crrectin in the rbt psitin due t the EKF update at time-step k +1 Using inductin, we can shw that: Φ Φ Rk +l 1 R Φ k +l 2 R = (35) k ( J ˆp Rk+l k+l 1 ˆp Rk k ) k +l 1 j=k p +1 R j where l > Therefre (cf (11), (12), and (13)) H Φ Rk +l R Φ k +l 1 R = H k L k +l ( J ˆp Lk+l k+l 1 ˆp Rk k ) k +l 1 j=k p +1 R j (36) Using this result, we can write M (cf (24)) as: M = Diag ( H, H Lk L,, H ) k +1 L k +m }{{} (37) D ( ) J ˆp Lk ˆp R k 1 k ( k 1 ) J ˆp Lk ˆp R +1 k k k ) J (ˆp Lk+2 k+1 ˆp Rk k p Rk+1 ( J ˆp Lk ˆp R ) k+m 1 +m k+m 1 k k j=k+1 p R j }{{} N Lemma 44: The rank f the bservability matrix, M, f the system mdel f the standard EKF is equal t 3 Prf: First, we nte that the estimates f any given state variable at different time instants are generally different Hence, in cntrast t the case f the ideal EKF-SLAM, the fllwing inequalities generally hld: ˆp Rk+i k+i 1 ˆp Rk and ˆp ˆp +i k+i Lk +i k+i 1 L k, fr i l +l k+l 1 Therefre, the third clumn f N will be, in general, a vectr with unequal elements, and thus rank(n) = 3 Prceeding similarly t the prf f Lemma 43, we first find ne pssible basis fr the range space f N, R(N) By inspectin, we see that such a basis is given simply by the first 3 clumns f N, which we dente by u i (i = 1, 2, 3) Mrever, it can be verified that generally Du i Therefre, dim(n (D) R(N)) =, and finally rank(m) = rank(n) dim(n (D) R(N)) = rank(n) = 3 We thus see that the linearized errr-state mdel emplyed in the standard EKF-SLAM has different bservability prperties than that f the ideal EKF-SLAM (cf Lemma 43) and that f the underlying nnlinear system (cf Lemma 42) In particular, by prcessing the measurements cllected in the interval k, k + m, the filter acquires infrmatin in 3 dimensins f the state space (alng the directins crrespnding t the bservable subspace f the EKF) Hwever, the measurements actually prvide infrmatin in nly 2 directins f the state space (ie, the rbt-t-landmark relative psitin) As a result, the EKF gains spurius infrmatin alng the unbservable directins f the underlying nnlinear SLAM system, which leads t incnsistency T prbe further, we nte that the basis f the right nullspace f M is given by: N (M) = span cl 1 2 = span n 1 n 2 (38) Nte that these tw vectrs crrespnd t a shifting f the x y plane, which implies that such a shifting is unbservable On the ther hand, the directin crrespnding t the glbal rientatin is missing frm the unbservable subspace f the EKF system mdel (cf (18) and (19)) Therefre, we see that the filter will gain nnexistent infrmatin abut the rbt s glbal rientatin This will lead t an unjustified reductin in the rientatin uncertainty, which will, in turn, further reduce the uncertainty in all the state variables This agrees in sme respects with Bailey et al, 26, Huang and Dissanayake, 27, where it was argued that the rientatin uncertainty is the main cause f the filter s incnsistency in SLAM Hwever, we pint ut that the rt cause f the prblem is that the linearizatin pints used fr cmputing the Jacbians in the standard EKF-SLAM (ie, the latest state estimates) change the dimensin f the bservable subspace, and thus fundamentally alter the prperties f the estimatin prcess Identical cnclusins can be drawn when M > 1 landmarks are included in the state vectr (cf Huang et al, 28c) Fr this general case, the nullspace f the bservability matrix can be shwn t be equal t: N (M) = span cl 1 2 (39) We thus see that the glbal rientatin is errneusly bservable in this case as well, which leads t incnsistent estimates An interesting remark is that the cvariance matrices f the system and measurement nise d nt appear in the bservability analysis f the filter s system mdel Therefre, even if these cvariance matrices are artificially inflated, the filter will retain the same bservability prperties (ie, the same bservable and unbservable subspaces) This shws that n amunt f cvariance inflatin can result in crrect bservability prperties Similarly, even if the iterated EKF Bar- Shalm et al, 21 is emplyed fr state estimatin, the same, errneus, bservability prperties will arise, since the landmark psitin estimates will generally differ at different time steps

8 8 V OBSERVABILITY-CONSTRAINED EKF DESIGN In the preceding sectin, it was shwn that when the EKF Jacbians are evaluated using the latest state estimates, the EKF errr-state mdel has an bservable subspace f dimensin higher than the actual nnlinear SLAM system This will always lead t unjustified reductin f the cvariance estimates, and thus incnsistency We nw describe a framewrk fr addressing this prblem Our key cnjecture is that, by ensuring an unbservable subspace f apprpriate dimensin, we can avid the influx f spurius infrmatin in the errneusly bservable directin f the state space, and thus imprve the cnsistency f the estimates Therefre, we prpse selecting the linearizatin pints f the EKF in a way that guarantees an unbservable subspace f dimensin 3 fr the linearized errr-state mdel This crrespnds t satisfying cnditins (4)-(41) f the fllwing lemma: Lemma 51: If the linearizatin pints, x k k and x k+1 k, at which the EKF Jacbians Φ k = Φ k (x +1 k, x k ) and H k+1 = H k+1 (x +1 k, p L k+1 k ) are evaluated, are selected s as t fulfill the cnditins: H k U =, fr l = (4) H k +lφ k +l 1 Φ k U =, l > (41) where U is a 5 3 full-rank matrix, then the crrespnding bservability matrix is f rank 2 Prf: When (4)-(41) hld, then all the blck rws f the bservability matrix (cf (23)) will have the same nullspace, spanned by the clumns f U Essentially, the selectin f U is a design chice, which allws us t cntrl the unbservable subspace f the EKF system mdel Ideally we wuld like the clumn vectrs f U t be identical t thse in (18), which define the unbservable directins f the actual, nnlinear SLAM system Hwever, this cannt be achieved in practice, since these directins depend n the true values f the state, which are unavailable during any real-wrld implementatin A natural selectin, which is realizable in practice, is t define the unbservable subspace f the bservability matrix based n the state estimates at the first time instant a landmark was detected, ie, fr the single-landmark case t chse 3 Jˆp Rk k 1 U = (42) Jˆp Lk k which satisfies cnditin (4) We stress that this is just ne f several appraches fr selecting the matrix U Fr instance, ne limitatin with this apprach is that, in cases where the initial estimates f the landmarks are nt f sufficient accuracy, the subspace defined in this manner might nt be clse t the actual unbservable subspace T address this prblem ne can emply advanced techniques fr landmark initializatin (eg, delayed-state initializatin Lenard et al, 22), t btain mre precise initial 3 When multiple (M > 1) landmarks are included in the state vectr, U can be chsen analgusly, augmented by a new blck rw, Jˆp Li,k k, crrespnding t each landmark, L i (i = 1, 2,, M) Huang et al, 28c estimates, and use these t define a matrix U This apprach, which culd lead t imprved accuracy in certain situatins, is ne f several interesting ptins t explre within the prpsed design methdlgy Once U has been selected, the next design decisin t be made is the chice f the linearizatin pints at each time step Fr the particular selectin f U in (42), this amunts t chsing the linearizatin pints fr all k > k t ensure that (41) hlds (nte that (4) is satisfied by cnstructin in this case) Clearly, several ptins exist, each f which leads t a different algrithm within the general framewrk described here In what fllws, we present tw appraches t achieve this gal A First Estimates Jacbian (FEJ)-EKF We first describe the First Estimates Jacbian (FEJ)- EKF estimatr that was riginally prpsed in ur previus wrk Huang et al, 28a, Huang et al, 28b The key idea f this apprach is t chse the first-ever available estimates fr all the state variables as the linearizatin pints In particular, cmpared t the standard EKF, the fllwing tw changes are required in the way that the Jacbians are evaluated: 1) Instead f cmputing the state-prpagatin Jacbian matrix Φ Rk as in (8), we emply the expressin: Φ I2 J (ˆp = ˆp Rk+1 k k 1) (43) The difference cmpared t (8) is that the prir rbt psitin estimate, ˆp Rk k 1, is used in place f the psterir estimate, ˆp Rk k 2) In the evaluatin f the measurement Jacbian matrix H k+1 (cf (11), (12), and (13)), we always utilize the landmark estimate frm the first time the landmark was detected and initialized Thus, if a landmark was first seen at time-step k, we cmpute the measurement Jacbian as: H k+1 = H +1 H L k+1 = ( h k+1 )C T ( ˆφ Rk+1 k ) J(ˆp Lk k ˆp +1 k ) (44) As a result f the abve mdificatins, nly the first estimates f all landmark psitins and all rbt pses appear in the filter Jacbians It is easy t verify that the abve Jacbians satisfy (4) and (41) fr the chice f U in (42) Thus, the FEJ-EKF is based n an errr-state system mdel whse unbservable subspace is f dimensin 3 B Observability Cnstrained (OC)-EKF Even thugh the FEJ-EKF typically perfrms substantially better than the standard EKF (cf Sectins VI and VII), it relies heavily n the initial state estimates, since it uses them at all time steps fr cmputing the filter Jacbians If these estimates are far frm the true state, the linearizatin errrs incurred may be large, and culd degrade the perfrmance f the estimatr As a mtivating example, cnsider the linearizatin

9 9 f a general, scalar nnlinear functin f(x) arund a pint x By emplying Taylr expansin, we btain: f(x) = f(x ) + f (x )(x x ) + f (ξ) (x x ) 2 (45) 2 In this expressin, which hlds in the interval (x, x ), f and f are the first- and secnd-rder derivatives f f, and ξ (x, x ) The last term in the abve expressin, f (ξ) 2 (x x ) 2, describes the linearizatin errr, which shuld be kept as small as pssible t maintain the validity f the linear apprximatin Since we d nt have cntrl ver the term f (ξ), t keep the linearizatin errr small, we see that the term (x x ) 2 shuld be kept as small as pssible An interesting bservatin is that if x in the abve example is a Gaussian randm variable with mean ˆx, then the expected value f (x x ) 2 is minimized by chsing x = ˆx This is precisely what the standard EKF des: at each time step, it emplys the mean f the state fr cmputing the linearizatin Jacbians This leads t small linearizatin errr fr each time step, but as explained in Sectin IV-B2, it als changes the bservability prperties f the SLAM system mdel, and adversely affects perfrmance The abve discussin shws that, in the cntext f SLAM, there are tw cmpeting gals that shuld be recnciled: reduced linearizatin errrs at each time step and crrect bservability prperties f the linearized system mdel Therefre, we prpse selecting the linearizatin pints f the EKF s as t minimize the expected squared errr f the linearizatin pints while satisfying the bservability cnditins (4)-(41) This can be frmulated as a cnstrained minimizatin prblem where the cnstraints express the bservability requirements Thus we term the resulting filter Observability-Cnstrained (OC)-EKF Specifically, at time-step k+1, we aim at minimizing the linearizatin errr f the pints x k and x k+1 k, which appear in the Jacbians Φ k and H k+1, subject t the bservability cnstraint (41) k+1 k( Mathematically, this is expressed as: min x x R, x Rk x 2 k p(x Rk z :k ) Rk + k k x k+1 x ) 2 p(x k+1 z :k ) k+1 (46) k+1 k st H k+1 Φ k Φ k U =, k k (47) where z :k dentes all the measurements available during the time interval, k Nte that nly the rbt pse appears in the Jacbians f the prpagatin mdel (cf (6)), while bth the rbt pse and the landmark psitins appear in the Jacbians f the measurement equatins (cf (11)) This justifies the chice f the abve cst functin In general, the cnstrained minimizatin prblem (46)- (47) is intractable Hwever, when the tw pdfs, p(x Rk z :k ) and p(x k+1 z :k ), are Gaussian distributins (which is the assumptin emplyed in the EKF), we can slve the prblem analytically and find a clsed-frm slutin In the fllwing, we shw hw the clsed-frm slutin can be cmputed fr the simple case where nly ne landmark is included in the state vectr The case f multiple landmarks is presented in Huang et al, 28c We nte that the fllwing lemma will be helpful fr the ensuing derivatins: Lemma 52: The cnstrained ptimizatin prblem (46)- (47) is equivalent t the fllwing: min ˆxRk k x R 2 k k + ˆx k+1 k x 2 k+1 k (48) x k, x k+1 k k 1 st p L k+1 k p k = ˆp p Lk k k 1 + p R j j=k (49) where p R j p R j j p R j j 1 Prf: See Appendix D Using the technique f Lagrangian multipliers, the ptimal slutin t the prblem (48)-(49) can be btained as: p k = ˆp Rk k + λ k 2, φ k = ˆφ Rk k, x +1 k = ˆx Rk+1 k, p L k+1 k = ˆp Lk+1 k λ k (5) 2 with λ k = (ˆp k 1 ) Lk+1 k ˆp Lk k ˆp Rk k p k 1 + p R j j=k Nte that in the case where multiple landmarks are included in the state vectr, each landmark impses a cnstraint analgus t (49), and thus the analytical slutin f the ptimal linearizatin pints can be btained similarly Huang et al, 28c Using the linearizatin pints in (5), the filter Jacbians in the OC-EKF are nw cmputed as fllws: 1) The state-prpagatin Jacbian matrix is calculated as: Φ I2 J (ˆp = ˆp Rk+1 k R λ ) k k k 2 (51) ) The measurement Jacbian matrix is calculated as: H k+1 = H +1 H L k+1 = ( h k+1 )C T ( ˆφ Rk+1 k ) (52) J (ˆp Lk+1 k ˆp Rk+1 k λ ) k 2 It is imprtant t nte that, cmpared t the FEJ-EKF, the OC-EKF nt nly guarantees the crrect bservability prperties f the EKF linearized system mdel (s des the FEJ-EKF), but als minimizes the linearizatin errrs under the given bservability requirements The simulatin and experimental results presented in Sectins VI and VII shw the OC-EKF attains slightly better perfrmance than the FEJ-EKF We als pint ut that, cmpared t the standard EKF, the nly change in the OC-EKF is the way in which the Jacbians are cmputed The state estimates in the OC-EKF are prpagated and updated in the same way as in the standard EKF, as utlined in Algrithm 1 In additin, we stress that bth the FEJ-EKF and OC-EKF estimatrs are als causal and realizable in the real wrld, since they d nt utilize any knwledge f the true state Interestingly, althugh bth the FEJ-EKF and the OC-EKF d nt use the latest available state estimates (and thus utilize Jacbians that are less accurate

10 1 than thse f the standard EKF), bth simulatin tests and realwrld experiments demnstrate that they perfrm significantly better than the standard EKF in terms f cnsistency and accuracy (cf Sectins VI and VII) Algrithm 1 Observability Cnstrained (OC)-EKF SLAM Prpagatin: When an dmetry measurement is received: prpagate the rbt pse estimate, via (2)-(3) cmpute the rbt pse prpagatin Jacbian (cf (51)) prpagate the state cvariance matrix: P k+1 k = Φ kp k k Φ kt + Gk Q k G T k where Φ k = Diag ( ) Φ, M and Gk = G T T (2M) 2 Update: When a rbt-t-landmark measurement is received: cmpute the measurement residual: r k+1 = z k+1 h(ˆx k+1 k ) cmpute the measurement Jacbian matrix (cf (52)) cmpute the Kalman gain: with K k+1 = P k+1 k H T k+1s 1 k+1 S k+1 = H k+1p k+1 k H T k update the state estimate: ˆx k+1 k+1 = ˆx k+1 k + K k+1 r k+1 update the state cvariance matrix: P k+1 k+1 = P k+1 k K k+1 S k+1 K T k+1 C Relatin t Prir Wrk At this pint, it is interesting t examine the relatin f ur analysis, which addresses the general case f a mving rbt, t the previus wrk that has fcused n special cases Julier and Uhlmann, 21, Huang and Dissanayake, 27 We first nte that the crrect bservability prperties f the FEJ-EKF and OC-EKF are attributed t the fact that cnditins (4)-(41) hld, which is nt the case fr the standard EKF Thus, (4)- (41) can be seen as sufficient cnditins that, when satisfied by the filter Jacbians, ensure that the bservability matrix has a nullspace f apprpriate dimensins Nte als that, due t the identity (27), the cnditins (4)-(41) are trivially satisfied by the ideal EKF with null space U = n 1 n 2 n 3 (cf (18)) In what fllws, we shw that the cnditins (4)- (41) encmpass the nes derived in Julier and Uhlmann, 21 and Huang and Dissanayake, 27 as special cases 1) Statinary rbt: We first examine the special case studied in Julier and Uhlmann, 21, where the rbt remains statinary, while bserving the relative psitin f a single landmark In Julier and Uhlmann, 21 the fllwing Jacbian cnstraint fr cnsistent estimatin was derived (cf T Therem 1 therein): h x h p g x = H Rk + H Lk g x = I H Rk H 3 Lk g x = H k U s = (53) where, using ur ntatin, h x = H Rk and h p = H Lk are the measurement Jacbian matrices with respect t the rbt pse and landmark psitin, respectively, and g x is the landmark initializatin Jacbian with respect t the rbt pse at time-step k Nte that the cnditin (53) is identical t the ne in (4) fr the special case f a statinary rbt Remarkably, the space spanned by the clumns f the matrix U s, fr this special case, is same as the ne spanned by the clumns f U in (42) T see that, we first need t derive an expressin fr g x In Julier and Uhlmann, 21, a relative-psitin measurement mdel is emplyed (by cmbining a distance and a bearing measurement), and thus the initializatin functin g( ) is given by: p Lk = g(x, z k, v k ) = C(φ Rk ) (z k v k ) + p Rk (54) where z k is the first measurement f the landmark s relative psitin and v k dentes the nise in this measurement Evaluating the derivative f this functin with respect t the rbt pse at the current state estimate we have: g x = JC( ˆφ )z Rk k 1 k = J (ˆp ˆp ) Lk k (55) k 1 where this last equatin results frm taking cnditinal expectatins n bth sides f (54) and slving fr z k Substituting (55) in the expressin fr U s (cf (53)), yields: 2 1 U s = J (ˆp ) Lk k ˆp Rk k 1 One can easily verify that U s and U span the same clumn I2 Jˆp space by nting that U Rk k 1 s = U ) Mving rbt with ne-step mtin: We nw cnsider the special case studied in Huang and Dissanayake, 27, where a rbt bserves a landmark, mves nce and then rebserves the landmark In Huang and Dissanayake, 27, the key Jacbian relatinship that needs t be satisfied in rder t btain cnsistent estimatin in this case (cf Therem 42 therein) is given by: A e = B e f A φx r (56) Using ur ntatin, the abve matrices are written as: fφx A r = Φ Rk A e = H 1 L k H Rk B e = H 1 L k+1 H Rk+1

11 11 Substituting in (56) and rearranging terms yields: H 1 L H k R Φ +1 k +1 R H 1 k L H k R = k H H Φ Rk 3 2 I 3 Rk +1 L k +1 T 3 2 H 1 = L k H Rk H k +1Φ k U 1 = which is the same as the cnditin in (41) fr the special case f l = 1 (ie, the rbt mves nly nce) Additinally, it is easy t verify that H k U 1 =, which crrespnds t cnditin (4) Mrever, it is fairly straightfrward t shw that fr the case f distance and bearing measurements cnsidered in Huang and Dissanayake, 27, the matrix U 1 spans the same clumn space as U in (42) This analysis demnstrates that the Jacbian cnstraints (4)-(41) derived based n the bservability criterin are general, and encmpass the cnditin f Huang and Dissanayake, 27 as a special case VI SIMULATION RESULTS A series f Mnte-Carl cmparisn studies were cnducted under varius cnditins, in rder t validate the preceding theretical analysis and t demnstrate the capability f the FEJ-EKF and OC-EKF estimatrs t imprve the cnsistency f EKF-SLAM The metrics used t evaluate filter perfrmance are: (i) the RMS errr, and (ii) the average nrmalized (state) estimatin errr squared (NEES) Bar-Shalm et al, 21 Specifically, fr the landmarks we cmpute the average RMS errrs and average NEES by averaging the squared errrs and the NEES, respectively, ver all Mnte Carl runs, all landmarks, and all time steps On the ther hand, fr the rbt pse we cmpute these errr metrics by averaging ver all Mnte Carl runs fr each time step (cf Huang et al, 28c fr a mre detailed descriptin) The RMS f the estimatin errrs prvides us with a cncise metric f the accuracy f a given estimatr On the ther hand, the NEES is a metric fr evaluating filter cnsistency Specifically, it is knwn that the NEES f an N-dimensinal Gaussian randm variable fllws a χ 2 distributin with N degrees f freedm Therefre, if a certain filter is cnsistent, we expect that the average NEES fr the rbt pse will be clse t 3 fr all k, and that the average landmark NEES will be clse t 2 The larger the deviatins f the NEES frm these values, the wrse the incnsistency f the filter By studying bth the RMS errrs and NEES f all the filters cnsidered here, we btain a cmprehensive picture f the estimatrs perfrmance In the simulatin tests presented in this sectin, tw SLAM scenaris with lp clsure were cnsidered In the first case, a rbt mves n a circular trajectry and cntinuusly bserves 2 landmarks, while in the secnd case the rbt sequentially bserves 2 landmarks in ttal A First Simulatin: Always Observing 2 Landmarks T validate the preceding bservability analysis, we first ran a SLAM simulatin where a rbt executes 8 lps n a circular trajectry, and cntinuusly bserves 2 landmarks at φ φ hat (rad) Est err (Ideal EKF) Est err (Std EKF) Est err (FEJ EKF) Est err (OC EKF) Est err (Rbcentric) ±3σ bunds (Ideal EKF) ±3σ bunds (Std EKF) ±3σ bunds (FEJ EKF) ±3σ bunds (OC EKF) ±3σ bunds (Rbcentric) Time (sec) x 1 4 Fig 1 Orientatin estimatin errrs vs 3σ bunds btained frm ne typical realizatin f the Mnte Carl simulatins The σ values are cmputed as the square-rt f the crrespnding diagnal element f the estimated cvariance matrix Nte that the estimatin errrs and the 3σ bunds f the ideal EKF, the FEJ-EKF, the OC-EKF and the rbcentric mapping filter are almst identical, which makes the crrespnding lines difficult t distinguish every time step Nte that this simulatin was run sufficiently lng t ensure that the filters (apprximately) reach their steady states and thus exhibit divergence (if any) mre clearly In this simulatin, all filters prcess the same data, t ensure a fair cmparisn The five EKF estimatrs cmpared are: (1) the ideal EKF, (2) the standard EKF, (3) the FEJ-EKF, (4) the OC-EKF, and (5) the rbcentric mapping filter presented in Castellans et al, 24, which aims at imprving the cnsistency f SLAM by expressing the landmarks in a rbtrelative frame Fr the results presented in this sectin, a rbt with a simple differential drive mdel mves n a planar surface, at a cnstant linear velcity f v = 25 m/sec The tw drive wheels are equipped with encders that measure revlutins and prvide measurements f velcity (ie, right and left wheel velcities, v r and v l, respectively) with standard deviatin equal t σ = 5%v fr each wheel These measurements are used t btain linear and rtatinal velcity measurements fr the rbt, which are given by v = vr+v l 2 and ω = vr v l a, where a = 5 m is the distance between the drive wheels Thus, the standard deviatins f the linear and rtatinal velcity measurements are σ v = 2 2 σ and σ ω = 2 a σ, respectively The rbt cntinuusly recrds measurements f the relative psitins f the landmarks which are placed inside the trajectry circle, with standard deviatin equal t 2% f the rbt-t-landmark distance alng each axis Fig 1 shws the results fr the rbt rientatin estimatin errrs in a typical realizatin As evident, the errrs f the standard EKF grw significantly faster than thse f all ther filters, which indicates that the standard EKF tends t diverge Nte als that althugh the rientatin errrs f the ideal EKF, FEJ-EKF, OC-EKF as well as the rbcentric mapping filter remain well within their crrespnding 3σ bunds (cmputed frm the square-rt f the crrespnding diagnal element

12 12 Ideal-EKF Std-EKF FEJ-EKF OC-EKF Rbcentric Rbt Psitin Err RMS (m) Rbt Heading Err RMS (rad) Rbt Pse NEES Landmark Psitin Err RMS (m) Landmark Psitin NEES TABLE I ROBOT POSE AND LANDMARK POSITION ESTIMATION PERFORMANCE f the estimated cvariance matrix), thse f the standard EKF exceed them Mst imprtantly, the 3σ bunds f the standard EKF cntinuusly decrease ver time, as if the rbt rientatin was bservable Hwever, the rbt has n access t any new abslute rientatin infrmatin (beynd what is available by re-bserving the same tw landmarks), and thus its rientatin cvariance shuld nt cntinuusly decrease at steady state The results f Fig 1 further strengthen ur claim that in cntrast t the ideal EKF, FEJ-EKF, OC-EKF, and rbcentric mapping filter (cf Sectins IV-B1, V-A, V-B, and VI-C), the incrrect bservability prperties f the standard EKF cause an unjustified reductin in the rientatin uncertainty B Secnd Simulatin: Lp Clsure T further test the perfrmance f the five estimatrs, we cnducted 5 Mnte Carl simulatins in a SLAM scenari with lp clsure In this scenari, a rbt executes 1 lps n a circular trajectry and bserves 2 landmarks in ttal Fr the results presented in the fllwing, identical rbt and sensr mdels t the preceding simulatin (cf Sectin VI-A) are used, while different sensr nise characteristics are emplyed Specifically, the standard deviatin fr each wheel f the rbt is equal t σ = 2%v, while the standard deviatin f the relative-psitin measurements is equal t 12% f the rbtt-landmark distance alng each axis Mrever, the rbt nw nly bserves the landmarks that lie within its sensing range f 5 m It shuld be pinted ut that the sensr-nise levels selected fr this simulatin are larger than what is typically encuntered in practice This was dne purpsefully, since higher nise levels lead t larger estimatin errrs, which make the effects f incnsistency mre apparent The cmparative results fr all filters are presented in Fig 2 and Table I Specifically, Fig 2(a) and Fig 2(b) shw the average NEES and RMS errrs fr the rbt pse, respectively, versus time On the ther hand, Table I presents the average values f all relevant perfrmance metrics fr bth the landmarks and the rbt As evident, the perfrmance f the FEJ-EKF and the OC-EKF is very clse t that f the ideal EKF, and substantially better than that f the standard EKF, bth in terms f RMS errrs and NEES This ccurs even thugh the Jacbians used in the FEJ-EKF and OC- EKF are less accurate than thse used in the standard EKF, as explained in the preceding sectin This fact indicates that the errrs intrduced by the use f inaccurate Jacbians have a less detrimental effect n cnsistency and accuracy than the use f an errr-state system mdel with incrrect bservability prperties Mrever, it is imprtant t nte that the perfrmance f the OC-EKF is superir t that f the FEJ-EKF by a small margin This is attributed t the fact that the FEJ-EKF has larger linearizatin errrs than the OC- EKF, since the OC-EKF is ptimal by cnstructin, in terms f linearizatin errrs, under the bservability cnstraints C Cmparisn t Rbcentric Mapping Filter Frm the plts f Fig 2, we clearly see that bth the FEJ- EKF and the OC-EKF als perfrm better than the rbcentric mapping filter Castellans et al, 24, Castellans et al, 27, bth in terms f accuracy and cnsistency This result cannt be justified based n the bservability prperties f the filters: in Castellans et al, 24, Castellans et al, 27, the landmarks are represented in the rbt frame, which can be shwn t result in a system mdel with 3 unbservable degrees f freedm Huang et al, 28c Hwever, in the rbcentric mapping filter, during each prpagatin step all landmark psitin estimates need t be changed, since they are expressed with respect t the mving rbt frame As a result, during each prpagatin step (termed cmpsitin in Castellans et al, 24, Castellans et al, 27), all landmark estimates and their cvariance are affected by the linearizatin errrs f the prcess mdel This prblem des nt exist in the wrld-centric frmulatin f SLAM, and it culd ffer an explanatin fr the bserved behavir T test this argument, we first examine the Kullback-Leibler divergence (KLD), between the pdf estimated by each filter, and the pdf estimated by its ideal cunterpart Specifically, we cmpute the KLD (i) between the pdf cmputed by the FEJ-EKF and that f the ideal EKF, (ii) between the pdf cmputed by the OC-EKF and that f the ideal EKF, and (iii) between the pdf cmputed by the rbcentric mapping filter and that prduced by an ideal rbcentric mapping filter, which emplys the true states in cmputing all the Jacbian matrices The KLD is a standard measure fr the difference between prbability distributins It is nnnegative, and equals zer nly if the tw distributins are identical Cver and Thmas, 1991 By cmputing the KLD between the estimated pdf and that f the ideal filter in each case, we can evaluate hw clse each filter is t its respective glden standard These results pertain t the same simulatin setup presented in Sectin VI-B Since the five filters cnsidered here (ie, the OC-EKF, the FEJ-EKF, the ideal EKF, the rbcentric mapping filter, and the ideal rbcentric mapping filter) emply a Gaussian apprximatin f the pdf, we can cmpute the KLD in clsed frm Specifically, the KLD frm an apprximatin distributin, p a (x) = N (µ a, P a ), t the ideal distributin,

13 13 Rbt pse NEES Ideal EKF Std EKF FEJ EKF OC EKF Rbcentric Psitin RMS (m) Ideal EKF Std EKF FEJ EKF OC EKF Rbcentric Heading RMS (rad) Time (sec) Time (sec) Fig 2 Mnte Carl results fr a SLAM scenari with multiple lp clsures (a) Average NEES f the rbt pse errrs (b) RMS errrs fr the rbt pse (psitin and rientatin) In these plts, the slid lines crrespnd t the ideal EKF, the dashed lines t the FEJ-EKF, the dtted lines t the OC-EKF, the slid lines with circles t the standard EKF, and the dash-dtted lines t the rbcentric mapping filter f Castellans et al, 24, Castellans et al, 27 Nte that the RMS errrs f the ideal EKF, FEJ-EKF, and OC-EKF are almst identical, which makes the crrespnding lines difficult t distinguish p (x) = N (µ, P ), is given by: ( d KL = 1 ( ) det(p ) ln + tr(p 1 P a ) 2 det(p a ) ) FEJ EKF OC EKF Rbcentric + (µ µ a ) T P 1 (µ µ a ) dim(x) (57) 1 3 Fig 3 presents the KLD ver time, between the Gaussian distributins cmputed by the rbcentric mapping filter, the FEJ- EKF and the OC-EKF, and thse cmputed by their respective ideal filters (nte that the y-axis scale is lgarithmic) It is evident that the KLD in the case f the rbcentric mapping filter is rders f magnitude larger than in the cases f the FEJ- EKF and the OC-EKF This indicates that the linearizatin errrs in the rbcentric mapping filter result in a wrse apprximatin f the ideal pdf We attribute this fact t the structure f the filter Jacbians During the update step, the structure f the Jacbians in bth the rbcentric and the wrld-centric frmulatins is quite similar Huang et al, 28c In bth cases, the terms appearing in the measurement Jacbians are either rtatin matrices, r the rbt-t-landmark psitin vectr Hwever, the Jacbians emplyed during the cmpsitin step in the rbcentric mapping filter are substantially mre cmplex than thse appearing in the wrld-centric EKF prpagatin (cf (6)) Specifically, in the rbcentric mapping filter, the state vectr is given by (assuming a single landmark fr simplicity): x k = p T G R φ k G p T T L (58) The cmpsitin step is described by the fllwing equatins: Rk ˆp G = C T ( 1 ˆφRk )( 1 ˆp G 1 ˆp Rk ) (59) ˆφG = 1 ˆφG 1 ˆφRk (6) Rk ˆp L = C T ( 1 ˆφRk )( 1 ˆp L 1 ˆp Rk ) (61) where R l ˆpL is the estimate f the landmark psitin with respect t the rbt frame at time-step l (l = k 1, k), KLD Time (sec) Fig 3 Cmparisn results f the KLD in the SLAM scenari with multiple lp clsures In this plt, the slid line with circles crrespnds t the FEJ- EKF, the slid line with crsses t the OC-EKF, and the slid line with squares t the rbcentric mapping filter Castellans et al, 24 Nte that the y- axis scale is lgarithmic Nte that the KLD f the FEJ-EKF and OC-EKF are almst identical, which makes the crrespnding lines difficult t distinguish { 1 ˆpRk, 1 ˆφRk } is the estimate fr the rbt pse change between time-steps k 1 and k, expressed with respect t the rbt frame at time-step k 1, and { R l p G, R l ˆφG } is the estimate fr the transfrmatin between the rbt frame and the glbal frame at time-step l The linearized errr prpagatin equatin is given by: pg Rk 1 Rk 1 φg R pg prk = J k 1 L p L + J G 1 + J R R φg k 1 φrk pl (62)

14 Rbt pse NEES Ideal EKF Std EKF 5 FEJ EKF OC EKF Rbcentric Time (sec) Psitin RMS (m) Heading RMS (rad) 3 2 Ideal EKF Std EKF 1 FEJ EKF OC EKF Rbcentric Time (sec) Fig 4 Mnte Carl results fr a mini-slam scenari with multiple lp clsures where the rbt trajectry and all landmarks are cnfined within a very small area f 1 m 1 m (a) Average NEES f the rbt pse errrs (b) RMS errrs fr the rbt pse (psitin and rientatin) In these plts, the slid lines crrespnd t the ideal EKF, the dashed lines t the FEJ-EKF, the dtted lines t the OC-EKF, the slid lines with circles t the standard EKF, and the dash-dtted lines t the rbcentric mapping filter f Castellans et al, 24 Nte that in this case bth the NEES and the RMS errrs f the ideal EKF, FEJ-EKF, OC-EKF, and the rbcentric mapping filter are almst identical, which makes the crrespnding lines difficult t distinguish where 3 2 J L = C T ( R, J k 1 ˆφRk ) G = CT ( 1 ˆφRk ) J R = CT ( 1 ˆφRk ) J ˆpG (63) C T ( 1 ˆφRk ) J ˆpL We nte that the state estimates appear in the Jacbian matrices J L and J G nly thrugh the rtatin matrix C( 1 ˆφRk ) As a result, the difference between the ideal and actual Jacbians, J L J L and J G J G will nly cntain terms f the frm c( 1 ˆφRk ) c( 1 φ Rk ), and s( 1 ˆφRk ) s( 1 φ Rk ) The magnitude f these terms is in the same rder as 1 φrk, which is typically a very small quantity Thus, the discrepancy between the actual and ideal Jacbians is expected t be very small fr J L and J G On the ther hand, in J R the estimates fr the landmark psitin and fr the rigin f the glbal frame with respect t the rbt appear as well As a result, the difference J R J R will als cntain the terms pg and pl, whse magnitude can be significantly larger, eg, in the rder f meters (cf Fig 2) Thus, the Jacbian J R can be very inaccurate In cntrast, the prpagatin Jacbians in the wrldcentric frmulatin cntain terms depending n (i) the rbt s displacement between cnsecutive time steps, and (ii) the rtatin matrix f the rbt s rientatin (cf (8) and (9)) Since bth f these quantities can be estimated with small errrs, the wrld-centric EKF Jacbians are significantly mre accurate than thse f the rbcentric frmulatin T further test this argument, we ran a simulatin f a mini- SLAM scenari, where bth the rbt trajectry and the landmarks are cnfined within a small area f 1 m 1 m (while all ther settings are identical t the preceding simulatin) In this setup, the estimatin errrs pg and pl remain small, and thus the Jacbians f the rbcentric mapping filter becme mre accurate The plts f Fig 4 shw the average NEES and RMS errrs fr the rbt pse in this scenari Interestingly, we bserve that in this case the perfrmance f the FEJ-EKF, the OC-EKF, and the rbcentric mapping filter are almst identical This validates the preceding discussin, and indicates that the representatin used in the rbcentric mapping filter results in perfrmance lss in the case f large envirnments This may justify the fact that the FEJ-EKF and OC-EKF utperfrm the algrithm f Castellans et al, 24, even thugh all three filters emply a system mdel with three unbservable degrees f freedm As a final remark, we nte that, in cmparisn t the FEJ- EKF and OC-EKF, the cmputatinal cst f the rbcentric mapping filter is significantly higher Specifically, bth the FEJ-EKF and the OC-EKF have cmputatinal cst identical t the standard wrld-centric SLAM algrithm: linear in the number f landmarks during prpagatin, and quadratic during updates On the ther hand, bth the update and the cmpsitin steps in the rbcentric mapping filter have cmputatinal cst quadratic in the number f features, which results in apprximately duble verall cmputatinal burden VII EXPERIMENTAL RESULTS Tw sets f real-wrld experiments were perfrmed t further test the prpsed FEJ-EKF and OC-EKF algrithms The results are presented next A First Experiment: Indrs The first experiment was cnducted in an indr ffice envirnment The rbt was cmmanded t perfrm 1 lps arund a square with sides apprximately equal t 2 m (cf Fig 5) This special trajectry was selected since repeated re-bservatin f the same landmarks tends t make the effects f incnsistency mre apparent, and facilitates discerning the perfrmance f the varius filters A Pineer rbt equipped

15 15 Fig 5 The MAP estimate f the rbt trajectry in the indr experiment (slid line), verlaid n the blueprint f the building The bxes ( ) dente the crners whse exact lcatin is knwn frm the building s blueprint Std EKF FEJ EKF OC EKF Rbcentric Psitin RMS (m) Std EKF FEJ EKF OC EKF Rbcentric Rbt Pse NEES Heading RMS (rad) Time (sec) Time (sec) Fig 6 (a) NEES f the rbt pse errrs (b) RMS errrs fr the rbt pse (psitin and rientatin) In these plts, the slid lines crrespnd t the standard EKF, the dashed lines t the FEJ-EKF, and the dtted lines t the OC-EKF, the dash-dtted lines t the rbcentric mapping filter f Castellans et al, 24 Nte that bth the NEES and the RMS errrs f the FEJ-EKF and OC-EKF are almst identical, which makes the crrespnding lines difficult t distinguish with a SICK LMS2 laser range-finder and wheel encders was used in this experiment Frm the laser range data, crner features were extracted and used as landmarks, while the wheel encders prvided the linear and rtatinal velcity measurements Prpagatin was carried ut using the kinematic mdel described in Appendix A Because the grund truth f the rbt pse culd nt be btained using external sensrs (eg, verhead cameras), in this experiment, we btained a reference trajectry by utilizing the knwn map f the area where the experiment tk place Specifically, the exact lcatin f 2 crners was knwn frm the blueprints f the building Measurements t these crners, as well as all ther measurements btained by the rbt (including t crners whse lcatin was nt knwn a priri), were prcessed using a batch maximum a psteriri (MAP) estimatr, t btain an accurate estimate f the entire trajectry This estimate, as well as the lcatins f the knwn crners, are shwn in Fig 5 This cnstitutes the grund truth against which the perfrmance f the fllwing filters was cmpared: (1) the standard EKF, (2) the FEJ-EKF, (3) the OC-EKF, and (4) the rbcentric mapping filter Clearly, due t the way the grund truth is cmputed, the filter errrs are expected t have sme crrelatin t the errrs in the grund truth Hwever, since these crrelatins are the same fr all