Martin Brossard

Silvère Bonnabel

Axel Barrau

Axel Barrau Invariant Kalman

Invariant Kalman Filtering for Visual Inertial SLAM

Keywords: Lie groups, invariant Kalman filtering, unscented Kalman filter, visual inertial SLAM, sensor fusion, localization

come   L'archive ouverte pluridisciplinaire

I. INTRODUCTION

Over the last decades, tremendous progresses have been achieved in visual simultaneous localization and mapping frameworks (SLAM), see e.g., [START_REF] Mur-Artal | ORB-SLAM: a versatile and accurate monocular SLAM system[END_REF]. Most approaches include data fusion using filters [START_REF] Hesch | Observability-constrained vision-aided inertial navigation[END_REF]- [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF], or optimization/bundle adjustment techniques, e.g., [START_REF] Engel | LSD-SLAM: Large-scale direct monocular SLAM[END_REF]- [START_REF] Forster | On-manifold preintegration for real-time visual-inertial odometry[END_REF]. Optimization based methods are more efficient but generally come with higher computational demands than more basic filtering algorithms which are generally more suited to real-time applications.

In this paper, we tackle the problem of fusing Inertial Measurement Unit (IMU) signals with monocular vision for SLAM for Micro Aerial Vehicles (MAVs). We propose a novel Unscented Kalman Filter (UKF) that mainly builds upon two components. First, the recent Lie group structure of SLAM advocated in the field of invariant filtering, see [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF]- [START_REF] Zhang | Convergence and consistency analysis for a 3-D Invariant-EKF SLAM[END_REF]. Secondly, the UKF on Lie Groups (UKF-LG), whose general methodology has been recently introduced in [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF]. The effectiveness of our algorithm is tested both on simulations and on real data [START_REF] Burri | The EuRoC MAV datasets[END_REF]. The method, an UKF-LG visual SLAM, favorably compares to some state-of-the-art Kalman filter based solutions.

Note that, in the present paper we do not specifically address Visual Inertial Odometry (VIO) which is a powerful technique where the features (i.e., the map) are not included in the state, saving execution time. As in SLAM, VIO estimates the sequential changes of the robot over time using an IMU and cameras, but there is no attempt to build a map [START_REF] Li | High-precision, consistent EKF-based visualinertial odometry[END_REF]. Here, by contrast, we explicitly consider the probabilistic visual SLAM problem, where a consistent map of the environment is also pursued. Note that, even for navigation purposes only, building a map allows loop closures, a powerful method to drastically reduce uncertainty on the state, when applicable.

A. Contributions and Links with Previous Literature

In robotics, it has been long recognized that the Lie group structure of the space of poses SE(3) plays an important role, see e.g., [START_REF] Park | A lie group formulation of robot dynamics[END_REF][START_REF] Bullo | Geometric control of mechanical systems: modeling, analysis, and design for simple mechanical control systems[END_REF]. More recently, probability distributions on SE(3), and their role for control and estimation, have been well studied, see e.g., [START_REF] Wang | Error propagation on the euclidean group with applications to manipulator kinematics[END_REF]- [START_REF] Zefran | Metrics and connections for rigidbody kinematics[END_REF], and the monographs [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF][START_REF] Barfoot | State Estimation for Robotics[END_REF].

In [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF], we proposed the Right Invariant Extended Kalman Filter (RIEKF) based SLAM. Letting the output function h(.) of [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF] be the projection onto the camera frame, we readily obtain a RIEKF for visual SLAM, and this filter has been described and also advocated recently in [START_REF] Wu | An Invariant-EKF VINS algorithm for improving consistency[END_REF] for 3D Visual Inertial Navigation Systems (VINS), owing to its consistency properties. However, in the implementation section of the latter paper, the authors rather opt for a multi-state constrained RIEKF. The resulting RI-MSCKF does not address the SLAM problem since landmarks are removed from the state (see eq. 21 therein). Very recently [START_REF] Heo | Consistent ekf-based visual-inertial odometry on matrix lie group[END_REF] also proposed a multistate constrained Right invariant EKF (RIEKF) for 3D VINS. In Section VI dedicated to experiments, we also apply the framework [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF] to the inertial and vision fusion, to implement a RIEKF where the landmarks are part of the state. To our best knowledge this is the first published implementation of an RIEKF for visual inertial SLAM.

