Geometric Nonlinear Observer Design for SLAM on a Matrix Lie Group

Transcription

1 218 IEEE Conference on Decision and Control (CDC Miami Beach, FL, USA, Dec , 218 Geometric Nonlinear Observer Design for SLAM on a Matrix Lie Group Miaomiao Wang and Abdelhamid Tayebi Abstract This paper considers the Simultaneous Localization and Mapping (SLAM problem for a rigid body evolving in 3D space. We propose a nonlinear geometric observer, designed on the matrix Lie group SE 1+n(3, using group velocity and landmark measurements. The proposed observer is also extended to handle unknown biases in the linear and angular velocity. Simulation results are presented to illustrate the effectiveness of the proposed observer. I. INTRODUCTION The development of reliable navigation algorithms for autonomous vehicles evolving in unknown environments is instrumental in many applications, such as autonomous underwater vehicles and unmanned aerial vehicles. This is particularly important when the absolute positioning systems (e.g., Global ositioning Systems (GS are not available (e.g., indoor applications. The problem of building a map of an unknown environment while keeping track of the vehicle s location is refereed to as Simultaneous Localization and Mapping (SLAM problem. The SLAM problem has been widely studied in robotics over the past three decades [1], [2]. One of the most widely used techniques dealing with the SLAM problem is the extended Kalman filter (EKF. However, It is well known that this type of EKF-based SLAM techniques suffers from the consistency problem and the lack of global stability results [3], [4]. Considerable efforts have been made to improve the consistency and convergence of the EKF-based SLAM [4] [6]. In this context, a sensor-based SLAM, with global asymptotic stability results, has been proposed in [7]. An extension of the sensor-based SLAM filter to the motion in 3D space has been presented in [8]. Recently, more and more researchers recognize the importance of Lie group representations of orientation and/or pose, which take advantage of the symmetry structure of the motion space [9], [1]. In recent years, research on symmetry-preserving observers design on Lie groups has become very popular [11] [13]. One of the first examples of invariant EKF-based SLAM (IEFK-based SLAM, combining the symmetry-preserving approach and the EKF techniques, has been presented in [14]. An extension of the IEKF-based SLAM has been proposed in [15] to solve the inconsistency problem of classical EKF-based SLAM. The convergence and consistency properties of IEKF-based This work was supported by the National Sciences and Engineering Research Council of Canada (NSERC M. Wang and A. Tayebi are with the Department of Electrical and Computer Engineering, Western University, London, Ontario, Canada. A. Tayebi is also with the Department of Electrical Engineering, Lakehead University, Thunder Bay, Ontario, Canada. mwang448@uwo.ca, atayebi@lakeheadu.ca SLAM have been analyzed in [16]. However, only local convergence is guaranteed with this approaches due to the linearization of the dynamics of the intrinsic error around the group identity. To the authors best knowledge, very few publications are available in the literature that address the full SLAM problem from a Lie group perspective with strong convergence guarantees. More recently, an interesting approach, which models the landmarks and the pose of a rigid body in a single geometric structure, has been considered in [17]. Under this symmetric structure, a novel geometric nonlinear observer for SLAM has been derived by applying the techniques presented in [13]. Another gradientbased geometric approach for SLAM was proposed in [18], which generates the gradient-based innovation terms for the estimated pose and map separately. Motivated by [17], a geometric nonlinear observer for SLAM in terms of group velocity and landmark measurements has been proposed in this paper. A rigorous analysis of the global convergence has been provided. The proposed observer is also extended to handle linear and angular velocity-biases as in [18]. The presented paper differs from [17], [18] in many ways. First, the SLAM dynamics are formulated in the framework of a specific matrix Lie group denoted by SE 1+n (3. Second, the innovation terms are generated directly on the Lie group SE 1+n (3 using geometric techniques on matrix Lie groups. Finally, the problem of time-varying landmarks has been considered in the present paper. The remainder of this paper is organized as follows: Section II provides some preliminary notations and definitions that will be used throughout this paper. In Section III, we present a geometric nonlinear observer for SLAM on a matrix Lie group along with a rigorous stability analysis. In Section IV, the proposed observer is extended to the case where the measurements of the rotational and translational velocities are corrupted by unknown constant biases. Section V presents some simulation results showing the performance of our proposed approach. A. Notations II. RELIMINARY MATERIAL The sets of real, nonnegative real and natural numbers are denoted as R, R + and N, respectively. We denote by R n the n-dimensional Euclidean space. Given two matrices, A, B R m n, their Euclidean inner product is defined as A, B := tr(a B. The Euclidean norm of a vector x R n is defined as x := x x, and the Frobenius norm of a /18/$ IEEE 1488

2 matrix X R n m is given by X F := X, X. The n- by-n identity matrix is denoted by I n and the n-dimensional vector of zero elements is denoted by n. Let e 1,, e n be the standard basis vectors of R n, that is, e i has all entries equal to zero except for the i-th entry which is equal to 1. Consider the matrix Lie group SE 1+n (3 := SO(3 R 3 R 3 n R (4+n (4+n defined as follows [15]: SE 1+n (3 = {X = T (R, p, p R SO(3, p R 3, p R 3 n }, (1 with the map T : SO(3 R 3 R 3 n R (4+n (4+n defined as R p p T (R, p, p = n. n 3 n I n The inverse of X is given by X 1 = T (R, R p, R p SE 1+n (3. It is also clear that, for any X 1, X 2 SE 1+n (3, one has X 1 X1 1 = X1 1 X 1 = I n+4 and X 1 X 2 SE 1+n (3. The Lie algebra associated to the Lie group SE 1+n (3, denoted by se 1+n (3 := so(3 R 3 R 3 n R (4+n (4+n, is given by se 1+n (3 = { [ U = Ω v v (n+1 3 n+1 (n+1 n ] Ω so(3, v R 3, v R 3 n }. (2 The tangent space of the group SE 1+n (3 is defined as T X SE 1+n (3 := {XU X SE 1+n (3, U se 1+n (3}. Let, X : T X SE 1+n (3 T X SE 1+n (3 R be a Riemannian metric on SE 1+n (3, such that for all X SE 1+n (3, U 1, U 2 se 1+n (3 one has XU 1, XU 2 X := U 1, U 2. (3 Given a differentiable smooth function f : SE 1+n (3 R, the gradient of f, denoted by X f T X SE 1+n (3, relative to the Riemannian metric, X is uniquely defined by df XU = X f, XU X = X 1 X f, U, (4 Let be the vector cross-product on R 3 and define the map ( : R 3 so(3 such that x y = x y, for all x, y R 3. For any matrix A 1 R 3 3, define a (A 1 as the anti-symmetric projection of A 1, such that a (A 1 = (A 1 A 1 /2. Let : R (4+n (4+n se 1+n (3 denote the unique orthogonal projection of R (4+n (4+n onto se 1+n (3 with respect to the matrix inner product, that is, for all U se 1+n (3, A R (4+n (4+n, one has U, A = U, (A = (A, U, (5 and for all A 1 R 3 3, A 2, A 3 R 3 (n+1, A 4 R (n+1 (n+1, ([ ] [ ] A1 A 2 a (A A = 1 A 2. (6 3 A 4 (n+1 3 (n+1 (n+1 Let M denote the sub-manifold of R (4+n (4+n such that { [ ] M M := M = 1 M 2 (n+1 3 n+1 M 1 R 3 3, M 2 R 3 (n+1}. Then, for all M, M M and X SE 1+n (3, one has (XM = (X M, (7 tr(x XM M = tr(m M. (8 For any X SE 1+n (3, U se 1+n (3, the Adjoint action map Ad X : SE 1+n (3 se 1+n (3 se 1+n (3 is defined as Ad X U := XUX 1. (9 One can verify that Ad X1 Ad X2 U = Ad X1X 2 U, for all X 1, X 2 SE 1+n (3, U se 1+n (3. B. SLAM Kinematics and Measurements Let {I} be an inertial frame and {B} be a body-attached frame. Let R SO(3 and p R 3 be the rotation of frame {B} with respect to the inertial frame {I}, and position of the rigid-body expressed in the inertial frame {I}, respectively. Consider a family of n landmarks p i R 3, i = 1,, n, expressed in the inertial frame {I}. Define Ω so(3 and v R 3 as the angular velocity and linear velocity, respectively, expressed in frame {B}. Consider the following motion kinematics of a rigid-body and a family of n landmarks Ṙ = RΩ, (1 ṗ = Rv, (11 ṗ i = Rv i, i = 1, 2,, n, (12 where v i is the linear velocity of the i-th landmark expressed in the body frame {B}. In this work, we formulate the SLAM problem using the kinematics of an element of the matrix Lie group X = T (R, p, p SE 1+n (3 with p := [p 1,, p n ] R 3 n. From (1, the overall kinematics (1- (12 can be rewritten in the following compact left invariant form Ẋ = XU. (13 where U se 1+n (3 is defined in (2 with v = [v 1,, v n ]. Assume that the group velocity U is continuous, bounded, and available for measurement. Assume also that the landmarks are measured in the body frame {B}, and are given by y i = R (p i p, i = 1, 2,, n. (14 Let us introduce the following new vectors: r i := [ 3 1 e i ], b i := [yi 1 e i ] R n+4 for all i = 1,, n. From (14, one has the following compact equations: b i = h(x, r i := X 1 r i, i = 1, 2,, n. (15 Note that the Lie group action h : SE 1+n (3 R n+4 R n+4 is a right group action in the sense that for all X 1, X 2 SE 1+n (3 and r R n+4, one has h(x 2, h(x 1, r = h(x 1 X 2, r. 1489

3 III. GRADIENT-LIKE OBSERVER DESIGN WITHOUT VELOCITY-BIAS Let ˆR SO(3 and ˆp R 3 denote the estimates of the attitude R and the position p of the rigid body, respectively. Let ˆp i R 3 be the estimated position of the i-th landmark p i. Let us define the estimate of the state X as ˆX := T ( ˆR, ˆp, ˆp SE 1+n (3 with ˆp := [ˆp 1, ˆp 2,, ˆp n ]. Then, one has the estimation error X := X ˆX 1 = T ( R, p, p SE 1+n (3 with R := R ˆR, p := p Rˆp, p := [ p 1, p 2,, p n ] and p i := p i Rˆp i for all i = 1,, n. The following Lemmas (whose proofs are given in the Appendix will be useful in the remainder of this paper: Lemma 1: Consider the following smooth real-valued function: U(X = 1 2 tr((i XA(I X, (16 where A = A R (4+n (n+4. Then, for all X SE 1+n (3, the gradient of U with respect to X along the trajectories of Ẋ = XU is given by: X U = X((I X 1 A. (17 Lemma 2: Consider the definitions of r i, b i, i = 1,, n. Let A := i=1 k ir i ri. From (6 and (15, one has i=1 k i r i ˆXb i 2 = tr (((I XA(I X, (18 i ri = ((I X 1 A. (19 Moreover, letting ɛ i := ˆp i ˆp ˆRy i for all i = 1,, n, one has the following identities: i=1 k i r i ˆXb i 2 = i=1 k i ɛ i 2, (2 i ri [ 3 3 = i=1 k ] iɛ i ɛ, (21 (n+1 3 n+1 (n+1 n i ri ˆX [ Θa = i=1 k ˆR ] i ɛ i 3 n. (22 (n+1 3 n+1 (n+1 n ( ˆX where ɛ := [k 1 ɛ 1 k n ɛ n ] R 3 n and Θ a := 1 n 2 i=1 k i( ˆR ɛ i ˆRy i R 3 3. We consider the following nonlinear observer: ˆX = ˆX(U, (23 where se 1+n (3 is an innovation term to be designed. Using the fact that ˆX 1 ˆX = I, one has ˆX 1 = ˆX 1 Ẋ ˆX 1, which leads to the following error dynamics: X = XU ˆX 1 X ˆX 1 ˆX(U ˆX 1 = X(Ad ˆX. (24 Let us define the landmark estimation errors as η i := r i ˆXb i = [ɛ i 1 n ], i = 1,, n. One can verify that η i = ɛ i, and consequently, the convergence of ɛ i to zero implies the convergence of η i to zero, which in turn implies that ˆX 1 r i converges to b i. Let us introduce the following compact set: A := {(η 1,, η n R n+4 R n+4 η i =, i = 1,, n}. (25 Now, one can state our first result. Theorem 1: Consider the SLAM kinematics (13 with the system outputs (15, and the observer (23 with the following innovation term: := Ad ˆX 1 i ri, (26 with k i > for all i = 1,, n. Then, the set A is globally exponentially stable. Moreover, there exist a constant matrix R SO(3 and a constant vector p R 3 such that lim t R(t = R and lim t p(t = p. roof: In view of (9 and (19, the closed-loop system (24 can be rewritten as X = X ( (I X 1 A. (27 Consider the following Lyapunov function candidate: U( X = 1 n k i r i 2 ˆXb i 2 = 1 n k i ɛ i 2, (28 2 i=1 i=1 which is positive definite with respect to A. In view of (5 and (17, the time-derivative of U along the trajectories of (27 is given by U = XU, X((I X 1 A X = X 1 XU, ((I X 1 A = ((I X 1 A, ((I X 1 A = ((I X 1 A 2 F. In view of (21 in Lemma 2, one can further show that U = i=1 k iɛ i 2 i=1 k2 i ɛ i 2 λu (29 where λ := 2 min{k 1, k 2,, k n }. From (29, one has U(t e λt U(. It follows that ɛ i converges to zero exponentially for all i = 1,, n. Therefore, one can conclude that the set A is globally exponentially stable. Next, we are going to show the convergence of ( R, p. The closedloop system (27 can be expressed as follows: R = p = i=1 k Rɛ i i. (3 p i = k i Rɛi Since ɛ i = ˆp i ˆp ˆRy i = R ( p p i, it is clear that the closed-loop dynamics (3 are autonomous. Consequently, LaSalle s theorem can be applied. From U, it follows that ɛ i, i = 1,, n, and consequently p and p i. In view of R and p, it is clear that there exist constants R SO(3 and p R 3 such that ( R(t, p(t (R, p as t. This completes the proof. Remark 1: It is important to note that the SLAM problem is not observable [19]. The best one can achieve is that there exist some constants (R, p such that ( R(t, p(t (R, p and ˆp i (t (R (p p i (t as t. 149

4 IV. OBSERVER DESIGN WITH VELOCITY BIAS COMENSATION In this section, the previous observer will be extended to the case where the measurements Ω y so(3 and v y R 3 of the angular velocity Ω and linear velocity v contain unknown constant biases denoted as b Ω so(3 and b v R 3, respectively. That is Ω y = Ω + b Ω and v y = v + b v. Let M b denote the sub-manifold of R (n+4 (n+4 defined as M b := {b U = b (b Ω, b v b Ω so(3, b v R 3 }. (31 with the map b : so(3 R 3 R (n+4 (n+4 defined as [ ] bω b b (b Ω, b v := v 3 n. (n+1 3 n+1 (n+1 n Note that M b is also a subset of se 1+n (3. Define the diagonal matrix I = diag([1, 1, 1, 1, 1 n ], such that for all b U M b, A R (n+4 (n+4, one has (AI M b and A, b U = (A, b U = (AI, b U. (32 The group velocity measurement is given by U y := U + b U with b U := b (b Ω, b v M b. Let ˆb Ω and ˆb v be the estimates of b Ω and b v, respectively. The estimate of the velocity-bias b U is defined as ˆb U := b (ˆb Ω, ˆb v M b. Define the group velocity-bias error as bu := b U ˆb U = b ( b Ω, b v (33 where b Ω := b Ω ˆb Ω and b v := b v ˆb v. Define the following compact set: Ā = A {( b Ω, b v so(3 R 3 b Ω =, b v = }. (34 Assumption 1: Assume that among the n 3 measured landmarks, at least three landmarks define a plane. Remark 2: This assumption is needed to show the convergence of the bias estimation errors to zero. A similar assumption has been used in [8]. Now, one can state our second result. Theorem 2: Consider the SLAM kinematics (13 with the system outputs (15, and the following observer ˆX = ˆX(U y ˆb U ( ˆb U = ˆX i ri ˆX K, (35 ( n := Ad ˆX 1 i ri where ˆX( SE 1+n (3, ˆb U ( M b, k i >, i = 1,, n, and K := diag([k ω, k ω, k ω, k v, 1 n ] with k ω, k v >. Assume that X and U are bounded for all times. Then, the set Ā is globally asymptotically stable. Moreover, there exist a constant matrix R SO(3 and a constant vector p R 3 such that lim t R(t = R and lim t p(t = p. roof: Using the fact U y ˆb U = U + b U, it follows that bu = ˆbU and X = X(Ad ˆX( b U. (36 From (19 and (35, one has the following closed-loop system: X = X( ((I X 1 A Ad ˆX bu ( ˆX (I X 1 A ˆX K b U = Consider the following Lyapunov function candidate:. (37 V( X, b U = U( X tr( b U K b U, (38 with K := diag([1/k ω, 1/k ω, 1/k ω, 1/k v, 1 n ] such that K K = KK = I. In view of (33, V can be rewritten as V( X, b U = U( X + 1 2k ω b Ω 2 F + 1 2k v b v 2, which is positive definite with respect to Ā. In view of (5 and (17, the time-derivative of V along the trajectories of (37 is given by V = XU, X( ((I X 1 A Ad ˆX bu X + ( ˆX (I X 1 A ˆX KK, b U = ((I X 1 A, ( ((I X 1 A Ad ˆX bu + ( ˆX (I X 1 A ˆX I, b U, where we made use of the fact KK = I. In view of (32, one deduces V = ((I X 1 A 2 F (I X 1 A, ˆX b U ˆX 1 + ( ˆX (I X 1 A ˆX I, b U = ((I X 1 A 2 F ˆX (I X 1 A ˆX, b U + ( ˆX (I X 1 A ˆX I, b U. From (21 and (32, one can easily show that V = ((I X 1 A 2 F = i=1 k iɛ i 2 i=1 k2 i ɛ i 2. (39 Since V, it is clear that V is non-increasing along the trajectories of (37, which implies that b Ω, b v and ɛ i, i = 1,, n are bounded. Since the dynamics (37 are not autonomous, we will use Barbalat s lemma to show that V converges to zero. In view of (9, (22 and (33, closed-loop system (37 can be rewritten as R = R( ˆR b Ω ˆR, (4 p = R( ˆR b Ω ˆR ˆp ˆR b v i=1 k iɛ i, (41 p i = R( ˆR b Ω ˆR ˆp i k i ɛ i, i = 1, 2,, n, (42 b Ω = 1 2 k ω i=1 k ˆR i ( ( R (p p i ɛ i ˆR, (43 b v = k v i=1 k i ˆR ɛ i, (44 where we made use of the fact ˆRyi = R (p i p. In view of (4-(42, one has ɛ i = R ( p p i + R ( p p i = ˆR b Ω ˆR ɛ i + ˆR b Ω ˆR (ˆp ˆp i ˆR b v j=1,j i k jɛ j 1491

5 = R R( b Ω R (p p i b v j=1,j i k jɛ j, (45 where we made use of the fact ˆR (ˆp ˆp i = ˆR ɛ i + R (p p i. Since X and U are bounded by assumption, it follows that R, p p i, i = 1,, n and their derivatives are bounded, which in turn implies the boundedness of ɛ i. From (39, V is bounded. By virtue of Barbalat s lemma, one concludes that V is uniformly continuous, and V converges to zero, i.e., lim t ɛ i (t =. In view of (4, (43 and (44, it follows that R, bω and b v are bounded, and consequently ɛ i is bounded according to (45. Therefore, since ɛ i is bounded and ɛ i converges to zero, it follows that lim t ɛ i (t =. Since lim t ɛ i (t = and lim t ɛ i (t =, from (45, one has lim t bω R (p p i + b v = for all i = 1,, n. Without loss of generality, let p 1, p 2 and p 3 be three noncollinear landmarks, i.e., they define a plane. Then, one has { lim t bω R (p 1 p 3 =, lim t bω R (p 2 p 3 =. Let b ω R 3 be the vector such that b ω = b Ω. Let x 1 := p 1 p 3, x 2 := p 2 p 3 and x 3 := x 1 x 2. Since x 1 and x 2 are non-collinear, it is clear that rank([x 1, x 2, x 3 ] = 3. Using the facts R (x 1 x 2 = (R x 1 (R x 2 and b Ω R x 3 = bω R (x 1 x 2 = ( b ω R x 1 ( b ω R x 2, one has lim t bω x 3 =, consequently, lim t bω [x 1 x 2 x 3 ] =, which leads to lim t bω (t = and lim t bv (t =. In view of (4 and (41, one concludes that there exist a constant matrix R SO(3 and a constant vector p R 3 such that ( R, p converges to (R, p asymptotically. This completes the proof. Remark 3: From (21 and (22, the observer in (35 can be rewritten, in terms of the measurements, as follows: ˆR = ˆR(Ω y ˆb Ω ˆp = ˆR(v y ˆb v + i=1 k i(ˆp i ˆp ˆRy i ˆp i = ˆRv i k i (ˆp i ˆp ˆRy i, (46 ˆb Ω = 1 2 k ω i=1 k ˆR i ((ˆp i ˆp ˆRyi ˆR ˆb v = k v ˆR i=1 k i(ˆp i ˆp ˆRy i where we made use of the fact ( ˆRy i ɛ i = ( ˆRy i (ˆp i ˆp ˆRy i = ( ˆRy i (ˆp i ˆp = (ˆp i ˆp ˆRyi. Due to the integral action, the terms ˆb Ω and ˆb v may grow arbitrarily large in the presence of measurement noise. To cope with this problem, in practical applications, one can use a projection mechanism as follows: ˆb ω = roj δ (ˆbω, kω n 2 i=1 k ˆR i ((ˆp i ˆp ˆRyi, (47 ˆb v = roj δ (ˆbv, k v ˆR i=1 k i(ˆp i ˆp ˆRy i, (48 where ˆb ω is the vector representation of ˆb Ω, and the projection map is defined as [2] { σb, ˆb Πδ or ˆb σ b roj δ (ˆb, σ b = (I ϱ(ˆb b b, σ b, otherwise b b where (ˆb := ˆb δ, Π δ = {ˆb (ˆb }, Π δ,ε = {ˆb (ˆb ε} and ϱ(ˆb := min{1, (ˆb/ε} for some positive parameters δ and ε. V. SIMULATION In this section, the performance of the proposed observer (35 with (47 and (48 is illustrated by a numerical simulation. We consider a vehicle moving on a 1-meter diameter circle at 1-meter height, along the trajectory p(t = 1[cos(.2t sin(.2t 1]. The rotation matrix is initialized as R( = I 3 and the angular velocity is given by ω(t = [ 1]. In addition, a set of landmarks located on the ground are available for measurements. Moreover, the rotational velocity and translational velocity measurements are corrupted by the following constant biases: b ω = [.2.2.1], b v = [.2.1.1]. The estimates are initialized as ˆR( = exp(.2πu with u = [ 1], ˆp( =, ˆb ω = and ˆb v =. The initial positions of the landmarks are randomly selected. The gains involved in the proposed observer are chosen as k ω =.2, k v = 1, δ =.5, ε =.1 and k i = 5/22 for all i = 1,, 22. The simulation results are given in Fig.1, showing the convergence to zero of landmark and biases estimation errors, and the convergence of I R F and p to some constant values as discussed earlier. VI. CONCLUSIONS A geometric nonlinear observer for SLAM in terms of group velocity and landmark measurements has been derived directly on the matrix Lie group SE 1+n (3. An extension to the case of biased linear and angular velocity measurements has also been proposed. As illustrated in the simulation results, the proposed approach is capable of mapping an unknown environment and providing a relative localization of the vehicle based on the measured landmarks and linear and angular velocities. REFERENCES [1] H. Durrant-Whyte and T. Bailey, Simultaneous localization and mapping: part I, IEEE robotics & automation magazine, vol. 13, no. 2, pp , 26. [2] T. Bailey and H. Durrant-Whyte, Simultaneous localization and mapping (SLAM: art II, IEEE Robotics & Automation Magazine, vol. 13, no. 3, pp , 26. [3] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, Consistency of the EKF-SLAM algorithm, in roceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 26, pp [4] G.. Huang, A. I. Mourikis, and S. I. Roumeliotis, Observabilitybased rules for designing consistent EKF SLAM estimators, The International Journal of Robotics Research, vol. 29, no. 5, pp , 21. [5] J. A. Castellanos, R. Martinez-Cantin, J. D. Tardós, and J. Neira, Robocentric map joining: Improving the consistency of EKF-SLAM, Robotics and autonomous systems, vol. 55, no. 1, pp , 27. [6] G.. Huang, A. I. Mourikis, and S. I. Roumeliotis, Analysis and improvement of the consistency of extended Kalman filter based SLAM, in roceedings of the IEEE International Conference on Robotics and Automation (ICRA. IEEE, 28, pp [7] B. J. Guerreiro,. Batista, C. Silvestre, and. Oliveira, Globally asymptotically stable sensor-based simultaneous localization and mapping, IEEE Transactions on Robotics, vol. 29, no. 6, pp ,

6 Fig. 1: Estimation errors of rotation, position and landmarks with respect to time. [8]. Lourenço, B. J. Guerreiro,. Batista,. Oliveira, and C. Silvestre, Simultaneous localization and mapping for aerial vehicles: a 3-D sensor-based GAS filter, Autonomous Robots, vol. 4, no. 5, pp , 216. [9] J.-L. Blanco, A tutorial on SE(3 transformation parameterizations and on-manifold optimization, University of Malaga, Tech. Rep, vol. 3, 21. [1] G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, A tutorial on graph-based SLAM, IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp , 21. [11] S. Bonnabel,. Martin, and. Rouchon, Symmetry-preserving observers, IEEE Transactions on Automatic Control, vol. 53, no. 11, pp , 28. [12], Non-linear symmetry-preserving observers on Lie groups, IEEE Transactions on Automatic Control, vol. 54, no. 7, pp , 29. [13] R. Mahony, J. Trumpf, and T. Hamel, Observers for kinematic systems with symmetry, IFAC roceedings Volumes, vol. 46, no. 23, pp , 213. [14] S. Bonnabel, Symmetries in observer design: Review of some recent results and applications to EKF-based SLAM, in Robot Motion and Control 211. Springer, 212, pp [15] A. Barrau and S. Bonnabel, An EKF-SLAM algorithm with consistency properties, arxiv preprint arxiv: , 215. [16] T. Zhang, K. Wu, J. Song, S. Huang, and G. Dissanayake, Convergence and consistency analysis for a 3-D Invariant-EKF SLAM, IEEE Robotics and Automation Letters, vol. 2, no. 2, pp , 217. [17] R. Mahony and T. Hamel, A geometric nonlinear observer for simultaneous localisation and mapping, in roceedings of the 56th IEEE Annual Conference on Decision and Control (CDC, 217, pp [18] D. E. Zlotnik and J. R. Forbes, Gradient-based observer for simultaneous localization and mapping, IEEE Transactions on Automatic Control, 218. [19] K. W. Lee, W. S. Wijesoma, and I. G. Javier, On the observability and observability analysis of slam, in Intelligent Robots and Systems, 26 IEEE/RSJ International Conference on. IEEE, 26, pp [2]. A. Ioannou and J. Sun, Robust adaptive control. rentice-hall Englewood Cliffs, NJ, A. roof of Lemma 1 AENDIX The gradient X U can be computed using the differential of U in an arbitrary tangential direction XU T X SE 1+n (3 with some U se 1+n (3 du XU = X U, XU X = XX 1 X U, XU X = X 1 X U, U. (49 On the other hand, from the definition of the tangent map, one has du XU = tr ( XUA(I 4 X = (X (X IA, U = (X 1 (X IA, U = ((I X 1 A, U, (5 where the fact (I X 1 A M and property (7 has been used. Hence, in view of (49 and (5, one has (17. This completes the proof. B. roof of Lemma 2 From the definition of r i, b i and (15, one has i=1 k i r i ˆXb i 2 = i=1 k i (I X 1 r i 2 = tr((i X 1 A(I X 1 = tr( X X(I X 1 A(I X 1 = tr((i XA(I X, where we made use of the fact u 2 = tr(uu for all u R n+4 and property (8. Similarly, one verifies that i ri = ((I X 1 A. On the other hand, using the fact r i ˆXb i = [ɛ i 1 (n+1 ], one can easily verify (2. Moreover, one can further show that i ri [ 3 3 = i=1 k ] iɛ i k 1 ɛ 1 k n ɛ n, (n+1 3 n+1 n+1 n+1 ˆX i ri ˆX [ = ˆR Θ ˆR i=1 k ˆR i ɛ i k ˆR 1 ɛ 1 k ˆR n ɛ n ( where denotes some irrelevant terms, and Θ := n i=1 k iɛ i ˆp i=1 k iɛ i ˆp i = i=1 k iɛ i (ˆp ˆp i. Let Θ a := a ( ˆR Θ ˆR. From the definition of the map a ( and Θ, one has Θ a = 1 2 i=1 k ˆR i ((ˆp ˆp i ɛ i ˆR = 1 2 i=1 k i( ˆR ɛ i ˆRy i, where we made use of the facts: ˆp ˆp i = ˆRy i ɛ i and yx xy = (x y, R x R = (R x, x y = y x, x x = for all x, y R 3 and R SO(3. From (6, one obtains (21 and (22. This completes the proof. ], 1493