Symmetries in observer design: review of some recent results and applications to EKF-based SLAM

Symmetries in observ er design: review of some recen t results and applications to EKF-based SLAM Silv ` ere Bonnab el ∗ No v em b er 6, 2018 Abstract In this pap er, w e first review the theory of symmetry -preserving observ ers and we mentio n some recen t results. Then, w e apply the theory to Ex tended Kalman Filter- based Simultaneous Localization and Mapping (EKF SLAM). It allo ws to derive a new (symmetry-preserving) Extended Kalman Filter for the non-linear SLAM problem that p osses ses con ve rgence p roperties. W e also pro ve a special c hoice of the gains ensures global exp onential converg ence. 1 In tro du ction Symmetries and Lie groups h a ve b een widely used for feedback control in rob otics, see e.g. [7 , 13]. More generally control of systems p ossessing sy mmetries has also b een studied for q uite a long time, see e.g. [9, 12]. The use of sy mmetries and Lie groups for observer design is more recent [1, 3]. The main prop erties of those observers are based on the reduction of the estimation error complexity . When the symmetry group coincides with the state space (observers on Lie groups), the error equation can b e p articularly simple [4]. This property has b een used to derive n on-linear observers with (almost) global con vergence p roperties for several lo calisa tion problems [11, 4, 15]. Recently [5] established a link b et w een observer d esign and control of systems on Lie group by proving a non-linear separation principle on Lie groups. This pap er prop oses to recap the main elements of the th eory along with some recent results, and to apply it t o the domain of Extended Kalman Filter-based Simultaneous Localization and Mapping (EKF S LAM). It is organized as follo ws: Section 2 is a brief recap on linear observ ers. I n Section 3 we recap the theory of symmetry-p reserving observers [3] and mentio n some recen t results [5, 6]. In Section 4 we apply it in a straigh tforw ard w ay to EKF SLAM. In Section 5 some results for the special case of observers for inv arian t systems on Lie groups [4] are recalled. In Section 6, it is prov ed that those results can b e (surprisingly) applied to EKF SLAM. W e deriv e a simple globally con vergen t observ er for the non-linear problem. W e also prop ose a mo d ified EKF such that the cov ariance matrix and the gain matrix b eh a ve as if t h e system was linear and time-inv arian t. Such non-linear conv ergence guarantees for EKF SLAM are new to the author’s kno wledge. The auth or would like to mention and to th an k his regular co-authors on the sub ject of symmetry-preserving observers : Philippe Martin, Pierre Rouchon, and Erw an Sala ¨ un. ∗ Silv` ere Bonnabel, Cen tre de Robotique, Math ´ ematiques et Syst` emes, Mines P arisT ec h, 60 b d Saint- Michel, 75272 P aris cedex 06. silv ere.bonnab el@mines-paristec h.fr 1 2 Luen b erger observ ers, extended Kalman filte r 2.1 Observ ers for linear systems Observers are mean t to compute an estimation of the state of a dynamical system from sever al sensor measurements. Let x ∈ R n denote the state of the system, u ∈ R m b e the inputs (a set of m kno wn scala r v ariables suc h as controls, constant parameters, etc.). W e assume th e sensors provide measurements y ∈ R p that can b e expressed as a function of the state and the inputs. When th e underlying dyn amical mo del is a linear differential equation, and the output is a linear function as w ell, the system can be written d dt x = Ax + B u, y = C x + D u. (1) A Luenberger observer (or Kalman fi lter) writes d dt ˆ x = A ˆ x + B u − L · ( C ˆ x + D u − y ) , (2) where ˆ x is the estimated state, an d L is a gain matrix th at can b e freely chosen. W e see th at the observer consists in a copy of t he system dynamics A ˆ x + B u , plus a cor- rection term L ( C ˆ x + D u − y ) that “corrects” t h e trusted dynamics in fun ction of the discrepancy b etw een the estimated output ˆ y = C ˆ x + Du and the measured out p ut y . One important issue is the choice (or “tuning”) of the gai n matrix L . The Lu - enberger observe r is based on a choice of a fix ed matrix L . In the Kalman filter tw o p ositive definite matrices M and N denote th e co varia nce matrices of th e state noise and measurement noise, and L relies on a Ricatti equation : L = P C T N , where d dt P = AP + P A T + M − 1 − P C T N C P . As M and N must b e defined by th e user, they can b e view ed as tun ing matrices. In b oth cases the observer has the form (2) with L constant or not. Let ˜ x = ˆ x − x b e the estimation error, and let us compute the differen tial equation satisfied b y the error. W e have d dt ˜ x = ( A + LC ) ˜ x. (3) As the goal of the observe r is t o fin d an estimate of x , w e w an t ˜ x to go to zero. When the system is observa ble, one can alwa ys find L such that ˜ x asymptotically exp onen tially goes to zero, and the negative real part of the eigen values of A + LC can b e freely assigned. W e see that th e theory is particularly simple as the error equation (3) is autonomous , i.e. it do es not dep end on the tra jectory follo wed by t he system. In particular, the inpu t term u has v anished in (3). The well-know n separation principle stems from th is fact. 2.2 Some p opular extensions to nonlinear systems Consider a general n onlinear system d dt x = f ( x, u ) , y = h ( x, u ) , (4) where x ∈ X ⊂ R n is th e state, u ∈ U ⊂ R m the input, and y ∈ Y ⊂ R p the out p ut. Mimic k ing the linear case, a class of p opular nonlinear observers writes d dt ˆ x = f ( ˆ x, u ) − L ( ˆ x, y , t ) ·  h ( ˆ x, u ) − y ( t )  , (5) where the gain matrix can dep end on the v ariables ˆ x, y , t . The error equ ation can still b e computed, but as th e system is nonlinear, it does not necessarily lead to an appropriate gain matrix L . Indeed w e hav e d dt ˜ x = f ( ˆ x, u ( t )) − f ( x , u ( t )) − L ( ˆ x, y ( t ) , t ) ·  h ( ˆ x, u ( t ) ) − 2 y ( t )  . The error eq uation is no longer autonomous, and the p roblem of finding L such that ˜ x goes asymptotically to zero can not b e solved in the general case. The most p opular observer for n on linear systems is the Extend ed Kalman Filter (EKF). The prin cip le is to linearize t he system around the estimated tra jectory , build a Kalman filter fo r the linear mo del, and implement it on the non linear system. The EKF has th e form (5), where th e gain matrix is computed th e follo wing wa y: A = ∂ f ∂ x ( ˆ x, u ) C = ∂ h ∂ x ( ˆ x, u ) (6) L = P C T N − 1 d dt P = AP + P A T + M − P C T N − 1 C P . (7) The EKF h as t w o main fla ws when compared to the KF for time-inv ariant linear systems. First the linearized system around an y tra jectory is generally time-v arying and the co v ariance matrix does n ot tend to a fixed v alue. Then, when ˆ x − x is large th e linearized error equation can b e a very erroneous appro ximation of the true error equ ation. 3 Symmetry-preserving observ ers 3.1 Symmetry group of a system of differen tial equations Let G b e a group, and M b e a set. A group action can b e defin ed on M if to any g ∈ G on can asso ciate a diffeomorphic transformation φ g : M → M suc h that φ gh = φ g ◦ φ h , and ( φ g ) − 1 = φ g − 1 , i. e. , the group multiplica tion correspon d s to the transformation composition, and th e recipro cal elemen ts corresp on d to recipro cal transformations. Definition 1 G is a symmetry gr oup of a system of di ffer ential e quations define d on M if it maps solutions to solutions. In this c ase we say the system is invariant. Definition 2 A ve ctor field w on M is said i nvariant if the syst em d dt z = w ( z ) is invariant. Definition 3 A sc alar invariant is a function I : M → R such that I ( φ g ( z )) = I ( z ) for al l g ∈ G . Salar inv arian ts and inv arian t vector fields can b e built v ia Cartan’s moving frame metho d [14]. Definition 4 A moving fr ame i s a function γ : M → G such that γ ( φ g ( z )) = g · γ ( z ) for al l g , z . Supp ose dim G = r ≤ dim M . Under some mild assumptions on the action (free, regular) there exists locally a mo ving frame. The sets O z = { φ g ( z ) , g ∈ G } are called the group orbits. Let K b e a cross-section to the orbits. A moving frame can b e bu ilt locally via implicit functions theorem as th e solution g = γ ( z ) of th e equation φ g ( z ) = k where k ∈ O z ∩ K . A complete set of functionnaly indep endent inv arian ts is given by the non- constan t comp onen ts of φ γ ( z ) ( z ). Figure 1 illustrates those definitions and t h e mo ving frame method . 3.2 Symmetry group of an observ er Consider the general system (4). Consider also the lo cal group of transformations on X × U defined for any x, u, g by φ g ( x, u ) =  ϕ g ( x ) , ψ g ( u )  , (8) where ϕ g and ψ g correspond to separate local group of transformations of X and U . Proposition 1 The system d dt x = f ( x, u ) is said invariant if it is invariant to the gr oup action (8) . 3 Figure 1: An illustrativ e example. M = R 2 , and t he symmetry group is made of horizontal translations. W e hav e φ g ( z 1 , z 2 ) = ( z 1 + g , z 2 ) T where g ∈ G = R . In local rectifying co ordinates, every inv ariant system can b e represented by a similar figure (under mild assumptions on the group action). Left: In v ariant system. The symmetry group maps each integral line of the vector field into another integ ral line. Right: Mo ving frame metho d. K is a cross-section to the orbits and γ ( z 1 , z 2 ) is the group element that maps ( z 1 , z 2 ) to K along the orbit. F or exemple if K is the set { z 1 ≡ 0 } , the moving frame is γ ( z 1 , z 2 ) = z 1 and a complete set of inv arian ts is I ( z 1 , z 2 ) = z 2 . The group maps solutions to solutions if we have d dt X = f ( X , U ), where ( X , U ) = ( ϕ g ( x ) , ψ g ( u )) for all g ∈ G . W e understand from this defi nition, that u can d enote the con trol v ariables as usu al, b ut it also denotes ev ery feature of the environmen t that makes the system not beh a ve the same w a y after it has b een tran sformed (via ϕ g ). The action of ψ g is mean t to allo w some features of th e environment to b e also mov ed ove r. W e would like th e observer to b e an inv ariant system for the same symmetry group. Definition 5 The observe r (5) is invariant or “symmetry-pr eserving” if it is an invari- ant system for the gr oup action ( ˆ x, x, u, y ) 7→  ϕ g ( x ) , ϕ g ( ˆ x ) , ψ g ( u ) , h ( ϕ g ( x ) , ψ g ( u ))  . In t his case, the structu re of th e observer mimic ks the non linear structure of the system. Let us recall how to build such observers (see [3] for more details). T o d o so, we n eed the output to b e eq u iv arian t: Definition 6 The output is e quivariant if ther e exists a gr oup action on the output sp ac e (via ρ g ) such that h ( ϕ g ( x ) , ψ g ( u )) = ρ g ( h ( x, u )) for al l g , x, u . W e will systematically assume the outp ut is equ iva riant. Let us define an in v ariant output error, instead of the usual linear output error ˆ y − y : Definition 7 The smo oth map ( ˆ x, u, y ) 7→ E ( ˆ x, u, y ) ∈ R p is an in vari ant output error if • E  ϕ g ( ˆ x ) , ψ g ( u ) , ρ g ( y )  = E ( ˆ x, u, y ) for al l ˆ x, u, y (invariant) • the map y 7→ E ( ˆ x, u, y ) is invertible f or al l ˆ x, u (output) • E  ˆ x, u, h ( ˆ x, u )  = 0 f or al l ˆ x , u (err or) An inv arian t error is given (lo cally) by E ( ˆ x, u, y ) = ρ γ ( ˆ x,u ) ( y ) − ρ γ ( ˆ x,u ) ( ˆ y ). Finally , an inv arian t frame ( w 1 , ..., w n ) on X , whic h is a set of n linearly p oint-wise indep endent inv arian t vector fi elds, i.e ( w 1 ( x ) , ..., w n ( x )) is a b asis of the tangent space to X at x . Once again such a frame can b e b uilt (locally) via the moving frame metho d. Proposition 2 [3] The system d dt ˆ x = F ( ˆ x, u, y ) is an invariant observer for the invari- ant system d dt x = f ( x, u ) if and only if: F ( ˆ x, u, y ) = f ( ˆ x, u ) + n X i =1 L i  I ( ˆ x, u ) , E ( ˆ x, u, y )  w i ( ˆ x ) (9) 4 Figure 2: V ehicle taking relative measuremen t s to environmen tal landmarks. wher e E is an invariant output err or, I ( ˆ x, u ) is a c omplete set of sc al ar invariants, the L i ’s ar e smo oth functions such that for al l ˆ x , L i  I ( ˆ x, u ) , 0  = 0 , and ( w 1 , ..., w n ) i s an invariant fr ame. The gains L i must b e tuned in order to ge t some con vergence prop erties if p ossible, and their magnitude should depend on the trade-off betw een measurement noise and conv ergence sp eed. The conv ergence analysis of the observer often relies on an inv ariant state-error: Definition 8 The smo oth map ( ˆ x, x ) 7→ η ( ˆ x, x ) ∈ R n is an invariant state err or if η ( ϕ g ( ˆ x ) , ϕ g ( x )) = η ( ˆ x, x ) (invariant), the map x 7→ η ( ˆ x, x ) is invertible for al l ˆ x (state), and η ( x, x ) = 0 (err or). 3.3 An example: symmetry-preserving observ ers for p osi- tiv e linear systems The linear system d dt x = Ax, y = C x admits scalings G = R ∗ as a symmetry group via the group action φ g ( x ) = g x . Eve ry linear observer is obviously an inv ariant observer. The u n it sphere is a cross-section K to the orbits. A moving frame maps the orbits to the sphere and thus writes γ ( x ) = 1 / k x k ∈ G . A complete set of in vari ants is giv en locally b y n − 1 indep endent coordinates of φ γ ( x ) ( x ) = x/ k x k . Let I ( x ) ∈ R n − 1 b e a complete set of ind ependent inv arian ts. I ( x ) and k x k provide alternative co ordinates named base and fib er coordinates. Moreov er the system has a nice triangular structure in those coordinates. One can prov e that d dt I ( x ( t )) is an inv ariant fun ction and thus it is necessarily of the form g ( I ). As a result w e h a ve d dt I ( x ) = g ( I ( x )) which do es n ot dep end on k x k . W e ha ve thus the follo wing ( general) result : if the restriction of the vector field on the cross-section is a contraction, it suffices t o d efi ne a reduced observer on the orbits i.e. in our case a n orm observer (which means that a scalar outp ut suffices for observ ability). This is the case for instance when A is a matrix whose co efficien ts are stricly p ositive (according to the P erron-F ro ebenius theorem). This fact was recen tly used in [6] to derive inv ariant asymptotic p ositiv e observers for p ositive linear systems. 4 A first applicat ion to EKF SL A M Simultaneous localisation an d mapping (SLAM) addresses th e problem of building a map of an environment from a sequence of sensor measurements obtained from a moving robot. A solution to the SLAM problem has been seen for more than tw ent y years as a 5 “holy grail” in the rob otics communit y since it would b e a means t o make a rob ot tru ly autonomous in an un kno wn environmen t . A very well -known approach that app eared in the early 2000’s is the EKF SLAM [8]. Its main adv antage is to formulate the problem in the form of a state-space mod el with additive Gaussian noise and to provide conve rgence prop erties in t he linear case (i.e. straig ht line motion). I ndeed, the key idea is to include the p osition of the several landmarks (i.e. the map) in the state space. This solution has b een gradually replaced by other techniques su c h as F astSLAM, Graph SLAM etc. In t he framewo rk of EKF SLAM, the problem of estimating online the tra jectory of the rob ot as well as the lo cation of all landm ark s without the need for any a priori knowl edge of location can be form ulated as follo ws [8]. The v ehicle state is defined by the p osition in t h e reference frame (earth-fixed frame) x ∈ R 2 of the centre of the rear axle and the orientation of the vehicle axis θ . The vehicle tru sted motion relies on non-holonomic constraints. The landmarks are modeled as p oints and represen ted b y their p osition in the reference frame p i ∈ R 2 where 1 ≤ i ≤ N . u, v ∈ R are control inputs. Both vehicle and landmark states are registered in the same frame of reference. In a determistic setting (state noises turned off ), the time evolution of the (huge) state vector is ˙ x = u R θ e 1 , ˙ θ = uv , ˙ p i = 0 1 ≤ i ≤ N (10) where e 1 = (1 , 0) T and R θ is the rotation matrix of angle θ . Supp osing that the data association b etw een landmarks from one instant to the n ex t is correctly done, the ob- serv ation mod el for the i -th landmark (disregarding measurement noise) is its p osition seen from the vehicle’s frame: z i = R − θ ( p i − x ). The standard EKF SLA M estimator has the form d dt ˆ x = uR ˆ θ e 1 + L k x ( ˆ z k − z k ) , d dt ˆ θ = uv + N X 1 L k θ ( ˆ z k − z k ) , d dt ˆ p i = N X 1 L k i ( ˆ z k − z k ) , 1 ≤ i ≤ N (11) where ˆ z i = R − ˆ θ ( ˆ p i − ˆ x ) and where the L i ’s are the lines of L tu n ed via th e EKF equations (6)-(7). Here the group of symmetry of the system corresp onds to Galilea n inv ariances, and it is made of rotations and translations of the plane S E (2). Indeed, lo oking at Figure 2, it is obvious that the equations of motion are th e same whether the first h orizon tal axis of the reference frame is p ointing North, or East, or in any direction. F or g = ( x 0 , θ 0 ) ∈ S E (2), the action of the group on th e state space is ϕ g ( x, θ , p i ) = ( R θ 0 x + x 0 , θ + θ 0 , R θ 0 p i + x 0 ) and ψ g ( u, v ) = u, v . The outpu t is also unc hanged by the group transformation as it is expressed in the vehicle frame and is thus insensitive to rotations and translations of the reference frame. A pplying the theory of the last section, the observer ab o ve can b e “in v ariantized”, yielding the follow ing inv arian t observer: d dt ˆ x = uR ˆ θ e 1 + R ˆ θ ( N X 1 L k x ( ˆ z k − z k )) , d dt ˆ θ = uv + N X 1 L k θ ( ˆ z k − z k ) , d dt ˆ p i = R ˆ θ ( N X 1 L k i ( ˆ z k − z k )) (12) It is easy to see that the inv ariant observer is muc h more meaningful, esp ecially if the L ′ i s are c hosen as constant matrices [3]. Indeed, one could really wonder if it is sensible to correct vectors expressed in the reference frame directly with measurements expressed in the vehicle frame. T o b e convinced, consider th e follo wing simple case : suppose ˆ θ = θ = ˆ x = x = 0 remain fixed. W e hav e d dt ( ˆ p i − p i ) = L i ( ˆ p i − p i ). Choosing L i = − k I yields d dt k ˆ p i − p i k 2 = − k k ˆ p i − p i k 2 leading to a correct estimation of landmark p i . Now supp ose that the vehicl e has changed its orientation and ˆ θ = θ = π / 2. The output error is now R − π/ 2 ( ˆ p i − p i ). With an observer of the form (11) t he same choice L i = − k I yields d dt k ˆ p i − p i k = 0 and the landmark is not correctly estimated. O n the other hand, 6 with (12) we ha ve in b oth cases d dt k ˆ p i − p i k 2 = − k k ˆ p i − p i k 2 ensuring conv ergence of ˆ p to w ards p . Constan t gains is a special (simple) choice, b ut the observ er gains can also b e t uned via Kalman eq u ations. Ind eed on can define noises on the linearized inv ariant error system and tune the L i ’s v ia Kalman equations (see Inv ariant EKF method [2]). T o sum up, any Luen b erger observer or EKF can b e inv ariantized v ia equations (12). This yields in the author’s opinion a m uch more meaningful non- linear observer t hat is well- adapted to the problem’s structure. The inv arian tized observer (12) is simply a ve rsion of (11) whic h is less sensitive to c hange of co ordinates, and even if no pro of can supp ort this claim we b eliev e it can only impro ve the p erformances of (11). 5 P articular case where the state space coincides with its symmetry group Over the last h alf d ecade, inv arian t observers on Lie groups for lo w-cost aided in ertial naviga tion h a ve b een stud ied b y several teams in the wo rld, [11, 3, 15] to name a few. Several p ow erful con vergence results hav e b een obtained. T hey are all link ed to the sp ecial prop erties of the inv ariant state error on a Lie group. T o recap briefly the construction of in v ariant observers on Lie groups [4], w e assume th at th e sy mmetry group G is a matrix gro up, and th at X = G . The system is assumed to b e inv ariant to left multipli cations i.e. d dt X = X Ω( t ) . W e hav e indeed for any g ∈ G that d dt ( g X ) = ( g X )Ω. F or instance the motion of the vehicle in the considered SLA M problem ˙ x = uR θ e 1 , ˙ θ = uv can b e view ed as a left-inv ariant system on the Lie group SE(2) via the matrix representa tion X =  R θ x 0 1 × 2 1  , Ω =  ω x ue 1 0 1 × 2 0  , with ω x =  0 − uv uv 0  Supp ose t h e output y = h ( X ) is equiv ariant, i. e. there exists a group action on the output space such t hat h ( g X ) = ρ g ( X ). In this case th e inv ariant observer ( 9) can b e written intrinsical ly d dt ˆ X = ˆ X Ω + ˆ X L ( ρ ˆ X − 1 ( y )) . with L ( e ) = 0 where e is th e gro up identit y element. The inv ariant state error is th e natural group d ifference η = X − 1 ˆ X and the error equ ation is d dt η = [Ω , η ] + η L ◦ h ( η − 1 ) A remark able fa ct is th at the error equation only dep ends on η and Ω, whereas th e system is n on-linear and the error should also dep end on ˆ X (th ink ab out the EKF whic h is based on a lineariza tion around any ˆ X at eac h time). Moreo ver, if Ω = cst , the error equ ation is clearly autonomous . Th us the motion p rimitives generated by constant Ω are sp ecial tra jectories called “permanent tra jectories”. Aroun d such tra jectories one can alw a ys ac hieve lo cal conv ergence (as so on as the linearized system is observ able). It is w orth noting this prop ert y was recently used t o derive a n on-linear sep ar ation principle on Lie groups [5]. It applies to some cart-like underactuated vehicles and some underwater or aerial fully actuated vehicl es. An ev en more interesting case o ccurs when the outp ut satisfies right-equiv ariance i.e., h ( X g ) = ρ g ( h ( X )). In this case w e let the input be u = Ω and we consider the action of G by righ t multipli cation, i.e. ϕ g ( X ) = X g and ψ g (Ω) = g − 1 Ω g . The outp ut is equiv ariant as h ( ϕ g ( X )) = ρ g ◦ h ( X ). The inv ariant observer associated with this group of symmetry writes d dt ˆ X = ˆ X Ω + L ( ρ ˆ X − 1 ( y )) ˆ X . The inv arian t state error is η = ˆ X X − 1 and the error equation is d dt η = ˆ X Ω X − 1 + L ( h ( η − 1 )) η − ˆ X Ω X − 1 = L ( h ( η − 1 )) η (13) 7 The error equation is completely autonomous ! I n p articular the linearized system around any tra jectory is the same time-inv ariant system. Au tonom y is the key for numerous p ow erful con verge nce results for observers on Lie groups see e.g. [10, 15, 4]. 6 A new re sult in EKF SLA M In this section we prop ose a n ew non-linear observer for EKF SLAM with guaranteed conv ergence prop erties. In the SLAM problem th e state space is muc h bigger than its symmetry group. The orbits ha ve dimension 3 and th us there are N + 1 − 3 + 2 = N in v ariants (dimension of the cross-section, see Fig.1 ). Thus an aut onomous error equation seems to be out of reac h. Suprinsingly considering the symmetry group of rotations and translations in the ve hicle frame yields suc h a result. A simple trick makes it obvious. Consider the follo wing matrix representatio n: X =  R θ x 0 1 × 2 1  , P i =  R θ p i 0 1 × 2 1  , Ω =  ω x ue 1 0 1 × 2 0  , Ω i =  ω x 0 0 1 × 2 0  The eq u ations of the system (10) can b e written d dt X = X Ω , d dt P i = P i Ω i , 1 ≤ i ≤ N and the system can b e view ed as a left-inv ariant d ynamics system on the (h u ge) Lie group G × · · · × G . Let η x = ˆ X X − 1 , η i = ˆ P i P − 1 i b e the in va rian t state error. The system has the inv arian t output errors ˜ Y i = R ˆ θ ( ˆ z i − z i ), i. e.  ˜ Y i 1  T = ( η i − η x ) H for 1 ≤ i ≤ N where H =  0 1 × 2 1  T . Consider the follow ing inv ariant observ er d dt ˆ X = ˆ X Ω + L X ( ˜ Y 1 , · · · , ˜ Y N ) ˆ X , d dt ˆ P i = ˆ P i Ω i + L i ( ˜ Y 1 , · · · , ˜ Y N ) ˆ P i . F rom (13), the (non-linear) error equation is completely autonomous remind ing th e linear case (3). I t implies the follo wing global conv ergence result for th e n on-linear deterministic system: Proposition 3 Consider the SLAM pr oblem ( 10) without noise. The fol lowing observe r d dt ˆ θ = uv , d dt ˆ x = uR ˆ θ e 1 , d dt ˆ p i = k i R ˆ θ ( ˆ z i − z i ) with k i > 0 is such that d dt ( R ˆ θ ( ˆ z i − z i )) = − k i R ˆ θ ( ˆ z i − z i ) , i.e., al l the estimation err ors ( ˆ z i − z i ) , 1 ≤ i ≤ N c onver ge glob al ly exp onential ly to zer o wi th r ate k i , which me ans the vehicle tr aje ctory and the map ar e c orr e ctly identifie d. The p ar ameter k i must b e tune d ac c or di ng to the level of noi se asso ciate d to landmark i , and vehicle sensors’ noise. If one w ants to define noise co v ariance matrices M , N to tune the observer (and compute an estimation P of t he cov ariance error matrix at each t ime), it is also possible to defi ne a mo dified EKF with guaranteed conv ergence prop erties: Proposition 4 Consider the SLAM pr oblem (10) . Le t E = ( R ˆ θ ( ˆ z i − z i )) 1 ≤ i ≤ N b e the invariant output err or. L et e 3 b e the vertic al axis. Consider the observer d dt ˆ θ = uv + L θ ( E ) , d dt ˆ x = uR ˆ θ e 1 + L θ ( E ) e 3 ∧ ˆ x + L x ( E ) , d dt ˆ p i = L θ ( E ) e 3 ∧ ˆ p + L i ( E ) L et η = ( ˜ θ, ˜ x, ˜ p 1 , · · · , ˜ p n ) b e the i nvariant state err or wher e ˜ θ = ˆ θ − θ , ˜ x = ˆ x − R ˜ θ x, ˜ p i = ˆ p i − R ˜ θ p i . The state err or e quation is autonomous, i.e. d dt η only dep ends on η . It is thus c ompletely indep endent of the tr aj e ctory and of u ( t ) , v ( t ) . The li ne arize d err or e quation writes d dt δ η = ( LC ) δ η wher e L c an b e fr e ely chosen and C is a fixe d matrix. As in the usual EKF metho d, one c an define c ovarianc e matries M , N , build a Kalman filter for the line arize d system, i. e. tune L via the usual e quations (7) i. e. ˙ P = M − P C T N − 1 C P , L = P C T N − 1 , and implement it on the non-line ar mo del. A l l the c onver genc e r esults on P and L valid for stationnary systems (1) with A = 0 , B = 0 , D = 0 apply. Simulati ons ( Fig. 3) with one landmark and noisy measurements indicate the mo d- ified EKF (IEKF) b ehav es very similarly , or slightly b etter than the EKF, but the gain matrix tends quickly to a fixed matrix L indep endently from the tra jectory and th e 8 inputs u, v . So the I n v ariant EKF prop osed in t his pap er 1- is incomparably cheaper computationaly as it relies on a constant matrix L that can b e compu t ed offline once and for all (t h e number of land marks can thus b e muc h increased) 2- is such that the linearized error system is stable as so on as LC has negative eigen v alues, which is easy to verify . Remark 1 The c al culations ab ove ar e valid on S E (3) and the r esults apply to 6 DOF SLAM. References [1] N . A ghannan and P . Rouchon. On inv arian t asymptotic observers. In 41st IEEE Confer enc e on De cision and Contr ol , pages 1479–1484, 2002. [2] S . Bonnab el, P . Martin, and E. Salaun. Inv ariant extended k alman filter: Theory and application to a velocity-aided attitude estimation problem. In IEEE Confer- enc e on De cision and C ontr ol , 2009. [3] S . Bonnabel, Ph. Martin, and P . Rouchon. Symm etry -preserving observers. I EEE T r ans. on Aut omatic Contr ol , 53(11):25 14–2526 , 2008. [4] S . Bonn abel, Ph. Martin, and P . Rouc hon. Non-linear symmetry-preserving ob- serve rs on lie groups. IEEE T r ans. on Automatic Contr ol , 54(7):1709 – 1713 , 2009. [5] S . Bonn ab el, Ph. Martin, P . Rouc hon, an d E. Salaun. A separation prin ciple on lie groups. In I F A C (available on Arxiv) , 2011. [6] S . Bonnab el and R. S epulc hre. Contraction and observer d esign on cones. Ar xiv , 2011. [7] F. Bullo and R.M. Murra y . T racking for fully actu ated mec hanical systems: A geometric framew ork. A utomatic a , 35(1):17–34, 1999. [8] G. Dissana ya ke, P . Newman, H.F. Du rrant-Whyte, S. Clark, and M. Csobra. A solution to the simultaneous localisation and mapping (slam) problem. IEEE T r ans. R ob ot. Automat. , 17:229–241 , 2001. [9] J.W. Grizzle and S.I. Marcus. The structu re of n onlinear systems p ossessing sym- metries. IEEE T r ans. Auto mat. Contr ol , 30:248–258 , 1985. [10] C. Lagemann, J. T rumpf, and R. Ma hony . Gradient-like observers for in v ariant dynamics on a lie group. IEEE T r ans. on Automatic Contr ol , 55:2:367 – 377, 2010. [11] R. Mahon y , T. Hamel, and J-M Pflimlin. Nonlinear complementary filters on th e sp ecial orthogonal group. IEEE-T r ans. on Automatic C ontr ol , 53(5): 1203–121 8, 2008. [12] Ph. Martin, P . Rouc hon, and J. R udolph. Inv ariant tracking. ESAIM: Contr ol, Optimisation and Calculus of V ariations , 10:1–13, 2004. [13] P . Morin and C. Samson. Practical stabilization of driftless systems on lie groups, the transverse fun ction approach. IEEE T r ans. A utomat. Contr ol , 48:1493–15 08, 2003. [14] P . J. Olver. Classic al Invariant The ory . Cam bridge Universit y Press, 1999. [15] J.F. V asconcelos, R. Cunh a, C. S ilv estre, and P . Oliveira. A nonlinear p osition and attitude observer on se(3) using landmark measurements. Systems Contr ol L etter s , 59:155– 166, 2010. 9 Figure 3: Simulations with one landmark and a car mo ving ov er a circular path with a 20% noise. Up: 1-Estimated vehicle tra jectory (plain blue line) and landmark position (dashed green line) with Inv arian t EKF, 2-Estimation with the u sual EKF, 3- tru e v eh icle tra jectory (plain blue line) and landmark position (green cros s). After a short transien t, the tra jectory is correctly id en tified for b oth observers (up to a rotation-translation). Bottom : 1-co efficien ts of L ( t ) o ver t ime for Inv arian t EKF, 2-co efficien ts of L ( t ) for EKF. W ee see the EKF gain matrix is p ermanently adapting to the motion of the car (right) whereas its inv arian t counterpart (left) is directly exp ressed in well-adapted v ariables. 10