Our main contribution is a Right-UKF-LG, which can be viewed as an unscented-based transform alternative to the RIEKF, but which has the advantage of being much more versatile than the RIEKF. Indeed, it spares the user the computation of Jacobians, that can prove difficult, especially in the Invariant EKF framework where Jacobians are defined with respect to the Lie structure, see e.g., [START_REF] Barrau | Invariant Kalman filtering[END_REF]. As a result,

• the practitioner can readily implement our algorithm when using, e.g., a different camera model, or if one wants to add additional measurements such as GPS measurements outdoors, or a complementary depth sensor; • should additional parameters/variables be estimated, such as IMU's scale factors and/or harmonization angles, the algorithm is straightforward to adapt following the state augmentation technique of Section III-C3. Note that, the present paper presents some new developments on the UKF-LG methodology, that are as follows: the state augmentation technique of Section III-C3 allows dealing with state spaces that are not Lie groups, a square-root form implementation detailed in Appendix A and a modification to deal with large updates described in Appendix B.

In [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF], the authors consider the same visual inertial fusion problem, and devise an UKF that takes advantage of the Lie group structure of the body pose SE(3). The main differences are threefold. First, the Lie group we use SE 2+p (3), introduced in [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF][START_REF] Bonnabel | Symmetries in observer design: Review of some recent results and applications to ekf-based slam[END_REF], is much bigger than SE(3), and includes the pose but also the velocity and the landmarks' positions. Then, and more generally, the UKF-LG [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] generates sigma points in the Lie algebra, and then uses concentrated Gaussian distributions (as in e.g., [START_REF] Barfoot | Associating uncertainty with threedimensional poses for use in estimation problems[END_REF]) to map them onto the group. In contrast, [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF] uses a probability distribution directly defined on the group [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF] to generate the sigma points, which is akin to the general unscented Kalman filtering on manifolds of [START_REF] Hauberg | Unscented kalman filtering on riemannian manifolds[END_REF]. Moreover, while [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF] uses parallel transport operations based on left multiplications, we explore two variants based both on left and right multiplications, and the right one proves to be actually much better.

B. Paper's Organization

The paper is organized as follows. Section II formulates the fusion problem. Section III contains mathematical preliminaries on unscented Kalman filtering on Lie groups. Section IV describes the two proposed UKFs for monocular visual and inertial SLAM. Section V and VI illustrate the performances of the proposed filters based both on Monte-Carlo simulations and on real datasets.

II. VISUAL INERTIAL SLAM PROBLEM MODELING

We recall in this section the standard dynamic model for flying devices equipped with an IMU. We then detail the visual measurement model, and we finally pose the SLAM problem.

A. Variables of Interest and Dynamical Model

Let us consider an aerial body equipped with an IMU whose biases are modeled as random walks. Assume moreover that p fixed landmarks of the scene can be tracked visually, and that they constitute the map. The state we want to estimate consists of the position x ∈ R 3 , velocity v ∈ R 3 , orientation R ∈ SO(3) of the body, the IMU biases b ω ∈ R 3 and b a ∈ R 3 , as well as the 3D positions p 1 , . . . , p p ∈ R 3 of the landmarks in the global frame. The dynamics of the system read: body state dynamics

     Ṙ = R (ω -b ω + w ω ) × v = R (a -b a + w a ) + g ẋ = v , (1) IMU biases dynamics ḃω = w bω ḃa = w ba , (2)
landmarks dynamics ṗi = 0, i = 1, . . . , p ,

where (ω) × denotes the skew symmetric matrix associated with the cross product with vector ω ∈ R 3 . The various white Gaussian continuous time noises can be stacked as

w = w T ω w T a w T bω w T ba T , (4)
where w is centered with autocorrelation E(w(t)w(s)) = Wδ(t -s). These equations correspond to the equations of navigation, provided the earth is considered as locally flat.

They model the dynamics of most of MAVs such as quadrotors where the IMU measurements ω and a in (1) are considered as noisy and biased inputs of the system.

B. Measurement Model

In addition to the IMU measurements used as input for the dynamics, the vehicle gets visual information from a calibrated monocular camera. It observes and tracks the p landmarks through a standard pinhole model [START_REF] Forsyth | Computer vision: a modern approach[END_REF]. Landmark p i is observed through the camera as

y i = 1 y i w y i u y i v + n i y , (5)
where y i is the measured normalized pixel location of the landmark in the camera frame, that is,

  y i u y i v y i w   = R T C R T (p i -x) -x C , (6)
