arXiv:1105.2254v1 [math.OC] 11 May 2011 Symmetries in observer design: review of some recent results and applications to EKF-based SLAM Silv`ere Bonnabel∗ November 6, 2018 Abstract In this paper, we first review the theory of symmetry-preserving observers and we mention some recent results. Then, we apply the theory to Extended Kalman Filter- based Simultaneous Localization and Mapping (EKF SLAM). It allows to derive a new (symmetry-preserving) Extended Kalman Filter for the non-linear SLAM problem that possesses convergence properties. We also prove a special choice of the gains ensures global exponential convergence. 1 Introduction Symmetries and Lie groups have been widely used for feedback control in robotics, see e.g. [7, 13]. More generally control of systems possessing symmetries has also been studied for quite a long time, see e.g. [9, 12]. The use of symmetries and Lie groups for observer design is more recent [1, 3]. The main properties 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 be particularly simple [4]. This property has been used to derive non-linear observers with (almost) global convergence properties for several localisation problems [11, 4, 15]. Recently [5] established a link between observer design and control of systems on Lie group by proving a non-linear separation principle on Lie groups. This paper proposes to recap the main elements of the theory along with some recent results, and to apply it to the domain of Extended Kalman Filter-based Simultaneous Localization and Mapping (EKF SLAM). It is organized as follows: Section 2 is a brief recap on linear observers. In Section 3 we recap the theory of symmetry-preserving observers [3] and mention some recent results [5, 6]. In Section 4 we apply it in a straightforward way to EKF SLAM. In Section 5 some results for the special case of observers for invariant systems on Lie groups [4] are recalled. In Section 6, it is proved that those results can be (surprisingly) applied to EKF SLAM. We derive a simple globally convergent observer for the non-linear problem. We also propose a modified EKF such that the covariance matrix and the gain matrix behave as if the system was linear and time-invariant. Such non-linear convergence guarantees for EKF SLAM are new to the author’s knowledge. The author would like to mention and to thank his regular co-authors on the subject of symmetry-preserving observers : Philippe Martin, Pierre Rouchon, and Erwan Sala¨un. ∗Silv`ere Bonnabel, Centre de Robotique, Math´ematiques et Syst`emes, Mines ParisTech, 60 bd Saint- Michel, 75272 Paris cedex 06. silvere.bonnabel@mines-paristech.fr 1 2 Luenberger observers, extended Kalman filter 2.1 Observers for linear systems Observers are meant to compute an estimation of the state of a dynamical system from several sensor measurements. Let x ∈Rn denote the state of the system, u ∈Rm be the inputs (a set of m known scalar variables such as controls, constant parameters, etc.). We assume the sensors provide measurements y ∈Rp that can be expressed as a function of the state and the inputs. When the underlying dynamical model is a linear differential equation, and the output is a linear function as well, the system can be written d dtx = Ax + Bu, y = Cx + Du. (1) A Luenberger observer (or Kalman filter) writes d dt ˆx = Aˆx + Bu −L · (Cˆx + Du −y), (2) where ˆx is the estimated state, and L is a gain matrix that can be freely chosen. We see that the observer consists in a copy of the system dynamics Aˆx + Bu, plus a cor- rection term L(Cˆx + Du −y) that “corrects” the trusted dynamics in function of the discrepancy between the estimated output ˆy = Cˆx + Du and the measured output y. One important issue is the choice (or “tuning”) of the gain matrix L. The Lu- enberger observer is based on a choice of a fixed matrix L. In the Kalman filter two positive definite matrices M and N denote the covariance matrices of the state noise and measurement noise, and L relies on a Ricatti equation : L = PCT N, where d dtP = AP + PAT + M −1 −PCT NCP. As M and N must be defined by the user, they can be viewed as tuning matrices. In both cases the observer has the form (2) with L constant or not. Let ˜x = ˆx−x be the estimation error, and let us compute the differential equation satisfied by the error. We have d dt ˜x = (A + LC)˜x. (3) As the goal of the observer is to find an estimate of x, we want ˜x to go to zero. When the system is observable, one can always find L such that ˜x asymptotically exponentially goes to zero, and the negative real part of the eigenvalues of A + LC can be freely assigned. We see that the theory is particularly simple as the error equation (3) is autonomous, i.e. it does not depend on the trajectory followed by the system. In particular, the input term u has vanished in (3). The well-known separation principle stems from this fact. 2.2 Some popular extensions to nonlinear systems Consider a general nonlinear system d dtx = f(x, u), y = h(x, u), (4) where x ∈X ⊂Rn is the state, u ∈U ⊂Rm the input, and y ∈Y ⊂Rp the output. Mimicking the linear case, a class of popular nonlinear observers writes d dt ˆx = f(ˆx, u) −L(ˆx, y, t) · h(ˆx, u) −y(t)  , (5) where the gain matrix can depend on the variables ˆx, y, t. The error equation can still be computed, but as the system is nonlinear, it does not necessarily lead to an appropriate gain matrix L. Indeed we have 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 equation is no longer autonomous, and the problem of finding L such that ˜x goes asymptotically to zero can not be solved in the general case. The most popular observer for nonlinear systems is the Extended Kalman Filter (EKF). The principle is to linearize the system around the estimated trajectory, build a Kalman filter for the linear model, and implement it on the nonlinear system. The EKF has the form (5), where the gain matrix is computed the following way: A = ∂f ∂x(ˆx, u) C = ∂h ∂x(ˆx, u) (6) L = PCT N −1 d dtP = AP + PAT + M −PCT N −1CP. (7) The EKF has two main flaws when compared to the KF for time-invariant linear systems. First the linearized system around any trajectory is generally time-varying and the covariance matrix does not tend to a fixed value. Then, when ˆx−x is large the linearized error equation can be a very erroneous approximation of the true error equation. 3 Symmetry-preserving observers 3.1 Symmetry group of a system of differential equations Let G be a group, and M be a set. A group action can be defined on M if to any g ∈G on can associate a diffeomorphic transformation φg : M →M such that φgh = φg ◦φh, and (φg)−1 = φg−1, i.e., the group multiplication corresponds to the transformation composition, and the reciprocal elements correspond to reciprocal transformations. Definition 1 G is a symmetry group of a system of differential equations defined on M if it maps solutions to solutions. In this case we say the system is invariant. Definition 2 A vector field w on M is said invariant if the system d dtz = w(z) is invariant. Definition 3 A scalar invariant is a function I : M →R such that I(φg(z)) = I(z) for all g ∈G. Salar invariants and invariant vector fields can be built via Cartan’s moving frame method [14]. Definition 4 A moving frame is a function γ : M →G such that γ(φg(z)) = g · γ(z) for all g, z. Suppose dim G = r ≤dim M. Under some mild assumptions on the action (free, regular) there exists locally a moving frame. The sets Oz = {φg(z), g ∈G} are called the group orbits. Let K be a cross-section to the orbits. A moving frame can be built locally via implicit functions theorem as the solution g = γ(z) of the equation φg(z) = k where k ∈Oz ∩K. A complete set of functionnaly independent invariants is given by the non-constant components of φγ(z)(z). Figure 1 illustrates those definitions and the moving frame method. 3.2 Symmetry group of an observer Consider the general system (4). Consider also the local 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 dtx = f(x, u) is said invariant if it is invariant to the group action (8). 3 Figure 1: An illustrative example. M = R2, and the symmetry group is made of horizontal translations. We have φg(z1, z2) = (z1 + g, z2)T where g ∈G = R. In local rectifying coordinates, every invariant system can be represented by a similar figure (under mild assumptions on the group action). Left: Invariant system. The symmetry group maps each integral line of the vector field into another integral line. Right: Moving frame method. K is a cross-section to the orbits and γ(z1, z2) is the group element that maps (z1, z2) to K along the orbit. For exemple if K is the set {z1 ≡0}, the moving frame is γ(z1, z2) = z1 and a complete set of invariants is I(z1, z2) = z2. The group maps solutions to solutions if we have d dtX = f(X, U), where (X, U) = (ϕg(x), ψg(u)) for all g ∈G. We understand from this definition, that u can denote the control variables as usual, but it also denotes every feature of the environment that makes the system not behave the same way after it has been transformed (via ϕg). The action of ψg is meant to allow some features of the environment to be also moved over. We would like the observer to be an invariant system for the same symmetry group. Definition 5 The observer (5) is invariant or “symmetry-preserving” if it is an invari- ant system for the group action (ˆx, x, u, y) 7→ ϕg(x), ϕg(ˆx), ψg(u), h(ϕg(x), ψg(u))  . In this case, the structure of the observer mimicks the nonlinear structure of the system. Let us recall how to build such observers (see [3] for more details). To do so, we need the output to be equivariant: Definition 6 The output is equivariant if there exists a group action on the output space (via ρg) such that h(ϕg(x), ψg(u)) = ρg(h(x, u)) for all g, x, u. We will systematically assume the output is equivariant. Let us define an invariant output error, instead of the usual linear output error ˆy −y: Definition 7 The smooth map (ˆx, u, y) 7→E(ˆx, u, y) ∈Rp is an invariant output error if • E ϕg(ˆx), ψg(u), ρg(y)  = E(ˆx, u, y) for all ˆx, u, y (invariant) • the map y 7→E(ˆx, u, y) is invertible for all ˆx, u (output) • E ˆx, u, h(ˆx, u)  = 0 for all ˆx, u (error) An invariant error is given (locally) by E(ˆx, u, y) = ργ(ˆx,u)(y) −ργ(ˆx,u)(ˆy). Finally, an invariant frame (w1, ..., wn) on X , which is a set of n linearly point-wise independent invariant vector fields, i.e (w1(x), ..., wn(x)) is a basis of the tangent space to X at x. Once again such a frame can be built (locally) via the moving frame method. Proposition 2 [3] The system d dt ˆx = F(ˆx, u, y) is an invariant observer for the invari- ant system d dtx = f(x, u) if and only if: F(ˆx, u, y) = f(ˆx, u) + n X i=1 Li I(ˆx, u), E(ˆx, u, y)  wi(ˆx) (9) 4 Figure 2: Vehicle taking relative measurements to environmental landmarks. where E is an invariant output error, I(ˆx, u) is a complete set of scalar invariants, the Li’s are smooth functions such that for all ˆx, Li I(ˆx, u), 0  = 0, and (w1, ..., wn) is an invariant frame. The gains Li must be tuned in order to get some convergence properties if possible, and their magnitude should depend on the trade-offbetween measurement noise and convergence speed. The convergence analysis of the observer often relies on an invariant state-error: Definition 8 The smooth map (ˆx, x) 7→η(ˆx, x) ∈Rn is an invariant state error if η(ϕg(ˆx), ϕg(x)) = η(ˆx, x) (invariant), the map x 7→η(ˆx, x) is invertible for all ˆx (state), and η(x, x) = 0 (error). 3.3 An example: symmetry-preserving observers for posi- tive linear systems The linear system d dtx = Ax, y = Cx admits scalings G = R∗as a symmetry group via the group action φg(x) = gx. Every linear observer is obviously an invariant observer. The unit sphere is a cross-section K to the orbits. A moving frame maps the orbits to the sphere and thus writes γ(x) = 1/∥x∥∈G. A complete set of invariants is given locally by n −1 independent coordinates of φγ(x)(x) = x/∥x∥. Let I(x) ∈Rn−1 be a complete set of independent invariants. I(x) and ∥x∥provide alternative coordinates named base and fiber coordinates. Moreover the system has a nice triangular structure in those coordinates. One can prove that d dtI(x(t)) is an invariant function and thus it is necessarily of the form g(I). As a result we have d dtI(x) = g(I(x)) which does not depend on ∥x∥. We have thus the following (general) result : if the restriction of the vector field on the cross-section is a contraction, it suffices to define a reduced observer on the orbits i.e. in our case a norm observer (which means that a scalar output suffices for observability). This is the case for instance when A is a matrix whose coefficients are stricly positive (according to the Perron-Froebenius theorem). This fact was recently used in [6] to derive invariant asymptotic positive observers for positive linear systems. 4 A first application to EKF SLAM Simultaneous localisation and mapping (SLAM) addresses the 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 twenty years as a 5 “holy grail” in the robotics community since it would be a means to make a robot truly autonomous in an unknown environment. A very well-known approach that appeared in the early 2000’s is the EKF SLAM [8]. Its main advantage is to formulate the problem in the form of a state-space model with additive Gaussian noise and to provide convergence properties in the linear case (i.e. straight line motion). Indeed, the key idea is to include the position of the several landmarks (i.e. the map) in the state space. This solution has been gradually replaced by other techniques such as FastSLAM, Graph SLAM etc. In the framework of EKF SLAM, the problem of estimating online the trajectory of the robot as well as the location of all landmarks without the need for any a priori knowledge of location can be formulated as follows [8]. The vehicle state is defined by the position in the reference frame (earth-fixed frame) x ∈R2 of the centre of the rear axle and the orientation of the vehicle axis θ. The vehicle trusted motion relies on non-holonomic constraints. The landmarks are modeled as points and represented by their position in the reference frame pi ∈R2 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θe1, ˙θ = uv, ˙pi = 0 1 ≤i ≤N (10) where e1 = (1, 0)T and Rθ is the rotation matrix of angle θ. Supposing that the data association between landmarks from one instant to the next is correctly done, the ob- servation model for the i-th landmark (disregarding measurement noise) is its position seen from the vehicle’s frame: zi = R−θ(pi −x). The standard EKF SLAM estimator has the form d dt ˆx = uRˆθe1 + Lk x(ˆzk −zk), d dt ˆθ = uv + N X 1 Lk θ(ˆzk −zk), d dt ˆpi = N X 1 Lk i (ˆzk −zk), 1 ≤i ≤N (11) where ˆzi = R−ˆθ(ˆpi−ˆx) and where the Li’s are the lines of L tuned via the EKF equations (6)-(7). Here the group of symmetry of the system corresponds to Galilean invariances, and it is made of rotations and translations of the plane SE(2). Indeed, looking at Figure 2, it is obvious that the equations of motion are the same whether the first horizontal axis of the reference frame is pointing North, or East, or in any direction. For g = (x0, θ0) ∈SE(2), the action of the group on the state space is ϕg(x, θ, pi) = (Rθ0x+x0, θ +θ0, Rθ0pi +x0) and ψg(u, v) = u, v. The output is also unchanged by the group transformation as it is expressed in the vehicle frame and is thus insensitive to rotations and translations of the reference frame. Applying the theory of the last section, the observer above can be “invariantized”, yielding the following invariant observer: d dt ˆx = uRˆθe1 + Rˆθ( N X 1 Lk x(ˆzk −zk)), d dt ˆθ = uv + N X 1 Lk θ(ˆzk −zk), d dt ˆpi = Rˆθ( N X 1 Lk i (ˆzk −zk)) (12) It is easy to see that the invariant observer is much more meaningful, especially if the L′ is are chosen 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. To be convinced, consider the following simple case : suppose ˆθ = θ = ˆx = x = 0 remain fixed. We have d dt(ˆpi −pi) = Li(ˆpi −pi). Choosing Li = −k I yields d dt∥ˆpi −pi∥2 = −k∥ˆpi −pi∥2 leading to a correct estimation of landmark pi. Now suppose that the vehicle has changed its orientation and ˆθ = θ = π/2. The output error is now R−π/2(ˆpi −pi). With an observer of the form (11) the same choice Li = −k I yields d dt∥ˆpi −pi∥= 0 and the landmark is not correctly estimated. On the other hand, 6 with (12) we have in both cases d dt∥ˆpi −pi∥2 = −k∥ˆpi −pi∥2 ensuring convergence of ˆp towards p. Constant gains is a special (simple) choice, but the observer gains can also be tuned via Kalman equations. Indeed on can define noises on the linearized invariant error system and tune the Li’s via Kalman equations (see Invariant EKF method [2]). To sum up, any Luenberger observer or EKF can be invariantized via equations (12). This yields in the author’s opinion a much more meaningful non-linear observer that is well- adapted to the problem’s structure. The invariantized observer (12) is simply a version of (11) which is less sensitive to change of coordinates, and even if no proof can support this claim we believe it can only improve the performances of (11). 5 Particular case where the state space coincides with its symmetry group Over the last half decade, invariant observers on Lie groups for low-cost aided inertial navigation have been studied by several teams in the world, [11, 3, 15] to name a few. Several powerful convergence results have been obtained. They are all linked to the special properties of the invariant state error on a Lie group. To recap briefly the construction of invariant observers on Lie groups [4], we assume that the symmetry group G is a matrix group, and that X = G. The system is assumed to be invariant to left multiplications i.e. d dtX = XΩ(t). We have indeed for any g ∈G that d dt(gX) = (gX)Ω. For instance the motion of the vehicle in the considered SLAM problem ˙x = uRθe1, ˙θ = uv can be viewed as a left-invariant system on the Lie group SE(2) via the matrix representation X =  Rθ x 01×2 1  , Ω=  ωx ue1 01×2 0  , with ωx =  0 −uv uv 0  Suppose the output y = h(X) is equivariant, i.e. there exists a group action on the output space such that h(gX) = ρg(X). In this case the invariant observer (9) can be written intrinsically d dt ˆX = ˆ XΩ+ ˆ XL(ρ ˆ X−1(y)). with L(e) = 0 where e is the group identity element. The invariant state error is the natural group difference η = X−1 ˆ X and the error equation is d dtη = [Ω, η] + ηL ◦h(η−1) A remarkable fact is that the error equation only depends on η and Ω, whereas the system is non-linear and the error should also depend on ˆ X (think about the EKF which is based on a linearization around any ˆ X at each time). Moreover, if Ω= cst, the error equation is clearly autonomous. Thus the motion primitives generated by constant Ωare special trajectories called “permanent trajectories”. Around such trajectories one can always achieve local convergence (as soon as the linearized system is observable). It is worth noting this property was recently used to derive a non-linear separation principle on Lie groups [5]. It applies to some cart-like underactuated vehicles and some underwater or aerial fully actuated vehicles. An even more interesting case occurs when the output satisfies right-equivariance i.e., h(Xg) = ρg(h(X)). In this case we let the input be u = Ωand we consider the action of G by right multiplication, i.e. ϕg(X) = Xg and ψg(Ω) = g−1Ωg. The output is equivariant as h(ϕg(X)) = ρg ◦h(X). The invariant observer associated with this group of symmetry writes d dt ˆ X = ˆ XΩ+ L(ρ ˆ X−1(y)) ˆX. The invariant state error is η = ˆXX−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 ! In particular the linearized system around any trajectory is the same time-invariant system. Autonomy is the key for numerous powerful convergence results for observers on Lie groups see e.g. [10, 15, 4]. 6 A new result in EKF SLAM In this section we propose a new non-linear observer for EKF SLAM with guaranteed convergence properties. In the SLAM problem the state space is much bigger than its symmetry group. The orbits have dimension 3 and thus there are N + 1 −3 + 2 = N invariants (dimension of the cross-section, see Fig.1). Thus an autonomous error equation seems to be out of reach. Suprinsingly considering the symmetry group of rotations and translations in the vehicle frame yields such a result. A simple trick makes it obvious. Consider the following matrix representation: X =  Rθ x 01×2 1  , Pi =  Rθ pi 01×2 1  , Ω=  ωx ue1 01×2 0  , Ωi =  ωx 0 01×2 0  The equations of the system (10) can be written d dtX = XΩ, d dtPi = PiΩi, 1 ≤i ≤N and the system can be viewed as a left-invariant dynamics system on the (huge) Lie group G × · · · × G. Let ηx = ˆ XX−1, ηi = ˆPiP −1 i be the invariant state error. The system has the invariant output errors ˜Yi = Rˆθ(ˆzi −zi), i.e. ˜Yi 1 T = (ηi −ηx)H for 1 ≤i ≤N where H = 01×2 1 T . Consider the following invariant observer d dt ˆ X = ˆXΩ+ LX( ˜Y1, · · · , ˜YN) ˆ X, d dt ˆPi = ˆPiΩi + Li( ˜Y1, · · · , ˜YN) ˆPi. From (13), the (non-linear) error equation is completely autonomous reminding the linear case (3). It implies the following global convergence result for the non-linear deterministic system: Proposition 3 Consider the SLAM problem (10) without noise. The following observer d dt ˆθ = uv, d dt ˆx = uRˆθe1, d dt ˆpi = ki Rˆθ(ˆzi −zi) with ki > 0 is such that d dt(Rˆθ(ˆzi −zi)) = −ki Rˆθ(ˆzi −zi), i.e., all the estimation errors (ˆzi−zi), 1 ≤i ≤N converge globally exponentially to zero with rate ki, which means the vehicle trajectory and the map are correctly identified. The parameter ki must be tuned according to the level of noise associated to landmark i, and vehicle sensors’ noise. If one wants to define noise covariance matrices M, N to tune the observer (and compute an estimation P of the covariance error matrix at each time), it is also possible to define a modified EKF with guaranteed convergence properties: Proposition 4 Consider the SLAM problem (10). Let E = (Rˆθ(ˆzi −zi))1≤i≤N be the invariant output error. Let e3 be the vertical axis. Consider the observer d dt ˆθ = uv + Lθ(E), d dt ˆx = uRˆθe1 + Lθ(E)e3 ∧ˆx + Lx(E), d dt ˆpi = Lθ(E)e3 ∧ˆp + Li(E) Let η = (˜θ, ˜x, ˜p1, · · · , ˜pn) be the invariant state error where ˜θ = ˆθ−θ, ˜x = ˆx−R˜θx, ˜pi = ˆpi−R˜θpi. The state error equation is autonomous, i.e. d dtη only depends on η. It is thus completely independent of the trajectory and of u(t), v(t). The linearized error equation writes d dtδη = (LC)δη where L can be freely chosen and C is a fixed matrix. As in the usual EKF method, one can define covariance matries M, N, build a Kalman filter for the linearized system, i.e. tune L via the usual equations (7) i.e. ˙P = M −PCT N −1CP, L = PCT N −1, and implement it on the non-linear model. All the convergence results on P and L valid for stationnary systems (1) with A = 0, B = 0, D = 0 apply. Simulations (Fig. 3) with one landmark and noisy measurements indicate the mod- ified EKF (IEKF) behaves very similarly, or slightly better than the EKF, but the gain matrix tends quickly to a fixed matrix L independently from the trajectory and the 8 inputs u, v. So the Invariant EKF proposed in this paper 1- is incomparably cheaper computationaly as it relies on a constant matrix L that can be computed offline once and for all (the number of landmarks can thus be much increased) 2- is such that the linearized error system is stable as soon as LC has negative eigenvalues, which is easy to verify. Remark 1 The calculations above are valid on SE(3) and the results apply to 6 DOF SLAM. References [1] N. Aghannan and P. Rouchon. On invariant asymptotic observers. In 41st IEEE Conference on Decision and Control, pages 1479–1484, 2002. [2] S. Bonnabel, P. Martin, and E. Salaun. Invariant extended kalman filter: Theory and application to a velocity-aided attitude estimation problem. In IEEE Confer- ence on Decision and Control, 2009. [3] S. Bonnabel, Ph. Martin, and P. Rouchon. Symmetry-preserving observers. IEEE Trans. on Automatic Control, 53(11):2514–2526, 2008. [4] S. Bonnabel, Ph. Martin, and P. Rouchon. Non-linear symmetry-preserving ob- servers on lie groups. IEEE Trans. on Automatic Control, 54(7):1709 – 1713, 2009. [5] S. Bonnabel, Ph. Martin, P. Rouchon, and E. Salaun. A separation principle on lie groups. In IFAC (available on Arxiv), 2011. [6] S. Bonnabel and R. Sepulchre. Contraction and observer design on cones. Arxiv, 2011. [7] F. Bullo and R.M. Murray. Tracking for fully actuated mechanical systems: A geometric framework. Automatica, 35(1):17–34, 1999. [8] G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark, and M. Csobra. A solution to the simultaneous localisation and mapping (slam) problem. IEEE Trans. Robot. Automat., 17:229–241, 2001. [9] J.W. Grizzle and S.I. Marcus. The structure of nonlinear systems possessing sym- metries. IEEE Trans. Automat. Control, 30:248–258, 1985. [10] C. Lagemann, J. Trumpf, and R. Mahony. Gradient-like observers for invariant dynamics on a lie group. IEEE Trans. on Automatic Control, 55:2:367 – 377, 2010. [11] R. Mahony, T. Hamel, and J-M Pflimlin. Nonlinear complementary filters on the special orthogonal group. IEEE-Trans. on Automatic Control, 53(5):1203–1218, 2008. [12] Ph. Martin, P. Rouchon, and J. Rudolph. Invariant tracking. ESAIM: Control, Optimisation and Calculus of Variations, 10:1–13, 2004. [13] P. Morin and C. Samson. Practical stabilization of driftless systems on lie groups, the transverse function approach. IEEE Trans. Automat. Control, 48:1493–1508, 2003. [14] P. J. Olver. Classical Invariant Theory. Cambridge University Press, 1999. [15] J.F. Vasconcelos, R. Cunha, C. Silvestre, and P. Oliveira. A nonlinear position and attitude observer on se(3) using landmark measurements. Systems Control Letters, 59:155–166, 2010. 9 Figure 3: Simulations with one landmark and a car moving over a circular path with a 20% noise. Up: 1-Estimated vehicle trajectory (plain blue line) and landmark position (dashed green line) with Invariant EKF, 2-Estimation with the usual EKF, 3- true vehicle trajectory (plain blue line) and landmark position (green cross). After a short transient, the trajectory is correctly identified for both observers (up to a rotation-translation). Bottom : 1-coefficients of L(t) over time for Invariant EKF, 2-coefficients of L(t) for EKF. Wee see the EKF gain matrix is permanently adapting to the motion of the car (right) whereas its invariant counterpart (left) is directly expressed in well-adapted variables. 10 arXiv:1105.2254v1 [math.OC] 11 May 2011 Symmetries in observer design: review of some recent results and applications to EKF-based SLAM Silv`ere Bonnabel∗ November 6, 2018 Abstract In this paper, we first review the theory of symmetry-preserving observers and we mention some recent results. Then, we apply the theory to Extended Kalman Filter- based Simultaneous Localization and Mapping (EKF SLAM). It allows to derive a new (symmetry-preserving) Extended Kalman Filter for the non-linear SLAM problem that possesses convergence properties. We also prove a special choice of the gains ensures global exponential convergence. 1 Introduction Symmetries and Lie groups have been widely used for feedback control in robotics, see e.g. [7, 13]. More generally control of systems possessing symmetries has also been studied for quite a long time, see e.g. [9, 12]. The use of symmetries and Lie groups for observer design is more recent [1, 3]. The main properties 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 be particularly simple [4]. This property has been used to derive non-linear observers with (almost) global convergence properties for several localisation problems [11, 4, 15]. Recently [5] established a link between observer design and control of systems on Lie group by proving a non-linear separation principle on Lie groups. This paper proposes to recap the main elements of the theory along with some recent results, and to apply it to the domain of Extended Kalman Filter-based Simultaneous Localization and Mapping (EKF SLAM). It is organized as follows: Section 2 is a brief recap on linear observers. In Section 3 we recap the theory of symmetry-preserving observers [3] and mention some recent results [5, 6]. In Section 4 we apply it in a straightforward way to EKF SLAM. In Section 5 some results for the special case of observers for invariant systems on Lie groups [4] are recalled. In Section 6, it is proved that those results can be (surprisingly) applied to EKF SLAM. We derive a simple globally convergent observer for the non-linear problem. We also propose a modified EKF such that the covariance matrix and the gain matrix behave as if the system was linear and time-invariant. Such non-linear convergence guarantees for EKF SLAM are new to the author’s knowledge. The author would like to mention and to thank his regular co-authors on the subject of symmetry-preserving observers : Philippe Martin, Pierre Rouchon, and Erwan Sala¨un. ∗Silv`ere Bonnabel, Centre de Robotique, Math´ematiques et Syst`emes, Mines ParisTech, 60 bd Saint- Michel, 75272 Paris cedex 06. silvere.bonnabel@mines-paristech.fr 1 2 Luenberger observers, extended Kalman filter 2.1 Observers for linear systems Observers are meant to compute an estimation of the state of a dynamical system from several sensor measurements. Let x ∈Rn denote the state of the system, u ∈Rm be the inputs (a set of m known scalar variables such as controls, constant parameters, etc.). We assume the sensors provide measurements y ∈Rp that can be expressed as a function of the state and the inputs. When the underlying dynamical model is a linear differential equation, and the output is a linear function as well, the system can be written d dtx = Ax + Bu, y = Cx + Du. (1) A Luenberger observer (or Kalman filter) writes d dt ˆx = Aˆx + Bu −L · (Cˆx + Du −y), (2) where ˆx is the estimated state, and L is a gain matrix that can be freely chosen. We see that the observer consists in a copy of the system dynamics Aˆx + Bu, plus a cor- rection term L(Cˆx + Du −y) that “corrects” the trusted dynamics in function of the discrepancy between the estimated output ˆy = Cˆx + Du and the measured output y. One important issue is the choice (or “tuning”) of the gain matrix L. The Lu- enberger observer is based on a choice of a fixed matrix L. In the Kalman filter two positive definite matrices M and N denote the covariance matrices of the state noise and measurement noise, and L relies on a Ricatti equation : L = PCT N, where d dtP = AP + PAT + M −1 −PCT NCP. As M and N must be defined by the user, they can be viewed as tuning matrices. In both cases the observer has the form (2) with L constant or not. Let ˜x = ˆx−x be the estimation error, and let us compute the differential equation satisfied by the error. We have d dt ˜x = (A + LC)˜x. (3) As the goal of the observer is to find an estimate of x, we want ˜x to go to zero. When the system is observable, one can always find L such that ˜x asymptotically exponentially goes to zero, and the negative real part of the eigenvalues of A + LC can be freely assigned. We see that the theory is particularly simple as the error equation (3) is autonomous, i.e. it does not depend on the trajectory followed by the system. In particular, the input term u has vanished in (3). The well-known separation principle stems from this fact. 2.2 Some popular extensions to nonlinear systems Consider a general nonlinear system d dtx = f(x, u), y = h(x, u), (4) where x ∈X ⊂Rn is the state, u ∈U ⊂Rm the input, and y ∈Y ⊂Rp the output. Mimicking the linear case, a class of popular nonlinear observers writes d dt ˆx = f(ˆx, u) −L(ˆx, y, t) · h(ˆx, u) −y(t)  , (5) where the gain matrix can depend on the variables ˆx, y, t. The error equation can still be computed, but as the system is nonlinear, it does not necessarily lead to an appropriate gain matrix L. Indeed we have 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 equation is no longer autonomous, and the problem of finding L such that ˜x goes asymptotically to zero can not be solved in the general case. The most popular observer for nonlinear systems is the Extended Kalman Filter (EKF). The principle is to linearize the system around the estimated trajectory, build a Kalman filter for the linear model, and implement it on the nonlinear system. The EKF has the form (5), where the gain matrix is computed the following way: A = ∂f ∂x(ˆx, u) C = ∂h ∂x(ˆx, u) (6) L = PCT N −1 d dtP = AP + PAT + M −PCT N −1CP. (7) The EKF has two main flaws when compared to the KF for time-invariant linear systems. First the linearized system around any trajectory is generally time-varying and the covariance matrix does not tend to a fixed value. Then, when ˆx−x is large the linearized error equation can be a very erroneous approximation of the true error equation. 3 Symmetry-preserving observers 3.1 Symmetry group of a system of differential equations Let G be a group, and M be a set. A group action can be defined on M if to any g ∈G on can associate a diffeomorphic transformation φg : M →M such that φgh = φg ◦φh, and (φg)−1 = φg−1, i.e., the group multiplication corresponds to the transformation composition, and the reciprocal elements correspond to reciprocal transformations. Definition 1 G is a symmetry group of a system of differential equations defined on M if it maps solutions to solutions. In this case we say the system is invariant. Definition 2 A vector field w on M is said invariant if the system d dtz = w(z) is invariant. Definition 3 A scalar invariant is a function I : M →R such that I(φg(z)) = I(z) for all g ∈G. Salar invariants and invariant vector fields can be built via Cartan’s moving frame method [14]. Definition 4 A moving frame is a function γ : M →G such that γ(φg(z)) = g · γ(z) for all g, z. Suppose dim G = r ≤dim M. Under some mild assumptions on the action (free, regular) there exists locally a moving frame. The sets Oz = {φg(z), g ∈G} are called the group orbits. Let K be a cross-section to the orbits. A moving frame can be built locally via implicit functions theorem as the solution g = γ(z) of the equation φg(z) = k where k ∈Oz ∩K. A complete set of functionnaly independent invariants is given by the non-constant components of φγ(z)(z). Figure 1 illustrates those definitions and the moving frame method. 3.2 Symmetry group of an observer Consider the general system (4). Consider also the local 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 dtx = f(x, u) is said invariant if it is invariant to the group action (8). 3 Figure 1: An illustrative example. M = R2, and the symmetry group is made of horizontal translations. We have φg(z1, z2) = (z1 + g, z2)T where g ∈G = R. In local rectifying coordinates, every invariant system can be represented by a similar figure (under mild assumptions on the group action). Left: Invariant system. The symmetry group maps each integral line of the vector field into another integral line. Right: Moving frame method. K is a cross-section to the orbits and γ(z1, z2) is the group element that maps (z1, z2) to K along the orbit. For exemple if K is the set {z1 ≡0}, the moving frame is γ(z1, z2) = z1 and a complete set of invariants is I(z1, z2) = z2. The group maps solutions to solutions if we have d dtX = f(X, U), where (X, U) = (ϕg(x), ψg(u)) for all g ∈G. We understand from this definition, that u can denote the control variables as usual, but it also denotes every feature of the environment that makes the system not behave the same way after it has been transformed (via ϕg). The action of ψg is meant to allow some features of the environment to be also moved over. We would like the observer to be an invariant system for the same symmetry group. Definition 5 The observer (5) is invariant or “symmetry-preserving” if it is an invari- ant system for the group action (ˆx, x, u, y) 7→ ϕg(x), ϕg(ˆx), ψg(u), h(ϕg(x), ψg(u))  . In this case, the structure of the observer mimicks the nonlinear structure of the system. Let us recall how to build such observers (see [3] for more details). To do so, we need the output to be equivariant: Definition 6 The output is equivariant if there exists a group action on the output space (via ρg) such that h(ϕg(x), ψg(u)) = ρg(h(x, u)) for all g, x, u. We will systematically assume the output is equivariant. Let us define an invariant output error, instead of the usual linear output error ˆy −y: Definition 7 The smooth map (ˆx, u, y) 7→E(ˆx, u, y) ∈Rp is an invariant output error if • E ϕg(ˆx), ψg(u), ρg(y)  = E(ˆx, u, y) for all ˆx, u, y (invariant) • the map y 7→E(ˆx, u, y) is invertible for all ˆx, u (output) • E ˆx, u, h(ˆx, u)  = 0 for all ˆx, u (error) An invariant error is given (locally) by E(ˆx, u, y) = ργ(ˆx,u)(y) −ργ(ˆx,u)(ˆy). Finally, an invariant frame (w1, ..., wn) on X , which is a set of n linearly point-wise independent invariant vector fields, i.e (w1(x), ..., wn(x)) is a basis of the tangent space to X at x. Once again such a frame can be built (locally) via the moving frame method. Proposition 2 [3] The system d dt ˆx = F(ˆx, u, y) is an invariant observer for the invari- ant system d dtx = f(x, u) if and only if: F(ˆx, u, y) = f(ˆx, u) + n X i=1 Li I(ˆx, u), E(ˆx, u, y)  wi(ˆx) (9) 4 Figure 2: Vehicle taking relative measurements to environmental landmarks. where E is an invariant output error, I(ˆx, u) is a complete set of scalar invariants, the Li’s are smooth functions such that for all ˆx, Li I(ˆx, u), 0  = 0, and (w1, ..., wn) is an invariant frame. The gains Li must be tuned in order to get some convergence properties if possible, and their magnitude should depend on the trade-offbetween measurement noise and convergence speed. The convergence analysis of the observer often relies on an invariant state-error: Definition 8 The smooth map (ˆx, x) 7→η(ˆx, x) ∈Rn is an invariant state error if η(ϕg(ˆx), ϕg(x)) = η(ˆx, x) (invariant), the map x 7→η(ˆx, x) is invertible for all ˆx (state), and η(x, x) = 0 (error). 3.3 An example: symmetry-preserving observers for posi- tive linear systems The linear system d dtx = Ax, y = Cx admits scalings G = R∗as a symmetry group via the group action φg(x) = gx. Every linear observer is obviously an invariant observer. The unit sphere is a cross-section K to the orbits. A moving frame maps the orbits to the sphere and thus writes γ(x) = 1/∥x∥∈G. A complete set of invariants is given locally by n −1 independent coordinates of φγ(x)(x) = x/∥x∥. Let I(x) ∈Rn−1 be a complete set of independent invariants. I(x) and ∥x∥provide alternative coordinates named base and fiber coordinates. Moreover the system has a nice triangular structure in those coordinates. One can prove that d dtI(x(t)) is an invariant function and thus it is necessarily of the form g(I). As a result we have d dtI(x) = g(I(x)) which does not depend on ∥x∥. We have thus the following (general) result : if the restriction of the vector field on the cross-section is a contraction, it suffices to define a reduced observer on the orbits i.e. in our case a norm observer (which means that a scalar output suffices for observability). This is the case for instance when A is a matrix whose coefficients are stricly positive (according to the Perron-Froebenius theorem). This fact was recently used in [6] to derive invariant asymptotic positive observers for positive linear systems. 4 A first application to EKF SLAM Simultaneous localisation and mapping (SLAM) addresses the 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 twenty years as a 5 “holy grail” in the robotics community since it would be a means to make a robot truly autonomous in an unknown environment. A very well-known approach that appeared in the early 2000’s is the EKF SLAM [8]. Its main advantage is to formulate the problem in the form of a state-space model with additive Gaussian noise and to provide convergence properties in the linear case (i.e. straight line motion). Indeed, the key idea is to include the position of the several landmarks (i.e. the map) in the state space. This solution has been gradually replaced by other techniques such as FastSLAM, Graph SLAM etc. In the framework of EKF SLAM, the problem of estimating online the trajectory of the robot as well as the location of all landmarks without the need for any a priori knowledge of location can be formulated as follows [8]. The vehicle state is defined by the position in the reference frame (earth-fixed frame) x ∈R2 of the centre of the rear axle and the orientation of the vehicle axis θ. The vehicle trusted motion relies on non-holonomic constraints. The landmarks are modeled as points and represented by their position in the reference frame pi ∈R2 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θe1, ˙θ = uv, ˙pi = 0 1 ≤i ≤N (10) where e1 = (1, 0)T and Rθ is the rotation matrix of angle θ. Supposing that the data association between landmarks from one instant to the next is correctly done, the ob- servation model for the i-th landmark (disregarding measurement noise) is its position seen from the vehicle’s frame: zi = R−θ(pi −x). The standard EKF SLAM estimator has the form d dt ˆx = uRˆθe1 + Lk x(ˆzk −zk), d dt ˆθ = uv + N X 1 Lk θ(ˆzk −zk), d dt ˆpi = N X 1 Lk i (ˆzk −zk), 1 ≤i ≤N (11) where ˆzi = R−ˆθ(ˆpi−ˆx) and where the Li’s are the lines of L tuned via the EKF equations (6)-(7). Here the group of symmetry of the system corresponds to Galilean invariances, and it is made of rotations and translations of the plane SE(2). Indeed, looking at Figure 2, it is obvious that the equations of motion are the same whether the first horizontal axis of the reference frame is pointing North, or East, or in any direction. For g = (x0, θ0) ∈SE(2), the action of the group on the state space is ϕg(x, θ, pi) = (Rθ0x+x0, θ +θ0, Rθ0pi +x0) and ψg(u, v) = u, v. The output is also unchanged by the group transformation as it is expressed in the vehicle frame and is thus insensitive to rotations and translations of the reference frame. Applying the theory of the last section, the observer above can be “invariantized”, yielding the following invariant observer: d dt ˆx = uRˆθe1 + Rˆθ( N X 1 Lk x(ˆzk −zk)), d dt ˆθ = uv + N X 1 Lk θ(ˆzk −zk), d dt ˆpi = Rˆθ( N X 1 Lk i (ˆzk −zk)) (12) It is easy to see that the invariant observer is much more meaningful, especially if the L′ is are chosen 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. To be convinced, consider the following simple case : suppose ˆθ = θ = ˆx = x = 0 remain fixed. We have d dt(ˆpi −pi) = Li(ˆpi −pi). Choosing Li = −k I yields d dt∥ˆpi −pi∥2 = −k∥ˆpi −pi∥2 leading to a correct estimation of landmark pi. Now suppose that the vehicle has changed its orientation and ˆθ = θ = π/2. The output error is now R−π/2(ˆpi −pi). With an observer of the form (11) the same choice Li = −k I yields d dt∥ˆpi −pi∥= 0 and the landmark is not correctly estimated. On the other hand, 6 with (12) we have in both cases d dt∥ˆpi −pi∥2 = −k∥ˆpi −pi∥2 ensuring convergence of ˆp towards p. Constant gains is a special (simple) choice, but the observer gains can also be tuned via Kalman equations. Indeed on can define noises on the linearized invariant error system and tune the Li’s via Kalman equations (see Invariant EKF method [2]). To sum up, any Luenberger observer or EKF can be invariantized via equations (12). This yields in the author’s opinion a much more meaningful non-linear observer that is well- adapted to the problem’s structure. The invariantized observer (12) is simply a version of (11) which is less sensitive to change of coordinates, and even if no proof can support this claim we believe it can only improve the performances of (11). 5 Particular case where the state space coincides with its symmetry group Over the last half decade, invariant observers on Lie groups for low-cost aided inertial navigation have been studied by several teams in the world, [11, 3, 15] to name a few. Several powerful convergence results have been obtained. They are all linked to the special properties of the invariant state error on a Lie group. To recap briefly the construction of invariant observers on Lie groups [4], we assume that the symmetry group G is a matrix group, and that X = G. The system is assumed to be invariant to left multiplications i.e. d dtX = XΩ(t). We have indeed for any g ∈G that d dt(gX) = (gX)Ω. For instance the motion of the vehicle in the considered SLAM problem ˙x = uRθe1, ˙θ = uv can be viewed as a left-invariant system on the Lie group SE(2) via the matrix representation X =  Rθ x 01×2 1  , Ω=  ωx ue1 01×2 0  , with ωx =  0 −uv uv 0  Suppose the output y = h(X) is equivariant, i.e. there exists a group action on the output space such that h(gX) = ρg(X). In this case the invariant observer (9) can be written intrinsically d dt ˆX = ˆ XΩ+ ˆ XL(ρ ˆ X−1(y)). with L(e) = 0 where e is the group identity element. The invariant state error is the natural group difference η = X−1 ˆ X and the error equation is d dtη = [Ω, η] + ηL ◦h(η−1) A remarkable fact is that the error equation only depends on η and Ω, whereas the system is non-linear and the error should also depend on ˆ X (think about the EKF which is based on a linearization around any ˆ X at each time). Moreover, if Ω= cst, the error equation is clearly autonomous. Thus the motion primitives generated by constant Ωare special trajectories called “permanent trajectories”. Around such trajectories one can always achieve local convergence (as soon as the linearized system is observable). It is worth noting this property was recently used to derive a non-linear separation principle on Lie groups [5]. It applies to some cart-like underactuated vehicles and some underwater or aerial fully actuated vehicles. An even more interesting case occurs when the output satisfies right-equivariance i.e., h(Xg) = ρg(h(X)). In this case we let the input be u = Ωand we consider the action of G by right multiplication, i.e. ϕg(X) = Xg and ψg(Ω) = g−1Ωg. The output is equivariant as h(ϕg(X)) = ρg ◦h(X). The invariant observer associated with this group of symmetry writes d dt ˆ X = ˆ XΩ+ L(ρ ˆ X−1(y)) ˆX. The invariant state error is η = ˆXX−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 ! In particular the linearized system around any trajectory is the same time-invariant system. Autonomy is the key for numerous powerful convergence results for observers on Lie groups see e.g. [10, 15, 4]. 6 A new result in EKF SLAM In this section we propose a new non-linear observer for EKF SLAM with guaranteed convergence properties. In the SLAM problem the state space is much bigger than its symmetry group. The orbits have dimension 3 and thus there are N + 1 −3 + 2 = N invariants (dimension of the cross-section, see Fig.1). Thus an autonomous error equation seems to be out of reach. Suprinsingly considering the symmetry group of rotations and translations in the vehicle frame yields such a result. A simple trick makes it obvious. Consider the following matrix representation: X =  Rθ x 01×2 1  , Pi =  Rθ pi 01×2 1  , Ω=  ωx ue1 01×2 0  , Ωi =  ωx 0 01×2 0  The equations of the system (10) can be written d dtX = XΩ, d dtPi = PiΩi, 1 ≤i ≤N and the system can be viewed as a left-invariant dynamics system on the (huge) Lie group G × · · · × G. Let ηx = ˆ XX−1, ηi = ˆPiP −1 i be the invariant state error. The system has the invariant output errors ˜Yi = Rˆθ(ˆzi −zi), i.e. ˜Yi 1 T = (ηi −ηx)H for 1 ≤i ≤N where H = 01×2 1 T . Consider the following invariant observer d dt ˆ X = ˆXΩ+ LX( ˜Y1, · · · , ˜YN) ˆ X, d dt ˆPi = ˆPiΩi + Li( ˜Y1, · · · , ˜YN) ˆPi. From (13), the (non-linear) error equation is completely autonomous reminding the linear case (3). It implies the following global convergence result for the non-linear deterministic system: Proposition 3 Consider the SLAM problem (10) without noise. The following observer d dt ˆθ = uv, d dt ˆx = uRˆθe1, d dt ˆpi = ki Rˆθ(ˆzi −zi) with ki > 0 is such that d dt(Rˆθ(ˆzi −zi)) = −ki Rˆθ(ˆzi −zi), i.e., all the estimation errors (ˆzi−zi), 1 ≤i ≤N converge globally exponentially to zero with rate ki, which means the vehicle trajectory and the map are correctly identified. The parameter ki must be tuned according to the level of noise associated to landmark i, and vehicle sensors’ noise. If one wants to define noise covariance matrices M, N to tune the observer (and compute an estimation P of the covariance error matrix at each time), it is also possible to define a modified EKF with guaranteed convergence properties: Proposition 4 Consider the SLAM problem (10). Let E = (Rˆθ(ˆzi −zi))1≤i≤N be the invariant output error. Let e3 be the vertical axis. Consider the observer d dt ˆθ = uv + Lθ(E), d dt ˆx = uRˆθe1 + Lθ(E)e3 ∧ˆx + Lx(E), d dt ˆpi = Lθ(E)e3 ∧ˆp + Li(E) Let η = (˜θ, ˜x, ˜p1, · · · , ˜pn) be the invariant state error where ˜θ = ˆθ−θ, ˜x = ˆx−R˜θx, ˜pi = ˆpi−R˜θpi. The state error equation is autonomous, i.e. d dtη only depends on η. It is thus completely independent of the trajectory and of u(t), v(t). The linearized error equation writes d dtδη = (LC)δη where L can be freely chosen and C is a fixed matrix. As in the usual EKF method, one can define covariance matries M, N, build a Kalman filter for the linearized system, i.e. tune L via the usual equations (7) i.e. ˙P = M −PCT N −1CP, L = PCT N −1, and implement it on the non-linear model. All the convergence results on P and L valid for stationnary systems (1) with A = 0, B = 0, D = 0 apply. Simulations (Fig. 3) with one landmark and noisy measurements indicate the mod- ified EKF (IEKF) behaves very similarly, or slightly better than the EKF, but the gain matrix tends quickly to a fixed matrix L independently from the trajectory and the 8 inputs u, v. So the Invariant EKF proposed in this paper 1- is incomparably cheaper computationaly as it relies on a constant matrix L that can be computed offline once and for all (the number of landmarks can thus be much increased) 2- is such that the linearized error system is stable as soon as LC has negative eigenvalues, which is easy to verify. Remark 1 The calculations above are valid on SE(3) and the results apply to 6 DOF SLAM. References [1] N. Aghannan and P. Rouchon. On invariant asymptotic observers. In 41st IEEE Conference on Decision and Control, pages 1479–1484, 2002. [2] S. Bonnabel, P. Martin, and E. Salaun. Invariant extended kalman filter: Theory and application to a velocity-aided attitude estimation problem. In IEEE Confer- ence on Decision and Control, 2009. [3] S. Bonnabel, Ph. Martin, and P. Rouchon. Symmetry-preserving observers. IEEE Trans. on Automatic Control, 53(11):2514–2526, 2008. [4] S. Bonnabel, Ph. Martin, and P. Rouchon. Non-linear symmetry-preserving ob- servers on lie groups. IEEE Trans. on Automatic Control, 54(7):1709 – 1713, 2009. [5] S. Bonnabel, Ph. Martin, P. Rouchon, and E. Salaun. A separation principle on lie groups. In IFAC (available on Arxiv), 2011. [6] S. Bonnabel and R. Sepulchre. Contraction and observer design on cones. Arxiv, 2011. [7] F. Bullo and R.M. Murray. Tracking for fully actuated mechanical systems: A geometric framework. Automatica, 35(1):17–34, 1999. [8] G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark, and M. Csobra. A solution to the simultaneous localisation and mapping (slam) problem. IEEE Trans. Robot. Automat., 17:229–241, 2001. [9] J.W. Grizzle and S.I. Marcus. The structure of nonlinear systems possessing sym- metries. IEEE Trans. Automat. Control, 30:248–258, 1985. [10] C. Lagemann, J. Trumpf, and R. Mahony. Gradient-like observers for invariant dynamics on a lie group. IEEE Trans. on Automatic Control, 55:2:367 – 377, 2010. [11] R. Mahony, T. Hamel, and J-M Pflimlin. Nonlinear complementary filters on the special orthogonal group. IEEE-Trans. on Automatic Control, 53(5):1203–1218, 2008. [12] Ph. Martin, P. Rouchon, and J. Rudolph. Invariant tracking. ESAIM: Control, Optimisation and Calculus of Variations, 10:1–13, 2004. [13] P. Morin and C. Samson. Practical stabilization of driftless systems on lie groups, the transverse function approach. IEEE Trans. Automat. Control, 48:1493–1508, 2003. [14] P. J. Olver. Classical Invariant Theory. Cambridge University Press, 1999. [15] J.F. Vasconcelos, R. Cunha, C. Silvestre, and P. Oliveira. A nonlinear position and attitude observer on se(3) using landmark measurements. Systems Control Letters, 59:155–166, 2010. 9 Figure 3: Simulations with one landmark and a car moving over a circular path with a 20% noise. Up: 1-Estimated vehicle trajectory (plain blue line) and landmark position (dashed green line) with Invariant EKF, 2-Estimation with the usual EKF, 3- true vehicle trajectory (plain blue line) and landmark position (green cross). After a short transient, the trajectory is correctly identified for both observers (up to a rotation-translation). Bottom : 1-coefficients of L(t) over time for Invariant EKF, 2-coefficients of L(t) for EKF. Wee see the EKF gain matrix is permanently adapting to the motion of the car (right) whereas its invariant counterpart (left) is directly expressed in well-adapted variables. 10