Marion

Xavier

Florent Hughes Aubrey

Daniele Phillip

Michelle Guillaume

Grégoire, Sami, Sofiane, aux encore nouveaux ± Récent Doctorants Marin

Jean-Pierre Colin

Emmanuel, Olivier Ainsi Qu'à Martin

Alexis Christophe

François, Amaury, Fabien Cyril & Kelly

Brigitte Je Remer- Cie

to report this thesis. It is an honor for me to consider their insightful suggestions to make this manuscript clearer. I switch to pour remercier David Filliat d'avoir présider la soutenance et Yacine Chitour en temps que membre du jury et ancien prof.

Je resterai ad vitam aeternam redevable envers Silvère pour m'avoir fait confiance et guidé durant toute cette aventure. Je ne mesure pas encore la chance d'avoir eu un directeur de thèse avec un tel niveau de bienveillance! Merci 1-ment à Axel pour m'avoir co-encadré et partagé avec moi ses idées les

CHAPTER 1 Introduction

This thesis deals with state estimation of vehicles or robots that navigate and which are equipped with various sensors. The first practical digital state estimators appeared in the 1960s, during the advent of the space age and digital computers. The Kalman filter was introduced by Rudolph Emil Kalman in 1960 as a theoretical optimal state estimator and was immediately applied to navigation for the Apollo project, to estimate the trajectories of the manned space capsule when going to the Moon and back. More generally the Kalman filter is a tool to estimate the state of a dynamical system had a huge impact on the engineering world, especially the aerospace community. This is dramatically illustrated by awarding Kalman for the development of his optimal digital technique for state estimation with the 2008 Draper Prize, which is one of three prizes that constitute the "Nobel prizes of engineering".

Sixty years later, and though the field of Kalman filter based navigation may be considered as mature, there is still ongoing research on the subject of digital state estimation to estimate the trajectories of vehicles in real time. There are various reasons why.

From the theoretical point of view, researchers have long tried to overcome the shortcomings of the Kalman filter when applied to nonlinear systems. Indeed, the Kalman filter is optimal for state estimation of linear systems. However, systems are hardly linear in practice, especially in the field of navigation, and the simple extension of the Kalman filter, the Extended Kalman Filter (EKF), based on linearizations about the estimated trajectory turns out to work well in many cases (e.g., to estimate the trajectory of the space capsule) but may completely fail when confronted with challenging problems. This has prompted other Kalman filter variants, like the unscented Kalman filter, as well as different approaches such as particle filters that were very successful in the signal processing community and the optimization-based methods known as smoothing that is currently very successful in the robotics community.

Another reason why there is still research on the subject of state estimation for navigation is the recent ubiquity of cameras (vision) as a low-cost and very informative sensor. They return a high dimensional information that is not easily related to the state, and pose quite a number of problems (both practical and theoretical) when one wants to fuse vision with a more classical sensor suite, such as inertial sensors, wheelspeed sensors, and Global Navigation Satellite Systems (GNSS). Navigating without absolute measurements (as those provided by GNSS), but only with measurements that are relative to an unknown environment is referred to as the problem of Simultaneous Localization And Mapping (SLAM) in the robotics community, and has proved a very challenging application to the extended Kalman filter, even at a theoretical level.

Finally, (Kalman) filtering is based on physical models, for the trajectory followed by the vehicle where a physical model of the state evolution is used, and to mathematically relate the state to the sensors' measurements. Filtering also relies on some parameters that need to be tuned by the user. As can be expected, the models are idealized and do not completely match physical reality. In the same way, tuning the parameters is difficult as the parameters are supposed to represent the magnitude of the uncertainty, but this uncertainty is largely unknown and relies on idealized statistical models that do not match what is actually unknown in practice.

Contributions of the Thesis

The goal of this thesis is to contribute to the state-of-the-art regarding Kalman filtering for navigation, by focusing on various challenging aspects. The contributions focus on two different routes. The first route consists in building on the recent variant of the Kalman filter called Invariant Extended Kalman Filter (IEKF) to address challenging issues, namely the inconsistency of EKF for SLAM, navigation with vision sensors, and the more general question of Kalman filtering in the case where the state space is not an Euclidean space (filtering on manifolds). The second route consists in using recent tools from the field of Artificial Intelligence (AI), namely (deep) neural networks, to improve Kalman filters, notably to automatically relate sensors' measurements to the state, and to tune the filter "optimally", that is, use machine learning techniques to find a dynamical tuning strategy of the Kalman filter parameters that best matches the data.

This goal is ambitious, and it leads to contributions that all revolve around the Kalman filter but which intervene at various levels and which differ in nature at times. As a result, the reading of the present manuscript might prove a little difficult, in the sense that contributions are to some extent "scattered". To help the reader better situate where the contributions lie, we have made a diagram, represented on Figure 1.1, where the thesis contributions are depicted in the green rectangles. The goal of the state estimator is to compute as best as possible accurate state estimate, and to convey the extent of uncertainty about its own estimate, which is encoded in the state covariance matrix as concerns the Kalman filter. The success in practice of a state estimator relies on three main components: correct physical and uncertainty models, an efficient estimator for the given models, and finely tuned uncertainty magnitude, and estimator parameters. Those correspond to various blocks on the figure, with relations between them that appear. Although it might be too early for the reader being unfamiliar with filtering to look much into the diagram, the reader should bear in mind its existence and refer to it throughout the manuscript when need be.

A last note regarding the present work. Our work is essentially concerned with the production of novel algorithms in the field of navigation and state estimation for robotics or aerospace. In many cases, our algorithms are tested and benchmarked on publicly available datasets. In this spirit, we have strived for the production of open source code, and most of our algorithms have been made publicly available.

.1: Schema of a general state estimation system, along with the contributions of the present thesis (green rectangles). The state estimator computes state estimates along with their uncertainty in function of measurement inputs. The state estimator is divided into a sensor fusion algorithm (red), and models (yellow), where models are parametrized by uncertainty (noise covariance matrices) Q and R.

Part I: the first part is dedicated to theoretical contributions to the field of nonlinear Kalman filtering, and their application to the context where one navigates with camera and seeks to fuse inertial sensors with vision sensors. In Chapter 3, we focus on the (abstract) theoretical problem of SLAM, that is, navigating with relative measurements only in a totally unknown environment. For this problem, the EKF has been long known as a debateable solution, owing to its inconsistency in this context. We revisit the theoretical problem of EKF inconsistency for SLAM, and show how the use of the recently introduced Kalman filter variant IEKF may resolve those issues.

Then, in Chapter 4, we study how the well established variant of EKF called Unscented Kalman Filter (UKF), may be adapted in the case where the state does not belong to a vector space, but to a more complicated space, namely a manifold. The most basic example, and also a historical motivation, is attitude estimation, where the state is a rotation matrix. Our method applies to all Lie groups, and in particular allows to propose an unscented version of the IEKF. This is desirable as UKF readily allows fast prototyping, in the sense that it spares the sometimes difficult computation of Jacobians that must be performed in the EKF methodology. We have proposed a code available online that allows devising and benchmarking algorithms quickly.

Finally, Chapters 5 and 6 are devoted to the problem of SLAM in the presence of vision sensors. We build on the state-of-the art Kalman filter based solution, namely the Multi-State Constrained Kalman Filter (MSCKF), to attack the problem. Our contributions essentially consists in building an invariant version of this filter, that then comes with consistency properties using the results of Chapter 3, and to propose a computationally efficient unscented version, building on our work described in Chapter 4.

Part II: the second part focuses on the assessment of uncertainty of sensors. Although each sensor comes with specifications, some issues arise. For instance when a wheeled vehicle navigates some assumptions may be made regarding the motion, such as the fact the vertical velocity (in the vehicle's frame) is negligible, and the lateral velocity is small (wheeled vehicles tend to roll without slip although slip is inevitable). This information may be used by the Kalman filter in the form of a pseudo-measurement. However, this information is uncertain: in turns the vehicle slips more than in straight lines. As a result, a machine (deep) learning algorithm may "learn" the extent of uncertainty that lies in the formulated assumption, and relate it to the inertial measurements. This is the object of Chapter 8, where we show our method based only on inertial sensors competes with state-of-the art methods that use inertial sensors plus vision.

Another type of uncertainty may stem from the nature of the observation. Indeed mobile robotics systems are increasingly equipped with laser scanners that return depth images of the environment. An algorithm, called Iterative Closest Point (ICP), allows estimating the displacement of the robot between two scans. It is then important for the filter that estimates the robot's trajectory based on various sensors (including the scanners) to know the amount of uncertainty present in the ICP's estimated displacement. Even if one knows the precision of the depth measurements from the scanner, it is not easily related to the ICP's estimate precision. In Chapter 9, we propose a simple theoretical approach based on statistics and an algorithm for this problem which goes beyond existing solutions, and overcomes their main drawbacks.

Part III: the third part is wholly focused on the use of deep learning, which is a subset of modern Artificial Intelligence (AI), as a tool to extract pertinent information from inertial measurements, and to use this information to navigate. In all cases, the method is supervised, and some ground truth information is necessary for the algorithms to learn.

In Chapter 11, we build upon the Zero velocity UPdaTe (ZUPT), which is a way to correct the estimates from the Inertial Navigation System (INS) when a wheeled vehicle stops. Indeed, the INS cannot measure directly the vehicle has stopped, and hence benefits from this side information (indeed, when a vehicle stops its trajectory is then in part known and the corresponding estimates may be corrected). In commercial INS this information when available always comes from additional wheel speed sensors, which are connected to the INS. In our work, we leverage deep learning to make the INS detect stops based only on the inertial sensors, hence relaxing the need for additional sensors besides inertial sensors. Evaluations on a publicly available car dataset demonstrates indeed that the proposed scheme may achieve a final precision of 20 m for a 21 km long trajectory of a vehicle driving for over an hour, equipped with an IMU of moderate precision (the gyro drift rate is 10 deg/h).

In Chapter 12 we focus on the estimation of the orientation of the vehicle only, namely its attitude. We leverage deep learning to refine the modelling of the measurements emanating from the inertial sensors, and to calibrate them. The algorithm outperforms the state-of-the-art.

Chapter 13 presents a few additional results in the same vein. Finally, in Appendix, we provide the reader with a few tips for Kalman filtering enhanced by deep learning. This chapter is a feedback to the community of our experience with this subject, developed throughout this thesis.

Part I Unscented Kalman Filtering on

Manifold & Lie Groups CHAPTER 2

Introduction to Part I

This chapter introduces preliminary mathematical notions about Kalman filtering, manifolds, Lie groups, along with the notation used throughout this thesis. In this chapter, the mathematical problem is based on models (regarding state's dynamics, measurements, and uncertainty) that are considered as given and wholly valid, and we discuss state estimation in this context 1 .

The Problem of Bayesian Filtering and the Kalman Filter

Consider a classical discrete linear system in R p :

x n+1 = F n x n + u n + G n w n ,
(2.1)

y n+1 = H n+1 x n+1 + n n+1 , ( 2.2)
where x n 2 R p is the state of the system at time n, u n a given input vector, y n a measurement observation, and w n ⇠N(0, Q n ), n n ⇠N(0, N n ) are independent Gaussian noises polluting the dynamics and the observation at step n. F n , G n and H n are the matrices defining the dynamics of the system and the function of the system observed through y n . Let x 0 ⇠N(x 0 , P 0 ) be a Gaussian prior on the state at time n = 0. The problem of Bayesian filtering we address is to find the best estimates xn conditionally to initial state and past measurements y 1 ,...,y n , i.e. finding xn that minimizes E [kx n xn k 2 |y 1 ,...,y n ].

When the system is linear, the Kalman filter is the best Bayesian filter, as the distribution of x n , given the propagation matrices F n and the observations y n stays Gaussian, thanks to the following two standard properties:

• if x ⇠N(x, P), w ⇠N( ŵ, Q), and F, G, u are given matrices and vector, then Fx + u + Gw is a Gaussian, of mean Fx + u + G ŵ and covariance FPF T + GQG T ;

• if x ⇠N(x, P), n ⇠N(0, N), and H is a given matrix, then (x, Hx + n) forms a Gaussian vector. In turn, E[x|Hx + n] is also a Gaussian, whose mean and covariance are given by the conditioning formulas, which lead to the Kalman filter equations, defined thereafter.

Introduction to Part I Thus, given a Gaussian prior, the Kalman filter defines an estimate xn at each step n through the following initialization and two step sequence: Initialization x0|0 ⇠N(x 0 , P 0 ) (2.3) Propagation

( xn+1|n = F n xn|n + u n P n+1|n = F n P n F T n + G n Q n G T n (2.4) Update 8 > > > > > < > > > > > :
S = H n+1 P n+1|n H T n+1 + N n+1 K = P n+1|n H T n+1 S 1 xn+1|n+1 = xn+1|n + K(y n+1 H n+1 xn+1|n ) P n+1|n+1 =(I KH n+1 )P n+1|n (2.5) The linear Kalman filter exhibits several optimality properties, it is in particular an unbiased estimator with minimum variance. These properties mostly come from two of its features: the particular Kalman gain K used, and the fact that its error is autonomous: it does not depend on the state estimates xn|n nor the observations y n . Indeed, let the error of the system be e n = x n xn .

(2.6)

Then it satisfies e n+1|n = F n e n|n + G n w n (2.7) e n+1|n+1 = e n+1|n KH n+1 e n+1|n .

(2.8)

Therefore, the error evolves solely according to the noises and their statistical properties, which are supposed to be known in this context. In particular, this also holds for its associated covariance P n . This autonomy property is crucial, as will be pointed out in invariant filtering in Section 7.

2 2 Nonlinear Filtering: the Extended Kalman Filter (EKF)

The original Kalman filter was designed for linear systems with linear observations. To handle non-linear dynamics or observations, an extension of the algorithm was proposed, called the Extended Kalman Filter (EKF). Consider the following non-linear system

x n+1 = f (x n , u n , w n ), (2.9) y n = h(x n )+n n .

(2.10)

Then the EKF follows similar steps as the linear version, up to the following differences:

• the estimate is propagated as xn+1|n = f (x n|n , u n , 0);

• the update writes xn+1|n+1 = xn+1|n + K(y n+1 h(x n+1|n ));

• F n , G n , and H n in (2.1) and (2.2) are replaced by Fn , Ĝn and Ĥn , the Jacobians of f (•) and h(•) computed at the estimates xn|n and xn+1|n respectively.

This last point is crucial, as it means that the estimated covariance depends on the current estimate. That, in addition to the fact that the EKF outputs only an approximation of the probability P (x n |x, y 1 ,...,y n ) which is not a Gaussian anymore, is well-known to make the EKF lose the optimality properties of the linear case.

The EKF is an error state filter, where the covariance P n represents the uncertainty of the error (2.6), e n ⇠N(0, P n ). By the way, one compute the Jacobian after first-order approximation of the error, which is obtained for propagation, when noise is turned off, as

e n+1|n = x n+1 xn+1|n
(2.11)

= f (x n , u n , 0) f (x n|n , u n , 0)

= f (x n|n + e n|n , u n , 0) f (x n|n , u n , 0)

= ⇠ ⇠ ⇠ ⇠ ⇠ ⇠ f (x n|n , u n , 0)+ @f @x | xn|n e n|n ⇠ ⇠ ⇠ ⇠ ⇠ ⇠ f (x n|n , u n , 0)+o(e n|n ) = @f @x | xn|n e n|n + o(e n|n ), (2.12)
where f (x n|n + e n|n , u n , 0)=f (x n|n , u n , 0)+ @f @x | xn|n e n|n + o(e n|n ). Thus it appears that Fn = @f @x | xn|n albeit Fn is not defined as the first-order Taylor expansion of f (•) and change if, e.g., e n = p kx n xn k. The same holds for Ĝn and Ĥn .

3 3 Nonlinear Filtering: the Unscented Kalman Filter (UKF)

The Unscented Kalman Filter (UKF) [START_REF] Julier | A New Extension of the Kalman Filter to Nonlinear Systems[END_REF][START_REF] Julier | Unscented Filtering and Nonlinear Estimation[END_REF] is an alternative to the EKF for handling non-linear dynamics or observations (2.9)-(2.10) that uses a deterministic sampling technique known as the unscented transform to pick a minimal set of sample points (called sigma points) around the mean. The sigma points are then propagated through the nonlinear functions, from which a new state estimate and error state covariance are inferred. UKF is arguably more accurate than EKF, and to some extent simpler to implement for the practitioner not familiar with Kalman filter theory (no analytical Jacobian are required for an UKF).

It exists several variant of UKFs [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] and we consider here the one that provides a direct analogy with the EKF [START_REF] Lefebvre | Comment on "A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Rstimators[END_REF][START_REF] Huang | A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM[END_REF], where the unscented transform numerically computes statistical Jacobians. Thus the UKF follows similar steps as the EKF version, up to the following differences:

• F n , G n , and H n in (2.1) and (2.2) are replaced by Fn , Ĝn and Ĥn , which are the statistical Jacobians of f (•) and h(•). As for the EKF, they are computed at the estimates xn|n and xn+1|n respectively and depend on the state estimates.

Jacobians are inferred following Algorithm 1 and Algorithm 2, where sigma point parameters are computed in set_weights(•), see Algorithm 3, where the parameter ↵ is chosen by the practitioner, typically ↵ = 10 3 , and controls the spread of the sigma points. This technique has two main advantages over EKF and one caveat: it removes the requirement to explicitly calculate Jacobians, performs generally better than the EKF, but its computational complexity is cubic in the size of the state vector (the computational complexity of EKF is quadratic).

Introduction to Filtering on Manifolds

We now consider the situation where the state lives on a manifold M, see [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF] for a introduction to manifold with optimization perspective, and [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF] which is a reference from the robotics perspective. We now provide a rather "loose" definition of a manifold. Definition 1 (manifold). A manifold M is a set whose subsets are identified through charts (injective mappings) with subsets of R n .

Consider a practical example where the state lives on the 2-sphere manifold, modeling e.g., a spherical pendulum [START_REF] Kotaru | Variation Based Extended Kalman Filter on S2[END_REF], see Figure 2.1 where 2 w 0 = /( + d)+3 ↵ 2 ; 3 w j =1/(2(d + )),j =1,...,2d;

x n 2 S 2 := {x 2 R 3 |k xk =1}. ( 2
Output: , {w j } j=0,...,2d ;

such that the estimated state xn+1|n+1 as no reason to stay on the 2-sphere. The Kalman filter indeed assumes x n+1 lives in a vector space R p . However, it exists many problems in robotics involving rotation, pose and sphere which are not vector space but manifold. Two problems directly appears here:

• the error-state covariance P n has dimension three although the state has only two degree of freedom. In addition, P n is supposed to represent a statistical dispersion over the manifold, and this very concept is even not clear;

• one would hope that e n and xn remains at all times in the manifold, however adding elements of a manifold does not make sense as

x 1 , x 2 2M; x 1 + x 2 2M. (2.15)
The addition operator + is just not defined on a manifold, and xn needs to be constrained on the manifold.

An instinctive solution consists in changing the coordinate of the state space and writing the systems in term of spherical coordinates ( ,✓), see Figure 2.1, as = arctan(y/x) (2.16) ✓ = arccos(z).

(2.17) This is especially cumbersome as one need to define a new filter where the observation becomes nonlinear, while now state estimates have singularities at the north and south poles (for x = 0). This raises important questions when designing a filter on a manifold M:

• How may we design an error e n that handles the manifold constraints? Is it only related to the coordinate system we used? How may we avoid singularity issues?

• How may we perform state update and compute xn+1|n+1 ? How to compute Jacobians?

• Solving those questions naturally leads to different ways to define a state error. So, which error should be used for a given problem?

Manifolds are highly general objects, and one may search to answer these questions in the more restrictive case when the manifold is a matrix Lie group (and hence comes with additional structure).

Matrix Lie Groups

In this section we recall the definitions and basic properties of matrix Lie groups and Lie algebra. A matrix Lie group G 2 R n⇥n is a set of square invertible matrices that is a group, i.e., the following properties hold:

I n 2 G; 8χ 2 G,χ 1 2 G; 8χ 1 , χ 2 2 G,χ 1 χ 2 2 G. (2.18)
Matrix Lie groups are particular examples of Lie groups, which are sets being

• groups, i.e., have an inner product, an inverse function, and a neutral element (e.g., matrix product, matrix inverse and identity for matrix Lie groups);

• differential manifolds, i.e., one can derive at each point on them;

• and such that the inner product and the inverse function are smooth for the manifold structure.

Although Lie group theory can get quite involved, all the considered sets in this thesis are matrix Lie groups, therefore we will restrict to them.

Locally about the identity matrix I n , the group G can be identified with a vector space R d using the matrix exponential map exp m (•), where d = dim(G). Indeed, to any ξ 2 R d one can associate a matrix ξ ^of the tangent space of G at I n , called the Lie algebra g, and we call (•) ^: R d ! g the wedge operator. We then define the exponential map exp : R d ! G for Lie groups as exp(ξ) = exp m (ξ ^) (2.19) Locally, around zero, it is a bijection, and one can define the Lie logarithm map log : G ! R d as its inverse log(exp(ξ)) = ξ. Two specific matrix Lie groups are heavily used in robotics: the special orthogonal group, denoted SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], which can represent rotations, and the special Euclidean group, SE [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], which can represent poses. Additionally, in [START_REF] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF][START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF], the author noticed there is a natural Lie group structure underlying the 3D SLAM problem, called SE p [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], which resolves some consistency issues of the conventional EKF based SLAM, see Chapter 5.

The Special Orthogonal Group SO(3)

The special orthogonal group, representing rotations, is the set of valid rotation matrices

SO(3) = n R 2 R 3⇥3 |RR T = I, det(R)=1 o .
(2.20)

The RR T = I orthogonality condition impose six constraints on the nine-parameter rotation matrix, thereby reducing the number of degrees of freedom to three. SO(3) is clearly not a vector space: it is not closed under addition and the zero matrix does live in SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF].

The inverse, wedge operator and exponential maps are given as We note here the wedge operator corresponds to the skew-symmetric operator (•) ⇥ . The inverse log(•) is the vector associated to the skew-symmetric matrix defined as

R 1 = R T , ( 2
log(R)= ✓ 2 sin(✓) (R R T ), (2.24)
where ✓ = arccos ⇣ trace(R) 1 2 ⌘ .

The Special Euclidean Group SE(3)

The special Euclidean group, representing poses (i.e., translation and rotation), is the set of valid transformation matrices

SE(3) := ( T = " Rp 0 1 # 2 R 4⇥4 |R 2 SO(3), p 2 R 3
) , (2.25) and are thus represented by a matrix T (a.k.a. homogeneous coordinates), where R denotes a rotation matrix and p a translation. The inverse, wedge operator and exponential maps are given as

T 1 = " R T R T p 0 1 # , (2.26) ξ ^= " φ ⇥ ρ 0 0 # , (2.27) exp(ξ)=I 4 + ξ ^+ 1 cos(✓) ✓ 2 (ξ ^)2 + ✓ sin(✓) ✓ 3 (ξ ^)3 , ( 2.28)
where ξ =[φ T , ρ T ] T and ✓ = kφk. The inverse log(•) involves the logarithm of SO(3), log SO(3) (•), and is defined as

log(T)= " log SO(3) (R) (I 3 1 2 φ ⇥ + 1 ✓ 2 (1 A 2B )(φ ⇥ ) 2 )ρ # , ( 2.29)
where A = sin(✓)/✓ and B = (1 cos(✓))/✓ 2 . For more information about SE [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] and its use in state estimation for robotics see the recent monographs [START_REF] Barfoot | State Estimation for Robotics[END_REF][START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF].

The Group SE p (3)

The group SE p [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] is an extension of the group SE [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] introduced in [START_REF] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF][START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] essentially for SLAM, and is defined as follows

SE p (3) := ( T = " Rp 1 ••• p p 0I p # 2 R 3+p⇥3+p |R 2 SO(3), p 1 ,...,p p 2 R 3
) , (2.30) and are thus represented by a matrix T, where R denotes a rotation matrix and p 1 ,...,p p are translations. The dimension of the group, and thus of the Lie algebra, is 3 + 3p. The inverse, wedge operator and exponential maps are given as

T 1 = " R T R T p 1 ••• R T p p 0I p # , ( 2.31
)

ξ ^= " φ ⇥ ρ 1 ••• ρ p 00 # , ( 2
where A = sin(✓)/✓, B = (1 cos(✓))/✓ 2 , and J =(I 3 2 ). We leverage this group in Chapter 5 and Chapter 6, where we embed the robot orientation R 2 SO(3), velocity v 2 R 3 and position p 2 R 3 along with the position of landmarks p i 2 R 3 in a matrix χ 2 SE p (3) defined as

1 2 φ ⇥ + 1 ✓ 2 (1 A 2B )(φ ⇥ )
χ = " Rvpp 1 ••• p p 2 0I p # .
(2.35)

Nonlinear & Invariant Observers on Lie Groups

There has been a huge body of research devoted to nonlinear and invariant observers and filters on Lie groups over the past decade, especially for attitude estimation, where invariant observers are deterministic observers meant to be appropriate to dynamical systems possessing symmetries [START_REF] Bonnabel | Observateurs asymptotiques invariants: théorie et exemples[END_REF].

In the early 2000s, there were essentially two streams of research that bolstered the development of observers on Lie groups. The first was initiated by [START_REF] Aghannan | On Invariant Asymptotic Observers[END_REF] and seeks to design nonlinear observers that share the symmetries of the original system. The theory was formalized and developed in [START_REF] Bonnabel | Symmetry-Preserving Observers[END_REF] and applied to estimation on Lie groups in [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF]. At the same time, the complementary filter on SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] for attitude estimation was introduced in [START_REF] Mahony | Nonlinear Complementary Filters on the Special Orthogonal Group[END_REF]. This filter makes extensive use of left-invariant errors on SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] and the autonomy properties of the error equation. Owing to its simplicity and global convergence guarantees, it has become a renowned attitude estimator and has proved useful for micro aerial vehicle control. [START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF][START_REF] Bohn | Unscented State Estimation for Rigid Body Motion on SE(3)[END_REF][START_REF] Batista | A GES Attitude Observer with Single Vector Observations[END_REF][START_REF] Khosravian | Observers for Invariant Systems on Lie Groups with Biased Input Measurements and Homogeneous Outputs[END_REF][START_REF] Lee | Global Unscented Attitude Estimation via the Matrix Fisher Distributions on SO(3)[END_REF][START_REF] Zlotnik | Nonlinear Estimator Design on the Special Orthogonal Group Using Vector Measurements Directly[END_REF][START_REF] Walsh | Invariant Sliding Window Filtering for Attitude and Bias Estimation[END_REF] study observers that are akin to the complementary filter on SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] for attitude estimation, and similar ideas have been applied to pose estimation in [START_REF] Scandaroli | A Nonlinear Observer Approach for Concurrent Estimation of Pose, IMU bias and Camera-to-IMU Rotation[END_REF] and homography in [START_REF] Malis | Estimation of Homography Dynamics on the Special Linear Group[END_REF]. Invariant observer theory were then developed in e.g. [START_REF] Batista | A GES Attitude Observer with Single Vector Observations[END_REF][START_REF] Zlotnik | Higher Order Nonlinear Complementary Filtering on Lie Groups[END_REF][START_REF] Hashim | Nonlinear Stochastic Attitude Filters on the Special Orthogonal Group 3: Ito and Stratonovich[END_REF][START_REF] Berkane | Hybrid Attitude and Gyro-Bias Observer Design on SO(3)[END_REF] for attitude estimation, [START_REF] Bonnabel | A Non-Linear Symmetry-Preserving Observer for Velocity-Aided Inertial Navigation[END_REF][START_REF] Barczyk | Invariant EKF Design for Scan Matching-Aided Localization[END_REF] for navigation, [START_REF] Lageman | Gradient-Like Observers for Invariant Dynamics on a Lie Group[END_REF] for gradient-like observers, and applied in [START_REF] Diemer | An invariant Linear Quadratic Gaussian Controller for a Simplified Car[END_REF] for the design of invariant controllers.

The idea to use similar techniques for noisy (instead of deterministic) systems on Lie groups lead to the more recent theory of invariant Kalman filtering.

Invariant Extended Kalman Filter

The first introduction of invariant Kalman filtering is presented to our best knowledge in [START_REF] Bonnabel | Left-Invariant Extended Kalman Filter and Attitude Estimation[END_REF] that was then applied for attitude estimation in [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF]. These results showed that, when considering distributions inspired from the Gaussian but specifically designed to take symmetries into account, theoretical results similar to those of the linear case could be derived, and have been applied in [START_REF] Condomines | Nonlinear State Estimation Using an Invariant Unscented Kalman Filter[END_REF].

In turn, this showed that there was a kind of filters which could efficiently encode the past of the system, and carry out reliable estimation of local navigation in the long run [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF][START_REF] Barrau | Three examples of the stability properties of the invariant extended Kalman filter[END_REF]. This framework, based on the theory of Lie groups, is known as invariant filtering, and especially the Invariant Extended Kalman Filter (IEKF). Its theoretical and practical advantages were brought to light by the Ph.D. thesis of Axel Barrau [START_REF] Barrau | Non-Linear State Error Based Extended Kalman Filters with Applications to Navigation[END_REF], in particular with high-grade IMUs. Its extension to H1 norm [START_REF] Lavoie | An Invariant Extended H infini Filter[END_REF] and smoothing has been recently proposed in [START_REF] Chauchat | Invariant Smoothing on Lie Groups[END_REF][START_REF] Chauchat | Kalman Filtering with a Class of Geometric State Equality Constraints[END_REF]. Generally speaking, the IEKF is close to the approach of [START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF], however these did not leverage its links with group affine dynamics and group actions, and therefore autonomous errors. The IEKF can be seen as an extension of the Multiplicative EKF [START_REF] Crassidis | Unscented Filtering for Spacecraft Attitude Estimation[END_REF] for more general state spaces.

The IEKF [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Barrau | Invariant Kalman Filtering[END_REF] is a nonlinear Kalman filter variant devised to estimate systems where the state live a on matrix Lie groups, e.g. SO(3) for attitude estimation, where the state is a rotation matrix. We note here the state χ n 2 G to make clear the distinction with a state living in Euclidean space.

Its goal is to track the mean and covariance of a distribution, χ = ' ( χ, ξ) , ξ ⇠N(0, P) , (2.36) where ' : M⇥R d !Mis a smooth function, which is defined Gaussian in the Lie algebra g of the state space (see Chapter 4). The IEKF is designed to be applied to specific systems: as the Kalman filter is adapted to linear systems, the IEKF is wellsuited for the so called linear observed systems on groups [START_REF] Barrau | Linear Observed Systems on Groups[END_REF]. Thus, the IEKF make advantages of:

• the state-space structure G. Notably, the estimated state χn is guarantee to remain in the group without any ad hoc reproduction;

• the system equations f (•) and h(•), such that the dynamics of the error is autonomous, i.e. the error does not depend on state estimate. This is due the computation of the Jacobian, e.g. F n , which does not depend on χn and avoid thus some linearization error.

Considered State-Space Systems

Let us define two types of discrete linear systems in a matrix Lie group G as:

Left invariant system χ n+1 = f (χ n , u n ) exp(w n ) (2.37) y n+1 = χ n+1 d + n n (2.38)

Right invariant system

χ n+1 = f (χ n , u n ) exp(w n ) (2.39
)

y n+1 = χ 1 n+1 d + n n (2.40)
In these systems, w n and n n are noise similarly defined as in Section 2, the propagation function f (•) respects the group affine dynamics property [START_REF] Barrau | Linear Observed Systems on Groups[END_REF]

8u, a, b f (a, u)f (I, u) 1 f (b, u), (2.41)
and with d a vector. The two observation models are respectively left and right equivariant, each associated to a variant of the IEKF: the left IEKF and the right IEKF.

Invariant Extended Kalman Filter Equations

The Left IEKF: the left IEKF estimates the state of the system through the same propagation and update sequence scheme of the of Kalman filter, where the state error relation is defined as

χ n = χn exp(ξ n ), (2.42)
where ξ n ⇠N(0, P n ) is the error and the exp(

Propagation ( χn+1|n = f ( χn|n , u n ) P n+1|n = F n P n F T n + G n Q n G T n (2.43) Update 8 > > > > > > < > > > > > > : S = H n+1 P n+1|n H T n+1 + N n+1 K = P n+1|n H T n+1 S 1 χn+1|n+1 = χn+1|n exp ⇣ K( χ 1 n+1 (y n+1 d) ⌘ P n+1|n+1 =(I KH n+1 )P n+1|n . (2.44)
Let us first focus on the propagation step. To compute the Jacobian F n and G n , the key idea underlying the EKF is apply, see Section 2, and we linearize the error system through a first-order Taylor expansion of the nonlinear functions f (•) at the estimated state. To compute F n (and similarly G n ), we write

exp(ξ n+1|n )=f ( χn|n , u n ) 1 f (χ n|n , u n ) (2.45) = f ( χn|n , u n ) 1 f ( χn|n exp(ξ n|n ), u n ) (2.46) = f ( χn|n , u n ) 1 f ( χn|n [I + ξ n|n ], u n )+o(kξ n|n k) (2.47) = ⇠ ⇠ ⇠ ⇠ ⇠ ⇠ ⇠ f ( χn|n , u n ) 1 ⇠ ⇠ ⇠ ⇠ ⇠ ⇠ f ( χn|n , u n )[I +( @f @χ | χn|n ,u n ξ n|n ) ^]+o(kξ n|n k) (2.48) = I +( @f @χ | χn|n ,u n ξ n|n ) ^+ o(kξ n|n k) (2.49) I + ξ n+1|n = I +( @f @χ | χn|n ,u n ξ n|n ) ^+ o(kξ n+1|n k, kξ n|n k) (2.50)
where the error definition is used in (2.45)-(2.46) and the first order Baker-Campbell-Hausdorff (BCH) approximation exp(ξ) ' I+ξ ^is applied in (2.47) and (2.50). We thus identify

F n = @f @χ | χn|n ,u n such that ξ n+1|n = F n ξ n|n , (2.51)
where

@f @χ = @f (χ exp(ξ),u) @ξ
is defined by small perturbation acting on right of the state. When f (•) respects (2.41), F n is independent of the state estimate.

The derivation of the update is simpler as we measure a vector y. First, the update is based on the measurement rewritten as

z n+1 = χ 1 n+1 y n+1 (2.52) = exp(ξ n )d + χ 1 n+1 n n . (2.53)
and thus contains a modified innovation step. The Jacobian H n is obtained by the following identification

z ẑ = d exp(ξ n+1|n )d (2.54) = d [I + ξ n+1|n ]d + o(kξ n+1|n k) (2.55) = ξ n+1|n d + o(kξ n+1|n k) (2.56) = H n ξ n+1|n + o(kξ n+1|n ), (2.57)
and we see H n is independent w.r.t. the state estimate.

The Right IEKF: the right IEKF differs from the left IEKF by defining the state error as

χ n = exp(ξ n ) χn , ( 2.58)
and rewriting the measurement as

z n+1 = χn+1 y n+1 (2.59) = exp(ξ n )d + χn+1 n n , (2.60)
where we have a different noise so now N n+1 depends on the state estimate. This leads to the right IEKF algorithm defined as

Propagation ( χn+1|n = f ( χn|n , u n ) P n+1|n = F n P n F T n + G n Q n G T n (2.61) Update 8 > > > > > < > > > > > : S = H n+1 P n+1|n H T n+1 + N n+1 K = P n+1|n H T n+1 S 1 χn+1|n+1 = exp (K( χn+1 (y n+1 d)) χn+1|n P n+1|n+1 =(I KH n+1 )P n+1|n (2.62)
The Jacobians are obtained similarly as for the left IEKF, and H n is also independent w.r.t. state estimates.

CHAPTER 3 Exploiting Symmetries to Design EKFs with Consistency Properties for Navigation and SLAM

Part of the content of this chapter has been published in IEEE Sensors Journal.

Résumé

Le filtre de Kalman étendu peut s'avérer inconsistent en présence d'inobservabilité sous un groupe de symétrie. Dans ce chapitre, nous construisons d'abord un EKF alternatif basé sur une erreur d'état non linéaire alternative. Cet EKF est intimement lié à la théorie de l'EKF invariant. Ensuite, sous une simple hypothèse de compatibilité entre l'erreur et le groupe de symétrie, nous prouvons que le modèle linéarisé de l'EKF alternatif capture automatiquement les directions non observables, et de nombreuses propriétés souhaitables du cas linéaire suivent alors directement. Cela fournit un nouveau résultat fondamental dans la théorie du filtrage. Nous appliquons la théorie à la fusion multi-capteurs pour la navigation, lorsque tous les capteurs sont liés au véhicule et n'ont pas accès à des mesures absolues, comme cela se produit généralement dans des environnements privés de GPS. Dans le contexte du SLAM, des simulations Monte-Carlo et des comparaisons avec l'OC-EKF, l'EKF et les algorithmes de lissage basés sur l'optimisation (iSAM) illustrent les résultats.

Chapter abstract

The extended Kalman filter may prove inconsistent in the presence of unobservability under a group of transformations. In this chapter we first build an alternative EKF based on an alternative nonlinear state error. This EKF is intimately related to the theory of the Invariant EKF. Then, under a simple compatibility assumption between the error and the transformation group, we prove the linearized model of the alternative EKF automatically captures the unobservable directions, and many desirable properties of the linear case then directly follow. This provides a novel fundamental result in filtering theory. We apply the theory to multi-sensor fusion for navigation, when all the sensors are attached to the vehicle and do not have access to absolute information, as typically occurs in GPS-denied environments. In the context of SLAM, Monte-Carlo runs and comparisons to OC-EKF, robocentric EKF, and optimization-based smoothing algorithms (iSAM) illustrate the results.

Introduction

We first start with a very basic example that explains the specificity of the theoretical estimation problem posed by SLAM. Then we proceed with a brief literature review about the subproblem we consider.

Introduction to SLAM Unobservability

As in SLAM we only have relative measurements between components of the state, the Kalman filter tends to correlate those components after each measurement. This correlation profoundly modifies the information that the filter possesses about the system, although it does not necessarily improve the marginal knowledge about each component (e.g., the localization of the robot which is what we are most interested in throughout this thesis). This is why it is not obvious for the reader unfamiliar with this problem to grasp what is at play in the theoretical SLAM problem and this is why we present the following simple example. Consider a linear one-dimensional mobile robot at position m that observes one landmark, at position l, as represented in Figure 3.1. The robot is equipped with a device (a telemeter) that allows to measure the distance to the landmark. The range measurement of the landmark viewed from the robot writes

y n = l n m n + w n , ( 3.1)
where w n is a Gaussian noise (the uncertainty associated with the telemeter's measurement). We search to estimate m and l with a Kalman filter with initial state estimation error e 0|0 =[m 0 m0|0 ,l 0 l0|0 ] whose initial covariance is P 0|0 = diag([ m , l ] 2 ), i.e. the robot has a prior (uncertain) knowledge on the landmark and on its own position. Assume the robot now observes the landmark with a perfect telemeter (w n has standard deviation equal to zero), which is an extreme yet illustrative case. The covariance matrix of the state error, after this measurement has been incorporated writes according to the Kalman filter's equations

P 1|1 = 2 m 2 l 2 m + 2 l " 11 11 # , ( 3.2)
which has rank 1. In this limit of perfect measurement, the landmark and robot estimates become fully correlated. As max

( 2 m , 2 l ) 2 m 2 l 2 m + 2 l min( 2 m , 2 l )
, we see the uncertainty on both the robot's and landmark's position has decreased, albeit not necessarily much (typically a factor 2). However, full correlation implies that the instant when the robot knows perfectly its position, it may infer the exact position of the landmark.

What we see is that the state remains uncertain after measurement has been performed (and will always remain so if we don't have access to absolute measurements), but in a particular direction of the state space (that is, the Kernel of the covariance matrix [1, 1] T ), much information has been gained: in this direction the filter has aquired full certainty. This makes the SLAM problem quite specific.

In more control theoretic terms, this is a matter of observability: the SLAM problem is unobservable, which is rather uncommon in state estimation for engineering problems. Indeed, the robot obtains only relative information as shifting the system as m n m n + d,l n l n + d lets the measurement y n unchanged. This hold even if the robot is moving following

m n+1 = m n + u n , ( 3.3)
with u n a known input (e.g. stemming from odometers) and that multiple observations are done, as shifting the robot lets the propagation function unchanged (if m n m n + d and m n+1 m n+1 + d, (3.3) still holds). We say d = [1, 1] T is an observable direction, or degree of freedom, of the system (3.1)-eq:robot1d. Indeed, we saw that [1, 1] T is a vector along which there is no statistical dispersion (it lies in the Kernel of the covariance matrix), and its orthocomplement is in fact a direction in which on the contrary no information may be acquired. The intuitive explanation is clear [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF]: "if the robot and landmark positions are shifted equally, it will not be possible to distinguish the shifted position from the original one through the measurements.".

In terms of mathematics, the fact the Kalman filter may not acquire information in this direction may be explained by resorting to the so-called information matrix, that is, the inverse of the covariance matrix. We have indeed that at all times and no matter the measurements d T P 1 n|n d is non-increasing [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] as long as measurements are relative to the robot's frame. This holds for this toy example as

d T P 0|0 d = d T P n|n d = 2 r + 2 l , (3.4)
However this consistency property w.r.t. unobservable directions is not guaranteed when we move to nonlinear problems: when using an EKF P n|n depend on state estimates through the computations of Jacobians about the estimated trajectory. If this quantity is increasing, we say the filter acquires spurious information along the direction d and this is what is referred to as inconsistency in the present context of statistical filtering. We now provide the three main examples of unobservable directions for SLAM systems.

Example 1 (2D SLAM). The state consists on the robot position, the robot heading, and landmark positions [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF]. There are three unobservable direction corresponding to the global translation and rotation of the system.

Example 2 (3D monocular visual SLAM). The state consists on the robot position, the robot heading, and landmark positions in 3D where observation are provided with a monocular camera. There are seven unobservable directions: corresponding to the global translation, global rotation and scale of the system [START_REF] Li | High-Precision, Consistent EKF-Based Visual-Inertial Odometry[END_REF].

Example 3 (3D stereo visual SLAM).

The state is the same as in Example 2 where observation are provided with a stereo camera. There are six unobservable directions: corresponding to the global translation and global rotation of the system.

Example 4 (3D visual inertial SLAM). The state is the same as in Example 2 enhanced with robot velocity, where an IMU provides scale information and allow to recover roll and pitch. There are thus four unobservable directions corresponding to the global translation and the rotation around gravity (yaw) [START_REF] Hesch | Camera-IMUbased Localization: Observability Analysis and Consistency Improvement[END_REF].

EKF SLAM Inconsistency: Previous Literature

EKF inconsistency is defined as the fact that the filter returns a covariance matrix that is too optimistic [START_REF] Bar-Shalom | Estimation with Applications to Tracking and Navigation[END_REF], leading to inaccurate estimates. EKF inconsistency in the context of SLAM has been the object of many papers, see e.g. [START_REF] Li | High-Precision, Consistent EKF-Based Visual-Inertial Odometry[END_REF][START_REF] Hesch | Camera-IMUbased Localization: Observability Analysis and Consistency Improvement[END_REF][START_REF] Julier | A Counter Example to the Theory of Simultaneous Localization and Map Building[END_REF][START_REF] Castellanos | Limits to the Consistency of EKF-based SLAM[END_REF][START_REF] Bailey | Consistency of the EKF-SLAM Algorithm[END_REF][START_REF] Huang | Towards Consistent Filtering for Discrete-Time Partially-Observable Nonlinear Systems[END_REF][START_REF] Huang | Analysis and Improvement of the Consistency of Extended Kalman Filter Based SLAM[END_REF][START_REF] Huang | Towards Consistent Visual-Inertial Navigation[END_REF][START_REF] Huang | Optimal-State-Constraint EKF for Visual-Inertial Navigation[END_REF][START_REF] Huang | Observability-Based Consistent EKF Estimators for Multi-Robot Cooperative Localization[END_REF]. Theoretical analysis [START_REF] Julier | A Counter Example to the Theory of Simultaneous Localization and Map Building[END_REF][START_REF] Huang | Analysis and Improvement of the Consistency of Extended Kalman Filter Based SLAM[END_REF][START_REF] Huang | Observability-Based Consistent EKF Estimators for Multi-Robot Cooperative Localization[END_REF] reveals inconsistency is caused by the inability of EKF to reflect the unobservable degrees of freedom of SLAM raised in the above section. Indeed, the filter tends to erroneously acquire information along the directions spanned by these unobservable degrees of freedom. The Observability Constrained (OC)-EKF [START_REF] Huang | Towards Consistent Filtering for Discrete-Time Partially-Observable Nonlinear Systems[END_REF][START_REF] Huang | Observability-Based Consistent EKF Estimators for Multi-Robot Cooperative Localization[END_REF] constitutes one of the most advanced solutions to remedy this problem and has been fruitfully adapted, e.g. for VIO, cooperative localization, and unscented Kalman filter [START_REF] Huang | A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM[END_REF][START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF][START_REF] Li | High-Precision, Consistent EKF-Based Visual-Inertial Odometry[END_REF]. The idea is to pick a linearization point that is such that the unobservable subspace "seen" by the filter is of appropriate dimension.

Contributions

In this chapter we propose a novel general theory. We first propose to build EKFs based on an alternative error e = ⌘(χ, χ), generalizing the IEKF methodology [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF], where we opt for EKF to obtain theoretical guarantee which can not be demonstrated with UKF. This means the covariance matrix P reflects the dispersion of e, and not of χ χ. When unobservability stems from symmetries, the technique may resolve the consistency issues of the EKF. Indeed symmetries are encoded by the action ↵ (χ) of a transformation group G [START_REF] Olver | Classical Invariant Theory[END_REF], where ↵ 2 G denotes the corresponding infinitesimal unobservable transformation. Under the simple condition that the image of matrix @ @↵ ⌘( ↵ (χ), χ) is independent of χ, the EKF based on e is proved to possess the desirables properties of the linear case regarding unobservability, and is thus consistent.

Specific Application to SLAM

The specific application to SLAM was released on Arxiv in 2015 [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] as part of Axel Barrau's PhD and encountered immediate successes reported in [START_REF] Barrau | Invariant Kalman Filtering[END_REF][START_REF] Zhang | Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF][START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF][START_REF] Caruso | Improving Visual-Inertial Navigation Using Stationary Environmental Magnetic Disturbances[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF], although the work was never published elsewhere.

More precisely, [START_REF] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF] notice back the SLAM problem bears a nontrivial Lie group structure. [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] formalize the group introduced in [START_REF] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF] and call it SE l+1 [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], and proved that for odometry based SLAM, using the right invariant error of SE l+1 [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] and devising an EKF based on this error, i.e., a Right-Invariant EKF (RIEKF), the linearized system possesses the desirable properties of the linear case, since it automatically correctly captures unobservable directions for SLAM. Thus, virtually all properties of the linear Kalman filter regarding unobservability may be directly transposed: the information about unobservable directions is non-increasing (see Proposition 2), the dimension of the unobservable subspace has appropriate dimension (this relates to the result of OC-EKF [START_REF] Huang | A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM[END_REF][START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF][START_REF] Huang | Observability-Based Consistent EKF Estimators for Multi-Robot Cooperative Localization[END_REF]), the filter's output is invariant to linear unobservable transformations, even if they are stochastic and thus change the EKF's covariance matrix along unobservable directions [START_REF] Zhang | Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM[END_REF]. The right-invariant error for the proposed Lie group structure was also recently shown to lead to deterministic observers having exponential convergence properties in [START_REF] Mahony | A Geometric Nonlinear Observer for Simultaneous Localisation and Mapping[END_REF].

Along the same lines, using the right-invariant error of the group SE 2 (3), [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] propose alternative consistent IEKF for visual inertial SLAM and VIO applications [START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF][START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF][START_REF] Caruso | Improving Visual-Inertial Navigation Using Stationary Environmental Magnetic Disturbances[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF]. In particular, [START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF] demonstrate that an alternative Invariant MSCKF based on the right-invariant error of SE 2 (3) naturally enforces the state vector to remain in the unobservable subspace, a consistency property which is preserved when considering point and line features [START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF], or when a network of magnetometers is available [START_REF] Caruso | Improving Visual-Inertial Navigation Using Stationary Environmental Magnetic Disturbances[END_REF].

Chapter's Organization

Section 3 presents the general theory. Section 4 applies the theory to the general problem of navigation in the absence of absolute measurements, as typically occurs in GPSdenied environments. Section 5 is dedicated to SLAM and compares the proposed EKF to conventional EKF, OC-EKF [START_REF] Huang | Observability-Based Consistent EKF Estimators for Multi-Robot Cooperative Localization[END_REF], robocentric mapping filter [START_REF] Castellanos | Robocentric Map Joining: Improving the Consistency of EKF-SLAM[END_REF] and iSAM [START_REF] Kaess | iSAM2: Incremental Smoothing and Mapping using the Bayes Tree[END_REF][START_REF] Kummerle | G2o: A General Framework for Graph Optimization[END_REF].

Preliminary ideas and results can be found in the preprint [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] posted on Arxiv in 2015. Although the present chapter is a major rewrite, notably including a novel general theory encompassing the particular application to SLAM of [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF], [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] is a technical report serving as preliminary material for the present chapter. Matlab codes used for the chapter are available at https://github.com/CAOR-MINES-ParisTech/esde.

General Theory

Let us consider the following dynamical system in discrete time with state χ n 2Mand observations y n 2 R p :

χ n = f (χ n 1 , u n , w n ) , (3.5) y n = h (χ n , v n ) , (3.6)
where f (•) is the function encoding the evolution of the system, w n ⇠N(0, Q n ) is the Gaussian process noise, u n is the input, h(•) is the observation function and v n ⇠ N (0, R n ) a Gaussian measurement noise. This model is slightly more general than (4.13)- (4.14).

We now define mathematical symmetries, see [START_REF] Bonnabel | Symmetry-Preserving Observers[END_REF][START_REF] Olver | Classical Invariant Theory[END_REF].

Definition 2. An action of a (Lie) group G on M is defined as a family of bijective maps

↵ : M!M,↵2 G satisfying 8χ 2M Id (χ)=χ, (3.7) 8↵, 2 G,χ 2M ↵ ⇣ (χ) ⌘ = ↵ (χ) , ( 3.8)
where Id corresponds to the identity of the group G.

Definition 3. Let • (•) be defined as in (3.7)- (3.8). We say that system (3.5)-(3.6) is totally invariant under the action of

• (•) if 1. the dynamics are equivariant under • (•), i.e., 8↵,χ, u, w ↵ (f (χ, u, w)) = f ( ↵ (χ), u, w) , (3.9)

the observation map

h(•) is invariant w.r.t. • (•) 8↵,χ h( ↵ (χ), v)=h(χ, v). (3.10)
"Symmetry" is defined as invariance to transformations ↵ (•). Throughout the chapter we will rather use the term invariant, along the lines of the preceding definition.

As in this chapter we pursue the design of consistent EKFs, we will focus on the system "seen" by an EKF: it consists of the linearization of system (3.5)-(3.6) about the estimated trajectory ( χn ) n 0 in the state space. Along the lines of [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF][START_REF] Huang | Analysis and Improvement of the Consistency of Extended Kalman Filter Based SLAM[END_REF] we use the linearized system about a trajectory.

Let (χ n ) n 0 denote a solution of (3.5) with noise turned off. The local observability matrix [START_REF] Chen | Local Observability Matrix and its Application to Observability Analyses[END_REF] at χ n 0 for the time interval between time-steps n 0 and n 0 + N is defined as

O(χ n 0 )=
H n 0 H n 0 +1 F n 0 +1
. . . with the Jacobians

H n 0 +N F n 0 +N •••F n 0 +1
F n = @f @χ | χ n 1 ,u n ,w n , H n = @h @χ | χ n ,v n .
First we show the directions spanned by the action of G are necessarily unobservable directions of the linearized system, that is, they lie in the kernel of the observability matrix.

@ @↵ ↵ ↵=Id (f (χ, u, w)) = @f @χ | (χ,u,w) @ @↵ ↵ ↵=Id (χ), (3.12
)

@h @χ | (χ,v) @ @↵ ↵ ↵=Id (χ)=0 8χ 2M. (3.13)
Let (χ n ) n 0 denote a solution of (3.5) with noise turned off. (3.13) applied at χ n 0 yields H n 0 @ @↵ ↵ | Id (χ n 0 )=0. Considering then (3.12) at χ n 0 leads to

@ @↵ ↵ | Id ⇣ f (χ n 0 , u n 0 +1 , w n 0 +1 ) ⌘ = F n 0 +1 @ @↵ ↵ | Id ⇣ χ n 0 ⌘ . Applying (3.13) at χ n 0 +1 = f (χ n 0 , u n 0 +1 , w n 0 +1 ) yields H n 0 +1 @ @↵ ↵ | Id ⇣ χ n 0 +1 ⌘ = 0,
and thus

H n 0 +1 F n 0 +1 @ @↵ ↵ | Id ⇣ χ n 0 ⌘ = 0. A simple recursion proves @ @↵ ↵ | Id ⇣ χ n 0 ⌘ ⇢ Ker O(χ n 0 ).
Indeed, no matter the number of observations and moves, we are inherently unable to detect an initial (infinitesimal) transformation ↵ (•), the problem being invariant to it.

Observability Issues of the Standard EKF

Let ( χn ) n 0 be a sequence of state estimates given by an EKF. The linearized system "seen" by the EKF involves the estimated observability matrix Ô( χn 0 )= where Jacobians are computed at the estimates. The directions spanned by • (•)a t χn 0 necessarily lie in Ker O( χn 0 ), as proved by Proposition 1, but there is a null probability that they lie in Ker Ô( χn 0 ), because of the noise and Kalman updates, see [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF]. A major consequence is that the EKF gains spurious information along the unobservable directions.

In fact, this problem stems from the choice of estimation error χ χ that does not match unobservability of system (3.5)-(3.6): changing the estimation error may resolve the problem.

EKF Based on a Nonlinear Error

In this section, we define an EKF based on a nonlinear function ⌘(χ, χ) 2Mthat provides an alternative to the usual linear estimation error χ χ. We prove consistency under compatibility assumptions of the group action and ⌘(•).

The methodology builds upon the alternative errors

e n 1|n 1 = ⌘ ⇣ χ n 1 , χn 1|n 1 ⌘ , ( 3.15)
e n|n 1 = ⌘ ⇣ χ n , χn|n 1 ⌘ . (3.16)
The filter is displayed in Algorithm 4. As the covariance matrix P e is supposed to reflect the dispersion of e, we need to define Jacobians w.r. At line 4, e + n denotes the (best) error estimate according to the EKF. However, defining the (best) state corresponding to estimate χn|n is not straightforward as in the linear case where χn|n = χn|n 1 + e + n . At line 5 we use a retraction ' : M⇥R q !M , that is, any function ' (•) which is consistent with the error to the first order, i.e., e + n ⇡ ⌘( χn|n , χn|n 1 ). This will make the link between this chapter and Chapter 4, where we first choose the retraction and then implicitly define the error. Note that, we recover the conventional EKF if we let e = ⌘ (χ, χ) = χ χ be the usual linear error.

Compatibility Assumptions and Main Consistency Result

The matrix2 @ @↵ | Id ⌘( ↵ (χ), χ) reflects how infinitesimal transformations of the state produced by the action of G affect the error variable e = ⌘(•). In Assumption 1 below this matrix is used to define a kind of "compatibility" between an invariance group G and a nonlinear error function ⌘, which leads to the main result of this chapter (Theorem 1). (3.11), and thus they are unobservable. We have also recalled this is not true for the linearized system seen by the EKF, i.e. for Ô( χn 0 ) [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF]. We have the following powerful result: Proof. Let Mn := @ @↵ | Id ⌘( ↵ ( χn ), χn ). Recalling (3.18), we have h( ↵ ( χn )) h( χn ) ' Ĥe n ⌘( ↵ ( χn ), χn ) ' Ĥe n Mn ↵ with ↵ a linearized approximation to ↵ 2 G. But also h( ↵ ( χn )) h( χn ) ' @h @χ | ( χn ,0) @ @↵ | Id ( χn ) ↵. From (3.13) the latter is 0.

Thus Ĥe

n u = 0 for any u 2C, i.e. Ĥe n C = 0. Using (3.17), Fe is defined as This proves the result through an immediate recursion.

⌘ (f (χ),f ( χ)) ' Fe ⌘ (χ, χ), thus ⌘(f ( ↵ ( χn )),f ( χn )) ' Fe n+1 ⌘( ↵ ( χn ), χn ) ' Fe n+1 Mn ↵. Besides, using (3.9) yields ⌘(f ( ↵ ( χn )),f ( χn )) =⌘( ↵ (f ( χn )),f ( χn )) ' @ @↵ | Id ⌘( ↵ (f ( χn )),f ( χn )) ↵ 2Capplying As- sumption 1 at f ( χn ).
We obtain the consistency property we pursue: the linearized model has the desirable property of the linear Kalman filter regarding the unobservabilities, when expressed in terms of error ⌘(•). As a byproduct, the unobservable subspace seen by the filter is automatically of appropriate dimension.

Consequences in Terms of Information

In the linear Gaussian case, the inverse of the covariance matrix output by the Kalman filter is the Fisher information available to the filter (as stated in [START_REF] Bar-Shalom | Estimation with Applications to Tracking and Navigation[END_REF] p. 304). Thus, the inverse of the covariance matrix P 1 n|n output by any EKF should reflect an absence of information gain along unobservable directions. Otherwise, the output covariance matrix would be too optimistic, i.e., the filter inconsistent [START_REF] Bar-Shalom | Estimation with Applications to Tracking and Navigation[END_REF].

Note that, the covariance matrix P e n|n reflects the dispersion of the error e of (3.15)-(3.16), as emphasized by the superscript e. We have the following consistency result: Theorem 2. Let u n 0 2 Im @ @↵ | Id ⌘( ↵ ( χn 0 ), χn 0 ) be an unobservable direction spanned by • (•) at the estimate χn 0 , measured using alternative error ⌘(•). Let (u n ) n n 0 with u n = Fe n u n 1 be its propagation through the linearized model. Under Assumption 1 the Fisher information according to the filter about (u n ) n n 0 is non-increasing , i.e.,

u T n (P e n|n ) 1 u n  u T n 1 (P e n 1|n 1 ) 1 u n 1 . (3.20)
Proof. At propagation step we have u T n (P e n|n 1 ) The theorem essentially ensures the linearized model of the filter has a structure which guarantees that the covariance matrix at all times reflects an absence of "spurious" (Bayesian Fisher) information gain over unobservable directions, ensuring strong consistency properties of our alternative EKF.

1 u n = u T n 1 ( Fe n ) T ( Fe n P e n 1|n 1 ( Fe n ) T + Ĝe n Q n ( Ĝe n ) T ) 1 Fe n u n 1 6 u T n 1 ( Fe n ) T ⇣ Fe n P e n 1|n 1 ( Fe n ) T ⌘ 1 Fe n u n 1 since Q is positive semidef- inite. As ( Fe n ) 1

Application to Multi-Sensor Fusion for Navigation

In this section, we consider a navigating vehicle or a robot equipped with sensors which only measure quantities relative to the vehicle's frame. Thus the vehicle cannot acquire information about its absolute position and orientation, which results in inevitable unobservability. The state space is M = SO(3) ⇥ R 3l+3m+k and the state χ is defined as

χ = (R, p 1 , ••• , p l , v 1 , ••• , v m , b) 2M, (3.21)
where R 2 SO(3) represents the orientation of the vehicle, i.e., its columns are the axes of the vehicle's frame, and where

1. {p i 2 R 3 } i=1,...,
(3) = {↵ =( R ↵ , p ↵ ), R ↵ 2 SO(3), p ↵ 2 R 3 }. Given ↵, 2 SE(3), the group op- eration is ↵ =(R ↵ R , R ↵ p + p ↵ ) and the inverse ↵ 1 =(R T ↵ , R T ↵ p ↵ ).
We denote Id the identity.

Changes of global frame are encoded as the action ↵ (χ) of an element ↵ 2 SE(3) on M. Thus, quantities expressed in the global frame (such as the vehicle position) are rotated and translated by the action ↵ (•), whereas quantities expressed in the vehicle's frame, such as Inertial Measurement Unit (IMU) biases, are left unchanged. The action then writes (with i =1,...,l and j =1,...,m) (3.25) we see the directions infinitesimally spanned by • (•)atχ in the direction ↵ = ( ω ⇥ , p) write for the state (3.21):

↵ (χ)= ⇣ R ↵ R, R ↵ p i + p ↵ , R ↵ v j , b ⌘ . ( 3
=1,m =0,k =0). Writing (R ↵ , p ↵ ) = exp SE(3) [( ω ⇥ , p)] in
@ @↵ ↵ | Id (χ) ↵ = ⇣ ω ⇥ R, ω ⇥ p i + p, ω ⇥ v j , b ⌘ (3.23)
where i =1,...,l and j =1,...,m.

Example 5.

[SLAM] Consider a simple SLAM system with one robot and one landmark [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF]. Let p R be the position of the robot, R the orientation of the robot, and p L the landmark's position. The state is

χ = (R, p R , p L ) 2M= SO(3) ⇥ R 6 . (3.24)
The dynamics write f (χ, u, w)=(R R, p R + R R p, p L ) where R, p denote orientation and position increments typically measured through odometry. The observation of the landmark in the robot's frame is of the form y = h(R T (p L p R )). Translations and rotations of the global frame correspond to actions of elements ↵ =(R ↵ , p ↵ ) 2 SE(3) as

↵ (χ)=(R ↵ R, R ↵ p R + p ↵ , R ↵ p L + p ↵ ) . (3.25)
The system is obviously invariant.

Referring to (3.23), as l =2,m =0and p R = p 1 , p L = p 2 , the directions spanned by • (•) at χ are as follows

( ω ⇥ R, ω ⇥ p R + p, ω ⇥ p L + p) . ( 3
↵ (χ)=(R ↵ R, R ↵ p R + p ↵ , R ↵ v, b) , (3.28)
where we restrict R ↵ to be around the gravity axis, i.e. with R ↵ g = g, since the vertical is measured [START_REF] Li | High-Precision, Consistent EKF-Based Visual-Inertial Odometry[END_REF]. The action • (•) is then also invariant.

EKF Based on a Nonlinear Error

For the general state χ of (3.21) consider the nonlinear error

⌘(χ, χ)= ⇣ R RT , pi RR T p i , vj RR T v j , b b⌘ . (3. 29
)
where i =1,...,l and j =1,...,m. We set l = m = 1 for simplicity. To linearize, we have the following first order vector approximation of (3.29) that lives in

R 3(1+l+m)+k ⌘(χ, χ)= ⇣ e R , p (e R ) ⇥ p, v (e R ) ⇥ v, b b⌘ , ( 3.30)
R RT = exp SO(3) (e R ) ' I +(e R ) ⇥ + o(ke R k 2 ), (3.31)
where e R 2 R 3 . Proof. For ↵ 2 SE(3), using (3.29) we obtain that

⌘( ↵ (χ), χ)= ⇣ R ↵ RR T , p R(R ↵ R) T [R ↵ p + p ↵ ], v R(R ↵ R) T R ↵ v, b b ⌘ = ⇣ R ↵ , R T ↵ p ↵ , 0 3 , 0 k ⌘ , ( 3.32)
such that ⌘( ↵ (χ), χ) turns out to be independent of χ, and the result is readily obtained by differentiation at ↵ = Id.

Choice of the Retraction

When the state space is a Lie group, and one uses errors that are invariant with respect to right multiplication, the theory of IEKF [START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and Application to a Velocity-Aided Attitude Estimation Problem[END_REF][START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] suggests to use ' (χ, e) = exp(e)χ where exp(•) denotes the Lie group exponential. In [START_REF] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF], it was noticed a natural Lie group structure underlies the (odometry based) SLAM problem. [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF]

' (χ, e) = ⇣ R + R, R + p i + p + i , R + v j + v + j , b + b + ⌘ (3.33)
Error EKF [START_REF] Huang | Observability-Based Consistent EKF Estimators for Multi-Robot Cooperative Localization[END_REF] proposed, see (3.29) Robocent. [START_REF] Castellanos | Robocentric Map Joining: Improving the Consistency of EKF-SLAM[END_REF] orientation

✓ ✓ ✓ ✓ ✓ ✓ position pR p R pR R( ✓)R(✓) T p R R( ✓) T pR R(✓) T p R landmark pL p L pL R( ✓)R(✓) T p L R( ✓) T ( pL pR ) R(✓) T (p L p R )
Figure 3.2: Alternative state error definitions on the 2D SLAM problem for different solutions (extension of the approach to 3D is immediate), with state χ =(✓,p R , p L ) with p R the position of the robot, ✓ its orientation, and p L a landmark position. R(✓) denotes the planar rotation of angle ✓.

with i =1,...,l, j =1,...,m, and where

" R + p + 1 ••• p + l v + 1 ••• v + m 0 I 3l+3m # (3.34)
:= I + S + 1 cos(ke R k) ke R k 2 S 2 + ke R k sin(ke R k) ke R k 3 S 3 , ( 3.35)
S := " (e R ) ⇥ e p 1 ••• e p l e v 1 ••• e v m 0 0 3l+3m # , ( 3.36)
b + = e b , 0 = 0 3+3m+3l⇥3 and I = I 3+3m+3l⇥3 .

Extension to Problems Involving Multiple Robots

Consider a problem consisting of M systems. We have M global orientations, one for each system, that transform as the global frame's orientation. For such problems, we define a collection of χ i of (3.21), and the alternative state error of the problem ⌘(•) merely writes

⌘ (χ 1 ,...,χ M ) = (⌘ 1 (χ 1 , χ1 ) ,...,⌘ M (χ M , χM )) , ( 3.37)
where ⌘ i (χ i , χi ) is the error (3.29) for the i-th system.

Simulation Results

This section considers the 2D wheeled-robot SLAM problem and illustrates the performances of the proposed approach. We conduct similar numerical experiment as in the sound work [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF] dedicated to EKF inconsistency and benefits of the OC-EKF, i.e., a robot makes 7 circular loops and 20 landmarks are disposed around the trajectory, see Figure 3.3. We refer the interested reader to the available Matlab code for the parameter setting and reproducing the present results. We compare our approach to standard EKF which conveys an estimate of the linear error; OC-EKF [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF] which linearizes the model in a nontrivial way to enforce the unobservable subspace of Ô to have an appropriate dimension; robocentric EKF [START_REF] Castellanos | Robocentric Map Joining: Improving the Consistency of EKF-SLAM[END_REF][START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF][START_REF] Lourenço | Simultaneous Localization and Mapping for Aerial Vehicles: A 3-D Sensor-Based GAS Filter[END_REF] which express the state in the robot's frame and then devise an EKF; and iSAM [START_REF] Kaess | iSAM2: Incremental Smoothing and Mapping using the Bayes Tree[END_REF][START_REF] Kummerle | G2o: A General Framework for Graph Optimization[END_REF], a popular optimization technique both for SLAM and odometry estimation which finds the most likely state trajectory given all past measurements. The differences between the estimation errors used by the various EKF variants are recapped in Figure 3.2.

The results confirm the consistency guarantees of Theorem 1 and Proposition 2 are beneficial to the EKF in practice. Consistency Evaluation: the NEES [START_REF] Bar-Shalom | Estimation with Applications to Tracking and Navigation[END_REF] provides information about the filter consistency, such that NEES > 1 reveals an inconsistency issue: the actual uncertainty is higher than the computed uncertainty. As expected, the NEES of the robot pose estimates in Figure 3.4 indicates that the standard EKF is inconsistent, whereas the other approaches are more consistent. The proposed EKF and iSAM obtain the best NEES, whereas the NEES of OC-EKF and robocentric EKF slightly increase after the first turn, i.e. at the first loop closure.

Accuracy Performances: we evaluate accuracy through RMSE of the robot position error. This confirms that: "solving consistency issues improves the accuracy of the estimate as a byproduct, as wrong covariances yield wrong gains" [START_REF] Bar-Shalom | Estimation with Applications to Tracking and Navigation[END_REF]. Numerical results are displayed in Figure 3.1.

Distance to Maximum-Likelihood Estimate: we use as a third performance criterion distance to the estimates returned by iSAM [START_REF] Kaess | iSAM2: Incremental Smoothing and Mapping using the Bayes Tree[END_REF], which are optimal in the sense that it returns the Maximum A Posteriori (MAP) estimate. To this respect, we see that the proposed EKF is the closest to iSAM.

Execution Time: we provide the execution time of the filters for the 100 Monte-Carlo runs in Table 3.1, which are implemented in Matlab and tested on Precision Tower 7910 armed with CPU E5-2630 v4 2.20 Hz. The iSAM's execution time is not included since it cannot be compared: it is implemented using C++ and an optimized code, whereas we used Matlab based simulations. It is thus evidently lower. Regarding computational complexity, our proposed filter has similar complexity as the standard EKF and OC-EKF, since its EKF-based structure makes it quadratic in the state dimension, i.e., number of landmarks. The use of a retraction at the update step instead of mere We see proposed EKF and iSAM are the more consistent, followed by OC-EKF and robocentric EKF, whereas standard EKF is not consistent. The accuracy is evaluated in terms of the robot position RMSE. Standard EKF shows degraded performances as compared to others, which all achieve comparable performances. Finally, filters are evaluated in terms of average proximity of robot's estimated position with iSAM's, which computes the most likely state χ n given all past measurements y 1 ,...,y n . It is used as a reference of the best achievable estimate. We see that the proposed EKF are the closest to iSAM. Dashed lines correspond to 3 confidence upper bounds, and we see EKF is over-optimistic. Figures best seen in color.

addition may slightly increase the computational burden, but the impact in the execution time proves negligible. The robocentric filter is penalized because it moves the landmarks during propagation, which in turn impacts the propagation of the covariance matrix. In our solution landmarks remain fixed during propagation. Note that the proposed solution can be implemented using recent techniques [START_REF] Leung | Decentralized Cooperative SLAM for Sparsely-Communicating Robot Networks: A Centralized-Equivalent Approach[END_REF][START_REF] Huang | Sparse Local Submap Joining Filter for Building Large-Scale Maps[END_REF] to decrease computational load. To implement the robocentric and OC-EKF we report that we used the code of [START_REF] Huang | Observability-Based Rules for Designing Consistent EKF SLAM Estimators[END_REF], see Acknowledgments.

These simulations confirm that regarding SLAM, the proposed filter is an alternative to the OC-EKF. Contrarily to OC-EKF the model is linearized at the (best) estimate, and is thus much closer to standard EKF methodology, and applies to a large class of problems without explicit computation of the unobservable directions.

Chapter abstract

This chapter introduces a novel methodology for unscented Kalman filtering. Beyond filtering performances, the main interests of the approach are its versatility, as the method applies to numerous state estimation problems, and its simplicity of implementation for practitioners not being necessarily familiar with manifolds and Lie groups. We have developed the method on two independent open-source Python and Matlab frameworks we call UKF-M, for quickly implementing and testing the approach. The online repositories contain tutorials, documentations, and various relevant robotics examples that the user can readily reproduce and then adapt, for fast prototyping and benchmarking. The code is available at https://github.com/CAOR-MINES-ParisTech/ukfm.

Introduction

The present chapter tackles the more general problem of what happens to the Kalman filter when the state variable lives in a space which is not a vector space. Indeed, in this case the Kalman state error e n = x n xn introduced at Equation (2.6) does not even make sense. This is a problem as the Kalman filter theory is based on it, and even in the case where the equations are non-linear this error plays a key role to apply the EKF methodology. In this thesis, we chose to focus on the unscented based variant of the Kalman filter, namely the UKF, to study how it may be adapted to the context where the state variable is not a vector. A similar work for the EKF could be carried out, however it had been done already in part in the literature. Moreover, to treat this subject thoroughly one needs to properly study the way the equations are linearized on the manifold, and to derive a rigorous EKF on manifolds the notion of connection (in the sense of differential geometry) arises and complicates the exposure. In our work dedicated to UKF, we tried to keep things as simple as possible, and to excessively limit the prerequisites. This way we hope our approach may prove useful to the practioner, especially in the field of robotics.

Literature Review of Kalman Filtering on Manifolds

Filtering on manifolds is historically motivated by aerospace applications where one seeks to estimate (besides other quantities) the orientation of a body in space. Much work has been devoted to making the EKF work with orientations, namely quaternions or rotation matrices. The idea is to make the EKF estimate an error instead of the state directly, leading to error state EKFs [START_REF] Leffens | Kalman Filtering for Spacecraft Attitude Estimation[END_REF][START_REF] Hertzberg | Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds[END_REF][START_REF] Forbes | Continuous-Time Norm-Constrained Kalman Filtering[END_REF][START_REF] Solà | Quaternion Kinematics for the Error-State Kalman Filter[END_REF] and their UKF counterparts [START_REF] Lee | Global Unscented Attitude Estimation via the Matrix Fisher Distributions on SO(3)[END_REF][START_REF] Crassidis | Unscented Filtering for Spacecraft Attitude Estimation[END_REF][START_REF] Kraft | A Quaternion-Based Unscented Kalman Filter for Orientation Tracking[END_REF]. The set of orientations of a body in space is the Lie group SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] and efforts devoted to estimation on SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] have paved the way to EKF on Lie groups, see [START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF][START_REF] Bonnabel | Left-Invariant Extended Kalman Filter and Attitude Estimation[END_REF][START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Barrau | Invariant Kalman Filtering[END_REF][START_REF] Barrau | Intrinsic Filtering on Lie Groups With Applications to Attitude Estimation[END_REF][START_REF] Bourmaud | Discrete Extended Kalman Filter on Lie Groups[END_REF] and unscented Kalman filtering on Lie groups, see [START_REF] Bohn | Unscented State Estimation for Rigid Body Motion on SE(3)[END_REF][START_REF] Lee | Global Unscented Attitude Estimation via the Matrix Fisher Distributions on SO(3)[END_REF][START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF][START_REF] Bohn | Unscented State Estimation for Rigid Body Attitude Motion with a Finite-Time Stable Observer[END_REF][START_REF] Loianno | Visual Inertial Odometry for Quadrotors on SE(3)[END_REF][START_REF] Forbes | Sigma Point Kalman Filtering on Matrix Lie Groups Applied to the SLAM Problem[END_REF][START_REF] Svacha | Inertial Yaw-Independent Velocity and Attitude Estimation for High-Speed Quadrotor Flight[END_REF].

Lie groups play a prominent role in robotics [START_REF] Solà | A Micro Lie theory for State Estimation in Robotics[END_REF] and have drawn increasing attention for computer vision and robotics applications [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Forster | On-Manifold Preintegration for Real-Time Visual-Inertial Odometry[END_REF][START_REF] Barrau | Aligment Method for an Inertial Unit[END_REF]. In the context of state estimation and localization, viewing poses as elements of the Lie group SE(3) has proved relevant [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF][START_REF] Wang | Error Propagation on the Euclidean Group with Applications to Manipulator Kinematics[END_REF][START_REF] Park | Kinematic State Estimation and Motion Planning for Dtochastic Nonholonomic Systems using the Exponential Map[END_REF][START_REF] Chirikjian | Gaussian Approximation of Non-Linear Measurement Models on Lie Groups[END_REF][START_REF] Barfoot | Pose Estimation using Linearized Rotations and Quaternion Algebra[END_REF][START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Hartley | Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation[END_REF]. The use of the novel Lie group SE 2 (3) introduced in [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] has led to drastic improvement of Kalman filters for robot state estimation [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Barrau | Invariant Kalman Filtering[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF][START_REF] Hartley | Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation[END_REF][START_REF] Ko | Features of Invariant Extended Kalman Filter Applied to Unmanned Aerial Vehicle Navigation[END_REF][START_REF] Ko | Improvement of Extended Kalman Filter Using Invariant Extended Kalman Filter[END_REF][START_REF] Wang | A Globally Exponentially Stable Nonlinear Hybrid Observer for 3D Inertial Navigation[END_REF][START_REF] Brossard | Unscented Kalman Filter on Lie Groups for Visual Inertial Odometry[END_REF]. Similarly, using group SE k (n) introduced for SLAM in [START_REF] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF][START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] makes EKF consistent or convergent [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF][START_REF] Zhang | Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF][START_REF] Mahony | A Geometric Nonlinear Observer for Simultaneous Localisation and Mapping[END_REF][START_REF] Brossard | Exploiting Symmetries to Design EKFs with Consistency Properties for Navigation and SLAM[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Navigation Using Points and Lines[END_REF]. Specifically for visual inertial odometry purpose, in [START_REF] Loianno | Visual Inertial Odometry for Quadrotors on SE(3)[END_REF], the authors devise an UKF that takes advantage of the Lie group structure of the robot's (quadrotor) pose SE [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], and uses a probability distribution directly defined on the group (the distributions in [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]. Finally, there has been attempts to devise UKFs respecting natural symmetries of the systems' dynamics, namely the invariant UKF, see [START_REF] Condomines | Nonlinear State Estimation Using an Invariant Unscented Kalman Filter[END_REF][START_REF] Condomines | Pi-Invariant Unscented Kalman Filter for Sensor Fusion[END_REF].

We introduces a novel and general framework for UKF on manifolds that is simpler than existing methods, and whose versatility allows direct application to all manifolds encountered in practice. Indeed, [START_REF] Loianno | Visual Inertial Odometry for Quadrotors on SE(3)[END_REF][START_REF] Svacha | Inertial Yaw-Independent Velocity and Attitude Estimation for High-Speed Quadrotor Flight[END_REF] proposes UKF implementations based on the Levi-Civita connection but mastering differential geometry is difficult. [START_REF] Bohn | Unscented State Estimation for Rigid Body Motion on SE(3)[END_REF][START_REF] Lee | Global Unscented Attitude Estimation via the Matrix Fisher Distributions on SO(3)[END_REF][START_REF] Bohn | Unscented State Estimation for Rigid Body Attitude Motion with a Finite-Time Stable Observer[END_REF][START_REF] Loianno | Visual Inertial Odometry for Quadrotors on SE(3)[END_REF] are reserved for SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF] and SE(3), while [START_REF] Forbes | Sigma Point Kalman Filtering on Matrix Lie Groups Applied to the SLAM Problem[END_REF] is reserved for Lie groups and requires more knowledge of Lie theory than the present chapter.

Organization of the Chapter

In Section 2, we introduce a user-friendly approach to UKF on parallelizable manifolds. Section 3 applies the approach in the particular case where the manifold is a Lie group and recovers [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF], but without requiring much knowledge of Lie groups. Section 4 describes the open sourced framework. We then show in Section 5 the method The cylinder is a parallelizable manifold. We can define vector fields V 1 ,V 2 that form a basis of the tangent space at any point. may actually be extended to numerous manifolds encountered in robotics. The conclusion section discusses theoretical issues and provides clarifications related to Kalman filtering on manifolds.

Unscented Kalman Filtering on Parallelizable Manifolds

In this section we describe our simple methodology for UKF on parallelizable manifolds. We assume the reader to have approximate prior knowledge and intuition about manifolds and tangent spaces.

Parallelizable Manifolds

In order to "write" the equations of the extended or the unscented Kalman filter on a manifold, it may be advantageous to have global coordinates for tangent spaces that extends Definition 1.

Definition 6 (smooth manifold).

A smooth manifold M of dimension d is said parallelizable if there exists a set of smooth vector fields {V 1 ,V 2 , ••• ,V d } on the manifold such that for any point χ 2Mthe tangent vectors {V 1 (χ),V 2 (χ), ••• ,V d (χ)} form a basis of the tangent space at χ. Example 7 (R p ). For the vector space x 2 R p one can choose as vector fields:

V 1 (x)=e 1 ,..., V p (x)=e p , ( 4.1)
where the vector e i has zero except 1 at the i-th position. A specificity of R p is that the tangent vectors are independent of the point x and its tangent space is naturally identify with R p .

Example 8 (cylinder). The cylinder {x

=[x,y,z] T 2 R 3 | x 2 +y 2 =1} is a basic example with d =2. V 1 (x)=[y, x,0] T and V 2 = [0, 0, 1]
T are two tangent vectors that form a local basis at x, see Figure 4.1. The cylinder is a simple case but the notion of parallelizable manifolds is much broader. In particular, all Lie groups are parallelizable manifolds.

Example 9 (SO(3)). For the rotation matrices R 2 SO(3) one can choose as vector fields:

V 1 (R)=Re 1 ,V 2 (R)=Re 2 ,V 3 (R)=Re 3 , (4.2)
where e 1 = [1, 0, 0] T , e 2 = [0, 1, 0] T , and e 3 = [0, 0, 1] T .

Example 10 (SE(3)). Similarly as for rotation, one can choose as vector fields for pose T 2 SE(3):

V 1 (T)=Te 1 ,V 2 (T)=Te 2 ,V 3 (T)=Te 3 , V 4 (T)=Te 4 ,V 5 (T)=Te 5 ,V 6 (T)=Te 6 . (4.
3)

It should be noted, though, that not all manifolds fall in this category. However, we will see in Section 5 how this issue can be addressed over-parameterizing the state.

Uncertainty Representation on Parallelizable Manifolds

Our goal is to estimate the state χ 2Mgiven all the sensor measurements. As sensors are flawed, it is impossible to exactly reconstruct χ. Instead, a filter maintains a "belief" about the state, that is, its statistical distribution given past sensors' readings. The Kalman filter in R d typically maintains a Gaussian belief such that χ ⇠N( χ, P) , which may be re-written in the form:

χ = χ + ξ, ξ ⇠N(0, P) . (4.4)
We see that the belief is encoded using only a mean estimate χ, and a covariance matrix P that encodes the extent of dispersion of the belief around the estimate. The error is defined as ξ = χ χ which is coherent with (2.6). Consider a parallelizable manifold M, and let {V 1 ,V 2 , ••• ,V d } denote the associated vector fields. To devise a similar belief on M, one needs of course local coordinates to write the mean χ 2M. This poses no problem, though. The harder part is to find a way to encode dispersion around the estimate χ. It is now commonly admitted that the tangent space at χ should encode such dispersion, and that covariance P should hence reflect dispersion in the tangent space. As additive noise (4.4) makes no sense for χ 2M, we define a probability distribution χ ⇠N ' ( χ, P), for the random variable χ 2Mas χ = ' ( χ, ξ) , ξ ⇠N(0, P) , (

where ' : M⇥R d !Mis a smooth function chosen by the user and satisfying ' ( χ, 0) = χ. In (4.5), ξ 2 R d is a random Gaussian vector that encodes directions of the tangent space at χ, N (., .) is the classical Gaussian distribution in Euclidean space, and P 2 R d⇥d the associated covariance matrix; and we also impose the Jacobian of '(•)at( χ, 0) w.r.t. ξ to be Identity, see [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF]. Using the parallelizable manifold property, we implicity use coordinates in the tangent space, as ξ =(⇠ (1) ,⇠ (2)

, ••• ,⇠ (d) ) T 2 R d encodes the tangent vector ⇠ (1) V 1 ( χ)+•••+ ⇠ (d) V d ( χ).
Hence '(•) is called a "retraction", see [START_REF] Absil | Optimization Algorithms on Matrix Manifolds[END_REF]. In (4.5), the noise-free quantity χ is viewed as the mean, and the dispersion arises through '(•). We stress that the distribution defined at (4. Finding an appropriate map '(•) is not always straightforward. However there exists in theory some "canonical" '(•). Proposition 1. One may define define '( χ, ξ) as the point of M obtained by starting from χ and integrating the vector field P d i=1 ξ (i) V i during one unit of time. In that case we call '(•) an "exponential map".

V 1 (R)+! 2 V 2 (R)+! 3 V 3 (
However, we sometimes have no closed form for the exponential map and one resorts to simpler retractions '(•).

Bayesian Estimation Using the Unscented Transform

Consider a random variable χ 2Mwith prior probability distribution p (χ). Suppose we obtain some additional information about χ through a measurement y. The goal is to compute the posterior distribution p(χ|y). Let

y = h (χ) + n, (4.6)
be a measurement, where h(•):M!R p represents the observation function and n ⇠ N (0, ) is a white Gaussian noise in R p with known characteristics. The problem of Bayesian estimation we consider is as follows:

1. assume the prior distribution to follow (4.5) with known parameters χ and P;

2. assume one measurement y of (4.6) is available;

3. approximate the posterior distribution as

p(χ|y) ⇡ '( χ+ , ξ + ), (4.7)
where ξ + ⇠N(0, P + ), and find parameters χ+ and P + .

Letting χ = ' ( χ, ξ) in (4.6), we see y provides an information about ξ ⇠N(0, P) and we may use the unscented transform of [START_REF] Julier | A New Extension of the Kalman Filter to Nonlinear Systems[END_REF][START_REF] Julier | Unscented Filtering and Nonlinear Estimation[END_REF] to approximate the posterior p(ξ|y) for ξ as follows, see Algorithm 5: we compute a finite number of samples ξ j , j =1 ,...,2d, and pass each of these so-called sigma points through the measurement function

y j = h ⇣ '( χ, ⇣ ξ j ⌘⌘ ,j=1,...,2d. (4.8)
By noting y 0 = h('( χ, 0)) we then compute successively the measurement mean ŷ = w m y 0 + P 2d j=1 w j y j , the measurement covariance P yy = P 2d j=0 w j (y j ŷ)(y j ŷ)

⇣ χ, ξ + ξ + ⌘ ⇡ ' ⇣ ' ⇣ χ, ξ⌘ , ξ + ⌘ .
When M = R d the latter equality holds up to the first order in the dispersions ξ, ξ + , both assumed small. In the case where M is not a vector space, it may be geometrically interpreted as saying that moving from χ along the direction ξ + ξ + approximately consists in moving from χ along ξ and then from the obtained point on M along ξ + .

Unscented Kalman Filtering on Parallelizable Manifolds

Consider the dynamics

χ n = f (χ n 1 , ω n , w n ) , ( 4.13)
where the state χ n lives in a parallelizable manifold M, ω n is a known input variable and w n ⇠N(0, Q n ) is a white Gaussian noise in R q . We consider observations of the form We define sigma points using (4.5) and the statistics of noise w n , and pass them through (4.13). Then, to find χn one is faced with the optimization problem of computing a weighted mean on M. This route has already been advocated in [START_REF] Lee | Global Unscented Attitude Estimation via the Matrix Fisher Distributions on SO(3)[END_REF][START_REF] Crassidis | Unscented Filtering for Spacecraft Attitude Estimation[END_REF][START_REF] Kraft | A Quaternion-Based Unscented Kalman Filter for Orientation Tracking[END_REF][START_REF] Forbes | Sigma Point Kalman Filtering on Matrix Lie Groups Applied to the SLAM Problem[END_REF]. However, to keep the implementation simple and analog to the EKF, we suggest to merely propagate the mean using the unnoisy state model, leading to χn = f ( χn 1 , ω n , 0).

y n = h (χ n ) + n n , ( 4
(4.16)

To compute the covariance P n from P n 1 of ξ n 1 we use the fact w n and ξ n 1 are uncorrelated and proceed in two steps.

1. we generate sigma points in R d corresponding to P n 1 and pass them through the unnoisy model (4.16) for nonlinear propagation of P n 1 through f . We obtain points χ j n on the manifold M, and the distribution of propagated state is described as ' ( χn , ξ n ), with χn known from (4.16). We need to be able to locally invert ξ 7 ! '( χ, ξ), i.e., to find a map denoted by

' 1 χ (•):M!R d such that ' 1 χ ('( χ, ξ)) = ξ + O(||ξ|| 2 ), (4.17)
that is, a map that allows one to assess the discrepancy between χ and '( χ, ξ) is ξ indeed. Then we use ' 1 χn to map sigma points χ j n back into R d and compute their empirical covariance Σ n .

2. we then generate sigma points for process noise w n similarly and obtain another covariance matrix encoding dispersion in R d owed to noise, that adds up to Σ n and thus clearly distinguish the contribution of the state error dispersion ξ n from noise w n . When a new measurement arrives, belief is updated via Algorithm 5.

Algorithm 6 summarizes both steps, where the weights defined through set_weights(d,↵) depend on a scale parameter ↵ (generally set between 10 3 and 1), and sigma point dimension, see [START_REF] Julier | A New Extension of the Kalman Filter to Nonlinear Systems[END_REF][START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] and documentation in source code. Using (4.16) to propagate the mean while using sigma points to compute covariance is also done in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF], in the particular case of pose compounding on SE(3), with '(•) the SE(3) exponential map.

Application to UKF on Lie Groups

To apply the preceding methodology to any d-dimensional group G = M, one first defines a basis of the Lie algebra. Then, to any vector ξ 2 R d , recall one may associate an element denoted by ξ ^of the Lie algebra g. Let the vee operator _ denote its inverse, as in e.g., [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF]. The Lie exponential map maps elements of the Lie algebra to the group. In (4.5) we may choose '( χ, ξ) := χ exp m (ξ ^)= χ exp(ξ), which corresponds to left concentrated Gaussians on Lie groups [START_REF] Bourmaud | Discrete Extended Kalman Filter on Lie Groups[END_REF]. Note that, in the Lie group case, choosing left invariant vector fields for the V i 's and following Proposition 1 we exactly recover the latter expression.

We may invert '(•) using the logarithm map exp 1 (•) := log(•) of G, and we get

'( χ, ξ) := χ exp(ξ),' 1 χ (χ) := log ⇣ χ 1 χ ⌘ . ( 4
Σ n = P 2d j=1 w j ' 1 χn (χ j n ) ✓ ' 1 χn (χ j n ) ◆ T ;
// proceed similarly for noise 6 , {w j } j=0,...,2q = set_weights(q,↵);

7 w j = col( p ( + q)Q n ) j ,j=1,...,q, w j = col( p ( + q)Q n ) j d ,j= q +1,...,2q; 8 χj n = f ( χn 1 , ω n , w j ),j =1,...,2q; 9 P n = Σ n + P 2q j=1 w j ' 1 χn ( χj n )('

Uncertainty on Lie Groups

Based on the above section and Section 2.2, it appears two natural manners to define random variables on Lie groups. We first define the probability distribution χ ⇠ N R ( χ, P) for the random variable χ 2 G as [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Wang | Error Propagation on the Euclidean Group with Applications to Manipulator Kinematics[END_REF] χ = exp (ξ) χ, ξ ⇠N(0, P) , (

where P 2 R d⇥d is a covariance matrix. In (4.20), the original Gaussian ξ of the Lie algebra is moved over by right multiplication to be centered at χ 2 G, hence the letter R which stands for "right", this type of uncertainty being also referred to as rightequivariant [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF]. In (4.20), χ may represent a large, noise-free and deterministic value, whereas P is the covariance of the small, noisy perturbation ξ. As (4.4), we have defined this probability density function directly in the vector space

R d such that N R (•) is not Gaussian distribution.
One can similarly define the distribution χ ⇠N L ( χ, P) for left multiplication of χ, as χ = χ exp (ξ) , ξ ⇠N(0, P) . (4.21)

The advantages of using (4.20) for SLAM, instead of (4.5) or a standard Euclidean error can be found in [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF].

Applications in Mobile Robotics: the Group SE k (d)

It is well known that orientations of body in spaces are described by elements of SO [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF].

It is also well known that the use of SE( 3) is advantageous to describe the position and the orientation of a robot (pose), especially for estimation, see [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF][START_REF] Wang | Error Propagation on the Euclidean Group with Applications to Manipulator Kinematics[END_REF][START_REF] Park | Kinematic State Estimation and Motion Planning for Dtochastic Nonholonomic Systems using the Exponential Map[END_REF][START_REF] Chirikjian | Gaussian Approximation of Non-Linear Measurement Models on Lie Groups[END_REF][START_REF] Barfoot | Pose Estimation using Linearized Rotations and Quaternion Algebra[END_REF][START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Hartley | Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation[END_REF]. In [START_REF] Barrau | Non-Linear State Error Based Extended Kalman Filters with Applications to Navigation[END_REF][START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] the group of double direct isometries SE 2 (3) was introduced to address estimation problems for robot navigation when the motion equations are based on an IMU. In [START_REF] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF][START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF] where the Lie group methodology has been so far useful (along with trivial extensions to be mentioned in Section 3.3). Since it was introduced for navigation and SLAM, this group has been successfully used in various contexts, see [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][START_REF] Barrau | Invariant Kalman Filtering[END_REF][START_REF] Zhang | Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF][START_REF] Mahony | A Geometric Nonlinear Observer for Simultaneous Localisation and Mapping[END_REF][START_REF] Hartley | Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation[END_REF][START_REF] Ko | Features of Invariant Extended Kalman Filter Applied to Unmanned Aerial Vehicle Navigation[END_REF][START_REF] Ko | Improvement of Extended Kalman Filter Using Invariant Extended Kalman Filter[END_REF][START_REF] Wang | A Globally Exponentially Stable Nonlinear Hybrid Observer for 3D Inertial Navigation[END_REF][START_REF] Brossard | Unscented Kalman Filter on Lie Groups for Visual Inertial Odometry[END_REF][START_REF] Brossard | Exploiting Symmetries to Design EKFs with Consistency Properties for Navigation and SLAM[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Navigation Using Points and Lines[END_REF][START_REF] Wang | Geometric Nonlinear Observer Design for SLAM on a Matrix Lie Group[END_REF].

The Mixed Case

We call mixed the case where M = G ⇥ R N . This typically arises when one wants to estimate some additional parameters besides the state assumed to live in the group G, such as sensor biases, see Chapter 5, Chapter 6, Chapter 8 and Chapter 11. By decomposing the state as χ =( χ1 , χ2 ) 2 G ⇥ R N and letting ξ =( ξ 1 , ξ 2 ), we typically define '(•) through right multiplication as

' ( χ, ξ) = (exp (ξ 1 ) χ1 , χ2 + ξ 2 ) (4.22)
or if left multiplications are privilegiated ' ( χ, ξ) =( χ1 exp (ξ 1 ) , χ2 + ξ 2 ). This way, as many additional quantities as desired may be estimated along the same lines.

Remark 1. When G = SE( 3) for example, it is tempting to let G 0 = SO(3) and to treat SE(3) as SO(3) ⇥ R 3 along the lines of mixed systems. However, in robotics contexts, it has been largely argued the Lie group structure of SE(3) to treat poses is more relevant than SO(3) ⇥ R 3 , as accounting for the coupling between orientation and position leads to important properties, see [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF][START_REF] Wang | Error Propagation on the Euclidean Group with Applications to Manipulator Kinematics[END_REF][START_REF] Park | Kinematic State Estimation and Motion Planning for Dtochastic Nonholonomic Systems using the Exponential Map[END_REF][START_REF] Chirikjian | Gaussian Approximation of Non-Linear Measurement Models on Lie Groups[END_REF][START_REF] Barfoot | Pose Estimation using Linearized Rotations and Quaternion Algebra[END_REF][START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Hartley | Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation[END_REF]. In the same way, SE k (3) resembles SO(3) ⇥ R 3k but has a special noncommutative group structure having recently led to many successes in robotics, see [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][START_REF] Barrau | Invariant Kalman Filtering[END_REF][START_REF] Zhang | Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF][START_REF] Mahony | A Geometric Nonlinear Observer for Simultaneous Localisation and Mapping[END_REF][START_REF] Hartley | Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation[END_REF][START_REF] Ko | Features of Invariant Extended Kalman Filter Applied to Unmanned Aerial Vehicle Navigation[END_REF][START_REF] Ko | Improvement of Extended Kalman Filter Using Invariant Extended Kalman Filter[END_REF][START_REF] Wang | A Globally Exponentially Stable Nonlinear Hybrid Observer for 3D Inertial Navigation[END_REF][START_REF] Brossard | Unscented Kalman Filter on Lie Groups for Visual Inertial Odometry[END_REF][START_REF] Brossard | Exploiting Symmetries to Design EKFs with Consistency Properties for Navigation and SLAM[END_REF][START_REF] Heo | Consistent EKF-Based Visual-Inertial Navigation Using Points and Lines[END_REF][START_REF] Wang | Geometric Nonlinear Observer Design for SLAM on a Matrix Lie Group[END_REF].

Example 14. The state χ for fusing IMU with GNSS may be divided into the vehicle state χ 1 2 SE 2 (3) (orientation, velocity and position of the vehicle) and IMU biases χ 2 = b 2 R 6 , see e.g. our example on the KITTI dataset [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF]. Further augmenting χ 2 with new parameters, e.g. time synchronization and force variables [START_REF] Nisar | VIMO: Simultaneous Visual Inertial Model-Based Odometry and Force Estimation[END_REF], is straightforward.

UKF-M Implementation

We have released both open source Python package and Matlab toolbox UKF-M implementations of our method at https://github.com/CAOR-MINES-ParisTech/ukfm. Both implementations are wholly independent, and their design guidelines pursue simplicity, intuitiveness and easy adaptation rather than optimization. We adapt the code to the user preferences as follow: the Python code follows class-object paradigm and is heavily documented through the Sphinx documentation generator, whereas the Matlab toolbox contains equivalent functions without class as we believe choosing well function names is best suited for the Matlab use as compared to class definition. The following code snippets are based on the Python package that we recommend using.

Recipe for Designing a UKF on Manifolds

To devise an UKF for any fusion problem on a parrallelizable manifold (or Lie group) M the ingredients required in terms of implementation are as follows, see Snippet 1.

1. A model that specifies the functions f and h used in the filter;

2. An uncertainty representation (4.5). This implies an expression for the function '(•) and its inverse ' 1 (•), defined by the user;

3. Filter parameters, that define noise covariance matrices Q n , N n and weights ( , w m , and w j ) through ↵. Noise covariance values are commonly guided by the model and tuned by the practitioner, whereas ↵ is generally set between 10 3 and 1 [START_REF] Julier | A New Extension of the Kalman Filter to Nonlinear Systems[END_REF]. (3:6) , p + ξ (6:9) ⌘ and

) ⇥ R 6 , '( χ, ξ)= ⇣ R ot exp(ξ (0:3) ), v + ξ
' 1 χ (χ) = (log( R ot T Rot), v v, p p).
In the particular case where M is a Lie group we follow the rules above but we simplify step 2) as follows: we pick an uncertainty representation, either (4.18) or (4.19). This directly implies an expression for the map ^and its inverse _, as well as for the exponential exp and its (local) inverse log. Applying the present general methodology for the particular case of Lie groups, we recover the method of [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF].

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 Lie groups following Section 3.1.

Right-UKF-

LG: the state is modeled as χ n ⇠N R ( χn , P n ), that is, using the representation (4.20) 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 ξ variables, and mapped to the group through the model (4.20). 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 Rigid-Body 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.

Left-UKF-LG: the state is alternatively modeled as χ n ⇠N L ( χn , P n ), that is, using the left-equivariant formulation (4.21) of the uncertainties.

Implemented Examples

In the code, we implement the frameworks on relevant vanilla robotics examples which are listed as follows:

• 2D vanilla robot localization tutorial based on odometry and GNSS measurements;

• 3D attitude estimation from an IMU equipped with gyro, accelerometer and magnetometer;

• 3D inertial navigation on flat Earth where the vehicle obtains observations of known landmarks;

• 2D SLAM where the UKFs follows [START_REF] Huang | A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM[END_REF] to limit computational complexity and adding new observed landmarks in the state;

• IMU-GNSS fusion on the KITTI dataset [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF];

• an example where the state lives on the 2-sphere manifold, modeling e.g., a spherical pendulum [START_REF] Kotaru | Variation Based Extended Kalman Filter on S2[END_REF].

We finally enhance code framework, documentation and examples with filter performance comparisons: for each example we simulate Monte-Carlo data and benchmark UKFs and EKFs based on different choices of uncertainty representation (4.5) through accuracy and consistency metrics. [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF], where initial heading and position errors are large, respectively 45 degrees and 1 m. The second UKF, whose uncertainty representation (4.5) is based on SE 2 (3) exponential, see Section 3.2, clearly outperforms the EKF, the first UKF, and improves the EKF of [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] during the first 10 seconds of the trajectory.

Example 17. Figure 4.2 displays two EKFs and two UKFs for inertial navigation in the setting of

Extension to General Manifolds

The main problem when M is not parallelizable is that one cannot define a global uncertainty representation through a map '(•) as in (4.5). Indeed ξ =(ξ (1) , ••• , ξ (d) ) encodes at any χ 2Mcoordinates in the tangent space related to a basis

(V 1 (χ), ••• ,V d (χ))
of the tangent space. On general manifolds, though, it is always possible to cover the manifold with "patches" M 1 , ••• , M K , such that on each patch i we have a set of vector fields (V

(i) 1 , ••• ,V (i)
d ) allowing one to apply our methodology. For instance on the 2-sphere one could choose a North-East frame in between the polar circles, and then some other smooth set of frames beyond polar circles. However two main issues arise. First, we feel such a procedure induces discontinuities at the polar circles that will inevitably degrade the filter perfomances. Indeed by moving χ slightly at the polar circle, one may obtain a jump in the distribution N ' ( χ, P) with fixed covariance P, see Figure 4.3. Then, we see the obtained filter wholly depends on the way patches are chosen, which is undesirable.

The Lifting "Trick"

It turns out a number of manifolds of interest called homogeneous spaces may be "lifted" to a Lie group, hence a parallelizable manifold. By simplicity 1 we consider as a tutorial example the 2-sphere M = S 2 = {x 2 R 3 | ||x|| =1 } with state x n 2 S 2 . As x n+1 and x n necessarily lie on the sphere, they are related by a rotation, that is,

x n+1 = Ω n x n (4.23) Figure 4.3:
We see covering the 2-sphere with 3 parallelizable patches (in between polar circles, and beyond each) inevitably induces discontinuities that may degrade filtering performances. This is a consequence of the theorem that states it is not possible to "comb a hairy ball", see [START_REF] Milnor | Analytic Proofs of the Hairy Ball Theorem and the Brouwer Fixed Point Theorem[END_REF].

with Ω n 2 SO(3) that may be written as exp m (ω n ) exp m (w n ) where ω n is a known input, and w n ⇠N(0, Q n ) represents a noise, see (2.22) for the definition of wedge operator, and exp m (•) is the usual matrix exponential of SO(3). We assume x n is measured through a linear observation, that is,

y n = Hx n + n n 2 R p . ( 4.24)
Example 18. We provide a (novel) script which simulates a point of a pendulum with stiff wire living on a sphere, where two components are measured through e.g. a monocular camera, i.e. H =[e 1 , e 2 ] T .

The dynamics can be lifted into SO(3) by writing x n via a rotation matrix R n , that is, we posit x n = R n L with L 2 R 3 . In terms of R n , dynamics (4.23) may be lifted letting R n+1 = Ω n R n as then R n L satisfies (4.23) indeed. Similarly, the output in terms of R n writes y n = HR n L + n n = h(R n )+n n . Having transposed the problem into estimation on the parallelizable manifold SO(3), we can then apply the two UKFs by setting '(•) to either (4.18) or (4.19).

Covariance Retrieval

The practitioner may wonder how to retrieve the covariance in the original variables. Assume we have a Gaussian vector x ⇠ (µ, Σ), and we want to approximate g(x) as a Gaussian. This might addressed resorting to the unscented transform but a more basic and direct approach is as follows. Consider A a matrix and b a vector. Then it is known from probability theory that

Ax + b ⇠ (Aµ + b, AΣA T ). (4.25)
Then, we can write x = µ + e with e ⇠N(0, Σ) and linearizing we find g(x) ⇡ g(µ)+ @g @x (µ)e and applying linear Gaussian vectors transform yields approximately g(x) ⇠ (g(µ), AΣA T ), where we let A := @g @x (µ). In the 2-sphere example of the present section, our uncertainty representation may be taken as R n = exp(ξ ^) Rn with ξ ⇠N(0, P), see (4.19). As a result it is rather easy to compute the covariance matrix of R n L as follows. We may use linearizations to write that exp(ξ

^) ⇡ I + ξ ^and thus R n L = exp(ξ ^) Rn L ⇡ Rn L + ξ ^R n L = Rn L ( Rn L) ^ξ = Rn L + Aξ with A = ( Rn L) ^.
As a result, the probability distribution of R n L is under a linear approximation N ( Rn L, APA T ).

Concluding Remarks

If we step back a little and look at the bigger picture, we see the main problem when designing filters on a manifold M is that we often lack coordinates to write down the filter equations on M. Even if we do, e.g. longitude and latitude on the sphere, this implicitly defines probability distributions on the manifold in a way that may not suit the problem well, see Figure 4.3. Over the past decades, researchers have advocated the intrinsic approach based on the tangent space [START_REF] Pennec | Intrinsic Statistics on Riemannian Manifolds: Basic Tools for Geometric Measurements[END_REF]. This way the filter becomes independent of a particular choice of coordinates on the manifold, but it depends on the way tangent spaces at different locations correspond. Notably, we see at lines 5, 6, 7, 9 of Algorithm 5 the covariance matrix P + is computed using local information at χ, in total disregard of χ+ , although P + is supposed to encode dispersion at χ+ ! This means it is up to the user to define the way "Gaussians" are transported over M from χ to χ+ , as early noticed in [START_REF] Loianno | Visual Inertial Odometry for Quadrotors on SE(3)[END_REF], see also [START_REF] Svacha | Inertial Yaw-Independent Velocity and Attitude Estimation for High-Speed Quadrotor Flight[END_REF]. The route we have followed herein consists in focusing on parallelizable manifolds where a global coordinate system of tangent spaces exists, and readily provides a transport operation over M.

However, there are multiple choices for the parallel transport operation. In [START_REF] Loianno | Visual Inertial Odometry for Quadrotors on SE(3)[END_REF][START_REF] Svacha | Inertial Yaw-Independent Velocity and Attitude Estimation for High-Speed Quadrotor Flight[END_REF] the authors advocate using the Levi-Civita connection for parallel transport, which depends on the chosen metric, and argue its virtue is that it is torsion free. In the context of state estimation on Lie groups, though, the transport operations that lead to the best performances are not torsion free, see [START_REF] Barrau | Invariant Kalman Filtering[END_REF]. In cases where it is unclear to the user which transport operation (in our case parallelization+retraction) shall be best, we suggest using our code for quick benchmarking, as done in

Chapter abstract

In this chapter, building upon both the theory of Unscented Kalman Filtering on Lie Groups developped in Chapter 4 and more generally the theory of invariant Kalman filtering, an innovative UKF is derived for the monocular visual SLAM problem. The body pose, velocity, and the 3D landmarks' positions are viewed as a single element of a (high dimensional) Lie group SE 2+p [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], which constitutes the state, and where the accelerometers' and gyrometers' biases are appended to the state and estimated as well. The efficiency of the approach is validated both on simulations and on five real datasets.

Introduction

In this chapter, we come back to the problem of SLAM, but from a more practical viewpoint than in Chapter 3. Indeed, our main goal here is to deal with vision measurements.

We consider a robot equipped with inertial sensors, namely an IMU , and a monocular vision for SLAM for Micro Aerial Vehicles (MAVs). We seek to "fuse" those measurements in order to estimate the robot's state. To this aim, we propose a novel 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 3D Invariant-EKF SLAM[END_REF][START_REF] Barrau | Aligment Method for an Inertial Unit[END_REF] and the results of Chapter 3. Secondly, the UKF on Lie Groups (UKF-LG), whose general methodology has been recently introduced in Chapter 4. The effectiveness of our algorithm is tested both on simulations and on real data [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF]. The method, an UKF-LG visual SLAM, favorably compares to state-of-the-art Kalman filter based solutions.

Note that, in the present chapter we do not specifically address Visual Inertial Odometry (VIO) which is addressed in Chapter 6 and 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 Visual-Inertial 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 (contrary to visual inertial odometry), a powerful method to drastically reduce uncertainty on the state, when applicable.

Contributions and Links with Previous Literature

Over the last decades, tremendous progresses have been achieved in visual localization frameworks, whose estimation and robustness can be improved by tightly coupling visual and inertial informations, which is the major focus of this chapter. Most approaches combine data using filtering based solutions [START_REF] Li | High-Precision, Consistent EKF-Based Visual-Inertial Odometry[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF][START_REF] Loianno | Visual Inertial Odometry for Quadrotors on SE(3)[END_REF][START_REF] Hesch | Observability-Constrained Vision-aided Inertial Navigation[END_REF][START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF][START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF][START_REF] Kelly | Visual-Inertial Simultaneous Localization, Mapping and Sensor-to-Sensor Self-Calibration[END_REF], or optimization/bundle adjustment techniques, e.g., [START_REF] Forster | On-Manifold Preintegration for Real-Time Visual-Inertial Odometry[END_REF][START_REF] Leutenegger | Keyframe-Based Visual-Inertial Odometry using Nonlinear Optimization[END_REF][START_REF] Qin | VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[END_REF]. Optimization based methods are more accurate but generally come with higher computational demands, and filtering approaches are well suited to real time applications.

Additionally, it has been shown that, by expressing the EKF estimation error directly on the Lie group and leveraging an IEKF, consistency guarantees can be obtained without ad hoc remedies both for wheel odometry SLAM [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF][START_REF] Zhang | Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM[END_REF] and VIO [START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF] filtering algorithms. Very recently [START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF] proposed a multi-state constrained Right IEKF (RIEKF) for 3D VINS. In Section 5 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 unscentedbased 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 IEKF framework where Jacobians are defined with respect to the Lie structure, see e.g., [START_REF] Barrau | Aligment Method for an Inertial Unit[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 Chapter 4.

Note that, the present chapter presents two new developments on the UKF-LG methodology, that are as follows: 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 [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF]. The main differences are threefold. First, the Lie group we use SE 2+p [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], introduced in [9,10], 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 presented in Chapter 4 generates sigma points in the Lie algebra, and then uses concentrated Gaussian distributions (as in e.g., [START_REF] Wang | Error Propagation on the Euclidean Group with Applications to Manipulator Kinematics[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.

Chapter's Organization

The chapter is organized as follows. Section 2 formulates the fusion problem. Section 2 contains mathematical preliminaries on unscented Kalman filtering on Lie groups. Section 3 describes the two proposed UKFs for monocular visual and inertial SLAM. Section 4 and 5 illustrate the performances of the proposed filters based both on Monte-Carlo simulations and on real datasets.

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.

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 p imu 2 R 3 , velocity v imu 2 R 3 , orientation R imu 2 SO(3) of the body, the IMU biases b ω 2 R 3 and b a 2 R 3 , as well as the 3D positions p 1 ,...,p p 2 R 3 of the landmarks in the global frame. The dynamics of the system read: body state dynamics

8 > > > > < > > > > : Ṙimu = R imu (ω b ω + w ω ) ⇥ vimu = R imu (a b a + w a ) + g ṗimu = v imu , IMU biases dynamics 8 > > < > > : ḃω = w b ω ḃa = w b a , landmarks dynamics n ṗi = 0,i=1,...,p , (5.1)
(5.2)

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

w = h w ωT w aT w b ω T w b a T i T , ( 5.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 (5.1) are considered as noisy and biased inputs of the system.

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 u i y v i # + n i , (5.5)
where y i is the measured normalized pixel location of the landmark in the camera frame, that is,

2 6 6 6 6 6 6 6 4
y u i y v i y i w 3 7 7 7 7 7 7 7 5
=(R C ) T ⇣ (R imu ) T (p i p imu ) p C ⌘ , ( 5.6)
where the right term corresponds to the distance from the landmark to the camera expressed in the camera frame. R C and p C are the known rotation matrix and the translation mapping from the body frame onto the camera frame. Finally, n i ⇠N(0, N) represents the pixel image noise.

Estimation/Fusion Problem

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

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.

Time Discretization

Equations (5.1) are standard navigation equations, and their discretization is well established. In this chapter, we implemented the method of pre-integration on manifolds of [START_REF] Forster | On-Manifold Preintegration for Real-Time Visual-Inertial Odometry[END_REF].

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, p, p 1 , ••• , p p )
χ n = exp (ξ) χn b n = bn + b , " ξ b# ⇠N(0, P n ) , (5.7)
dynamics n χ n , b n = f (χ n 1 , u n b n 1 , w n ) , (5.8) observations 8 > > < > > : Y n = h y T 1 •••y T p i T := Y (χ n , n n ) y i given in (5.5),i=1,...,p , (5.9
) 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 7. 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.

such that ⇣ χn , bn ⌘ 2 R 15+3p represents the mean (estimated) state at time n, P n 2 R (15+3p)⇥(
Remark 2. for the Left-UKF-LG, we define χ n = χn exp (ξ) and substitute it in (5.7). 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] Zhang | Convergence and Consistency Analysis for A 3D Invariant-EKF SLAM[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF].

Propagation Step

The propagation step is described in Algorithm 8 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] Wang | Error Propagation on the Euclidean Group with Applications to Manipulator Kinematics[END_REF] regarding propagation), line 7 deserves a few explanations. According to uncertainty model, 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 ⌘ . h ξ ⌥ j b ⌥ j n ⌥ j i = ⌥ 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 ⌘ ;
// see Appendix for definition of qr // the notation p ⌥ is used to denote the two variables +p and p Output: χ, b, S;

Algorithm 9: Update function for the Right-UKF-LG

Input: χ, b, S, Y, N; 1 Y 0 = Y ( χ, 0); // see (5.9) and (5.5)-(5.6) 2 h ξ ⌥ j b ⌥ j i = ⌥ col j (S) ,j=1,...,J 0 ; 3 χ ⌥ j = exp ⇣ ξ ⌥ j ⌘ χ,j=1,...,J 0 ; 4 Y j = Y ⇣ χ ⌥ j , w ⌥ j ⌘ ,j=1,...,J 0 ; 5 ξ, b, S qr' ⇣ Y n , Y 0 , Y ⌥ j , ξ ⌥ j ,j =1,...,J 0 , N ⌘ ; 6 χ exp ⇣ ξ⌘ χ, b b + b ;
// update mean // See Appendix for definition of qr' Output: χ, b, S;

Update Step

The update step incorporates the observation of the p landmarks at time n and is described in Algorithm 9. It operates as follow. The sigma points generated in the Lie algebra at line 2 are mapped to the group at line 3, and used to compute 2J 0 +1 measurement sigma points at line 4. The function qr 0 (•) 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 the biases, see next subsection for more details. Remark 3. following [START_REF] Holmes | An O(N) 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.

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 7. According to right uncertainty model (4.20), 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 ξ 2 R 9+3p as ξ ⇠N( ξ, P + ), i.e., ξ = ξ + ξ + with ξ + ⇠N(0, P + ). Back to the Lie group this implies χ ⇡ exp ⇣ ξ + + ξ⌘ χ, where ξ + ⇠N(0, P + ) .

(5.10)

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

χ ' exp (ξ + )

Simulation Results

Five different filters are compared on Monte-Carlo simulations:

• an UKF that considers the attitude as an element SO(3) 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 left-invariant vector fields of SE(3);

• the Right-Invariant visual EKF SLAM (RIEKF) of [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF][START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF] (where the biases are appended to the state and treated as in the conventional EKF);

• the proposed Right-UKF-LG described in Section 3;

• the proposed Left-UKF-LG, as an alternative to Right-UKF-LG based on the left uncertainty representation (4.5).

Simulation Setting

We generate a noise-free trajectory displayed on Figure 5.1. This trajectory is realistic since it is inspired by true quadrotor trajectories from [START_REF] Burri | The EuRoC Micro Aerial Vehicle 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.

Results

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

• three groups appear: the RIEKF and Right-UKF-LG achieve the best results. This confirms that the right-invariant 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 3D 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 [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], but the first one uses the EKF methodology and the second one the UKF methodology.

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 Micro Aerial Vehicle 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 Micro Aerial Vehicle Datasets[END_REF] in which landmarks can be well tracked in order to minimize the influence of the frontend image processor.

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 Micro Aerial Vehicle Datasets[END_REF]. The initial state corresponds to the ground truth.

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 5.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.

Comparison of Execution Times

We compare in this section the execution time of the filters, both for the propagation and update steps. Figure 5.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.

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] Bonnabel | Symmetries in Observer Design: Review of Some Recent Results and Applications to EKF-based SLAM[END_REF][START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[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.

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 0 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 0 = 15 + 3p and

= p J/(1 W 0 ),W 0 =1 J/3,W j = 1 W 0 2J , (5.14) 0 = q J 0 /(1 W 0 0 ),W 0 0 =1 J 0 /3,W 0 j = 1 W 0 0 2J 0 . (5.15)
The function qr(•) operates as taking the QR decomposition of (5.17)

QR p W 1 2 6 6 6 6 6 6 6 4 ξ + 1 ••• ξ + J ξ 1 ••• ξ J b + 1 ••• b + J b 1 ••• b J 0 chol (Q) 0 chol (Q)
The function qr 0 (•) operates as follow: first, compute the mean measurement and weighted deviation

Ȳ = W 0 0 Y 0 + J 0 X j=1 W 0 i ⇣ Y + j + Y j ⌘ , ( 5.18)
e 0 = q |W 0 0 | ⇣ Y 0 Ȳ⌘ , (5.19) e ⌥ j = q W 0 j ⇣ Y ⌥ j Ȳ⌘ ,j=1,...,J 0 , ( 5.20)
and compute the Cholesky factors of the measurement covariance and the cross covariance as

QR h e + 1 ••• e + J 0 e 1 ••• e J 0 R 0 i , (5.21) R = " S 0 0 # , ( 5.22)
S 0 CholUpdate (S 0 , sign (W 0 0 ) , e 0 ) , (5.23)

P 0 = J 0 X j=1 q W 0 j 0 B B B B B @ " ξ + j b + j #T e + j + " ξ j b j #T e j 1 C C C C C A , (5.24)
R 0 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 0 ⇣ S 0 T S 0 ⌘ 1 (5.25) = P 0 S 0 1 S 0 T (5.26) " ξ b# = K ⇣ Y Ȳ⌘ , ( 5.27)
S SeqCholUpdate ⇣ S, 1, KS 0 T ⌘ , ( 5.28)
where SeqCholUpdate denotes repeated Cholesky updating CholUpdate using successive columns of KS 0 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 3.6), we compute S SJ T , (

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) 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, (5.27) 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 to possibly use the more accurate approximation of [START_REF] Barfoot | State Estimation for Robotics[END_REF][START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF] exp

⇣ ξ + + ξ⌘ = exp (Jξ + ) exp ⇣ ξ⌘ + o (ξ + ) , (5.30)
where J is the left Jacobian. In this case we compute the updated parameters as χ+ = exp ⇣ ξ⌘ χ, (5.31)

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

CHAPTER 6

Unscented Kalman Filter on Lie Groups for Visual Inertial Odometry Résumé Dans ce chapitre, nous présentons un filtre innovant pour l'odométrie basée inertie et vision stéréo en s'appuyant sur: i) le filtre de Kalman à contraintes multi-états stéréo récemment introduit; ii) la théorie du filtrage invariant; et iii) le filtre de Kalman sansparfum sur les variétés parallélisables (plus précisément les groupes de Lie). Notre solution allie précision, robustesse et polyvalence de l'UKF. Nous comparons ensuite notre approche des solutions de pointe en termes de précision, de robustesse et de complexité de calcul sur le jeu de données EuRoC et ainsi que sur un jeu de donnée acquis en extérieur difficile.

Chapter abstract

In this chapter, we present an innovative filter for stereo visual inertial odometry building on: i) the recently introduced stereo multi-state constraint Kalman filter; ii) the invariant filtering theory; and iii) the UKF on parallelizable manifolds (more precisely Lie groups). Our solution combines accuracy, robustness and versatility of the UKF. We then compare our approach to state-of-art solutions in terms of accuracy, robustness and computational complexity on the EuRoC dataset and a challenging MAV outdoor dataset.

Introduction

In this chapter, we tackle the problem of fusing IMU signals with stereo vision. To this respect the problem is similar to the previous chapter. However, there are two main differences. First, we use stereo vision. Indeed, adopting a stereo configuration provides higher robustness compared to the popular monocular configuration (but it comes at the cost of an additional sensor and the need for further calibration). Then, we adopt the VIO framework, where landmarks are not kept in the state. This is desirable indeed as keeping all the landmarks in the state makes the state variable grow indefinitely and sooner or later leads to an intractable problem. Because of this, VIO allows for efficient real-time implementation over arbitrary long time periods.

More precisely, we propose a novel algorithm whose implementation mainly builds on the very recent Stereo Multi-State Constraint Kalman Filter (S-MSCKF) [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF]. In this chapter:

1. We benefit from the UKF methodology as compared to the standard EKF. Since the unscented transform spares the computation of Jacobians, the algorithm is versatile and allows fast prototyping in the presence variations in the model (e.g., the camera model).

2. We build upon the theory of the UKF on Lie Groups [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF], and leverage the Lie group structure of the SLAM problem introduced in [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF].

We demonstrate that the proposed stereo VIO filter is able to achieve similar or even higher accuracy than state-of-art solutions on two distinct datasets with high efficiency.

Contributions

Among popular visual inertial navigation solutions, the MSCKF developed by Mourikis and Roumeliotis [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF] and its state-of-the-art variants, notably [START_REF] Li | High-Precision, Consistent EKF-Based Visual-Inertial Odometry[END_REF], offer an efficient compromise between accuracy and computational complexity, which was applied e.g. to the application of spacecraft descent and landing [START_REF] Mourikis | Vision-Aided Inertial Navigation for Spacecraft Entry, Descent, and Landing[END_REF] and fast UAV autonomous flight [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF].

In this chapter, we propose an UKF-based stereo VIO solution that leverages the MSCKF methodology and the Lie group structure of the state space SE(3) 2+p . Our main contributions are:

• an embedding of the state and the uncertainties into a matrix Lie group which additionally considers the unknown IMU to camera transformation;

• the derivation of a Kalman filter that combines an IEKF propagation for computational efficiency and an UKF-LG update, in which our choice is motivated by the UKF superiority performance compared with the EKF for many non-linear problems, and its ease of implementation for the practitioner, allowing him to readily handle additional measurements (such as GPS measurements) or variations in the output model (the camera model) since the update is derivative free;

• since the computational demands of a standard UKF update is generally greater than those of the EKF, we provide: i) a computationally efficient strategy for computing our UKF-LG update in the formalism of an IEKF update, inspired by [START_REF] Lefebvre | Comment on "A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Rstimators[END_REF][START_REF] Huang | A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM[END_REF]; and ii) a closed-form expression for alternatively performing the update in a full IEKF manner;

• the publicly available C++/ROS source code used for this chapter, available at https://github.com/mbrossar/msckf_vio.git. It uses building blocks from the code of [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF].

Finally, the accuracy and computational complexity of the proposed filter is validated and compared with state-of-the-art VIO solutions on two challenging real-world MAV datasets [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF][START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF].

Chapter's Organization

Section 2 formulates the filtering problem. Section 3 contains mathematical preliminaries for unscented Kalman filtering on Lie groups. Section 4 describes the proposed filter for stereo VIO. Section 5 illustrates the performances of the proposed filter based on two publicly available datasets.

G MAV R, x IMU C L T IC T C i C R T LR g p j p j+1

Problem Modeling

We define in this section the kinematic model for flying devices equipped with an IMU where N past cloned camera poses make up the state [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF]. We then detail the stereo visual measurement model, and we finally pose the filtering problem we seek to address.

Variables of Interest and Dynamical Model

Let us consider an aerial body navigating on flat earth equipped with an IMU. The dynamics of the system read IMU-related state

8 > > > > > > > > > > < > > > > > > > > > > : Ṙimu = R imu (ω b ω + n ω ) ⇥ vimu = R imu (a b a + n a ) + g ṗimu = v imu ḃω = n b ω ḃa = n b a (6.1) camera-related state 8 > > < > > : ṪIC = 0 ṪC i = 0,i=1,...,N (6.2)
where the state we want to estimate consists of the current orientation R imu 2 SO(3) of the body frame (referred to as the IMU frame), that is, the rotation matrix that maps the IMU frame to the global frame, velocity v imu 2 R 3 , position p 2 R 3 , IMU biases b ω 2 R 3 and b a 2 R 3 , as well as the relative transformation between the IMU frame and the left camera frame T IC 2 SE(3) and an arbitrary number of N recorded left camera poses T C i 2 SE(3), i =1,...,N, see Figure 6.1. Finally, (ω) ⇥ denotes the skew symmetric matrix associated with the cross product with vector ω 2 R 3 , and the various white Gaussian noises can be stacked as

w = h w ωT w aT w b ω T w b a T i T ⇠N(0, Q) . (6.3)
These equations model the dynamics of small MAVs such as quadrotors where the IMU measurements ω and a in (6.1) are considered as noisy and biased inputs of the system.

Measurement Model

In addition to the IMU measurements used as inputs for the dynamics, the vehicle observes and tracks static landmarks in the global frame from a calibrated stereo camera. A landmark p j 2 R 3 is observed through both the left and right cameras corresponding to the recorded i-th pose as

y j i = h ⇣ T C i , p j ⌘ + n y 2 R 4 , (6.4)
where the non-linear stereo measurement model h(•) is given as [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF] h (T, p) =

" 1 z l I0 0 1 z r I # 2
in which p l = [ x l y l z l ] T and p r = [ x r y r z r ] T refer to the landmark coordinates expressed in the left and in the right camera frames (i.e., [

p T l 1 ] T = T 1 [ p imuT 1 ] T and [ p T r 1 ] T = ⇣ TT LR ⌘ 1 [ p imuT 1 ] T ).
Note that the stereo cameras have different poses at the same time instance, represented as T C i for the left camera and T C i T LR for the right camera, although the state only contains the pose of the left camera, since using the assumed known extrinsic parameters T LR 2 SE(3) leads to an expression for the pose of the right camera, see Figure 6.1.

Estimation/Fusion Problem

We would like to compute the probability distribution of the system's state (R imu , p imu , v imu , b ω , b a , T IC , T C 1 ,...,T C N ) defined through an initial Gaussian prior and the probabilistic evolution model (6.1)-(6.2), conditionally on the measurements of the form (6.4). This is the standard probabilistic formulation of the (Stereo-)MSCKF [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF].

Unscented Based Inferred Jacobian for UKF-LG Update

In [START_REF] Lefebvre | Comment on "A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Rstimators[END_REF], the authors interpret the conventional UKF as a linear regression Kalman filter and show how the propagation and update steps in UKF can be performed in a similar fashion as an EKF, which can save execution time [START_REF] Huang | A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM[END_REF]. The method is here straightforwardly adapted for the case of the Right-UKF-LG update. Within this interpretation, the filter seeks to find the optimal linear approximation to the nonlinear function

y = h ⇣ exp G (ξ) χ⌘ = g(ξ) (6.6) ' Hξ + ŷ (6.7)
given a weighted discrete representation (the so-called sigma-points) of the distribution ξ ⇠N(0, P). The objective is thus to find the regression matrix H and vector ŷ that minimize the linearization error e = y (Hξ + ŷ). The optimal linear regression matrix is given as [START_REF] Lefebvre | Comment on "A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Rstimators[END_REF] H = P yξ P 1 , (

where P yξ is the cross-correlation between y and ξ, and ŷ is the estimated mean of y, both computed from the unscented transform of the UKF-LG. The numerically inferred Jacobian H serves as a linear approximation to y ŷ = g(ξ) ŷ and can then be used for the Right-EKF-LG update.

Proposed Filters

In this section, we propose the Stereo-UKF-LG (S-UKF-LG), a VIO filtering solution which embeds the state in a specially defined and high dimensional Lie group. Our solution operates in two steps, as for any Kalman filter-based algorithm:

• a propagation step that propagates both the mean state and the error covariance, where the matrix covariance is computed with IEKF linearization [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] for computational efficiency.

• an update step that considers the visual information obtained from the feature tracking, in which we used as a basis the UKF-LG [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF]. We additionally provide Jacobian expressions to alternatively perform IEKF update.

State and Error Embedding on Lie Groups

Based on Chapter 4, we embed the state into a high dimensional Lie group, by letting χ 2 G be the matrix that represents:

• the IMU variables R imu , v imu and p imu through χ I 2 SE 2 (3), a group obtained by letting p = 0 in (2.35);

• the IMU bias

χ b = [ b ωT b aT ] 2 R 6 ;
• the IMU to left camera transformation T IC 2 SE(3);

• the N left camera poses T C i 2 SE(3),i=1,...,N. The dispersion on the state

χ I , χ b , T IC , T C 1 ,...,T C N , χ ⇠N R ( χ, ξ) , ( 6.9)
where , stands for "identifiable to" and ( •) for estimated mean value, is partitioned into

ξ = h ξ T R ξ T v ξ T p ξ T ω ξ T a ξ T IC ξ T C 1 ••• ξ T C N i T (6.10)
and encoded using the right uncertainty (4.20), i.e., the uncertainty representation is defined as

exp (ξ) χ , 8 > > > > > > > > > > < > > > > > > > > > > : exp SE 2 (3) ✓ h ξ T R ξ T v ξ T p i T ◆ χ I h ξ T ! ξ T a i T + χ b exp SE(3) (ξ IC ) T IC exp SE(3) ⇣ ξ C i ⌘ T C i ,i =1,...,N (6.11)
and we define for convenience the IMU error as

ξ I = [ ξ T R ξ T v ξ T p ξ T ω ξ T a ]
T . Any unknown feature p j , albeit not explicitly considered in the state, appear in the measurement (6.4) and consequently we have to define an error on this feature. In the following we propose to identify each ⇣ T C 1 , p j ⌘ as a element of the Lie group SE 2 (3) [START_REF] Barrau | An EKF-SLAM Algorithm with Consistency Properties[END_REF]. Note that, error ξ p j on landmark j then differs from the standard Euclidean error.

Remark 6. Using another camera pose than T C

1 does not influence the performances of the filter in our experiments.

Propagation Step

Let us now present the proposed filter's mechanics. To deal with discrete time measurement from the IMU, we essentially proceed along the lines of [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF]. We apply a 4-th order Runge-Kutta numerical integration of the model dynamic (6.1) to propagate the estimated state χ. To propagate the uncertainty of the state, let us consider the dynamic of the IMU linearized error as ξI = Fξ I + Gn, (6.12)

where

F = 2
Φ n = Φ (t n+1 ,t n ) = exp m Z t n+1 t n F(⌧)d⌧ ! (6.15)
and discrete time noise covariance matrix

Q n = Z t n+1 t n Φ (t n+1 ,⌧) GQG T Φ (t n+1 ,⌧) T d⌧. ( 6.16)
The covariance matrix from t n to t n+1 is propagated as the combination of partitioned covariance matrix as follows. The propagated covariance of the IMU state becomes

P II n+1 = Φ n P II n Φ n + Q n , ( 6.17)
and the full uncertainty propagation can be computed as

P n+1 = " P II n+1 Φ n P IC n P CI n Φ T n P CC n # . (6.18)
When new images are received, the state should be augmented with the new camera state. The new augmented covariance matrix becomes

P n+1 = " I J # P n+1 " I J # T . (6.19)
To obtain the expression of J in (6. [START_REF] Bohn | Unscented State Estimation for Rigid Body Motion on SE(3)[END_REF]), let us denote T 2 SE(3) as the sub-matrix of χ I that contain only the IMU pose (R, p) with ξ T = [ ξ T R ξ T p ] T its corresponding uncertainties and thus write the current camera pose as

T C = TT IC = exp SE(3) (ξ T ) T exp SE(3) (ξ IC ) TIC = exp SE(3) (ξ T ) exp SE(3) (Ad T ξ IC ) T TIC ' exp SE(3) (ξ C ) TC , ( 6.20)
in which TC = T TIC and ξ C ' ξ T +AdT ξ IC after using a first order BCH approximation, and where Ad T is the adjoint notation of SE [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], which finally leads to

J = " I000 3⇥6 R imu 000 3⇥6N 00I0 3⇥6 p ⇥ R imu 0R imu 0 3⇥6N # . (6.21)

Update Step in the MSCKF Methodology

Let us first consider the observation of a single feature p j , in which the estimated unbiased feature position LS p j is computed using least squares estimate based on the current estimated camera poses [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF]. Linearizing the measurement model at the obtained estimates χ, LS p j , the residual of the measurement is approximated as

r j i = y j i UKF y j i = UKF H j i ξ + H i p j ξ p j + n j i , (6.22)
where UKF H j i , H i p j and UKF y j i are computed in Section 4.4 and are not the usual Jacobians appearing in [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF] (beyond the fact they are computed using the unscented transform) since we use here alternative state errors ξ, ξ p j related to the Lie group structure we have endowed the state space with. By stacking multiple observations of the same feature p j , we then dispose of r j = y j UKF y j = UKF H j ξ + H p j ξ p j + n j .

(6.23)

Then, along the lines of [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF], to eliminate the landmark errors from the residual, measurements are projected onto the null space V of H i p j , i.e.,

r j o = V T r j = V T UKF H j ξ + V T n j = H j o ξ + n j o . ( 6
K = P n H T o /S.
We then compute the innovation to update the mean state as [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] χ+ = exp (Kr o ) χ, (6.25) which is an update that is consistent with our uncertainties (6.11) defined using the right multiplication based representation (4.20). The associated covariance matrix writes

P + n = P n (I KH o ) . (6.26)
The filter concludes the update step with a marginalization of the last camera pose.

Proposed Lie Group Based Update

In this section, we describe the computation of UKF H j i , H i p j and UKF y j i in (6.22) following Section 3.

Computation of H i

p j : since the covariance of ξ j p is unknown, we can not apply UKF-LG update and consequently we compute H i p j in closed-form, as H 1 p j = 0, and for i>1, as is the intermediate Jacobian after applying the chain rule in (6.4). We stress that even if (6.27) is the same expression as the Jacobians appearing in [START_REF] Mourikis | A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[END_REF], they are associated to the alternative state errors ξ p j .

H i p j = J j i 2 6
Computation of UKF H j i and UKF y j i : we apply the UKF-LG [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] to numerically infer the Jacobian UKF H j i and estimated measurement UKF y j i (see Section 3). This computation allows us to then project the residual in (6.24) and can be computed efficiently as follow. First, the filter stacks multiple observations w.r.t. the same camera pose in a vector y i that can be written in the form

y i = g i ⇣ ξ C 1 , ξ C i ⌘
, when noise and landmark errors are marginalized, as follow. Since y i is the concatenation of (6.4) for multiple landmarks, it is sufficient to write y j i as a function of ξ C 1 and ξ C i only. Let us first write our alternative state error as

exp SO(3) (ξ R C i )=R C i RT C i , (6.29
)

ξ p C i = R C i RT C i p C i xC i , (6.30
)

ξ p j = R C 1 RT C 1 p j LS p j , ( 6.31)
and consider the measurement function of the stereo camera such that

y j i = l ⇣ R T C i ⇣ p j p C i ⌘⌘ , ( 6.32)
where l(•) is the projection function. After inserting the uncertainties (6.29)-(6.31) in (6.32), we obtain

y j i = l( RT C i (exp SO(3) (ξ R C i ) exp SO(3) ( ξ R C 1 ) LS p j RT C i (p C i + ξ p C i )), (6.33) which depends on ξ R C 1 and ξ C i = [ ξ R C i ξ p C i ]
only, such that we can write y i j and by extension y i as function of ξ R C 1 and ξ C i only. Following then [START_REF] Huang | A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM[END_REF], we sample only sigma-points from variables the observations depend on, i.e., from the rotational part of ξ C 1 and ξ C i , such that the complexity remains dominated by (6.26) and comparable to the S-MSCKF, which is illustrated in Section 5.

Alternative IEKF Update

Albeit the UKF-LG update is computed efficiently, an IEKF update remains slightly more computationally efficient and can be adopted for computationally restricted platforms. Thus, as an alternative, we provide the closed-form expression for an IEKF update, yielding a stereo extension of [START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF], and where moreover the IMU to camera transformation is also estimated. The mean

y j i = ⇣ TC i ,
H j i = J j i h 0H j C 1 0H j C i 0 i , ( 6.34)
where J j i is computed in (6.28), for i>1,

H j C 1 = 2
⇣ RL i ⌘ T LS p j ⇥ ⇣ RL i ⌘ T ⇣ RR i ⌘ T LS p j ⇥ ⇣ RR i ⌘ T 3 7 7 7 7 7 7 5
, ( 6.36)
and, for i = 1,

H j C 1 = H j C i = 2 6 6 6 6 6 6 4 0 ⇣ RL 1 ⌘ T 0 ⇣ RR 1 ⌘ T 3 7 7 7 7 7 7 5
. (6.37)

The rest of the update follows Section 4.3.

Filter Update Mechanism and Image Processing Frontend

Our implementation builds upon [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF], and preserves its original methodology both for the filter update mechanism, marginalization of camera poses, outlier removal and the image processing frontend.

Experimental Results

In this section, we compare the performances of the proposed S-UKF-LG and its full IEKF variant, that we call S-IEKF, with state-of-the-art VIO algorithms including S-MSCKF [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF], OKVIS (stereo-optimization) [START_REF] Leutenegger | Keyframe-Based Visual-Inertial Odometry using Nonlinear Optimization[END_REF] and VINS-MONO (monocular-optimization) [112], i.e., with different combinations of monocular, stereo, filter-based and optimizationbased solutions. We first evaluate the algorithms on the EuRoC dataset [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF], and then on a runway environment [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF] with high speed flights. In both of the experiments, VINS-MONO considers only the images from the left camera and has its loop closure functionality disabled. The three filters (S-MSCKF, S-UKF-LG and S-IEKF) use the same frontend and the same parameters provided from the S-MSCKF github repository, with N = 20 camera poses. Finally, we provide to each algorithms the off-line estimating extrinsic parameters between the IMU and camera frames. Summary of the results can be found in Figure 6.2 and Figure 6.4. They reveal good performances of our proposed S-UKF-LG, which favorably compares to its conventional Stereo-MSCKF counterpart in terms of RMSE both on position and orientation. Note that, optimization based OKVIS achieves best estimation accuracy, but at the cost of extended CPU load.

EuRoC Dataset

The EuRoC [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF] dataset includes synchronized 20 Hz stereo images and 200 Hz IMU messages collected on a MAV. The dataset contains sequences of flights with different level of flight dynamics. Figure 6.2 shows the Root Mean Square Error (RMSE) and Figure 6.3 the average CPU load of the different algorithms. As in [START_REF] Sun | Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[END_REF], the filter-based methods do not work properly on V2_03_difficult due to their same KLT optical flow algorithm. In terms of accuracy, the filters compete with the stereo-optimization approach OKVIS, whereas the results of VINS-MONO are affected by its monocular camera frontend. The S-UKF-LG and S-IEKF compare favorably to the recent S-MSCKF. Since filters use the same frontend, differences come from the filters' backends. In terms of computational complexity, filter-based solutions reclaims clearly less computational resources than optimization-based methods, in which 80 % of the computation is caused by the frontend including feature detection, tracking and matching, on Precision Tower 7910 with CPU E5-2630 v4 2.20 Hz. The filters themselves take about 10 % of one core when the camera rate is set at 20 Hz. directions after making corrections on both time and yaw offsets. In this experiment, OKVIS obtains the best results in terms of accuracy, while VINS-MONO generally obtains an accurate estimation with higher variance than than the other methods which increase its RMSE. The S-UKF-LG appears to be slightly more robust than S-MSCKF and S-IEKF. From both experiments, it can be observed that the three filters achieve the lowest CPU usage while maintaining comparable accuracy regarding optimization solutions. However, compared to the experiments with the EuRoC dataset, the image processing frontend spends more computational effort, since the image frequency and resolution are higher, and the fast flight requires more detections of new features.

Fast Flight Dataset

Conclusion

In this chapter, we introduced a novel filter-based stereo visual inertial state estimation algorithm that is a stereo multi-state constraint Kalman filter variant. It has the merit of using the UKF approach while achieving execution times that are akin to the standard EKF-based solution, namely S-MSCKF. The UKF approach is generally more robust to non-linearities, and allows fast prototyping since Jacobian explicit computation is not required (and thus readily adapts to model modifications, estimation of additional parameters, and fusion with other sensors). We exploited an efficient inference of the Jacobian that leads to similar computational complexity between our S-UKF-LG solution and the S-MSCKF. We also provided the closed-forms for the measurement Jacobian that lead to an alternative S-IEKF, that can be considered as a stereo version of [START_REF] Wu | An Invariant-EKF VINS Algorithm for Improving Consistency[END_REF] extending it also with an on-line estimation of the extrinsic parameters, resulting in a consistent filter with high level of accuracy and robustness. Accuracy, efficiency, and robustness of our filters are demonstrated using two challenging datasets.

Part II

Measurement Noise Estimation for Kalman Filter Tuning CHAPTER 7

Introduction to Part II

The Kalman filter and more generally virtually all state estimation algorithms (particle filter, smoothers) must be tuned: given the state transition function f (•) that encodes the dynamical model at play, and observation function h(•) (see Chapter 2), the extent of uncertainty that corrupts both the dynamical model and the observations must be assessed. In the Kalman filter-like approach, uncertainties are modelled as Gaussian (centered) noises with covariance matrices Q and N that encode the magnitude of the noises (and their inner correlations). Those matrices appear as tuning parameters of the filter: their value is based on the knowledge one has of the statistics of the noise when available. Wether noise statistics be available or not, their fine tuning often requires a deal of manual "tweaking" from the engineers. The noise terms encoded in matrices Q and N capture what the deterministic model fails to [START_REF] Abbeel | Discriminative Training of Kalman Filters[END_REF], i.e. unmodeled perturbations on the system, which is usually the result of various effects:

• mis-modeled dynamics, e.g. an unfixed level arm between IMU and camera;

• the existence of unmodelled hidden state in the environment, e.g. temperature and its effect on inertial sensors;

• the integration scheme, as time discretization, which introduces additional error;

• and the algorithmic approximations itself, such as the Taylor approximation commonly used for linearization in EKF.

All these effects are commonly characterized as noise. Furthermore, the noise is assumed to be Gaussian and independent over time whereas the phenomena described above cause highly correlated noises. The magnitude of the noise in an EKF is therefore extremely difficult to estimate. This difficulty was yet early noticed in the seminal paper [START_REF] Kalman | A New Approach to Linear Filtering and Prediction Problems[END_REF], where Kalman himself commented on the difficulty of identifying such parameters: "In real life, however, the situation is usually reversed. One is given the covariance matrix and the problem is to get the statistical properties [of the perturbation]. This is a subtle and presently largely unsolved problem in experimentation and data reduction."

As a result, many have proposed methods for tuning the parameters in Kalman filters. One of the first methods proposed was to jointly learn the parameters and state/output sequence using expectation-maximization [START_REF] Shumway | An Approach To Time Series Smoothing And Forecasting Using The Em Algorithm[END_REF]. More recent approaches employ different optimization approaches, including the simplex algorithm [START_REF] Powell | Automated Tuning of an Extended Kalman Filter Using the Downhill Simplex Algorithm[END_REF], coordinate descent [START_REF] Abbeel | Discriminative Training of Kalman Filters[END_REF], genetic algorithms [START_REF] Oshman | Optimal tuning of a Kalman filter using genetic algorithms[END_REF], Bayesian optimization [START_REF] Chen | Weak in the NEES?: Auto-Tuning Kalman Filters with Bayesian Optimization[END_REF], and least squares [START_REF] Barratt | Fitting a Kalman Smoother to Data[END_REF].

Illustrative Example

To illustrate the importance and difficulties of optimizing the Kalman filter parameters (either by machine learning or manual tuning), consider the practical problem of estimating the variance parameter for a GNSS unit that is being used to estimate the position x n of a robot or more generally a vehicle. A standard Kalman filter approach models the GPS readings measured as the true position plus noise as

y n = x n + n n (7.1)
where n n is a zero mean noise term with variance σ 2 I. The manufacturer specifications of the sensor generally give σ 2 for the unit; otherwise, one can also straightforwardly estimate σ 2 by placing the vehicle at a fixed position and measuring the variability of the GNSS readings. However, in practice either of these choices work very poorly if it is the parameter used in the Kalman filter. This is because GNSS errors are often correlated over time, whereas the straightforward implementation of the Kalman filter assumes that the errors are independent. Thus, if the vehicle is stationary and we average k GNSS readings, the filter assumes that the variance of the resulting estimate is σ 2 /kI (obviously this estimate may drop below the actual limit on the sensor's precision). However, if the errors are correlated over time, then the true variance of the resulting position state estimate can be significantly larger. The extreme of this case would be full correlation: if the errors were perfectly correlated so that all k readings are identical, then the variance of the average would be σ 2 I instead of σ 2 /kI. Additionally, if σ 2 I is the parameter used in the filter, it tends to make the filter inconsistent. Consequently, in practice, significant human time was expended to try to "tweak" the variance parameter to what they guessed [START_REF] Castella | An Adaptive Two-Dimensional Kalman Tracking Filter[END_REF].

Content of Part II

In this part we focus on the problem of assessing noise parameters of the state estimator in the context of navigation. This route is explored in two quite different directions.

In Chapter 9, we focus on a very specific problem. Some recent sensors may generate scans of the environment in the form of clouds of points, and by matching scans one may find the rigid body transformation (translation+rotation) that aligns them best. This may be used to evaluate relative displacements of any vehicle equipped with such sensors between different instants (as long as there is sufficient overlap between the scans). In turn, this information may be used to navigate, either as a means for pure odometry, or to fuse it with other sensors such as inertial sensors. The scan matching algorithm that is used the most is called Iterative Closest Point (ICP). Even if one knows well the precision of the scanner, that is, the uncertainty associated with the depth of each point in the cloud, it is not obvious to deduce the associated uncertainty on the relative displacement. But the knowledge of this extent of uncertainty the ICP estimate bears is paramount to use ICP/scan matching for navigation. Although there has been a number of papers devoted to the subject, we believe our approach clarifies a number of important points, and yields a novel efficient algorithm for assessment of the covariance of the ICP estimate.

In chapter 8, we pursue a different approach to a different problem. Indeed, we consider a wheeled vehicle that navigates with an IMU. Wheeled vehicles have a specific way of moving as they are linked to a surface, and their velocity is approximately aligned with the body frame. This side information about the motion may be used by the Inertial Navigation System (INS) to improve the accuracy of localization. To do so, one may use the standard IMU equations as a dynamical model (exactly as what would be done in an aerial or underwater vehicle), and incorporate the side information in the form of pseudo-measurements, that is, "tell" the Kalman filter it measures a null lateral velocity for instance. However, the lateral velocity is not null, as the vehicle slips. It is thus important to "let the filter know" this information is approximate. The measurement noise matrix N encodes the magnitude of the uncertainty, that is, essentially the "extent of slip" in the present case. Thus it should be larger when maneuvers are performed, and smaller when the vehicle is moving at constant speed in straight line. The interesting point is that the IMU does measure information related to the vehicle's motion, and notably the gyroscopes detect turns. Our idea is thus to use the IMU measurements to assess in real time an appropriate value for N. However, the precise relation between the most appropriate N and the inertial sensors, is far from obvious, either it be theoretically or experimentally. Hence we propose to rely on machine (deep) learning to automatically identify a function that relates both in a relevant way.

CHAPTER 8 AI-IMU Dead-Reckoning

The present chapter has been published in IEEE Transactions on Intelligent Vehicles.

Résumé

Chapter abstract

In this chapter we propose a novel accurate method for dead-reckoning of wheeled vehicles based only on an IMU. In the context of intelligent vehicles, robust and accurate dead-reckoning based on the IMU may prove useful to correlate feeds from imaging sensors, to safely navigate through obstructions, or for safe emergency stops in the extreme case of exteroceptive sensors failure. The key components of the method are the Kalman filter and the use of deep neural networks to dynamically adapt the noise parameters of the filter. The method is tested on the KITTI odometry dataset, and our dead-reckoning inertial method based only on the IMU accurately estimates 3D position, velocity, orientation of the vehicle and self-calibrates the IMU biases. We achieve on average a 1.10% translational error and the algorithm competes with top-ranked methods which, by contrast, use LiDAR or stereo vision. We make our implementation open-source at: https://github.com/mbrossar/ai-imu-dr

Introduction

Intelligent vehicles need to know where they are located in the environment, and how they are moving through it. An accurate estimate of vehicle dynamics allows validating information from imaging sensors such as lasers, ultrasonic systems and video cameras, correlating the feeds, and also ensuring safe motion throughout whatever may be seen along the road [START_REF] Bresson | Simultaneous Localization and Mapping: A Survey of Current Trends in Autonomous Driving[END_REF]. Moreover, in the extreme case where an emergency stop must be performed owing to severe occlusions, lack of texture, or more generally imaging system failure, the vehicle must be able to assess accurately its dynamical motion. For all those reasons, the IMU appears as a key component of intelligent vehicles [START_REF] Oxts | Why it is Necessary to Integrate an Inertial Measurement Unit with Imaging Systems on an Autonomous Vehicle[END_REF]. Note that Global Navigation Satellite System (GNSS) allows for global position estimation but it suffers from phase tracking loss in densely built-up areas or through tunnels, is sensitive to jamming, and may not be used to provide continuous accurate and robust localization information, as exemplified by a GPS outage in the well known KITTI dataset [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF], see Figure 8.1.

Kalman filters are routinely used to integrate the outputs of IMUs. When the IMU is mounted on a car, it is common practice to make the Kalman filter incorporate side information about the specificity of wheeled vehicle dynamics, such as approximately null lateral and upward velocity assumption in the form of pseudomeasurements [START_REF] Zheng | Odometry Vision Based Ground Vehicle Motion Estimation With SE(2)-Constrained SE(3) Poses[END_REF][START_REF] Zheng | SE(2)-Constrained Visual Inertial Fusion for Ground Vehicles[END_REF][START_REF] Buczko | Self-Validation for Automotive Visual Odometry[END_REF][START_REF] Dissanayake | The Aiding of a Low-cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for Land Vehicle Applications[END_REF][START_REF] Simon | Kalman Filtering with State Constraints: A Survey of Linear and Nonlinear Algorithms[END_REF][START_REF] Kilic | Improved Planetary Rover Inertial Navigation and Wheel Odometry Performance through Periodic Use of Zero-Type Constraints[END_REF]. However, the degree of confidence the filter should have in this side information is encoded in a covariance noise parameter which is difficult to set manually, and moreover which should dynamically adapt to the motion, e.g., lateral slip is larger in bends than in straight lines. Using the recent tools from the field of Artificial Intelligence (AI), namely deep neural networks, we propose a method to automatically learn those parameters and their dynamic adaptation for IMU only deadreckoning. Our contributions, and the chapter's organization, are as follows:

• we introduce a state-space model for wheeled vehicles as well as simple assumptions about the motion of the car;

• we implement a state-of-the-art Kalman filter [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Barrau | Invariant Kalman Filtering[END_REF] that exploits the kinematic assumptions and combines them with the IMU outputs in a statistical way in Section 4.3. It yields accurate estimates of position, orientation and velocity of the car, as well as IMU biases, along with associated uncertainty (covariance matrices);

• we exploit deep learning for dynamic adaptation of covariance noise parameters of the Kalman filter in Section 5.1. This module greatly improves filter's robustness and accuracy, see Section 6.4;

• we demonstrate the performances of the approach on the KITTI dataset [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF] in Section 6. Our approach solely based on the IMU produces accurate estimates and competes with top-ranked LiDAR and stereo camera methods [START_REF] Deschaud | IMLS-SLAM: Scan-to-Model Matching Based on 3D Data[END_REF][START_REF] Mur-Artal | ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras[END_REF]; and we do not know of IMU based dead-reckoning methods capable to compete with such results;

• the approach is not restricted to inertial only dead-reckoning of wheeled vehicles. Thanks to the versatility of the Kalman filter, it can easily be coupled with GNSS which is the backbone for IMU self-calibration, applied for railway vehicles [START_REF] Tschopp | Experimental Comparison of Visual-Aided Odometry Methods for Rail Vehicles[END_REF], or for using IMU as a speedometer in path-reconstruction and map-matching methods [START_REF] Mousa | Inertial Measurement Units-Based Probe Vehicles: Automatic Calibration, Trajectory Estimation, and Context Detection[END_REF][START_REF] Wahlstrom | Map-Aided Dead-Reckoning Using Only Measurements of Speed[END_REF][START_REF] Mahmoud | Integrated Positioning for Connected Vehicles[END_REF][START_REF] Karlsson | The Future of Automotive Localization Algorithms: Available, Reliable, and Scalable Localization: Anywhere and Anytime[END_REF].

Relation to Previous Literature

Autonomous vehicle must robustly self-localize with their embarked sensor suite which generally consists of odometers, IMUs, radars or LiDARs, and cameras [START_REF] Bresson | Simultaneous Localization and Mapping: A Survey of Current Trends in Autonomous Driving[END_REF][START_REF] Oxts | Why it is Necessary to Integrate an Inertial Measurement Unit with Imaging Systems on an Autonomous Vehicle[END_REF][START_REF] Karlsson | The Future of Automotive Localization Algorithms: Available, Reliable, and Scalable Localization: Anywhere and Anytime[END_REF]. SLAM based on inertial sensors, cameras, and/or LiDARs have enabled robust realtime localization systems, see e.g., [START_REF] Deschaud | IMLS-SLAM: Scan-to-Model Matching Based on 3D Data[END_REF][START_REF] Mur-Artal | ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras[END_REF]. Although these highly accurate solutions based on those sensors have recently emerged, they may drift when the imaging system encounters troubles.

As concerns wheeled vehicles, taking into account vehicle constraints and odometer measurements are known to increase the robustness of localization systems [START_REF] Zheng | SE(2)-Constrained Visual Inertial Fusion for Ground Vehicles[END_REF][START_REF] Buczko | Self-Validation for Automotive Visual Odometry[END_REF][START_REF] Wu | VINS on Wheels[END_REF][START_REF] Brunker | Odometry 2.0: A Slip-Adaptive EIF-Based Four-Wheel-Odometry Model for Parking[END_REF]. Although quite successful, such systems continuously process a large amount of data which is computationally demanding and energy consuming. Moreover, an autonomous vehicle should run in parallel its own robust IMU-based localization algorithm to perform maneuvers such as emergency stops in case of other sensors failures, or as an aid for correlation and interpretation of image feeds [START_REF] Oxts | Why it is Necessary to Integrate an Inertial Measurement Unit with Imaging Systems on an Autonomous Vehicle[END_REF].

High precision aerial or military inertial navigation systems achieve very small localization errors but are too costly for consumer vehicles. By contrast, low and medium-cost IMUs suffer from errors such as scale factor, axis misalignment and random walk noise, resulting in rapid localization drift [START_REF] Kok | Using Inertial Sensors for Position and Orientation Estimation[END_REF]. This makes the IMU-based positioning unsuitable, even during short periods.

Inertial navigation systems have long leveraged virtual and pseudo-measurements from IMU signals, e.g. the widespread Zero velocity UPdaTe (ZUPT) [START_REF] Dissanayake | The Aiding of a Low-cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for Land Vehicle Applications[END_REF][START_REF] Ramanandan | Inertial Navigation Aiding by Stationary Updates[END_REF][START_REF] Brossard | RINS-W: Robust Inertial Navigation System on Wheels[END_REF], and covariance adaptation [START_REF] Aghili | Robust Relative Navigation by Integration of ICP and Adaptive Kalman Filter Using Laser Scanner and IMU[END_REF]. In parallel, deep learning (more generally machine learning) are gaining much interest for inertial navigation [START_REF] Yan | RIDI: Robust IMU Double Integration[END_REF][START_REF] Chen | IONet: Learning to Cure the Curse of Drift in Inertial Odometry[END_REF]. In [START_REF] Yan | RIDI: Robust IMU Double Integration[END_REF] velocity is estimated using support vector regression whereas [START_REF] Chen | IONet: Learning to Cure the Curse of Drift in Inertial Odometry[END_REF] use recurrent neural networks for end-to-end inertial navigation (this means the entire traditional pipeline is replaced with a single learning algorithm that learns the whole input to output process from examples). This is promising but restricted to pedestrian dead-reckoning since they generally assume slow horizontal planar motion, and must infer velocity directly from a small sequence of IMU measurements, whereas we can afford using larger sequences.

General "deep" Kalman filter theories [START_REF] Krishnan | Deep Kalman Filter[END_REF][START_REF] Krishnan | Structured Inference Networks for Nonlinear State Space Models[END_REF][START_REF] Chen | DynaNet: Neural Kalman Dynamical Model for Motion Estimation and Prediction[END_REF][START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF] consist in learning a full statespace model from inputs, measurements, and ground-truth: learning the state space structure, learning the propagation function, and learning the observation function, from scratch. Our method is quite different as it builds upon well established dynamical and measurements equations, and achieves better performances. In particular, the end-to-end learning approach [START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF], albeit promising, obtain large translational error > 30% in their stereo odometry experiment. [START_REF] Hosseinyalamdary | Deep Kalman Filter: Simultaneous Multi-Sensor Integration and Modelling; A GNSS/IMU Case Study[END_REF] combines IMU and GNSS for learning a specific propagation model of IMU errors. In contrast, our method, once parameters are optimized (learned), does not require any GNSS signal. Finally, [START_REF] Liu | Deep Inference for Covariance Estimation: Learning Gaussian Noise Models for State Estimation[END_REF] uses deep learning for estimating covariance of a local odometry algorithm that is fed into a global optimization procedure, and in [START_REF] Brossard | Learning Wheel Odometry and IMU Errors for Localization[END_REF] we used Gaussian processes to learn a wheel encoders error.

Dynamic adaptation of noise parameters in the Kalman filter is standard in the tracking literature [START_REF] Castella | An Adaptive Two-Dimensional Kalman Tracking Filter[END_REF] and generally based on Auto covariance Least Squares (ALS) [START_REF] Odelson | A New Autocovariance Least-Squares Method for Estimating Noise Covariances[END_REF] and adaptive Kalman filter methods [START_REF] Aghili | Robust Relative Navigation by Integration of ICP and Adaptive Kalman Filter Using Laser Scanner and IMU[END_REF][START_REF] Mohamed | Adaptive Kalman Filtering for INS/GPS[END_REF][START_REF] Susi | Tuning a Kalman Filter Carrier Tracking Algorithm in the Presence of Ionospheric Scintillation[END_REF], where adaptation rules are application dependent and are generally the result of manual "tweaking" by engineers. Our approach is wholly different. In the above covariance adaptation is based on statistical tests about measurement error residuals (simply put, a large discrepancy between estimates and actual measurements indicates the parameters are not optimal and prompts the filter to adapt the covariance), whereas in our framework it is solely based on raw IMU data: it is independent of the current state estimates and the observed measurement errors (see Figure 8.8).

Finally, in [START_REF] Abbeel | Discriminative Training of Kalman Filters[END_REF] the authors propose to use classical machine learning techniques to learn static noise parameters (without adaptation) of the Kalman filter, and apply it to the problem of IMU-GNSS fusion.

IMU and Problem Modelling

An inertial navigation system uses accelerometers and gyrometers provided by the IMU to track the orientation R imu n , velocity v imu n 2 R 3 and position p imu n 2 R 3 of a moving platform relative to a starting configuration (R imu 0 , v imu 0 , p imu 0 ). The orientation is encoded in a rotation matrix R imu n 2 SO(3) whose columns are the axes of a frame attached to the vehicle.

IMU Modelling

The IMU provides noisy and biased measurements of the instantaneous angular velocity vector ω n and specific forces a n as follows [START_REF] Kok | Using Inertial Sensors for Position and Orientation Estimation[END_REF]

ω imu n = ω n + b ω n + w ω n , (8.1) a imu n = a n + b a n + w a n , (8.2
R imu n+1 = R imu n exp (ω n dt) , (8.5) v imu n+1 = v imu n + (R imu n a n + g) dt, (8.6) p imu n+1 = p imu n + v imu n dt, ( 8.7)
between two discrete time instants sampling at dt, where we let the IMU velocity be v imu n 2 R 3 and its position p imu 3) is the 3 ⇥ 3 rotation matrix that represents the IMU orientation, i.e. that maps the IMU frame to the world frame, see Figure 8.2. Finally exp(•) denotes the SO(3) exponential map. The true angular velocity ω n 2 R 3 and the true specific acceleration a n 2 R 3 are the inputs of the system (8.5)-(8.7). In our application scenarios, the effects of earth rotation and Coriolis acceleration are ignored, Earth is considered flat, and the gravity vector g 2 R 3 is a known constant.

n 2 R 3 in the world frame. R imu n 2 SO(
All sources of error displayed in (8.1) and (8.2) are harmful since a simple implementation of (8.5)-(8.7) leads to a triple integration of raw data, which is much more harmful than the unique integration of differential wheel speeds [START_REF] Karlsson | The Future of Automotive Localization Algorithms: Available, Reliable, and Scalable Localization: Anywhere and Anytime[END_REF]. Indeed, a bias of order ✏ on the accelerometer measurements has an impact of order ✏t 2 /2 on the position after t seconds, potentially leading to a huge drift.

Problem Modelling

We distinguish between three different frames, see Figure 8.2: i) the static world frame, w; ii) the IMU frame, i, where (8.1)-(8.2) are measured; and iii) the car frame, c. The car frame is an ideal frame attached to the car, that will be estimated online and plays a key role in our approach. Its orientation w.r.t. i is denoted R c n 2 SO(3) and its origin denoted p c n 2 R 3 is the car to IMU level arm. In the rest of the chapter, we tackle the following problem:

IMU Dead-Reckoning Problem. Given an initial known configuration ⇣ R imu 0 , v imu 0 , p imu 0 ⌘ , perform in real-time IMU dead-reckoning, i.e.

estimate the IMU and car variables

χ n := (R imu n , v imu n , p imu n , b ω n , b a n , R c n , p c n ) (8.8)
using only the inertial measurements ω imu n and a imu n .

Kalman Filtering with Pseudo-Measurements

The EKF presented in Chapter 3 starts from a dynamical discrete-time non-linear law of the form

χ n+1 = f (χ n , u n , w n ) (8.9)
where χ n denotes the state to be estimated, u n is a general input, and w n is the process noise which is assumed Gaussian with zero mean and covariance matrix Q n . Assume side information is in the form of loose equality constraints h(χ n ) ⇡ 0 is available. It is then customary to generate a fictitious observation from the constraint function:

y n = h(χ n )+n n , ( 8.10)
and to feed the filter with the information that y n = 0 (pseudo-measurement) as first advocated by [START_REF] Tahk | Target Tracking Problems Subject to Kinematic Constraints[END_REF], see also [START_REF] Simon | Kalman Filtering with State Constraints: A Survey of Linear and Nonlinear Algorithms[END_REF][START_REF] Wu | VINS on Wheels[END_REF] for application to visual inertial localization and general considerations. The noise is assumed to be a centered Gaussian n n ⇠N(0, N n ) where the covariance matrix N n is set by the user and reflects the degree of validity of the information: the larger N n the less confidence is put in the information.

Starting from an initial Gaussian belief about the state, χ 0 ⇠N( χ0 , P 0 ) where χ0 represents the initial estimate and the covariance matrix P 0 the uncertainty associated to it, the EKF alternates between two steps. At the propagation step, the estimate χn is propagated through model (8.9) with noise turned off, w n = 0, and the covariance matrix is updated through

P n+1 = F n P n F T n + G n Q n G T n , (8.11)
where F n , G n are Jacobian matrices of f (•) at current estimate w.r.t. χ n and u n . At the update step, pseudo-measurement is taken into account, and Kalman equations allow updating the estimate χn+1 and its covariance matrix P n+1 accordingly.

To implement an EKF, the engineer needs to determine the functions f (•) and h(•), and the associated noise matrices Q n and N n . In this chapter, noise parameters Q n and N n will be wholly learned by a neural network. The main point of the present work is that the validity of the null lateral and vertical velocity assumptions widely vary depending on what maneuver is being performed: for instance, v lat n is much larger in turns than in straight lines. The role of the noise parameter adapter of Section 5.1, based on AI techniques, will be to dynamically assess the parameter N n that reflects confidence in the assumptions, as a function of past and present IMU measurements.

The Invariant Extended Kalman Filter

For inertial navigation, we advocate the use of the IEKF, see [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Barrau | Invariant Kalman Filtering[END_REF], that has recently given raise to a commercial aeronautics product [START_REF] Barrau | Invariant Kalman Filtering[END_REF][START_REF] Barrau | Aligment Method for an Inertial Unit[END_REF] and to various successes in the field of visual inertial odometry [START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF][START_REF] Hartley | Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation[END_REF][START_REF] Brossard | Unscented Kalman Filter on Lie Groups for Visual Inertial Odometry[END_REF].

We opt for an IEKF to perform the fusion between the IMU measurements (8.1)-(8.2) and (8.15) treated as pseudo-measurements we advocate IEKF for inertial navigation, see Chapter 2. Its architecture, which is identical to the conventional EKF's, is recapped in Figure 8.3. The interested reader is referred to the Appendix where the exact equations of the filter are provided.

Proposed AI-IMU Dead-Reckoning

This section describes our system for recovering all the variables of interest from IMU signals only. Figure 8.4 illustrates the approach which consists of two main blocks summarized as follows:

• the filter integrates the inertial measurements ( Note that the adapter computes covariances meant to improve localization accuracy, and thus the computed values may broadly differ from the actual statistical covariance of y n (8.15), see Section 6.4 for more details. In this respect, our approach is related to [START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF] but the considered problem is more challenging: our state-space is of dimension 21 whereas [START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF] has a state-space of dimension only 3. Moreover, we compare our results in the sequel to state-of-the-art methods based on stereo cameras and LiDARs, and we show we may achieve similar results based on the moderately precise IMU only.

AI-based Measurement Noise Parameter Adapter

The measurement noise parameter adapter computes at each instant n the covariance N n+1 used in the filter update, see Figure 8.3. The base core of the adapter is a Convolutional Neural Network (CNN) [START_REF] Goodfellow | Deep Learning[END_REF]. The networks takes as input a window of N inertial measurements and computes

N n+1 = CNN ⇣n ω imu i , a imu i o n i=n N ⌘ . (8.16)
Our motivations for the above simple CNN-like architecture are threefold:

i) avoiding over-fitting by using a relatively small number of parameters in the network and also by making its outputs independent of state estimates;

ii) obtaining an interpretable adapter from which one can infer general and safe rules using reverse engineering, e.g. to which extent must one inflate the covariance during turns, for e.g., generalization to all sorts of wheeled and commercial vehicles, see Section 6.4;

iii) letting the network be trainable. Indeed, as reported in [START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF], training is quite difficult and slow. Setting the adapter with a recurrent architecture [START_REF] Goodfellow | Deep Learning[END_REF] would make the training even much harder.

The complete architecture of the adapter consists of CNN layers (we use 2 layers, see Section 5.2) followed by a full layer outputting a vector

z n = h z lat n ,z up n i T 2 R 2 .
Based on the latter, the covariance N n+1 2 R 2⇥2 is then computed as

N n+1 = diag ✓ 2 lat 10 tanh(z lat n ) , 2 up 10 tanh(z up n ) ◆ , ( 8.17)
with 2 R >0 , and where lat and up correspond to our initial guess for the noise parameters. The network thus may inflate covariance up to a factor 10 and squeeze it up to a factor 10 with respect to its original values. Additionally, as long as the network is disabled or barely reactive (e.g. when starting training), we get z n ⇡ 0 and recover the initial covariance diag ⇣ lat , up ⌘ 2 . We manually remove the part of the sequences where outlier are present. Handling outlier with robust uncertainty estimation [START_REF] Barfoot | State Estimation for Robotics[END_REF] is set for future work.

Regarding process noise parameter Q n , we choose to fix it to a value Q and leave its dynamic adaptation for future work. However its entries are optimized during training, see Section 5.3.

Implementation Details

We provide in this section the setting and the implementation details of our method. We implement the full approach in Python with the PyTorch 1 library for the noise parameter adapter part. We set as initial values before training

P 0 = diag ⇣ R 0 I 2 , 0, v 0 I 2 , 0 4 , b ω 0 I, b a 0 I, R c 0 I, p c 0 I ⌘ 2 , ( 8.18)
Q = diag ⇣ ω I, a I, b ω I, b a I, R c I, p c I ⌘ 2 , ( 8.19
)

N n = diag ⇣ lat , up ⌘ 2 , ( 8.20)
where

I = I 3 , R 0 = 10 3 rad, v 0 =0 .3m/s, b ω 0 = 10 4 rad/s, b a 0 =3
.10 2 m/s 2 , R c 0 = 3.10 3 rad, p c 0 = 10 1 m in the initial error covariance P 0 , ω =1 .4.10 2 rad/s, a = 3.10 2 m/s 2 , b ω = 10 4 rad/s, b a = 10 3 m/s 2 , R c = 10 4 rad, p c = 10 4 m for the noise propagation covariance matrix Q, lat =1m /s, and up =3m /s for the measurement covariance matrix. The zero values in the diagonal of P 0 in (8.18) corresponds to a perfect prior of the initial yaw, position and zero vertical speed, as (IMU-based) dead reckoning methods can only estimate a trajectory and a yaw relative to the starting point.

The adapter is a 1D temporal convolutional neural network with 2 layers. The first layer has kernel size 5, output dimension 32, and dilatation parameter 1. The second has kernel size 5, output dimension 32 and dilatation parameter 3, thus it set the window size equal to N = 15. The CNN is followed by a fully connected layer that output the scalars z lat and z up . Each activation function between two layers is a ReLU unit [START_REF] Goodfellow | Deep Learning[END_REF]. We define = 3 in the right part of (8.17) which allows for each covariance element to be 10 3 higher or smaller than its original values.

Training

We seek to optimize the relative translation error t rel computed from the filter estimates χn , which is the averaged increment error for all possible sub-sequences of length 100 m to 800 m.

Toward this aim, we first define the learnable parameters. It consists of the 6210 parameters of the adapter, along with the parameter elements of P 0 and Q in (8.18)- (8.19), which add 12 parameters to learn. We then choose an Adam optimizer [START_REF] Kingma | ADAM: A Method for Stochastic Optimization[END_REF] with learning rate 10 4 that updates the trainable parameters. Training consists of repeating for a chosen number of epochs the following iterations: i) sample a part of the dataset;

ii) get the filter estimates for then computing loss and gradient w.r.t. the learnable parameters;

ii) update the learnable parameters with gradient and optimizer.

We train the networks for 400 epochs and then applying continual training strategies [START_REF] Parisi | Continual Lifelong Learning with Neural Networks: A Review[END_REF]. This makes sense for online training in a context where the vehicle gathers accurate ground-truth poses from e.g. its LiDAR system or precise GNSS. It requires careful procedures for avoiding over-fitting, such that we use dropout and data augmentation [START_REF] Goodfellow | Deep Learning[END_REF]. [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF]. IMU integration tends to drift or diverge, whereas the proposed method may be used as an alternative to LiDAR based (IMLS [START_REF] Deschaud | IMLS-SLAM: Scan-to-Model Matching Based on 3D Data[END_REF]) and stereo vision based (ORB-SLAM2 [START_REF] Mur-Artal | ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras[END_REF]) methods, using only IMU information. Indeed, on average, our dead-reckoning solution performs better than ORB-SLAM2 and achieves a translational error being close to that of the LiDAR based method IMLS, which is ranked 3 rd on the KITTI online benchmarking system. Data from seq. 03 was unavailable for testing algorithms, and sequences 00, 02 and 05 are discussed separately in Section 6.3. It should be duly noted, though, that IMLS, ORB-SLAM2, and the proposed AI-IMU algorithm all use different sensors. The interest of ranking algorithms based on different information is debatable. Our goal here is rather to evidence that using data from a moderately precise IMU only, one can achieve similar results as stateof-the art systems based on imaging sensors, which is a rather surprising feature.

we set the probability p =0.5 of any CNN element to be ignored (set to zero) during a sequence iteration.

Regarding i), we sample a batch of nine 1 min sequences, where each sequence starts at a random arbitrary time. We add to data a small Gaussian noise with standard deviation 10 4 , a.k.a. data augmentation technique. We compute ii) with standard backpropagation, and we finally clip the gradient norm to a maximal value of 1 to avoid gradient explosion at step iii).

We stress the loss function consists of the relative translation error t rel , i.e. we optimize parameters for improving the filter accuracy, disregarding the values actually taken by N n , in the spirit of [START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF].

Experimental Results

We evaluate the proposed method on the KITTI dataset [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF], which contains data recorded from LiDAR, cameras and IMU, along with centimeter accurate ground-truth pose from different environments (e.g., urban, highways, and streets). The dataset contains 22 sequences for benchmarking odometry algorithms, 11 of them contain publicly available ground-truth trajectory, raw and synchronized IMU data. We download the raw data with IMU signals sampled at 100 Hz (dt = 10 2 s) rather than the synchronized data sampled at 10 Hz, and discard seq. 03 since we did not find raw data for this sequence. The RT3003 2 IMU has announced gyro and accelerometer bias stabil-ity of respectively 36 deg /h and 1 mg. The KITTI dataset has an online benchmarking system that ranks algorithms. However we could not submit our algorithm for online ranking since sequences used for ranking do not contain IMU data, which is reserved for training only. Our implementation is made open-source at: https://github.com/mbrossar/ai-imu-dr.

Evaluation Metrics and Compared Methods

To assess performances we consider the two error metrics proposed in [START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF]:

Relative Translation Error (t rel ): which is the averaged relative translation increment error for all possible sub-sequences of length 100 m, . . . , 800 m, in percent of the traveled distance;

Relative Rotational Error (r rel ): that is the relative rotational increment error for all possible sub-sequences of length 100 m, . . . , 800 m, in degree per kilometer.

We compare four methods which alternatively use LiDAR, stereo vision, and IMUbased estimations:

• IMLS [START_REF] Deschaud | IMLS-SLAM: Scan-to-Model Matching Based on 3D Data[END_REF]: a recent state-of-the-art LiDAR-based approach ranked 3 rd in the KITTI benchmark. The author provided us with the code after disabling the loopclosure module;

• ORB-SLAM2 [START_REF] Mur-Artal | ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras[END_REF]: a popular and versatile library for monocular, stereo and RGB-D cameras that computes a sparse reconstruction of the map. We got the open-source code, disabled loop-closure and we then evaluate the stereo algorithm without modifying any parameter;

• IMU: the direct integration of the IMU measurements based on (8.4)-(8.5), that is, pure inertial navigation;

• proposed: the proposed approach, that uses only the IMU signals and involves no other sensor.

Trajectory Results

We follow the same protocol for evaluating each sequence: i) we initialize the filter with parameters described in Section 5.2; ii) we train then the noise parameter adapter following Section 5.3 for 400 epochs without the evaluated sequence (e.g. for testing seq. 10, we train on seq. 00-09) so that the noise parameter has never been confronted with the evaluated sequence; iii) we run the IMU-based methods on the full raw sequence with ground-truth initial configuration (R imu 0 , v imu 0 , p imu 0 ), whereas we initialize remaining variables at zero (b ω 0 = b a 0 = p c 0 = 0, R c 0 = I); and iv) we get the estimates only on time corresponding to the odometry benchmark sequence. LiDAR and visual methods are directly evaluated on the odometry sequences.

Results are averaged in Table 8.1 and illustrated in Figures 8.1, 8.5 and 8.6, where we exclude sequences 00, 02 and 05 which contain problems with the data, and will be discussed separately in Section 6.3. From these results, we see that:

• LiDAR and visual methods perform generally well in all sequences, and the Li-DAR method achieves slightly better results than its visual counterpart; . The proposed method competes with LiDAR and visual odometry methods, whereas the IMU integration broadly drifts after the car's stop.

• our method competes on average with the latter imaging based methods, see Table 8.1;

• directly integrating the IMU signals leads to rapid drift of the estimates, especially for the longest sequences but even for short periods;

• Our method looks unaffected by stops of the car, as in seq. 07, see Figure 8.5.

The results are remarkable as we use none of the vision sensors, nor wheel odometry. We only use the IMU, which moreover has moderate precision. We also sought to compare our method to visual inertial odometry algorithms. However, we could not find open-source code for such methods that perform well on the full KITTI dataset. We tested [START_REF] Qin | A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors[END_REF] but the code in still under development (results sometimes diverge), and the authors in [START_REF] Ramezani | Vehicle Positioning in GNSS-Deprived Urban Areas by Stereo Visual-Inertial Odometry[END_REF] evaluate their not open-source method for short sequences ( 30 s). The chapter [START_REF] Heo | Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group[END_REF][START_REF] Heo | EKF-Based Visual Inertial Navigation Using Sliding Window Nonlinear Optimization[END_REF] evaluate their visual inertial odometry methods on seq. 08, both get a final error around 20 m, which is four times what our method achieves (final distance to ground-truth is as low as 5 m). This clearly evidences that methods taylored for ground vehicles [START_REF] Zheng | SE(2)-Constrained Visual Inertial Fusion for Ground Vehicles[END_REF][START_REF] Wu | VINS on Wheels[END_REF] may achieve higher accuracy and robustness than general methods designed for smartphones, drones and aerial vehicles.

Results on Sequences 00, 02 and 05

Following the procedure described in Section 6.2, the proposed method seems to have degraded performances on seq. 00, 02 and 05, see Figure 8.9. However, the behavior is wholly explainable: data are missing for a couple of seconds due to logging problems which appear both for IMU and ground-truth. This is illustrated in Figure 8.10 for seq. 02 where we plot available data over time. We observe a jump in the IMU and groundtruth signals, evidencing that data are missing between t = 1 and t = 3. The problem was corrected manually when using those sequences in the training phase described in Section 5.3. Although those sequences could have been discarded due to logging problems, we used them for testing without correcting their problems. This naturally results in degraded performance, but also evidences our method is remarkably robust to such errors in spite of their inherent harmfulness. For instance, the 2 s time jump of seq. 02 results in estimate shift, but no divergence occurs for all that, see Figure 8.9.

Further Results

The performances may be explained by: i) the use of a recent IEKF that has been proved to be well suited for IMU based localization; ii) incorporation of side information in the form of pseudo-measurements with dynamic noise parameter adaptation learned by a neural network; and iii) accounting for a "loose" misalignment between the IMU and the car.

As concerns i), it should be stressed the method is perfectly suited to the use of a conventional EKF and is easily adapted if need be. However we advocate the use of an IEKF owing to its accuracy and convergence properties [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF].

To illustrate the benefits of points ii) and iii), we consider two sub-versions of the proposed algorithm. One without alignment, i.e. where R c n and p c n are not included in the state and fixed at their initial values R c n = I, p c n = 0, and a second one that uses the static filter parameters (8.18)- (8.20). End trajectory results for the highway seq. 01 are plotted in Figure 8.7, where we see that the two sub-version methods have trouble when the car is turning. Therefore their respective translational errors t rel are 950 1,000 1,050 1,100 Dynamically adapting the measurement covariance and considering misalignment between car and inertial frames makes the translational error drop from 1.94% to 1.11%, see Section 6.4.

higher than the full version of the proposed method: the proposed method achieves 1.11%, the method without alignment level arm achieves 1.65%, and the absence of covariance adaptation yields 1.94% error. All methods have the same rotational error r rel =0.12 deg /m. This could be anticipated for the considered sequence since the full method has the same rotational error than standard IMU integration method.

Limitations and Discussion

Limitations regarding the approach revolve around the following points: 1) one may not want to depend on AI; 2) generalization; and 3) computational complexity and real-time embedded applications. Regarding the first point, it may be objected AI is only as good as the data available and hence has weaknesses. Moreover, when vision sensors fail, IMU is critical to provide a refuge trajectory but in case of emergency one might want to depend on more classical and robust IMU algorithms, s AI is still difficult to certify [START_REF] Sünderhauf | The Limits and Potentials of Deep Learning for Robotics[END_REF]. To this respect, note that our deep network only affects the Kalman filter tuning parameters. If one wants to remove AI while achieving good performances, it is possible to infer some adaptation rules from the obtained results.

To this end, we plot the covariances computed by the adapter for seq. 01 in Figure 8.8. The adapter clearly increases the covariances during the bend, i.e. when the gyro yaw rate is important. This is especially the case for the zero velocity measurement (8.15): its associated covariance is inflated by a factor of 10 2 between t = 90 s and t = 110 s. This illustrates the kind of information the adapter has learned and how this behavior may be emulated in a more traditional pipeline.

Discussion

As our deep network only affects the Kalman filter tuning parameters, it is possible to infer some adaptation rules from the obtained results. These rules may substitute the deep network while achieving good performances, if one wants to remove AI for safety and commercial purposes (as AI is still difficult to certify [START_REF] Sünderhauf | The Limits and Potentials of Deep Learning for Robotics[END_REF]), or simply due to hardware limitations. To this end, we plot the covariances computed by the adapter for seq. 01 in Figure 8.8. The adapter clearly increases the covariances during the bend, i.e. when the gyrometer's yaw rate is larger. This is especially the case for the zero velocity measurement (8.15): its associated covariance is inflated by a factor of 10 2 between t = 90 s and t = 110 s. Indeed, such a large noise parameter inflation indicates the AI-based part of the algorithm has learned and recognizes that pseudo-measurements have no value for localization at those precise moments, so the filter should barely consider them. This illustrates the kind of information the adapter has learned and how this behavior may be emulated in a more traditional pipeline. An alternative consists in using these pseudo-measurement with robust cost [START_REF] Barfoot | State Estimation for Robotics[END_REF], e.g. Huber loss in the filter update or in the loss for training neural networks. Learning to adapt with robust (non Gaussian) uncertainty is a promishing perspectives of future works. Figure 8.8 gives also a striking illustration of the role of observation noise autocorrelation in Kalman filter tuning. We lack space to get into the theory of effective sample size [START_REF] Thiébaux | The Interpretation and Estimation of Effective Sample Size[END_REF] but intuitively, with a minimal time ⌧ between 2 independent observations of 1 s, measurements at 100 Hz do not bring much more information than at 1 Hz. As a consequence, if the frequency f s of the Kalman updates (100 Hz here) is high, i.e., f s ⌧ 1 , then the instant statistical uncertainty of the observation is not the optimal value for the matrix N n and a good rule of thumb is multiplying it by f s ⌧. These theoretical considerations become very concrete on Figure 8.8: we see the optimal value of the "covariance" of observation y up n to be given to the Kalman filter is 100 m 2 /s 2 , which models for instance a noise of instant covariance 1 m 2 /s 2 with autocorrelation time 1 s, which is coherent for vertical variations of velocity. However if for some reason the practitioner wants to keep the noise covariance parameter N n at bounded values at all times, it is always possible to turn off pseudo-measurements or enforce a predefined upper bound on N n that feeds the Kalman filter when the AIbased adapter goes above the predefined bound.

Interestingly, we see large statistical uncertainty (which should clearly be below 100 m 2 /s 2 ) and the inflated covariances whose values are computed for the sole pur- Finally, in terms of execution time in view of real time applications, once the network is trained, using the network is very cheap computationally speaking. Indeed the network takes the raw IMU data and outputs a covariance for the Kalman filter. This process takes 15% only of the overall algorithm time execution, while the Kalman filter takes 85%. As Kalman filter routinely run on embedded code in inertial navigation in aerospace engineering, and the method is marginally more computationally demanding than a Kalman filter there shall be no problem. Note that, however, training the network may take much time but of course this should be done once beforehand.

Conclusion

This chapter proposes a novel approach for inertial only dead-reckoning for wheeled vehicles which builds upon deep neural networks to dynamically adapt the parameters of a Kalman filter. We have shown the following facts. 1) It is possible to obtain surprisingly accurate results using only a moderate cost IMU, thanks to the use of a Kalman filter that combines standard IMU equations with side information about dynamics of wheeled vehicles. The algorithm competes with vision based methods, although only the IMU is used (and not a single other sensor, such as GNSS). 2) Deep neural networks are a powerful tool for dynamic adaptation of Kalman filter tuning parameters (noise covariance matrices). 3) Beyond deep neural nets, correctly assessing measurement covariance dynamically allows Kalman filters to achieve much better performance, and this opens avenues for fusion with other sensors. The subject of generalization, and notably how the network architecture may be reused in similar applications, is left for future research, as tuning CNNs represents in itself a current research field. That said, the code we made publicly available may be used as is, and adapted to other vehicles. Moreover, as mentioned in Section 6.6, the chapter proves that dynamic covariance adaptation plays a huge role for accurate localization, and simple practical engineered adaptation rules might be pursued instead of AI-based ones. Futures works would adress learning to adapt uncertainties in a robust (non-Gaussian) context and the question of generalization, i.e. how a neural network trained on a platform may be used in another platform.

Appendix A

The IEKF [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Barrau | Invariant Kalman Filtering[END_REF] is an EKF based on an alternative state error, see Chapter 2. One must define a linearized error, an underlying group to derive the exponential map, and then the methodology is akin to the EKF's, see Chapter 2.

Linearized Error: the filter state χ n is given by (8.8). The state evolution is given by the dynamics (8.5)-(8.7) and (8.12)-(8.13), see Section 3. Along the lines of [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF], variables

χ imu n := (R imu n , v imu n , p imu n ) are embedded in the Lie group SE 2 (3). Then biases vector b n =[b ωT n , b aT n ] T 2 R 6
is merely treated as a vector, that is, as an element of R 6 viewed as a Lie group endowed with standard addition, R c n is treated as element of Lie group SO(3), and p c n 2 R 3 as a vector. Once the state is broken into several Lie groups, the linearized error writes as the concatenation of corresponding linearized errors, that is,

e n = h ξ imuT n e bT n ξ R c T n e p c T n i T ⇠N(0, P n ) , ( 8.21)
where state uncertainty e n 2 R 21 is a zero-mean Gaussian variable with covariance P n 2 R 21⇥21 . As (8.15) are measurements expressed in the robot's frame, they lend themselves to the Right IEKF methodology. This means each linearized error is mapped to the state using the corresponding Lie group exponential map, and multiplying it on the right by elements of the state space. This yields: with R n = R imu n , 0 = 0 3⇥3 , and Q n denotes the classical covariance matrix of the process noise as in Section 5.2.

χ imu n = exp SE 2 (3) (ξ imu n ) χimu n , (8.22)
b n = bn + e b n , (8.23)
R c n = exp SO(3) ⇣ ξ R c n ⌘ Rc n , ( 8
g ⇥ 00 (v imu n ) ⇥ R n R n 0 3⇥6 0I 3 0 (p imu n ) ⇥ R n 00 3⇥6 0 12⇥21 3
R n 00 3⇥12 (v imu n ) ⇥ R n R n 0 3⇥12 (p imu n ) ⇥ R n 00 3⇥12 0 12⇥3 0 12⇥3 I 12

Update

Step: the measurement vector y n+1 is computed by stacking the motion information

y n+1 = " v lat n+1 v up n+1 # = 0, (8.28)
with assessed uncertainty a zero-mean Gaussian variable with covariance N n+1 = cov (y n+1 ).

We then compute an updated state χ+ n+1 and updated covariance P + n+1 following the IEKF methodology, i.e. we compute

S = ⇣ H n+1 P n+1 H T n+1 + N n+1 ⌘ , ( 8
H n = A h 0R imuT n 0 (p c n ) ⇥ 0BC i , ( 8.37)
where A = [I 2 0 2 ] R cT n selects the two first row of the right part of (8.37),

B = R imuT n (v imu n + (ω imu n b ω n ) p c n ) ⇥ and C = (ω imu n b ω n ) ⇥ .
CHAPTER 9

A New Approach to 3D ICP Covariance Estimation

The present chapter has been published in IEEE Robotics and Automation Letters.

Résumé

Dans ce chapitre, nous adressons le problème d'estimer la covariance de l'algorithme ICP, covariance qui peut ensuite être donnée comme covariance de l'observation à un filtre de Kalman où un algorithme d'optimisation pour la fusion de données En effet, en robotique mobile, la recherche de correspondance entre des nuages de points en utilisant l'algorithme itératif ICP permet d'estimer les déplacements succesifs du capteur, et il peut s'avérer important d'évaluer l'incertitude associée à la transformation obtenue, notamment à des fins de fusion de capteurs. Dans ce chapitre, nous proposons une nouvelle approche de l'estimation de l'incertitude 3D de l'ICP qui tient compte de toutes les sources d'erreur énumérées dans les travaux pionniers de Censi [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF], à savoir une convergence vers un minimum local, des situations sous faibles contraintes, et le bruit du capteur. Notre approche repose sur deux faits. Premièrement, l'incertitude de l'ICP dépend entièrement de l'incertitude sur la transformation initiale. Ainsi parler de la covariance de l'ICP n'a de sens que par rapport à l'incertitude d'initialisation, qui découle généralement des erreurs d'odométrie. Nous capturons cela en utilisant la transformation sans-parfum, qui reflète également la corrélation entre les incertitudes initiales et finales. Ensuite, le bruit supossé blanc du capteur conduit à une incertitude car l'ICP est biaisé en raison par exemple du biais du capteur que nous expliquons. Notre solution est testée sur des données réelles accessible mellant des environnements structurés et non structurés, où notre algorithme prédit des résultats cohérents avec une incertitude réelle et se compare favorablement aux méthodes précédentes.

Chapter abstract

In the present chapter we address the problem of estimating the covariance of the Iterative Closest Point (ICP) algorithm, which can be fed in a Kalman filter or an optimization-based sensor-fusion algorithm as the measurement covariance. Indeed, in mobile robotics, scan matching of point clouds using ICP allows estimating sensor displacements, and it may prove important to assess the associated uncertainty about the obtained rigid transformation, especially for sensor fusion purposes. In this chapter we propose a novel approach to 3D uncertainty of ICP that accounts for all the sources of error as listed in Censi's pioneering work [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF], namely wrong convergence, underconstrained situations, and sensor noise. Our approach builds on two facts. First, the uncertainty about the ICP's output fully depends on the initialization accuracy. Thus speaking of the covariance of ICP makes sense only in relation to the initialization uncertainty, which generally stems from odometry errors. We capture this using the unscented transform, which also reflects correlations between initial and final uncertainties. Then, assuming white sensor noise leads to overoptimism as ICP is biased owing to e.g. calibration biases, which we account for. Our solution is tested on publicly available real data ranging from structured to unstructured environments, where our algorithm predicts consistent results with actual uncertainty, and compares favorably to previous methods.

Introduction

Point clouds and the Iterative Closest Point (ICP) algorithm play a crucial role for localization and mapping in modern mobile robotics [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF][START_REF] Holz | Registration with the Point Cloud Library: A Modular Framework for Aligning in 3-D[END_REF]. ICP computes an estimate of the 3D rigid transformation that aligns a reading point cloud to a reference point cloud (or more generally a model or a surface). The algorithm starts with a first transformation estimate, and repeats -until convergence -point association and leastsquare minimization, where initialization is naturally provided in mobile robotics by odometry [START_REF] Dube | An Online Multi-robot SLAM System for 3D LiDARs[END_REF][START_REF] Geneva | LIPS: LiDAR-Inertial 3D Plane SLAM[END_REF] based on wheel speeds, inertial sensors, or vision. The point association matches points between the two clouds by generally associating each point of the second cloud to its closest point in the first one. Then, the algorithm minimizes a user-chosen metric between the matched points that provides an update of the current estimate. In spite of robust filtering that are broadly used during the alignment of point clouds, a.k.a. registration, ICP is subject to errors stemming from sensor noises, underconstrained environments that result in unobservable directions, and local minima [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF][START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF].

Sources of ICP Uncertainty

The pioneering work of Censi [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] identifies the following sources of error for ICP registration: wrong convergence (not handled by Censi's formula), underconstrained situations, and sensor noise. As indicated by preliminary remarks in [START_REF] Pfister | Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation[END_REF][START_REF] Barczyk | Towards Realistic Covariance Estimation of ICPbased Kinect V1 Scan Matching: The 1D Case[END_REF] we believe a fourth important source is missing: the one that stems from sensor biases. In the present chapter we consider indeed the following sources of error:

Initial Transformation: ICP is subject to error due to wrong initialization that makes the algorithm converge to a local minimum out of the attraction basin of the true solution, as largely observed in practice, see e.g. [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF][START_REF] Iversen | Prediction of ICP Pose Uncertainties Using Monte Carlo Simulation with Synthetic Depth Images[END_REF] and Figure 9.1. In practice it often proves to be the dominant error, where very recent works adress this problem for obtaining "certifiable" solvers, such as TEASER [START_REF] Yang | TEASER: Fast and Certifiable Point Cloud Registration[END_REF], which claims to be able to either find the global optimum alignment between two pointclouds or report an error if it fails to do so, even with a very high rate of outlier measurements.

Sensor White Noise: each point measured in a point cloud is affected by an independent random sensor noise of centimetric magnitude which is a function of point depth and beam angle [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF][START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF]. , where we sample 1000 initial estimates from two distributions reflecting accurate (a) and dispersed (b) ICP initialization and that respectively correspond to the easy and medium scenarios of [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF]. We see the uncertainty on the ICP estimate, that is, dispersion of red points, wholly depends on the accuracy of initialization. There is no "uncertainty of ICP" per se.

Sensor Bias Noise: the observed points share common errors that stem from: temperature drift effect, i.e. stability of the laser [START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF]; observed material [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF]; incidence and beam angles resulting in large bias [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF]; or wrong calibration, e.g. [START_REF] Deschaud | IMLS-SLAM: Scan-to-Model Matching Based on 3D Data[END_REF] found a distortion of 0.22 deg of the scan point clouds due to intrinsic calibration process. This correlated noise, a.k.a. bias, strictly limits the confidence we may have in the ICP estimate. To our best knowledge this is often omitted with a few exceptions: e.g., [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF] removes bias on point measurements due to sensor beam angle, and preliminary ideas may be found in [START_REF] Pfister | Weighted Range Sensor Matching Algorithms for Mobile Robot Displacement Estimation[END_REF][START_REF] Barczyk | Towards Realistic Covariance Estimation of ICPbased Kinect V1 Scan Matching: The 1D Case[END_REF].

Randomness Inherent to the ICP Algorithm: ICP is generally configured with random filtering processes [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF], e.g. sub-sampling, such that two solutions with exactly the same inputs would differ.

In the following we address uncertainty coming from 1), 2) and 3) and do not consider 4), which should be marginal.

Brief Literature Review

Various approaches exist for estimating the covariance of the ICP algorithm, each of which being a trade-off between accuracy and execution time. Monte-Carlo algorithms, e.g. [START_REF] Iversen | Prediction of ICP Pose Uncertainties Using Monte Carlo Simulation with Synthetic Depth Images[END_REF][START_REF] Bengtsson | Robot Localization Based on Scan-Matching[END_REF], sample noisy scans (from a reference scan) and ICP initializations to compute a large number of ICP registration results, define the covariance of the sampled results as the covariance estimation, and use the estimated covariance for all future registration with the reference scan, thus getting a covariance function of the reference scan only. Another category of covariance estimation methods relies on closed-form expressions [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Biber | The Normal Distributions Transform: A New Approach to Laser Scan Matching[END_REF][START_REF] Bonnabel | On the Covariance of ICP-Based Scan-Matching Techniques[END_REF][START_REF] Prakhya | A Closed-form Estimate of 3D ICP Covariance[END_REF], whose underlying assumption consists in linearizing the objective function used in ICP around the convergence point, ruling out the possibility for wrong convergence and the uncertainty that stems from it. Albeit still used in practice, Censi's pioneering formula [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] is widely considered as overoptimistic, see e.g. [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF]. Recently, [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF] leveraged learning based approaches to estimate ICP uncertainty stemming from inaccurate ICP initialization.

Contributions and Chapter's Organization

Our approach introduced in Section 2 extends existing works in three ways: 1) we consider ICP uncertainty coming both from sensor errors and ICP initialization. 2) we raise an important point which is that ICP uncertainty in itself is meaningless as it is inherently related to uncertainty in the initialization pose (unless there is a global minimum). This is supported by experiments displayed in Figure 9.1. We address this problem by outputting a covariance matrix of larger dimension that also reflects the correlation between ICP final and initial estimates. And 3) we estimate in Section 3 the ICP uncertainty combining a closed-form expression using [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bonnabel | On the Covariance of ICP-Based Scan-Matching Techniques[END_REF] accounting for sensor biases, and derivative-free methods using the unscented transform of [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF][START_REF] Julier | A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[END_REF], which comes at a lower computational cost than Monte-Carlo runs.

Besides, we evaluate, compare and discuss our approach on the dataset of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF] in Sections 4 and 5, where our approach obtains consistent estimates and achieves better results than existing methods. The code to reproduce the results of the chapter is made publicly available at: https://github.com/CAOR-MINES-ParisTech/3d-icp-cov.

Throughout the chapter, we configurate the ICP as suggested in [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF] with a pointto-plane error metric.

Proposed Approach 2.1 2.1 Pose Representation and Pose Uncertainty Representation

The true transformation between two point clouds and its ICP-based estimate both live in the set of 3D rigid transformations SE [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], see Chapter 2. Note that, it is consistent with matrix multiplication: if T 1 transforms a first point cloud into a second one, and then T 2 transforms the latter into a third cloud, then the matrix T 2 T 1 2 SE(3) encodes the transformation between the first and the third clouds.

We use the uncertainty representation for poses as

T = T exp(ξ), where ξ ⇠N(0, Q) , ξ 2 R 6 , (9.1)
with ξ a zero-mean Gaussian variable of covariance Q and where exp(•) denotes the exponential map of SE [START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF], which maps elements ξ to poses. For uncertainty representation (9.1), we adopt the notation T ⇠N L (T, Q). Note that, in (9.1), the vector ξ 2 R 6 may be viewed as the error between T and T. Indeed the relative transformation between poses T and T is encoded in ξ as T 1 T = exp(ξ).

The Role of ICP Initialization

The ICP procedure seeks to estimate the transformation T true 2 SE(3) that maps a first cloud of points P to a second cloud (or a model) Q as follows [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF][START_REF] Holz | Registration with the Point Cloud Library: A Modular Framework for Aligning in 3-D[END_REF]:

i) we have a first "guess" for the transformation we call T ini , a.k.a. initial or coarse alignment [START_REF] Holz | Registration with the Point Cloud Library: A Modular Framework for Aligning in 3-D[END_REF];

ii) then we initialize the ICP algorithm by applying a transformation T 1 ini to the cloud Q. This way the transformation the ICP seeks to estimate become the relative pose T rel := T 1 ini T true . We thus get an estimate Trel = icp

⇣ P , T 1 ini Q ⌘ for T rel ;
iii) finally the estimate of T true that the algorithm outputs is Note that if T rel is perfectly estimated we recover T true as then Ticp = T ini T rel = T ini T 1 ini T true = T true no matter how far the initial guess T ini is from T true . Let us introduce the various errors at play in ICP. In robotics, the initial guess in i) is typically provided through inertial sensors or wheeled odometry [START_REF] Dube | An Online Multi-robot SLAM System for 3D LiDARs[END_REF][START_REF] Geneva | LIPS: LiDAR-Inertial 3D Plane SLAM[END_REF]. We have thus an initialization error that stems from sensor imperfections, encoded by a vector ξ ini , and one may write

Ticp := T ini Trel = T ini icp ⇣ P , T 1 ini Q ⌘ . i) ICP initialization: t ini = t true + ξ ini ii) ICP
T ini = T true exp(ξ ini ), ξ ini ⇠N(0, Q ini ), (9.2)
which is advocated in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Long | The Banana Distribution is Gaussian: A Localization Study with Exponential Coordinates[END_REF] to suit particularly well represent odometry errors in terms of pose. Then, ICP estimates the relative transformation between T true and T ini , that is, outputs an estimate Trel of the actual initial error T rel which writes

T rel = T 1 ini T true = exp( ξ ini )T 1 true T true = exp( ξ ini ) ' I 4 ξ îni . (9.3)

ICP Estimate Trel of Relative Pose T rel

At step ii) above, that is, once initialization is done, see [START_REF] Pomerleau | A Review of Point Cloud Registration Algorithms for Mobile Robotics[END_REF], ICP provides an estimate Trel of the relative transformation T rel of (9.3) as a function

Trel := icp ⇣ P , T 1 ini Q ⌘ (9.4)
of the point clouds P and Q. Thus Trel appears as a function f (T rel ) 2 SE(3) of the true relative transformation T rel , typically affected by the phenomena of wrong convergence. Moreover, sensor (scanner) noise induces small fluctuations in the point clouds that affects this estimation. This yields:

Trel = f (T rel ) exp(Gw)=f ⇣ exp( ξ ini ) ⌘ exp(Gw), (9.5)
where w 2 R 6K encodes errors due to sensor noise on each of the K pairs of points in the clouds, and Gw 2 R 6 the resulting 6 degrees of freedom error made on T rel . Sensor noise stems from unknown parameters that depend upon the calibration process and drift with temperature [START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF]. If the ICP is initialized on the true pose T true then there is no wrong convergence and the only error stems from noise, i.e., f (exp(0)=f (I 4 )=I 4 . Thus f (•) 2 SE( 3) is close to I 4 , and the model may be linearized around ξ ini = 0, w = 0 as

f ⇣ exp( ξ ini ) ⌘ exp(Gw) ' f (I 4 ξ îni ) exp(Gw) ' I 4 +( Jξ ini + Gw) ^, (9.6)
where matrix J encodes the linear approximation of f (•).

ICP Final Pose Error

Let us now consider step iii) of the ICP algorithm, i.e., the final estimate

Ticp = T ini Trel (9.7) = T true exp(ξ ini )f ⇣ exp( ξ ini ) ⌘ exp(Gw). ( 9
Ticp ' T true exp ((I 6 J)ξ ini + Gw) .
(9.9) Figure 9.2 recaps the computations for 1D translations. There are a couple of situations of interest. Let us momentarily assume sensor noise to be turned off, w = 0, for simplicity.

• If there is one global minimum, then the ICP systematically recovers the relative transformation (9.3) at step ii) of the algorithm, i.e. Trel = T rel and thus f (T rel )= T rel . So f (exp( ξ ini )) ' I 4 ξ îni and we identify J = I 6 in this case. As a result the final estimate (9.9) is T true exp(0)=T true indeed.

• On the other hand, in the directions where we have no information, e.g. along hallways or in underconstrained environment [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF], the relative transformation will not be affected in the corresponding directions meaning that (along those directions) J = 0 and the final error then has the form T 1 true Ticp = T 1 true T true exp(ξ ini )= exp(ξ ini ), that is, the initialization error fully remains.

In intermediate cases (when there are local minima) the remaining error is a fraction Jξ ini of the initialization error.

Corresponding ICP Error Covariance

If we represent ICP uncertainties resorting to concentrated Gaussian (9.1) as Ticp ⇠ N L ⇣ T true , Q icp ⌘ , i.e., we posit T 1 true Ticp = exp(ξ icp ), then the covariance matrix Q icp of ξ icp describes dispersion (hence uncertainty) of the ICP error. Plugging the latter representation into (9.9), we have ξ icp =( I 6 J)ξ ini + Gw. As the initialization error is assumed to have covariance matrix Q ini typically inferred though an odometry error model [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Long | The Banana Distribution is Gaussian: A Localization Study with Exponential Coordinates[END_REF] and by denoting Q sensor the covariance of scan sensor noise w, the covariances add up owing to independence of sensor noises, and by squaring ξ icp =(I 6 J)ξ ini + Gw we find

Q icp =(I 6 J)Q ini (I 6 J) T + GQ sensor G T . (9.10)
This is our first result about ICP covariance. The first term related to the initialization uncertainty and that accounts for wrong convergence, lack of constraints in the clouds and unobservable directions, and the second one related to scan noise and that may be computed through "Censi-like" [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] formulas as we will show in section 3.

Discussion

Albeit not obvious, J actually heavily depends on Q ini . This is an insight of the present chapter: uncertainty of ICP does not exist in itself. Assume indeed there are various local minima. If Q ini is very small, then all initializations T ini fall within the attraction basin of T true and thus f (T rel )=T rel and we identify J = I 6 . But if Q ini is large enough only a fraction of initializations T ini lead to f (T rel )=T rel , the ones that get trapped in other local minimas do not lead to correct estimate of T rel and J , I 6 . Thus J is not the analytical Jacobian of function f (•), and may be viewed as its "statistical linearization" [START_REF] Gustafsson | Some Relations Between Extended and Unscented Kalman Filters[END_REF]. This prompts the use of an unscented transform [START_REF] Julier | A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[END_REF] to compute it, see Section 3.2.

Maximum Likelihood Fusion of Initial and ICP Estimates

T ini and Ticp may be viewed as two estimates of T true associated with uncertainty respectively Q ini = cov(ξ ini ) and Q icp = cov(ξ icp ) where ξ icp =(I J)ξ ini + Gw. The corresponding pose fusion problem of finding the Maximum Likelihood (ML) of a pose T true given two uncertain pose estimates was considered in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF], with the important difference that herein ξ ini and ξ icp are not independent, they are correlated, with joint matrix of initialization and ICP errors .11) Using linearization as previously and following first-order computations in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Mangelson | Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra[END_REF], the maximum likelihood estimate of T true may be approximated as TML = T true exp(ξ ML ) with cov(ξ ML )=Q ML , with Q ML defined through its inverse:

Q := cov ⇣ " ξ ini ξ icp # ⌘ = " Q ini Q ini (I J) T (I J)Q ini Q icp # . ( 9
Q 1 ML = " I I # T " Q ini Q ini (I J) T (I J)Q ini Q icp # 1 " I I # . (9.
12)

The latter stems from classical linear estimation theory and may be proved using the Kalman information filter: to the first order Tini and Ticp are considered as two noisy measurements of T true with joint covariance (9.11), the measurement matrix is thus

H := h II i T
and as T true is initially totally unknown the prior covariance satisfies P 1 = 0. The covariance of the Kalman estimate in the light of measurements is thus updated in information form as

P 1 0 + H T Q 1 H.

Practical Covariance Computation

This section describes our algorithm for estimating the 3D ICP uncertainty covariance (9.10) leveraging findings of Section 2. We propose to first compute the rightmost term of (9.10) which is due to sensor noise.

Computation of Dispersion Owing to Sensor Noise

We now focus on the computation of GQ sensor G T . The cost function of point-to-plane ICP after initialization writes

J Trel ⇣ P , T 1 ini Q ⌘ = P K k=1 k ⇣ Trel p k qk ⌘ • n k k 2 ,
where the qk 's denote the points of T 1 ini Q and K is the number of pairs of matched points. Linearizing on SE(3), we may linearize the cost

J Trel exp(ξ) ⇣ P , T 1 ini Q ⌘ = P K k=1 k ⇣ Trel exp (ξ) p k qk ⌘ •n k k 2
w.r.t. estimate Trel at Trel = T rel as

J Trel exp(ξ) ⇣ P , T 1 ini Q ⌘ ' K X k=1 kB k ξ d k k 2 , (9.13)
with d k a scalar being function of differences between pairs of points and point normals. Least squares formulas yield an optimal value ξ ⇤ = A 1 P K k=1 B T k d k , where we let

A = P K k=1 B T k B k .
Each d k is affected by k-th component w i of previously introduced sensor noise w, and this induces fluctuations in ξ ⇤ over various experiments. Let's postulate w i = b + ⌫ i with ⌫ i a white noise of variance 2 , and b and unknown calibration bias that is identical for all points but varies from one experiment to the next. Following least squares covariance, see [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bonnabel | On the Covariance of ICP-Based Scan-Matching Techniques[END_REF], we end up with:

GQ sensor G T = 2 A 1 + A 1 B cov (b) B T A 1 , (9.14)
where

A = P K k=1 B T k B k , and B = P K k=1 B T k .
We recover the covariance 2 A 1 of [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bonnabel | On the Covariance of ICP-Based Scan-Matching Techniques[END_REF] w.r.t. sensor white noise, and a new term, A 1 B cov (b) B T A 1 , that represents the covariance w.r.t. the unknown bias b, that is, correlated noise. This new additional term is paramount as A has magnitude proportional to K, hence A 1 is very small, explaining that Censi's formula (based on A 1 only) seems overoptimistic [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF]. For example A 1 has trace 0.2 cm for the registration displayed in Figure 9.1 whereas the covariance w.r.t. the unknown bias has trace 2.6 cm. In practice b arises from sensor calibration, laser stability [START_REF] Wang | Characterization of a RS-LiDAR for 3D Perception[END_REF], observed material [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF], and incidence of beams [START_REF] Laconte | LiDAR Measurement Bias Estimation via Return Waveform Modelling in a Context of 3D Mapping[END_REF].

In the remainder we assume bias standard deviation to be approximately 5 cm as in [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF].

Note that (9.14) captures the effect of underconstrained situations like hallways. Indeed in unobservable directions the cost J Trel (•) is constant, yielding small eigenvalues for A and hence large covariance (9.14). Derivation of B k and extraction of G in (9.14) are available with chapter code.

Computation of Dispersion owing to ICP Initialization

Computation of (I 6 J)Q ini (I 6 J) T in (9.10) is of greater importance as in practice it largely dominates GQ sensor G T . We propose to compute it in a deterministic derivativefree method, in which we adapt the unscented transform [START_REF] Julier | A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[END_REF] (see e.g. Algorithm 1) pose by following [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF][START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF]. The advantages of using our unscented based method rather than Monte-Carlo sampling are fourfold: 1) it is deterministic; 2) it remains computationally reasonable by adding only 12 ICP registrations; 3) it explicitly computes the cross-covariance matrix between Ticp and T ini as a by-product without extra computational operations; and 4) it scales with Q ini , i.e. our approach naturally self-adapts to the confidence we have in initialization without extra parameter tuning.

We compute the covariance as follows, see Algorithm 10:

• we consider the prior distribution T prior ⇠N L (T ini , Q ini ), which is approximated by a set of so-called sigma-points ξ j ini , see step 1);

Algorithm 10: Computation of matrix J in (9.10)

Input: P , Q, T ini , Q ini , Ticp = T ini icp ⇣ P , T 1 ini Q ⌘ ;
// set sigma points

1 ξ j ini = col ⇣ p 6Q ini ⌘ j ,j=1,...,6, ξ j ini = col ⇣ p 6Q ini ⌘ j 6
,j=7,...,12;

// propagate sigma points through ( 7) [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF]; ) divided into translation and rotation parts. As the ICP error distributions are not Gaussian [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF], we provide robust statistics (starred, *) by removing both the more and less accurate quantiles of each registration. The proposed method outperforms the two others.

2 T j ini = T ini exp ✓ ξ j ini ◆ ,j=1,...,
Tj icp = T j ini icp ✓ P , (T j ini ) 1 Q ◆ ,
• we approximate the propagated distribution T prop = T prior icp(P , T 1 prior Q) as

T prop = N L (T ini , Q ini ) icp ⇣ P , N L (T ini , Q ini ) 1 Q ⌘ ⇠N L ⇣ Ticp , (I 6 J)Q ini (I 6 J) T ⌘ , ( 9.15)
after propagating each sigma-point in steps 2) and 3), where Ticp is the given ICP pose estimate. We compute the covariance and infer the matrix J as a by-product in respectively steps 4) and 6).

We derive the algorithm by following [START_REF] Brossard | Unscented Kalman filtering on Lie groups[END_REF] for pose measurement, zero-mean prior distribution, and where we set ↵ = 1.

Experimental Results

Dataset Description & ICP Algorithm Setting

This section evaluates the ability of the approach to estimate ICP uncertainty on the Challenging data sets for point cloud registration algorithms [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF]. It comprises eight sequences where point clouds are taken in environments ranging from structured to unstructured, and indoor to outdoor. Each sequence contains between 31 and 45 point cloud scans along with ground-truth pose for each scan, that provides a total of 268 scans and 1020 different registrations as we align each scan with the three scans the following.

We configure the ICP as in [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF] with 95% random sub-sampling, kd-tree for data association, and point-to-plane error metric where we keep the 70% closest point associations for rejecting outliers.

Compared Methods and Evaluation Metrics

This section evaluates the following methods:

Qcensi : the close-form method of [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF] adapted for the ICP setting defined above;

Qmonte

carlo : the covariance computed after sampling of 65 Monte-Carlo ICP estimates;

Qicp : our proposed approach detailed in Section 3.

Each method assumes depth sensor white noise and bias with 5 cm standard deviation, which is the mean value found in [START_REF] Pomerleau | Noise Characterization of Depth Sensors for Surface Inspections[END_REF] for the Hokuyo sensor used for these experiments, and all methods know the initial uncertainty Q ini , whose magnitude 0.1 m and 10 deg corresponds to the easy scenario of [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF]. We compare the above methods using two metrics:

Normalized Norm Error (NNE): that evaluates the historically challenging [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF][START_REF] Bengtsson | Robot Localization Based on Scan-Matching[END_REF] prediction of the covariance scale, and is computed as

NNE = ⇣ 1 N N X n=1 kξ n k 2 2 / trace( Qn ) ⌘ 1/2 , ( 9.16)
where ξ n = exp 1 (T 1 true Tn ) with is the transformation error and Qn the estimated uncertainty covariance matrix, and averaged over N samples. This metric characterizes the uncertainty as only the true registration is known (the exact distribution of the point cloud is unknown). The target value is one, below one the estimation is pessimistic, whereas a value over one indicates an overoptimistic estimation.

Kullback-Leibler Divergence (KL div.): which is computed between a pseudo-true distribution and the estimated distribution. The pseudo-true distribution is computed after sampling 1000 ICP estimates of the evaluated registration over the initial position. As sensor noise is fixed in the point clouds, this distribution represents the uncertainty stemming from initialization errors. [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where Mountain is not considered in [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF]. Our method obtains on average the best uncertainty assessment, albeit slightly optimistic.

Results

Results are averaged over 1000 initializations for each of the 1020 considered pairs of point clouds, representing a total of more than one million registrations, where the ICP is initialized with a different estimate T ini sampled from N L (T true , Q ini ). Table 9.1 provides average results over the eight sequences, and Figure 9.3 illustrates typical registrations from structured to unstructured environments. We observe:

• Qcensi is far too optimistic and unreliable for sensor-fusion, as noted in [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF]. Its centimetric confidence interval makes sense only when ICP is very accurate;

• Qmonte carlo is overoptimistic when the discrepancy arising from ICP initialization remains negligible, see Figure 9.3 (b), for which the method predicts a confidence interval with millimetric size. This is naturally explained as the method assumes no error caused by sensor noises;

• the proposed method obtains the best results for both metrics as displayed in Table 9.1. It notably outperforms Qmonte carlo while deterministic hence more reliable, and computationally much cheaper. The dominant term is generally due to initial uncertainty. However in "global minimum" cases the sensor bias used for computing Qicp slightly inflates the covariance of [START_REF] Censi | An Accurate Closed-form Estimate of ICP's Covariance[END_REF], and more closely captures actual uncertainty, see Figure 9. 3 (a,b).

Besides outperforming the other methods, our method provides simple parameter tuning: we set the bias noise standard deviation as having same magnitude as sensor white noise, and the error stemming from ICP initialization does not need to be tuned when Q ini is an output of inertial, visual, or wheeled odometry system [START_REF] Dube | An Online Multi-robot SLAM System for 3D LiDARs[END_REF][START_REF] Geneva | LIPS: LiDAR-Inertial 3D Plane SLAM[END_REF].

Regarding computational complexity and execution time, step 2) of the algorithm requires 12 registrations which take 6 s when registrations are computed parallely, whereas the remaining part of the algorithm takes less than 0.1 s. The 65 Monte-Carlo runs are more than five times more demanding than the the proposed method.

Complementary Experimental Results

This section provides an in-depth analysis of the method in term of trajectory consistency, robustness to high and misknown initial uncertainty, and discusses the advantages and the validity of the approach.

Application to Trajectory Consistency

We asses the quality of the covariance estimation in Section 3 over trajectories as follows. For each sequence of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], we compute the global pose estimate at scan l by compounding transformations such that Tl = T0,1 ... Tl 1,l , whose covariance is computed with the closed-form expressions of [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF] which are valid up to 4-th order approximation. We compare three methods defined as: CELLO-3D : reproduced results of [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF], that proposes a learning based method for estimating the ICP covariance, which is trained on environments similar to the tested sequence. The results are indicative as the ICP setting of [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF] slightly differs from the setting of [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF] we use;

ini.+ICP : combines initialization and ICP measurements with the covariance estimate (9.12) without considering cross-covariance terms, i.e., applying formulas of [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF] ; [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where the "ellipses" (lines) represent the 95% (3 ) final confidence sets.

proposed : based on the full proposed covariance of the maximum-likelihood estimate (9.12).

We set initial errors as in Section 4 and evaluate the above methods using the Mahalanobis Distance proposed in [START_REF] Landry | CELLO-3D: Estimating the Covariance of ICP in the Real World[END_REF] between final trajectory estimates and ground truth

Mah. dist. = ✓ N X n=0 ξ T n Q 1 n ξ n dim(ξ n )N ◆ 1/2 , ( 9.17)
where ξ n = exp 1 (T 1 true Tn ) is the transformation error and Qn the estimated covariance matrix, averaged over N samples. The target value is one, below one the estimation is pessimistic, and above one the estimates are optimistic.

We average results over 40 different initial trajectories for each sequence, which are numerically displayed in Table 9.2 and illustrated in Figure 9.4. We observe:

• CELLO-3D is the only pessimistic method, which estimates uncertainty ranging from 3 to 10 times higher than actual uncertainty. It evidences how difficult it is to asses ICP uncertainty in practice;

• the proposed approach obtains on average the best results. It obtains similar estimates than ini.+ICP when the ICP algorithm is accurate (Gazebo and Wood).

In more difficult environments, e.g. Stairs, it better incorporates initialization than ini.+ICP thanks to it accounting for measurement correlation encoded in (9.11), see Figure 9.4.

These results confirm the ability of the method to compute covariance estimates over trajectories also and the relevance of correlation terms between ICP and initial estimates.

Role of Initial Uncertainties in Covariance Estimation

We evaluate the influence of Q ini on the covariance estimation in challenging situations where Q ini is high, inaccurately known (the estimation of Q ini is in itself challenging), and sensor noise is inflated. For each environment of [START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], we evaluate the method in 9 situations where initial uncertainty is easy, medium and difficult with respectively 0.1, 0.5, 1 m and 10, 20, 50 deg standard deviation, see [START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF]. In each situation we evaluate the algorithm with different magnitudes for Q ini , hence assessing its robustness to pessimistic and optimisitic parametrization. We finally add white and depth bias Figure 9.5: ICP results (dots) and 95% (3 ) confidence sets (lines) following our method for three levels of initial uncertainty. We see the latter highly influences the ICP registrations and ICP covariance estimations accordingly.

noises on already noisy point clouds with 5 cm standard deviation. Results are given in Table 9.3, and illustrated in Figure 9.5.

• The ICP algorithm obtains unreliable results for large initial uncertainty, see Figure 9.5, whereas it obtains centimetric errors for low levels of initial uncertainty. As anticipated in Section 2.2, significant ICP errors are caused by inaccurate initialization;

• ICP final outputs are agnostic to initialization when a global minimum exists, see e.g. the Gazebo and Wood environments in Table 9.3 for levels of Q ini corresponding to easy and medium scenario. The method obtains correct estimates where the sensor noise terms numerically dominate the estimated covariances, and J ⇡ I, see Section 2.4;

• another environments contain local minima, e.g. Hauptgebaude. Then the algorithm outputs reflect the pessimistic or optimistic belief about the initial uncertainty. We recommend in these situations to set Q ini sufficiently high to favour conservatism;

• our method is able to detect inaccurate ICP registrations by providing very high covariance estimates, although it cannot accurately describe non Gaussian distributions, see Figure 9.5 (a,b). Difficult initial uncertainty leads to highly erroneous ICP outputs that the proposed method detects if correctly parametrized.

Q ini (

Discussion about the Proposed Approach

We finally examine the pros, cons, and fundamental assumptions of the method. The main advantages are: it being anchored in a mathematical theory, its efficiency to assess uncertainty for acceptable levels of initial uncertainties, its simplicity, while being computationally reasonable, see Algorithm 1. The cross-covariance term in (9.12) may be fruitful for increasing robustness of back-end systems, e.g. pose-graph [START_REF] Mendes | ICP-based Pose-graph SLAM[END_REF], as it correlates two previously supposed independent measurements. Comparisons between diagonal terms in (9.12) finally provides a way for trading-off between initial odometry guesses and ICP estimates, see [START_REF] Ovchinnikov | Windowed Multiscan Optimization using Weighted Least Squares for Improving Localization Accuracy of Mobile Robots[END_REF].

The Gaussian error assumption of the ICP estimates is the core hypothesis of the method. We required this assumption to obtain a tractable method able to provide a covariance for a state estimator, e.g. a Kalman filter. However, if one pursues a more accurate estimation of the ICP distribution, we suggest massive sampling methods as an expansive alternative, although our method largely proves sufficient to detect problematic situations.

The method finally requires the covariance of the initial uncertainty as an input. If the provided initialization confidence is inexact, the method outputs may reflect the initial optimism or pessimism in general situations. Nonetheless an insight of the present chapter, see Section 2 and e.g. Figure 9.5, is that ICP errors intrinsically depend on the initial accuracy so that a coarse idea of initial uncertainty is essential, whatever the method one desires to use.

Finally, [START_REF] Bourmaud | Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions[END_REF][START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF][START_REF] Long | The Banana Distribution is Gaussian: A Localization Study with Exponential Coordinates[END_REF] show that concentrated Gaussian distribution (9.1) faithfully describe robot odometry models, such that methods like the Kalman filter are able to provide an accurate (concentrated) Gaussian approximation to the true initial uncertainty for which our approach has been designed.

Conclusion

This chapter presents a novel method for real time estimation of 3D uncertainty covariance matrix of the ICP algorithm. The method relies on a careful study of the influence of both sensor noises and algorithm initialization on the ICP estimates, that we leverage in a deterministic scheme which remains very simple in terms of parameter tuning. The core of our approach is versatile as one can apply it to various choices of error metrics. However with point-to-point ICP the closed form part of the covariance is not valid, see [START_REF] Bonnabel | On the Covariance of ICP-Based Scan-Matching Techniques[END_REF]. The approach is successfully validated on individual pairs of point clouds and over trajectories on challenging real datasets, where it obtains consistent results.

CHAPTER 10 Introduction to Part III

Deep neural networks allows building data-driven models that learn features for realizing a given task, e.g. image classification. Especially since 2014 and its success in computer vision, deep learning [START_REF] Goodfellow | Deep Learning[END_REF] spread among all the subjects of computer sciences, robotics, natural language programming, and others. This fast development comes from 3 enablers:

1. New theories for training and applying deep learning in a wide range of topics; 2. Powerful hardware like graphic card with mass parallelization; 3. Lots of data, with the development of big data and publicly available datasets. This part of the thesis leverages deep learning for inertial navigation and enhancing of more "traditional" state-estimation methods such as the Kalman filter.

Relations between Neural Networks and the Kalman Filter

Both neural networks and Kalman filters may estimate parameters from a stream of data/measurements, see Figure 10.1, and have non-linear transformation that are local linearization of the measurements and propagation error models in the EKF, while neural networks use linear operations between consecutive layers.

In a Kalman filter, the researcher applies a specific domain knowledge, e.g. physics equations, and noise models to estimate the parameters in a way which is optimal as it minimizes the mean squared error under certain assumptions (zero-mean Gaussian noise). Indeed, the Kalman filter computes the posterior of the state given observations through the prediction step and update step. So the propagation f (•) and the observation h(•) functions should be explicit and tractable, which requires great efforts on designing an approximation.

In a neural network the model is not explicitly written, but learned from the measurements, by looking at many examples and by using an appropriate optimization scheme. Indeed, vanilla neural networks ignore whatever happens in the middle and directly compute the mapping between input and output, with a massive parameterized differentiable function. The a priori absence of expert knowledge makes neural networks appears as a dream tool for complex cross-domain tasks.

However, in practice, applying neural networks requires some kind of expertise, see e.g. the success of [START_REF] Qi | PointNet: Deep Learning on Point Sets for 3D Classification and Segmentation[END_REF] for point cloud classification and segmentation. Regarding state estimation, many works combine neural networks and Kalman filters which can be classified in two manners. The first is to use neural networks to learn the model of the Kalman filters, where the neural network optimizes directly the filter estimates, see [START_REF] Krishnan | Deep Kalman Filter[END_REF][START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF][START_REF] Coskun | Long Short-Term Memory Kalman Filters: Recurrent Neural Estimators for Pose Regularization[END_REF] and Chapter 8. The other method is to train a neural network for an auxillary and decoupled task. The network outputs are feed, once training is over, to the Kalman filter.

2 2 Tightly Coupled Approach: Designed "Deep" Kalman Filter with Neural Networks

Recent works such as [START_REF] Krishnan | Deep Kalman Filter[END_REF][START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF] propose two methods to learn Kalman filters using neural networks, where the authors search to learn respectively a generative temporal model f (•) and an observation model h(•) from high-dimensional data, using deep neural networks as a building block, see also [START_REF] Ko | GP-UKF: Unscented Kalman Filters with Gaussian Process Prediction and Observation Models[END_REF] that performs preliminary researches to design model for UKFs. One main advantage is that the Kalman filter pipeline is yet given, and only the models (or parts of these model such as noise covariance matrices) have to be learn. Thus the neural network is trained for directly improving state estimates.

Albeit the authors get promising results and that training is that way seem optimal, see also [START_REF] Chen | DynaNet: Neural Kalman Dynamical Model for Motion Estimation and Prediction[END_REF] for pedestrian navigation, this type of methods is still limited by accuracy, generalization, robustness, data, and computational requirement. This holds for Chapter 8, where adding a new sensor would require to completely relearn the neural networks.

Loosely Coupled Approach: Adding Information to Kalman Filter with Neural Networks

Let us consider the safety features of an autonomous car which is an example to observe how neural networks and Kalman filters are related in practical scenario.

For the autonomous pilot feature to function seamlessly, the vehicle needs to know the type of objects on the road (car, truck or motor cycle) and also it needs to know the distance to the object, its velocity and heading, in order to predict the trajectory of the object and make decisions on how the vehicle should function.

Consider now a simple case of how does an autonomous vehicle detect a car in the surroundings and estimate its position, velocity and heading. The front camera captures the images and sends it to an embedded platform that runs the software to detect what kind of an object are present in the image. This image classifier is a trained neural network that has been trained with thousands of images to classify various objects. i.e. the input to the neural network is an image and output is a classified object such as a car or a truck or a pedestrian. Furthermore, from a series of images it is possible to estimate the distance of the car and its velocity as well, but the estimated distance and velocity are not accurate. In order to get a better accuracy of the estimations, the information from the front looking radar is used, that can estimate the distance to the car, its velocity and its heading as well. Now here comes the Kalman filter, where the filter fuses the information (such as the type of the object, its distance, velocity and heading) from both the camera and the radar in a prediction and update step, to get an accurate estimation of the state of the object, which will then be fed into the decision making unit of the autonomous pilot to make maneuvers accordingly.

In these paradigm, neural networks are trained for another purpose, e.g. modelling f (•) or h(•), but independently from the filter, and then used in the filter. We illustrate this point here with two examples:

• Learning relative displacement from IMU and use them in a MSCKF like filter based on a IMU only [START_REF] Liu | IMU-Based Pedestrian Dead Reckoning with Learned Motion Model[END_REF];

• Learning depth from convolutional neural networks that provides initial depth estimates of a semi-direct visual odometry system [START_REF] Loo | CNN-SVO: Improving the Mapping in Semi-Direct Visual Odometry Using Single-Image Depth Prediction[END_REF].

Both examples successfully train the neural networks, and require manual tweaking to adapt the network output to the filter, e.g. by inflating network output uncertainty. We choose to perform research in this direction, that is, leveraging our expert knowledge in inertial navigation problem to design deep neural network for specific tasks thar are measurement detection in Chapter 11, and measurement denoising in Chapter 12.

Chapter 13 presents a few additional results as the combination of the methods of Chapter 8 and Chapter 11. Finally, we provide in Appendix few tips to practitioners regarding Kalman filtering enhanced by deep learning.

CHAPTER 11 RINS-W: Robust Inertial Navigation System on Wheels

The present chapter has been published in the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

Résumé

Ce chapitre propose une approche en temps réel pour la navigation inertielle à long terme basée uniquement sur une centrale inertielle pour les robots à roues se localisant de manière autonome. L'approche repose sur deux composantes: 1) un détecteur robuste qui utilise des réseaux de neurones profonds récurrents pour détecter dynamiquement une variété de situations d'intérêt, telles qu'une vitesse nulle ou aucun glissement latéral; et 2) un filtre de Kalman qui incorpore ces connaissances en tant que pseudo-mesures pour la localisation. Les évaluations faites sur un jeu de données provenant d'une voiture et publiquement accessible démontrent que le schéma proposé peut atteindre une précision finale de 20 m pour une trajectoire de 21 km de long d'un véhicule roulant pendant plus d'une heure, équipé d'une centrale inertielle de précision modérée (le taux de dérive du gyroscope est de 10 degrés par heure). Ce chapitre combine des techniques d'apprentissage approfondies sophistiquées avec des méthodes de filtrage de pointe pour la navigation inertielle pure sur véhicules à roues et, à ce titre, ouvre la voie à de nouvelles techniques de navigation inertielle basées sur les approaches d'apprentissage.

Chapter abstract

This chapter proposes a real-time approach for odometry inertial navigation based only on an IMU for self-localizing wheeled robots. The approach builds upon two components: 1) a robust detector that uses recurrent deep neural networks to dynamically detect a variety of situations of interest, such as zero velocity or no lateral slip; and 2) a state-of-the-art Kalman filter which incorporates this knowledge as pseudomeasurements for localization. Evaluations on a publicly available car dataset demonstrates that the proposed scheme may achieve a final precision of 20 m for a 21 km long trajectory of a vehicle driving for over an hour, equipped with an IMU of moderate precision (the gyro drift rate is 10 deg/h). This chapter combines sophisticated deep learning techniques with state-of-the-art filtering methods for pure inertial navigation on wheeled vehicles and as such opens up for novel data-driven inertial navigation techniques.

Introduction

In this chapter, we provide a method for odometry localization for wheeled robots only using an IMU. Our contributions, and the chapter's organization, are as follows:

• we introduce specific motion profiles frequently encountered by a wheeled vehicle which bring information about the motion when correctly identified, along with their mathematical description in Section 3;

• we design an algorithm which automatically detects in real time if any of those assumptions about the motion holds in Section 4.1, based only on the IMU measurements. The detector builds upon recurrent deep neural networks [START_REF] Goodfellow | Deep Learning[END_REF] and is trained using IMU and ground truth data;

• we implement a state-of-the-art invariant extended Kalman filter [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF][START_REF] Barrau | Invariant Kalman Filtering[END_REF] that exploits the detector's outputs as pseudo-measurements to combine them with the IMU outputs in a statistical way, in Section 4.2. It yields accurate estimates of pose, velocity and sensor biases, along with associated uncertainty (covariance);

• we demonstrate the performances of the approach on a publicly available car dataset [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF] in Section 5. Our approach solely based on the IMU produces accurate estimates with a final distance w.r.t. ground truth of 20 m on the 73 minutes test sequence urban16, see Figure 11.1. In particular, our approach outperforms differential wheel odometry, as well as wheel odometry aided by an expensive fiber optics gyro whose drift is 200 times smaller than the one of the IMU we use;

• this evidences that accurately detecting some specific patterns in the IMU data and incorporating this information in a filter may prove very fruitful for localization;

• the method is not restricted to IMU only based navigation. It is possible to feed the Kalman filter with other sensors' measurements. Besides, once trained the detector may be used as a building block in any localization algorithm.

Inertial Navigation System & Sensor Model

Denoting the IMU orientation by R n 2 SO(3), i.e. the rotation matrix that maps the body frame to the world frame w, its velocity in w by v imu n 2 R 3 and its position in w by p imu n 2 R 3 , the dynamics are as follows

R imu n+1 = R imu n exp SO(3) (ω n dt) , (11.1)
v imu n+1 = v imu n + (R n a n + g) dt, (11.2) p imu n+1 = p imu n + v imu n dt, ( 11.3)
between two discrete times, with sampling time dt, and where

⇣ R imu 0 , v imu 0 , p imu 0 ⌘
is the initial configuration. The true angular velocity ω n 2 R 3 and the true specific acceleration a n 2 R 3 are the inputs of the system (11.1)- (11.3). In our application scenarios, the effects of earth rotation and Coriolis acceleration are ignored, and earth is considered flat. Trajectory ground truth and estimates obtained by various methods: integration of IMU signals; odometry based on a differential wheel encoder system; odometry combined with an highly accurate and expensive Fiber optics Gyro (FoG) that provides orientation estimates; and the proposed RINS-W approach which considers only the IMU sensor embarked in the vehicle, and which outperforms the other schemes. The final distance error for this long-term sequence urban16 (73 minutes) of the car dataset [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF] is 20 m for the RINS-W solution. The deep learning based detector (see Section 4.1) has of course not been trained or cross-validated on this sequence.

Inertial Measurement Unit (IMU) Model

The IMU provides noisy and biased measurements of ω n 2 R 3 and a n 2 R 3 as follows

ω imu n = ω n + b ω n + w ω n , (11.4) a imu n = a n + b a n + w a n , (11.5)
where b ω n , b a n are quasi-constant biases and w ω n , w a n are zero-mean Gaussian noises. The biases follow a random-walk

b ω n+1 = b ω n + w b ! n , (11.6) b a n+1 = b a n + w b a n , (11.7)
where

w b ω n , w b a
n are zero-mean Gaussian noises. All sources of error -particularly biases -are yet undesirable since a simple implementation of (11.1)- (11.3) leads to a triple integration of raw data, which is much more harmful that the unique integration of differential wheel speeds. Even a small error may thus cause the position estimate to be way off the true position, within seconds.

Specific Motion Profiles for Wheeled Systems

We describe in this section characteristic motions frequently encountered by a wheeled robot, that provide useful complementary information to the IMU when correctly detected.

Considered Motion Profiles

We consider 4 distinct specific motion profiles, whose validity is encoded in the following binary vector:

z n = (z vel n ,z ang n ,z lat n ,z up n ) 2{0, 1} 4 . ( 11.8)
• Zero velocity: the velocity of the platform is null. As a consequence so is the linear acceleration, yielding:

z vel n =1) ( v imu n = 0 R imu n a n + g = 0 . ( 11.9)
Such profiles frequently occur for vehicles moving in urban environments, and are leveraged in the well known Zero velocity UPdaTe (ZUPT), see e.g. [START_REF] Ramanandan | Inertial Navigation Aiding by Stationary Updates[END_REF][START_REF] Skog | Zero-Velocity Detection-An Algorithm Evaluation[END_REF].

• Zero angular velocity: the heading is constant:

z ang n =1) ω n = 0. (11.10)
• Zero lateral velocity 1 : the lateral velocity is considered roughly null (which is not always true)

z lat n =1) v lat n ' 0, (11.11)
where we obtain the lateral velocity v lat n after expressing the velocity in the body frame b as .

v b n =(R imu n ) T v imu n = 2
(11.12)

1 Without loss of generality, we assume that the body frame is aligned with the IMU frame. Real IMU data of a car stopping from sequence urban06 of [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF]. We see (11.9) holds and thus z vel n =1att =5.8s while (11.10) does not hold yet.

• Zero vertical velocity: the vertical velocity is considered roughly null

z up n =1) v up n ' 0. ( 11.13)
The two latter are common assumptions for wheeled robots or cars moving forward on human made roads.

Discussion on the Choice of Profiles

The motion profiles were carefully chosen in order to match the specificity of wheeled robots equipped with shock absorbers. Indeed, as illustrated on Figure 11.2, when the wheels of a car actually stop, the car undergoes a rotational motion forward and then backward. As a result, null velocity (11.9) does not imply null angular velocity (11.10).

For the level of precision we pursue in the present chapter, distinguishing between (11.9) and (11.10) is pivotal since it allows us to: i) properly label motion profiles before training (see Section 5.2, where we have different thresholds on position and on angular velocity); and: ii) improve detection accuracy since only one motion pattern can be identified as valid. (11.11) and (11.13) generally hold for robots moving indoors or cars on roads. Note that (11.13) is expressed in the body frame, and thus generally holds for a car moving on a road even if the road is not level. As such (11.13) is more refined that just assuming the car is moving in a 2D horizontal plane. And it is actually quite a challenge for an IMU-based detector to identify when (11.11) and (11.13) are not valid.

Expected Impacts on Robot Localization

The motion profiles fall into two categories in terms of the information they bring:

1. Zero velocity constraints: the profiles (11.9)-(11.10), when correctly detected may allow to correct the IMU biases, the pitch and the roll.

2. Vehicle motion constraints: the profiles (11.11) and (11.13) are useful constraints for the estimates accuracy over the long term. In particular, Section 5.5 will experimentally demonstrate the benefits of accounting for (11.11) and (11.13).

Proposed RINS-W Algorithm

This section describes our system for recovering trajectory and sensor bias estimates from an IMU. Figure 11.3 illustrates the approach which consists of two main blocks summarized as follow:

• the detector estimates the binary vector z n from raw IMU signals, and consists of recurrent neural networks;

• the filter integrates the IMU measurements in its dynamic model and exploits the detected motion profiles as pseudo-measurements to refine its estimates, as customary in inertial navigation, see e.g. [START_REF] Ramanandan | Inertial Navigation Aiding by Stationary Updates[END_REF].

The detector does not use the filter's output and is based only on IMU measurements. Thus the detector operates autonomously and is trained independently, see Section 5.2. Note that our approach is different from more tightly coupled approaches such as [START_REF] Haarnoja | Backprop KF: Learning Discriminative Deterministic State Estimators[END_REF]. We now describe each block.

The Specific Motion Profile Detector

The detector determines at each instant n which ones of the specific motion profiles (11.8) are valid, see Figure 11.4. The base core of the detector is a recurrent neural network, namely a Long-Short Term Memory (LSTM) [START_REF] Goodfellow | Deep Learning[END_REF]. The LSTMs take as input the IMU measurements and compute:

ûn+1 , h n+1 = LSTM ⇣n ω imu i , a imu i o n i=0 ⌘ (11.14) = LSTM (ω imu n , a imu n , h n ) , (11.15)
where ûn+1 2 R 4 contains probability scores for each motion profiles and h n is the hidden state of the neural network. Probability scores are then converted to a binary vector ẑn = Threshold ( ûn+1 ) with a threshold for each motion profile. The thresholds must be set with care, and the procedure will be described in Section 5.1. Indeed, false alarms lead to degraded performance, since a zero velocity assumption is incompatible with an actual motion. On the other hand, a missed profile is not harmful since it results in standard IMU based dead reckoning using (11.1)- (11.3).

The Invariant Extended Kalman Filter

We opt for an IEKF, see Chapter 2, to perform the fusion between the IMU measurements and the detected specific motion profiles. The IEKF outputs the state χn that consists of pose and velocity of the IMU, the IMU biases, along with their covariance. We now describe the filter more in detail, whose architecture is displayed on Figure 11.5.

IMU state: we define the IMU state as

χ n = (R imu n , v imu n , p imu n , b ω n , b a n ) , (11.16)
which contains robot pose, velocity, and the IMU biases. The state evolution is given by the dynamics (11.1)-(11.7), see Section 2. As (11.9)-(11.13) are all measurements expressed in the robot's frame, they lend themselves to the Right IEKF methodology, see [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF]. Applying it, we define the linearized state error as

e n = " ξ n e b n # ⇠N(0, P n ) , (11.17)
where uncertainty is based on the use of the Lie exponential as advocated in [START_REF] Barfoot | Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems[END_REF] in a wheel odometry context, and mapped to the state as

χ n = exp SE 2 (3) (ξ n ) χn , (11.18)
b n = bn + e b n , (11.19) where χ n 2 SE 2 (3) is a matrix that lives is the Lie group SE 2 (3) and represents the vehi-

cle state R n , v imu n , p imu n , P n 2 R 15⇥15 is the error state covariance matrix, b n = (b ω n , b a n ) 2 R 6 , bn = ⇣ bω n , ba n ⌘ 2 R 6
, and ( •) denote estimated variables.

Propagation

Step: if no specific motion is detected, i.e. ẑvel n+1 = 0, ẑang n+1 = 0, we apply (11.1)-(11.7) to propagate the state and obtain χn+1 and associated covariance through the Riccati equation

P n+1 = F n P n F T n + G n Q n G T n , (11.20)
where the Jacobians F n , G n are given in Appendix 6, and where Q n denotes the covariance matrix of the noise

w n = ⇣ w ω n , w a n , w b ω n , w b a n ⌘ ⇠N(0, Q n ).
By contrast, if a specific motion profile is detected, we modify model (11.1)-(11.7) as follows:

ẑvel n+1 =1) ( v imu n+1 = v imu n p imu n+1 = p imu n , (11.21)
ẑang n+1 =1) R imu n+1 = R imu n , (11.22)
and the estimated state χn+1 and covariance P n+1 are modified accordingly.

Update: each motion profile yields one of the following pseudo-measurements:

y vel n+1 = " (R imu n+1 ) T v imu n+1 b a n+1 (R imu n+1 ) T g # = " 0 a imu n # , (11.23)
y ang n+1 = b ω n+1 = ω imu n , (11.24)
y lat n+1 = v lat n+1 =0, (11.25)
y up n+1 = v up n+1 =0. (11.26)
A vector y n+1 is computed by stacking the pseudo-measurements of the detected motion profiles. Note that, if ẑvel n+1 = 1 we do not consider (11.25)-(11.26) since (11.23) implies (11.25)- (11.26). If no specific motion is detected, the update step is skipped, otherwise we follow the IEKF methodology [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF] and compute

K = P n+1 H T n+1 / ⇣ H n+1 P n+1 H T n+1 + N n+1 ⌘ , (11.27)
e + = K (y n+1 ŷn+1 ) = " ξ + e b+ # , (11.28)
χ+ n+1 = exp (ξ + ) χn+1 , b + n+1 = b n+1 + e b+ , (11.29)
P + n+1 = (I KH n+1 ) P n+1 , (11.30)
summarized as Kalman gain (11.27), state innovation (11.28), state update (11.29) and covariance update (11.30), where H n+1 is the measurement Jacobian matrix given in Appendix 6 and N n+1 the noise measurement covariance.

Initialization: launching the system when the platform is moving without estimating biases and orientation can induce drift at the beginning which then is impossible to compensate. As the filter is able to correctly self-initialize the biases, pitch, roll and its covariance matrix when the trajectory is first stationary, we enforce each sequence in Section 5 to start by a minimum of 1 s stop. Admittedly restrictive, the required stop is of extremely short duration, especially as compared to standard calibration techniques [201]. The considered dataset [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF] contains data logs of a Xsens MTi-3002 (right) recorded at 100 Hz along with the ground truth pose.

Results on Car Dataset

The following results are obtained on the complex urban LiDAR dataset [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF], that consists of data recorded on a consumer car moving in complex urban environments, e.g. metropolitan areas, large building complexes and underground parking lots, see Figure 11.6. Our goal is to show that using an IMU of moderate cost, one manages to obtain surprisingly accurate dead reckoning by using state-of-the-art machine learning techniques to detect relevant assumptions that can be fed into a state-of-the-art Kalman filter. The detector is trained on a series of sequences and tested on another sequences, but all sequences involve the same car and inertial sensors.

Implementation Details

We provide in this section the detector and filter setting of the RINS-W system. The detector disposes of four LSTMs, one for each motion profile. Each LSTM consists of 2 layers of 250 hidden units and its hidden state is mapped to a score probability by a 2 layers multi-perceptron network with a ReLU activation function and is followed by a sigmoid function [START_REF] Goodfellow | Deep Learning[END_REF] that outputs a scalar value in the range [0, 1]. We implement the detector on PyTorch3 and set the threshold values to 0.95 for (z vel n , z ang n ), and 0.5 for (z lat n , z up n ). The filter operates at the 100 Hz IMU rate (dt = 10 2 s) and its noise covariance matrices are parameterized as

Q n = diag ⇣ 2 ω I, 2 a I, 2 b ω I, 2 b a I ⌘ , (11.31)
N n = diag ⇣ 2 vel,v I,

Detector Training

The detector is trained with the sequences urban06 to urban14, that represents 100 km of training data (sequences urban00 to urban05 does not have acceleration data). For each sequence, we compute ground truth position velocity v imu n and angular velocity ω n after differentiating the ground pose and applying smoothing. We then compute the ground-truth z n by applying a small threshold on the ground truth velocities, e.g. The detector is trained during 500 epochs with the ADAM optimizer [START_REF] Kingma | ADAM: A Method for Stochastic Optimization[END_REF], whose learning rate is initializing at 10 3 and managed by a learning rate scheduler. Regularization is enforced with dropout layer, where p =0.4 is the probability of any element to be zero. We use the binary cross entropy loss since we have four binary classification problems. For each epoch, we organize data as a batch of 2 min sequences, where we randomly set the start instant of each sequence, and constraints each starting sequence to be a stop of at minimum 1 s. Training the full detector takes less than one day with a GTX 1080 GPU.

Evaluation Metrics

To assess performances we consider three error metrics:

Mean Absolute Trajectory Error (m-ATE): which averages the planar translation error of estimated poses with respect to a ground truth trajectory and is less sensitive to single poor estimates than root mean square error;

Mean Absolute Aligned Trajectory Error (aligned m-ATE): that first aligns the estimated trajectory with the ground truth and then computes the mean absolute trajectory error. This metric evaluates the consistency of the trajectory estimates;

Final distance error: which is the final distance between the un-aligned estimates and the ground truth.

Trajectory Results

After training the detector on sequences urban06 to urban14, we evaluate the approach on test sequences urban15 to urban17, that represent 40 km of evaluation data. We compare 4 methods:

• IMU: the direct integration of the IMU measurements based on (11.1)-(11.7), that is, pure inertial navigation; [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF]. RINS-W obtains results comparable with FoG-based odometry.

• Odometry: the integration of a differential wheel encoder which computes linear and angular velocities;

• RINS-W (ours): the proposed approach, that uses only the IMU signals and involves no other sensor.

• Odometry + FoG: the integration of a differential wheel encoder which computes only linear velocity. The angular velocity is obtained after integrating the increments of an highly accurate and costly KVH DSP-1760 4 Fiber optics Gyro (FoG). The FoG gyro bias stability (0.05 deg /h) is 200 times smaller than the gyro stability of the IMU used by RINS-W;

We delay each sequence such that the trajectory starts with a 1 s stop to initialize the orientation and the biases, see Section 4.2. Bias initialization is also performed for IMU pure integration and the FoG. Optimized parameters for wheel speeds sensors calibration are provided by the dataset.

Experimental results in terms of error with respect to ground truth are displayed in Table 11.1, and illustrated on Figures 11.1,11.7,and 11.8. Results demonstrate that:

• directly integrating IMU leads to divergence at the first turn, even after a careful calibration procedure;

• wheel-based differential odometry accurately estimates the linear velocity but has troubles estimating the yaw during sharp bends, even if the dataset has been obtained in an urban environment and the odometry parameters are calibrated. This drawback may be remedied at the expense of using an additional high-cost gyroscope; [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF]. RINS-W clearly outperforms the odometry and even the odometry + FoG solution. We note that RINS-W accurately follows the interchange road located at (x =2,y = 1).

• the proposed scheme completely outperforms wheel encoders, albeit in urban environment. More surprisingly, our approach competes with the combination of wheel speed sensors + (200 hundred times more accurate) Fyber optics Gyro, and even outperforms it on average. Furthermore, although comparisons were performed in 2D environments our method yields the full 3D pose of the robot, and as such is compatible with non planar environments.

Discussion

The performances of RINS-W can be explained by: i) a false-alarm free detector; ii) the fact incorporating side information into IMU integration obviously yields better results; and iii) the use of IEKF that has been proved to be well suited for localization tasks.

We also emphasize the importance of (11.11) and (11.13) in the procedure, i.e. applying (11.25)- (11.26). For illustration, we consider sequence urban07 of [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF], where the vehicle moves during 7 minutes without stop so that ZUPT may not be used. We implement the detector trained on the first 6 sequences, and compare the proposed RINS-W to a RINS-W which does not use updates (11.25)- (11.26) when detected, see Figure 11.9. Clearly, the reduced RINS-W diverges at the first turn whereas the full RINS-W is accurate along all the trajectory and obtains a final distance w.r.t. ground truth of 5 m. In contrast, odometry + FoG achieves 16 m.

Detector Evaluation

In Section 5.5 we mentioned three possible reasons explaining the performances of RINS-W, but could not assess what is owed to each. To assess the detector's performance, and to demonstrate the interest of our powerful deep neural network based approach (see Section 4.1) we focus on the zero velocity detection (11.9), and compare the detector with the Acceleration-Moving Variance Detector (AMVD) [START_REF] Skog | Zero-Velocity Detection-An Algorithm Evaluation[END_REF] on the test sequences urban15-17, which represent 64.10 4 measurements. The AMVD computes the accelerometer variance over a sample window W = 10 2 and assumes the vehicle is stationary if the variance falls below a threshold = 10 3 . To make sure AMVD performs at its best, the parameters W and are optimized by grid search on the test sequences. Results are shown in Table 11.2 and demonstrate the detector is more accurate than this "ideal" AMVD.

Conclusion

This chapter proposes a novel approach for robust inertial navigation for wheeled robots. Our approach exploits deep neural networks to identify specific patterns in wheeled vehicle motions and incorporates this knowledge in IMU integration for localization. The entire algorithm is fed with IMU signals only, and requires no other sensor. The method leads to surprisingly accurate results, and opens new perspectives for combination of data-driven methods with well established methods for autonomous systems. (11.9) detection obtained by an ideal AMVD [START_REF] Skog | Zero-Velocity Detection-An Algorithm Evaluation[END_REF] and the proposed detector on test sequences urban15-17. The proposed detector systematically obtains better results and precision. This is remarkable because the detector is not trained on those sequences, whereas AMVD parameters were optimized on the considered test sequences.

Appendix

Following the Right IEKF of [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF], the Jacobians required for the computation of the filter propagation (11.20) are given as dt, (11.34) when ẑvel n = 0 and ẑang n = 0. Otherwise, we set the appropriate rows to zero in F n and G n , i.e.:

F n = I + 2
R imu n 00 0 (v imu n ) ⇥ R imu n R imu n 00 (p imu n ) ⇥ R imu
• if ẑvel n = 1 we set the 4 to 9 rows of the right part of F n in (11.33) and of G n to zero.

• if ẑang n = 1 we set the 3 first rows of the right part of F n in (11.33) and of G n to zero.

Once again following [START_REF] Barrau | The Invariant Extended Kalman Filter as a Stable Observer[END_REF], the measurement Jacobians used in the filter update (11.27)- (11.30) are given as

H vel n = " 0 (R imu n ) T 00 0 (R imu n ) T g ⇥ 00 0 I # , ( 11.35)
H ang n = h 000 I0 i , ( 11.36)
and we obtains H lat n and H up n as respectively the second and third row of H vel n .

CHAPTER 12

Denoising IMU Gyroscopes with Deep Learning for Open-Loop Attitude Estimation

The present chapter has been accepted in IEEE Robotics and Automation Letters. https://github.com/mbrossar/denoise-imu-gyro.

Résumé

Chapter abstract

This chapter proposes a learning method for denoising gyroscopes of IMUs using ground truth data, to estimate in real time the orientation (attitude) of a robot in dead reckoning. The obtained algorithm outperforms the state-of-the-art on the (unseen) test sequences. The obtained performances are achieved thanks to a well chosen model, a proper loss function for orientation increments, and through the identification of key points when training with high-frequency inertial data. Our approach builds upon a neural network based on dilated convolutions, without requiring any recurrent neural network. We demonstrate how efficient our strategy is for 3D attitude estimation on the EuRoC and TUM-VI datasets. Interestingly, we observe our dead reckoning algorithm manages to beat top-ranked visual-inertial odometry systems in terms of attitude estimation although it does not use vision sensors. Our open-source implementation is available at: https://github.com/mbrossar/denoise-imu-gyro. We then perform open-loop time integration on the noise-free measurements for regressing low frequency errors between ground truth and estimated orientation increments.

Introduction

IMUs allow estimating a robot's trajectory relative to its starting position, a task called odometry [START_REF] Scaramuzza | Visual-Inertial Odometry of Aerial Robots[END_REF]. Small and cheap IMUs are ubiquitous in smartphones, industrial and robotics applications, but suffer from difficulties to estimate sources of error, such as axis-misalignment, scale factors and time-varying offsets [START_REF] Rehder | Extending Kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes[END_REF][START_REF] Rohac | Calibration of Low-cost Triaxial Inertial Sensors[END_REF]. Hence, IMU signals are not only noisy, but they are biased. In the present chapter, we propose to leverage deep learning for denoising the gyroscopes (gyros) of an IMU, that is, reduce noise and biases. As a byproduct, we obtain accurate attitude (i.e. orientation) estimates simply by open-loop integration of the obtained noise-free gyro measurements.

1.1 1.1 Links and Differences with Existing Literature

IMUs are generally coupled with complementary sensors to obtain robust pose estimates in sensor-fusion systems. To obtain accurate pose estimates, a proper IMU calibration is required, see e.g. the widely used Kalibr library [START_REF] Rehder | Extending Kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes[END_REF][START_REF] Furgale | Unified Temporal and Spatial Calibration for Multi-Sensor Systems[END_REF], which computes offline the underlying IMU intrinsic and extrinsic calibration parameters. Our approach, which is recapped in Figure 12.1, is applicable to any system equipped with an IMU. It estimates offline the IMU calibration parameters and extends methods such as [START_REF] Rehder | Extending Kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes[END_REF][START_REF] Furgale | Unified Temporal and Spatial Calibration for Multi-Sensor Systems[END_REF] to time-varying and difficult to model signal corrections. Machine learning (more specifically deep learning) has been recently leveraged to perform LiDAR, visual-inertial, and purely inertial localization, where methods are divided into supervised [START_REF] Yan | RIDI: Robust IMU Double Integration[END_REF][START_REF] Chen | IONet: Learning to Cure the Curse of Drift in Inertial Odometry[END_REF][START_REF] Clark | VINet: Visual-Inertial Odometry as a Sequence-to-Sequence Learning Problem[END_REF][START_REF] Esfahani | OriNet: Robust 3-D Orientation Estimation With a Single Particular IMU[END_REF] and unsupervised [START_REF] Almalioglu | SelfVIO: Self-Supervised Deep Monocular Visual-Inertial Odometry and Depth Estimation[END_REF]. Most works extract relevant features in the sensors' signals which are propagated in time through recurrent neural networks, whereas [START_REF] Yan | RoNIN: Robust Neural Inertial Navigation in the Wild: Benchmark, Evaluations, and New Methods[END_REF] proposes convolutional neural networks for pedestrian inertial navigation. A related approach [START_REF] Nobre | Learning to Calibrate: Reinforcement Learning for Guided Calibration of Visual-Inertial Rigs[END_REF] applies reinforcement learning for guiding the user to properly calibrate visual-inertial rigs. Our method is supervised (we require ground truth poses for training), leverages convolutions rather than recurrent architectures, and outperforms the latter approach. We obtain significantly better results while requiring considerably less data and less time. Finally, the reference [START_REF] Esfahani | OriNet: Robust 3-D Orientation Estimation With a Single Particular IMU[END_REF] estimates orientation with an IMU and recurrent neural networks, but our approach proves simpler.

Contributions

Our main contributions are as follows:

• detailed modelling of the problem of learning orientation increments from lowcost IMUs;

• the convolutional neural network which regresses gyro corrections and whose features are carefully selected;

• the training procedure which involves a trackable loss function for estimating relative orientation increments;

• the approach evaluation on datasets acquired by a drone and a hand-held device [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF][START_REF] Schubert | The TUM VI Benchmark for Evaluating Visual-Inertial Odometry[END_REF], where our method outperforms [START_REF] Esfahani | OriNet: Robust 3-D Orientation Estimation With a Single Particular IMU[END_REF] and competes with VIO methods [START_REF] Qin | VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[END_REF][START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF] although it does not use vision;

• perspectives towards more efficient VIO and IMU based learning methods;

• a publicly available open-sourced code, where training is done in 5 minutes per dataset.

Kinematic & Low-Cost IMU Models

We detail in this section our model.

Kinematic Model based on Orientation Increments

The 3D orientation of a rigid platform is obtained by integrating orientation increments, that is, gyro outputs of an IMU, through

R n = R n 1 exp (ω n dt) , ( 12.1)
where the rotation matrix R n 2 SO(3) at timestamp n maps the IMU frame to the global frame, the angular velocity ω n 2 R 3 is averaged during dt, and with exp(•) the SO(3) exponential map. The model (12.1) successively integrates in open-loop ω n and propagates estimation errors. Indeed, let Rn denotes an estimate of R n . The error present in Rn 1 is propagated in Rn through (12.1).

Low-Cost Inertial Measurement Unit (IMU) Sensor Model

The IMU provides noisy and biased measurements of angular rate ω n and specific acceleration a n at high frequency (200 Hz in our experiments) as, see [START_REF] Rehder | Extending Kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes[END_REF][START_REF] Rohac | Calibration of Low-cost Triaxial Inertial Sensors[END_REF],

u imu n = " ω imu n a imu n # = C " ω n a n # + b n + w n , ( 12.2)
where b n 2 R 6 are quasi-constant biases, w n 2 R 6 are commonly assumed zero-mean white Gaussian noises, and

a n = R T n 1 ((v n v n 1 ) /dt g) 2 R 3 (12.3)
is the acceleration in the IMU frame without the effects of gravity g, with v n 2 R 3 the IMU velocity expressed in the global frame. The intrinsic calibration matrix (12.4) contains the information for correcting signals: axis misalignments (matrices M ω ⇡ I 3 , M a ⇡ I 3 ); scale factors (diagonal matrices S ω ⇡ I 3 , S a ⇡ I 3 ); and linear accelerations on gyro measurements, a.k.a. g-sensitivity (matrix A ⇡ 0 3⇥3 ). Remaining intrinsic parameters, e.g. level-arm between gyro and accelerometer, can be found in [START_REF] Rehder | Extending Kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes[END_REF][START_REF] Rohac | Calibration of Low-cost Triaxial Inertial Sensors[END_REF]. We now make three remarks regarding (12.1)-(12.4):

C = " S ω M ω A 0 3⇥3 S a M a # ⇡ I 6
1. equations (12.2)-(12.4) represent a model that approximates reality. Indeed, calibration parameters C and biases b n should both depend on time as they vary with temperature and stress [START_REF] Kok | Using Inertial Sensors for Position and Orientation Estimation[END_REF][START_REF] Rohac | Calibration of Low-cost Triaxial Inertial Sensors[END_REF], but are difficult to estimate in real-time. Then, vibrations and platform excitations due to, e.g., rotors make Gaussian noise w n colored in practice [START_REF] Lu | IMU-Based Attitude Estimation in the Presence of Narrow-Band Noise[END_REF], albeit commonly assumed white;

2. substituting actual measurements ω imu n in place of true value ω n in (12.1) leads generally to quick drift (in a few seconds) and poor orientation estimates; 3. in terms of method evaluation, one should always compare the learning method with respect to results obtained with a properly calibrated IMU as a proper estimation of the parameters C and b n in (12.2) actually leads to surprisingly precise results, see Section 4.

Learning Method for Denoising the IMU

We describe in this section our approach for regression of noise-free gyro increments ωn in (12.2) in order to obtain accurate orientation estimates by integration of ωn in (12.1). Our goal thus boils down to estimating b n , w n , and correcting the misknown C.

Proposed Gyro Correction Model

Leveraging the analysis of Section 2, we compute the noise-free increments as ωn = Ĉω ω imu n + ωn , (12.5) with Ĉω = Ŝω Mω 2 R 3⇥3 the intrinsic parameters that account for gyro axis-misalignment and scale factors, and where the gyro bias is included in the gyro correction ωn . Explicitly considering the small accelerometer influence A, see (12.2)-(12.4), does not affect the results so it is ignored. We now search to compute ωn and Ĉω . The neural network described in Section 3.2 computes ωn by leveraging information present in a past local window of size N around ω imu n . In contrast, we let Ĉω be static parameters initialized at I 3 and optimized during training since each considered dataset uses one IMU. The learning problem involving a time varying Ĉω and/or multiple IMUs is let for future works.

The consequences of opting for the simple model (12.5) and the proposed network structure are as follows. First, the corrected gyro may be initialized on the original gyro, i.e. ωn ⇡ ω imu n with Ĉω = I 3 and ωn ⇡ 0 3 before training. This way, the method improves the estimates as soon as the first training epoch. Then, our method is intrinsically robust to overfitting as measurements outside the local windows, i.e. whose timestamps are less than n N or greater than n, see Figure 12.2, do not participate in infering ωn . This allows us to train the method with 8 or less minutes of data, see Section 4.1. [START_REF] Ioffe | Batch Normalization: Accelerating Deep Network Training by Reducing Internal Covariate Shift[END_REF] and a smooth GELU activation function [START_REF] Ramachandran | Searching for Activation Functions[END_REF] (magenta arrows). The adopted structure defines the window size N = max (kernel dim. ⇥ dilation gap) = 7 ⇥ 64 = 448, corresponding to 2.24 s of past information, and has been found after trial-and-error on datasets [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF][START_REF] Schubert | The TUM VI Benchmark for Evaluating Visual-Inertial Odometry[END_REF].

C N N C N N C N N C N N C N N u IMU n-N . . . u IMU

Dilated Convolutional Neural Network Structure

We define here the neural network structure which infers the gyro correction as

ωn = f (u imu n N ,...,u imu n ), (12.6)
where f (•) is the function defined by the neural network. The network should extract information at temporal multi-scales and compute smooth corrections. Note that, the input of the network consists of IMU data, that is, gyros naturally, but also accelerometers signals. Indeed, from (12.3), if the velocity varies slowly between successive increments we have

a n+1 a n ⇡ (R n R n 1 ) T g ⇡ (exp ( ω n dt) I 3 )R T n 1 g, (12.7)
which also provides information about angular velocity. We leverage dilated convolutions that infer a correction based on a local window of N = 448 previous measurements, which represents 2.24 s of information before timestamp n in our experiments. Dilated convolutions are a method based on convolutions applied to input with defined dilatation gap, see [START_REF] Yu | Multi-Scale Context Aggregation by Dilated Convolutions[END_REF], which: i) supports exponential expansion of the receptive field, i.e., N , without loss of resolution or coverage; ii) is computationally efficient with few memory consumption; and iii) maintains the temporal ordering of data. We thus expect the network to detect and correct various features such as rotor vibrations that are not modeled in (12.2). Our configuration given in Figure 12.2 requires learning 77 052 parameters, which is extremely low and contrasts with recent (visual-)inertial learning methods, see e.g. [START_REF] Almalioglu | SelfVIO: Self-Supervised Deep Monocular Visual-Inertial Odometry and Depth Estimation[END_REF] Figure 2, where IMU processing only requires more than 2 600 000 parameters.

Loss Function based on Integrated Gyro Increments

Defining a loss function directly based on errors ω n ωn requires having a ground truth ω n at IMU frequency (200 Hz), which is not feasible in practice as the best tracking ) by viewing the orientation integration (12.8) as a tree of matrix multiplications. Computation for length j requires log 2 (j) "batch" operations as denoted by the blue vertical arrow on the left. We see we need 3 batches of parallel operations for j = 8 on the chart above. In the same way, we only need 5 operations for j = 32.

systems are accurate at 20-120 Hz. In place, we suggest defining a loss based on the following integrated increments

R i,i+j = R T i R i+j = i+j 1 Y k=i exp (ω k ) , ( 12.8)
i.e., where the IMU frequency is reduced by a factor j. We then compute the loss for a given j as

L j = X i ⇢ ⇣ log ⇣ R i,i+j RT i,i+j ⌘⌘ , ( 12.9)
where log(•) is the SO(3) logarithm map, and ⇢(•) is the Huber loss function. We set in our experiments the Huber loss parameter to 0.005, and define our loss function as

L = L 16 + L 32 .
(12.10)

The motivations for (12.9)-(12.10) are as follows:

• the choice of Huber loss ⇢(•) yields robustness to ground truth outliers;

• (12.8) is invariant to rotations which suits well IMUs, whose gyro and accelerometer measurements are respectively invariant to rotations and yaw changes [START_REF] Scaramuzza | Visual-Inertial Odometry of Aerial Robots[END_REF][START_REF] Zhang | A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry[END_REF], i.e., left shifting R n RR n and Rn R Rn with R n 2 SO(3) leaves (12.9) unchanged;

• the choice of (12.10) corresponds to error increments at 200/16 ⇡ 12 Hz and 200/32 ⇡ 6 Hz, which is barely slower than ground truth. Setting too high a j, or in the extreme case using a loss based on the overall orientation error R T n Rn , would make the algorithm prone to overfitting, and hence makes the method too sensitive to specific trajectory patterns of training data.

3.4 3.4 Efficient Computation of (12.8)- (12.10) First, note that thanks to parallelization applying e.g., exp(•), to one or parallelly to many ωn takes similar execution time on a GPU (we found experimentally that one operation takes 5 ms whereas 10 million operations in parallel take 70 ms, that is, the time per operation drops to 7 ns). We call an operation that is parallelly applied to many instances a batch operation. That said, an apparent drawback of (12.8) is to require j matrix multiplications, i.e. j operations. However, first, we may compute ground truth R i,i+j only once, store it, and then we only need to compute Ri,i+j multiple times. Second, by viewing (12.8) as a tree of matrix multiplications, see Figure 12.3, we reduce the computation to log 2 (j) batch GPU operations only. We finally apply subsampling and take one i every j timestamps to avoid counting multiple times the same increment. Training speed is thus increased by a factor 32/ log 2 (32) ⇡ 6.

Training with Data Augmentation

Data augmentation is a way to significantly increase the diversity of data available for training without actually collecting new data, to avoid overfitting. This may be done for the IMU model of Section 2 by adding Gaussian noise w n , adding static bias b n , uncalibrating the IMU, and shifting the orientation of the IMU in the accelerometer measurement. The two first points were noted in [START_REF] Esfahani | OriNet: Robust 3-D Orientation Estimation With a Single Particular IMU[END_REF], whereas the two latter are to the best of our knowledge novel.

Although each point may increase the diversity of data, we found they do not necessarily improve the results. We opted for addition of a Gaussian noise (only), during each training epoch, whose standard deviation is the half the standard deviation that the dataset provides (0.01 deg/s).

Experiments

We evaluate the method in term of 3D orientation and yaw estimates, as the latter are more critical regarding long-term odometry estimation [START_REF] Scaramuzza | Visual-Inertial Odometry of Aerial Robots[END_REF][START_REF] Delmerico | A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots[END_REF].

Dataset Descriptions

We divide data into training, validation, and test sets, defined as follows, see Chapter I.5.3 of [START_REF] Goodfellow | Deep Learning[END_REF]. We optimize the neural network and calibration parameters on the training set. Validation set intervenes when training is over, and provides a biased evaluation, as the validation set is used for training (data are seen, although never used for dataset sequence VINS-VINS-Mono Open-Open-VINS Mono [START_REF] Qin | VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[END_REF] (loop-closure) VINS [START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF] EuRoC: the dataset [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF] contains image and inertial data at 200 Hz from a micro aerial vehicle divided into 11 flight trajectories of 2-3 minutes in two environments. The ADIS16448 IMU is uncalibrated and we note ground truth from laser tracker and motion capture system is accurately time-synchronized with the IMU, although dynamic motions deteriorate the measurement accuracy. As yet noticed in [START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF], ground truth for the sequence V1 01 easy needs to be recomputed.

We define the train set as the first 50 s of the six sequences MH{01,03,05}, V1{02}, V2{01,03}, the validation set as the remaining ending parts of these sequences, and we constitute the test set as the five remaining sequences. We show in Section 4.5 that using only 8 minutes of accurate data for training -the beginning and end of each trajectory are the most accurately measured -is sufficient to obtain relevant results.

TUM-VI: the recent dataset [START_REF] Schubert | The TUM VI Benchmark for Evaluating Visual-Inertial Odometry[END_REF] consists of visual-inertial sequences in different scenes from an hand-held device. The cheap BMI160 IMU logs data at 200 Hz and was properly calibrated. Ground truth is accurately time-synchronized with the IMU, although each sequence contains periodic instants of duration 0.2 s where ground truth is unavailable as the acquisition platform was hidden from the motion capture system, see Figure 12.4. We take the 6 room sequences, which are the sequences having longest ground truth (2-3 minutes each).

We define the train set as the first 50 s of the sequences room 1, room 3, and room 5, the validation set as the remaining ending parts of these sequences, and we set the test set as the 3 other room sequences. This slipt corresponds to 45 000 training data points (90 000 for EuRoC) which is in the same order as the number of optimized parameters, 77 052, and requires regularization techniques such as weight decay and dropout during training.

Method Implementation & Training

Our open-source method is implemented on PyTorch 1.5, where we configure the training hyperparameters as follows. We set weight decay with parameter 0.1, and dropout with 0.1 the probability of an element to be set equal to zero. Both techniques reduce overfitting.

We choose the ADAM optimizer [START_REF] Kingma | ADAM: A Method for Stochastic Optimization[END_REF] with cosines warning restart scheduler [START_REF] Loshchilov | SGDR: Stochastic Gradient Descent with Warm Restarts[END_REF] where learning rate is initialized at 0.01. We train for 1800 epochs, which is is very fast as it takes less than 5 minutes for each dataset with a GTX 1080 GPU device.

Compared Methods

We compare a set of methods based on camera and/or IMU.

Methods Based on the IMU Only: we compare the following approaches:

• raw IMU, that is an uncalibrated IMU. It refers also to the proposed method once initialized but not trained;

• OriNet [START_REF] Esfahani | OriNet: Robust 3-D Orientation Estimation With a Single Particular IMU[END_REF], which is based on recurrent neural networks, and whose validation set corresponds to the test set (our training setting is thus more challenging); • calibrated IMU, that is, our method where the 12 parameters Ĉω and ωn are constant, nonzero, and optimized;

• proposed IMU, which is our learning based method described in Section 3.

Methods Based on Camera and the IMU: we run each of the following method with the same setting, ten times to then average results, and on a Dell Precision Tower 7910 workstation desktop, i.e., without deterioration due to computational limitations [START_REF] Delmerico | A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots[END_REF].

We compare:

• VINS-Mono [START_REF] Qin | VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[END_REF], a monocular VIO framework with notable performances on the EuRoC benchmark [START_REF] Delmerico | A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots[END_REF];

• VINS-Mono (loop closure), which is the original VINS-Mono [START_REF] Qin | VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[END_REF] reinforced with loop-closure ability;

• Open-VINS [START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF], a versatile filter-based visual-inertial estimator for which we choose the stereo configuration, and that is top-ranked on the drone dataset of [START_REF] Delmerico | Are We Ready for Autonomous Drone Racing? The UZH-FPV Drone Racing Dataset[END_REF];

• Open-VINS (proposed), which is Open-VINS of [START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF] but where gyro inputs are the proposed corrected gyro measurements (12.5) output by our method (trained on sequences that are of course different from those used for evaluation).

Remaining Methods: we finally add a basic zero motion, that is ω n = 0 3 considered as the standard prior in visual odometry when IMU is not available.

Evaluation Metrics

We evaluate the above methods using the following metrics that we compute with the toolbox of [START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF].

Absolute Orientation Error (AOE): which computes the mean square error between the ground truth and estimates for a given sequence as

AOE = v u t M X n=1 1 M k log ⇣ R T n Rn ⌘ k 2 2 , ( 12.11)
with M the sequence length, log(•) the SO(3) logarithm map, and where the estimated trajectory has been aligned on the ground truth at the first instant n = 0.

Relative Orientation Error (ROE): which is computed as [START_REF] Zhang | A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry[END_REF] ROE

= k log ⇣ R T n,g(n) Rn,g(n) ⌘ k 2 , ( 12.12)
for each pair of timestamps (n, g(n)) representing an IMU displacement of 7, 21 or 35 meters. Collecting the error (12.12) for all the pairs of sub-trajectories generates a collection of errors where informative statistics such as the median and percentiles are calculated. As [START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF][START_REF] Zhang | A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry[END_REF][START_REF] Delmerico | A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots[END_REF], we strongly recommend ROE for comparing odometry estimation methods since AOE is highly sensitive to the time when the estimation error occurs. We finally consider slight variants of (12.11)-(12.12) when considering yaw (only) errors, and note that errors of visual methods generally scale with distance travelled whereas errors of inertial only methods scales with time. We provide in the present chapter errors w.r.t. distance travelled to favor comparison with benchmarks such as [START_REF] Delmerico | A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots[END_REF], and same conclusions hold when computing ROE as function of different times.

Results

Results are given in term of AOE and ROE respectively in The Proposed Method Competes with VIO: our IMU only method is accurate even on the high motion dynamics present in both datasets, see Figure 12.6, and competes with VINS-Mono and Open-VINS, although trained with only a few minutes of data. Finally, as the performance of each method depends on the dataset and the algorithm setting, see Figure 12.5, it is difficult to conclude which VIO algorithm is the best.

Further Results and Comments

We provide a few more comments, supported by further experimental results.

Small Corrections Might Lead to Large Improvement: the calibrated and corrected gyro signals are visually undistinguishable: differences between them rely in corrections ωn of few deg/s, as shown in Figure 12.8. However, they bring drastic improvement in the estimates. This confirms the interest of leveraging neural networks for model correction (12.2)- (12.4).

The Proposed Method is Well Suited to Yaw Estimation: according to Table 12.1 and Figure 12.5, we see yaw estimates are particularly accurate. Indeed, VIO methods are able to recover at any time roll and pitch thanks to accelerometers, but the yaw estimates drift with time. In contrast our dead-reckoning method never has access to information allowing to recover roll and pitch during testing, and nor does it use "future" information such as VINS-Mono with loop-closure ability. We finally note that accurate yaw estimates could be fruitful for yaw-independent VIO methods such as [START_REF] Svacha | Inertial Yaw-Independent Velocity and Attitude Estimation for High-Speed Quadrotor Flight[END_REF].

Correcting Gyro Slightly Improves Open-VINS [START_REF] Geneva | OpenVINS: A Research Platform for Visual-Inertial Estimation[END_REF]: both methods based on Open-VINS perform similarly, which is not surprising as camera alone already provides accurate orientation estimates and the gyro assists stereo cameras. Figure 12.7: Orientation errors on the sequence room 4 of [START_REF] Schubert | The TUM VI Benchmark for Evaluating Visual-Inertial Odometry[END_REF]. Our method removes errors of the calibrated IMU and competes with VIO algorithms.

Our Method Requires few Computational Ressources: each VIO method performs here at its best while resorting to high computational requirements, and we expect our method -once trained -is very attractive when running onboard with limited resources. Note that, the proposed method performs e.g. 3 times better in terms of yaw estimates than a slightly restricted VINS-Mono, see Figure 3 of [START_REF] Delmerico | A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots[END_REF].

Discussion

We now provide the community with feedback regarding the method and its implementation. Notably, we emphasize a few points that seem key to a successful implementation when working with a low-cost high frequency IMU.

Key Points Regarding the Dataset

One should be careful regarding the quality of data, especially when IMU is sampled at high-frequency. This concerns: IMU Signal: the IMU signal acquisition should be correct with constant sampling time.

Ground Truth Pose Accuracy: we note that the EuRoC ground truth accuracy is better at the beginning of the trajectory. As such, training with only this part of data (the first 50 s of the training sequences) is sufficient (and best) to succeed.

Ground Truth Time-Alignement: the time alignment between ground truth and IMU is significant for success, otherwise the method is prone to learn a time delay.

We admit that our approach requires a proper dataset, which is what constitutes its main limitation.

Key Points Regarding the Neural Network

Our conclusions about the neural network are as follows.

Activation Function: the GELU and other smooth activation functions [START_REF] Ramachandran | Searching for Activation Functions[END_REF], such as ELU, perform well, whereas ReLU based network is more prone to overfit. We believe ReLU activation function favors sharp corrections which does not make sense when dealing with physical signals.

Neural Network Hyperparameters: increasing the depth, channel and/or kernel sizes of the network, see Figure 12.2, does not systematically lead to better results. We tuned these hyperparameters with random search, although more sophisticated methods such as [START_REF] Li | Hyperband: A Novel Bandit-Based Approach to Hyperparameter Optimization[END_REF] exist.

Normalization Layer: batchnorm layer improves both training speed and accuracy [START_REF] Ioffe | Batch Normalization: Accelerating Deep Network Training by Reducing Internal Covariate Shift[END_REF], and is highly recommended. . We see we manage to divide orientation errors by a factor at least 2 w.r.t. calibrated IMU applying corrections whose amplitude is as low as 1 deg/s (max).

Key Points Regarding Training

As in any machine learning application, the neural network architecture is a key component among others [START_REF] Goodfellow | Deep Learning[END_REF]. Our comments regarding training are as follows:

Optimizer: the ADAM optimizer [START_REF] Kingma | ADAM: A Method for Stochastic Optimization[END_REF] performs well.

Learning Rate Scheduler: adopting a learning rate policy with cosinus warning restart [START_REF] Loshchilov | SGDR: Stochastic Gradient Descent with Warm Restarts[END_REF] leads to substantial improvement and helps to find a correct learning rate.

Regularization: dropout and weight decay hyperparameters are crucial to avoid overfitting. Each has a range of ideal values which is quickly tuned with basic grid-search.

Remaining Key Points

We finally outline two points that we consider useful to the practitioner:

Orientation Implementation: we did not find any difference between rotation matrix or quaternion loss function implementation once numerical issues are solved, e.g., by enforcing quaternion unit norm. Both implementations result in similar accuracy performance and execution time.

Generalization and Transfert Learning: it may prove useful to assess to what extent a learning method is generalizable. The extension of the method, trained on one dataset, to another device or to the same device on another platform is considered as challenging, though, and left for future work.

Conclusion

This chapter proposes a deep-learning method for denoising IMU gyroscopes and obtains remarkable accurate attitude estimates with only a low-cost IMU, that outperforms state-of-the-art [START_REF] Esfahani | OriNet: Robust 3-D Orientation Estimation With a Single Particular IMU[END_REF]. The core of the approach is based on a careful design and feature selection of a dilated convolutional network, and an appropriate loss function leveraged for training on orientation increment at the ground truth frequency. This leads to a method robust to overfitting, efficient and fast to train, which serves as offline IMU calibration and may enhance it. As a remarkable byproduct, the method competes with state-of-the-art visual-inertial methods in term of attitude estimates on a drone and hand-held device datasets, where we simply integrate noise-free gyro measurements.

CHAPTER 13 Additional Results for Inertial Navigation of Cars

In this Chapter, we show unpublished additional results for inertial navigation of car.

The approach is a direct extension of the Chapter 11 where the noise covariance matrix of the filter is adapted by the method described in Chapter 8.

Introduction

We show here a modular method for odometry localization of ground vehicles, whose overview is depicted in Figure 13.1, that requires only an IMU. The approach identifies instants where the vehicle is stopping from raw sensor signals and leverages them through an invariant Kalman filter for robust state estimation of vehicle pose, vehicle velocity and IMU biases. We perform substantial evaluations on three vehicle datasets, which represent a total amount of 366 km of data acquired during 15 hours in diverse environments such as cities, highway and countries, where the vehicle moves up to 25 m/s during three sequences of minimum 70 minutes long on our own specifically dedicated experiments.

The method obtain impressively accurate results that exhibit how sufficient a mediumcost IMU contains information for odometry localization.

Experimental Results

We evaluate the performances of the proposed approach on three datasets. Our primary goal in this section is to show that using an IMU of moderate cost, one manages to obtain surprisingly accurate dead-reckoning by combining machine learning techniques with a state-of-the-art Kalman filter.

Dataset Descriptions

The three datasets have been acquired on distinct vehicles, each of them being equipped with its proper sensor suite, see Figure 13.2. We dispose of a total amount of 366 km high-quality experimental data acquired during more than 15 h with a great diversity in terms of vehicle dynamics, sensors, and external environments, see Table 13.1. We gives in the following a brief description of each dataset. The approach is evaluated on three distinct datasets: from left to right, images of the vehicle in the publicly available KAIST dataset [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF][START_REF] Jeong | Complex Urban Dataset with Multi-Level Sensors from Highly Diverse Urban Environments[END_REF], the Lille dataset which is publicly available for point cloud classification, see [START_REF] Roynard | Paris-Lille-3D: A Large and High-Quality Ground-Truth Urban Point Cloud Dataset for Automatic Segmentation and Classification[END_REF][START_REF] Roynard | Paris-Lille-3D: A Point Cloud Dataset for Urban Scene Segmentation and Classification[END_REF] KAIST Dataset [START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF][START_REF] Jeong | Complex Urban Dataset with Multi-Level Sensors from Highly Diverse Urban Environments[END_REF]: The KAIST Urban Data Set is a publicly available dataset that provides IMU, fiber optic gyro, vision, radar, encoder and altimeter data targeting the highly complex urban environment around Daejong, Korea. The Xsens MTi-300 IMU logs data at 100 Hz and has a 10 deg /h gyro bias stability. We download 32 sequences urban06 to urban37 and exclude the remaining sequences that are provided without accelerometer measurements.

Lille Dataset: The Paris-Lille-3D dataset [START_REF] Roynard | Paris-Lille-3D: A Large and High-Quality Ground-Truth Urban Point Cloud Dataset for Automatic Segmentation and Classification[END_REF][START_REF] Roynard | Paris-Lille-3D: A Point Cloud Dataset for Urban Scene Segmentation and Classification[END_REF] is a publicly available urban point cloud dataset for automatic segmentation from which we recover IMU data along with centimeter accurate ground-truth pose for trajectories located in the city of Lille, France. The vehicle is a Citroën Jumper van equipped with a precise Novatel FlexPak 6 dualphase RTK-GPS and an Ixsea LANDINS IMU whose signal is sampled at 100 Hz. The dataset contains 28 km of experiments divided into 8 sequences up to 20 min long.

Magny-Les-Hameaux Dataset (MLH):

We obtain experiments from the Safran1 company, its R&T center "SafranTech" located at Magny-Les-Hameaux, France, and its laboratory specifically dedicated for autonomous vehicles. We collect IMU, GPS, and wheel encoder data from a Citroën Picasso in the countryside environment around the R&T center. The Epsilon10 IMU2 is a medium-cost device specifically dedicated for hybrid land navigation and orientation systems, and the Geoflex RTK-GPS provides ground-truth poses. Three sequences of minimum 70 min long have been recorded, representing a total of 105 km data with relatively high vehicle velocity (25 m/s), speed bumps and speed reducers.

Evaluation

This section evaluates the dead-reckoning method on the KAIST, Lille and MLH datasets. We benchmark our method in this section considering only IMU signals.

= 1 N N X n=0 kv imu n vimu n k 1 , (13.3)
and indicates the ability of a method for regressing velocity in the world frame.

The three above l 1 metrics are less sensitive to poor estimates than root mean square error and allow correcting position when zero-motion is detected. We compare on the aforementioned metrics three methods:

INS: the direct integration of the inertial measurements;

odo.: the integration of a differential wheel encoder which computes linear and angular velocities, a.k.a. wheel-based odometry;

IMU (ours): the proposed approach, that uses only the IMU signals and involves no other sensor, that is our purely inertial navigation system.

Results

Results are averaged in Table 13.5 and detailed for each dataset in Tables 13.6, 13.7, 13.8 and 13.9. Figures 13.15, 13.16, 13.17, and 13.18 illustrate diverse trajectory results. From these results, we see that:

• directly integrating the IMU signals leads to rapid drift of the estimates;

• wheel-based differential odometry accurately estimates the linear velocity but encounters troubles estimating the yaw, even in Figure 13.17 whose data has been obtained in an urban environment and provided with calibrated odometry intrinsics;

• the proposed schemes completely outperforms wheel based odometry in urban, countryside and highway environments. The results are remarkable as we use for each dataset an IMU which has moderate precision and is targeted for hybridation with GPS. Beyond these unexpected long-term accurate position estimations, one may benefit from the method for promptly calibrating inherent and strong IMU biases whose magnitude is greater than 10 3 rad/s and 10 2 m/s 2 on the KAIST dataset. This hold when the car is first stopping and the first motions contain sufficiently turns and accelerations that render IMU bias observable. However, when both the car starts by moving and follows a straight highway, one can not identify IMU bias and the estimation drifts, which is similarly encountered in visual-inertial odometry when degenerate motions occur, see [START_REF] Yang | Degenerate Motion Analysis for Aided INS With Online Spatial and Temporal Sensor Calibration[END_REF]. This explains the results obtained in Table 13.7. Finally, the AVE metrics confirms us that the method is a particularly precise speedometer.

Further Experimental Results

We provide in this section experimental results that completes the dead-reckoning evaluation performed in Section 2. We analyze the zero-motion detector, and inspects the computational execution times of the method. All the results are based on the three datasets described in Section 2.1.

Detector Results

To assess the performances of our learning based detection approach (see Section 4.1), we focus on the zero velocity detection (11.9), and compare the proposed detectors with two popular detectors [START_REF] Skog | Zero-Velocity Detection-An Algorithm Evaluation[END_REF][START_REF] Wagstaff | LSTM-Based Zero-Velocity Detection for Robust Inertial Navigation[END_REF]. To make sure such traditional detectors perform at their best, their parameters are optimized by grid search on each full dataset, i.e. with test sequences, whereas our detectors still learn only on training data. Since generalization to new type of data is a cumbersome task for deep learning based algo- Table 13.11: Execution times of the approach averaged for 1 s of data, in ms and percentage of the total execution time of the approach. Time execution slightly depends on the ratio of the number of non zero-motion detections over the number of zeromotion detections. The approach is more than 10 times faster than real-time execution with Python implementation.

rithms [START_REF] Goodfellow | Deep Learning[END_REF], we provide in this section preliminary results toward the generalization of the method to a new vehicle, by training the method on a platform and then validating it on another platform. We compare three zero-motion detectors:

ideal ARED: the Angular Rate Energy Detector computes a measure of the gyro variance over a sample window. If the variance measure falls below a threshold, the vehicle is assumed to be stationary;

ideal AMVD: the Acceleration-Moving Variance Detector computes a measure of the accelerometer variance over a sample window. If the variance measure falls below a threshold, the vehicle is assumed to be stationary;

IMU-only (ours): the proposed detector with IMU only, and trained with training sequences of the evaluated dataset.

We plot the partial Receiver Operating Characteristic (ROC) curve in Figure 13.23, which illustrates the ability of a classifier as its threshold is varying [START_REF] Mcclish | Analyzing a Portion of the ROC Curve[END_REF][START_REF] Pedregosa | Scikit-learn: Machine Learning in Python[END_REF]. The curve displays the true positive rate (i.e. where ẑvel n = z vel n = 1 occurs) against the false positive rate for a total of 10 9 evaluated points. The best possible detector would yield a point in the upper left corner of coordinate (0, 1), representing 100% sensitivity (no false negatives) and 100% specificity (no false positives). Figure 13.23 zooms at a specific region of the whole curve since detectors get no false negatives once a sufficiently high false negative rate is crossed.

We numerically compare the classifiers in term of Area Under Curve (AUC) in Table 13.10, which is the area under the ROC curve, where we adjust the maximal false positive rate to 0.1 for highlighting discrepancies between methods. It measures the performance of a classifier as representing the probability that the classifier ranks a randomly drawn positive example higher than a randomly drawn negative example, and is independent from a manually defined threshold. From these results, we observe:

• the conventional albeit ideal AMVD and ARED can not detect zero-motion without avoiding false detection;

• the proposed IMU-only detector identifies zero-motion with a particularly low level of false detections, a.k.a. false alarms. We set the detector with a threshold of 0.95 and thus respect the filter constraint for almost never encountering falsealarm.

Computational Execution Times

The computational execution times of the method is provided in Table 13.11, where we divide time statistics into datasets. We run the method on a Dell Precision Tower 7910 machine which accelerates the detector and adapter computations through a GTX 1080 GPU, and we establish time statistic with the Python profiler. The filter operates on the CPU since computing on GPU is slower in our case and generally unefficient for computations effected in double precision with many back-and-forth transfers of data. We observe in Table 13.11 small variations of execution times across the datasets, which is due to different ratios of the number of non zero-motion detections over the number of zero-motion detections. Indeed, detecting a zero-motion modifies the propagation step of the filter and the dimension of the observation vector y n . The computational requirements are reasonable as the methods keeps 10 times faster than real execution times, and we finally note our Python implementation can be drastically accelerated both for the filter and deep neural networks blocks with the recent development of the PyTorch C++ API3 .

Conclusion

This chapter extends the approach for inertial based navigation of ground vehicles. Our approach exploits deep neural networks to identify zero-motion information from raw sensor signals which is then leveraged in an invariant extended Kalman filter that performs localization, velocity and sensor bias estimation. We additionally design deep learning based blocks that feed to the filter measurements along with time-varying measurement uncertainties which alleviate the practitioner for cumbersome parameter tuning. The entire pipeline requires only IMU data, and can incorporate GPS, wheel encoders and altimeter. The method leads to surprisingly accurate results on four datasets, and open new perspectives for combination of data-driven methods with well established methods for autonomous systems. Future works are an ablation study to precisely asses the advantages and limitations of each pseudo-measurement we use.

CHAPTER 14 Conclusion of the Thesis

This thesis investigates various challenging aspects regarding Kalman filtering for localization and navigation for vehicles equipped with sensors such as cameras and inertial measurement units. It brings several contributions divided in two categories.

The first category of contributions is for enhancing and applying Kalman filter theory. It consists in building on the recent IEKF theory to address the inconsistency of EKF for SLAM, navigation with vision sensors, and the more general question of Kalman filtering on manifolds. The thesis starts by revisiting the theoretical problem of EKF inconsistency for SLAM, and shows how the use of the IEKF may resolve those issues. It is then studied and shown how the well established unscented Kalman filter may be adapted in the case where the state does not belong to a vector space, but to a more complicated space, namely a manifold. The proposed method applies to all Lie groups, and in particular allows to propose an unscented version of the IEKF. This allows fast prototyping, in the sense that it spares the sometimes difficult computation of Jacobians that must be performed in the extended Kalman filter methodology. The practical problem of SLAM and odometry in the presence of vision and inertial sensors is then addressed. The thesis builds on the state-of-the art multi-state constrained Kalman filter to attack the problem, and the contribution essentially consists in building an invariant version of this filter, which then comes with consistency properties, and to propose a computationally efficient unscented version.

The second category of contributions of the thesis explores how recent tools from the field of deep learning can be used to improve Kalman filters. The thesis notably focuses on the assessment of uncertainty of sensors or pseudo-measurements, and designs an approach to automatically relate sensors' measurements to the state, and to tune the filter, that is, using machine learning techniques to find a dynamical tuning strategy of the Kalman filter parameters that best matches the data. As a result, a machine learning algorithm may learn the extent of uncertainty that lies in the formulated assumptions of zero lateral and vertical velocities for inertial navigation of ground vehicles, where the proposed method based only on inertial sensors competes with state-of-the art methods that use inertial sensors plus vision. Another type of uncertainty may stem from the nature of the observation, as pose estimates from laser scanners and the ICP algorithm. As it is important for the filter that estimates the robot's trajectory to "know" the amount of uncertainty present in the ICP's estimated displacement, the thesis proposes a simple theoretical approach based on statistics and an algorithm for this problem which goes beyond existing solutions, and overcomes their main drawbacks. The thesis then wholly focuses on the use of deep learning as a tool to extract pertinent information from inertial measurements, and to use this information to navigate. In all cases, the method is supervised, and some ground truth information is necessary for the algorithms to learn. The thesis builds upon the zero velocity update which is a way to correct the estimates when a wheeled vehicle stops, and leverages deep learning to make the estimator system detect stops based only on the inertial sensors, hence relaxing the need for additional sensors besides inertial sensors. Evaluations on a publicly available car dataset demonstrates indeed that the proposed scheme may achieve a final precision of 20 m for a 21 km long trajectory of a vehicle driving for over an hour, equipped with an IMU of moderate precision. The thesis then focuses on the estimation of the orientation of the vehicle only, namely its attitude, and leverage deep learning to refine the modelling of the measurements emanating from the inertial sensors, and to calibrate them.

Along the thesis, several questions about filtering on manifolds and deep learning for Kalman filtering were raised. First, theoretical questions arise regarding filtering on manifold. The thesis has followed in focusing on parallelizable manifolds where a global coordinate system of tangent spaces exists, and readily provides a transport operation over the manifold. However, there are multiple choices for the parallel transport operation, e.g. Levi-Civita connection for parallel transport, which depends on the chosen metric.

Second this thesis addresses filtering methods for SLAM and visual-inertial odometry. While current systems provide accurate motion tracking in small-scale "friendly" environments, they are not robust enough for long-term, large-scale and safety-critical deployments for e.g. autonomous driving. More than accurate, these methods should be robust. These algorithms are thus subject to both theoretical and practical challenges: how a VIO system equipped with biased IMU is able to estimate scale, and to correctly perform state initialization are unclear, especially for degenerated motions. Other sources of inconsistency may come from the visual feature tracking which is subject to outliers. The tuning of MSCKF measurement update is a computer vision and filtering challenging question. The strategy to choose which camera pose to marginalize has a large impact on performances, if well done, one could expect accuracy with low computational requirement, but this problem has hardly been addressed, see [START_REF] Thalagala | Comparison of State Marginalization Techniques in Visual Inertial Navigation Filters[END_REF].

Regarding deep learning, the thesis opens new perspectives for combination of data-driven methods with well established methods for autonomous systems, and opens up for exciting avenues. As a MSCKF is complex, using deep learning to improve it in a coupled-manner is debatable and we suggest to first attack specific challenge as the marginalization strategy mentionned above. Secondly, we managed to train neural networks by using accurate datasets ( [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF][START_REF] Schubert | The TUM VI Benchmark for Evaluating Visual-Inertial Odometry[END_REF] have less than centimeter accuracy). If one searches to enhance a yet accurate system or sensors, it seems natural to first dispose of a sufficiently accurate ground-truth. A typical solution would consist in first computing a time expansive solution based on smoothing or optimization algorithms with various sensors. An other way is to leverage unsupervised learning, see e.g. [START_REF] Almalioglu | SelfVIO: Self-Supervised Deep Monocular Visual-Inertial Odometry and Depth Estimation[END_REF] for visual-inertial odometry. Finally, the question of transfer learning, i.e. how using a neural network trained on one device for another device, which is decisive for industrial application, is left for future research works. These questions, along with the crucial issue of dealing with safety, computational and economical trade-offs, are fundamental if one wants to provide autonomous systems with reliable information in an industrial context. an IMU where we yet dispose of a (invariant) Kalman filter. We then apply a step-bystep recipe that search to obtain more reliable and efficient Kalman filter estimates, where the learning block provides to the filter observations and observation uncertainty, see Figure 15.1.

This chapter is organized as follow. Section 2 presents the problem of attitude estimation from an IMU and the related invariant Kalman filter that we search to improve. Section 3 describes the method for learning accurate gravity observations that are then feed to the filter. Section 4 evaluates and shows how the patient approach outperforms the fast-and-furious ones. Finally, Section 5 concludes the chapter.

Problem Modelling

This section briefly describes the model used in this chapter. The 3D orientation of a rigid platform is obtained by integrating gyro outputs of an IMU through R n = R n 1 exp (ω n dt) , (15.1) where the rotation matrix R n 2 SO(3) at timestamp n maps the IMU frame to the global frame, the angular velocity ω n 2 R 3 is averaged during dt, and with exp( This invariant Kalman filter mimics the complementary filter [START_REF] Mahony | Nonlinear Complementary Filters on the Special Orthogonal Group[END_REF] with the advantage here that the gain K is optimal given Q n and cov(y n ) while Jacobian matrices are constant. The measurement (15.4) provides information around the gravity direction, such that the roll and the pitch of the sensor are observable, while the yaw keeps non observable and its estimates drifts along time.

Enhancing the Kalman Filter with Deep Learning

We describe in this section how improving the Kalman filter observation with deep learning, as measurements (15.4) are noisy and setting its uncertainty require apparently manual tweaking. We first design a neural network and a training scheme to obtain reliable observations of the gravity orientation y n along with the observation uncertainty cov(y n ), see Figure 15 Unfortunately, neural networks are nothing like that. They are not "off-the-shelf" technology or theory the second one deviate slightly from training a vanilla neural network classifier. ADAM [START_REF] Kingma | ADAM: A Method for Stochastic Optimization[END_REF] does not magically make the network trains, batch normalization [START_REF] Ioffe | Batch Normalization: Accelerating Deep Network Training by Reducing Internal Covariate Shift[END_REF] does not magically make it converge faster, . . . Using the technology and theory without understanding how it works are prone to fail.

• Neural Network Training Fails Silently: when the implementation of a filter contains mistakes, the filter often crash or gets definitely bad results. The situation where the filters "surprisingly" works whereas it should not (a misconfiguration or an incorrect propagation matrix for example) barely appears compared to deep learning.

Indeed, when it comes to training neural networks, everything could be correct syntactically, but the whole thing can not be arranged properly, and it is really hard to tell. The possible error are large, logical (as opposed to syntactic), and very tricky to test. Therefore, a misconfigured neural network will most of the time train but silently work a bit worse.

In light of the above points, one should be highly defensive and obsessed with visualizations of basically every possible thing. The qualities that correlate most strongly to success are patience and attention to detail.

We now details chronologically the steps to successively obtain a neural network able to improve the estimates of the filter. This starts by being an expert of the full nonlearning problem, i.e. the Kalman filter and data, that lead to appropriately defining what one can expect from the neural networks. Then, we follow all the gold standard of deep learning. In particular, we build from simple to complex and at every step of the way we make concrete hypotheses about what will happen and then either validate them with an experiment or investigate until we find some issue. Finally, we combine then the deep learning block and the Kalman filter, and evaluate the results of the enhanced filter.

Before Training: Visualizing the Non-Learning Problem

What we remark about leaky abstractions for neural networks also hold for any traditional problem. One must mastering its subject, possibly explains any strange behavior regarding data and the filter outputs, and thoroughly inspect data. This step is critical.

For our problem, we used the EuRoC dataset [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF], that contains sequences of IMU data along with ground-truth. We search to understand the data, especially through visualization tools. Plotting inputs versus ground-truth or dead-reckoning estimates is essentials to perform basic checks: is the IMU vector contains first the gyro or the accelerometer, what are units, how is synchronization between ground-truth and inputs. This process is essential to detect fails and outliers in data. Concretely, it leads here to plot curves similar to the ones in Figure 15.3, and detect that the IMU is uncalibrated (see also Chapter 12), and break the model assumption (15.2). We use Chapter 12 to first calibrate the IMU sensor.

The same checking must be done for the Kalman filter. One must observe state estimates, state uncertainty (P n|n ), residual, . . . The filter itself must be clean: if its estimations are surprisingly bad after one minutes, it could be to P n|n being non symmetric; or if P n|n is too high, it is probably due to a misconfiguration of model parameters. Deep learning will not magically solve that.

At the middle of this step, both the dataset and the filter work as expected. Ones can then get a first idea of what to learn. This should be useful for the filter (improving something that works optimally is both difficult and inefficient) and possible. For the given problem, it is clear that the main limitation of the filter is the assumption done in (15.3), see Figure 15.4. We thus hope to learn (or correct) this observation. At least, we expect estimating its uncertainty, possibly better than a manual tweak on the accelerometer norm. Of course, this idea evolve along time. Learning uncertainty may just be to difficult if ground truth is insufficiently accurate.

Set Up the Training & Getting Baselines

Our next step is to set up a full training and evaluation skeleton and gain trust in its correctness via a series of experiments. At this stage it is best to pick some simple model as a very tiny CNN. We want to train it, visualize the losses, any other metrics (e.g. accuracy), model predictions, and perform a series of ablation experiments with explicit hypotheses along the way.

We follow the convolutional neural network structure of Chapter 12 that computes accelerometer correction ãn and uncertainty σ n 2 R from N past IMU measurements, where we first restrict it to one layer.

We now define the corrected and corrected measurement as ân = a imu n + ãn (15.8) ŷn = ân kâ n k , (15.9)

where as in Chapter 12 the neural network is guided and initialized properly at a imu n . This thus provides us with the following simple baseline: the measurement where ãn = 0.

We now define the loss function that must ensures measurement and uncertainty estimations as [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF], where the acceleration assumption (15.3) is visibly broken. We observe that accelerometer is highly noisy to estimation gravity direction, even when the drone is static at the first secondes of the trajectory.

which is the negative log likelihood. At this stage, we are not certain that (15.10) is the best metric. Perhaps a robust l 1 metric is better, but this is fine-tunning question, see Section 3.3. Now, we have a small network, a loss and a baseline. We train the network with vanilla optimizer and perform check. For instance, we verify loss at init and that it starts at the correct loss value, i.e. the one where ãn = 0. Indeed, if the loss is very high, it just indicate we did not initialize the final layer weights correctly. Setting these correctly will speed up convergence and eliminate the first few iteration the network is basically just learning a bias.

We then monitor metrics other than loss that are human interpretable and checkable. The loss in (15.10) is difficult to understand, but we can compute and plot RMSE loss based on y n ŷn and see how it evolves along training. We can also compare this metrics to the baseline.

Then we overfit a small batch. Overfit a single batch of only a few examples. To do so we increase the capacity of our model (here the kernel size) and verify that we can reach the lowest achievable loss (here minus infinity).

Training Fine-Tuning

Overfit & Regularize: At this stage we have a good understanding of the dataset and we have the full training and evaluation pipeline working. For any given model we reproducibly compute a metric that we trust. We are also armed with our performance for a baseline (that we should beat). The stage is now set for iterating on a good model.

The approach generally adopted to a good neural network model has two stages: first, we get a model large enough that it can overfit (i.e. focus on training loss) and then we regularize it appropriately (that increases the training but improves the validation loss). The advocated reason is that if we are not able to reach a low error rate with any model at all that may again indicate some issues, bugs, or misconfiguration. Then, we are at a place where we have a large model that is fitting at least the training set. We now regularize it and gain some validation accuracy by giving up some of the training accuracy.

We have two main advices:

• To reach a good training loss we choose an appropriate architecture for the data. We now do not search to be a hero. We were the first to be eager to get crazy and creative in stacking up the blocks of the neural net toolbox in various exotic architectures that make sense to us, but this was inefficient.

• ADAM is the safest optimized, which gas a large good learning rate region compared to other optimizers. We do not trust learning rate decay defaults, especially when we are re-purposing code from some other domain (mainly computer vision).

Network and Training hyperparameters Fine-Tunning: We are now "in the loop" with the dataset and explore a wide model space for architectures that achieve low validation loss. For simultaneously tuning multiple hyperparameters it may sound tempting to use grid search to ensure coverage of all settings, but it is best to use random search instead. Intuitively, this is because neural nets are often much more sensitive to some parameters than others. Indeed, there is a large number of fancy bayesian hyper-parameter optimization toolboxes around and our personal experience is that the state-of-the art approach to exploring a nice and wide space of models and hyperparameters is to use trial-and-error.

Once you find the best types of architectures and hyper-parameters we now feed the filter with the networks. We then still massively visualizing the filter behavior and verify if measurements are correct and uncertainty relevant. For example, we found it is sufficient to learn the same parameters for each measurement σ n = n 1 3 . This has a spin-off advantage to make the Kalman filter really cheap in term of covariance matrix computation. All the gain can be parametrized with a scalar, see Appendix.

Results

We now dispose of a finely tunned neural network. We choose to evaluate the filter performances as evaluating the network alone is here difficult to properly correlate with the filter performance, and we show an example of network estimates in Figure 15.5. It visibly reduces the noise on the raw measurements.

Compared Methods & Metric

We compare the filter results with different configuration and learning methods, a.k.a. ablation study (in the context of deep learning, an ablation study refers to removing some feature of the training pipeline, and seeing how that affects performance.).

• w/o calibration, the proposed method without calibrating data;

• w/o initialization, the proposed method without properly initialing the network output in (15.9), i.e. ân = ãn ;

• w l 2 loss, the proposed method where we adopt the l 2 loss P i ky i ŷi k 2 during training instead of (15.10);

• baseline, the Kalman filter used without neural networks but calibrated data;

• proposed, the proposed method described in Section 3.

Each filter is parametrized as follow: P 0 = 0, i.e. the state is initialized at ground truth, Q n = q 2 I 3 with q =1.10 5 rad/s and cov(y n ) is given by the neural network.

We evaluate the above methods on the Absolute Orientation Error (AOE), which computes the mean square error between the ground truth and estimates for a given sequence as (15.11) with M the sequence length, log(•) the SO(3) logarithm map, and where the estimated trajectory has been aligned on the ground truth at the first instant n = 0. In contrast to Chapter 12, AOE error makes more sense as the filter is here able to recover roll and pitch.

AOE = v u t M X n=1 1 M k log ⇣ R T n Rn ⌘ k 2 2 ,

Results

We provide to the filter the IMU input where the sensor is calibrated and the calibrated IMU of Chapter 12. Results are given in Figure 15.6. We observe

• The most important improvement occurs when we calibrate data, see difference between w/o calibration and baseline. It evidences that careful data analysis is required to provide yet accurate baseline;

• The assumption of constant speed in the measurement update indeed degrades performances for these small sequences if we compare baseline with results of Chapter 12. The assumption (15.3) is not really valid;

• Properly initializing the correction helps to improve results, as the network outputs weak values and we can thus regularize the neural network more efficiently;

• There are no importance of optimal loss here. Correctly fixed the measurement noise R is sufficient.

Conclusion

This chapter shows a general approach for safely improving a Kalman filter with deep learning, where a deep learning block is designed and provides the filter with valuable observations. This lead to a method that leads practically to improve the filter (in contrast to method that starts a deep neural network from scratch) and where multiple well trained learning blocks can be stacked. On the attitude estimation problems from an IMU, it leads to improve the estimation of the gravity measurements that can be used to obtain accurate attitude results with minimal computational requirement. The improvement between the baseline and the proposed approach is noticeable.

Appendix: Efficient Kalman Filtering

In the particular case where the propagation noise covariance matrix Q n = Q is static and that the measurement noise covariance matrix cov(y n )= 2 n I is time-varying but isotropic, the Kalman filter only requires one parameter to compute the gain, which is computationally really efficient.

We assume the filter is initialized with P 0 = 0, and that Q = qI 3 , and cov(y n )= 2 n I.

Proposition: only a scalar p n is required to compute the Kalman gain.

Proof: the proof is shown by induction. The proposition hold for n = 0. We assume that if it hold for n 1, i.e. , where pn 1 is any scalar, then the proposition hold for n. After propagation, we obtain P n = P n 1 + Q ) p n = p n 1 + q, pn = pn 1 + q. (15.

P

Algorithm 3 :

 3 set_weights(•) function used for computing UKF parameters. Input: d,↵; 1 =(↵ 2 1)d ;

Figure 3 . 1 :

 31 Figure 3.1: 1D toy SLAM problem. A robot at position m observes a landmark at position l with a range measurement y i .

Theorem 1 .

 1 If system (3.5)-(3.6) is invariant in the sense of Definition 3, then the directions @ @↵ ↵ | Id (χ n 0 ) infinitesimally spanned by • (•) at any χ n 0 necessarily lie in Ker O(χ n 0 ), with O(χ n 0 ) defined by (3.11), and are thus unobservable. Proof. Differentiating 1 (3.9) and (3.10) w.r.t. ↵ at Id we obtain

Theorem 1 .

 1 If the system (3.5)-(3.6) is invariant, and under Assumption 1, the unobservable directions C spanned by • (•) at any point χn 0 , measured using error ⌘(•) necessary lie in Ker Ôe ( χn 0 ), with ( Fe n , Ĥe n ) n n 0 defined by (3.17)-(3.18).

Theorem 3 .

 3 The error (3.29) is compatible with the action (3.22) of SE(3) in the sense of Assumption 1.

Figure 3 . 3 :

 33 Figure 3.3: Simulated trajectory : the displayed loop is driven by a robot able to measure the relative position of the landmarks lying in a range of 5 m around it. Velocity is constant as well as angular velocity.

5.1 5 . 1

 51 Monte-Carlo Based Numerical Results

Figure 3 .

 3 Figure 3.4 displays the Normalized Estimation Error Squared (NEES), Root Mean Square Error (RMSE) and distance to Maximum-Likelihood estimate, over 1000 Monte-Carlo runs.

Figure 3 . 4 :

 34 Figure 3.4: Average performances of the different methods over 1000 runs. NEES (for the robot 3-DoF pose) is the consistency indicator, and full consistency corresponds to NEES equal to 1. We see proposed EKF and iSAM are the more consistent, followed by OC-EKF and robocentric EKF, whereas standard EKF is not consistent. The accuracy is evaluated in terms of the robot position RMSE. Standard EKF shows degraded performances as compared to others, which all achieve comparable performances. Finally, filters are evaluated in terms of average proximity of robot's estimated position with iSAM's, which computes the most likely state χ n given all past measurements y 1 ,...,y n . It is used as a reference of the best achievable estimate. We see that the proposed EKF are the closest to iSAM. Dashed lines correspond to 3 confidence upper bounds, and we see EKF is over-optimistic. Figures best seen in color.

Filter

CHAPTER 4 A

 4 New Approach to Unscented Kalman Filtering on Manifolds Part of the content of this chapter has been presented in the International Conference on Robotics and Automation (ICRA 2020). Résumé Le présent chapitre présente une nouvelle méthodologie pour le filtrage de Kalman sans-parfum (UKF) sur les variétés et les groupes de Lie. Au-delà des performances d'estimation, l'intérêt principal de l'approche est sa polyvalence, car notre méthode s'applique à de nombreux problèmes d'estimation d'état, et dispose d'une simplicité dans sa mise en oeuvre pour ceux ne connaissant que partiellement les variétés et les groupes de Lie. Nous avons développé la méthode sur deux frameworks open source Python et Matlab indépendants, et qui permet d'implémenter et de tester rapidement l'approche. Les codes en ligne contiennent des didacticiels, des documentations et divers exemples que l'utilisateur peut facilement reproduire puis adapter pour un prototypage et une analyse comparative rapides. Le code est disponible à l'adresse https://github.com/CAOR-MINES-ParisTech/ukfm.

Figure 4 . 1 :

 41 Figure 4.1:The cylinder is a parallelizable manifold. We can define vector fields V 1 ,V 2 that form a basis of the tangent space at any point.

  the group of multiple spatial isometries SE k (d) was introduced in the context of SLAM. The group SE k (d), allows recovering SE(3) with k =1,d = 3, SE(2) with k = 1,d = 2 and SO(3) with k =0,d = 3. It seems to cover virtually all robotics applications

Example 16 .

 16 We may modify the representation used in Example 15 by viewing the state as an element χ 2 SE 2 (3) instead. This defines two alternative retractions. See e.g. implementation for corresponding ' 1 (•)'s in Snippet 3. A quick comparison displayed in Figure 4.2 indicates the SE 2 (3)-UKF with right multiplications (4.19) outperforms the other filters, notably the one based on the naive structure of Example 15.

Figure 4 . 2 :

 42 Figure 4.2: Inertial navigation with heavy initial errors in the setting of [42]. SE 2 (3)-UKF obtains the best results.

Figure 4 . 2 .

 42 Indeed, the group structures SE 2 (3) versus SO(3) ⇥ R 6 actually boil down to particular choices of parallelization (hence transport), and the filter based on SE 2 (3) outperforms the other. CHAPTER 5 Invariant Kalman Filtering for Visual Inertial SLAM Résumé Dans ce chapitre, nous nous appuyons à la fois sur la théorie du filtrage de Kalman sans-parfum sur les groupes de Lie dévelopée dans le Chapitre 4 et plus généralement sur la théorie du filtrage de Kalman invariant, un UKF innovant est dérivé pour le problème de la la localisation et cartographie simultanées basée inertie et vision monoculaire. La position du véhicule, sa vitesse et les positions des points de repère 3D sont considérées comme un seul élément du groupe de Lie (de grande dimension) SE 2+p (3), qui constitue l'état, et où les biais des accéléromètres et des gyromètres sont ajoutés à l'état et estimés également. L'efficacité de l'approche est validée à la fois sur des simulations et sur cinq séquences provenant d'un jeu de données réelles.

  at time step n through representation(2.35). Using this embedding, the state at time n can in turn be represented as (χ n , b n ), letting the bias vector be b= h b ωT b aT i T 2 R 6 .The dispersion on χ n can be encoded using the left uncertainty or the right one, leading to two alternative filters (see Section 4.2). In the following, we detail the Right-UKF-LG which adopts the right-equivariant uncertainties of χ n 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 (4.5).

Algorithm 7 :

 7 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 ⌘ ; end Output: χ, b, S; 3.3 3.3 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:

Figure 5 . 1 :Figure 5 . 2 :

 5152 Figure 5.1: Simulation trajectory used in Section 4, and trajectories estimated by the various filters.

Figure 5 . 3 :

 53 Figure 5.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 [106].

Figure 5 . 4 :

 54 Figure 5.4: Root Mean Squared Error on position with respect to ground truth, on five different experiments.

Figure 5 . 5 :

 55 Figure 5.5: Execution times of the different filters, both for the propagation and the update step. The indicated times correspond to the time execution of all sequences averaged for 10 s of flight.

Figure 6 . 1 :

 61 Figure 6.1: The coordinate systems that are used in the chapter. The IMU pose (R, p) maps vectors expressed in the IMU frame to the global frame (G). The transformations T IC and T LR pass, respectively, from the IMU frame to the left camera (C L ) frame, and from the left camera frame to the right camera (C R ) frame. The pose T C i consists of the position of C L in the global frame and the rotation mapping vectors expressed in the C L frame to vectors expressed in the global frame. Unknown 3D features p j (expressed in the global frame) are tracked across a stereo camera system.

Figure 6 . 2 :

 62 Figure 6.2: Root Mean Square Error of the proposed S-UKF-LG and S-IEKF compared to various methods on the EuRoC dataset [106]. Statistics are averaged over ten runs on each sequence.

Figure 6 . 3 :

 63 Figure 6.3: Average CPU load of the proposed S-UKF-LG and S-IEKF compared to various methods on the EuRoC dataset [106].

Figure 6 . 4 :Figure 6 . 5 :

 6465 Figure 6.4: Root Mean Square Error of the proposed S-UKF-LG and S-IEKF compared to various methods on the dataset [108]. Statistics are averaged over ten runs on each dataset.

  Dans ce chapitre, nous proposons une nouvelle méthode précise pour la navigation des véhicules roulant basée uniquement sur une centrake inertielle. Dans le contexte des véhicules autonomes et intelligents, un calcul à la fois robuste et précis basé sur la centrale inertielle peut s'avérer utile pour corréler les flux provenant des capteurs d'imagerie, pour naviguer en toute sécurité à travers lesdifférents obstacles ou pour des arrêts d'urgence sûrs dans le cas extrême de défaillance des capteurs extéroceptifs. Les composants clés de la méthode sont le filtre de Kalman et l'utilisation de réseaux de neurones profonds pour adapter dynamiquement les paramètres de bruit du filtre. La méthode est testée sur le jeu de données KITTI, et notre méthode inertielle d'estimation basée uniquement sur l'IMU estime avec précision la position 3D, la vitesse, l'orientation du véhicule et calibre automatiquement les biais de la centrale inertielle. Nous obtenons en moyenne une erreur de translation de 1,10 % et l'algorithme rivalise avec les méthodes les mieux classées qui, en revanche, utilisent un LiDAR ou de la vision stéréo. Nous mettons notre implémentation open source à l'adresse: https://github.com/mbrossar/ai-imu-dr

Figure 8 . 1 :

 81 Figure 8.1: Trajectory results on seq. 08 (drive #28, 2011/09/30)[START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF] of the KITTI dataset. The proposed method (green) accurately follows the benchmark trajectory for the entire sequence (4.2 km, 9 min), whereas the pure integration of (calibrated) IMU signals (cyan) quickly diverges. Both methods use only IMU signals and are initialized with the benchmark pose and velocity. We see during the GPS outage which occurs in this sequence that our solution keeps estimating accurately the trajectory.

Figure 8 . 2 :

 82 Figure 8.2: The coordinate systems that are used in the chapter. The IMU pose (R imu n , p imu n ) maps vectors expressed in the IMU frame i (red) to the world frame w (black). The IMU frame is attached to the vehicle and misaligned with the car frame c (blue). The pose between the car and inertial frames (R c n , p c n ) is unknown. IMU velocity v imu n and car velocity v c n are respectively expressed in the world frame and in the car frame.

n

  ) where b ω n , b a n are quasi-constant biases and w ω n , w a n are zero-mean Gaussian noises. The biases follow a random walk b ω n+1 = b ω n are zero-mean Gaussian noises. The kinematic model is governed by the following equations

Figure 8 . 3 :

 83 Figure 8.3: Structure of the IEKF. The filter uses the noise parameter N n+1 of pseudomeasurements (8.15) to yield a real time estimate of the state χn+1 along with covariance P n+1 , identically to the conventional EKF.

Figure 8 . 4 :

 84 Figure 8.4: Structure of the proposed system for inertial dead-reckoning. The measurement noise adapter feeds the filter with noise parameter N n computed from raw IMU signals only.

Figure 8 . 5 :

 85 Figure 8.5: Results on seq. 07 (drive #27, 2011/09/30)[START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF]. The proposed method competes with LiDAR and visual odometry methods, whereas the IMU integration broadly drifts after the car's stop.

Figure 8 . 6 :

 86 Figure 8.6: Results on seq. 09 (drive #33, 2011/09/30)[START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF]. The proposed method competes with LiDAR and visual odometry methods, whereas the IMU integration drifts quickly after the first turn.

Figure 8 . 8 :

 88 Figure 8.8: Covariance values computed by the adapter on the highway seq. 01 (drive #42, 2011/10/30)[START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF]. We clearly observe a large increase in the covariance values when the car is turning between t = 90 s and t = 110 s.

Figure 8 . 9 :

 89 Figure 8.9: Results on seq. 02 (drive #34, 2011/09/30)[START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF]. The proposed method competes with LiDAR and visual odometry methods until a problem in data occurs (2 seconds are missing). It is remarkable that the proposed method be robust to such trouble causing a shift estimates but no divergence.

Figure 9 . 1 :

 91 Figure 9.1: Horizontal translation estimates according to ICP ( Ticp , red dots) for various initial estimates (T ini , black dots) and ground-truth (T true , square) for registering two scans of the sequence Stairs of[START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where we sample 1000 initial estimates from two distributions reflecting accurate (a) and dispersed (b) ICP initialization and that respectively correspond to the easy and medium scenarios of[START_REF] Pomerleau | Comparing ICP Variants on Real-world Data Sets: Open-source Library and Experimental Protocol[END_REF]. We see the uncertainty on the ICP estimate, that is, dispersion of red points, wholly depends on the accuracy of initialization. There is no "uncertainty of ICP" per se.

Figure 9 . 2 :

 92 Figure 9.2: Schematic illustration of the ICP procedure, error definitions and linearizations in the case of 1D translation t 2 R (Id = 0).

4 • 10 - 2 Figure 9 . 3 :

 410293 Figure 9.3: Results on real data of[START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF] projected onto the ground plane for visualization. Ellipses represent the 95% (3 ) confidence sets for each uncertainty estimation method. (a): "true convergence situation", the errors are mainly caused by sensor noises and Censi's formula should apply. (b): however we see the Censi ellipse seems optimistic as ground truth is almost outside it whereas it falls well within our ellipse (green). (c, d): "wrong convergence", the large errors are due ICP initialization. Only our approach is consistent with ICP uncertainty in each environment and does not suffer from overoptimism.

Figure 9 . 4 :

 94 Figure 9.4: Results projected onto the ground plane for visualization in the Stairs sequence of[START_REF] Pomerleau | Challenging Data Sets for Point Cloud Registration Algorithms[END_REF], where the "ellipses" (lines) represent the 95% (3 ) final confidence sets.

Figure 10 . 1 :

 101 Figure 10.1: Overview of Kalman filter (left) and neural networks for state estimation (right). Both approaches share same inputs and outputs but differ in their design. A Kalman filter is generally built with domain expertise while neural networks are trained with data.

  Figure 11.1: Trajectory ground truth and estimates obtained by various methods: integration of IMU signals; odometry based on a differential wheel encoder system; odometry combined with an highly accurate and expensive Fiber optics Gyro (FoG) that provides orientation estimates; and the proposed RINS-W approach which considers only the IMU sensor embarked in the vehicle, and which outperforms the other schemes. The final distance error for this long-term sequence urban16 (73 minutes) of the car dataset[START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF] is 20 m for the RINS-W solution. The deep learning based detector (see Section 4.1) has of course not been trained or cross-validated on this sequence.

  Figure 11.2: Real IMU data of a car stopping from sequence urban06 of[START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF]. We see (11.9) holds and thus z vel n =1att =5.8s while (11.10) does not hold yet.

Figure 11 . 3 :

 113 Figure 11.3: Structure of the proposed RINS-W system for inertial navigation. The detector identifies specific motion profiles (11.8) from raw IMU signals only.

Figure 11 . 4 :Figure 11 . 5 :

 114115 Figure 11.4: Structure of the detector, which consists for each motion pattern of a recurrent neural network (LSTM) followed by a threshold to obtain an output vector ẑn that consists of binary components. The hidden state h n of the neural network allows the full structure to be recursive.

Figure 11

 11 Figure 11.6:The considered dataset[START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF] contains data logs of a Xsens MTi-300 2 (right) recorded at 100 Hz along with the ground truth pose.

Figure 11 . 7 :

 117 Figure 11.7: Ground truth and trajectory estimates for the test sequence urban15 of the car dataset[START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF]. RINS-W obtains results comparable with FoG-based odometry.

Figure 11 . 8 :

 118 Figure 11.8: Ground truth and trajectory estimates for the test sequence urban17 of the car dataset[START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF]. RINS-W clearly outperforms the odometry and even the odometry + FoG solution. We note that RINS-W accurately follows the interchange road located at (x =2,y = 1).

Figure 11 . 9 :

 119 Figure 11.9: Comparison on the sequence urban07 between proposed RINS-W, usual integration that only integrates the IMU measurements without applying pseudomeasurement update. The final distance between ground truth and RINS-W estimates is as small as 5 m, whereas ignoring (11.25)-(11.26) yields divergence.

  Ce chapitre propose une méthode d'apprentissage pour le débruitage des gyroscopes des centrales inertielles pour estimer en temps réel l'orientation d'un robot à l'estime. L'algorithme obtenu surpasse l'état de l'art sur les séquences de test. Les performances sont obtenues grâce à un modèle bien choisi, une fonction de coût appropriée pour des incréments d'orientation, et grâce à l'identification de points clés lors de la formulation du problème avec des données inertielles à haute fréquence. Notre approche s'appuie sur un réseau de neurones basé sur des convolutions dilatées, sans nécessiter de réseau récurrent. Nous démontrons l'efficacité de notre stratégie pour l' estimation d'orientation 3D sur les jeux de données EuRoC et TUM-VI. Fait intéressant, nous observons que notre algorithme réussit à rivaliser avec le meilleurs systèmes d'odométrie visuel-inertiel en termes d'estimation d'attitude bien qu'il n'utilise pas de capteurs de vision. Notre implémentation open-source est disponible à l':

Figure 12 . 1 :

 121 Figure 12.1: Schematic illustration of the proposed method. The convolutional neural network computes gyro corrections (based on past IMU measurements) that filters undesirable errors in the raw IMU signals.We then perform open-loop time integration on the noise-free measurements for regressing low frequency errors between ground truth and estimated orientation increments.

Figure 12 . 4 :

 124 Figure 12.4: Horizontal ground truth trajectory for the sequence room 1 of[START_REF] Schubert | The TUM VI Benchmark for Evaluating Visual-Inertial Odometry[END_REF]. Ground truth is periodically absent due to occlusions of the hand-held device from the motion capture system, see the color lines on the right of the figure.

Figure 12 . 5 :

 125 Figure 12.5: Relative Orientation Error (ROE) in terms of 3D orientation and yaw errors on the test sequences. Our method outperforms calibrated IMU and competes with VIO methods albeit based only on IMU signals. Raw IMU and zero motion are way off. Results from OriNet are unavailable.

Figure 12 . 6 :

 126 Figure 12.6: Orientation estimates on the test sequence MH 04 difficult of [106] (left), and room 4 of [211] (right). Our method removes errors of the calibrated IMU and competes with VIO algorithms.

Figure 12 . 8 :

 128 Figure 12.8: Gyro correction ωn on the sequence room 4 of[START_REF] Schubert | The TUM VI Benchmark for Evaluating Visual-Inertial Odometry[END_REF]. We see we manage to divide orientation errors by a factor at least 2 w.r.t. calibrated IMU applying corrections whose amplitude is as low as 1 deg/s (max).

Figure 13 . 1 :

 131 Figure 13.1: Structure of the proposed approach for inertial based navigation, which requires only an IMU. The detector (blue) feeds zero-motion information to the measurement adapter and the filter from raw sensor data. The adapter (green) outputs a vector of observations y n along with its uncertainty covariance matrix cov (y n ), both are inputs of the filter (red) that is in charge of state estimation.

Figure 13 . 2 :

 132 Figure 13.2:The approach is evaluated on three distinct datasets: from left to right, images of the vehicle in the publicly available KAIST dataset[START_REF] Jeong | Complex Urban LiDAR Data Set[END_REF][START_REF] Jeong | Complex Urban Dataset with Multi-Level Sensors from Highly Diverse Urban Environments[END_REF], the Lille dataset which is publicly available for point cloud classification, see[START_REF] Roynard | Paris-Lille-3D: A Large and High-Quality Ground-Truth Urban Point Cloud Dataset for Automatic Segmentation and Classification[END_REF][START_REF] Roynard | Paris-Lille-3D: A Point Cloud Dataset for Urban Scene Segmentation and Classification[END_REF], and our Magny-Les-Hameaux (MLH) dataset. Each dataset disposes among other of IMU data, groundtruth poses and refers to diverse vehicle dynamics and environments.

Figure 13 . 16 :

 1316 Figure 13.16: Trajectory estimates on the sequence urban10 of the KAIST dataset (14 km, 40 min). IMU keeps accurate methods and follows many corner bends.

Figure 13 . 17 :

 1317 Figure 13.17: Trajectory estimates on the sequence urban07 of the KAIST dataset (2 km, 9 min). IMU is especially accurate on smooth trajectories like in this apartment complex area.

Figure 13 . 18 :Figure 13 . 19 :Figure 13 . 20 :

 131813191320 Figure 13.18: Trajectory estimates for the sequence 07 on the Lille dataset (6 km, 23 min). IMU keeps accurate after parking slot.

Figure 15 . 1 :

 151 Figure 15.1: Overview of the proposed approach in a general situation. A neural network learns to regress observations from sensor inputs, which generally requires ground truth during training. A Kalman filter leverages learned observation to obtain reliable state estimates.

5 P

 5 n|n = (I KH) P n|n 1 (I KH) T + K cov (y n ) K T ; // Joseph form

  .

2 .Figure 15 . 2 :

 2152 Figure 15.2: Overview of the proposed approach applied to the considered attitude estimation problem. A convolutional neural network learns to regress the gravity direction from inertial signals, which requires ground truth orientation during training.An invariant filter leverages estimated gravity direction with dead-reckoning orientation from gyro to obtain accurate estimates.

Figure 15 . 3 :Figure 15 . 4 :

 153154 Figure 15.3: IMU measurements and dead-reckoning estimations. The IMU behavior is strange. Indeed, the IMU is is uncalibrated, and should be calibrated to correspond to the filter assumption.

Figure 15 . 5 :

 155 Figure 15.5: Accelerometer measurements nd true gravity direction along time on the sequence V1 01 easy of [106]. The neural network (blue) correctly estimates gravity direction.

  Algorithm 1: UKF computation of Fn and ĜnInput: xn+1|n , xn|n , P n|n , u n , G n ,↵; // Sigma-point parameters 1 , {w j } j=0,...,2d = set_weights(dim(P n|n ),↵);

	2 ξ j = col(	p	( + d)P n|n ) j ,j=1,...,d
		ξ j = col( p ( + d)P n|n ) j d ,j= d +1,...,2d// sigma point errors
		// add sigma points on state and propagate them
	3	xj n = xn|n + ξ j ,j =1,...,2d;
	4	xj n+1 = f (x j n , u n , 0),j =1,...,2d; // Compute cross-covariance
	5 Σ = 6 Fn = ΣP 1 P 2d j=1 w j (x n+1|n n|n // Infer Jacobian xj n+1 )(x n|n	xj n ) T ;
		// Proceed similarly for noise
	7	, {w j } j=0,...,2d = set_weights(dim(Q),↵);
	8 w j = col(	p ( + d)Q) j ,j=1,...,d
		w j = col( p	( + d)Q) j d ,j= d +1,...,2d;
	10 Σ =	P 2d j=1 w j (x n+1|n	xj n+1 )(w j )

9 xj n+1 = f (x n|n , u n , w j ),j =1,...,2d; T ; 11 Ĝn = ΣQ 1 // Infer Jacobian Output: Fn , Ĝn

  Proposition 1 proved that if the system (3.5)-(3.6) is totally invariant, then the directions infinitesimally spanned by • (•) at any point χ n 0 lie in Ker O(χ n 0 ), with O(χ n 0 ) defined by

	Algorithm 4: EKF based on a non-linear state error
	Input: Initial estimate χ0 and uncertainty matrix P e 0 while filter is running do
	Propagation
	1	χn|n 1 = f	⇣	χn 1|n 1 , u n , 0 ⌘ ;
	2	P e n|n 1 = Fe n P e n 1|n 1 ( Fe n ) T + Ĝe n Q n ( Ĝe n ) T ;
	end
	Update
	3	K n = Ĥe n P e n|n 1 /	⇣ Ĥe n P e n|n 1 ( Ĥe n ) T + Ĵe n R n ( Ĵe n ) T	⌘	;
	4	e + n = K n	⇣ y n h	⇣	χn|n 1 , 0 ⌘⌘ ;
	5	χn|n = '	⇣	χn|n 1 , e + n	⌘ ; // state update
	6	P e n|n =	⇣ I K n	Ĥe n	⌘	P e n|n 1 ;
	end
	end
	Assumption 1. The image of the matrix @ @↵ | Id ⌘( ↵ (χ), χ) is a fixed subspace C that does not depend on χ.

  The Lie algebra se(3) of SE(3) encodes small rigid motions about the identity. It is defined as {( ω ⇥ , p) ; ω, p 2 R 3 }, where ω ⇥ is the skew symmetric matrix associated with cross product with ω 2 R 3 . For any ↵ 2 se(3), we have ↵ := exp SE(3) ( ↵) 2 SE[START_REF] Van Der Merwe | Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[END_REF]

	.22)
	Assumption 2. The vehicle's dynamic does not depend on the choice of global frame, and
	the vehicle's sensors only have access to relative observations, i.e. no absolute information is
	available.
	As a result, the equations write (3.5)-(3.6) and are invariant to the action of SE(3)
	in the sense of Definition 3.
	To differentiate w.r.t. elements of SE(3) we resort to its Lie algebra and do in detail
	what is sketched in Footnote 1.
	Definition 5. where exp SE(3) (•) denotes the exponential map of SE(3) (for a definition see (3.34)-(3.36) below with l

  Tangent vector Rω ^indeed writes ! 1

	5) is not Gaussian. It is "only" Gaussian in
	coordinates related to map '(•).
	Example 11 (R p ). Consider Example 7. We naturally recover the addition operator on
	vector space, i.e. '(x, ξ)=x + ξ.
	Example 12 (SO(3)). Consider Example 9. Recall tangent vectors at R indicate small mo-
	tions around R 2 SO(3).

  Snippet 3: defining ' 1 (•) via (4.18) or (4.19) for χ 2 SE 2 (3)

	def phi_inv(state, hat_state):
	chi = state2chi(state)
	hat_chi = state2chi(hat_state)
	# if left multiplication (3.18)
	return SEK3.log(SEK3.inv(hat_hat).dot(chi))
	# if right multiplication (3.19)
	return SEK3.log(chi.dot(SEK3.inv(hat_hat)))
	error position norm (m)	0 0.5 1 1.5	0	5	10	15 EKF IEKF	20 SO(3) × R 6 UKF 25 SE 2 (3) UKF	30
						t (s)

Table 8 .

 8

	test seq.	length (km)	dura -tion (s)	IMLS r rel (%) (deg/km) t rel	ORB-SLAM2 t rel r rel (%) (deg/km)	IMU r rel (%) (deg/km) t rel	proposed t rel r rel (%) (deg/km)
	01	2.6	110	0.82	1.0	1.41	1.9	5.35	1.2	1.56	1.2
	03	-	80	-	-	-	-	-	-	-	-
	04	0.4	27	0.33	1.2	0.47	2.2	0.97	1.0	1.22	0.4
	06	1.2	110	0.33	0.8	0.73	2.2	5.78	1.9	1.57	1.9
	07	0.7	110	0.33	1.5	0.91	4.9	12.6	3.0	1.32	3.0
	08	3.2	407	0.80	1.8	1.03	3.0	549	5.6	1.08	3.2
	09	1.7	159	0.55	1.2	0.81	2.3	23.4	3.5	0.82	2.2
	10	0.9	120	0.53	1.7	0.66	3.1	4.58	2.5	1.05	2.5
	average scores	0.64	1.2	0.99	2.6	171	3.1	0.97	2.3

Dropout refers to ignoring units of the adapter during training, and 1: Results on

Table 8 .

 8 2: Results on[START_REF] Geiger | Vision Meets Robotics: The KITTI Dataset[END_REF] on seq. 00, 02 and 05. The degraded results of the proposed method are wholly explained by a problem of missing data, see Section 6.3 and Figure8.10.pose of filter's performance enhancement. Indeed, such a large noise parameter inflation indicates the AI-based part of the algorithm has learned and recognizes that pseudo-measurements have no value for localization at those precise moments, so the filter should barely consider them. Note that, if for some reason the practitioner wants to keep the noise covariance parameter N n at more realistic values at all times, for instance in the context of fusion with other sensors, it is always possible to turnoff pseudo-measurements or enforce a predefined upper bound on N n that feeds the Kalman filter when the AI-based adapter goes above the predefined bound.

	test seq.	length (km)	dura -tion (s)	IMLS r rel (%) (deg/km) (%) (deg/km) (%) (deg/km) (%) (deg/km) ORB-SLAM2 IMU proposed t rel t rel r rel t rel r rel t rel r rel
	00	3.7	454	0.50	1.8	0.84	2.9	426	46.8	6.81	24.8
	02	5.1	466	0.53	1.4	0.79	2.7	346	8.7	3.37	3.8
	05	2.2	278	0.32	1.4	0.55	2.2	189	5.2	3.05	8.7

  relative pose estimate: trel = f (t true t ini , w)iii) ICP final pose estimate: ticp = t ini + trel

	Id	t ini	ξ ini	t true
	Id	t ini	t rel = ξ ini	t true	ticp
			trel ' Id + J(t true t ini )+Gw
			' Id Jξ ini + Gw
	Id			t true	ticp
			ξ icp = ticp t true
					' (I J)ξ ini + Gw

Table 9 .

 9 j=1,...,12; trans. rot. trans. rot. trans. rot. trans. rot.

	3	ξj icp = exp 1	✓	T 1 icp	Tj icp	◆	,j=1,...,12;
		// compute covariance and infer J
	4 (I 6 J)Q ini (I 6 J) T =	P 12 j=1	1 12	ξj icp	ξjT icp ;
	5 ξicp =	P 12 j=1	1 12	ξj icp ;
	6 J =	✓ P 12 j=1	1 12	✓	ξj icp		ξicp	◆	ξ	jT ini	◆	Q 1 ini + I 6 ;
		Output: J, (I 6 J)Q ini (I 6 J) T ;
				metric KL div.Qcensi NNE KL div. NNE* 10 3 10 3 10 4 10 5 38 10 2 10 3 10 5 Qmonte 10 3 10 2 10 4 10 4 22 20 10 3 10 3 carlo proposed 4.2 34 10 2 10 2 0.8 3.8 31 98

* 1: Results of ICP uncertainty estimation in term of Normalized Norm Error (NNE) and Kullbach-Leibler divergence (KL div.

Table 9 .

 9 Qini (algo) easy med. diff. easy med. diff. easy med. diff. 3: NNE, see(9.16), for different levels of true and supposed initial uncertainty.

	true)		easy			medium		difficult
	Appart.	1.6	1.5	10 -6	1.8	1.7	10 -6 10 5	10 5	0.6
	Haupt.	1.8	0.3	10 -3	3.6	0.7	10 -3 10 3	10 3	0.3
	Stairs	0.7	0.2	10 -3	1.9	0.8	10 -3 10 3	10 3	0.7
	Montain	2.1	1.1	10 -3	3.3	1.8	10 -3 10 3	10 3	4.0
	Gazebo	1.1	1.0	10 -5	1.3	1.2	10 -5 10 4	10 4	0.8
	Wood	2.1	2.1	10 -3	2.2	2.2	10 -3 10 3	10 3	0.3

  =0.01 rad/s, a =0.2m/s 2 , b ω =0.001 rad/s, b a =0.02 m/s 2 for the noise propagation covariance matrix Q n , and vel,v =1m /s, vel,a =0 .4m/s 2 , ang = 0.04 rad/s, lat =3m/s, and up =3m/s for the noise measurement covariance matrix N n .

	2 vel,a I, 2 ang I, 2 lat , 2 up	⌘	,	(11.32)
	where we set ω

Table 11 .

 11 1: Results obtained by the 3 methods on urban test sequences 15, 16, 17 in terms of: m-ATE /aligned m-ATE / final distance error to ground truth, in m. Last line is the concatenation of the three sequences. Direct IMU integration always diverges. The proposed RINS-W outperforms differential wheel speeds based odometry and outperforms on average (see last line) the expensive combination of odometry + FoG. Indeed, RINS-W uses an IMU with gyro stability of 10 deg /h, whereas FoG stability is 0.05 deg /h. use a threshold of 0.005 rad/s for the angular velocity, and a threshold of 0.1m/s for the lateral and upward velocities.

	we consider z vel n = 1 if kv imu n k < 0.01 m/s. We set similarly the other motion profiles and

  /4.10 3 48.10 4 / 7.10 2 true negative / false neg. 16.10 4 /1.10 4 16.10 4 / 9.10 3

	z vel n detection	ideal AMVD our detector
	true positive / false pos. 47.10 4 precision / recall 0.974 / 0.940	0.996 / 0.940
	Table 11.2: Results on zero velocity

  Figure 12.2: Proposed neural network structure which computes gyro correction ωn in (12.5) from the N past IMU measurements. We set the Convolutional Neural Networks (CNNs) as indicated in the table, and define between two convolutional layers a batchnorm layer

						ωn
	n
	CNN layer #	1	2	3	4	5
	kernel dim.	7	7	7	7	1
	dilatation gap	1	4	16	64	1
	channel dim.	16	32	64	128	1

δ

  R0,1 δ R1,2 δ R2,3 δ R3,4 δ R4,5 δ R5,6 δ R6,7 δ R7,8

	1
	δ R0,2	δ R2,4	δ R4,6	δ R6,8
	2
	δ R0,4		δ R4,8
	3
		δ R0,8
	Figure 12.3: Time efficient computation of the loss (12.9

Table 12 .

 12 1: Absolute Orientation Error (AOE) in terms of 3D orientation/yaw, in degree, on the test sequences. We see our approach competes with VIO (while entirely based on IMU signals) and outperforms other inertial methods. (*) Note that, results from OriNet correspond instead to the mean orientation error P M n=1 k log(R T n Rn )k 2 /M which is by definition always less than AOE criterion, and are unavailable on the TUM-VI dataset."learning"). The test set is the gold standard to provide an unbiased evaluation. It is only used once training (using the training and validation sets) is terminated. The datasets we use are as follows.

	(proposed)

Table 12

 12 .1 and Figure12.5. Figure12.6 illustrates roll, pitch and yaw estimates for a test sequence of each dataset, and Figure12.7 shows orientation errors. We note that: Uncalibrated IMU is Unreliable: raw IMU estimates deviate from ground truth in as low as 10 s, see Figure12.6, and are barely more reliable than null rotation assumption.Calibrated IMU Outperforms OriNet: only calibrating an IMU (via our optimization method) leads to surprisingly accurate results, see e.g., Figure12.6 (right) where it is difficult to distinguish it from ground truth. This evidences cheap sensors can provide very accurate information once they are correctly calibrated.The Proposed Method Outperforms Inertial Methods: OriNet[START_REF] Esfahani | OriNet: Robust 3-D Orientation Estimation With a Single Particular IMU[END_REF] is outperformed. Moreover, our method improves accurate calibrated IMU by a factor 2 to 4. Our approach notably obtains as low as a median error of 1.34 deg /min and 0.68 deg /min on respectively EuRoC and TUM-VI datasets.

Table 13 .

 13 5: Results on different datasets in terms of mean-Absolute Trajectory Error (ATE), aligned mean-Absolute Trajectory Error (a-ATE) and mean-Absolute Velocity Error (AVE), where we discard INS results since purely inertial integration often diverges. IMU performs particularly well in cities on the KAIST and Lille datasets, keep robust on our challenging MLH dataset, and never diverge. They completely outperform standard wheel odometry (odo.).

	dataset	length (km)	duration (min)	environment
	KAIST	235	518	urban, highway
	Lille	28	132	urban
	MLH (ours)	103	242	country
	total	366	892 (⇠15 h)
	Table 13.1: Description of the considered datasets. They dispose all of IMU, centimeter
	position ground-truth, represent various vehicle dynamics, sensors, and consist of a
	tremendous amount of data targeting diverse environments.

, and our Magny-Les-Hameaux (MLH) dataset. Each dataset disposes among other of IMU data, groundtruth poses and refers to diverse vehicle dynamics and environments.

Table 13 .

 13 6: Results on the KAIST dataset, where we discard INS results since purely inertial integration often diverges. IMU outperforms odometry based on wheels. The vehicle starts by a minimum of 5 s stop in all these sequences that is sufficient for the proposed method to self-initialize orientation and sensor biases.

	Evaluation Metrics and Compared Methods: To assess performances we consider
	three error metrics:

Table 13 .

 13 7: Results on the KAIST dataset. We discard INS results since purely inertial integration often diverges. The proposed IMU outperforms the odometry. The vehicle is always moving in these sequences and the proposed method manage to early initialize sensor biases and state error covariance which constitutes itself a notable feature.

					INS			IMU
	test seq.	length duration	ATE	a-ATE AVE ATE a-ATE AVE
		(km)	(min)	(m)	(m)	(m/s)	(m)	(m)	(m/s)
	00	6	23	10.10 3 7.10 3	22	8	4	0.12
	01	2	10	6.10 3	5.10 3	33	4	1	0.07
	02	1	6	10 3	10 3	6	2	2	0.13
	03	4	21	18.10 3 13.10 3	66	14	12	0.14
	04	4	23	25.10 3 22.10 3	74	42	25	0.09
	05	4	22	23.10 3 23.10 3 105	8	5	0.10
	06	1	6	10 3	10 3	8	2	1	0.07
	07	6	21	32.10 3 24.10 3 103	16	12	0.16
	average	28	132	18.10 3 15.10 3	64	7.1	4.2	0.12

Table 13 .

 13 8: Results on the Lille dataset. Purely inertial integration (INS) tends to diverge, whereas the proposed IMU keep accurate for the longest 20 min long sequences. Both methods uses only the IMU. The proposed method particularly well estimates curved bends and roundabouts, see Figure 13.18.

					odo.			IMU
	test seq.	length duration	ATE a-ATE AVE	ATE a-ATE AVE
		(km)	(min)	(m)	(m)	(m/s)	(m)	(m)	(m/s)
	00	38	70	5700	300	10	151	49	0.48
	01	27	71	2100 1400	50	106	42	0.34
	02	38	101	2000	800	7	102	44	0.21
	average	103	242	3100	831	20	117	45	0.33

Table 13 .

 13 9: Results on our specifically dedicated Magny-Les-Hameaux (MLH) dataset. We discard INS results since purely inertial integration often diverges. The proposed IMU outperforms the odometry. Such results are impression on this countryside dataset which contains many speed bump and wheel slip in long sequences at relatively high speed (25 m/s).

		5				ground-truth
						INS
						odo.
						IMU
		4
		3
	y (km)	2
		1
		0
		-3	-2	-1	0	1
				x (km)
	Figure 13.15: Trajectory estimates on the sequence 01 on the MLH dataset (27 km,
	71 min). IMU follows ground-truth for this particularly long experiment.

Table 13 .

 13 10: Results of various classifiers for detecting zero velocity z vel n in term of standardized partial Area Under Curve (AUC) with maximal false positive rate 0.1. More the AUC is, better is the detector. The proposed detectors outperforms their conventional counterparts. All methods excepted used only IMU signal.

	dataset	ideal AMVD ideal ARED IMU
	KAIST	0.943	0.986	0.993
	Lille	0.715	0.950	0.957
	Magny-Les-Hameaux	0.935	0.974	0.995
	average score	0.907	0.977	0.988

  •) the SO(3) exponential map, seeChapter 12. The IMU is assumed unbiased and measurements are given as ⇠N(0, Q n ) is Gaussian noise. The model (15.2) assumes the sensor is calibrated and does contain the remaining parameters of Chapter 12.Problem Statement: given an initial attitude R 0 , performs in real time IMU attitude estimation.2.1 2.1 The Invariant Kalman Filter for Attitude EstimationTo obtain reliable estimates, we design an invariant Kalman filter based on the propagation model (15.1) and observation of gravity direction. Indeed, if velocity variation is negligible w.r.t. gravity, the accelerometer measures Algorithm 11: invariant Kalman filtering for attitude estimation // State and covariance propagation 1 Rn|n 1 = Rn 1|n 1 exp (ω imu n ); 2 P n|n 1 = P n 1|n 1 + Q n ; // Measurement update

	and we choose to consider the normalized acceleration measurement
	a imu n ka imu n k = R T y n = n 2 6 6 6 6 6 6 6 4 3 0 0 1 7 7 7 7 7 7 5 7	= R T n g 0 ,	(15.4)
	which contains orientation information around the gravity g. Building on the right-
	invariant error
		R n = ( Rn , ξ n )	(15.5)
		= exp(ξ n ) Rn ,	(15.6)
	see Section 2 of Chapter 2, we obtain a filter whose equation are recapped in Algorithm
	11, where
		H =	2 6 6 6 6 6 6 6 4	0 100 10 000	3 7 7 7 7 7 5 7 7	.	(15.7)
	"	ω imu n a imu n	#	=	"	ω n a n	#	+ w n ,	(15.2)
	where w n a imu n ⇡ R T n g,	(15.3)

3 K = P n|n 1 H T ⇣ HP n|n 1 H + cov (y n ) ⌘ 1 ; // Kain 4 Rn|n = exp ⇣ Rn|n 1 y n g 0 ⌘ Rn|n 1 ;

  sequence w/o calibration w/o initializing w l 2 loss baseline proposed Figure 15.6: Results in term AOE when filter inputs are provided by calibrated IMU.

	MH 02	15.4	1.36	1.31	7.69	1.30
	MH 04	14.6	1.35	1.27	5.24	1.25
	V1 01	10.9	1.14	1.14	2.13	1.15
	V1 03	8.9	2.70	2.60	3.96	2.54
	V2 02	7.7	3.65	3.30	4.85	3.35
	average	11.5	2.04	2.10	4.76	1.91

  MOTS CLÉSfusion de capteurs, filtre de Kalman, SLAM, navigation, centrale inertielle, apprentissage profond RÉSUMÉ Cette thèse aborde les méthodes d'estimation d'état pour les véhicules équipés de divers capteurs tels que des caméras et des centrales inertielles. Le filtre de Kalman est un outil largement utilisé pour estimer l'état d'un système dynamique, qui soulève des questions théoriques pour les systèmes non linéaires présents dans la navigation, et qui repose sur des modèles physiques et des paramètres qui doivent être optimisés efficacement par l'utilisateur. La thèse contribue au filtrage de Kalman pour la navigation, où les contributions se divisent en deux contributions majeures. La première contribution consiste à s'appuyer sur le récent filtre de Kalman étendu invariant pour résoudre les problèmes difficiles que sont l'inconsistence du filtre de Kalman étendu pour le problème de la localisation et de la cartographie simultanées, de la navigation avec des capteurs visuels, et la question plus générale du filtrage de Kalman sur les variétés. La deuxième contribution consiste à utiliser des outils récents du domaine de l'intelligence artificielle, à savoir l'apprentissage profond, pour améliorer les filtres de Kalman, notamment pour relier les mesures des capteurs à l'état du système, et pour régler le filtre efficacement, c'est-à-dire utiliser des techniques d'apprentissage automatique pour trouver une stratégie de réglage dynamique des paramètres du filtre de Kalman qui correspond aux données et qui est également capable de fournir de nouvelles informations au filtre. La thèse introduit ainsi différents algorithmes de filtrage et réseaux de neurones profonds dont l'implémentation est rendue open-source.

	p n p n + 2 n	H T	(15.15)
	P + n = (I KH) P n ) p + n = p n (1 p n /(p n + r n )) p + n = p n r n n p n + 2 ,	(15.16) (15.17)
	which conclude the proof.

[START_REF] Bonnabel | Observateurs asymptotiques invariants: théorie et exemples[END_REF]

And at update, we compute

S = HP n H T + cov(y n ) ) S =(p n + 2 n )I 2

(15.14)

K = P n H T S 1 ) K =

it is only in the second and third parts of this thesis that we will question the mathematical models used for state estimation, and see how we can use machine learning to refine them in the face of collected data.

i.e. ⌘( ↵ (χ), χ) is a matrix-valued map g(χ,↵), differentiated w.r.t. ↵.

Generalizations to the Stiefel manifold St(p,n), that is, a set of p orthonormal vectors of R n , and hence to the set of p-dimensional subspaces of R n called the Grassmann manifold are then straightforward.

https://pytorch.org/

https://www.oxts.com/

https://www.xsens.com/

https://pytorch.org/

https://www.kvh.com

https://www.safran-group.com

https://www.safran-electronics-defense.com/security/navigation-systems

https://pytorch.org/cppdocs/

List of Publications

This thesis lead to the publication of four peer-reviewed international journal papers and six articles in the proceedings of international peer-reviewed conferences. We now need to assess the evolution of state variables (8.8). The evolution of R imu n , p imu n , v imu n , b ω n and b a n is already given by the standard equations (8.3)-(8.7), leading to the input

The additional variables R c n and p c n represent the car frame with respect to the IMU. This car frame is rigidly attached to the car and encodes an unknown point where the pseudo-measurements of Section 4.2 are most advantageously made. In [START_REF] Kilic | Improved Planetary Rover Inertial Navigation and Wheel Odometry Performance through Periodic Use of Zero-Type Constraints[END_REF] this point is located in the rear wheel, but in our approach we let the algorithm find its location to enhance performances. As IMU is also rigidly attached to the car, and R c n , p c n represent misalignment between IMU and car frame, they are approximately constant:

)

n . (8.13) where we let w R c n , w p c n be centered Gaussian noises with small covariance matrices R c I, p c I that will be learned during training. Although those variables are bounded in practice, we model them as random walks to ensure good tracking by the Kalman filter, as is usually done in the navigation literature regarding IMU biases (see (8.3)- (8.4), that are also bounded in practice). Noises w R c n and w p c n encode possible small variations through time of level arm due to the lack of rigidity stemming from dampers and shock absorbers.

Defining the Pseudo-Measurements h(•)

Consider the different frames depicted on Figure 8.2. The velocity of the origin point of the car frame, expressed in the car frame, writes

from basic screw theory. In the car frame, we consider that the car lateral and vertical velocities are roughly null, that is, we generate two scalar pseudo observations of the form (8.10)

where the noises

i T are assumed centered and Gaussian with covariance matrix N n 2 R 2⇥2 . The filter is then fed with the pseudo-measurement that

Assumptions that v lat n and v up n are roughly null are common for cars moving forward on human made roads or wheeled robots moving indoor. Treating them as loose constraints, i.e., allowing the uncertainty encoded in N n to be non strictly null, leads to much better estimates than treating them as hard constraints, that is, N n = 0 [START_REF] Wu | VINS on Wheels[END_REF]. These assumptions are statistically true but does not hold for a given instant.

It should be duly noted the vertical velocity v up n is expressed in the car frame, and thus the assumption it is roughly null generally holds for a car moving on a road even if the motion is 3D. It is quite different from assuming null vertical velocity in the world frame, which then boils down to planar horizontal motion.

Part III Using Deep Learning to Extract

Information from an IMU CHAPTER 15

Appendix: Enhancing Kalman Filter with Deep Learning in Practice (Experience Feedback)

Introduction

This chapter presents a step-by-step guide for adding or improving an observation of a Kalman filter with deep learning. This part is build on the accumulation of acquired experiences in the previous chapters of Part II and Part III. We write this chapter in a tutorial and easy-to-follow form for the one that searches to improve its estimation algorithm with deep learning on the real but simple attitude estimation problem on the EuRoC dataset [START_REF] Burri | The EuRoC Micro Aerial Vehicle Datasets[END_REF].

Retrospective Analysis

The results that we acquired in this thesis are demonstrative research works and there are still a gap between these algorithms, and a robust industrial solution.

It appears retrospectively that the zero velocity detector (see Chapter 11) and a neural network for denoising gyro (see Chapter 12) are closer to a concrete application than the works we presented in Chapter 8 and in [START_REF] Brossard | Learning Wheel Odometry and IMU Errors for Localization[END_REF]. Indeed, the first ones:

• are efficient to train and fine-tune, e.g. Chapter 12 trains neural networks in less than five minutes, and scales well with large amount data;

• are analyzable as we can compare the network outputs and the expected results, and see where results may be improved;

• and constitutes in itself a standalone block. Chapter 11 proposes a neural network to detect a car stop, that is invariant w.r.t. ad hoc filter modifications.

In contrast, the second part of works, albeit promising, are much difficult to generalize and optimize to large experiments, see similar works in [START_REF] Peretroukhin | Inferring Sun Direction to Improve Visual Odometry: A Deep Learning Approach[END_REF][START_REF] Maio | On Learning Visual Odometry Errors[END_REF]. Chapter 8 designs a neural network that must be adapted each time the filter changes, and [START_REF] Brossard | Learning Wheel Odometry and IMU Errors for Localization[END_REF] uses heavy and cumbersome deep Gaussian processes which are difficult to train.

Goals and Organization of the Chapter

In the following, we present an approach that should be used for enhancing a Kalman filter with deep neural networks. We consider the problem of attitude estimation from ABSTRACT This thesis deals with state estimation for vehicles that are equipped with various sensors such as cameras and inertial measurement units. The Kalman filter is a widely used tool that estimates the state of a dynamical system, which raises theoritical questions for the nonlinear systems present in navigation, and that relies on physical models and parameters that need to be efficient tuned by the user. This thesis contributes to Kalman filtering for navigation, where the contributions focus on two different manners. The first manner consists in building on the recent invariant extended Kalman filter to address challenging issues, namely the inconsistency of extended Kalman filter for the problem of simultaneous localization and mapping, navigation with vision sensors, and the more general question of Kalman filtering on manifolds. The second manner consists in using recent tools from the field of artificial intelligence, namely deep learining, to improve Kalman filters, notably to relate sensors' measurements to the state, and to tune the filter efficiently, that is, using machine learning techniques to find a dynamical tuning strategy of the Kalman filter parameters that best matches the data and that is also able to provide new information to the filter. The thesis thus introduces differents filtering algorithms and deep neural networks whose implementation are made open-source.

KEYWORDS

Sensor fusion, Kalman filter, SLAM, navigation, IMU, deep learning