where the right term corresponds to the distance from the landmark to the camera expressed in the camera frame. R C and x C are the known rotation matrix and the translation mapping from the body frame onto the camera frame. Finally, n y ∼ N (0, N) represents the pixel image noise.

C. Estimation/Fusion Problem

Our goal is to compute the probability distribution of the high dimensional system's state (R, x, v, b ω , b a , p 1 , . . . , p p ) defined through an initial Gaussian prior and the probabilistic dynamic model ( 1)-( 3), conditionally on the visual landmarks measurements of the form (5) for 1 ≤ i ≤ p. This is the standard probabilistic formulation of the visual 3D SLAM problem with an IMU.

III. UNSCENTED KALMAN FILTERING ON LIE GROUPS

In this section we provide the reader with the bare minimum about the UKF on Lie Groups (UKF-LG) introduced in [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF].

A. Matrix Lie Groups

A matrix Lie group G ⊂ R N ×N is a subset of square invertible matrices such that the following properties hold

I ∈ G (7) ∀ χ ∈ G, χ -1 ∈ G (8) ∀ χ 1 , χ 2 ∈ G, χ 1 χ 2 ∈ G. (9)
Locally about the identity matrix I, the group G can be identified with an Euclidean space R q using the matrix exponential map exp m (.), where q = dim G. Indeed, to any ξ ∈ R q one can associate a matrix L g (ξ) = ξ ∧ of the tangent space of G at I, called the Lie algebra g. We then define the exponential map exp : R q → G for Lie groups as

exp (ξ) = exp m (ξ ∧ ) . (10)
Locally, it is a bijection, and one can define the Lie logarithm map log : G → R q as the exponential inverse, leading to

log (exp (ξ)) = ξ. (11)

B. Uncertainty on Lie Groups

To define random variables on Lie groups, we cannot apply the usual approach of additive noise for χ ∈ G as G is not a vector space. In contrast, we define the probability distribution χ ∼ N L ( χ, P) for the random variable χ ∈ G as [START_REF] Barfoot | Associating uncertainty with threedimensional poses for use in estimation problems[END_REF][START_REF] Barrau | The Invariant Extended Kalman Filter as a stable observer[END_REF]

χ = χ exp (ξ) , ξ ∼ N (0, P) , (12)
where N (., .) is the classical Gaussian distribution in Euclidean space R q and P ∈ R q×q is a covariance matrix. In [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF], the original Gaussian ξ of the Lie algebra is moved over by left multiplication to be centered at χ ∈ G, hence the letter L which stands for "left", this type of uncertainty being also referred to as left-equivariant [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF]. We similarly define the distribution χ ∼ N R ( χ, P) for right multiplication of χ, as

χ = exp (ξ) χ, ξ ∼ N (0, P) . (13)
In ( 12) and ( 13), χ may represent a large, noise-free and deterministic value, whereas P is the covariance of the small, noisy perturbation ξ. We stress that ξ ∈ R q is Gaussian, but N L (., .) and N R (., .) are not. Remark 1: defining Gaussian distributions on Lie groups through [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] and ( 13) is advocated notably in [START_REF] Barfoot | Associating uncertainty with threedimensional poses for use in estimation problems[END_REF][START_REF] Barrau | The Invariant Extended Kalman Filter as a stable observer[END_REF], and the corresponding distribution is sometimes referred to as concentrated Gaussian on Lie groups, see [START_REF] Bourmaud | Continuousdiscrete extended Kalman filter on matrix lie groups using concentrated Gaussian distributions[END_REF]. An alternative approach, introduced to our best knowledge in [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF], and used in [START_REF] Zefran | Metrics and connections for rigidbody kinematics[END_REF], consists of defining a (Gaussian) density directly on the group using the Haar measure. In the latter case, the group needs be unimodular, but such a requirement is in fact unnecessary to define the random variables [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] and [START_REF] Burri | The EuRoC MAV datasets[END_REF].

C. Unscented Kalman Filtering on Lie Groups

By representing the state error as a variable ξ of the Lie algebra, we can build two alternative unscented filters for any state living in a Lie group, along the lines of [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF]. Let us consider a discrete time dynamical system of the form

χ n+1 = f ( χ n , u n , w n ) , (14)
where the state χ n ∈ G, u n is a known input variable and w n ∼ N (0, Q n ) is a white Gaussian noise. Consider discrete measurements of the form

y n = h ( χ n , n n ) , (15)
where n n ∼ N (0, N n ) is a white Gaussian noise. Two different UKFs follow from the above uncertainty representation. 1) Left-UKF-LG: the state is modeled as χ n ∼ N L ( χn , P n ), that is, using the left-equivariant formulation [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] of the uncertainties. The mean state is thus encoded in χn and dispersion in ξ ∼ N (0, P n ). The sigma points are generated based on the variable ξ, and mapped to the group through the model [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF]. Note that, this is in slight contrast with [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF][START_REF] Zefran | Metrics and connections for rigidbody kinematics[END_REF], which generate sigma points through a distribution defined directly on the group. The filter consists of two steps along the lines of the conventional UKF: propagation and update, and compute estimates χn and P n at each n.

2) Right-UKF-LG: the state is alternatively modeled as χ n ∼ N R ( χn , P n ), that is, using the right-equivariant formulation (13) of the uncertainties.

3) UKF-LG with state augmentation: the paper [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] is dedicated to the case where the state is a Lie group. However, when the state consists of a matrix χ belonging to a Lie group, and an additional vector, say b, the methodology is straightforward to apply by augmenting the state space: vector b is appended to the state which then becomes the couple ( χ , b). The Lie group variable χ is treated using the UKF-LG methodology, whereas the vector variable is treated as in the conventional UKF. We use this approach in the sequel, where the vector variable corresponds to the IMU biases.

D. The Special Euclidean Group SE 2+p (3)

In [START_REF] Bonnabel | Symmetries in observer design: Review of some recent results and applications to ekf-based slam[END_REF], the author noticed there is a natural Lie group structure underlying the (wheeled robot based) SLAM problem. The corresponding Lie group was named SE 1+p (3) in [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF], and leveraged therein to design an Invariant EKF, which resolves some well-known consistency issues of the conventional EKF based SLAM. Some alternative properties of the Invariant EKF based SLAM have also recently been proved in [START_REF] Wu | An Invariant-EKF VINS algorithm for improving consistency[END_REF][START_REF] Zhang | Convergence and consistency analysis for a 3-D Invariant-EKF SLAM[END_REF].

Any matrix χ ∈ SE 2+p (3) is defined as

χ = R v x p 1 • • • p p 0 2+p×3 I 2+p×2+p . ( 16
)
The dimension of the group, and thus of the Lie algebra, is 3 + 3(2 + p). The uncertainties, defined as

ξ = ξ T R ξ T v ξ T x ξ T p1 • • • ξ T pp T ∈ R 9+3p
, are mapped to the Lie algebra through the transformation L g : ξ → L g (ξ) = ξ ∧ as

ξ ∧ = (ξ R ) × ξ v ξ x ξ p1 • • • ξ pp 0 2+p×5+p . ( 17
)
The closed form expressions for exponential, logarithm and Jacobian can be deduced along the lines of [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF][START_REF] Barrau | The Invariant Extended Kalman Filter as a stable observer[END_REF].

IV. PROPOSED ALGORITHMS

To apply the methodology of UKF on Lie groups, the dynamics must first be discretized, and the state space must be (partly) embedded in a matrix Lie group.

A. Time Discretization

Equations ( 1) are standard navigation equations, and their discretization is well established. In this paper, we implemented the method of pre-integration on manifolds of [START_REF] Forster | On-manifold preintegration for real-time visual-inertial odometry[END_REF].

B. Lie Group Embedding

The state space can be partially embedded into a Lie group, by letting χ n be the matrix of the group G = SE 2+p (3) that represents the state variables (R, v, x, p 1 , • • • , p p ) at time step n through representation [START_REF] Bullo | Geometric control of mechanical systems: modeling, analysis, and design for simple mechanical control systems[END_REF]. Using this embedding, the state at time n can in turn be represented as (χ n , b n ), letting the bias vector be b

= b T ω b T a T ∈ R 6 .
The dispersion on χ n can be encoded using the left uncertainty [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] or the right one [START_REF] Burri | The EuRoC MAV datasets[END_REF], leading to two alternative filters (see Section III-C). In the following, we detail the Right-UKF-LG which adopts the right-equivariant uncertainties (13) of χ n and and conventional additive uncertainties on the biases b. We leave to the reader the derivation of the Left-UKF-LG, based upon left-equivariant uncertainties [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF].

C. Final Retained Model and Filter Architecture

Defining the input vector u = ω T a T T , and gathering the results of the two preceding subsections, we obtain the following uncertainty representation and discrete time model associated to the Right-UKF-LG: uncertainty rep.

χ n = exp (ξ) χn

b n = bn + b , ξ b ∼ N (0, P n ) , ( 18
)
dynamics χ n , b n = f ( χ n-1 , u n -b n-1 , w n ) , (19)
observations

Y n = y T 1 • • • y T p T := Y ( χ n , n n ) y i given in (5), i = 1, . . . , p , (20)
such that χn , bn ∈ R 15+3p represents the mean (estimated) state at time n, P n ∈ R (15+3p)×(15+3p) is the covariance matrix that defines the state uncertainties ξ, b , and the vector Y n contains the observations of the p landmarks with associated discrete Gaussian noise w n ∼ N (0, Q). The filter consists of two steps: propagation and update; as shown in Algorithm 1. We detail these two main steps in the following with the formalism of square-root implementation [START_REF] Van Der Merwe | The square-root unscented Kalman filter for state and parameter-estimation[END_REF] where S is the Cholesky decomposition of the covariance, sparing the computation of covariance matrices and being numerically more stable. Remark 2: for the Left-UKF-LG, we define χ n = χn exp (ξ) and substitute it in [START_REF] Barfoot | Associating uncertainty with threedimensional poses for use in estimation problems[END_REF]. This results in quite different filters, though. In particular, consistence properties for EKF SLAM are characteristics of the right-invariant formalism, see [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF] and also [START_REF] Wu | An Invariant-EKF VINS algorithm for improving consistency[END_REF][START_REF] Zhang | Convergence and consistency analysis for a 3-D Invariant-EKF SLAM[END_REF].

D. Propagation Step

The propagation step is described in Algorithm 2 and operates as follow. The filter first computes the propagated mean state, and then the 2J sigma points obtained at line 5 are propagated at lines 6-7. It is then convenient to view the propagated Cholesky factors S as an output of the function qr(.). Details are provided in Appendix A along with the definitions of J and γ. Although more details on the methodology can be found in [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] (see also [START_REF] Barfoot | Associating uncertainty with threedimensional poses for use in estimation problems[END_REF] regarding propagation), line 7 deserves a few explanations for the paper to be selfcontained. According to uncertainty model [START_REF] Burri | The EuRoC MAV datasets[END_REF], dispersion around the mean is modeled as exp(ξ) χ , so if χ denotes the propagated mean, and χ j denotes a propagated sigma point, then the corresponding sigma point in the Lie algebra is defined through exp(ξ j ) χ = χ j , i.e., ξ j = log χ j χ-1 .

Algorithm 2: Propagation function for Right-UKF-LG

Input: χ, b, S, u, Q; 1 u ← u -b ; // unbiased input 2 S a = blkdiag (S, chol (Q)); 3 χ = χ ;
// save non propagated state 4 χ, b = f ( χ , u, 0) ; // propagate mean // step 5: sigma points generation 5 ξ ∓ j b ∓ j n ∓ j = ∓γ col j (S a ) , j = 1, . . . , J; // steps 6-7: sigma point propagation

6 χ ∓ j , b ∓ j ← f exp ξ ∓ j χ , u -b ∓ j , n ∓ j , j = 1, . . . , J; 7 ξ ∓ j ← log χ ∓ j χ-1 , j = 1, . . . , J ; 8 S ← qr ξ ∓ j , b ∓ j , j = 1, . . . , J, Q ; //

E. Update Step

The update step incorporates the observation of the p landmarks at time n and is described in Algorithm 3. It operates as follow. The sigma points generated in the Lie algebra at line 2 are mapped to the group through model ( 13) at line 3, and used to compute 2J + 1 measurement sigma points at line 4. The function qr (.) then evaluates the updated Cholesky factors and the correction term δ ξ, δ b used to update the mean state, along the lines of the conventional UKF methodology, and it is detailed in the Appendix A. Line 6 is the update of [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] as concerns the Lie group part of the state, and conventional update as concerns teh biases, see next subsection for more details.

Remark 3: following [START_REF] Holmes | An O(N 2 ) square root unscented Kalman filter for visual simultaneous localization and mapping[END_REF], the square-root implementation can add or remove landmarks, initializing landmark position as inverse depth point and allows computationally efficient propagation steps.

F. Discussion on the Final Update

Let χ denote the propagated mean, and P = SS T the propagated covariance matrix of the state error after the propagation step, i.e. the outputs of Algorithm 2. According to model [START_REF] Burri | The EuRoC MAV datasets[END_REF], it means the propagated state is described by χ ≈ exp (ξ) χ, ξ ∼ N (0, P) before measurement Y n . At the update step, the UKF methodology takes into account the observation Y n to update the element ξ ∈ R 9+3p as ξ ∼ N (δ ξ, P + ), i.e., ξ = δ ξ + ξ + with ξ + ∼ N (0, P + ). Back to the Lie group this implies χ ≈ exp ξ + + δ ξ χ, where ξ + ∼ N 0, P + .

(21)

Following [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF], and assuming both correction δ ξ and uncertainty ξ + in ( 21) are small, we end up with the following posterior that matches with the uncertainty representation ( 13):

χ exp ξ + χ+ , where (22) ξ + ∼ N 0, P + , ( 23
) χ+ = exp δ ξ χ. ( 24
)
This approximation is based indeed upon the Baker-Campbell-Hausdorff (BCH) formula that states that exp ξ

+ + δ ξ = exp (ξ + ) exp δ ξ + O δ ξ+ , (ξ + ) 2 , ξ + δ ξ .
Remark 4: when the correction terms are large and the BCH based approximation does not hold true, we propose an alternative method in Appendix B.

V. SIMULATION RESULTS

Five different filters are compared on Monte-Carlo simulations:

• an UKF that considers the attitude as an element SO [START_REF] Huang | A quadraticcomplexity observability-constrained unscented Kalman filter for SLAM[END_REF] and the remaining variables as a vector space; • the SE(3)-based UKF recently introduced in [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF]. This filter is an UKF which encodes body attitude and position in SE(3) and uses parallel transport associated to leftinvariant vector fields of SE(3); • the Right-Invariant visual EKF SLAM (RIEKF) of [START_REF] Wu | An Invariant-EKF VINS algorithm for improving consistency[END_REF][START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF] (where the biases are appended to the state and treated as in the conventional EKF) whose first numerical evaluation appears to our knowledge in the present paper; • the proposed Right-UKF-LG described in Section IV;

• the proposed Left-UKF-LG, as an alternative to Right-UKF-LG based on the left uncertainty representation [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF].

A. Simulation Setting

We generate a noise-free trajectory displayed on Figure 1. This trajectory is realistic since it is inspired by true quadrotor trajectories from [START_REF] Burri | The EuRoC MAV datasets[END_REF]. Noises and slowly drifting small biases are added, and a standard deviation of 2 pixels is set for the observation noise. We define the number of landmarks in the state as p = 60 and at each observation, we observe a subset of 10 of these landmarks. 100 Monte Carlo simulations are then run. x

(cm) R ( • ) Conventional UKF 9.3 1.3 SE(3)-UKF of [5] 7.8 1.2
Left-UKF-LG 7.5

Right-UKF-LG

Fig. 2. Root Mean Squared Error averaged on 100 Monte Carlo simulations, on the body position and attitude, for the various filters, as described in Section V. The proposed Right-UKF-LG and RIEKF achieve the best results.

B. Results

The Root Mean Squared Error (RMSE) for the entire trajectory, averaged over 100 Monte Carlo runs, is displayed in Figure 2. From these results, we observe that:

• three groups appear: the RIEKF and Right-UKF-LG achieve the best results. This confirms that the rightinvariant errors on SE 2+p (3) are best suited to SLAM, as explained in [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF][START_REF] Zhang | Convergence and consistency analysis for a 3-D Invariant-EKF SLAM[END_REF]. Then, the Left-UKF-LG and the SE(3)-based UKF of [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF] run second, and the conventional UKF runs last; • The discrepancy between RIEKF and Right-UKF-LG is low at this noise level. Both algorithms are based on the right-invariant error on the Lie group SE 2+p (3), but the first one uses the EKF methodology and the second one the UKF methodology.

VI. EXPERIMENTAL RESULTS

To further validate then the two proposed filters (Right and Left UKF-LG) on real data, we evaluate them on the EuRoC dataset [START_REF] Burri | The EuRoC MAV datasets[END_REF]. The five compared filters are the same as in the previous section. We selected five sequences in [START_REF] Burri | The EuRoC MAV datasets[END_REF] in which landmarks can be well tracked in order to minimize the influence of the frontend image processor.

A. Experimental Setting

Owing to the number of landmarks that keeps growing, the state may grow unboundedly and the filters become intractable for real time implementation. We thus propose to marginalize out landmarks that are not seen anymore, and add new landmarks to the state as they arrive, along the lines of [START_REF] Loianno | Visual inertial odometry for quadrotors on SE(3)[END_REF]. This way, we conserve a constant number of 30 observed landmarks in the state, and the experimental results to come can be viewed as preliminary regarding our visual SLAM algorithm.

In our implementation, the filter tracks features via the KLT tracker using minimum eigenvalue feature detection [START_REF] Shi | Good features to track[END_REF] for its efficiency, and points are undistorted with the furnished camera parameters. The different filters are configured with the same parameters, where we set 2 pixels standard deviation for the landmark observations and IMU noise provided by [START_REF] Burri | The EuRoC MAV datasets[END_REF]. The initial state corresponds to the ground truth.

B. Results

The different filters are thus launched on the real data and we plot the position errors with respect to ground truth for five experiments in Figure 4. On this set of experiments, we see that as for the previous section, two groups appear: the RIEKF and Right-UKF-LG achieve the best results. However, the differences between the approaches are smaller than in the simulation section. This is mainly due to the small time presence of each landmark in the state, such that the different error representations have less influence on the results. This preliminary experiment confirms the potential of Right-UKF-LG and RIEKF over conventional UKF and SE(3)-UKF.

C. Comparison of Execution Times

We compare in this section the execution time of the filters, both for the propagation and update steps. Figure 5 summarizes the results (frontend execution time is excluded). From this table, we observe that • UKFs approaches require much more computational power than RIEKF during the propagation step. This is reinforced by the computation of logarithm at line 7 of Algorithm 2; • the propagation necessitates much more calculus than the update for each UKF solutions, since the IMU (propagation) frequency (200 Hz) is ten times the camera (update) frequency (20 Hz); • the differences between UKF-based approaches and RIEKF solutions for the update step appear as negligible compared to the propagation step. Note that, the various UKFs' propagation step seems particularly long, owing in part to the non-optimal use of Matlab. However, an alternative solution we advocate for low powerful devices is merely to implement an hybrid R-UKF-LG that combines the RIEKF propagation and the R-UKF-LG update, in which we preserve the versatility (and fast prototyping benefits) of the Unscented approach with respect to the addition of other sensors' measurements (such as GPS) and/or variations in the measurement camera model. We implemented this solution, and we obtained similar results as the (full) R-UKF-LG on those datasets. Finally, in practice, we note that the front image processing is anyway generally much higher than the execution time of the filter.

VII. CONCLUSION

Two novel UKFs for data fusion of inertial sensors and monocular vision in the context of visual SLAM have been proposed. They build upon the very recent theory of UKF on Lie groups of [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF], and have the merit of exploiting the full Lie group structure underlying the SLAM problem introduced in [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF][START_REF] Bonnabel | Symmetries in observer design: Review of some recent results and applications to ekf-based slam[END_REF]. Another advantage is that the UKF approach spares the user the computation of Jacobians inherent to EKF implementation, and thus the proposed filters can be readily adapted to small modifications in the model, estimation of additional parameters, and/or addition of one or several sensors. Results from simulations and real experimental data have shown the relevance of the approach based on invariance, and notably the Right-UKF-LG. Future works will explore the theoretical consistency properties that the proposed Right-UKF-LG might possess along the lines of [START_REF] Barrau | An EKF-SLAM algorithm with consistency properties[END_REF]. Regarding more practical aspects, sigma point selections [START_REF] Huang | A quadraticcomplexity observability-constrained unscented Kalman filter for SLAM[END_REF] is also an interesting issue left for future work.

APPENDIX A

We give here the details of parameters and functions used for L-UKF-LG and R-UKF-LG. We set the scale parameters γ and γ with the scaled unscented transform [START_REF] Julier | The scaled unscented transformation[END_REF], such that they depend on the augmented covariance size J = 27 + 3p, J = 15 + 3p and

γ = J/(1 -W 0 ), W 0 = 1 -J/3, W j = 1 -W 0 2J , (25)
γ = J /(1 -W 0 ), W 0 = 1 -J /3, W j = 1 -W 0 2J . (26)
The function qr(.) operates as taking the QR decomposition of

QR ← W 1   ξ + 1 • • • ξ + J ξ - 1 • • • ξ - J b + 1 • • • b + J b - 1 • • • b - J 0 chol (Q) 0 -chol (Q)   , (27)
from which we can extract the Cholesky factor as

R = S 0 . ( 28
)
The function qr (.) operates as follow: first, compute the mean measurement and weighted deviation

Ȳ = W 0 Y 0 + J j=1 W i Y + j + Y - j , (29)
e 0 = |W 0 | Y 0 -Ȳ , (30)
e ∓ j = W j Y ∓ j -Ȳ , j = 1, . . . , J ,

and compute the Cholesky factors of the measurement covariance and the cross covariance as

QR ← e + 1 • • • e + J e - 1 • • • e - J R , (32)
R = S 0 , (33)
S ← CholUpdate (S , sign (W 0 ) , e 0 ) ,

P = J j=1 W j ξ + j b + j T e + j + ξ - j b - j T e - j , (34)
R is a block diagonal matrix containing p times the matrix chol(N) along its diagonal, and then compute gain, innovation and covariance as K = P S T S -1

(36)

= P S -1 S -T (37) δ ξ δ b = K Y -Ȳ , (38)
S ← SeqCholUpdate S, -1, KS T ,

where SeqCholUpdate denotes repeated Cholesky updating CholUpdate using successive columns of KS T as the updating vector [START_REF] Van Der Merwe | The square-root unscented Kalman filter for state and parameter-estimation[END_REF]. To finally consider the Jacobian (see Section IV-F), we compute

S ← SJ T , (40)
letting S no longer triangular, but S keeps a valid matrix square root which could be used to define the next set of sigma points [START_REF] Holmes | An O(N 2 ) square root unscented Kalman filter for visual simultaneous localization and mapping[END_REF].

Remark 5: since we consider observation Y that lives in vector space, (38) is always valid and we do not have to compute any logarithm operation.

APPENDIX B

As concerns the update step, when the innovation δ ξ is important, we propose in the present paper to possibly use the more accurate approximation of [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF][START_REF] Barfoot | State Estimation for Robotics[END_REF] exp ξ + + δ ξ = exp Jξ + exp δ ξ + O ξ + , (41

)
where J is the left Jacobian. In this case we compute the updated parameters as χ+ = exp δ ξ χ, (42)

P + ← JP + J T , (43)
When δ ξ remains small, J ≈ I such that we can discard J in (43) for computational efficiency, recovering the update [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF].

Algorithm 1 :

 1 Left and Right UKF on Lie groups Input: χ, b, S = chol (P) , u, Q, Y, N; 1 χ, b, S ← Propagation χ, b, S, u, Q ; if received measurement then 2 χ, b, S ← Update χ, b, S, Y, N ; Output: χ, b, S;

Algorithm 3 :

 3 see Appendix A for definition of qr // the notation x ∓ is used to denote the two variables +x and -x Output: χ, b, S; Update function for the Right-UKF-LG Input: χ, b, S, Y, N; 1 Y 0 = Y ( χ, 0); // see (20) and (5)-(6) 2 ξ ∓ j b ∓ j = ∓γ col j (S) , j = 1, . . . , J ; 3 χ ∓ j = exp ξ ∓ j χ, j = 1, . . . , J ; 4 Y j = Y χ ∓ j , w ∓ j , j = 1, . . . , J ; 5 δ ξ, δ b, S ← qr' Y n , Y 0 , Y ∓ j , ξ ∓ j , j = 1, . . . , J , N ; 6 χ ← exp δ ξ χ, b ← b + δ b ; // update mean // See Appendix A for definition of qr' Output: χ, b, S;

Fig. 1 .

 1 Fig. 1. Simulation trajectory used in Section V, and trajectories estimated by the various filters.

Fig. 3 .

 3 Fig. 3. Landmark tracking in the experiment. Green crosses are the current pixel locations of the landmarks and red circles are the pixel locations of the landmarks five images (i.e., 1 s) earlier. Picture comes from the EuRoC dataset [13].

Fig. 4 .Fig. 5 .

 45 Fig. 4. Root Mean Squared Error on position with respect to ground trugh, on five different experiments.