Chapter 1 Introduction 1.1 Estimating the state of a dynamical system

Estimating unknown state of a dynamical system from indirect and imprecise measurements is essential in almost all technological fields. Navigation of aircrafts, weather forecast, autonomous vehicles control, underwater exploration, oil drilling and medical imaging to cite a few. From a mathematical point of view, if the state changes over time due to human intervention or physical laws, the problem is usually set down under the following general form:

d dt X = f u (X, w), Y = h(X,V ).
(1.1)

X is the information of interest, that is, the state: orientation and position of a flying aircraft, air masses situation in meteorology, environment of a vehicle in robotics, etc. Its evolution over time, represented by the function f , depends on its current value X, some additional known parameters u, and some unknown, unpredictable, erratic elements denoted by w. If X is the state of a plane (its heading, velocity, position, etc.), f can represent the (known) laws of Mechanics, u the (known) velocity of the propeller and w all the uncertainties over movements: wind variations, imperfect operating of the piloting commands, approximate knowledge of the aerodynamics of the aircraft, etc.

Y denotes here all the measured variables. Through h, they are defined as a function of X, i.e. of the state of interest. Y can be for example a GPS signal received by a plane: it gives only the position, from which course and attitude have still to be deduced. Moreover, this observation also depends on a set of unsteady and unpredictable parameters denoted by V (GPS errors). The two main approaches to this kind of problems are probabilistic filtering and deterministic observers.

Probabilistic filtering

Probabilistic filtering is a branch of stochastic process theory that has been strongly driven by applications in control and signal processing. It aims at computing the expecta-tion (or mean value) X of the state X defined above conditioned on the past information, which requires estimating its whole probability density function (p.d.f.). In the particular case where the system is linear and corrupted by independent white Gaussian process and observation noises, this function is Gaussian. This implies it can be fully described by its mean value and covariance matrix. The evolution of these two variables follows the equations of the celebrated Kalman filter [START_REF] Kalman | New results in linear filtering and prediction theory[END_REF]. Yet, when the system is non-linear there is no general method to derive efficient filters, and their design has encountered important difficulties in practice. Indeed, general filtering equations describing the evolution of the p.d.f. (see e.g., [START_REF] Burkholder | École d'été de probabilités de Saint-Flour XIX[END_REF] and references therein) concern a function and cannot be reduced to a finite-dimensional computation.

For this reason, many approximation-based solutions have been proposed. In engineering applications the most popular of them is the extended Kalman filter (EKF). This method amounts to linearize the system around the estimated trajectory, and build a Kalman filter for the linear model, which can in turn be implemented on the non-linear model. One merit of the EKF is to convey an estimation of the whole density probability of the system through a mean state Xt and an approximation of the covariance matrix P t of the error. These can be used to draw "uncertainty ellipsoids", that is, sets likely to contain the true value of X. They have to be seen as the confidence of the algorithm in its estimate X(t): the true state is not supposed to be exactly X(t) but at least to be in the area defined by the ellipsoid. They are hoped to be representative of the reality (i.e. to contain the true value of the state), but as they are obtained by ways of linearization, this has no reason to be true. A good illustration of this issue is given by Simultaneous Localization and Mapping (SLAM) problem. This problem consists for a robot in building a map of its environment while localizing itself in this map. A theoretical study performed in [START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF], as well as convincing simulations [START_REF] Bailey | Consistency of the ekf-slam algorithm[END_REF], showed that for this problem the EKF eventually becomes inconsistent, in the sense that the true state has excessive probability to be outside the trusted ellipsoid. This may be harmful as it may for instance result in a collision by misestimating the distance from the robot to an obstacle. In particular, the estimated uncertainty over heading decreases although the general orientation of the map and robot are not observable.

In order to capture more closely the distribution, several rather computationally extensive methods have recently attracted growing interest. Unscented Kalman Filter [START_REF] Julier | A new extension of the Kalman Filter to Nonlinear Systems[END_REF] proposes to compute the covariance matrices appearing in the EKF using a smart deterministic sampling. In most cases (see [START_REF] Eric | The unscented kalman filter for nonlinear estimation[END_REF]) the results obtained are better than those of the EKF but [START_REF] Bailey | Consistency of the ekf-slam algorithm[END_REF] showed it does not fix the problems of EKF SLAM inconsistency. Particle filters [START_REF] Cappé | An overview of existing methods and recent advances in sequential monte carlo[END_REF][START_REF] Neil J Gordon | Novel approach to nonlinear/non-gaussian bayesian state estimation[END_REF] generate a random set of points (the "particles") following a procedure making the result (almost) equivalent to a direct sampling of the p.d.f of interest. Then, the expectation of any useful quantity (the state for instance) can be evaluated as its mean value on the obtained points. With a sufficient number of particles, the result can be made as accurate as wanted. The main drawback of such an approach is the growing size of the sample needed as the dimension of the system increases. In particular, too a sparse sampling compared to the dimensionality of the problem leads to a type of degeneracy: no particle matches the data, even approximately, and the weights vanish. Several solutions have been proposed to tackle this issue, the two main ideas being either to use the observations to smartly propagate the particles [START_REF] Simandl | Sampling densities of particle filter: a survey and comparison[END_REF], or to sample only a reduced set of relevant variables then use a classical filtering method for each particle to marginalize out the remaining variables. Of course, both can be used simultaneously. The latter is known as the Rao-Blackwellized Particle Filter [START_REF] Doucet | Rao-blackwellised particle filtering for dynamic bayesian networks[END_REF]. Only some of the variables forming the state X are sampled, so that the evaluation of the remaining variables boils down to a linear problem. This residual estimation can be performed using classical Kalman filtering. This is the solution proposed in [START_REF] Montemerlo | Fastslam: A factored solution to the simultaneous localization and mapping problem[END_REF] to the non-linearity issues arising in SLAM: only the heading of the robot is sampled, and for each obtained value a Kalman filter builds the map and localizes the robot.

Filtering on manifolds If the system is naturally defined on a manifold (consider for instance 3D attitude estimation, where the state of the system is a rotation matrix), many works have sought to avoid parameterization and the implied distortions and singularities [START_REF] Duncan | Stochastic systems in Riemannian manifolds[END_REF][START_REF] Itô | Stochastic differential equations in a differentiable manifold[END_REF][START_REF] Ng | Nonlinear filtering in Riemannian manifolds[END_REF]. The specific situation where the process evolves in a vector space but the observations belong to a manifold has also been considered, see e.g. [START_REF] Duncan | Some filtering results in Riemann manifolds[END_REF][START_REF] Pontier | Filtering on manifolds[END_REF] and more recently [START_REF] Said | On filtering with observation in a manifold: Reduction to a classical filtering problem[END_REF]. For systems on Lie groups powerful tools to study the filtering equations (such as harmonic analysis [START_REF] Lo | Optimal estimation for the satellite attitude using star tracker measurements[END_REF][START_REF] Park | Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map[END_REF][START_REF] Willsky | Some estimation problems on Lie groups[END_REF][START_REF] Willsky | Dynamical Systems Defined on Groups: Structural Properties and Estimation[END_REF]) have been used, notably in the case of bilinear systems [START_REF] Willsky | Estimation for bilinear stochastic systems[END_REF] and estimation of the initial condition of a Brownian motion in [START_REF] Duncan | An estimation problem in compact Lie groups[END_REF].

Minimum energy filtering A somewhat different but related approach to filtering consists of finding the path that best fits the data in a deterministic setting, which is equivalent to compute the state maximizing the likelihood of the data. It is thus related to optimal control theory where geometric methods have long played an important role [START_REF] Brockett | Lie algebras and Lie groups in control theory[END_REF]. A certain class of least squares problems on the Euclidean group has been tackled in [START_REF] Han | Least squares tracking on the Euclidean group. Automatic Control[END_REF]. This approach has also been used to filter inertial data recently in [START_REF] Saccon | Secondorder-optimal filters on lie groups[END_REF], [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF]. Note that for linear systems with Gaussian noises, this definition of the optimal estimate coincides with the previous one and is again given by the Kalman Filter.

Deterministic observers

The other route to estimate the value of an unknown dynamical system is the design of a deterministic observer. This general concept denominates a variable whose evolution has been chosen by the practitioner as a function of the available data, in a way ensuring the convergence to zero of the error X(t) -X(t) where X(t) denotes the estimate of the true state X(t) of the dynamical system under consideration. The fundamental difference with the filtering approach is that it does not take into account the noise directly, that is we assume the noise is always turned off (w = 0 and V = 0 in the above equations). The very structure of the general form of observers (most of them are made of a copy of the dynamics plus a correction term) is built to make it perturbation-or noise-resistant, but the theoretical study focuses on its convergence only when noise is turned off. Although less intuitive, this simplification allows more powerful convergence results in some cases, and the rationale is that a globally convergent observer should be robust even to large noises as the error tends to return to zero no matter its size.

For linear systems, the problem of designing observers is now well understood and the celebrated Luenberger observer [START_REF] Luenberger | An introduction to observers[END_REF] allows to assign the velocity at which the estimation error should exponentially return to zero. Its tuning is experimental, and depends on the level of noises, the idea being that for large noises the convergence velocity should not be too high, as the incoming data cannot be fully trusted. For non-linear systems, there is no general method to design observers, but, there has been very interesting solutions introduced for certain classes of systems. Systems known as linearizable by output injection [START_REF] Hammouri | Global time-varying linearization up to output injection[END_REF] can be reduced to the linear case where usual methods apply. For system having observability normal form with triangular structure, high-gain observers are appropriate [2-5, 49, 85].

The Kalman filter as an observer An interesting property of the Kalman filter is to be also a convergent deterministic observer. Indeed, if the data are noiseless (i.e. V = 0, w = 0), and the equations are linear, for any choice of positive definite covariance matrices for the trusted noises (here they should be referred to as tuning matrices Q, R since we assume there is no noise) the estimate X converges to the true state X under natural assumptions [START_REF] Deyst | Conditions for asymptotic stability of the discrete minimumvariance linear estimator[END_REF]. In the non-linear case, the EKF does not possess any convergence guarantee, and its efficiency is aleatory. Indeed, its main flaw lies in its very nature: the Kalman gain is computed assuming the estimation error is sufficiently small to be propagated analytically through a first-order linearization of the dynamics about the estimated trajectory. When the estimate is actually far from the true state variable, the linearization is not valid, and results in an ill-adapted gain that may amplify the error. In turn, such positive feedback loop may lead to divergence of the filter. This is the reason why most of the papers dealing with the stability of the EKF (see [START_REF] Bonnabel | A contraction theory-based analysis of the stability of the extended kalman filter[END_REF][START_REF] Boutayeb | Convergence analysis of the extended Kalman filter used as an observer for nonlinear deterministic discrete-time systems[END_REF][START_REF] Sonnemann Reif | An ekf-based nonlinear observer with a prescribed degree of stability[END_REF][START_REF] Song | The extended Kalman filter as a local asymptotic observer[END_REF]) rely on the highly non-trivial assumption that the eigenvalues of the Kalman covariance matrix P(t) computed about the estimated trajectory are lower and upper bounded by strictly positive scalars. Under this assumption, the error as long as it is sufficiently small can not be amplified in a way that makes the filter diverge. To the author's knowledge, only a few papers deal with the stability of the EKF without invoking this assumption [START_REF] Arthur | The convergence of the extended kalman filter[END_REF]. It is then replaced by second-order properties whose verification can prove difficult in practical situations. This is also due to the fact that the filter can diverge indeed in a number of applications. Moreover, the lack of guarantees goes beyond the general theory: there are few engineering examples where the EKF is proved to be (locally) stable.

Autonomous error equations

Both Luenberger observer and Kalman filter allow to build estimation methods with convergence guarantees for deterministic linear problems defined by the general equations:

d dt X t = A t X t + B t u t , Y t n = H n X t n ,
where X t ∈ R n is a vector and A t , B t , H n are matrices with appropriate dimensions. Note that we removed the noises V n and w t from the equations (see explanations above). These filters share the general form:

Propagation:

d dt Xt = A t Xt + B t u t , t n-1 < t < t n ,
Update:

X+ t n = Xt n + K n Y n -H n Xt n , t = t n ,
where Xt is the estimate, the superscript + is used to denote the value of the estimate just after the update and Y n is the observation at time t n . Matrix K n , called "gain", is defined differently depending on the chosen method. But in any case, the estimation error e t = X t -Xt has the following evolution:

d dt e t = A t e t , e + t n = (Id -K n H n )e t n . The estimate Xt and true state X t do not appear in these equations. This property is the founding principle of all the convergence results of these methods. Indeed, it makes the analysis of the error e t independent from a specific trajectory of the system. One of the contributions of the present work is to study non-linear systems offering the same advantage. In more mathematical terms, the properties we are interested in are the following: Definition 1 (Autonomous error equation). Consider an observer providing an estimate Xt defined by the two usual steps:

Propagation: d dt Xt = f u t ( Xt ), t n-1 < t < t n ,
Update:

X+ t n = ψ Xt n , h(X t n ), (u s ) s t n , t = t n ,
where ψ is a function of the current estimate, the observation and the past inputs.

The error variable e t = ε(X t , Xt ) is told to follow an autonomous equation if there exist two functions g and ψ e such that for any couple of solutions X, X of respectively the system and the observer (or filter) dynamics, the error e t = ε(X, X) verifies at each step:

Propagation: d dt e t = g u t (e t ), t n-1 < t < t n ,

Update: e + t n = ψ e (e t n , (u s ) s t n ) , t = t n .

The terminology "fully autonomous error variable" will be used if the function ψ e is in addition totally independent from the inputs.

Note that the terminology "autonomous" above is slightly abusive at it allows the error equation to depend on the time through the inputs, this is why we introduce the concept of "fully autonomous" which corresponds to the standard notion of autonomy in the ordinary differential equations literature.

A natural question is whether or not non-linear systems offering such an error variable exist. A first answer is given by the theory of invariant observers.

Invariant observers

Invariant observers are a class of deterministic observers meant to be appropriate to dynamical systems possessing symmetries. Roughly speaking, a dynamical system possesses symmetries, if it "looks" the same after having undergone a certain transformation. For instance, the motion of a car looks the same whether it is heading west or north. However, a badly tuned extended Kalman filter meant to estimate the motion of the car, might behave differently in both cases. Invariant observers remedy this issue: their general form is such that the estimation is identical for the transformed system. They were introduced to our best knowledge in [START_REF] Aghannan | On invariant asymptotic observers[END_REF] and well developed in [START_REF] Bonnabel | Observateurs asymptotiques invariants: théorie et exemples[END_REF][START_REF] Bonnabel | Symmetry-preserving observers. Automatic Control[END_REF] (see also e.g. [START_REF] Barczyk | Invariant observer design for a helicopter uav aided inertial navigation system[END_REF][START_REF] Vasconcelos | A nonlinear position and attitude observer on SE(3) using landmark measurements[END_REF]). Such observers do not have any guaranteed convergence properties. However, for a system living on a Lie group and having enough symmetries, namely a left-invariant dynamics with right-equivariant output (and vice versa), the estimation error between the true state and the state estimated by an invariant observer has an autonomous evolution [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF]. This property has been discovered independently and leveraged by many authors in various theoretical and applicative contexts (see e.g. [START_REF] Hamel | Homography estimation on the special linear group based on direct point correspondence[END_REF][START_REF] Ch | Gradient-like observers for invariant dynamics on a Lie group[END_REF][START_REF] Mahony | Nonlinear complementary filters on the special linear group[END_REF][START_REF] Vasconcelos | Nonlinear extended images via image-domain interferometry[END_REF][START_REF] Vasconcelos | A nonlinear position and attitude observer on SE(3) using landmark measurements[END_REF]). The idea of taking advantage of the properties of Lie groups to design convergent observers (see also [START_REF] Mahony | Complementary filter design on the special orthogonal group SO(3)[END_REF][START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF][START_REF] Malis | Estimation of homography dynamics on the special linear group[END_REF]) is surprisingly new if compared to the wide literature dedicated to the subject of bringing the symmetries or Lie group structure to bear on the design of controllers, dating back to the 1970's [START_REF] Jurdjevic | Control systems on lie groups[END_REF]. The popularity of the approach has persisted [START_REF] Bullo | Geometric control of mechanical systems[END_REF][START_REF] Jurdjevic | Geometric control theory[END_REF] and has lead to very interesting and relatively recent methods [START_REF] Fruchard | A framework for the control of nonholonomic mobile manipulators[END_REF][START_REF] Morin | Practical stabilization of driftless systems on lie groups: the transverse function approach[END_REF][START_REF] Morin | Trajectory tracking for nonholonomic vehicles[END_REF].

Contributions of the present thesis

The present work has been made possible by previous research on invariant observers initiated in [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF][START_REF] Bonnabel | Symmetry-preserving observers. Automatic Control[END_REF]. We believe the breakthrough of this theory was to show that observers combining a non-linear error variable (error between true and estimated state) with a nonlinear innovation (that is, correction) term could lead to an autonomous error equation. The same authors also suggested to linearize the introduced non-linear invariant error and to use Kalman equations to make it decrease, leading to the theory of the Invariant EKF (IEKF), see [START_REF] Bonnabel | Left-invariant extended Kalman filter and attitude estimation[END_REF][START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and application to a velocity-aided attitude estimation problem[END_REF]. By the time this PhD started, some questions were however left unanswered:

1. Invariant systems are generally not linear and linear system are generally not invariant, but they share the property of having an error variable with autonomous error equation. Is there a more general class of systems containing both but preserving their main characteristics ?

2. Beyond the IEKF, what properties should one expect from an EKF which is meant to make a linearized non-linear error variable decrease, that is a general function ε(X, X) rather than the usual linear state error X -X ?

Industrial motivations

The starting point of the present work is industrial applications to inertial navigation and aircraft geo-localization. Indeed, the present PhD was done in partnership with the company SAGEM, the No. 1 company in Europe and No. 3 worldwide for inertial navigation systems (INS). Half of the time was spent within the company. The initial goal was to apply the theory of symmetry-preserving (or invariant) observers for the problem of geo-localization. This came with two constraints:

1. Current state estimation in SAGEM's products essentially relies on extended Kalman filtering. Indeed, the EKF has proved to be reliable for a wide range of aerospace applications for a long time now (starting with the Apollo program). Industrialists are conscious of its potential shortcomings; but generally trust its performances and appreciate its easiness of implementation and low computational requirements (compared to sampling based state of the art particle filters). The novel algorithms developed should be essentially based on extended Kalman filtering as well to be industrially acceptable and lead to commercial products. Last but not least, the high rate of inertial measurements and the discrete nature of other measurements (GPS, altimeter, etc.) favored the use of continuous-time EKFs with discrete observations.

2. Even the simple system of an aircraft or Unmanned Aerial Vehicle (UAV) navigating on flat earth equipped with accelerometers and gyrometers does not define a leftinvariant system on a Lie group, and thus does not fit into the framework of [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF] that allows for an autonomous error equation, a key property being at the heart of our present work. Yet the true navigation equations as used by SAGEM engineers are even much more complicated than this. This raised the question of extending the approach to systems that were neither left nor right invariant systems on Lie groups.

For those reasons, the present work essentially focuses on the Invariant EKF (which is a form of EKF) on Lie groups for state estimation, with continuous time dynamics and discrete observations, and seeks to broaden the class of systems for which such a tool may prove useful.

The IEKF as a stable observer for a broad class of systems The main contribution of the present work is to characterize the class of systems defined on Lie groups such that in the absence of measurements, the (invariant) error between the true state and the estimated state (that is, any other solution to the dynamical system in question) follows an autonomous equation (to be more precise an equation that may depend on the system's inputs but which does not explicitly depend on the true nor estimated system's state). Those systems include left and right invariant dynamics, but also a combination of both, linear dynamics and some more complicated systems also. We further show that, for this class of systems the logarithm of the invariant error follows a linear differential equation in the Lie algebra, although the system is totally non-linear (the dynamics is non-linear and the state space is not a vector space). The very unexpected latter property is leveraged to prove that for this class of systems on Lie groups, the IEKF viewed as a deterministic observer (that is turning all noises to zero) converges around any trajectory under standard observability conditions. This is a very remarkable feature as it is unusual to be able to design non-linear observers that converge around any trajectory for challenging non-linear systems of engineering interest, and here the linearized error system is time-dependent (through the system's inputs and the linearization around the estimated state) which makes it a challenge to stabilize. Many systems of engineering interest modeling the behavior of a body flying in space fit into the proposed framework, and the IEKF inherits the local convergence properties derived in the general case.

Stochastic convergence properties of invariant observers

Then, the present thesis investigates the stochastic properties possessed by estimators with the autonomous error equation property. More precisely, we consider invariant observers (with constant non-linear gains) such that the estimation error follows a fully autonomous equation (that is does not even depend on the inputs). Even if the design is made by assuming noise is turned off, we turn the noise on and study how the observer behaves. In this case, the estimation error becomes a time-homogeneous Markov chain due to the autonomy of the error. We then show that under some assumptions, the error's distribution (as a random variable) converges asymptotically to some fixed probability distribution. This is an unusual result in the theory of filtering on Lie groups, and from a practical viewpoint, it allows to tune the gains so as to minimize the dispersion of the asymptotic distribution, that can be evaluated empirically through Monte-Carlo simulations. The gain tuning can thus possess some optimality properties although assessed in advance, i.e. independently from the actual measurements, a surprising and remarkable property.

Non-linear state errors for EKF-SLAM

The last important contribution of the present work is to attempt to generalize the invariant approach, or in other terms attempt to understand at a more general level what makes it so appealing. Concretely, we raise the issue of the use of general non-linear state errors when devising EKFs, not necessarily on Lie groups. We wonder : what is the virtue of non-linear estimation errors rather than linear errors (beyond being able to prove some convergence properties) ? For example, given that one can devise two types of invariant errors on Lie groups (right-invariant and left-invariant, see [START_REF] Bonnabel | Observateurs asymptotiques invariants: théorie et exemples[END_REF]) which one should one prefer ? We propose two answers to those questions.

The first answer deals with the issue of observability. Indeed, if some variables (or some combination of variables) are not observable, then an observer, or an EKF should not try to estimate those variables. However, the EKF sometimes does not "see" those variables are not observable, and due to linearization it starts thinking it could estimate them. This leads to inconsistency of the EKF estimates (that is over-optimism with respect to its estimates) in the problem of [START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF] for example. A simple way to remedy this issue, is to devise a non-linear state error such that the non-observable directions are always the same for this error, independently of the state X. This is in general impossible to achieve, but becomes feasible when on can produce an error whose evolution is relatively independent of the trajectory (ideally is autonomous). Note that, such a problem does not arise in linear systems as the non observable directions are the same whatever the state X. For this reason, we have managed to devise an EKF for the SLAM (simultaneous localization and mapping) problem that solves a part of the very well-known inconsistency issues in the EKF-SLAM [START_REF] Bailey | Consistency of the ekf-slam algorithm[END_REF][START_REF] José A Castellanos | Limits to the consistency of ekf-based slam 1[END_REF][START_REF] Guoquan | Analysis and improvement of the consistency of extended kalman filter based slam[END_REF][START_REF] Guoquan | Observability-based rules for designing consistent ekf slam estimators[END_REF][START_REF] Huang | Convergence and consistency analysis for extended kalman filter based slam[END_REF][START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF] . This is a novel result to our knowledge, and we believe it is a fundamental result in estimation theory and robotics.

The second approaches the problem from the other end: in many applications of the EKF (and especially in SLAM) some function of the state is observable with extreme precision while the remaining part is still uncertain. This is what happens for example if a robot travels for a long time with little heading information. It is able to build an accurate map of its close environment but has no idea of the absolute orientation of this map. The covariance matrix P of the EKF is then almost singular. When this kind of situation arises in a linear problem, the Kalman filter still works perfectly and its updates are contained in the "authorized" space, i.e. in the vector space spanned by the eigenvectors of P having non-zero eigenvalues. In our (non-linear) SLAM example, the corresponding property would be the local map to be only rotated by a heading update resulting from a late heading measurement (but without shape distortion of the map). We will show this is not verified at all by a classical EKF. But with an appropriate error variable and a modified update discussed in this document, an EKF natively ensuring this property can be obtained without any patchwork forcing the constraint.

Industrial contributions

The theory developed in this document was successfully applied to some estimation problems arising in inertial navigation. This field aims at using inertial measurements coming from accelerometers and gyroscopes to compute the position, velocity and attitude of a vehicle. Merging the data provided by these sensors with other sources of information is usually performed using an EKF and gives a natural application to the class of filters we study here. As the current methods are already efficient, we focused on the situations where they reach their limits.

• The first one is the transitory phase of the convergence where the estimation error is possibly large. An EKF failing in this situation to achieve acceptable performance, some methods involving preliminary coarse estimation steps are currently in use. Their design is not trivial and has to be adapted to each new situation. Moreover, the coarse estimation phase extends the duration of the initialization, which can be critical in practice. The enhanced EKF we implemented preserves its performance even for very large initial errors and allows to skip the coarse estimation steps.

The implementation and adaptation become straightforward, the time required for initialization is reduced and some constrained imposed to the pilot can be loosened. This method has been industrially approved and adopted by SAGEM to replace their state of the art EKF algorithm on a wide-spread commercial military device.

• The second one is the case of a quasi-static pose, with constant heading but polluted by a specific pattern of movement. If the EKF maintains its estimate using as an observation the fact that the position is approximately constant, a systematic error appears on the long run, due to the issue of false observability explained further down this document. This can be solved by some patch-up work designed precisely to solve this problem, but raising also new issues. The method developed during the thesis for this filtering situation solved the encountered problem although not being specifically tuned for that.

Organization of the document

Chapter 2 is a brief tutorial giving the basic notions about Lie groups required to read this document. We divided the sequel into three parts. To some respect, the parts can be read independently. Note, this comes at the price of several repetitions throughout the manuscript.

Part I is dedicated to the IEKF. We first characterize in Chapter 3 a large class of systems on Lie groups where an error having autonomous propagation can be designed. This novel framework, containing left-invariant dynamics, right-invariant dynamics, combinations of both, linear dynamics, and some systems arising in inertial navigation, is shown to ensure novel and striking linearity properties. An IEKF for this extended class of systems is then presented in Chapter 4, along with new stability properties applied to a long list of examples. The benefits of the approach are illustrated in Chapter 5 by industrial applications and comparison with tried and tested commercial softwares.

Part II introduces a fully probabilistic framework and then considers the whole probability density function of the state of the system as the quantity of interest. The framework of stochastic processes on Lie groups is used in Chapter 6 to write the evolution of the error as a Markov chain (more precisely a Harris chain) and we show under proper conditions the almost global convergence property of the deterministic state error with noise turned off leads to a convergence in distribution of the error when noise is turned on. Still in a probabilistic framework, yet studying a quite different approach, a practical sampling method (that is particle filter) taking advantage of the autonomy property of the error, is proposed in Chapter 7, and applied to a non-Gaussian navigation problem.

Part III extends the use of non-linear error variables to the general design of an EKF and proves its dramatic implications. The exposure starts with the classical example of SLAM: Chapter 8 shows that the widely acknowledged flaw of the EKF for this problem is only due to the (classical yet arbitrary) linear error variable. A well chosen non-linear definition of the error is shown to solve the problem. The study is then generalized in Chapter 9 and novel properties are derived, which do not rely on first-order approximations (that is, some more global properties of EKFs based on alternative state errors are outlined). They are illustrated on another (non-SLAM) example which can be seen as a simplified version of the industrial applications described earlier, and provides physical insight into the EKFs based on non-linear state errors.

Chapter 2

Mathematical preliminaries

In this document we will have to design some error variables adapted to the different non-linear systems we will consider. A very convenient way to define an error is to start from an existing group operation. For this reason, the notion of matrix Lie group will be extensively used:

Definition 2.
A matrix group G is a set of square invertible matrices of size n × n on which matrix multiplication and inversion can be safely used without going outside the set. In other words, the following properties are verified:

Id ∈ G, ∀g ∈ G, g -1 ∈ G and ∀a, b ∈ G, ab ∈ G.
An example of matrix group is given by the group of 2D rotations SO(2):

cos(θ ) -sin(θ ) sin(θ ) cos(θ ) , θ ∈ R .
To obtain a Lie group, we also need a tangent space g about identity Id, defined as the matrix subspace spanned by all possible first-order variations inside G around Id. For rotation matrices, Id corresponds to θ = 0. For theta small, the first-order variation of the rotation matrix is:

cos(θ ) -sin(θ ) sin(θ ) cos(θ ) = Id + θ 0 -1 1 0 + •(θ ).
Thus, if G = SO(2), g is the one-dimensional matrix space spanned by 0 -1 1 0 . Note that G was parameterized by a single coordinate, and g is one-dimensional. Of course this no coincidence, the dimension of g is also the minimum number of real numbers needed to parameterize G, and is called the dimension of the Lie group G. Another way to see g is to consider all processes γ(t) taking values in G and starting from γ(0) = Id. g is then the space of all possible values of ( d dt γ)(0). This definition makes the derivation of g more convenient in the following example:

Example 1. G = SO(3) = {H ∈ M 3 (R), H T H = Id, det(H) > 0}.
Consider a process γ(t) on G and assume γ(0) = Id. We have ∀t > 0, γ(t) T γ(t) = Id. Deriving this expression we get:

( d dt γ)(t) T γ(t) + γ(t) T ( d dt γ)(t) = 0.
At t = 0 and denoting ( d dt γ)(0) by ξ we obtain ξ T + ξ = 0, i.e. ξ is skew-symmetric matrix. Reciprocally, any skew-symmetric matrix is the derivative at zero of a process taking values in G as the solution of the equation

M 0 = Id, d dt M t = M t ω × stays in SO(3) for t > O if ω × is a skew-symmetric matrix.
Finally, g is here the space of skew-symmetric matrices.

If this tangent space about identity is defined, it can also be defined about any element g of G as the possible derivatives at zero of a process γ(t) starting from γ(0) = g. Its characterization is obvious as the processes g -1 γ(t) and γ(t)g -1 start from Id. We obtain: Proposition 1. For g ∈ G, the tangent plan to G about g has the two equivalent characterizations g • g and g • g.

An interesting consequence of Proposition 1 is the property

g • g • g -1 = g.
In other words, for any g ∈ G and ξ ∈ g we have gξ g -1 ∈ g. The latter property defines for any g ∈ G a linear function from g to g. It is called adjoint operator and denoted by Ad g : g → g. The differential of the operator Ad at identity is denoted ad, which means that if we consider a smooth process g t taking values in G and verifying g 0 = Id, d dt g t = ξ ∈ g then we have:

d dt Ad g t | t=0 = ad ξ .
An equivalent notation for ad ξ 1 ξ 2 is [ξ 1 , ξ 2 ], usually called the Lie bracket of ξ 1 and ξ 2 . This multiplicative operation endows g with the structure of an algebra, and is the reason why g is often called the Lie algebra of G. As it proves useful to identify g to R dim g , a linear mapping L g : R dim g → g will often be used. The mapping is defined below for the matrix Lie groups considered in this document, but it is a mere convention and any choice of a mapping L g would be equivalent. An interesting property of the matrix exponential is the following:

Proposition 2. Let exp m denote the matrix exponential:

exp m (A) = +∞ ∑ i=1 A k k! .
Then we have:

exp m (g) ⊂ G.
This gives a simple way to map g to G (the rest of the document will give new arguments supporting the use of this function for estimation purposes). As well, R dim g can be mapped to G through a function exp defined by exp(ξ ) = exp m (L g (ξ )). For all matrix Lie groups considered in this document, no matrix exponentiation is actually needed as there exist closed formulas, given thereafter. The following property of the exponential mapping will be useful:

Proposition 3. The functions Ad and exp commute in the following sense:

∀g ∈ G, ∀ξ ∈ g, exp (Ad g ξ ) = g exp (ξ ) g -1 .
We give now a short description of the most useful matrix Lie groups for our purpose.

Group of direct planar isometries SE(2)

We have here G = SE(2) and g = se(2), where :

SE(2) = R(θ ) x 0 1,2 1 , θ x ∈ R 3 , so(2) =      0 -θ u 1 θ 0 u 2 0 0 0   ,   θ u 1 u 2   ∈ R 3    , L so(2)   θ u 1 u 2   =   0 -θ u 1 θ 0 u 2 0 0 0   , R(θ ) = cos(θ ) -sin(θ ) sin(θ ) cos(θ )
denoting the rotation of angle θ , and the exponential mapping is:

exp   θ u 1 u 2   =   θ Re(z) Im(z)   , where z = (u 1 + iu 2 ) e iθ -1 iθ .

Group of rotation matrices SO(3)

G is here the group of rotation matrices SO(3) and g the space of skew-symmetric matrices so(3):

SO(3) = R ∈ M 3 (R), RR T = Id, det(R) = 1 , so(3) = A ∈ M 3 (R), A = -A T , L so(3)   ξ 1 ξ 2 ξ 3   =   ξ 1 ξ 2 ξ 3   × =   0 -ξ 3 ξ 2 ξ 3 0 -ξ 1 -ξ 2 ξ 1 0   .
The notation (•) × just defined will be used throughout this document. The exponential mapping is given by the formula:

exp(ξ ) = I 3 + sin(||ξ ||) ||ξ || (ξ ) × + 2 sin(||ξ ||/2) 2 ||ξ || 2 (ξ ) 2 × .

Group of direct spatial isometries SE(3)

G is here the group of rigid motions SE(3) and g = se(3) its Lie algebra:

SE(3) = R x 0 1,3 1 , R ∈ SO(3), x ∈ R 3 , se(3) = (ξ ) × x 0 1,3 1 , ξ , x ∈ R 3 , L se(3) ξ x = (ξ ) × x 0 1,3 0 .
The exponential mapping is given by the formula:

exp ξ x = I 4 + S + 1 -cos(||ξ ||) ||ξ || 2 )S 2 + ||ξ || -sin(||ξ ||) ||ξ || 3 S 3 ,
where S = L se(3) ξ x .

Group of double direct isometries SE 2 (3)

We introduce here a matrix Lie group proving very useful for inertial navigation and we call it "group of double direct isometries", with the notation SE 2 (3), for lack of existing conventions (to our best knowledge). Its Lie algebra will be denoted se 2 (3) and we have:

SE 2 (3) =      R v x 0 1,3 1 0 0 1,3 0 1   , R ∈ SO(3), v, x ∈ R 3    , se 2 (3) =      (ξ ) × u y 0 1,3 0 0 0 1,3 0 0   , ξ , u, y ∈ R 3    , L se 2 (3)   ξ u y   =   (ξ ) × u y 0 1,3 0 0 0 1,3 0 0   , exp   ξ u y   = I 5 + S + 1 -cos(||ξ ||) ||ξ || 2 )S 2 + ||ξ || -sin(||ξ ||) ||ξ || 3 S 3 , where S = L se 2 (3)   ξ u y   .
Table 2.1 -Formulas for most frequent matrix Lie groups in navigation

G SO(3) (R dim g = R 3 ) SE(3) (R dim g = R 6 ) Embedding of G R ∈ M 3 (R), R T R = I 3 R v 0 1 , R ∈ SO(3), u ∈ R 3 Embedding of g A 3 : ψ ∈ M 3 (R), ψ T = -ψ ψ u 0 0 , ψ ∈ A 3 , u ∈ R 3 L g ξ → (ξ ) × ξ u → (ξ ) × u 0 0 x → Ad x R → R R T → R 0 (T ) × R R ξ → ad ξ ξ → (ξ ) × ξ u → (ξ ) × 0 (u) × (ξ ) × exp exp(x) = I 3 + sin(||x||) ||x|| (x) × exp(x) = I 4 + (x) × + [1-cos(||x||)] ||x|| 2 (x) 2 × + 1 ||x|| 2 (1 -cos |x|) (x) 2 × + 1 ||x|| 3 (||x|| -sin ||x||) (x)

Autonomous error equations

Part of this chapter has been merged to Chapter 4 and submitted to a journal. The paper is currently under revision.

Chapter abstract

In the present chapter we characterize dynamical systems on Lie groups that are such that the error (that is the difference in the sense of group multiplication) between two solutions follows an autonomous equation, a key property for the convergence results of the filters introduced in the next chapters. Previous publications noticed the error equation was autonomous for invariant systems [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF][START_REF] Bonnabel | Symmetry-preserving observers. Automatic Control[END_REF][START_REF] Ch | Gradient-like observers for invariant dynamics on a Lie group[END_REF] but did not try to characterize the systems having this property. We furthermore prove that for those systems the error surprisingly follows a linear differential equation, although the system at hand is non-linear. The two latter contributions are novel to our best knowledge.

Introduction

As stated in the introduction chapter, finding an error variable following an autonomous equation is very desirable in an estimation problem. The theory of invariant observers [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF], starting from considerations regarding the symmetries of a system, has brought to bear the use of group operations on obtaining such a property. Consider indeed a system of the form:

d dt χ t = χ t ω t , Y t n = χ t n • b n ,
where χ t belongs to a matrix Lie group G, the input ω t to the associated Lie algebra g, b n ∈ R p is a known vector, and "•" a group action. The reader feeling uncomfortable with this notion can assume "•" is the classical product of a matrix and a vector. Define the error variable as e t = χ -1 t χt and the update as

χ+ t n = χt n K n ( χ-1 t Y t n ) where K n can be any function of R p → G.
(e -1 t n • b n ).
We see the evolution of e t is independent from the estimate. Thus, we are now able to characterize two families of systems on which an error variable with autonomous equation is defined: linear and invariant systems. Note that to obtain autonomous error equation, the innovation has been defined in a specific way:

z = χ-1 t • Y = e -1
t n • b n , making it only a function of the error. Remark 1 below should convince the reader that a new class of estimation problems allowing autonomous error equation must exist, and contain both invariant and linear systems.

Remark 1.

Additive linear systems of the form d dt X t = u t are translation-invariant, but general linear systems are not a specific case of invariant systems, not more than invariant systems are a specific case of linear systems. Inspired by the latter example, we consider in this chapter systems defined on Lie groups and error variables of one of the two following forms:

e t = η L t = χ -1 t χt (left invariant), (3.1)
e t = η R t = χt χ -1 t (right invariant). (3.2)
The superscripts L and R refer to "left-invariant" and "right-invariant" as in [START_REF] Barrau | The invariant extended kalman filter as a stable observer[END_REF]. We will characterize a broad class of systems (containing invariant and linear systems) where the innovation and error propagation depend only on the error variable. We look for them under the following general form:

d dt χ t = f u t (χ t ), Y n = h(χ t n ), (3.3)
where χ t is the state of the system and Y the observation. Definition 1 has the drawback to refer to a specific observer. Yet, the property of autonomous error equation can surprisingly be stated only as a characteristic of the system and the error variable. This is what we show now, defining first the two following properties:

Definition 3 (Autonomous error propagation, Autonomous innovation). Let X t be a state vector having an evolution of the form:

d dt X t = f u t (X t ),
where u t is a sequence of inputs, and let h be an observation function measured at instants

(t n ) n 0 : Y n = h(X t n ).
An error variable e t is defined as a function ε of the true state X t and the estimate Xt : e t = ε(X t , Xt ), smooth and invertible with respect to each of its variables and equal to zero (or to the identity element on a Lie group) if its two arguments are identical. We define as follows two desirable properties of ε:

Autonomous error propagation: ε is told to have autonomous propagation if there exist a function g such that for any couple of solutions X, X of the dynamics, e t = ε(X, X) verifies:

d dt e t = g u t (e t ).
Autonomous innovation: ε is told to ensure autonomous innovation if there exist two functions ψ 1 and ψ 2 such that for any couple of states (X t , Xt ) we have:

ψ 1 Xt , h(X t ) = ψ 2 ε(X t , Xt ) ,
ψ 1 being invertible with respect to its second variable.

Remark 2. The idea of autonomous error propagation is quite transparent: the evolution of the error e t in the absence of observation depends on Xt and X t only through the value of e t itself. But the introduction of an "autonomous innovation" may deserve more explanation. Innovation measures the discrepancy between the expected observation h( Xt ) and the true observation h(X t ). We denote it by ψ 1 ( Xt , h(X t )) (instead of simply ψ 1 h( Xt ), h(X t ) ) to allow any additional dependency in the known variable Xt . We define this innovation function as "autonomous" if the result depends on Xt and X t only through the value of e t . The point of introducing this notion here is to remove any reference to a specific observer. Instead, we defined the two basic ingredients allowing to build observers having autonomous error equation.

In the context of multiplicative errors, definitions 3 and 1 are equivalent in the sense of Proposition 4: Proposition 4. If a system has autonomous error propagation and autonomous innovation as defined in Definition 3 then an observer having autonomous error equation can be build. If an observer having autonomous error equation can be build then the system has autonomous error propagation and autonomous innovation as defined in Definition 3.

Proof. The error is assumed right-invariant (ε(χ t , χt ) = χt χ -1 t ), the proof being similar for a left-invariant error. Assume the system verifies the properties introduced in Definition 3. Then an observer with update step of the form χ+ t n = K (ψ 1 ( χt , h(χ t ))) χt n has autonomous error equation. To show the converse, let ψ and ψ e denote the update functions of definition 1(we removed the inputs, any sequence can be chosen here). Autonomy of the update step reads:

ψ ( χt , h(χ t )) χ -1 t = ψ e ( χt χ -1 t ). Thus function ψ 1 ( χt , h(χ t )) = ψ( χt , h(χ t )) χ-1 t
appears to be an autonomous innovation function:

ψ( χt , h(χ t )) χ-1 t = ψ( χt , h(χ t ))χ -1 t χ t χ-1 t = ψ e (e)e -1 .
The characterization of dynamics f u t having autonomous error propagation on one hand, and the observation functions h ensuring autonomous innovation on the other hand, are independent and will be dealt with separately, in Section 3.2 for autonomous innovation, then Sections 3.3, 3.4 3.5 for autonomous propagation. Section 3.3 gives a first characterization, surprisingly simple although not allowing an exhaustive classification. Section 3.4 derives a fundamental log-linearity property of the systems having autonomous propagation. It will be advantageously leveraged in Chapter 4, but first used in Section 3.5 to link our study of the systems having autonomous propagation to the classification of Lie algebra homomorphisms. Remark 3. The reader could be wondering how restrictive is the framework we just adopted to study autonomous error equations. Although we don't claim perfect rigor here, we will try to give an argument showing Lie group structures and multiplicative errors arise quite naturally if error variables are at issue. Here the state χ belongs to any space X and we introduce an additional property of the error function: for any χ 1 , χ 2 , χ 3 , γ 1 , γ 2 , γ 3 ∈ X we assume

ε(χ 1 , χ 2 ) = ε(γ 1 , γ 2 ) and ε(χ 2 , χ 3 ) = ε(γ 2 , γ 3 ) ⇒ ε(χ 1 , χ 3 ) = ε(γ 1 , γ 3 ). (3.4)
This property means that you can deduce the error between χ 1 and χ 3 from the error between χ 1 and χ 2 on one hand, χ 2 and χ 3 on the other hand, without knowing the value of χ 1 , χ 2 or χ 3 . In other words the errors can be composed defining a group law: for any errors e 1 and e 2 we choose any element χ 1 of X , another one denoted by χ 2 such as ε(χ 1 , χ 2 ) = e 1 (whose existence is ensured by the invertibility of ε w.r.t. its second variable) and a third one denoted by χ 3 such as ε(χ 2 , χ 3 ) = e 2 . The product e 1 • e 2 is then defined by ε(χ 1 , χ 3 ). Hypothesis (3.4) ensures the result is independent from the choice of χ 1 and χ 2 . Error zero is the identity element, the inverse is defined by ε(χ 1 , χ 2 ) -1 = ε(χ 2 , χ 1 ) and associativity is obtained considering a sequence of four elements χ 1 , χ 2 , χ 3 , χ 4 . This law is then transported on the state space X : we can choose any point χ 0 as a reference to embed the state space into the error space using the bijection φ :

χ → ε(χ 0 , χ). The error becomes ε(χ 1 , χ 2 ) = ε(χ 1 , χ 0 ) • ε(χ 0 , χ 2 ) = ε(χ 0 , χ 1 ) -1 • ε(χ 0 , χ 2 ) = χ -1 1
• χ 2 and takes the general form introduced above.

Theorem 1. The observation functions h(χ) ensuring the existence of an autonomous innovation for the error e = χ -1 χ (resp.e = χ χ -1 ), i.e. two functions ψ 1 and

ψ 2 such that ∀χ, χ ∈ G, ψ 1 ( χ, h(χ)) = ψ 2 (χ -1 χ), (3.5)
are the left-actions (resp. right-actions) of G on a space Y . Moreover, the associated autonomous innovation is then defined by

ψ( χ,Y ) = χ-1 Y (resp. ψ( χ,Y ) = χY ).
To prove Theorem 1 we need the following lemma:

Lemma 1. Let Y denote any set and ψ x : Y → Y a function from Y to Y defined for any x ∈ G (with ψ Id = Id). If for an element y 0 ∈ Y the relation ∀a, b ∈ G, ψ a (ψ b (y 0 )) = ψ ba (y 0 ) is verified then ψ is a right action of G over ψ G (y 0 ).
Proof. The proof is straightforward:

let y ∈ ψ G (y 0 ) and c ∈ G such that y = ψ c (y 0 ). For any a, b ∈ G we have ψ a (ψ b (y)) = ψ a (ψ b (ψ c (y 0 ))) = ψ a (ψ cb (y 0 )) = ψ cba (y 0 ) = ψ ba (ψ c (y 0 )) = ψ ba (y).

Now we can prove Theorem 1:

Proof. Let Y denote the observation space and h : G → Y the observation function. Assume h ensures the existence of an autonomous innovation. Then for any χ, χ ∈ G we have ψ 1 ( χ, h(χ)) = ψ 2 (χ -1 χ). In particular, χ = Id gives

∀ χ, ψ 1 ( χ, h(Id)) = ψ 2 ( χ). (3.6)
Thus ψ 2 (χ) can be replaced by ψ 1 (χ, h(Id)) for any χ ∈ G and, denoting ψ 1 (χ, y) by ψ χ (y) in (3.5) then applying (3.6) the condition becomes:

∀χ, χ, ψ χ (h(χ)) = ψ χ -1 χ (h(Id)). (3.7) For χ = Id we get ψ Id (h(χ)) = ψ χ -1 (h(Id)) thus h(χ) = ψ -1 Id • ψ χ -1 • h(Id). Reintroducing this equality in (3.7) it becomes: ∀χ, χ, ψ χ • ψ -1 Id • ψ χ -1 • h(Id) = ψ χ -1 χ • h(Id). Thus ∀χ, χ, (ψ -1 Id • ψ χ ) • (ψ -1 Id • ψ χ -1 ) • h(Id) = (ψ -1 Id • ψ χ -1 χ ) • h(Id) which makes ψ → ψ -1 Id • ψ χ a right-action of G over Im(h) (see Lemma (3.7))
, and h a left-action over Im(h). The proof for the right-invariant error e = η R = χ χ -1 is similar.

Autonomous error propagation

To obtain autonomous error equations, the second property we seek is autonomy of the errors evolution, i.e. the existence of a function g u t (independent from χ or χ) such that the error evolution reads d dt e t = g u t (e t ), with e t defined as either χ -1 t χt or χt χ -1 t . It is verified by a very specific class of systems, as shown by Theorem 2. We recall before the property we are seeking: Definition 4. The left-invariant and right-invariant errors are said to have autonomous propagation if they satisfy a differential equation of the form d dt e t = g u t (e t ).

Note that, in general the time derivative of e t is a complicated function depending on u t and both χ -1 t and χt in a way that does not boil down to a function of e t . The following result allows to characterize the class of systems of the form (3.3) for which the property holds.

Theorem 2. The three following conditions are equivalent for the dynamics (3.3):

1. The left-invariant error has autonomous propagation 2. The right-invariant error has autonomous propagation 3. For all t > 0 and a, b ∈ G we have (in the tangent space at ab):

f u t (ab) = f u t (a)b + a f u t (b) -a f u t (I d )b. (3.8)
Moreover, if one of these conditions is satisfied we have

d dt η L t = g L u t (η L t ) where g L u t (η) = f u t (η) -f u t (I d )η, (3.9)
d dt η R t = g R u t (η R t ) where g R u t (η) = f u t (η ) -η f u t (I d ). (3.10)
Proof. Assume we have d dt η L t = g u t (η L t ) for a certain function g u t and any process of the form η L t = χ -1 t χt , where χ t and χt are solutions of (3.3). We have: .11) This has to hold for any χ t and η t . In the particular case where χ t = I d we obtain:

g u t (χ -1 t χt ) = d dt (χ -1 t χt ) = -χ -1 t d dt χ t χ -1 t χt + χ -1 t d dt χt = -χ -1 t f u t (χ t )η L t + χ -1 t f u t ( χt ) g u t (η L t ) = -χ -1 t f u t (χ t )η L t + χ -1 t f u t (χ t η L t ). ( 3
g u t (η L t ) = f u t (η L t ) -f u t (I d )η L t . (3.12)
Re-injecting (3.12) in (3.11) we obtain:

f u t (χ t η t ) = f u t (χ t )η t + χ t f u t (η t ) -χ t f u t (I d )η t .
The converse is trivial and the proof is similar for right-invariant errors.

The class of systems defined by Equation (3.8) has many similarities with linear systems, as will be shown in the sequel. It generalizes them in a way that preserves some interesting properties.

Remark 4. The particular cases of left-invariant and right-invariant dynamics, or the combination of both as follows, verify the hypothesis of Theorem 2. Let f v t ,ω t (χ) = v t χ + χω t . We have indeed:

f v t ,ω t (a)b + a f v t ,ω t (b) -a f v t ,ω t (I d )b =(v t a + aω t )b + a(v t b + bω t ) -a(v t + ω t )b = u t ab + abω t = f v t ,ω t t (ab).
Remark 5. In the particular case where G is a vector space with standard addition as the group composition law, the condition (3.8) boils down to

f u t (a + b) = f u t (a) + f u t (b) -f u t (0)
and we recover the affine functions.

A log-linearity property of errors having autonomous propagation

The general solution of a linear system of the form d dt X t = A t X t + B t is notoriously the sum of a particular solution Xt and a solution ξ t of the homogeneous linear system d dt ξ t = A t ξ t . In the broader case defined by Theorem 2, a surprising generalization can be derived: the general solution of the system is the product of a specific solution by the exponential of the solution of a linear homogeneous system (Corollary 1).

In the sequel, we will systematically consider systems of the form (3.3) with the additional property (3.8), i.e. systems defined by

d dt χ t = f u t (χ t ), where ∀(u, a, b) f u (ab) = a f u (b) + f u (a)b -a f u (Id)b. (3.13)
For such systems, Theorem 2 proves that the left (resp. right) invariant error is a solution to the equation d dt η t = g u t (η t ) where g u t is given by (3.9) (resp. (3.10)). For small errors η t , that is η t close to Id, the error system can be linearized as follows. As we have g u t (Id) ≡ 0, the Lie derivative of the vector field g u t along any other vector field only depends on the value of the vector field at Id. It can thus be identified to a linear operator (thus a matrix) in the tangent space at Id, and will be denoted by Dg u t (Id). Furthermore, a small error η t can be identified to an element of R dimg through the exponential map, that is η t = exp (ξ t ) (see Chapter 2 for a definition of the exponential map). The linearized error equation thus reads d dt ξ t = Dg u t (Id)ξ t . The next striking result proves this first order Taylor expansion of the non linear error system captures in fact the whole behavior of the error propagation. Theorem 3. Consider the left or right invariant error η i t as defined by (3.1) or (3.2) between two trajectories of (3.13), the superscript i denoting indifferently L or R, and assume η i t is in the image of the exponential mapping (this image is the whole group G in all the examples considered in this document). Let

ξ i 0 ∈ R dim g be such that exp(ξ i 0 ) = η i 0 . If ξ i t is defined for t > 0 by the linear differential equation in R dimg d dt ξ i t = A i t ξ i t , where A i t := Dg i u t (Id),
then we have:

∀t ≥ 0 η i t = exp(ξ i t ).
Recalling the definition of the invariant errors, the following corollary illustrated by Fig. 3.1 is easily derived.

Corollary 1.

Let χt denote a particular solution of (3.13). If the exponential mapping is surjective then the general solution of the system has the two following characterizations:

• χ t = exp(F R t ξ 0 ) χt , where F R t is defined by F R 0 = Id and d dt F R t = Dg R u t (Id)F R t .
• χ t = χt exp(F L t ξ 0 ), where F L t is defined by

F L 0 = Id and d dt F L t = Dg L u t (Id)F L t .
Figure 3.1 -Example illustration of the log-linear property of the error propagation. Central plot: a cloud of points centered on χt 0 having the property of being a Gaussian in the tangent space at χt 0 with covariance matrix P is transported by the flow (3.13) over the matrix group. Right plot: the cloud in the tangent space loses its Gaussianity due to the nonlinearity of the flow. Left plot: when using the exponential to map the tangent space to the manifold, we see at t = t 1 the cloud remains a Gaussian centered at χt 1 , and the covariance matrix becomes F t 1 PF T t 1 where F t 1 is defined in Cor 1.

The last two properties are trivial in the linear case as the exponential associated to the additive structure is the identity function. But they show that a wide range of nonlinear problems (see examples of Chapter 4 Section 4.3) can lead to linear error equations provided the error variable is correctly chosen. The result implies that the propagation step of an EKF making use of the exponential mapping and the invariant errors is expected to reflect the true evolution of the error variable. This will be extensively used in Chapter 4 to prove stability properties of Invariant Extended Kalman Filters.

The proof of Theorem 3 is based upon the following lemmas. We have the following properties:

• The function g R u (η) = f u t (η)η f u t (Id) verifies (3.14) and all the solutions of (3.13) have the form

χ t = η R t χt , where η R t verifies d dt η R t = g R u t (η R t ).
• The function g L u t (η) = f u t (η)f u t (Id)η verifies (3.14) and all the solutions of (3.13) have the form

χ t = χt η L t , where η L t verifies d dt η L t = g L u t (η L t ).
The verification of these two properties is trivial. The functions g u t governing the errors propagation turn out to possess an interesting property: Lemma 3. Let Φ t be the flow associated to the system d dt η t = g u t (η t ), where g u t verifies (3.14). Then:

∀η 0 , η 0 ∈ G, ∀p ∈ Z, Φ t (η 0 η 0 ) = Φ t (η 0 )Φ t (η 0 ).
Proof. We simply have to see that Φ t (η 0 ) Φ t (η 0 ) is solution of the system d dt η t = g u t (η t ):

d dt Φ t (η 0 ) Φ t η 0 = g u t (Φ t (η 0 )) Φ t η 0 + Φ t (η 0 ) g u t Φ t η 0 = g u t Φ t (η 0 ) Φ t η 0 .
An immediate recursion gives then: Lemma 4. For any η 0 ∈ G and p ∈ Z we have:

Φ t η p 0 = Φ t (η 0 ) p .
Lemmas 3 and 4 indicate the behavior of the flow infinitely close to Id dictates its behavior arbitrarily far from it, as the flow commutes with exponentiation (exponentiation in the sense of the iterated multiplication of an element of a group by itself). The use of the exponential (map) thus allows to derive an infinitesimal version of the Lemma 4, which is an equivalent formulation of Theorem 3 and its corollary. Theorem 4. Let Φ t be the flow associated to the system d dt η t = g u t (η t ) satisfying (3.14). As we have g u t (Id) = 0, the Lie derivative of g u t along a vector field ν verifying ν(Id) = ξ 0 only depends on ξ 0 and will be denoted by Dg u t ξ 0 . We have:

Φ t (exp(ξ 0 )) = exp (F t ξ 0 ) .
where F t is the solution of the matrix equation

F 0 = Id, d dt F t = Dg u t F t .
Proof. Thanks to Lemma 4 we have

Φ t e ξ 0 = Φ t e 1 n ξ 0 n = Φ t e 1 n ξ 0 n = e 1 n DΦ t | Id ξ 0 +r t ( 1 n ξ 0) n ,
where r t 1 n ξ 0 is a quadratic term, which ensures in turn e

1 n DΦ t | Id ξ 0 +r t ( 1 n ξ 0) n = e DΦ t | Id ξ 0 + 1 n r t (ξ 0 ) .
Letting n → ∞ we get Φ t e ξ 0 = e DΦ t | Id ξ 0 . We then introduce a parameterized curve ψ h (Id)

on G such that ψ 0 (Id) = Id and d dh ψ h (Id) = ξ 0 . We then depart from the definition of the flow Φ t starting at ψ h (Id) :

d dt t=s [Φ t (ψ h (Id))] = g u s (Φ s (ψ h (Id))) .
Using

A complete characterization of systems with autonomous errors

Relation (3.8) can be easily checked and is thus a satisfactory alternative definition of systems having autonomous error propagation. But it is not a characterization as it does not give a complete list of the vector fields f verifying

∀a, b, f (ab) = f (a)b + a f (b) -a f (Id)b. (3.15)
This set will be denoted F and its characterization is the purpose of the present section.

Proposition 5. Relation (3.15) being linear, the classical addition of vector fields makes F a vector space.

To formulate the next result, we need to introduce some new objects and definitions.

Definition 5 (Lie algebra homomorphism). Given a Lie algebra a with Lie bracket [•, •] a , an homomorphism a of a is a linear and invertible function from a to a ensuring:

∀ξ 1 , ξ 2 ∈ a, [aξ 1 , aξ 2 ] = a[ξ 1 , ξ 2 ].
The homomorphisms of a Lie algebra a constitute a Lie group A whose operation is the operators composition.

The group of homomorphisms of g will be denoted by G , and its Lie algebra by g . These notations allow us to give a full characterization of the vector fields f verifying (3.15):

Theorem 5. Assume G is connected. The functions F L and F R below are two linear injections of F into g × g :

F L : F → g × g , F L : f → ( f (0), D| x=0 [ f (x) -x f (0)]) , F R : F → g × g , F R : f → ( f (0), D| x=0 [ f (x) -f (0)x]) .
• If G is simply connected then F L and F R are bijections.

• Otherwise, for any element A ∈ (g × g ) there exists a vector field f defined on a neighborhood of Id such that F L ( f ) = A (resp. F R ( f ) = A), and verifying (3.15) for a and b small enough.

If defined, the result of the inverse operator

F -1 L α, α (resp. F -1 R α, α ) is the vector field x → xα + D exp log x ( α log x) (resp. x → αx + D exp log x ( α log x)).
If G is R n (n > 0) with the vector sum as group operation, we have g = R, G = Gl n (R) (group of invertible matrices of size n × n) and g = M n (R) (space of matrices of size n × n). Theorem 5 boils down to the proposition stated in example 2:

Example 2. The vector fields f on R n verifying ∀a, b ∈ R n , f (a + b) = f (a) + f (b) -f (0) are the functions of the form: f (X) = AX + B, with A ∈ M n (R) and B ∈ R n .
We see again that vector fields verifying (3.15) are very similar to affine functions, although defined on a general Lie group instead of R n .

The proof of theorem 5 is done here only for F L , it would be similar for F R . It consists of two steps:

1. The operator F L ( f ) : f → f (0), g L , where g L is defined as in Lemma 2 by ∀x ∈ G, g L (x) = f (x)x f (Id), is a bijection from F to g × G , where G is the space of vector fields verifying (3.14).

The derivative at

Id is an injection of G into g 1 is verified easily. To show 2 we first characterize the flows Φ t induced by the vector fields verifying (3.14). Lemma 3 shows that Φ t is a group homomorphism of G at any t > 0. As a consequence we have: Proposition 6. At any time t > 0, the differential DΦ t | Id = F t is a Lie algebra homomorphism of g, i.e. verifies:

∀ξ 1 , ξ 2 ∈ g, [F t ξ 1 , F t ξ 2 ] = F t [ξ 1 , ξ 2 ].
Proof. Lemma 3 shows that Φ t is a group homomorphism of G at any t > 0. As a consequence, F t is a Lie Algebra homomorphism of g. This result can be either obtained replacing Φ t in Lemma 3 by its exponential form given in Theorem 4, then developing the Baker-Campbell-Hausdorff (BCH) formula up to the second-order, or found directly in [START_REF] Alexander | An introduction to Lie groups and Lie algebras[END_REF] for instance.

We just showed that DΦ t | Id is in G . As it is also solution of d dt DΦ t | Id = Dg| Id DΦ t | Id , the differential Dg| Id is its left-trivialized derivative and is thus in g . To show the differential at Id is injective we consider a vector field g ∈ G verifying Dg| Id = 0. According to Theorem 4 the flow of g is constant over time in an open neighborhood of Id. As this flow is a group homomorphism the property extends to the whole group out of connexity. Deriving the flow w.r.t. the time we get ∀x ∈ G, g(x) = 0 which proves the injectivity. To obtain the reciprocal we first consider an element α of g and define the (linear) flow A t by A 0 = Id, d dt A t = αA t . A t is an homomorphism of g. For x ∈ G close to Id we consider now Φ t (x) = exp(A t log(x)). If x 1 , A t x 1 , A t x 2 and x 2 are in the convergence radius of exp we have:

Φ t (x 1 )Φ t (x 2 ) = exp A t log(x 1 ) exp A t log(x 2 ) = exp BCH (A t log(x 1 ), A t log(x 2 )) = exp A t BCH(log(x 1 ), log(x 2 )) = exp A t log(x 1 x 2 ) = Φ t x 1 x 2 .
We used here the fact that A t , as an homomorphism of g, commutes with all the terms of the BCH formula. The last step is simply to derive Φ t w.r.t. time to obtain a function of G defined around Id. The result extends to the whole group G only if the exponential map associates a Lie group homomorphism of G to each homomorphism A t of g. This is ensured if G is connected and simply connected (see Theorem 3.41 of [START_REF] Alexander | An introduction to Lie groups and Lie algebras[END_REF] for instance).

Conclusion

This chapter characterized a class of problems for which the error variable can be designed to have autonomous propagation. For these systems, the error variable was showed to have even linear propagation if re-defined through a logarithmic map. Some concrete examples of such systems, mostly related to navigation, are described in Chapter 4. The advantages of such an approach are obvious, regardless of the chosen estimation method. If an observer relies on pre-computed gains, the dependence on the specific trajectory has to be made as weak as possible. If the error equation is linearized for an EKF-like on-line gain tuning, this operation becomes independent from the estimated state and deriving stability properties becomes possible (see Chapter 4). Moreover, the discrepancy between the system linearized over its true trajectory and the system linearized on the current estimate disappears, which Chapter 9 will prove to be of dramatic importance. An EKF for this class of systems will be studied in the next chapter.

Chapter 4 Invariant Extended Kalman Filtering

Chapter abstract The previous chapter proved a certain class of estimation problems to be of specific interest as they allow designing observers ensuring the evolution of the error variable to be fully independent from the estimate. In this chapter, we consider a deterministic setting, and we use a first-order expansion of the error to derive a generalization of the Invariant Extended Kalman Filter (IEKF) introduced in [START_REF] Bonnabel | Left-invariant extended Kalman filter and attitude estimation[END_REF]. The terminology IEKF is retained although it is designed for a larger class of systems, and we allow for the tuning matrices (interpreted as noise covariance matrices) to depend on the filter's trajectory, as opposed to [START_REF] Bonnabel | Left-invariant extended Kalman filter and attitude estimation[END_REF][START_REF] Bonnabel | Invariant Extended Kalman Filter: Theory and application to a velocity-aided attitude estimation problem[END_REF] that presuppose fixed covariance matrices. The resulting IEKF is thus more suited to real implementation where the covariance of the noises may depend on the trajectory. We derive a novel local convergence result of the IEKF viewed as an observer, building upon the log-linearity property derived in Section 3.4 of Chapter 3. We provide a long list of challenging navigation examples and for each of them the IEKF is proved to converge around any trajectory under standard observablity conditions, a generally difficult to prove property in a non-linear setting.

Introduction

The multiplicative difference between two solutions χ t and χt of the systems introduced in Chapter 3, defined as χ -1 t χt or χt χ -1 t , has been shown to have extremely convenient properties. In this Chapter, we put them to good use extending the IEKF (proposed in [START_REF] Bonnabel | Left-invariant extended Kalman filter and attitude estimation[END_REF] for invariant systems) to this broader class, and improving it by the introduction of an exponential mapping in the update step (see Chapter 2), whose benefits will be discussed at length in Chapters 9 and 10. A novel stability result based on the property obtained in Section 3.4 of Chapter 3 is derived for this EKF-like method, whose assumptions are exactly those used in [START_REF] Deyst | Conditions for asymptotic stability of the discrete minimumvariance linear estimator[END_REF] for linear Kalman filtering. Of course, the computations involved in the proof proposed in this latter paper have been of great help here. The difference with the linear case is that global convergence is replaced by local stability. Yet, only firstorder observability assumptions are necessary. This is a decisive difference with most of the previous work on convergence of the EKF, where hypotheses regarding higher orders of the system and/or global behavior of the estimated error covariance matrix are involved [START_REF] Bonnabel | A contraction theory-based analysis of the stability of the extended kalman filter[END_REF][START_REF] Boutayeb | Convergence analysis of the extended Kalman filter used as an observer for nonlinear deterministic discrete-time systems[END_REF][START_REF] Sonnemann Reif | An ekf-based nonlinear observer with a prescribed degree of stability[END_REF][START_REF] Song | The extended Kalman filter as a local asymptotic observer[END_REF]. Their verification is usually extremely difficult on concrete sys-tems, to the opposite of the result derived here, easily applied to a long list of examples. We divided the exposure into two sections. Section 4.2 defines the framework, derives the equations of the IEKF then gives a novel stability property based on the results of Chapter 3. Section 4.3 details the computations in several specific cases corresponding to classical navigation problems, and applies each time the result of 4.2 to guarantee stability of the method.

Full noisy system

We first consider a noisy equation on a matrix Lie group G ⊂ R N×N of the form:

d dt χ t = f u t (χ t ) + χ t w t , (4.1)
where w t is a continuous white noise belonging to g (the Lie algebra of G) whose covariance matrix is denoted by Q t (for a proper discussion on multiplicative noise for systems defined on Lie groups, see Chapter 6), u t is an input and f u t verifies for any value of u t the relation:

∀a, b ∈ G, f u t (ab) = a f u t (b) + f u (a)b -a f u t (Id)b.
Guided by the results of Chapter 3, we assume there exists an action • of G on a space Y . The observations we are interested in are :

1. Left-invariant observations: Y = h(χ • b 0 ,V ) (with V a noise and h(., v) invertible for any v).

2. Right-invariant observations: Y = h(χ -1 • b 0 ,V ) (with V a noise and h(., v) invertible for any v).

In each case, h is smooth and verifies h(x, 0) = x. Before proceeding with the derivation of the filter we give some examples of such observation functions:

Example 3. The attitude of a vehicle is usually represented by a rotation matrix R ∈ G = SO(3). It is the rotation matrix mapping a vector expressed in the vehicle frame to a vector expressed in a fixed frame. The following observations have the general form just defined:

• Y = R T b + V , where V ∈ R 3 is a random (unknown) noise and b is a vector known in the fixed reference frame, for instance the gravity vector observed in the vehicle frame by an accelerometer. Here, the space Y is R 3 , the action of G on Y is simply the product of a matrix and a vector and the observation is of the right-invariant kind. • Y = Ru +V , where V ∈ R 3 is a random noise and u is a vector known in the vehicle reference frame, for instance an axis attached to a quadrotor but measured by a fixed camera. Here, the space Y is R 3 , the action of G on Y is again the product of a matrix and a vector and the observation is of the left-invariant kind.

• Y = R T b 1 +V 1 R T b 2 +V 2 , where V 1 ,V 2 ∈ R
• Y = RV , where V ∈ SO(3) is a random noise (note the values of V are rotation matrices). The observation is here the full state, resulting for instance from a reconstruction algorithm using a set of fixed cameras. Here, the space Y is SO(3), the action of G on Y is the product of matrices and the observation is of the left-invariant kind. Moreover, the element b 0 is here the identity matrix.

To make the description of the filters more concrete, we consider only the case where observations are vectors, and the group action is simply the multiplication of an element of G by a sequence of vectors. There is no specific difficulty in the extension of the approach to the general case. In this modified framework, the two kinds of observations are:

Left-invariant observations This family of outputs writes:

Y 1 t n = χ t n (d 1 + B 1 n ) +V 1 n , . . . , Y k t n = χ t n (d k + B k n ) +V k n ,
where (d i ) i≤k are known vectors and the (V i n ) i≤k , (B i n ) i≤k are noises with known characteristics. The Left-Invariant Extended Kalman Filter (LIEKF) is defined here through the following propagation and update steps:

d dt χt = f u t ( χt ), t n-1 ≤ t ≤ t n (propagation), (4.2)
χ+ t n = χt n exp L n   χ-1 t n Y 1 t n -d 1 . . . χ-1 t n Y k t n -d k   (update), (4.3)
where the function L n : R kN → R dim g is to be defined in the sequel using error linearizations.

A left-invariant error between true state χ t and estimated state χt can be associated to this filter:

η L t = χ -1 t χt .
During the Propagation step, as χ t and χt are two trajectories of the system (4.1) with noise turned off i.e. w t ≡ 0, Theorem 2 states that the evolution of the error is independent from the estimate χt (or alternatively from the true state χ t ). During the Update step, the evolution of the invariant error variable merely reads:

η L t n + = χ -1 t n χ+ t n = η L t n exp   L n   (η L t n ) -1 d 1 -d 1 + χ-1 t n V 1 n + B 1 n . . . (η L t n ) -1 d k -d k + χ-1 t n V k n + B k n     . (4.4)
We see that, as long as output noise V j n is turned off, i.e. ∀n, j V j n ≡ 0, the nice geometrical structure of the LIEKF allows the updated error (η L t n ) + to be only a function of the error just before update η L t n , i.e. to be independent from the estimate χt n .

Right-invariant observations

This family of observations has the form:

Y 1 t n = χ -1 t n (d 1 +V 1 n ) + B 1 n , . . . , Y k t n = χ -1 t n (d k +V k n ) + B k n ,
with the same notations as in the previous section. The Right-Invariant EKF (RIEKF) is defined here as:

d dt χt = f u t ( χt ), t n-1 < t < t n (propagation), (4.5)
χ+ t n = exp   L n   χt n Y 1 t n -d 1 . . . χt n Y k t n -d k     χt n (update). (4.6)
A right-invariant error can be associated to this filter:

η R t = χt χ -1 t .
During the update state, the evolution of the invariant error variable reads:

(η R t n ) + = χ+ t χ -1 t = exp   L n   η R t n d 1 -d 1 +V 1 n + χt n B 1 n . . . η R t n d k -d k +V k n + χt n B k n     η R t n .
Once again, due to Theorem 2, we see that when noise is turned off, the evolution of the error does not depend on the state of the system, for both Propagation and Update steps.

The case of full state observation An observation of the full state with multiplicative noise can be seen either as a left or as a right-invariant observation:

Y t n = V n χ t n B n or Y t n = B -1 n χ -1 t n V -1 n .
A wise choice is to keep the formulation which provides the more independent noisy error equation. We provide details for the first formulation (Y t n = V n χ t n B n ), left and right playing a symmetrical role here. The LIEKF is defined here as:

d dt χt = f u t ( χt ), t n-1 < t < t n (propagation), (4.7)
χ+ t n = exp L n exp -1 ( χ-1 t n Y t n ) χt n (update), (4.8)
and here we have L n : R dim g → R dim g . A left-invariant error can be associated to this filter:

η t = χ -1 t χt .
During the update state, the evolution of the invariant error variable reads:

η + t n = χ -1 t n χ+ t n = η t n exp L n exp -1 ( χ-1 t n V n χt n )η -1 t n B n . (4.9)
If there is no noise on the left side ( i.e. V n = Id) this evolution is independent from the estimate χt n . In the case B n = Id the right-invariant filter is to be preferred.

Gain tuning and IEKF equations

As in a conventional EKF, the error equation has to be assumed to be small (here close to Id) so that the error system can be linearized to compute the gains L n (the error is Id if χt = χ t ). By definition, the Lie algebra g represents the infinitesimal variations around Id of an element of G. Thus the natural way to define a vector error variable ξ t in R dim g is (see Chapter 2):

η t = exp(ξ t ) = exp m L g (ξ t ) . (4.10)
During the Propagation step, that is for t n-1 ≤ t < t n , elementary computations based on the results of Theorem 2 show that for the noisy model (4.1) we have

d dt η L t = g L u t (η L t ) -w t η L t , d dt η R t = g R u t (η R t ) -( χt w t χ-1 t )η R t . (4.11)
Defining ŵt ∈ R dim g by L g ( ŵt ) = -w t in the first case and L g ( ŵt ) = -χt w t χ-1 t (i.e. ŵt = -Ad χt L -1 g (w t )) in the second case, and using the superscript i to denote indifferently L or R we end up with the linearized error equation in R dimg :

d dt ξ t = A i u t ξ t + ŵt , (4.12)
where A i u t is defined by g i u t (exp (ξ )) = L g A i u t ξ + ξ t 2 (This expression makes sense because the state is embedded into a matrix space, otherwise we should have to compose the left-hand side by D log | exp(ξ ) ; see Section 3.3 for a more precise definition) and where we have neglected terms of order ( ξ t 2 ) as well as terms of order ( ŵt ξ t ). Note that, Theorem 3 states that the first order Taylor expansion D log | exp(ξ ) g i u t (exp(ξ )) = L g (A i u t ξ ) is in fact totally exact, i.e. all higher order terms are identically null. This implies the following result (illustrated by Figure 3.1): Proposition 7. With noise turned off, i.e. w t ≡ 0, if ξ t is defined as the solution to the linearized error system (4.12) and η t is defined as the solution to the nonlinear error system (4.11), then the equality (4.10) is verified at all times, even for arbitrarily large errors.

To derive the equations we consider for instance the case of left-invariant observations, and define ξ t n through the exponential mapping (4.10), i.e. exp(ξ

t n ) = η L t n . Moreover, for 1 ≤ i ≤ k let V i n denote χ-1 t n V i n .
To linearize the update equation (4.4) we proceed as follows. For 1 ≤ i ≤ k we have

(η t n ) -1 d i -d i = exp m (L g (ξ t n )) -1 d i -d i + V i n + B i n = exp m (-L g (ξ t n ))d i -d i + V i n + B i n = (I -L g (ξ ) t n )d i -d i + V i n + B i n + ξ t n 2 = -L g (ξ ) t n d i + V i n + B i n + ξ t n 2 ,
using a simple Taylor expansion of the matrix exponential map. Expanding in the same way equation (4.4) yields:

I + L g (ξ t n ) + = I + L g (a) + ξ t n 2 , with a = L n   -L g (ξ t n )d 1 + V 1 n + B 1 n . . . -L g (ξ t n )d k + V k n + B k n   . (4.13)
Neglecting terms of order ξ t n2 we finally get the following linearized error equation in R dimg :

ξ + t n = ξ t n + L n (Hξ t n + Vn + B n ), (4.14)
where H ∈ R kN×dimg , Vn ∈ R kN and B n ∈ R kN are defined by

Hξ =   -L g (ξ )d 1 . . . -L g (ξ )d k   , Vn =   V 1 n . . . V k n   , B n =   B 1 n . . . B k n   .
Note that, contrarily to the Propagation step (4.12), equation (4.14) is only a first-order approximation of the true error update (4.4). Let Qt denote the covariance of the modified process noise ŵt , and Nn denote the covariance of the modified measurement noise Vn + B n . Note that, equations (4.12) and (4.14) mimic those of a Kalman filter designed for the following auxiliary linear system with discrete measurements: d dt x t = A u t x t + ŵt , y n = Hx t n + Vn + B n . The standard Kalman theory thus states that if L n is computed through the following Riccati equation:

d dt P t = A u t P t + P t A T u t + Qt , S n = HP t n H T + Nn , L n = P t n H T S -1 , P + t n = (I -L n H)P t n , (4.15)
the dispersion of the linearized error ξ t is minimized at each step, indicating that χt should be a good estimate of χ t despite the noise, as long as χt and χ t are sufficiently close. Moreover, Kalman theory states that the solution ξ t to the linearized system (4.12)-(4.14) is a centered Gaussian with covariance matrix P t ∈ R dimg×dimg . Thus the true error η t = χ -1 t χt is approximately distributed as follows

P(η t ∈ exp ([ζ , ζ + dζ ]) ≈ 1 (2π) N/2 |P t | 1/2 e -1

Summary of IEFK equations

Gathering all the results above, the IEKF equations can be compactly written as follows

d dt χt = f u t ( χt ), t n-1 ≤ t ≤ t n χ+ t n = χt n exp   L n   χ-1 t n Y 1 t n -d 1 . . . χ-1 t n Y k t n -d k     (LIEKF)
or

χ+ t n = exp   L n   χt n Y 1 t n -d 1 . . . χt n Y k t n -d k     χt n (RIEKF) (4.17)
where the LIEKF (resp. RIEKF) is to be used in the case of left (resp. right) invariant outputs. The gain L n is obtained in each case through a Riccati equation:

d dt P t = A u t P t + P t A T u t + Qt , t n-1 < t < t n , S n = HP t n H T + Nn , L n = P t n H T S -1 , P + t n = (I -L n H)P t n . (4.18)
As concerns the LIEKF,

A u t is defined by g L u t (exp(ξ )) = L g (A u t ξ ) + ( ξ 2 ), and H ∈ R kN×dimg is defined by Hξ = -L g (ξ )d 1 , . . . , -L g (ξ )d k T , Qt ∈ R dim g×dim g
denotes the covariance of the modified process noise ŵt = -L -1 g (w t ) and Nn the covariance matrix of the noise Vn +B n , Vn and B n being defined as: Vn = ( χ-1

t n V 1 n , . . . , χ-1 t n V k n ) T , B n = (B 1 n , . . . , B k n ) T . As concerns the RIEKF, A u t is defined by g R u t (exp(ξ )) = L g (A u t ξ ) + ( ξ 2 ), H ∈ R kN×dimg is defined by Hξ = L g (ξ )d 1 , . . . , L g (ξ )d k T ,
Qt denotes the covariance of the modified process noise ŵt = -Ad χt L -1 g (w t ) and Nn the covariance matrix of the noise V n + Bn , V n and Bn being defined as:

V n = (V 1 n , . . . ,V k n ) T , Bn = ( χt n B 1 n , . . . , χt n B k n ) T .

Stability properties

The aim of the present section is to study the stability properties of the IEKF as a deterministic observer. Throughout the section, the noise is thus systematically turned off. Contrarily to the EKF, the IEKF has strong stability properties relying on the state trajectory independence of the deterministic part of the error equation. The stability of a filter is defined as its ability to recover from a perturbation or an erroneous initialization: Definition 6. Let (x 0 ,t 0 ,t) → X t t 0 (x 0 ) denote a continuous flow on a space X endowed with a distance d. A flow (z,t 0 ,t) → Xt t 0 (z) is an asymptotically stable observer of X about the trajectory X t t 0 (x) t≥t 0 if there exists ε > 0 such that:

d x, x < ε ⇒ d X t t 0 (x), Xt t 0 ( x) → 0 when t → +∞
Theorem 7 below is a consequence of Theorem 4 of Section 3.3. To maintain the independence of the different sections, and not to interrupt the flow of reading, its fairly technical proof has been removed to the Appendix. J. J. Deyst and C. F. Price have shown in [START_REF] Deyst | Conditions for asymptotic stability of the discrete minimumvariance linear estimator[END_REF] the following theorem, stating sufficient conditions for the Kalman filter to be a stable estimator for linear (time-varying) systems.

Theorem 6 [START_REF] Deyst | Conditions for asymptotic stability of the discrete minimumvariance linear estimator[END_REF]). Consider the deterministic linear system d dt x t = A t x t , y t n = Hx t n and let

Φ t t 0 = Id + t t 0 A u du. If there exist α 1 , α 2 , β 1 , β 2 , δ 1 , δ 2 , δ 3 , M > 0 such that: 1. (Φ t n+1 t n ) T Φ t n+1 t n δ 1 I 0 2. ∃q ∈ N * , ∀s > 0, ∃G s ∈ R q×dim g , Q s = G s Q where Q δ 2 I q 0 3. N n δ 3 I 0 4. α 1 I ≤ t n s=t n-M Φ(t, s)Q s Φ(t, s) T ≤ α 2 I 5. β 1 I ≤ ∑ n-1 i=n-M Φ(t n ,t i+1 ) T H T N -1 n HΦ(t n ,t i+1 ) ≤ β 2 I
Then the linear Kalman filter tuned with covariance matrices Q and N is asymptotically stable. More precisely there exist γ min , γ max > 0 such that γ min I P t I γ max I for all t and ( xtx t ) T P -1 t ( xtx t ) has an exponential decay.

The main theorem of this section is the extension of this linear result to the non-linear case when the Invariant Extended Kalman Filter is used for systems of Section 3.3. Theorem 7. Suppose the stability conditions of the linear Kalman filter given in Theorem 6 are verified about the true system's trajectory χ t (i.e. are verified for the linear system obtained by linearizing the system (3.13) with left (resp. right) invariant output about χ t ). Then the Left (resp. Right) Invariant Extended Kalman Filter χt is an asymptotically stable observer of χ t in the sense of Definition 6. Moreover, the convergence radius ε > 0 is valid over the whole trajectory (i.e. is independent of the initialization time t 0 ).

Proof. The full proof is technical and has been removed to Appendix A.1. The rationale is to compare the evolution of the logarithmic error ξ t defined as η t = exp(ξ t ), with its linearization. For the general EKF, the control of second-order terms in the error equation is difficult because: 1) they depend on the inputs u t , 2) they depend on the linearization point χt , 3) the estimation error impacts the gain matrices. For the IEKF, the main difficulties vanish for the following reasons:

• Due to Theorem 3 the linear propagation d dt ξ t = A u t ξ t of the logarithmic error ξ t is exact: all the non-linearity is contained in the update step.

• Due to the specific form of the IEKF update step, the non-linear rest r n , defined (here for the left-invariant filter) as:

exp (I -L n H)ξ n -r n (ξ ) = exp ξ exp -L n H(exp[ξ ]b - b) is second-order in ξ , uniformly over n if L n is bounded.
• Due to the error equation of the IEKF, the Riccati equation depends on the estimate only through the noise matrices Qt and Nt which affect stability in a minor way, as shown by Theorem 6.

In the detailed proof of Appendix A.1 we introduce the flow Ψ t t 0 of the linear part of the equations governing ξ t (that is,

Ψ t 0 t 0 = Id, d dt Ψ t t 0 = A u t Ψ t t 0 , Ψ t+ t 0 = (I -L n H)Ψ t t 0
) and decompose the solution ξ t as:

∀t ≥ 0, ξ t = Ψ t 0 ξ 0 + ∑ t n <t Ψ t t n r n (ξ t n ). (4.19)
All we have to verify is that the apparition of the second-order terms r n (ξ t n ) at each update is compensated by the exponential decay of Ψ t t 0 (Theorem 6).

The result displayed in Theorem 7 is in sharp contrast with the usual results available on the EKF which essentially assume the linearized system around the estimated trajectory is well-behaved. This is almost impossible to predict as when the estimate is (even slightly) away from the true state, the Kalman gain becomes erroneous, which can in turn amplify the discrepancy between estimate and true state. On the other hand, when considering an actual system undergoing a realistic physical motion (see the examples below), if sufficiently many sensors are available, the linearized system around the true trajectory is expected to posses all the desired properties. The following consequence proves useful in practice. Theorem 8. Assume the system linearized around the true trajectory has the following properties : the propagation matrix A t = A is constant, there exist matrices B, D such that Qt = B Qt B T and Nn = D Nn D T with Qt and Nn upper-and lower-bounded over time, with (A, H, B, D) detectable and reachable. Then the conditions of Theorem 7 are satisfied and the IEKF is asymptotically stable.

Examples

Each example can be read and implemented independently from the rest of the chapter, which is used only to prove the stability of the proposed algorithm. The computations require the basic notions about matrix Lie groups recalled in Chapter 2.

Simplified car or unicycle

Original model

Consider a (non-holonomic) car evolving on the 2D plan. Its heading is denoted by an angle θ t ∈ [-π, π] and its position by a vector x t ∈ R 2 . They follow the classical equations:

d dt θ t = u t v t + w θ t , d dt x 1 t = cos(θ t )(v t + w l t ) -sin(θ t )w tr t , d dt x 2 t = sin(θ t )(v t + w l t ) + cos(θ t )w tr t ,
where v t is the velocity measured by an odometer, u t (a function of) the steering angle, w θ t the differential odometry error, w l t the longitudinal odometry error and w tr t the transverse shift. Two kinds of observations are considered:

Ȳn = x t n +V n , (4.20)
or

Ȳ k n = R(θ t n ) T (x t n -p k ) + V k n , k ∈ [1, K], (4.21)
where R(θ ) is a rotation of angle θ , V n and V k n are random noises in R 2 . (4.20) represents a noisy position measure (GPS for instance) and has been considered in [START_REF] Hugh | An autonomous guided vehicle for cargo handling applications[END_REF] where a classical EKF has been proposed, but without convergence guarantees. (4.21) represents a range-and-bearing observation of a sequence of known features located at

p k ∈ R 2 for k ∈ [1, K].
This observation models the navigation of a robot in a known environment. Even in recent papers such as [START_REF] Ganganath | Mobile robot localization using odometry and kinect sensor[END_REF], the EKF is considered the best suited to a robot having low computation capacities. As opposed to previous work based on EKF, the IEKF will be shown to possess local convergence properties for the considered problem, and this under mild conditions and for a large class of noise covariances. Chapter 10 will be entirely dedicated to the discussion of this precise example, as it gives a simple and visual understanding of what shifting from EKF to IEKF concretely changes regarding the behavior of the filter.

Matrix form

This system can be embedded in the matrix Lie group SE(2) [START_REF] Morin | Trajectory tracking for nonholonomic vehicles[END_REF] (see Chapter 2) using the following matrices:

χ t =   cos(θ t ) -sin(θ t ) x 1 t sin(θ t ) cos(θ t ) x 2 t 0 0 1   , ν t =   0 -u t v t v t u t v t 0 0 0 0 0   , w t =   0 -w θ t w l t 1 w θ t 0 w tr t 0 0 0   .
The equation of the system becomes:

d dt χ t = χ t (ν t + w t ), (4.22)
and the observations (4.20) and (4.21) respectively have the equivalent form:

Y n = x t n +V n 1 = χ t n 0 2×1 1 + V n 0 , (4.23)
Y k n = R T t n (x t n -p k ) 1 + V k n 0 = -χ -1 t n p k 1 + V k n 1 . (4.24)
The reader can verify relation (3.8).

IEKF equations and stability for the left-invariant output (4.20)

The LIEKF (4.17) equations for the system (4.22), (4.23) write:

d dt χt = χt ν t , χ+ t n = χt n exp L n χ-1 t n Y n - 0 2×1 1 .
As the bottom element of χ-1

t n Y n - 0 2×1
1 is always zero we can use a reduced-dimension gain matrix Ln defined by L n = Ln p with p = (I 2 , 0 2,1 ). To compute the gains, we write the left-invariant error η t = χ -1 t χt whose evolution is:

d dt η t = η t ν t -ν t η t -w t η t , η + t n = η t n exp Ln p η -1 t n 0 2×1 1 - 0 2×1 1 + χ-1 t n V n 0 . (4.25)
To linearize of this equation we introduce the linearized error ξ t defined as [START_REF] Brockett | Lie algebras and Lie groups in control theory[END_REF]) and removing the second-order terms in ξ t , V n and w t we obtain:

η t = I 3 + L so(2) (ξ t ). Introducing η t = I 3 + L so(2) (ξ t ), η + t = I 3 + L so(2) (ξ + t ), exp(u) = I 3 + L so(2) (u) and η -1 t = I 3 -L so(2) (ξ t ) in (4.
d dt ξ t = -   0 0 0 0 0 -u t v t -v t u t v t 0   ξ t -   w θ t w l t w tr t   , ξ + t n = ξ t n -Ln [(0 2,1 , I 2 )ξ t -R( θt n ) T V n ],
The gains Ln are thus computed using the Riccati equation (4.18) with:

A t = -   0 0 0 0 0 -u t v t -v t u t v t 0   , H = (0 2,1 , I 2 ), Qt = Cov   w θ t w l t w tr t   , N = R( θt n )Cov(V n )R( θt n ) T .
Proposition 8. If there exists v max , v min > 0 such that the displacement satisfies

x t n+1 -x t n ≥ v min > 0,
and the input velocity satisfies u t ≤ v max then the IEKF is an asymptotically stable observer.

The proof is a verification of the hypotheses of 7 and has been removed to Appendix A.2. Note that it seems very difficult to improve on the assumptions: if x t n+1x t n = 0 the system becomes non-observable (the heading θ t cannot be known), and if u t is arbitrary high a large move between two observations makes the covariance of the propagation step infinite due to the uncertainty in the heading. In practice an arbitrary high velocity is anyway unfeasible.

IEKF equations and stability for the right-invariant output (4.21)

The RIEKF equations (4.17) for the system (4.22), (4.24) write:

d dt χt = χt ν t , χ+ t n = exp L n χt n Y 1 n + p 1 1 ; . . . ; χt n Y K n + p 1 1 χt n .
As the bottom element of χ-1

t n Y k n + p k 1
is always zero we can use a reduced-dimension gain matrix Ln defined by L n = Ln p with:

p =    [I 2 , 0 2,1 ] . . . [I 2 , 0 2,1 ]    .
To compute the gains Ln we derive the evolution of the right-invariant error variable η t = χt χ -1 t :

d dt η t = -( χt w t χ-1 t )η t , η + t n = exp( Ln p -η t n p 1 1 + p 1 1 + χt n V 1 n ; . . . ; -η t n p k 1 + p k 1 + χt n V k n )η t n . (4.26)
To linearize this equation we introduce the linearized error ξ t defined as

η t = I 3 +L so(2) (ξ t ). Introducing η t = I 3 + L so(2) (ξ t ), η + t = I 3 + L so(2) (ξ + t ), exp(u) = I 3 + L so(2) (u) and η -1 t = I 3 -L so(2) (ξ t ) in (4.26
) and removing the second-order terms in ξ t , V n and w t we obtain:

d dt ξ t = -   1 0 1,2 ( xt ) 2 -( xt ) 1 R( θt )     w θ t w l t w tr t   , ξ + t n = ξ t n -Ln             -p 1 2 1 0 p 1 1 0 1 . . . -p k 2 1 0 p k 1 0 1       ξ t n -   Rt n V 1 n . . . Rt n V k n         .
The gains are thus computed using the Riccati equation (4.18) with A t , H, Q and N defined as:

A t = 0 3,3 , H =       -p 1 2 1 0 p 1 1 0 1 . . . -p k 2 1 0 p k 1 0 1       , Q =   1 0 1,2 ( xt ) 2 -( xt ) 1 Rt   Cov   w θ t w l t w tr t     1 0 1,2 ( xt ) 2 -( xt ) 1 Rt   T , N =    Rt n Cov(N 1 ) Rt n 0 . . . 0 Rt n Cov(N k ) Rt n    .
Proposition 9. If at least two distinct points are observed then the IEKF is an asymptotically stable observer about any bounded trajectory.

Proof. According to Theorem 8 it is sufficient to show that in this case the observation matrix H is full-rank, i.e. of rank 3. This is obvious as the position and the heading are easily computed from the observation of two vectors at known locations.

Attitude estimation

Original model

The following example has been considered in numerous articles. An IEKF has already been built in [START_REF] Barrau | Intrinsic filtering on lie groups with applications to attitude estimation[END_REF], but a proof of stability is still lacking. There are well-known non-linear observers ensuring almost global convergence of the attitude estimate to the true state in the absence of noise. This property is retained by some variants ensuring simultaneous estimates of quantities of interest such as biases [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF] or camera to IMU rotation parameters [START_REF] Garcia Scandaroli | A nonlinear observer approach for concurrent estimation of pose, imu bias and camera-to-imu rotation[END_REF] to cite a few, which have flourished over the last decade. However, to our best knowledge those approaches rely on observers with time-invariant gain functions. They cannot adapt easily to large prior uncertainty or statistics of the noise as do usual Gaussian approximation-based methods. The latter are detailed in [START_REF] John L Crassidis | Survey of nonlinear attitude estimation methods[END_REF] in the specific context of attitude estimation and include Multiplicative EKF, Additive EKF, UKF and backward smoothing. To the opposite of fixed-gains methods they have no proved convergence properties but can more easily take into account prior uncertainty or noise statistics. In the following we allow for any covariance matrix of the noises (note that even time-varying ones can be handled) which may result in gains depending on the trajectory (see Proposition 10). Contrarily to those previous works this ensures 1-first-order optimality around the estimated state, as a Kalman filter is used to handle the error system as in the standard EKF philosophy, 2-a tuning based on the trusted actual noises covariances, and 3-at the same time we still prove local convergence around the true trajectory, a feature which is in general very hard if not impossible to prove when using a standard EKF. In [START_REF] Baerveldt | A low-cost and low-weight attitude estimation system for an autonomous helicopter[END_REF], a complementary filter was tested, without convergence properties and in [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF], gyro biases were taken into account and global convergence was achieved for constant gains. We would like to mention also that some near-optimality results using a different approach, yet based on a Riccati equation, have also been obtained in [START_REF] Trumpf | Near-optimal deterministic filtering on the rotation group[END_REF].

Consider the attitude of a vehicle, represented by the rotation matrix R t ∈ SO(3) mapping the coordinates of a vector expressed in the vehicle frame to its coordinates in the static frame. The vehicle is endowed with gyrometers giving an angular velocity ω t . The equation of the dynamics reads:

d dt R t = R t (ω t + w t ) × , (4.27)
where (b) × is the anti-symmetric matrix associated to vector b and w t is a noise (see [START_REF] Barrau | Intrinsic filtering on lie groups with applications to attitude estimation[END_REF] for a discussion on the noise). Two kinds of observations are considered:

Y n = R t n V n , (4.28)
or

Y n = (R T t g +V g n ; R T t b +V b n ), (4.29)
where V n is a noise in SO(3), g and b two vectors of R 3 (the gravity and magnetic fields for instance) and V g n ,V b n two noises in R 3 . (4.28) represents a full observation of the attitude returned for instance by an image processing algorithm. The noise V n on the right side corresponds to an uncertainty known in (attached to) the camera frame. (4.28) represents the measurement in the vehicle frame of known vectors of the fixed frame (e.g. using an accelerometer under the quasi-static hypothesis, and a magnetometer), where g et b are assumed non-collinear. This process is already defined on a matrix Lie group (see Chapter 2). The reader can verify relation (3.8).

IEKF equations and stability for the left-invariant output (4.28)

The LIEKF (4.17) for the system (4.27), (4.28) is defined by:

d dt Rt = Rt (ω t ) × , R+ t n = Rt n exp(L n exp -1 [ R-1 t n Y n ]).
To compute the gains L n we define the left-invariant error as η t = R T t Rt and its evolution reads:

d dt η t = -(ω t ) × η t + η t (ω t ) × -(w t ) × η t , η + t n = η t n exp(L n exp -1 (η -1 t n V n )). (4.30)
To linearize this equation we introduce the linearized error ξ t defined as [START_REF] Rodolfo | Combining stereo vision and inertial navigation system for a quadrotor uav[END_REF]) and removing the second-order terms in ξ t , V n and w t we obtain:

η t = I 3 + (ξ t ) × . Introducing η t = I 3 + (ξ t ) × , η + t = I 3 + (ξ + t ) × , exp(u) = I 3 + (u) × , η -1 t = I 3 -(ξ t ) × , exp -1 [I 3 + (u) × ] = u and exp -1 (ab) = exp -1 (a) + exp -1 (b) in (4.
d dt ξ t = -(ω t ) × ξ t -w t , ξ + = ξ -L n (ξ -Ṽn ). (4.31)
The gains L n are thus computed using the Riccati equation (4.18) with matrices A t , H, Q and N defined as:

A t = -(ω t ) × , H = I 3 , Q = Cov(w t ), N = Cov(V n ).
Proposition 10. The conditions of Theorem 7 being obvious, the IEKF for the system (4.27), (4.28) is an asymptotically stable observer.

IEKF equations and stability for the right-invariant output (4.29)

The RIEKF (4.17) for the system (4.27), (4.29) is defined by:

d dt Rt = Rt (ω t ) × , R+ t n = exp(L n [ Rt n Y n -(g; b)]) Rt n , with the notation R(x 1 ; x 2 ) = (Rx 1 ; Rx 2 ) for R ∈ SO(3) and x 1 , x 2 ∈ R 3 .
To compute the gains L n we write the evolution of the right invariant error

η t = Rt R T t : d dt η t = -( Rt w t ) × η t , η + t n = exp(L n [η t g -g + Rt V g n ; η t b -b + Rt V b n ])η t n . (4.32)
To linearize this equation we introduce the linearized error ξ t defined as [START_REF] José A Castellanos | Building a global map of the environment of a mobile robot: The importance of correlations[END_REF]) and removing the second-order terms in ξ t , V g n , V b n and w t we obtain:

η t = I 3 + (ξ t ) × . Introducing η t = I 3 + (ξ t ) × , η + t = I 3 + (ξ + t ) × , exp(u) = I 3 + (u) × , η -1 t = I 3 -(ξ t ) × and exp -1 [I 3 + (u) × ] = u in (4.
d dt ξ t = -Rt w t , ξ + t n = ξ t n -L n (g) × (b × ) ξ t n -Rt n V g n Rt n V b n .
The gains L n are thus computed using the Riccati equation (4.18) where matrices A t , H, Q and N defined as:

A t = 0 3,3 , H = (g) × (b × ) , Q = Rt Cov(w t ) RT t , N = Rt n Cov(V n ) RT t n .
Proposition 11. The IEKF for the attitude estimation system (4.27), (4.29), with observation of two non-collinear vectors is an asymptotically stable observer, as a straightforward application of Theorem 8.

Gyrocompass

Original model

Consider the attitude of a gyrocompass, represented by the rotation matrix R t ∈ SO(3) mapping the coordinates of a vector expressed in the gyrocompass frame to its coordinates in the static frame. The vehicle is endowed with gyrometers giving an angular velocity ω t , and precise enough to measure the earth rotation. This situation did not receive much attention from academics due to the high cost of the IMU required, but is of decisive importance in inertial navigation and led to a great deal of patents. Up to our knowledge, none of them was interested in convergence properties although instability of the EKF can occur for this problem as shown on true data in Chapter 5. This is due to the false observability issues explained in Chapters 8 and 9. The equation of the dynamics reads:

d dt R t = (Ω) × R t + R t (ω t + w t ) × , (4.33)
where Ω is the earth rotation vector and w t is a noise. An accelerometer gives moreover the gravity field in the reference frame of the gyrocompass (quasi-static hypothesis):

Y n = R T t g +V g n . (4.34)
This process is already defined on a matrix Lie group (see Chapter 2). The reader can verify the relation (3.8).

IEKF equations and stability for the right-invariant output (4.34)

The RIEKF (4.17) for the system (4.33), (4.34) is defined by:

d dt Rt = (Ω) × Rt + Rt (ω t ) × , R+ t n = exp(L n [ Rt n Y n -g]) Rt n .
The invariant error is η t = Rt R T t and its evolution reads:

d dt η t = (Ω) × η t -η t (Ω) × -( Rt w t ) × η t , η + t n = exp L n η t g -g + Rt V g n η t n . (4.35)
To linearize this equation we introduce the linearized error ξ t defined as [START_REF] John | Sigma-point kalman filtering for integrated gps and inertial navigation[END_REF]) and removing the second-order terms in ξ t , V g n , V b n and w t we obtain:

η t = I 3 + (ξ t ) × . Introducing η t = I 3 + (ξ t ) × , η + t = I 3 + (ξ + t ) × , exp(u) = I 3 + (u) × , η -1 t = I 3 -(ξ t ) × and exp -1 [I 3 + (u) × ] = u in (4.
d dt ξ t = (Ω) × ξ t -Rt w t , ξ + t n = ξ t n -L n (g) × ξ t n -Rt n V g n .
The gains L n are computed using the Riccati equation (4.18) and matrices A t , H, Q and N defined as:

A t = (Ω) × , H = -(g) × , Q = Rt Cov(w t ) RT t , N = Rt n Cov(V n ) RT t n .
Proposition 12. The IEKF for the gyrocompass problem (4.33), (4.34) is an asymptotically stable observer if g and Ω are non-collinear.Note that, if g and Ω are collinear (in other word the gyrocompass is on the north or south pole), the heading is anyway not observable.

Proof. According to Theorem 8 we only have to show that the pair (A t , H) is observable.

The corresponding observability matrix for two successive observations reads

(g) × (g) × R Ω , with R Ω = exp Ω × (t n+1 -t n ) .
The rank is not increased if this matrix is multiplied on the left by

I 3 0 3×3 0 3×3 R T Ω . We obtain (g) × R T Ω (g) × R Ω = (g) × (R T Ω g) ×
. This is the same observation matrix as in the case where vectors g and R T Ω g are observed. This matrix is of rank 3 if g and R T Ω g are non-collinear, i.e. if g and Ω are non-collinear.

Depth camera motion

Original model

The question of filtering poses computed from a depth camera has been investigated for navigation relying on scan matching [START_REF] Barczyk | Invariant ekf design for scan matching-aided localization[END_REF][START_REF] Hervier | Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering[END_REF]. It is relevant also for image denoising purposes, which can require to work on stabilized images. Some methods avoid reconstruction of camera pose, like [START_REF] Matyunin | Temporal filtering for depth maps generated by kinect depth camera[END_REF] where the motion is estimated using a window of several frames then represented as a vector field on the image. Some other resort to filtering methods to maintain a pose estimate, such as particle filtering in [START_REF] Yang | Robust video stabilization based on particle filter tracking of projected camera motion[END_REF]. Anyway, no stability proof has been proposed for now. Here, we adopt an approach close to [START_REF] Barczyk | Invariant ekf design for scan matching-aided localization[END_REF] and [START_REF] Hervier | Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering[END_REF] but use the previous results to study the stability of the method. We consider thus the problem of a hand-held depth-camera in a known environment, and an assumption of slow displacement. The attitude is denoted by the rotation matrix R t and the position by the 3-dimensional vector x t . The equations read:

d dt R t = (w R t ) × R t , d dt x t = (w R t ) × x + w x t , (4.36)
where w R t and w x t are noises. The process noise is assumed known in the static frame, as whatever the camera orientation is, the horizontal velocity is generally higher than the vertical one, resulting in a covariance linked to the static frame axes. We assume an algorithm returns the whole state (see e.g. [START_REF] Hervier | Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering[END_REF]).

Y R n = R T t n V R n , Y x n = R T t n (x +V x n ). (4.37)

Matrix form

The system can be embedded in the matrix Lie group SE(3) (see Chapter 2) :

χ t = R t x t 0 1×3 1 , w t = (w R t ) × w x t 0 1×3 0 , Y n = Y R n -Y x n 0 1,3 1 , V n = V R n -V x n 0 1,3 1 .
Equations (4.36), (4.37) become:

d dt χ t = w t χ t , (4.38)
Y n = χ -1 t n V n . (4.39)
IEKF equations and stability for the right invariant output (4.39)

The RIEKF (4.17) for the system (4.38), (4.39) is defined by:

d dt χt = 0 4,4 , χ+ t n = exp(L n [exp -1 ( χt n Y n )]) χt n ,
The right-invariant error is η t = χt χ -1 t and its evolution reads:

d dt η t = -η t w t , η + t n = exp(L n [exp -1 (η t n V n )])η t n . (4.40)
To linearize this equation we introduce the linearized error ξ t defined as η t = I 4 + L se(3) (ξ t ) and the linearized observation noise defined as [START_REF] Duncan | Some filtering results in Riemann manifolds[END_REF]) and removing the secondorder terms in ξ t , exp -1 (V n ) and w t we obtain:

V n = I 4 + L se(3) (v n ). Introducing η t = I 4 + L se(3) (ξ t ), η + t = I 4 + L se(3) (ξ + t ), exp(u) = I 4 + L se(3) (u) , η -1 t = I 4 -L se(3) (ξ t ), exp -1 [I 4 + L se(3) (u)] = u and exp -1 (ab) ≈ exp -1 (a) + exp -1 (b) in (4.
d dt ξ t = -w t , ξ + t n = ξ t n -L n (-ξ t n -v n ).
The gains L n are computed using the Riccati equation (4.18) and matrices A t , H, Q and N defined as:

A t = 0 3,3 , H = -I 6 , Q = Cov(w t ), N = Cov(exp -1 (V n )).
Proposition 13. The IEKF for the movement estimation problem (4.38), (4.39) is an asymptotically stable observer, as a straightforward application of Theorem 8.

3D pose and velocity estimation Original Model

We consider here the more complicated model of a vehicle evolving in the 3D space and characterized by its attitude R t , velocity v t and position x t . The vehicle is endowed with accelerometers and gyroscopes whose measures are denoted respectively by u t and ω t . The equation of the dynamics reads:

d dt R t = R t (ω t + w ω t ) × , d dt v t = g + R t (u t + w u t ) , d dt x t = v t . (4.41)
Two kinds of observations are considered:

Y n = x n +V n , (4.42)
or

Y 1 n , . . . ,Y K n = R T t n (p 1 -x 1 ) +V 1 n , . . . , R T t n (p 1 -x 1 ) +V K n , (4.43)
where

V n , V 1 n , . . . ,V K n are noises in R 3 . (4.42
) represents a position measure given by a GPS. IMU-GPS data fusion is of decisive industrial interest and EKF is a well-established method for this purpose [START_REF] Caron | Gps/imu data fusion using multisensor kalman filtering: introduction of contextual aspects[END_REF][START_REF] Farrell | Aided navigation: GPS with high rate sensors[END_REF][START_REF] Qi | Direct kalman filtering approach for gps/ins integration[END_REF][START_REF] Wendel | An integrated gps/mems-imu navigation system for an autonomous helicopter[END_REF]]. Yet, no convergence property has been obtained. Invariant methods have been proposed for inertial navigation [START_REF] Barczyk | Invariant observer design for a helicopter uav aided inertial navigation system[END_REF][START_REF] Bonnabel | A non-linear symmetry-preserving observer for velocity-aided inertial navigation[END_REF], only in the simplified situation where the system is reduced to attitude and velocity and assuming the GPS provides an absolute velocity measure. The resulting model is a combination of left-and right-invariant dynamics matching the usual theory. The extended framework proposed by the present document, combined with the introduction of a relevant group structure (see 2.4) makes straightforward the design of an IEKF for the whole attitude/velocity/position system although it does not verify the hypotheses required by invariant techniques. We don't have knowledge of an observer for this problem having convergence properties on uniformly observable trajectories, but the benefits of the IEKF below go beyond its nice theoretical guaranties. It is the core of a pool of patented methods developed for industrial purposes, whose results are presented in Chapter 5. They take into account many additional variables including bias and drifts and inherit largely of the global properties illustrated in Chapter 10.

(4.43) represents the measure of the relative position of known features (using for instance a depth camera): (p 1 , . . . , p K ) denote the position of the features in the static frame. The issue of merging IMU data with range and bearing measurements has been raised by the use of stereo cameras. Batch optimization has been used [START_REF] Konolige | Large-scale visual odometry for rough terrain[END_REF] but the most widespread approach is probably visual odometry followed by Kalman filtering (see, e.g., [START_REF] Rodolfo | Combining stereo vision and inertial navigation system for a quadrotor uav[END_REF][START_REF] Konolige | Large-scale visual odometry for rough terrain[END_REF][START_REF] Schmid | Stereo vision based indoor/outdoor navigation for flying robots[END_REF][START_REF] Vasconcelos | A nonlinear position and attitude observer on SE(3) using landmark measurements[END_REF] for recent publications). In the last years, the same question received a renewed attention due to the evolution of depth cameras and LiDAR [START_REF] Albert S Huang | Visual odometry and mapping for autonomous flight using an rgb-d camera[END_REF][START_REF] Kerl | Robust odometry estimation for rgb-d cameras[END_REF]. Theoretical stability of EKF-based methods used for this purpose have not been studied yet, but they can be obtained for the IEKF we propose as will be shown below.

Matrix form

As already noticed in the preliminary work [START_REF] Barrau | Invariant particle filtering with application to localization[END_REF], the system (4.41) can be embedded in the group of double homogeneous matrices (see Chapter 2) using the matrices χ t , w t and function f ω,u :

χ t =   R t v t x t 0 3,1 1 0 0 3,1 0 1   , w t =   (w ω t ) × w f t 0 3,1 0 1,3 0 0 0 1,3 0 0   , f ω,u :   R v x 0 3,1 1 0 0 3,1 0 1   →   R(ω) × g + Ru v 0 3,1 0 0 0 3,1 0 0   .
The equation of the dynamics becomes:

d dt χ t = f ω t ,u t (χ t ) + χ t w t , (4.44)
and the observations (4.42) and (4.43) find respectively the equivalent forms: The LIEKF (4.17) for the system (4.44), (4.45) reads:

Y n = χ t n 0 4,1 1 + V n 0 2,1 , (4.45)
(Y 1 n , . . . ,Y K n ) =   χ -1 t n   p 1 0 1   +V 1 n , . . . , χ -1 t n   p K 0 1   +V K n   . ( 4
d dt χt = f ω t ,u t ( χt ) , χ+ t = χt exp(L n ( χ-1 t Y n )). (4.47)
As the two last entries of matrix χ-1 t Y n are always zero one we can use a reduceddimension gain matrix Ln defined by L n = Ln p with p = (I 3 , 0 3,2 ). The left-invariant error is

η t = χ -1
t χt and its evolution reads:

d dt η t = f ω,u (η t ) -f ω,u (I 5 )η t -w t η t , (4.48)
η + t n = η t n exp Ln p η -1 t n 0 4,1 1 + χ-1 t n V n 0 2,1 . (4.49)
To linearize this equation we introduce the linearized error ξ t defined as [START_REF] Gauthier | Observability and observers for nonlinear systems[END_REF]) and removing the second-order terms in ξ t , V n and w t we obtain:

η t = I 3 +L se(3),2 (ξ t ). Introducing η t = I 3 + L se(3),2 (ξ t ), η + t = I 3 + L se(3),2 (ξ + t ), exp(u) = I 3 + L se(3),2 (u) and η -1 t = I 3 -L se(3),2 (ξ t ) in (4.
d dt ξ t =   -(ω t ) × 0 3,3 0 3,3 -(u t ) × -(ω t ) × 0 3,3 0 3,3 I 3 -(ω t ) ×   ξ t -w t , ξ + t n = ξ t n -Ln 0 3,3 0 3,3 I 3 ξ t n -RT t n V n .
The gains Ln are computed using the Riccati equation (4.18) and matrices A t , H, Q and N defined as:

A t =   -(ω t ) × 0 3,3 0 3,3 -(u t ) × -(ω t ) × 0 3,3 0 3,3 I 3 -(ω t ) ×   , H = 0 3,3 0 3,3 I 3 , Q = Cov(w t ), N = Cov R-1 t (V n ) .
Deriving a stability result of the IEKF for this system under the assumption the acceleration varies (as the state is not observable if the acceleration is constant) goes beyond the scope of the present work. However, due to the theoretical developments above, stability can be conjectured, and it is left for future work.

IEKF equations and stability for the right invariant output (4.43)

The RIEKF (4.17) for the system (4.44), (4.46) reads:

d dt χt = f ω t ,u t ( χt ) , χ+ t n = exp   L n   χt n Y 1 n . . . χt n Y K n     χt n .
As the two last entries of each matrix χ-1

t n Y k n are always zero, one we can use a reduceddimension gain matrix Ln defined by

L n = Ln p with p =    [I 3 , 0 3,2 ] . . . [I 3 , 0 3,2 ]   . The right-invariant error is η t = χt χ -1 t
and its evolution reads:

d dt η t = f ω,u (η t ) -η t f ω,u (I 5 ) -χt w t χ-1 η t , (4.50)
η + t n = exp           Ln p           η t n   p 1 0 1   + χt n V n 0 2,1
. . .

η t n   p K 0 1   + χt n V n 0 2,1                     η t n . (4.51)
To linearize this equation we introduce the linearized error ξ t defined as η t = I 3 +L se(3),2 (ξ t ). [START_REF] Neil J Gordon | Novel approach to nonlinear/non-gaussian bayesian state estimation[END_REF]), (4.51) and removing the second-order terms in ξ t , V n and w t we obtain:

Introducing η t = I 3 + L se(3),2 (ξ t ), η + t = I 3 + L se(3),2 (ξ + t ), exp(u) = I 3 + L se(3),2 (u) and η -1 t = I 3 -L se(3),2 (ξ t ) in (4.
d dt ξ t =   0 3,3 0 3,3 0 3,3 (g) × 0 3,3 0 3,3 0 3,3 I 3 0 3,3   ξ t -   Rt 0 3,3 0 3,3 ( vt ) × Rt Rt 0 3,3 ( xt ) × Rt 0 3,3 Rt   w t , ξ + t n = ξ t n -Ln     (p 1 ) × 0 3,3 -I 3 . . . (p K ) × 0 3,3 -I 3   ξ t n -   Rt n V 1 n . . . Rt n V K n     .
The gains Ln are computed using the Riccati equation (4.18) and matrices A t , H, Q and N defined as:

A t =   0 3,3 0 3,3 0 3,3 (g) × 0 3,3 0 3,3 0 3,3 I 3 0 3,3   , H =   (p 1 ) × 0 3,3 -I 3 . . . (p K ) × 0 3,3 -I 3   , Q =   Rt 0 3,3 0 3,3 ( vt ) × Rt Rt 0 3,3 ( xt ) × Rt 0 3,3 Rt   Cov(w t )   Rt 0 3,3 0 3,3 ( vt ) × Rt Rt 0 3,3 ( xt ) × Rt 0 3,3 Rt   T , N =    Rt n Cov(V 1 ) RT t n . . . Rt n Cov(V K ) RT t n    .
Proposition 15. If three non-collinear points are observed, then the IEKF is an asymptotically stable observer about any bounded trajectory.

Proof. According to theorem 8 we only have to ensure the couple (A,H) is observable. Integrating the propagation on one step we obtain the discrete propagation matrix Φ =  

I 3 0 3×3 0 3×3 t(g) × I 3 0 3×3 1 2 t 2 (g) × tId 3×3 I 3   .
The observation matrix is denoted H. We will show that [H; HΦ] has rank 9. We can keep only the rows corresponding to the observation of three non-collinear features p 1 , p 2 , p 3 . and denote the remaining matrix by H 1 . Matrices H 2 and H 3 , obtained using elementary operations on the columns of H 1 , have a rank inferior or equal to the rank of H 1 :

H 1 =         (p 1 ) × 0 3×3 -I 3 (p 2 ) × 0 3×3 -I 3 (p 3 ) × 0 3×3 -I 3 (p 1 ) × -1 2 t 2 (g) × -tI 3 -I 3 (p 2 ) × -1 2 t 2 (g) × -tI 3 -I 3 (p 3 ) × -1 2 t 2 (g) × -tI 3 -I 3         , H 2 =         (p 1 ) × 0 3×3 -I 3 (p 2 ) × 0 3×3 -I 3 (p 3 ) × 0 3×3 -I 3 -1 2 t 2 (g) × -tI 3 0 3×3 -1 2 t 2 (g) × -tI 3 0 3×3 -1 2 t 2 (g) × -tI 3 0 3×3         H 3 =     (p 1 -p 3 ) × 0 3×3 0 3×3 (p 2 -p 3 ) × 0 3×3 0 3×3 1 2 t 2 (g) × tI 3 0 3×3 (p 3 ) × 0 3×3 -I 3     The diagonal blocks -(p 1 -p 3 ) × 0 3×3 -(p 2 -p 3 ) × 0 3×3
, tI 3 and I 3 have rank 3 thus the full matrix has rank 9.

Conclusion

The methodology of the IEKF has been applied to the systems defined in Chapter 3, whose unusual properties have been used to derive stability guaranties under the same assumptions as in the linear case. These hypotheses have been easily checked for a long list of simple navigation examples. More complicated situations arising in industrial applications are presented in the next Chapter, along with results obtained on real and simulated data.

Chapter 5

Industrial applications

The methods used in this chapter are adaptations of those described in Chapter 4. They led to the registration of two patents (n°15-00654 and 14-01535) and the present results have been partially presented at the 2014 21 st Saint Petersburg International Conference on Integrated Navigation Systems. Note that, throughout the chapter, the level of details is limited by confidentiality of the results.

Chapter abstract When dealing with high precision navigation problems, in order to accurately estimate the position of the body, slowly time-varying biases affecting the gyroscopes and accelerometers measurements need be estimated online. Although the theory developed in the previous chapters does not readily apply to this extended problem, we show how to handle in practice the estimation of such additional variables. Then, we present some industrial results obtained in partnership with SAGEM. The IEKF is shown to outperform the conventional EKF on real experimental data for three different navigation problems.

Introduction

The present PhD thesis has been partly funded by the aerospace and defense company SAGEM (part of the Safran group), to improve its inertial navigation techniques. We give here a brief description of the specific problems motivating the theory presented in this document, as well as a methodology adopted to approach some applications of a higher degree of complexity than those mentioned in other chapters. Even if the following chapter is of reduced length and the exposure is limited by confidentiality requirements, it has been the object of many experiments, simulations, and hard work, and the results have been considered convincing enough to use the proposed algorithms in a currently developed military device. As such, it constitutes a key -if not the main -output of this PhD thesis. This chapter is divided into two sections. Section 5.2 discusses a generalized framework allowing to face most of the industrial problems encountered and Section 5.3 presents some inertial navigation issues and the striking improvements allowed by the algorithms implemented.

Autonomous error equations in practice

The perfect situation studied in chapter 4 is quite general, as illustrated by the various examples treated, but omits to simultaneously estimate biases in the inertial measurements. When it comes to high precision navigation, bias estimation is paramount. Of course, as there is no Lie group structure dictating the form of an EKF meant to simultaneously estimate biases, there is a great deal of freedom to devise such an EKF. What we propose is as follows.

A more general framework

The main idea allowing application of the EKFs described in this work to more complicated systems is to build an error following an autonomous equation for a simpler system with comparable behavior, then use it for the true system although most of the theoretical properties are lost. This approach is more justified than using a classical linear EKF which is, as already mentioned, an arbitrary and parameterization-dependent choice. To illustrate this approach we embed now the class of systems studied in the previous chapters into an environment impacting the dynamics and the observations:

d dt Θ = l(Θ), d dt χ t = f (Θ t ,u t ) (χ t ), Y = h(χ t • b, Θ) or h(χ -1 t b, Θ).
(

The core of the dynamics is still a function f verifying (3.13) but the inputs and observation function are both related to an additional vector variable Θ t ∈ R p . Moreover, the observation involves an additional function h possibly reducing the observability of the system. This framework can be used to describe most of the navigation problems.

"Imperfect" IEKF

The most straightforward way to extend the IEKF to the generalized situation (5.1) is to copy its error variable and add the errors regarding Θ as simple vector differences Θ t -Θt :

e t = (χ -1 t χt , Θ t -Θt ).
The matrices involved in the Riccati equation take the general form:

A t = A χ A Θ,χ 0 A Θ , H n = Dh H χ , H Θ ,
where Dh denotes the differential of h at χt n • b, Θt n . This method apparently sacrifices all the interesting properties of the IEKF, but its fundamental benefits are actually preserved.

The theoretical properties of an EKF using this kind of non-linear error variable will be extensively discussed in Part III of this document.

Fast Riccati integration

In this paragraph we underline some tricks to lower the numerical burden of gain computation. A common approximation in the integration of the Riccati equations appearing in Kalman filtering is:

t n+1 t n F t n+1 s Q s (F t n+1 s ) T ≈ (t n+1 -t n )Q t n+1 ,
where F t s = DΦ t s | Xs is the differential of the flow associated to the dynamics. This leads to the simplified equation:

P t n+1 = F n P t n F T n + (t n+1 -t n )Q t n+1 , where F n = F t n+1
t n . This makes the complexity of the integration depend only on the dynamics of the system, not on the error variable chosen, as shown by the following remark: Remark 6. Integration of the Jacobian F n is of equal difficulty for the left-and rightinvariant error as their Jacobian are related through the relation:

F t n+1 R t n = Ad χt n+1 F t n+1 L t n Ad χ-1 tn ,
where χt is any particular solution of the dynamics (note that the inversion of χ is trivial in all the examples given).

Proof. Let χt denote a particular solution of the dynamics and (Φ L , Φ R ) the flows associated to the left-and right-invariant errors. For any other solution χ t , with the usual notations

η L t = χ -1 t χt and η R t = χt χ -1 t we have η R t n+1 = χt n+1 η L t n+1 χ-1 t n+1 = χt n+1 Φ t n+1 L t n (η L t n ) χt n+1 (η R t ) = χt n+1 Φ t n+1 L t n ( χ-1 t n η R t n χt n ) χt n+1 .
A first-order expansion in η R t gives exactly:

F t n+1 R t n = Ad χt n+1 F t n+1 L t n Ad χ-1 tn .
Example 4. Consider the Riccati equation obtained in example 4.3.1. The Jacobian of the left-invariant error follows the equation:

d dt F tL t n =   0 0 0 0 0 -u t v t -v t u t v t 0   F tL t n .
The estimate χt is a particular solution over [t n ,t n+1 ], and the solution of the simplified Riccati equation associated to the right-invariant matrix is trivial (F tR t n = Id). We can thus compute instantaneously the solution for the right-invariant error:

F t n+1 L t n = Ad χt n+1 Ad ( χ+ tn ) -1 .
Going back to the general case (5.1), the Jacobian of the full system is the solution of:

F 0 = Id , d dt F t = A χ A Θ,χ 0 A Θ F t .
The solution takes the form F t = F χ F Θ,χ 0 F Θ . The method described above allows to optimize the computation of the top left block. Depending on the situation, the other blocks can become either trivial, or well approximated by a trivial solution. This can be illustrated by biased dynamics of the form d dt χ t = f (χ t ) + χ t b, for which the Jacobian describing the evolution of the left-invariant error is well approximated by:

F t ≈ F χ ∆T × Id 0 Id . Remark 7.
In this section, we gave some directions of the way to take advantage of the properties of systems having an error variable with autonomous error equation, when these are only part of a more complicated environment as in most engineering applications. The main interest of the theory thus appears to provide the practitioner with relevant error variables for the use of the non-linear state error based EKFs as presented in Chapter 9. Even if they lose their autonomy property due to the increased complexity of the system, a linear error variable has no reason to be preferred to them a priori. To be more specific, if a first system has observability properties comparable to those of a second system for which an error variable with autonomous equation is available, then this error should be also used for the first system. This heuristic finds a rigorous justification in the case described by Theorem 20 of Chapter 9: recycling the non-linear error variable of a simpler system can be sufficient to get rid of a dangerous non-linear effect described in details in Chapter 8 and known as false observability.

Another route to take advantage of the particular structure of a part of the state is to use sampling-based methods. The sampling space can thus be reduced by a Rao -Blackwellization procedure benefiting from the specificities of the Riccati equation involved in the Invariant Extended Kalman Filter as shown in Part II of the present thesis. The strong convergence properties of this latter method can be expected to have interesting consequences on the accuracy of the obtained sampling. No result in this direction is given here but addressing this issue seems an interesting research avenue.

Industrial applications

Inertial navigation

The basic function of an inertial navigation system is to produce real-time information about the attitude, velocity and position of a vehicle (aircraft, ship, submarine, car, etc.). It relies on an Inertial Measurement Unit, consisting of a set of accelerometers and gyroscopes (generally 3 of each). An accelerometer measures the projection over one axis of the sum of vehicle acceleration and gravitation. This vector is fully measured by three accelerometers forming a basis of the space. A gyroscope measures the projection of the angular rate of the vehicle over one axis, three non-coplanar gyroscopes measure the full instantaneous rotation vector. These measures are polluted by a noise whose modeling is a difficult problem in itself. In the design of filtering methods for inertial navigation, it is generally divided into two parts: a systematic error called "bias" or "drift", constant over time or having a well modeled behavior, and an additional perturbation having erratic behavior and assimilated to a white noise. The general equations in an inertial reference frame read:

d dt R t = R t (ω t + d t + w ω t ) × , d dt v t = R t ( f t + b t + w f t ) × + G(x t ), d dt x t = v t , (5.2)
where R t ∈ SO(3) denotes the attitude in an inertial reference frame, v t ∈ R 3 denotes the velocity, x t ∈ R 3 the position, ω t ∈ R 3 the measurements of the gyroscopes, f t ∈ R 3 the measurements of the accelerometers, d t ∈ R 3 and b t ∈ R 3 the biases of the gyroscopes and accelerometers respectively, w ω t and w f t the white additional noises and G : R ∈ R 3 → R ∈ R 3 the gravity field.

Owing to its nature, inertial navigation faces two major drawbacks. First it allows to maintain an estimate of attitude, velocity and position only if their initial value is known. Second, the sensor errors accumulate and make the estimate drift on the long run. Note that due to roundness of the Earth, an acceleration error does not lead to a position error growing quadratically over time [START_REF] Schuler | Die störung von pendel und kreiselapparaten durch die beschleunigung des fahrzeuges[END_REF] as could be expected. Anyway, coupling inertial navigation with another information is the usual way to circumvent these flaws. Three different situations where an IEKF has been successfully tested on real data are described in this chapter.

Static alignment

As previously mentioned, an initial guess of the attitude, velocity and position of the vehicle is necessary to start an inertial navigation. Finding position and velocity is generally the easy part, as the starting point of a mission is known quite precisely and can be given manually to the IMU. Concerning the velocity, the vehicle is likely to be stationed when the IMU is turned on. Finding the attitude is more tricky as no sensor can measure it accurately (a magnetic compass is subject to large errors incompatible with the requirements of inertial navigation). Fortunately, a stationed vehicle knowing it is stationed need not more information to estimate its attitude if the accuracy of its gyros is sufficient to make out the Earth rotation, whose projection over the horizontal plan (given by the accelerometer) is the north direction. Obviously, the procedure cannot be that simple as the possible rotations of the vehicle during alignment have to be taken into account. Moreover, some sensors bias are observable, some can be or not depending on the attitude changes during the alignment, some correlate with the attitude, and this information has to be encoded to improve future navigation. It makes this problem a perfect candidate for application of an EKF: the dynamics is given by the equations of inertial navigation where sensor biases are modeled (5.2) and the observation can be either position or velocity:

Y n = x t n +V x n or Y n = v t n +V v n .
Note that no velocity nor position sensor is actually used: Y n is the inertial position or velocity of the geographical point where the vehicle lies, V x or V v being the small variations of position and velocity around these reference values. This kind of virtual measurement is usually called ZUPT (Zero velocity UPdaTe). The Kalman filtering framework proves extremely convenient here: the algorithm stays the same although the observability properties of the problem vary depending on the situation as mentioned above. This made EKF the classical approach to this problem. Unfortunately, application of this local method requires an accurate initialization. Thus, a procedure consisting of three steps is usually implemented:

Vertical direction search: over a few seconds, the accelerometer measures are corrected from rotations thanks to the gyro then averaged to obtain a first guess of the vertical.

Coarse alignment: an approximate but reliable estimate of the attitude is computed.

Here different methods can be used, for instance building an approximate but linear modeling of the system and applying a classical Kalman filter.

Fine alignment: starting from a coarse estimate of the attitude (error within a few degrees), the EKF is applied on the full non-linear system to compute an estimate of all variables (including biases), as well as the covariance matrix of the errors.

Implementation of this procedure is heavy, as handling transitions between the different phases is not straightforward: the instant they are performed has to ensure the desired precision is achieved, at the risk of jeopardizing the final result. But waiting too much also means increasing the duration of the alignment. As the systems under consideration are embedded, having several phases also implies a more complicated validation process.

A way to circumvent these difficulties is IEKF. The problems caused here by large initial errors are similar to the one exposed in Chapter 10: state estimation error is mainly caused by one single variable (heading uncertainty) and the true state is thus (almost) constrained in a one-dimensional (non-linear) manifold. This constraint is natively preserved by the estimate of the IEKF, but not by the one of the EKF as shown in Chapter 10, for which the authorized manifold is left after the first update and never reached again (see Figure 10.6 of Chapter 10). Thus, the convergence radius of the IEKF is extremely large, which makes possible to use it without needing a first guess of the attitude. The alignment procedure then boils down to one step: applying an IEKF.

The method has been tested on real data from an IMU put in a car at stand, but with running engine producing small movements. The results are displayed on Figure 5.1. The blue line is the heading estimate of the IEKF, the green line the estimate of the classical method. The vertical line is a consequence of the three-steps procedure: although already computed during the coarse alignment phase, the heading estimate is displayed here only from the beginning of the fine alignment. The transition from a memorized value creates this artifact. As the experiment is performed in real conditions, no ground truth is available. Checking the validity of the method thus requires the assumption that the classical algorithm implemented in the software of the IMU is also correct. Looking at Figure 5.1, two conclusions can be drawn for the specific situation considered in the experiment:

1. The IEKF converges to the correct value of the heading, or at least to the same The blue line is the heading estimate of the IEKF, the green line the estimate of the commercial software. The vertical line is a consequence of the three-steps procedure: although already computed during the coarse alignment phase, the heading estimate is displayed here only from the beginning of the fine alignment. The transition from a memorized value creates this artifact. We see that the IEKF converges to the same value as the classical method although starting very far (120 degrees) from the true value.

value as the commercial software, although the initial heading error is extremely large (around 120 degrees here).

2. The coarse estimation steps being avoided, the convergence is much faster.

Prolonged static pose

Chapter 8 will explain in details the problem of "false observability": due to updates of the linearization point the observability properties of the linearized system used by the EKF can differ from those of the true system. IEKF is an efficient response to this issue as shown by Remark 17 in chapter 9. A convincing illustration is given by the following problem encountered in operational situations. A vehicle keeps the same pose for several hours while undergoing small zero-mean translation movements, and maintains its attitude estimate using the ZUPT mentioned above. In this case, no information can ever be obtained concerning the projection of the gyro biases over the west direction. To be more specific, distinguishing between a variation of this value and a variation of the heading is impossible. Yet, on the long run, the EKF creates artificial information as in the SLAM problem exposed in Chapter 9 and the estimated heading drifts as displayed on Figure 5.2 (purple plot). An EKF re-implemented from scratch and used on the same data encounters the exact same problem (black plot), which proves this is not due to a bug in the software.

A first solution to the problem is the Observability-Constrained EKF (OC-EKF) proposed in [START_REF] Guoquan | Analysis and improvement of the consistency of extended kalman filter based slam[END_REF]. Once the false observability issue has been identified and the dimension of the non-observable subspace analytically computed, the linearization is tuned to ensure the non-observable subspace to keep the right dimension. The results of this algorithm are displayed on Figure 5.2 (green plot). We see that the problem is solved: the drift of the heading estimate disappeared. But the filter has been explicitly "told", by a user possessing side information on the experiment the IMU is undergoing, to maintain a given dimension of the non-observable subspace. This can be worrying regarding possible unexpected consequences, and brings an additional issue: if a true attitude change occurs, making the problematic direction observable, it has to be detected to allow transition to a classical EKF.

An IEKF has been implemented for the same problem, but no constraint regarding non-observability was added. The results are displayed on Figure 5.2 (red plot): this also solves the problem and leads to the same estimate as the OC-EKF, but without ah-hoc patch-up. The blue plot is a second IEKF, modeling only attitude, position and velocity which prevents of course false observability as all its variables are observable. We see that the IEKF modeling the full system (red plot) gives the same results and thus is not perturbed by non-observability. Moreover, a procedure detecting the problematic situation in order to trigger the OCEKF is not required anymore. Nevertheless, note that the OC-EKF has the advantage of being easily added to an existing system, which is not the case of the IEKF.

In-flight alignment

The last practical situation considered in this document is the initialization of the navigation performed while the vehicle is moving, using a GPS aid instead of the "small movement" information. Here again, this functionality is usually achieved through a gradual fine-tuning possibly demanding operational constraints. The results of the software in use in Sagem's IMUs are not going to be displayed here. Yet, the benefits of the IEKF can be easily illustrated first through a comparison with the standard EKF on real data, then through a statistical analysis of the results over a large sample of simulated trajectories.

Figure 5.3 displays the estimation error of an EKF and an IEKF, tuned with the exact same parameters, for a GPS-INS alignment using real inertial measurements. We see the estimate of the EKF deteriorates rapidly as the initial value of the heading error grows. Of course, this specific plot is not representative of the behavior of an operational system as the EKF is not used until a reliable first guess of the attitude has been obtained (see above). It simply illustrates why a much more complicated procedure is required, usually limiting the movements of the vehicle during the alignment. On the other hand, as in the static case, the convergence of the IEKF is not disturbed by large initial errors, which Figure 5.2 -Prolonged static pose during which a "small movement" measure is provided to the EKF. This information is sufficient to maintain the yaw estimate thanks to the earth rotation. The true yaw is constant, but a specific pattern of small translations of the IMU makes the linearization point of the EKF move slightly at each update. The resulting linearized system has a non-observable subspace of smaller dimension than the true system. This has dramatic consequences on the yaw estimate: it drifts over time (purple plot) although the true yaw is constant. To make sure this is not due to a bug in the software, an EKF has been re-implemented from scratch with the exact same results (black plot). An engineer proposed and successfully implemented the OC-EKF introduced in [START_REF] Guoquan | Observability-based rules for designing consistent ekf slam estimators[END_REF]: the dimension of the non-observable subspace is analytically derived and the linearization point of the EKF is modified to preserve it (green plot). A method based on the IEKF, designed without taking into account the possible false observability issue, has been used on the same data (red plot): it did not encounter the problem. Superposed with the red plot is the result of a similar method, but where only the observable part of the system is modeled. The results are almost equivalent. This proves that the first method, like linear filters, is not perturbed by the addition of non-observable states. allows a great simplification of the general method: the successive fine-tuning steps can be skipped.

Results of Figure 5.4 were extracted from a simulations campaign aiming at the implementation of the IEKF on a commercial product. One of its goals was to verify the coherence of the estimation errors with the covariance matrix provided by the filter. Some results are displayed here but this time, the physical quantities estimated are not given, nor the units. 1000 sequences of increments are generated and the initializations are randomly distributed. We can see that the error over time exactly matches the 3σ envelopes, as in the linear case. This is a good argument confirming that the performances of the method are close to optimality. After thousands of simulations including those presented here, the decision was made to shift from EKF to IEKF, at least for the considered product.

Conclusion

We gave in this chapter the most decisive applications of the theoretical results presented in the rest of the document, and the general form of the equations involved. The existence of commercial products based on the EKF allows a fair comparison with a method relying on the IEKF. In several crucial situations, the second one results in dramatic improvement, possibly allowing a great simplification of the algorithms in use, of their design and validation process, as well as better performance and more flexibility for the user.

This closes the first part of the thesis, devoted to the introduction and study of a class of estimation problems generalizing both invariant and linear systems.

Stochastic properties of invariant observers

The present chapter has been published in IEEE Transactions on Automatic Control.

Chapter abstract

In the present chapter we address the problem of filtering in the context of an autonomous error equation: the state is now seen as a random process and the quantity of interest becomes its density over time. The interpretation of the stochastic equations involved (Ito or Stratoniovitch) is discussed. The error variable is shown to be a time-homogeneous Harris chain (a Markov chain taking values in a continuous space). We show that if the estimation error almost globally converges for noise turned off, then when noises are turned on the error converges in law to a distribution that is independent of the initial error density. This novel result is illustrated on concrete examples, and leveraged to optimize non-linear gains through the asymptotic error distribution they produce. The transitory phase is then studied and a gain tuning relying on particle sampling is proposed.

Introduction

In this chapter, the state of the system is studied as a stochastic process, which requires some clarifications regarding the sense of the equations involved (Itô or Stratonovitch). Because of this additional difficulty, the generalized framework built in Part I is temporarily dropped in favor of simplified equations. This makes all the computations much more readable as the dynamics appears under a concrete matrix form instead of involving a function f u t assumed to verify a given property. This is a handicap regarding the direct application of the results to some inertial navigation problems mentioned earlier, but the properties derived here would actually hold in the general framework under similar hypotheses. This simplification is used only in the present chapter.

Its contributions and organization are as follows. In Section 6.2, the problem of stochastic filtering on Lie groups with a continuous-time noisy model and discrete-time observations is rigorously posed. The problem is then transformed into a complete discrete-time model. The discretization is not straightforward because unlike the linear case, difficulties arise from non-commutativity. We propose in Section 6.3 a class of natural filters appearing as a loosening of the IEKF presented in Chapter 4, where the gains are allowed to be tuned by any other method than a Riccati equation. The proposed class is very broad and the tuning issue is far from trivial. Sections 6.4 and 6.5 explore two different routes, depending on if we are interested in the asymptotic or transitory phase. In Section 6.4, we propose to hold the gains fixed over time. As a result, the error equation becomes a homogeneous Markov chain. We first prove some new results about global convergence in a deterministic and discrete-time framework. Then, building upon the homogeneity property of the error, we prove that if the filter with noise turned off admits almost global convergence properties, the error with noise turned on converges to a stationary distribution. Mathematically this is a very strong and to some extent surprising result. From a practical viewpoint, the gains can be tuned numerically to minimize the asymptotic error's dispersion. This allows to "learn" sensible gains for very general types of noises. The theory is applied to two examples and gives convergence guarantees in each case. First an attitude estimation problem using two vector measurements and a gyroscope having isotropic noise (Section 6.4.2), then the construction of an artificial horizon with optimal gains, for a non-Gaussian noise model (Section 6.4.3). Each application is a contribution in itself and can be implemented without reading the whole thesis. In Section 6.5, we propose to optimize the convergence during the transitory phase using Gaussian approximations. We first stick to the general purpose of this thesis manuscript and adapt the IEKF studied in previous chapters to this discrete situation. As the linearizations always occur around the same point, the linearized model is time-invariant and thus the Kalman gains, as well as the Kalman covariance matrix, are proved to converge to fixed values (note that when on-board storage and computational resources are very limited, this advantageously allows to replace the gain with its asymptotic value. The IEKF is compared to the well-known MEKF [START_REF] Crassidis | A survey of nonlinear attitude estimation methods[END_REF][START_REF] Lefferts | Kalman filtering for spacecraft attitude estimation[END_REF] and UKF [START_REF] Crassidis | Unscented filtering for spacecraft attitude estimation[END_REF][START_REF] Julier | A new extension of the Kalman Filter to Nonlinear Systems[END_REF] on the attitude estimation problem, and simulations illustrate some convergence properties that the latter lack. In the case where the error equation is fully autonomous, we introduce a new method based on off-line simulations, the IEnKF, which outperforms the other filters in case of large noises by capturing very accurately the error's dispersion.

Problem setting

Considered continuous-time model

Consider a state variable χ t taking values in a matrix Lie group G with neutral element I d , and the following continuous-time model with discrete measurements:

d dt χ t = (υ t + w t )χ t + χ t ω t , (6.1)
Y n = h(χ t n ,V n ), (6.2)
where υ t and ω t are inputs taking their values in the Lie algebra g (i.e. the tangent space to the identity element of G), w t is a continuous white Langevin noise with diffusion matrix Σ t , whose precise definition will be discussed below in Subsection 6.2.3. (Y n ) n 0 are the discrete-time observations, belonging to some measurable space Y and V n is a noise taking values in R p for an integer p > 0. We further make the following additional assumption:

Assumption 1 (left-right equivariance) The output map h is left-right equivariant, i.e. there exists a left action of G on Y such that we have the equality in law:

∀g, χ ∈ G, h(χg,V n ) L = g -1 h(χ,V n ), (6.3)
and h(χg, 0) = g -1 h(χ, 0). (6.4)

The reader who is not familiar with stochastic calculus and Lie groups can view χ t as a rotation matrix, and replace the latter property with the following more restrictive assumption:

Assumption 1 bis For any n 0 there exists a vector b such that:

h(χ,V n )=χ -1 (b +V n ).
The assumptions could seem restrictive but are verified in practice in various cases as shown by the following examples, which will provide the reader with a more concrete picture than the formalism of Lie groups.

Examples

Attitude estimation on flat earth

Our motivating example for the model (6.1)-(6.2) is the attitude estimation of a rigid body assuming the earth is flat, and observing two vectors:

d dt R t = R t (ω t + w ω t ) × , Y n = (R T t n b 1 +V 1 n , R T t n b 2 +V 2 n ), (6.5)
where as usual R t ∈ SO(3) represents the rotation that maps the body frame to the earthfixed frame, and where ω t ∈ R 3 is the instantaneous rotation vector, and w ω t ∈ R 3 is a continuous Gaussian white noise representing the gyroscopes' noise. We have let (x) × ∈ R 3×3 denote the skew matrix associated with the cross product with a three dimensional vector, i.e., for a, x ∈ R 3 , we have (x

) × a = x × a. (Y n ) n≥0 is a sequence of discrete noisy measurements of two vectors b 1 , b 2 of the earth fixed frame verifying b 1 × b 2 = 0, and V 1 n and V 2
n are sequences of independent isotropic Gaussian white noises. Note that the noise w ω t is defined, as the input ω t , in the body frame (in other words it is multiplied on the left). Thus equations (6.5) do not match (6.1)-(6.2) which correspond to a noise defined in the earth-fixed frame. This can be remedied in the particular case where the gyroscope noise is isotropic, a restrictive yet relevant assumption in practice.

Definition 7.

A Langevin noise w t of g is said isotropic if wt := gw t g -1 L = w t for any g ∈ G.

Note that for a noise taking values in so(3) this definition corresponds to the physical intuition of an isotropic noise of R 3 . With this additional assumption the equation can be rewritten:

d dt R t = ( wω t ) × R t + R t (ω t ) × , Y n = (R T t n b 1 +V 1 n , R T t n b 2 +V 2 n
), which corresponds to (6.1)-(6.2).

Remark 8.

If the gyrometer noise is not isotropic the new noise wω t is related to R t by wω t = R t w ω t R T t . Depending on the degree of anisotropy this can prevent the use of methods based on the autonomy of the error (see 6.4) but not of the discrete-time IEKF (see 6.5.1).

Attitude estimation on a round rotating Earth

Another interesting case is attitude estimation on SO(3) using an observation of the vertical direction (given by an accelerometer) and taking into account the rotation of the earth. Whereas the flat earth assumption perfectly suits low-cost gyroscopes, precise gyroscopes can measure the complete attitude by taking into account the earth's rotation. We define a geographic frame with axis North-west-up. The attitude R t is the transition matrix from the body reference to the geographic reference. The earth instantaneous rotation vector υ ∈ R 3 and the gravity vector g ∈ R 3 are expressed in the geographic reference. Both are constant in this frame. The gyroscope gives a continuous rotation speed ω t , disturbed by an isotropic continuous white noise w ω t . The equation of the considered system reads:

d dt R t = (υ) × R t + R t (ω t + w ω t ) × , Y n = R T t n (g +V n ).

As w ω

t is supposed isotropic the equation can be rewritten:

d dt R t = (υ + wω t ) × R t + R t (ω t ) × , Y n = R T t n (g +V n )
, which corresponds to (6.1)-(6.2).

Attitude and velocity estimation on a flat Earth

We give here an example on a larger group. Consider the attitude R t ∈ SO(3) and speed v t ∈ R 3 of an aircraft evolving on a flat earth, equipped with a gyroscope and an accelerometer. The gyroscopes give continuous increments ψ t ∈ R 3 with isotropic noise w ψ t ∈ R 3 , and the accelerometers give continuous increments a t ∈ R 3 with isotropic noise w a t ∈ R 3 . The aircraft has noisy speed measurements in the earth reference frame Y n = v t n + V n , where V n is a supposed to be a Gaussian isotropic white noise. The equations read:

d dt R t = R t (ψ t + w ψ t ) × , d dt v t = R t (a t + w a t ) + g,
where R t is the rotation mapping the body frame to the earth-fixed frame, v t is the velocity expressed in the earth fixed frame, and g is the earth gravity vector, supposed to be (locally) constant. Using the matrix Lie group SE(3) we introduce:

A t = R T t R T t v t 0 1 , ω t = 0 g 0 0 , υ t = -(ψ t ) × a t 0 0 , w t = (w ψ t ) × w a t 0 0 ,
The problem can then be rewritten under the form (6.1)-( 6.2):

d dt A t = (υ t + w t )A t + A t ω t , Y t n = A -1 t n V n 1 .

Interpretation of Langevin noises on Lie Groups

This present subsection provides some mathematical considerations about white noises on Lie groups. It can be skipped by the uninterested reader who is directly referred to the model (6.6). Equation (6.1) is actually a Langevin equation that suffers from poly-interpretability because of its non-linearity, and its meaning must be clarified in the rigorous framework of stochastic calculus. Stratonovich stochastic differential equations on a Lie group can be intrinsically defined as in e.g. [START_REF] Liao | Lévy processes in Lie groups[END_REF]. A somewhat simpler (but equivalent) approach consists of using the natural embedding of the matrix Lie group G in a matrix space and to understand the equation in the sense of Stratonovich, see e.g. [START_REF] Said | Estimation and filtering of processes in matrix Lie groups[END_REF]. The mathematical reasons stem from the fact that the resulting stochastic process is well-defined on the Lie group, whereas this is not the case when opting for an Ito interpretation as underlined by the following easily provable proposition. Proposition 16. If the stochastic differential equation (6.1) is taken in the sense of Itô, for G = SO(3) embedded in R 3×3 , an application of the Itô formula to χ T t χ t shows that the solution almost surely leaves the submanifold G for any time t > 0.

Besides, the physical reasons for this stem from the fact that the sensors' noise are never completely white, and for colored noise the Stratonovich interpretation provides a better approximation to the true solution than Ito's, as advocated by the result of Wong and Zakai [START_REF] Wong | Riemann-Stieltjes approximations of stochastic integrals[END_REF].

Exact discretization of the considered model

To treat rigorously the problem of integrating discrete measurements we need to discretize the continuous model with the same time step as the measurements'. Unlike the general case of non-linear estimation, the exact discrete-time dynamics corresponding to Equation (6.1) can be obtained, as proved by the following result: Theorem 9. Let χ n = χ t n . Then the discrete system (χ n ,Y n ) satisfies the following equations: Proof. For n ∈ N consider the value of the process χ t is known until time t n . Let ϒ t and Ω t be the solutions of the following equations:

χ n+1 = ϒ n W n χ n Ω n , Y n = h(χ n ,V n ), (6.6
ϒ t n = I d , d dt ϒ t = υ t ϒ t ,
and

Ω t n = I d , d dt Ω t = Ω t ω t .
Let W t be the solution of the following (Stratonovich) stochastic differential equation:

W t n = I d , d dt W t = ϒ -1 t w t ϒ t W t .
Note that ϒ -1 t w t ϒ t being in g, W t is ensured to stay in G. Define the process χ t|t n = ϒ t W t χ t n Ω t . We will show that for t > t n the processes χ t and χ t|t n verify the same stochastic differential equation. Indeed:

d dt χ t|t n = d dt ϒ t W t χ t Ω t + ϒ t d dt W t χ t Ω t + ϒ t W t χ t d dt Ω t = υ t ϒ t W t χ t Ω t + w t ϒ t W t χ t Ω t + ϒ t W t χ t Ω t ω t = (υ t + w t )χ t|t n + χ t|t n ω t .
Thus In the sequel, for mathematical reasons (the equations do not suffer from poly-interpretability), tutorial reasons (the framework of diffusion processes on Lie groups needs not be known), and practical reasons (any filter must be implemented in discrete time), we will systematically consider the discrete-time model (6.6). Moreover, the noise W n will be a general random variable in G, not necessary a solution of the stochastic differential equation

d dt W t = ϒ -1 t w t ϒ t W t .

A class of discrete-time intrinsic filters 6.3.1 Proposed intrinsic filters

Inspired by the theory of continuous-time symmetry-preserving observers on Lie groups [START_REF] Bonnabel | Non-linear symmetry-preserving observers on Lie groups[END_REF] and the IEKF described in Chapter 4 we propose a class of multiplicative filters defined by:

χn+1 = ϒ n χ+ n Ω n , (6.7
)

χ+ n+1 = K n+1 ( χn+1 Y n+1 ) χn+1 , (6.8)
where the superscript + denotes the value of the estimate after the update and K n+1 (•) can be any function of Y → G, ensuring K(h(I d , 0)) = I d . The multiplicative error does not change:

η n = χ n χ-1 n , η + n = χ n χ+ n -1 . (6.9)
We have the following striking property, that is similar to the linear case:

Theorem 10. The error variables η n and η + n are Markov processes, and are independent of the inputs (Ω n ) n 0 . Proof. The equations followed by η n and η + n read:

η n+1 = χ n+1 χ-1 n+1 = ϒ n W n χ n Ω n Ω -1 n χ+ n -1 ϒ -1 n = ϒ n W n η + n ϒ -1 n , (6.10)
and: (6.11) thanks to the equivariance property of the output.

η + n+1 = χ n+1 χ-1 n+1 K n+1 ( χn+1 Y n+1 ) -1 = η n+1 K n+1 ( χn+1 h(χ n+1 ,V n+1 )) -1 L = η n+1 K n+1 (h(η n+1 ,V n+1 )) -1 ,
The most important consequence of this property is that if the inputs υ t are known in advance, or are fixed, as it is the case in the two first examples of 6.2.2, the gain functions K n can be optimized off-line, independently of the trajectory followed by the system. In any case, numerous choices are possible to tune the gains K n and the remaining sections are all devoted to various types of methods to tackle this problem.

Fixed gains filters

In certain cases, one can build an (almost) globally convergent observer for the associated deterministic system, i.e., with noise turned off, by using a family of constant gain function K n (•) ≡ K(•). If the filter with noise turned off has the desirable property of forgetting its initial condition, convergence to a single point is impossible to retain, because of the unpredictability of the noises that "steer" the system, but convergence of the distribution can be expected, assuming as in the linear case that ϒ n is independent of n. Indeed in this section we prove that, when noise is turned on, the error forgets its initial distribution under mild conditions. The results are illustrated by an attitude estimation example for which we propose an intrinsic filter having strong convergence properties.

It should be noted that in practice, the convergence to an invariant distribution allows in turn to pick the most desirable gain K(•) among the family based on a performance criterion, such as convergence speed, or filter's precision (that is, ensuring low error's dispersion). This fact will be illustrated by the artificial horizon example of Subsection 6.4.3.

Convergence results

Here the left-hand inputs ϒ n are assumed fixed. This, together with constant gains K n , makes the error sequence η n a homogeneous Markov chain. Thus, under appropriate technical conditions, the chain has a unique stationary distribution and the sequence converges in distribution to this invariant distribution. Let d denote a right-invariant distance on the group G. We propose the following assumptions:

1. Confinement of the error: there exists a compact set C such that ∀n ∈ N, η n ∈ C a.s.

for any η 0 ∈ C.

2. Diffusivity: the process noise has a continuous part with respect to Haar measure, with density positive and uniformly bounded from zero in a ball of radius α > 0 around I d .

Reasonable output noise:

∀g ∈ G, P gK(h(g,V n )) -1 ∈ B o gK(h(g, 0)) -1 , α 2 > ε for some ε > 0.

The second assumption implies, and can in fact be replaced with, the more general technical assumption that there exists ε > 0 such that for any subset U of the ball B o (I d , α) we have P(W n ⊂ U) > ε µ(U) for all n ≥ 0 where µ denotes the Haar measure. Those noise properties are relatively painless to verify, whereas the confinement property although stronger is automatically verified whenever G is compact, e.g. G = SO(3). Intuitively, the last two assumptions guarantee the error process is well approximated by its dynamics with noise turned off, followed by a small diffusion. In the theory of Harris chains, the latter diffusion step is a key element to allow probability laws to mix at each step and eventually forget their initial distribution.

Theorem 11. For constant left-hand inputs ϒ n ≡ ϒ, consider the filter:

χn+1 = ϒ χ+ n Ω n , (6.12
)

χn+1 = K( χ n+1 Y n+1 ) χ n+1 . (6.13)
Suppose that Assumptions 1)-2)-3) are verified, where the compact set satisfies C = cl(C o ), cl denoting the closure and o the interior. When noises are turned off, the error equation (6.10)-(6.11) becomes:

γ n+1 = ϒγ + n ϒ -1 , γ + n+1 = γ n+1 K n+1 (h(γ n+1 , 0)) -1 . (6.14)
Suppose that for any γ 0 ∈ C, except on a set of null Haar measure, γ n converges to I d . Then there exists a unique stationary distribution π on G such that for any prior law µ 0 of the error η 0 supported by C, the law (µ n ) n≥0 of (η n ) n≥0 satisfies the total variation (T.V.) norm convergence property:

µ n -π T.V. -→ n→∞ 0.
Corollary 2. When the group G is compact, The convergence results of Theorem 11 hold globally, i.e. without the confinement assumption 1). Theorem 12. Under the assumptions of Theorem 11, assuming only h(γ n , 0) → 0 instead of almost global convergence of (γ n ) n≥0 , that G is compact, the set K = {g ∈ G, h(g, 0) = h(I d , 0)} connected and h(ϒ, 0) = h(I d , 0), the results of Theorem 11 are still valid. Moreover, if W n is isotropic, we have π( ΥV ) = π(V ) for any Υ ∈ K commuting with ϒ ( Υϒ = ϒ Υ).

The proofs of the results above have all been moved to Appendix B.1.

Application to attitude estimation

Consider the attitude estimation example of Subsection 6.2.2. In a deterministic and continuous time setting, almost globally converging observers have been proposed in several papers (see e.g. [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF][START_REF] Salcudean | A globally convergent angular velocity observer for rigid body motion. Automatic Control[END_REF]) and have since been analyzed and extended in a number of papers. In order to apply the previously developed theory to this example, the challenge is twofold. First, the deterministic observer must be adapted to the discrete time and proved to be almost globally convergent. Then, the corresponding filter must be proved to satisfy the assumptions of the theorems above in the presence of noise. In discrete time, the system equations read:

R n+1 = W n R n Ω n , Y n = (R T n b 1 +V 1 n , R T n b 2 +V 2 n ),
with the notations introduced in 6.2.2. We propose the following filter on SO(3):

Rn+1 = R+ n Ω n , R+ n+1 = K( Rn+1 Y n+1 ) Rn+1 , with K(y 1 , y 2 ) = exp(k 1 (y 1 × b 1 ) + k 2 (y 2 × b 2 )), k 1 > 0, k 2 > 0, k 1 + k 2 1.
(6.15) Proposition 17. With noise turned off, the discrete invariant observer (6.15) is almost globally convergent, that is, the error converges to I d for any initial condition except one.

The proof has been moved to the Appendix, only the main idea is given here. For the continuous-time deterministic problem it is known, and easily seen, that

E : γ → k 1 ||γ T b 1 - b 1 || 2 + k 2 ||γ T b 2 -b 2 || 2 is
a Lyapunov function, allowing to prove almost global convergence of the corresponding observer. In the discrete time deterministic case, the function above remains a Lyapunov function for the sequence (γ n ) n≥0 , allowing to derive Proposition 17. This is not trivial to prove, and stems from the more general following novel result: Proposition 18. Consider a Lie group G equipped with a left-invariant metric ., . , and a left-invariant deterministic discrete equation on G of the form:

γ n+1 = γ n exp(-k(γ n )).
γ n → n→∞ I d .
The proof has been moved to Appendix B.2.1. Note that, the latter property is closely related to Lemma 2 of [START_REF] Tron | Riemannian consensus for manifolds with bounded curvature[END_REF]. Using Theorem 11 and Proposition 17 we finally directly get:

Theorem 13. The distribution of the error variable of the invariant filter (6.15) converges for the T.V. norm to an asymptotic distribution, which does not depend on its initial distribution.

Learning robust gains: application to the design of an artificial horizon

As proved in 6.4.1, under appropriate conditions the error variable is a converging Markov chain whose asymptotic law depends on the gain function but not on the trajectory followed by the system (which is a major difference with most nonlinear filters, such as the EKF). Hence a fixed gain can be asymptotically optimized off-line, leading to a very low numerical cost of the on-line update.

A classical aerospace problem is the design of an artificial horizon using an inertial measurement unit (IMU). An estimation of the vertical is maintained using the observations of the accelerometer (which senses the body acceleration minus the gravity vector, expressed in the body frame) and the stationary flight assumption according to which the body's linear velocity is constant. The problem is that this approximation is not valid in dynamical phases (take-off, landing, atmospheric turbulence), which are precisely when the artificial horizon is most needed. The problem is generally stated as follows:

d dt R t = R t (ω t + w t ) × , Y n = R T t n g +V n + N n
, where R t is the attitude of the aircraft (the rotation from body-frame coordinates to inertial coordinates), ω t is the continuous-time gyroscope increment and Y n is the observation of the accelerometer. The sensor noises w t and V n can be considered as Gaussian, and N n represents fluctuations due to accelerations of the aircraft that we propose to model as follows: N n is null with high probability but when non-zero it can take large values. The N n 's are assumed to be independent as usually.

Convergence results

Consider the following class of filters:

Rn+1 = R+ n Ω n , R+ n+1 = K( Rn+1 Y n+1 ) Rn+1 , with K(y) = exp( f k,λ (y)) (6.16)
where f k,λ (x) = k. min(angle(x, g), λ ) x×g ||x×g|| if x × g = 0 and f k,λ (x) = 0 otherwise. The rationale for the gain tuning is as follows: if the accelerometer measures a value y, we consider the smallest rotation giving to g the same direction as y. Conserving the same axis, the angle of this rotation is thresholded (hence the parameter λ ) to give less weight to outliers (without purely rejecting them, otherwise the filter couldn't converge when initialized too far). Then we choose as a gain function a rotation by a fraction of the obtained angle. We begin with the following preliminary result: Lemma 1. For any 0 < k 1 and 0 < λ π the output error Y n -RT n g associated to the observer (6.16) with noise turned off converges to 0.

Proof. Let us consider the error evolution when the noise is turned off. It writes γ n+1 = γ n exp(-f k,λ (γ -1 n g)), thus we have γ -1 n+1 g = exp( f k,λ (γ -1 n g))γ -1 n g. As for any n ∈ N, f k,λ (γ -1 n g) is orthogonal to g and γ -1 n g, γ -1 n+1 g stays in the plane spanned by g and γ -1 0 g, as well as the whole sequence (γ -1 n g) n 0 . Let φ n = angle(γ -1 n g, g). The dynamics of (φ n ) writes: φ n+1 = φ nk. min(λ , φ n ). Thus φ n goes to 0, i.e: γ -1 n g → n→∞ g, i.e. the observation error goes to 0.

The following result is a mere consequence of Lemma 1 and Theorem 12.

Proposition 19. The error variable associated to the filter defined by (6.16) converges to a stationary distribution for the T.V. norm, which does not depend on its initial distribution.

Numerical asymptotic gain optimization

To each couple (k, λ ) we can associate an asymptotic error dispersion (computed in the Lie Algebra) associated to the corresponding stationary distribution, and try to minimize it. As all computations are to be done off-line, the statistics of all distributions can be computed using particle methods. Table 6.1 gives the parameters of the model used in the following numerical experiment. Figure 6.1 displays the Root Mean Square Error RMSE = E(η ∞ gg), computed over a grid for the parameters (k, λ ). The minimum is obtained for k = 0.1202 and λ = 0.0029. If we compare it to a MEKF, we observe a huge difference. For our asymptotic invariant filter we get RMSE = 8.02 × 10 -4 . For the MEKF, the observation noise matrix giving the best results leads to the value RMSE = 4.3 × 10 -3 . This result is not surprising due to the fact that the outliers significantly pollute the estimates of the Kalman filter (see Fig. 6.1). This illustrates the fact that when the noise is highly non-Gaussian, an asymptotic gain with some optimality properties can still be found.

Gaussian filters

The present section focuses more on the transitory phase, and the gain is computed at each step based on Gaussian noise approximations and linearizations. Beyond the local optimization underlying the gain computation, the two following filters have the merit to readily convey an approximation of the extent of uncertainty carried by the estimations. We first derive in discrete time the IEKF described in Part I. Then we introduce the IEnKF, which computes the empirical covariance matrix of the error using particles. As all computations can be done off-line, it is easily implementable, as long as the gains can be stored, and is shown to convey a better indication of the error's dispersion for large noises. Simulations illustrate the results.

The discrete-time Invariant EKF

In the present section we give the equations of the IEKF in the simplified situation of this chapter. As in Chapter 4 and standard EKF theory, the idea is to linearize the error The highest values are not displayed to improve visualization. The performance is optimal for k = 0.1202, λ = 0.0029. Bottom plot: error evolution of three artificial horizons : the invariant filter with optimized gain functions (black), a MEKF ignoring outliers (red), and a MEKF where the observation noise covariance matrix has been numerically adjusted to minimize the RMSE (dashed line). We see that the MEKF cannot filter the outliers and at the same time be efficient in the absence of outliers, contrarily to the proposed invariant filter.

equation (6.10)-(6.11) assuming the noises and the state error are small enough, use Kalman equations to tune the gains on this linear system, and implement the gain on the nonlinear model. As in the heuristic theory of the IEKF in continuous time [START_REF] Bonnabel | Left-invariant extended Kalman filter and attitude estimation[END_REF] [21] the gains and the error covariance matrix are proved to converge to a fixed value. In particular this allows (after some time) to advantageously replace the gain by its constant final value leading to a numerically very cheap asymptotic version of the IEKF.

Linearization of the equations and IEKF formulas

We consider here the error equations (6.10)-(6.11). Assuming errors and noises are small, we introduce their projection in the tangent space at the identity I d (the so-called Lie algebra g of the group) using the matrix exponential map exp(.) (see Chapter 2). As the matrix vector space g can be identified with R dim g using the linear mapping L g (see Table 2.1) we can assume exp : R dim g → G. This function is defined in Table 2.1 for usual Lie groups of mechanics. Its inverse function will be denoted by exp -1 . We thus define the following quantities of R dim g :

ξ n = exp -1 (η n ), ξ + n = exp -1 (η + n ), w n = exp -1 (W n ).
The gain is designed as previously using a function which is linear in R dim g , then an exponential mapping:

K n : y → exp[L n (y -h(I d , 0))]
Equations (6.10) and (6.11) mapped to R dim g become:

ξ n+1 = exp -1 exp (Ad ϒ n w n ) exp Ad ϒ n ξ + n , ξ + n+1 = exp -1 (exp(ξ n+1 ) exp[-L n (h(exp(ξ n+1 ,V n+1 )) -h(I d , 0))]).
As in Chapter 4, the gains L n are tuned on the linearized system through the classical Kalman theory. Within our current simplified framework, this first-order expansion reads:

ξ n+1 = Ad ϒ n w n + Ad ϒ n ξ + n , h(exp(ξ n+1 ),V n+1 ) = h(I d , 0) + H ξ ξ n+1 + H V V n+1 , ξ + n+1 = ξ n+1 -L n+1 (H ξ ξ n+1 + H V V n+1 ).
The computation of the gains is then straightforward using the Kalman theory in R dim g , which is a vector space. It is described in Algorithm 1.

Convergence of the gains

The main benefit of the filter is with respect to its convergence properties. Indeed, under very mild conditions, the covariance matrix P n and the filter's gain K n are proved to converge to fixed values [START_REF] Kalman | New results in linear filtering and prediction theory[END_REF]. The practical consequences are at least twofold: 1-the error covariance converges to a fixed value and is thus much easier to interpret by the user than a matrix whose entries keep on changing (see Fig. 6.2) 2-due to computational limitations on-board, the covariance may be approximated by its asymptotic value, leading to an asymptotic version of the IEKF being numerically very cheap.

Theorem 14. If the noise matrices

Q w n = Var(w n ), Q V n = Var(V n
) and the left inputs ϒ n are fixed, the pair (Ad ϒ n , H ξ ) is observable and Q w n has full rank, then P n and K n converge to fixed values P ∞ and K ∞ , as in the linear time-invariant case.

Algorithm 2 IEnKF

Define H by h(exp(ξ ), 0) = h(I d , 0) + Hξ + ( ξ 2 ). Sample M particles (η i 0 ) 1 i M following the prior error density.

for n = 0 to N -1 do for i = 1 to M do η i n+1 = W i n ϒη i+ n ϒ -1 , y i n+1 = h(η i n+1 ,V i n+1 ), end for P n+1 = 1 M ∑ M i=1 exp -1 (η i n+1 ) exp -1 (η i n+1 ) T , S n+1 = 1 M ∑ M i=1 y i n+1 (y i n+1 ) T , L n+1 = P n+1 H T S -1 n+1 , for i = 1 to M do η i+ n+1 = η i n+1 exp(-L n+1 [y i n+1 -h(I d , 0)]
), end for Store the gain matrix L n+1 .

end for

The prior estimation χ0 is an input.

for n = 0 to N -1 do χn+1 = ϒ χ+ n Ω n , χ+ n+1 = exp(L n+1 [ χn+1 Y n+1 -h(I d , 0)]) χn+1 . end for

The discrete-time Invariant EnKF

When the error equation is independent of the inputs, the exact density of the error variable can be sampled off-line. This allows to radically improve the precision of the quantities involved in the Kalman gains computation of Section 6.5.1. We propose here the Invariant Ensemble Kalman filter (IEnKF) described in Algorithm 2. The idea is to compute recursively through Monte-Carlo simulations a sampling of the error density and to use it, instead of linearizations, to evaluate precisely the innovation and error covariance matrices used to compute the gains in the Kalman filter. The procedure is described in Algorithm 2.

Simulations

We will display here the results of Algorithms 1 and 2 for the attitude estimation problem described in Section 6.2.2:

d dt R t = R t (ω t + w ω t ) × , Y n = (R T t n b 1 +V 1 n , R T t n b 2 +V 2 n ),
where R t ∈ SO(3) represents the rotation that maps the body frame to the earth-fixed frame, and ω t is the instantaneous angular rotation vector which is assumed to be measured by noisy gyroscopes. (Y n ) n≥0 is a sequence of discrete noisy measurements of the vectors b 1 and b 2 (for instance the gravity and the magnetic field), V 1 n and V 2 n are sequences of independent isotropic white noises. A simulation has been performed using the parameters given in Table 6.2. In this case the IEKF equations are described by Algorithm 3. As the state-of-the-art methods for this particular problem are the Multiplicative Extended Kalman Filter (MEKF, see e.g. [START_REF] Crassidis | A survey of nonlinear attitude estimation methods[END_REF]) and the USQUE filter [START_REF] Crassidis | Unscented filtering for spacecraft attitude estimation[END_REF] (a quaternion implementation of the Unscented Kalman Filter), the four methods are compared. The evolution of the gains is displayed on Fig. 6.2 and shows the interest of the invariant approach. The error variable is expressed in the Lie algebra and its projection to the first axis is given on Fig. 6.3 for each method (the projections on other axes being very similar, they are not displayed due to space limitations). All perform satisfactorily, but some differences are worthy of note. First we can see that the MEKF and the IEKF yield comparable performances but only the gains of the IEKF converge. Moreover, the linearizations lead the MEKF and the IEKF to fail capturing accurately the error dispersion through a 3σ -envelope. To this respect, the USQUE performs better than the two latter filters but still does not succeed in capturing the uncertainty very accurately. On the other hand, the envelope provided by the EnKF is very satisfying. This result is not surprising: this envelope has been computed using a sampling of the true density of the error, there is thus no reason why it should not be a valid approximation as long as there are sufficiently many particles. This method is thus to be preferred to the other ones when the user is willing and able to perform extensive simulations, and has the capacity to store the gains over the whole trajectory.

Algorithm 3 Invariant Extended Kalman Filter on SO(3)

Returns at each time step Rn , P n such that R n ≈ exp(ξ n ) Rn , where ξ n is a centered Gaussian and var(ξ n ) = P n . R0 and P 0 are inputs.

H ξ = (b 1 ) × (b 2 ) × , H V = I 6 , Q V = Var(V 1 ) 0 0 Var(V 2 ) , Q w = t n+1 t n Var(w ω s )ds. Set n = 0. loop Let Ω n be the solution at t n+1 of T t n = I 3 , d dt T t = (ω t ) × . P n+1 = P + n + Q w , S n+1 = H V Q V H T V + H ξ P n+1 H T ξ , L n+1 = P n+1 H T ξ S -1 n+1 , P + n+1 = (I -L n+1 H ξ )P n+1 , Rn+1 = R+ n Ω n , R+ n+1 = exp L n+1 Rn+1 Y 1 n+1 Rn+1 Y 2 n+1 - b 1 b 2 Rn+1 , n ← n + 1. end loop

Table 6.2 -Observation of two vectors: parameters

Parameter b 1 b 2 Q V 1 Q V 2 Value (1, 0, 0) (0, 1, 0) 0, 0873 2 I 3 0, 0873 2 I 3 Parameter P 0 Q w N Simulations
Value 0.5236 2 I 3 0, 01745 2 I 3 50 1000

Conclusion

This chapter has introduced a proper stochastic framework for some filtering problems on Lie groups having autonomous error equation. This implies to consider not any more a classical dynamical system, but a full probability density function. Several methods to tune the gains have been proposed. When they are held fixed, the estimation error has the remarkable property to provably converge in distribution. Yet, the results obtained can look restricted compared to the difficulty of the corresponding proofs (see B.1). The deterministic properties derived for example in Chapter 4 where much more general and Figure 6.3 -Evolution of the error variable, projected on the first axis, for 1000 simulations using MEKF, IEKF, USQUE and IEnKF. The MEKF and the IEKF give similar results. The USQUE is doing slightly better but is outperformed by the IEnKF. Only the latter captures the error dispersion satisfactorily, as the 3σ envelope contains 99 % of the simulated trajectories.

have been easily applied to a large variety of examples. This shows the extreme complexity of the study of stochastic systems and explains the prevalence of the deterministic approach in the theory of estimation, as well as in the present work.

Chapter 7

Sampling-based methods

Part of the content of this chapter has been presented at the 2014 IEEE Conference on Decision and Control and led to the registration of a patent (n°14-01535).

Chapter abstract This Chapter proposes to approach the discrepancy between actual systems and the ideal situation studied in Chapter 4 through sampling. We transpose the idea of Rao-blackwellized particle filtering [START_REF] Doucet | Rao-blackwellised particle filtering for dynamic bayesian networks[END_REF], where only a subset of the variables is sampled and the rest is marginalized out using (if possible) linear Kalman filters running in parallel for each value of the sampled variables. Here, the remaining system is sought to have the form allowing its conditional density to be estimated using IEKFs running in parallel. Under proper conditions leading to autonomy of a partial error, the gains are identical for all particles, allowing an extremely cheap implementation of the particle filter.

Introduction

In this short chapter, we present two specific cases of 5.1 where sampling-based methods can be an interesting approach. We first discuss in Section 7.2 a property of biased dynamics. Then we develop in Section 7.3 the idea of introducing a sampling to reduce a complicated problem to the form studied in Part I. Of course, the procedure requires some hypotheses on the form of the equations. To make the understanding of the idea easier, we will start with two IEKFs running in parallel, without selection among them, and discuss the benefits of the situation over the general case of two EKFs. Then we introduce the full method, which resorts to a possibly large number of IEKF estimates. In Section, 7.4, we detail the example of a GPS-aided inertial navigation handling jumping biases in the position provided by the GPS.

Sampling biased dynamics

This section is a mere remark about dynamics of the form 5.1, where the additional variables gathered in Θ t actually take the form of a bias b on the dynamics. The result presented is very easy but has to be taken into account in any method requiring samplingbased propagation of a p.d.f. The equation considered takes the form:

d dt χ t = f u t (χ t ) + χ t b, ( 7
η L t = η χ t η b t , with: η χ 0 = η 0 , d dt η χ t = g L u t (η χ t ) + η χ t b, η b 0 = Id, d dt η b t = g L u t (η b t ) + η b t b + η b t (∆b),
where g L u t is defined by Equation (3.9).

Verification is trivial. This result means the evolution of the error due to the initial perturbation η L 0 and the one due to the bias perturbation ∆b = b -b can be fully decoupled and contained in separate variables (η χ t and η b t ) , which is usually true only up to the first order for nonlinear systems. Once again, we managed to retrieve a property of the linear case. For a given value of bt the dynamics d dt χ t = f (χ t ) + χ t b verifies (3.13) as the sum of two functions verifying (3.13)(see 5). Thus the first-order approximation of the evolution of η χ t is actually exact (see Theorem 4) and the only approximations in a first-order propagation of η L t come from second order terms in the integration of the bias. For instance, if sigma-points are sampled (see [START_REF] Julier | A new extension of the Kalman Filter to Nonlinear Systems[END_REF]), only those corresponding to a variation of the biases seem really necessary. Moreover, the remaining sigma-points are really required only if the second-order influence of biases is sufficiently important. This is also true for particle filtering: a sampling of the full density requires to integrate χ t for each sampled bias but then, all the values of χ t for this bias and different initializations are contained in one particular solution and the first-order expansion of the error (Corollary 1). The cost of maintaining a representative sampling of the density thus goes down from e dim G+dim b to e dim b (dim G) 2 .

Next section gives a second way to leverage autonomous error properties for samplingbased methods.

The invariant Rao-Blackwellized filter 7.3.1 A conditional invariance hypothesis

Assume the considered system can be divided into a set of variables gathered in a vector Θ t , and an element χ t of a matrix Lie group G, such (Θ, χ t ) verifies an equation of the form:

d dt Θ t = ϕ(Θ t , ν t ), (7.2)
d dt χ t = f u t (Θ t , χ t ) + χ t w t , (7.3)
Y t n = ψ Θ tn (χ t n • d +V n ), (7.4)
where w t and ν t are Langevin noises, ψ Θ : Y → Y is invertible for any Θ and f • (•, •) verifies 3.8 as well as the additional hypothesis:

∀u, Θ, η, f u (Θ, η) -f u (Θ, I d )η = f u (0, η) -f u (0, I d )η. (7.5)
The additional parameter Θ can affect the observation as in the example of Section 7.4 and/or the dynamics. An example of the latter case is given by the attitude χ t (here a rotation matrix) of a body endowed with a gyroscope undergoing the combination of a bias Θ and an isotropic white noise, the stochastic evolution of the bias being possibly complicated.

Invariant Extended Kalman Filters running in parallel

We first illustrate the idea in a simplified situation where we consider two distinct trajectories (Θ

d dt χi t = f u t Θ i t , χi t , (7.6
)

χi+ t n = χi t n exp K i n ( χi t ) -1 ψ -1 Θ i t (Y n ) -d , (7.7)
then the Kalman gains K i n are identical for both systems.

Proof. It suffices to write the error equation associated to (7.6)-(7.7) and use (7.5) to obtain:

d dt η i t = f u t (0, η i t ) -f u t (0, I d )η i t -w t η i t , (7.8
)

η i+ t n = η i t n exp K n η i t n -1 • d -d +V n , (7.9)
which is independent of Θ i t . The linearization can be performed as in Chapter 4.

Invariant Rao-Blackwellized algorithm

Inspired by the Rao-Blackwellized particle Filter, the Invariant Rao -Blackwellization consists of fully exploiting the conditional invariance property in the following way. The state being partitioned into (Θ t , χ t ), the variable Θ t can be sampled using particles (Θ j t ) 1≤ j≤N p , each particle having a weight w j reflecting its likelihood assigned to it. There are numerous sampling methods that we will not detail here. Each time a new observation Y n is available, the weight is updated and a re-sampling step may take place. The distribution of the state conditioned on the past outputs can then merely be factored using the chain rule as follows:

P(χ t , Θ t |Y 1 , . . . ,Y n ) = P(Θ t |Y 1 , . . . ,Y n )P(χ t |Θ t ,Y 1 , . . . ,Y n ).
This distribution can be approximated combining particle filters and IEKFs to approximate P(Θ t |Y 1 , . . . ,Y n ) and P(χ t |Θ t ,Y 1 , . . . ,Y n ) in the following way:

• P(Θ t |Y 1 , . . . ,Y n ) is computed using a particle filter, that is, is approximated by the distribution ∑ N p j=1 w j δ Θ j (Θ j ).

• P(χ t |Θ j t ,Y 1 , . . . ,Y n ) is approximated by a normal law with mean and covariance output by an IEKF.

Using Proposition 21 we see that as soon as the system satisfies the conditional invariance property, the Kalman gains and covariances are identical for all particles, leading to a much decreased numerical complexity.

Application to navigation with jumping GPS

The method described above will now be used to integrate an advanced non-Gaussian and non-white GPS noise model into a localization algorithm. The problem of inertia/GPS fusion has been addressed using several kinds of Kalman filters [START_REF] John | Sigma-point kalman filtering for integrated gps and inertial navigation[END_REF][START_REF] Ch | Adaptive kalman filtering for low-cost ins/gps[END_REF][START_REF] Ph | Generalized multiplicative extended kalman filter for aided attitude and heading reference system[END_REF], but most of them rely on Gaussian approximations. In [START_REF] Ph | Generalized multiplicative extended kalman filter for aided attitude and heading reference system[END_REF] for instance, the GPS error is a Gaussian white noise, in [START_REF] John | Sigma-point kalman filtering for integrated gps and inertial navigation[END_REF] it is modeled as the sum of a bias following a small random walk and a white noise. Here, the GPS estimates are assumed to be polluted not only by white noise, but also by an offset that abruptly jumps at unknown times. Such a model reflects the realistic effects of highly correlated noises in GPS and the way we propose to handle it opens the door to the efficient integration of an even more sophisticated noise model.

Model

The inertial sensors delivering estimates at high rate, and the GPS at a much lower rate, we opt for the following continuous-time model with discrete observations, based on the equations of a rigid body moving on a flat earth:

d dt R t = R t (ω t + w ω t ) × , (7.10)
d dt v t = R t (a t + w a t ) + g, (7.11)
d dt x t = v t , (7.12
)

Y t n = x t n + b t n +V n , (7.13)
where for u ∈ R 3 , (u) × is the skew-symmetric matrix such that ∀z ∈ R 3 , (u) × z = u × z, R t encodes the orientation of the body, v t its velocity, x t its position, b t is the (jumping) GPS bias, g the gravitation vector, Y t n the GPS measurement, ω t ∈ R 3 the gyroscope measurement, a t ∈ R 3 the accelerometer measurement, w ω t the gyroscope noise, w a t the accelerometer noise (the covariance matrix of the concatenated noise (w ω t , w a t ) will be denoted by Q), and V n the GPS noise, assumed to be isotropic (see Remark 10) with variance r 2 I d . The bias b t is modeled as follows: there exist a sequence of times (τ k ) k 0 independent of other variables and unknown to the user such that b t is constant on [τ k , τ k+1 [ and b τ k is a centered Gaussian variable with variance σ 2 b I d , independent of the past. The time elapsed between τ k and τ k+1 follows an exponential law with known intensity, and we let p J denote the probability of the bias to have jumped at leas once between two consecutive observations (p J is constant here because we assume the GPS information arrives at a constant rate but all the computations hold if p J is a function of (t nt n-1 )).

Remark 10. The isotropic GPS noise assumption is not really restrictive as the method still works if the noise V n is isotropic in the horizontal directions x, y only, and larger along axis z. This hypothesis is classical in GPS models.

IEKF for known biases

In this preliminary subsection we assume the bias sequence bn = b t 1 , . . . , b t n to be known. In this case the biases can be removed from the sequence of GPS measurements Ỹn = Y t 1 , . . . ,Y t n and the obtained system is the one described in 4.3.5. Using the same matrix embedding χt and letting the error be η t = χ -1 t χt we have

d dt η t = g u t (η t , ω t , a t ) = f u t (η t ) -f u t (I 5 )η t ,
and the matrices used in the gains computation are the same as in 4.3.5:

A t =   -(ω t ) × 0 3×3 0 3×3 -(a t ) × -(ω t ) × 0 3×3 0 3×3 I 3 -(ω t ) ×   , H =   0 3×3 0 3×3 I 3   . (7.14)
For χ t close to χt the Gaussian error approximation reads:

P exp -1 (χ -1 t n χt n ) = ξ bn , Ỹn ≈ N (ξ , P t n , 0),
where N (x, Σ, µ) is the value at x of the Gaussian density of mean µ and covariance matrix Σ.

Remark 11. There are two remarkable features of this filter. First, the Kalman gains do not depend on the bias b t n . This is logical but it will play an important role in the sequel. Then, around a large and relevant class of trajectories defined by constant inputs ω t , a t , the Riccati equation followed by the error covariance matrix is time-independent, and the Kalman gains are thus expected to converge.

Description of the proposed particle filter

The basis of the algorithm (and more generally of approximate Rao -Blackwellized Particle Filters) is the assumption that for a given sequence of bias values bn and an instant t ∈]t n ,t n+1 ] the marginal density P(χ t | bn , Ỹn ) is well approximated by a Gaussian with mean and covariance output by an IEKF. Thus, only the sequence of bias values has to be sampled, using N p particles. As we are interested in the density of the current bias, each particle only stores the last bias b j t and conditional mean χ j t (the conditional covariance matrix is computed using the invariant error equation). If the b j t are i.i.d sampled particles an empirical estimate of the posterior distribution P b t , χ t Ỹn of the whole current state (b t , χ t ) then writes:

P b t , χ t Ỹn = 1 N p N p ∑ j=1 δ b j t (b t )P χ t b j n .
As it is impossible to sample efficiently from the posterior distribution, we use importance sampling that weights the particles according to their likelihood. The following steps are thus repeated over time:

Propagation: the conditional estimates χ j t evolve following the deterministic part of (7.10), (7.11), (7.12) and the Riccati equation defined by (7.14) is integrated.

Re-sampling: when a measure Y n is available the bias of each particle is re-sampled following the law P(b n | b j n-1 , Ỹn ).

Re-weighting: the weight attached to each particle is multiplied by P(Y n | b j n-1 , Ỹn-1 ), then the weights are normalized. A re-sampling can be performed.

Marginal density update: the conditional mean χ j t n associated to each particle is updated using (4.47) and the sampled bias b j n . The (particle independent) invariant covariance matrix P t n is updated.

The detailed particle filter is described by Algorithm 4 and the various derivations leading to it are described in the following subsection.

Main formulas derivation

To implement the procedure we should be able to:

1. Compute P(χ t n | b j n , Ỹn ) from P(χ t n-1 | b j n-1 , Ỹn-1 ), b n , Y n . 2. Compute P(Y n | b j n-1 ,

Some remarks about bias observability

Before the first GPS jump A constant bias is not observable. Thus, the best one can do before the bias has jumped is to efficiently filter the GPS and inertial sensors noise to reconstruct the state up to a position offset.

After one jump

As the inertia gives an accurate short-term estimation of the trajectory, the bias jumps can be detected and the value updated using the difference between the estimated position and the GPS observation. But the residual centered noise V n of the GPS (after bias correction) affects this first estimation and can be handled only using future GPS measurements. Hence the interest of sampling several values of the new bias then selecting those which best fit the observations to come.

After many jumps

As the bias is assumed centered, it can be recovered asymptotically. Indeed in the proposed algorithm, the re-weighting will eventually eliminate the particles whose sequence of bias has a mean far from zero. This prevents the position to diverge from the GPS trajectory in the long run, this discrepancy being erroneously explained by a large non-centered estimated bias.

Results

Algorithm 4 has been implemented with the parameters given in table 7.1. The initial state is known. Then the estimate deviates from the true trajectory due to gyroscope, accelerometer and GPS noise. As a small set of parameters is sampled (the components of the GPS bias) using moreover an optimal sampling the filter gives reasonable results even with one particle. The accuracy of the localization is measured using the total Root Mean Square Error (RMSE) of the position over the whole trajectory. Its performance improves as the number of particles grows but stabilizes fast (see table 7.2) as the sampled variable is only 3-dimensional. The computation time is also displayed in table 7.2, for a naive Matlab implementation using "for" loops and it can be seen that the cost of 10 particles is only around twice the cost of one particle. Considering Algorithm 4 in detail the reader could even expect a much smaller difference between the cost of 1 and 10 particles but the result we obtain is affected by Matlab's poor ability to deal with loops over the particles. The end of the trajectory estimated by the Invariant Rao -Blackwellization is drawn on Figure 7.1 and compared to the true one. The apparent gap in the GPS observations is due to a bias change. We see that the filter is able to manage the non-Gaussian noise model of the GPS with an accuracy improved as the number of particles increases.

Conclusion

This chapter has introduced the idea of using autonomous errors variables to sample from a lower dimensional distribution in particle filtering. A class of system has been presented, for which the combination of invariant and particle filtering brings decisive prop-Figure 7.1 -Zoom on the end of the trajectory estimated by the Invariant Rao -Blackwellization with 1 and 100 particles, illustrating the usefulness of sampling. The beginning is not informative as the IMU gives an accurate estimation on the short run in both cases.

As an accurate near-optimal sampling is performed, a unique particle provides already a reasonable estimate. Sampling 100 particles improves the performance and, due to the properties of invariant filtering, is not much more expansive erties in terms of computational burden. The realism of the proposed model has been proved by the implementation of the method on a concrete non-linear localization problem with non-white noise. The strong convergence properties of the Invariant Extended Kalman Filter can be expected to have interesting consequences on the accuracy of the obtained sampling, but no result in this direction has been given here and the question is left for future work.

In Part III we come back to a deterministic setting, and step back a little to study, from a more general point of view, the benefits of an EKF based on a non-linear error variable. Part III

Nonlinear state error based EKFs

localization and mapping, and thus is very robust to erroneous feature identification. Yet, it comes at a price: the FastSLAM tends to become over confident in some trajectories and discard some other, less likely at a given time although proving more relevant once more information is obtained. In particular, when a loop closure occurs (that is, the same scene is viewed a long time after having been seen for the first time) the filter can fail to properly take it into account as the only thing it can do is increase the weight of the most likely hypothetical trajectories he has kept. On the other hand, the EKF-SLAM is very good at closing loops by maintaining full correlation between the features through the covariance matrix. Its weaknesses come from the linearization step which induces over confidence in the estimates and leads the filter to fail returning relevant estimates. Batch optimization takes as argument a large part of the trajectory [START_REF] Dellaert | Square root sam: Simultaneous localization and mapping via square root information smoothing[END_REF]. These methods allows re-linearization of the whole sequence, which the EKF cannot make.

Here, we will carefully analyze the problem of false observability and show that it is strongly coordinate-dependent, in the sense that simply changing the definition of the error variable can make it vanish. This strange phenomenon is better grasped once the error variable has been understood as a local state-dependent basis. First, we introduce in the Section 8.2 the tools required to study observability issues arising in the context of the EKF. We use them in Section 8.3 to develop an analysis of EKF SLAM inconsistency mainly relying on existing ideas [START_REF] José A Castellanos | Limits to the consistency of ekf-based slam 1[END_REF][START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF] yet presenting them, we believe, in a much simpler way. Then we show in Section 8.4 how to solve this problem through the choice of the error variable and we finally give in Section 8.5 and 8.6 an equivalent approach allowing a more physical understanding of the mechanisms at play.

Kalman filter and observability

Observability

Observability is a classical concept of the automatic control theory, whose definition is quite transparent. Consider a general dynamical system X t ∈ R n t , associated to a sequence of observations

(Y n ) n 0 . d dt X t = f u t (X t ), (8.1)
Y n = h(X t n ), (8.2)
where f is the function describing the evolution of the system X t , u t an input and h the observation function. Between the instants t 1 and t 2 the differential equation moves any vector X to a to a new position denoted by Φ t 2 t 1 (X), where the function Φ is called the "flow" associated to f. This denomination is visual: if the system lets itself be driven by the equation d dt X t = f u t (X t ) and is at X at time t 1 , then it is at Φ t 2 t 1 (X) at t 2 . Note that we can have t 1 > t 2 , in which case the flow is traced back from t 2 to t 1 . In more mathematical terms we have

Φ t 2 t 1 (X) = Φ t 1 t 2 -1 (X).
Definition 8 (non-observable transformation). We say a transformation φ : R n → R n of the system is non-observable if for any X 0 ∈ R n and n 0 we have:

h Φ t n 0 [φ (X 0 )] = h Φ t n 0 (X 0 ) .
It concretely means that if the transformation is applied to the initial state then none of the observations Y n are going to be affected. As a consequence, there is no way to guess this transformation has been applied. This notion has an infinitesimal counterpart strongly related to the notion of infinitesimal observability [START_REF] Gauthier | Observability and observers for nonlinear systems[END_REF] that we define as follows:

Definition 9 (non-observable direction). Let (X t ) t 0 denote a solution of (8.1). A vector δ X 0 ∈ R n is said to span a non-observable direction around X 0 if we have:

∀n 0, H n δ X t n = 0,
where H n is the linearization of h around X t n and δ X t n = DΦ t n 0 (X 0 )δ X 0 is the firstorder propagation of the perturbation δ X 0 until t n (DΦ t n 0 (X 0 ) being the differential of Φ t n 0 around X 0 ).

The denomination δ X t for a vector aims here at highlighting it has to be understood as an infinitesimal perturbation of the trajectory. This definition, in a more common language, could be reformulated as follows: if the initial state was X 0 + δ X 0 instead of X 0 , we would not see any difference on the sequence of observations up to the first order. The condition H n δ X t n = 0 means h(X t n + δ X t n ) = h(X t n ), still up to the first order. If f and h are linear these notions are equivalent: a direction δ X 0 is non-observable if and only if the translation of the system by the vector δ X 0 is non-observable. An estimation method providing the uncertainty of its own result (as the EKF does) should be able to detect that a direction is not observable, as obtaining an estimate accurate along this direction is impossible. This is the question studied in the rest of this section.

Kalman filtering and Extended Kalman filtering

Consider the specific case of a process X t following a linear dynamics, polluted by Gaussian white noise w t :

d dt X t = A t X t + B t + w t ,
where matrices A t and B t are known and w t is a random Gaussian white noise with covariance matrix Q t . The value of this process is estimated using a sequence of linear observations Y n , also polluted by a white noise V n having covariance matrix R n :

Y n = H n X t n +V n .
If the distribution of the initial state X 0 is Gaussian with mean X0 = E X 0 and covariance matrix P 0 = Cov(X 0 ) = E (X 0 -X0 )(X 0 -X0 ) T , then the exact expected value Xt of X t , as well as the conditional covariance matrix P t given the past observations, are given by the equations of the Kalman filter. We recall them here, in a way allowing to establish the terminology we are going to use in the rest of this document.

Propagation (while no observation is available):

d dt Xt = A t Xt + B t ,
d dt P t = A t P t + P t A T t + Q t , t n-1 < t < t n . (8.3)
Update (and each time an observation Y n appears):

• Computation of the innovation z n and its covariance matrix S n :

z n = Y n -H n Xt n , S n = H n P t n H T n + R n . (8.4)
• Estimation of the error ∆X:

K n = P t n H T n S -1 n , ∆X = K n z n .
• Update of the mean and covariance matrix:

X+ t n = Xt n + ∆X, P + t n = [I -K n H n ]P t n .
This is the exact solution to the problem of filtering in the linear case: we have P (X t |(Y n ) t n <t ) = N (X t , Xt , P t ) where N denotes the multivariate Gaussian density :

N (x, µ, Σ) = 1 (2π) k |Σ| exp (x -µ) T Σ -1 (x -µ) , with k the dimension of x.
If we come back to the general non-linear case, the p.d.f. conditioned on the observations does not stay Gaussian and its exact description cannot be derived in a close form. The usual way to deal with this issue is simply to use a first-order linearization of the error X t -Xt to maintain an approximation of the mean and covariance matrix of the system, denoted again by Xt and P t . This approach, called extended Kalman filtering, consists of the following steps:

Propagation (while no observation is available):

d dt Xt = f u t ( Xt ), d dt P t = A t P t + P t A T t + Q t , t n-1 < t < t n ,
where A t is the Jacobian matrix ∂ f ∂ X computed at Xt , and

Q t = ∂ f ∂ w | Xtn Cov(w t ) ∂ f ∂ w Xtn T
is the first-order covariance matrix of the noise perturbing the evolution of X t .

Update (each time an observation Y n appears):

• Computation of the innovation z n and its covariance matrix S n :

z n = Y n -H n Xt n ,
where H n is the Jacobian matrix ∂ h ∂ X computed at Xt n .

S n = H n P t n H T n + R n ,
where

R n = ( ∂ h ∂V | Xtn )Cov(V n )( ∂ h ∂V | Xtn )
T is the observation noise covariance matrix computed at Xt .

• Estimation of the error ∆X:

K n = P t n H T n S -1 n , ∆X = K n (Y n -H n Xt n ).
• Update of the mean and covariance matrix:

X+ t n = Xt n + ∆X, P + t n = [I -K n H n ]P t n .
The obtained estimate Xt and covariance matrix P t stem now from approximations and their behavior is not obvious. In particular, if a direction is not observable, this should be encoded one way or another in P t , meaning the filter "knows" it has no information along this direction. Failure to correctly handle this situation has been shown to be a major cause of failure of the EKF, especially in SLAM applications [START_REF] Bailey | Consistency of the ekf-slam algorithm[END_REF][START_REF] Guoquan | Observability-based rules for designing consistent ekf slam estimators[END_REF][START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF]. The property we seek is defined now more rigorously.

Error Covariance matrix and non-observability

The covariance σ 2 C of any scalar scalar variable C defined as the projection of X t over a known vector u ∈ R n is given by Cov(C) = u T P t u. If u is the projection over coordinate i then we obtain σ 2 C = P i,i t : the diagonal coefficients of P t are the covariances of the components of X t . The uncertainty of C is then usually represented through the standard deviation

σ C = σ 2
C , as this value is homogeneous to C. If the system is linear, σ 2 C is the exact covariance of the estimation error over the direction u. But if it is non-linear, then Xt and P t have been obtained through approximations and their validity has to be discussed. This is where the standard deviation σ C (derived from the computed P t ) proves very useful. Indeed, the difference between the estimate and the true state does not carry any useful information because 1) the notion of "large" difference has to be defined 2) an error due to non-observability does not mean the method is not working but only that the filter has not been given enough information. However, comparing the actual error with the computed standard deviation σ C is meaningful as it is a test of the ability of the filter to estimate its own uncertainty. For instance, having frequently an error superior to 3σ C is a good indicator of failure as the probability of having such an error should be less than 3.10 -3 .

Note that this uncertainty information can also be very useful to the user or the robot, typically for safety issues: an accurate uncertainty estimation will prevent some risky decisions that could for instance lead to a collision. Now we would like to translate the (deterministic) notion of non-observability in terms of covariance matrix. If a scalar component X i t , or more generally a scalar variable C = u T X t is non-observable, its estimated covariance defined as u T P t u should stay large in a certain sense. If not, we can conclude there is a problem with the estimation method. In this kind of analyses, remarks such as "the estimated covariance of the variable C decreases although this direction is not observable" are common but should not be understood literally. It would be erroneous to expect the covariance over a non-observable direction to be always decreasing as shown by the following linear counter-example:

Example 5. Consider a linear system consisting of a state (v, x 1 t , x 2 t ) T ∈ R 3 following the equation:

d dt v t = 0, d dt x 1 t = v, d dt x 2 t = v,
and assume one observes y = h(v, x 1 , x 2 ) = x 2 + V , where V is a white noise. A Kalman filter is run on this simple linear problem with initial covariance matrix I 3 and observation covariance Cov(V ) = r = 1. The results are displayed on the left plot of Figure 8.1. We see that the covariance of x 1 is not always increasing although x 1 is not observable. The explanation is simple: as the velocity v is unknown, the covariance of x 1 grows, then reduces as v is observed through x 2 , but stays larger than its initial value. This shows that increasing covariance is not the property we seek. Note however that the covariance along this direction is lower bounded by its initial value which is logical.

For the linear system of the previous example, we see that x 1 defines a non-observable direction as two solutions defined by the initial conditions (v 0 , x 1 0 , x 2 0 ) T and (v 0 , x1 0 , x 0 2 ) T with x1 0 = x 2 0 yield the same output trajectory (note that the direction (0, 1, 0) spanned by their difference lies in ker H x , with H x : (v, x 1 , x 2 ) → x 2 ). However, we see that the covariance matrix projected along this direction can decrease. As the system is linear, we know this not due to an approximation of in the computation of the covariance.

On the other hand, the inverse of the covariance matrix, the so-called information matrix, is actually much more suited to the notion of observability: its value is always decreasing along non-observable directions for any Kalman filter tuned with arbitrary noise covariance matrices as shown by the following result. Figure 8.1 -Covariance and information over the non-observable direction x 1 for the simple linear system d dt v = 0, d dt x 1 = v, d dt x 2 = v with observation y = x 2 +V . The variable x 1 is not observable. Yet the variance of x 1 conditioned on the outputs (i.e. P 2,2 t if the chosen order is v, x 1 , x 2 ) can decrease as it is correlated to x 2 through the fact they have the same velocity (left plot). Note on the other hand the information over x 1 (defined as (P -1 t ) 2,2 ) never increases, which is consistent with the fact it is not observable.

Proposition 22. Consider the linear system

d dt X t = A t X t + w t , Y t n = H n X t n +V n ,
and assume the direction δ X 0 is non-observable, i.e., we have at all observation time t n :

δ X t n ∈ ker H n ,
where δ X t verifies d dt δ X t = A t δ X t . For arbitrary choices regarding the covariance matrices Q t and R n involved in the propagation and update steps of the Kalman filter (8.4)- (8.4), and keeping the notation P t for the covariance matrix they compute, then the information (δ X) T t P -1 t (δ X) t along the considered direction (δ X t ) t≥0 is monotonically decreasing.

Proof. During the propagation step using the standard Kalman filter equations and the well-known fact that d dt P -1 t = -P -1 t ( d dt P t )P -1 t we have:

d dt (δ X) T t P -1 t (δ X) t =(A t (δ X) t ) T P -1 t (δ X) t + (δ X) T t P -1 t A t (δ X) t -(δ X) T t P -1 t A t P t + P t A T t + Q t P -1 t (δ X) t = -(δ X) T t P -1 t Q t P -1 t (δ X) t 0.
And at the update step (written here in information form as, e.g., in [START_REF] Thrun | Probabilistic robotics[END_REF]) we have:

(δ X) T t n (P + t n ) -1 (δ X) t n = (δ X) T t n P -1 t n + H T n R -1 t H n (δ X) t n = (δ X) T t n P -1 t n (δ X) t n ,
as H n (δ X) t n = 0. Thus the information (δ X) T t n (P + t n ) -1 (δ X) t n is always decreasing.

The remarks of this section allow us to give a mathematical definition of a good behavior of an EKF regarding non-observable directions: Definition 10. An EKF preserves the non-observable direction δ X 0 if the quantity (δ X t ) T P -1 t (δ X t ) is always decreasing, where δ X t follows the differential equation

d dt δ X t = A t δ X t , A t denoting the Jacobian matrix ∂ f ∂ X computed at Xt .
Remark 13. An immediate consequence of Proposition 22 is that a non-observable direction δ X t of the non-linear system is preserved by an EKF if it is also a non-observable direction of the system linearized over the estimated trajectory Xt . This not surprising: the EKF "sees" only the linearized system and computes its uncertainty consequently.

The next section approaches a famous false observability problem and proposes a new solution involving no complicated patch-up job.

SLAM inconsistency

It is now well-known, thanks to the works of [START_REF] Bailey | Consistency of the ekf-slam algorithm[END_REF][START_REF] Guoquan | Observability-based rules for designing consistent ekf slam estimators[END_REF][START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF], that the EKF-SLAM algorithm with relative observation of unknown features tends to gain information, especially on the heading uncertainty, although this variable cannot ever be inferred (a rotation of the full system is non-observable). This is essentially due to the changes in the linearization point over the observation sequence, which is a typically nonlinear problem, and has been well analyzed [START_REF] Guoquan | Analysis and improvement of the consistency of extended kalman filter based slam[END_REF][START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF]. In this section, we advocate part of the EKF-SLAM inconsistency can be remedied by choosing appropriate estimation error variables when designing an EKF. This will first be shown on the standard "steered" bicycle model [START_REF] Hugh | An autonomous guided vehicle for cargo handling applications[END_REF]. The state is defined as:

χ t = (ω t , X t , p t ) ,
where θ t ∈ R denotes the heading, X t ∈ R 2 the 2D position of the vehicle, p t ∈ R the position of an unknown feature and χ t a synthetic notation for the state of the system. Its dynamics reads:

d dt θ t = ω t , d dt X t = R(θ t ) v t 0 , d dt p t = 0, (8.5)
where ω t ∈ R denotes the angular velocity of the vehicle, v t the odometric velocity and R(θ ) is the matrix encoding a rotation by an angle θ :

R(θ ) = cos(θ ) -sin(θ ) sin(θ ) cos(θ ) .
Note that we simplified the notations: ω t can be expressed as a function of the velocity, wheel base and steer angle but keeping the formulas as simple as possible looked preferable. The observation of feature can be any function defined in the reference frame of the vehicle:

Y n = h R(θ t ) T (p -X t n ) , (8.6)
where Y n ∈ R p is the observation of the feature at time t n and h is a function of R 2 → R p .

Remark 14. Note that the observation model (8.6) encompasses the usual range and bearing observations used in the SLAM problem as they read:

Y n = h r,b R(θ t ) T (p -X t n ) , with: h r,b y 1 y 2 =   y 2 1 + y 2 2 arctan y 2 y 1   .
If we choose h y 1 y 2 = arctan y 2 y 1 instead, we obtain monocular SLAM, another classical model. Note also we do not provide any form for the noise in the output: this is because the properties we are about to prove only depend on the deterministic part of the system so they are insensitive to the way the noise enters the system.

All the measures being in the reference frame of the vehicle, an initial rotation and/or translation of both the vehicle and the map (i.e. the set of features) is non-observable. We will now examine the system linearized around an estimated trajectory ( θt , Xt , pt ) t>0 to see if an infinitesimal rotation of the vehicle and map is ensured to be a non-observable direction of this linear system, i.e., ensured to be considered non-observable by an extended Kalman filter.

Integration of the dynamics gives a discrete evolution between two observation times:

θ n+1 = θ n + ωn , X n+1 = X n + R(θ n ) vn , with ωn = t n+1
t n ω t ∈ R and vn ∈ R 2 the solution at time t n+1 of the following equation starting at t n :

vt n = (0, 0) T , d dt vt = R t t n ω t dt v t 0 .
We also used the short-hand notations θ n = θ t n and X n = X t n . The same notations will be used for the estimates: θn = θt n , Xn = Xt n , pn = pt n and χn = χt n . The propagation and observation functions can be linearized around an estimate Xn :

  θ n+1 -θn+1 X n+1 -Xn+1 p -pn+1   ≈ F n   θ n -θn X n -Xn p -pn   , h   θ n X n p   -h   θn Xn pn   ≈ H n   θ n -θn X n -Xn p -pn   ,
with:

F n =   1 0 1,2 0 1,2 JR θn vn I 2 0 2,2 0 2,1 0 2,2 I 2   , H n = Dh χn   ( pn -Xn ) T R( θn )J -R( θn ) R( θn )   T , (8.7)
where J = 0 -1 1 0 and Dh χn denotes the Jacobian of h computed at the point R( θn

) T pn -Xn ∈ R 2 .
Now, let us define carefully the linear error system "seen" by the EKF for a sequence of estimates ( χn , χ+ n ) n 0 . Note that we distinguish intentionally the estimates before the update ( χn ) and after the update χ+ n . Indeed, in the EKF, the dynamics is linearized around the updated estimate χ+ n although the observation is linearized around the estimate before update χn (as the updated value is not known yet). The linear system "seen" by the EKF is defined by the sequence of propagation and observation matrices:

F n =   1 0 1,2 0 1,2 JR( θ + n ) vn I 2 0 2 0 2,1 0 2 I 2   , H n = Dh χn   ( pn -Xn ) T R( θn )J -R( θn ) R( θn )   T .
Thus, following definition 9, a direction is non-observable for the linearized system if we have for any m 0:

H m+1 (Π m n=0 F n ) δ X 0 = 0. (8.8)
The initial perturbations we are interested in here correspond to overall rotations of the system. They are defined in Proposition 23 below:

Proposition 23. Let χ =   θ X p
 be an estimate of the state. The first-order perturbation of the estimate corresponding to an infinitesimal rotation of the system through an angle δ α around the origin reads

  1 J X J p  δ α, with J = 0 -1 1 0 .
Proof. The first-order perturbation is computed considering a rotation through an angle δ α of the overall system then keeping only the first-order terms. The heading changes as follows:

θ → θ + δ α.
The position of the vehicle changes as follows:

X → cos(δ α) -sin(δ α) sin(δ α) cos(δ α) X ≈ δ α 0 -1 1 0 X.
The position of the feature changes as follows:

p → cos(δ α) -sin(δ α) sin(δ α) cos(δ α) p ≈ δ α 0 -1 1 0 p.
Stacking these results we obtain the first-order variation of the full state vector:

  θ X p  →   θ X p  +   1 J X J p  δ α.
Denoting by

δ χ R 0 =   1 J X0 J p0   δ α
such a perturbation of the initial state we come to the main result of this section: Proposition 24. Overall rotations of the system are non-observable. But once the equations have been linearized, the observability of δ χ R 0 for the linearized system depends on the linearization points:

1. If the linearization point χt n is propagated over time but never updated (∀n, χ+ n = χn ) then δ χ R 0 is a non-observable direction of the linearized system.

2. In general, δ χ R 0 is not a non-observable direction of the system linearized on ( χn , χ+ n ) n 0 .

Result 1 confirms [START_REF] Guoquan | Analysis and improvement of the consistency of extended kalman filter based slam[END_REF]: linearizing the system on the prior trajectory prevents false observability. But result 2 is quite impractical as an EKF does not "see" anything else than the system linearized on ( χn , χ+ n ) n 0 . Moreover, it appears in the proof of Proposition 24 we give now that δ χ R 0 being non-observable for the system linearized on ( χn , χ+ n ) n 0 is highly unlikely, as soon as updates are involved.

Proof. δ χ R 0 is a non-observable direction of the linearized system if for any m 0 equation (8.8) is verified:

H m+1 (Π m n=0 F n ) δ χ R 0 = 0.

Let us develop this expression:

Π m n=0 F n = Π m n=0   1 0 1,2 0 1,2 JR θ + n vn I 2 0 2 0 2,1 0 2 I 2   =   1 0 1,2 0 1,2 J ∑ m n=0 R θ + n vn I 2 0 2 0 2,1 0 2 I 2   ,
where we recursively used the easily verified property:

  1 0 0 a 1 1 0 b 1 0 1     1 0 0 a 2 1 0 b 2 0 1   =   1 0 0 a 1 + a 2 1 0 b 1 + b 2 0 1   . (8.9)
We have thus:

Π m n=0 F n =   1 0 1,2 0 1,2 J ∑ m n=0 Xn+1 -X+ n I 2 0 2 0 2,1 0 2 I 2   =   1 0 1,2 0 1,2 J Xm+1 -X0 -∑ m n=1 X+ n -Xn I 2 0 2 0 2,1 0 2 I 2   ,
where the last equality is a mere re-ordering of the terms appearing in the sum(Note that X + 0 = X 0 as the observations start at n = 1). Re-injecting in (8.8) we obtain:

H m+1 (Π m n=0 F n ) δ χ R 0 = H m+1   1 0 1,2 0 1,2 J Xm+1 -X0 -∑ m n=1 X+ n -Xn I 2 0 2 0 2,1 0 2 I 2     1 J X0 J p0   δ α = H m+1   1 J Xm+1 -∑ m n=1 X+ n -Xn J p0   δ α = Dh χm+1   ( pm+1 -Xm+1 ) T R( θn )J -R( θm+1 ) R( θm+1 )   T   1 J Xm+1 -∑ m n=1 X+ n -Xn J p0   δ α = -Dh χm+1 JR( θm+1 ) pm+1 -p0 + m ∑ n=1 X+ n -Xn δ α.
In one particular case, the last parenthesis on the right side of this equation is zeros and the expression is ensured to be null: if the updates of X and p are always zero. This proves the first result. This is logical: the observability properties of the system are defined on a trajectory without updates. If no assumption is made on the updates (which can take any value depending on the noise), the latter expression can be non-zero and an initial rotation of the system is observable on the linearized system. This update issue is the basic mechanism of false observability making EKF acquire false information. Blue plot of Figure 8.2 displays the heading covariance estimated by an EKF for this problem where h is taken as Identity and one landmark is observed at each time step. As can be seen, the filter believes it improves its accuracy over time although it has physically no way to do better than its first guess. Red plot is not important for now. Figure 8.2 -Plot of the estimated heading variance (in rad 2 ) over time for a vehicle with known position, unknown heading, and one landmark. A sequence of estimates has been computed using the conventional EKF-SLAM algorithm. The blue solid curve corresponds the covariance matrix computed by this EKF. On the same sequence of estimates, the system has been linearized using a non-linear error variable (red dashed line). With a linear error variable, the second measurement at t 1 = 200 induces large decrease in the heading uncertainty, and the following measurements every 100 time steps keep decreasing the heading variance although the system has physically no way to acquire this information. On the other hand the heading variance remains at level when using the alternative nonlinear error variable as advocated in this paper. This simulation confirms again that the error variable chosen does change the result of the Riccati equation as soon as non-zero updates are involved.

Solving false observability through the choice of the error variable

We will show here that the way the error is defined has a dramatic impact on the Riccati equation as soon as the trajectory involves updates (if not, obviously, all the error variables are equivalent). Previous derivation linearized the evolution of an error defined as:

e n =   θ n -θn X n -Xn p -pn   .
Now we perform the same computation for an error inspired by the theory of symmetrypreserving observers [START_REF] Bonnabel | Observateurs asymptotiques invariants: théorie et exemples[END_REF][START_REF] Bonnabel | Symmetry-preserving observers. Automatic Control[END_REF] and introduced for the SLAM problem in [START_REF] Bonnabel | Symmetries in observer design: Review of some recent results and applications to ekf-based slam[END_REF] (to derive an autonomous error equation but without observability considerations) and which has a slightly different definition:

e n =   θ n -θn R θn -θ n X n -Xn R θn -θ n p -pn   . (8.10)
Considering again a sequence of estimates ( χn , χ+ n ), the firs-order system is defined by the first-order approximations around this trajectory:

  θ n+1 -θn+1 R( θn+1 -θ n+1 )X n+1 -Xn+1 R( θn+1 -θ n+1 )p -pn+1   ≈ F n   θ n -θ + n R( θ + n -θ n )X n -X+ n R( θ + n -θ n )p -p+ n   , h[R(θ n ) T (p -X n )] -h[R( θn ) T ( p -Xn )] ≈ H n   θ n -θn R( θn -θ n )X n -Xn R( θn -θ n )p -pn   .
This leads to the following values of F n and H n :

F n = Id, H n = Dh χ   0 1,2 -R( θn ) R( θn )   T ,
where Dh χ is the Jacobian of h computed at R( θn ) T ( pn -Xn ). The expression of the linearized system has become much simpler, and showing preservation of the non-observable directions is going to be easy. First we have to write the infinitesimal error corresponding to a small rotation of angle α of the whole system.

Proposition 25. Let χ =   θ X p
 be an estimate of the state. The first-order perturbation of the error (8.10) around zero corresponding to an infinitesimal rotation of the system through an angle δ α around the origin reads

  1 0 2,1 0 2,1   δ α.
Proof. The first-order perturbation is computed considering a rotation through an angle δ α of the overall system then keeping only the first-order terms. As shown by Proposition 23, the rotated system reads(up to the first order):

  θ + δ α X + δ αJ X p + δ αJ p   .
The error (defined by (8.10)) with the non-perturbed system reads:

  δ α R(-δ α)( X + δ αJ X) - X R(-δ α)( p + δ αJ p) - p   ≈   δ α (I 2 -δ αJ)( X + δ αJ X) - X (I 2 -δ αJ)( p + δ αJ p) - p   .
And removing the second-order terms:

  δ α R(-δ α)(X + δ αJX) -X R(-δ α)(p + δ αJ p) -p   ≈   δ α 0 2,1 0 2,1   .
With this modified error variable, an initial perturbation corresponding to an infinitesimal rotation is thus simply

δ χ R 0 =   1 0 2,1 0 2,1   δ α.
Now we can study the observability of such a perturbation for the linearized system using condition (8.8). For any number m of observations we have immediately:

H n Π m n=0 F n δ χ R 0 = Dh χn   0 -R( θn ) T R( θn ) T   T   1 0 2,1 0 2,1   = 0 2,1 .
As well, an infinitesimal translation of the whole system has the form

δ χ u 0 =   0 u u   with
u ∈ R 2 and we have:

H m+1 (Π m n=0 F n ) δ χ u 0 = Dh| χm+1   0 1,2 -R( θn ) R( θn )   T   0 u u   = 0 2,1 .
This proves the following Theorem: Theorem 15. The SLAM problem defined by equations (8.5)-(8.6) and linearized on a trajectory (χ n , χ + n ) n 0 using error variable

e n =   θ n -θn R( θn -θ n )X n -Xn R( θn -θ n )p n -pn   ,
boils down to the following propagation and observation matrices:

F n = I 3 , H n = Dh χ   0 1,2 -R( θn ) R( θn )   T .
Regardless of the linearization points (χ n , χ + n ) n 0 , the non-observability condition

∀m 0, H m+1 (Π m n=0 F n ) δ χ 0 = 0
is verified for the following values of the initial perturbation δ χ 0 :

δ χ 0 = δ χ R 0 =   1 J Xt n J pt n   , δ χ 0 = δ χ 1 0 =       0 1 0 1 0       , δ χ 0 = δ χ 2 0 =       0 0 1 0 1       .
They correspond to rotations and translations of the whole system.

Application of Proposition 22 gives then immediately

Theorem 16. Let (P n ) n 0 denote the solution of the Riccati equation obtained linearizing the SLAM problem (8.5)-(8.6) around any trajectory (χ n , χ + n ) n 0 with error variable

e n =   θ n -θn R( θn -θ n )X n -Xn R( θn -θ n )p n -pn   ,
and choosing arbitrary noise covariance matrices Q n and R n .

(P n ) n 0 verifies: ∀n 0,

P n = F n-1 P n-1 F n-1 + Q n-1 , K n = P n H T n H n P n H T n + R n -1 , P + n = (I -K n H n )P n , (8.11)
with matrices F n and H n defined as in Theorem 15 above. Let δ χ 0 denote a linear combination of the infinitesimal rotations and translations δ χ R 0 , δ χ 1 0 , δ χ 2 0 of the whole system, defined as in Theorem 15 above. Then the non-observability of δ χ 0 is preserved in the sense of Definition 10, regardless of the linearization points. We recall the meaning of this property, which takes a very simple form here due to the trivial form of F n :

(δ χ 0 ) T P -1 n (δ χ 0 ) decreases over time.

Red plot of Figure 8.2 displays the heading covariance computed on the same sequence of estimates as the blue one, but with the non-linear error variable. As proved by Theorem 15 the false observability issue disappeared. Before going further into the building of an EKF based on a non-linear error variable we give in the next section an alternative way to solve false observability problems, looking more natural than the use of a non-linear error variable but turning out to be strictly equivalent.

A different approach to the same result 8.5.1 A visual approach to the problem

Previous section explained the problem of false observability through computations. Figures 8.3 and 8.4 illustrate the same phenomenon through a drawing for a 2-dimensional non-linear system. It shows the fundamental difference between propagation step and update step in the EKF. During the propagation, the estimate changes (following equation d dt χ t = f u (χ t ) or its discrete form) while the covariance matrix changes in a coherent way (involving the Jacobian F n of the flow of f ). This preserves the non-observability of a move along the black line representing a non-linear non-observable transformation, because the Jacobian of the flow maps non-observable directions of an instant t n to the non-observable directions of an instant t n+1 . During the update step, the covariance matrix is updated using a first-order expansion around the current estimate χ t n+1 and the result P + t n+1 is coherent with the non-observable directions around χt n+1 . But the state is 1) Estimate and uncertainty ellipsoid at time t n if the filter has worked correctly: the ellipsoid is elongated in the non-observable direction.

2) Propagation of the estimate until t n+1 .

3) First-order propagation of the covariance: the direction estimated as "uncertain" sticks to the true non-observable direction.

Legend. 4) The updated state X+ t n+1 is computed using linearizations at Xt n+1 .

5) The updated covariance matrix P + t n+1 is computed using linearizations at Xt n+1 : its elongation expresses the nonobservability at Xt n+1 .

6) The non-observable direction at X+ t n+1 has no reason to correspond to the non-observable direction at Xt n+1 . t n+1 and the covariance is updated. But all the computations performed to update P n+1 rely on a linearization at Xt n+1 . Thus, the filter cannot "guess" the non-observable direction around X+ t n+1 . This is the moment where false observability is created.

updated and obviously, as shown by the last drawing, there is no reason for the nonobservable directions around χ + t n+1 to coincide with the non-observable directions around χ t n+1 .

Mathematical derivation

The natural reaction at the sight of the problem occurring in the update step is: "the ellipsoid should turn !". This is precisely the simple idea discussed now. The shorthand subscript n instead of t n will be used again. Mimicking the propagation step, we introduce a matrix L( χn , χ+ n ) mapping the non-observable directions around χn to the non-observable directions around the updated step χ+ n to transform P + n into a re-updated matrix P ++ n . The Riccati equation becomes for any n 0:

P n = F n-1 P n-1 F n-1 + Q n-1 , P + n = (I -K n H n )P n with K n = P n H T n H n P n H T n + R n -1 , P ++ n = L( χn , χ+ n )P + t n L( χn , χ+ n ) T , (8.12)
where the function L( χn , χ+ n ) : R dim χ → R dim χ has been chosen to map non-observable directions around χ n to non-observable directions around χ + n . Applied to our SLAM problem, this principle will lead to Theorem 17: Theorem 17. The SLAM problem (8.5), (8.6) linearized using the classical linear error variable gives the propagation and update matrices F n and H n defined by (8.7). Assume they are used in Equation 8.12 and let P n denote the obtained solution, where L is defined as:

L χn , χ + n =   1 0 1,2 0 1,2 J X+ n -Xn I 2 0 2,2 J ( p+ n -pn ) 0 2,2 I 2   .
Let δ χ 0 denote a linear combination of the infinitesimal rotations (defined in Proposition 23) and translations of the whole system. Then the non-observability of δ χ 0 is preserved in the sense of Definition 10, regardless of the linearization points. We recall the meaning of this property:

(δ χ n ) T P -1 n (δ χ n ) decreases over time,
where δ χ n is the propagation of χ 0 defined by:

∀n 0, δ χ n+1 = F n δ χ n .
The rest of this section shows how L is found and proves Theorem 15. The first-order rotations δ χ R 0 (see Proposition 23) and translations χ 1 0 , χ 1 0 are defined as follows:

δ X R =   1 J Xt n J pt n   , δ X 1 =       0 1 0 1 0       , δ X 2 =       0 0 1 0 1      
.

The reader can verify that the matrix L( χn , χ+ n ) defined below maps δ X R 0 , δ X 1 0 , δ X 2 0 on the same vectors defined around χ+ n . Note that δ X 1 0 and δ X 2 0 stay the same but δ X R 0 becomes

δ X R =   1 J X+ n J p+ n   . L( χn , χ + n ) =   1 0 1,2 0 1,2 J( X+ n -Xn ) I 2 0 2,2 J( p+ n -pn ) 0 2,2 I 2   .
The modified Riccati equation resulting of the introduction of L is easily studied, as L, used after the classical covariance update and before the next propagation step, plays the exact same role as the Jacobian of the propagation. Thus, considering again a sequence of estimates (χ n , χ + n ), the linear system "seen" by the EKF is defined by the modified propagation matrix Fn and the observation matrix H n associated to the linear error (see 8.7):

Fn = F n .L( χn , χ+ n ) =   1 0 1,2 0 1,2 JR( θ + n ) vn I 2 0 2,2 0 2,1 0 2,2 I 2     1 0 1,2 0 1,2 J( X+ n -Xn ) I 2 0 2,2 J( p+ n -pn ) 0 2,2 I 2   , H n = Dh χn   ( pn -Xn ) T R( θn )J -R( θn ) R( θn )   T .
To prove preservation of the non-observability of infinitesimal rotations and translations we want to show for any m 0:

H m+1 Π m n=1 Fn F 0 δ X 0 = 0,
where δ X 0 is either an infinitesimal rotation or translation. Although looking maybe complicated at the beginning, the computation is going to simplify as out of magic, to the contrary of the Riccati equation without correction studied in 8.3. We have:

(Π m n=1 Fn )F 0 = Π m n=1 F n L χn , χ+ n F 0 = F m Π m-1 n=0 L( χn+1 , χ+ n+1 )F n = F m Π m-1 n=0   1 0 1,2 0 1,2 J( X+ n+1 -Xn+1 ) I 2 0 2,2 J( p+ n+1 -pn+1 ) 0 2,2 I 2     1 0 1,2 0 1,2 JR( θ + n ) vn I 2 0 2,2 0 2,1 0 2,2 I 2   = F m Π m-1 n=0   1 0 1,2 0 1,2 J( X+ n+1 -Xn+1 + R( θ + n ) vn ) I 2 0 2,2 J( p+ n+1 -pn+1 ) 0 2,2 I 2   = F m Π m-1 n=0   1 0 1,2 0 1,2 J( X+ n+1 -X+ n ) I 2 0 2,2 J( p+ n+1 -p+ n ) 0 2,2 I 2   . (8.13)
To obtain the last equality we used the propagation equations of Xn ( Xn+1 = X + n+1 +R( θ + n ) vn ) and p ( pn+1 = p + n ). An iterated application of (8.9) on the result (8.13) gives:

(Π m n=1 Fn )F 0 = F m   1 0 1,2 0 1,2 J( X+ m -X+ 0 ) I 2 0 2,2 J( p+ m -p+ 0 ) 0 2,2 I 2   =   1 0 1,2 0 1,2 J( Xm+1 -X+ 0 ) I 2 0 2,2 J( pm+1 -p+ 0 ) 0 2,2 I 2   .
And finally:

H m+1 (Π m n=1 Fn )F 0 δ χ 0 = Dh χm+1   ( pm+1 -Xm+1 ) T R( θm+1 )J -R( θm+1 ) R( θm+1 )   T   1 0 1,2 0 1,2 J( Xm+1 -X+ 0 ) I 2 0 2,2 J( pm+1 -p+ 0 ) 0 2,2 I 2   δ χ 0 = Dh χm+1 R(θ m+1 )   -X0 -p0 T J -I 2 I 2   T δ χ 0 .
We recall the three values of δ X for which we want to obtain H m+1 (Π m n=1 Fn )F 0 δ χ 0 = 0 are:

δ χ R 0 =   1 J Xt n J pt n   , δ χ 1 0 =       0 1 0 1 0       , δ χ 2 0 =       0 0 1 0 1       .
Verification is obvious. We just proved Theorem 17.

This becomes easy if we notice the relations: To prove these relations we have to compute De χ n :

H n = Hn De
  θ n -θn R( θ -θ )X n -Xn R( θ -θ )p n -pn   ≈   θ n -θn X n -Xn -θ n -θn J Xn (p n -pn ) -θ n -θn J pn   , i.e. : De χ n = 1 0 1,2 0 1,2 -J Xn 0 1,2 0 1,2 -J pn 0 1,2 0 1,2 .
We only have to replace P ++ t n by its value in (8.14) and use 8.15 to obtain the result.

Remark 15. The same proof works for any system and any error variable: a non-linear error variable can be used virtually, adding only a correction L ( χn , χ+ n ) at each update.

Figure 8.5 explains differently why choosing an error variable is equivalent to choosing a way to make the covariance matrix "turn" after each update.

Conclusion

In this chapter we proposed an analysis of the classical problem of EKF SLAM inconsistency, and showed it could be solved by the mere use of a non-linear error variable. As this statement could seem surprising, we explained the physical meaning of using a non-linear error variable: this is a way to make the estimation error covariance matrices "turn" after each update to match the new non-observable subspace. Note that for now, we only discussed the Riccati equation associated to a given sequence of linearization points. Next chapter builds upon the important results obtained to discuss the properties of an EKF relying on a non-linear error variable.

(a) A linear error variable implicitly defines a basis of the state space, this basis is independent from the estimate (b) A non-linear error variable implicitly defines a basis of the state space, this basis depends on the estimate (c) With a linear error variable the covariance of the state is the same as the covariance of the error. As P + t n+1 represents the covariance of the error, it also represents the covariance of the state (d) With a non-linear error variable the covariance of the state is not the covariance of the error. For a given covariance of the error the covariance of the state depends on the estimate. As a consequence, each time the filter updates its estimate, an implicit modification of the state covariance is performed. Figure 8.5 -Relation between non-linear error variables and correction of the covariance matrix. A first-order perturbation of the state around a given value creates a first-order perturbation of the error around zero. Thus, a correspondence can be established between a basis of the error space and a basis of the state space. For a linear error, the basis of the state space depends only of the basis of the error space. For a non-linear error, it depends also on the point of the state space around which the perturbations are considered. It means that a non-linear error variable implicitly maps a basis around a point of the state space to a different basis around another point of the state space. The covariance matrix P + t n+1 is computed in the basis defined by the error. Thus, if the estimate is updated, the ellipsoid representing P + t n+1 turns with the basis. If the error variable is linear, the same behavior can be emulated with the covariance correction presented in Section 8.5.

Chapter 9 EKF based on a non-linear error

Chapter abstract This chapter extends the results obtained for the problem of SLAM to a general non-linear system and discusses the possibility of design of an EKF based on a non-linear error variable. We also propose a non-linear update step inspired by the exponential map of Lie groups and show that deriving some global properties for the EKF is possible, assuming the error variable and update procedure have been properly chosen. The novel properties obtained are to our knowledge among the few dealing with the global behavior of an EKF in a general enough non-linear context.

Introduction

We generalize now the results of Chapter 8. Note the use of non-linear errors to devise EKF has already been explored in various contexts, such as attitude filtering [START_REF] John L Crassidis | Survey of nonlinear attitude estimation methods[END_REF][START_REF] Landis | Attitude error representations for kalman filtering[END_REF], but our goal here is not anymore to find an error variable allowing the design of a better Kalman-like method, but to the discuss the implications of such a choice. Section 9.2 builds an EKF based on a non-linear error variable and shows that its update step is not as straightforward as could be expected. A method inspired by the theory of Lie groups and called "exponential update" is introduced and compared with more natural approaches. In sections 9.3 and 9.4, novel global properties of EKFs based on non-linear error variables are derived. Section 9.5 links the error variable issues with filtering on manifolds and explains the hidden choices underlying the classical EKF. Finally, Section 9.6 proposes the systems studied in Part I as a guideline to find relevant error variables.

Building an EKF upon a non-linear error variable 9.2.1 Transposition of the classical equations

We transpose the procedure of the EKF, described in Section 8.2.2, to the case where the error variable has been defined by a non-linear function:

e t = ε( χt , χ t ),
with ε : R n × R n → R n (n being the dimension of the state x) which verifies ε(x, x) = 0 and which is locally invertible with respect to each of its variables around ε = 0.

Indeed, in a nonlinear setting there is no reason why the linear error χ t -χt should prevail. We thus define some kind of Jacobian matrices A e χ ( χt n ), A e w ( χt n ), H e χ ( χt n ) and H e χ ( χt n ) through a first-order approximation of the error evolution and of the measurement error as follows:

d dt e t = A e χ ( χt n ) e t + A e w ( χt n ) w t + • (e t ) + • (w t ) , (9.1)
Y n -h( χt n , 0) = H e χ ( χt n ) e t n + H e V ( χt n )V n + • (e t n ) + • (V n ) . ( 9
) T + H e V ( χt n )R n H e V ( χt n ) T , K n = P t n H e χ ( χt n ) T S -1 n , ê+ t n = K n Y n -h( χt n , 0) . (9.4)
This done, deriving a new estimate χ+ t n consistent with the estimated error ê+ t n is not straightforward as in the linear case. It requires a mapping ψ taking the previous estimate χt n and the estimated error as argument, and giving an updated estimate:

χ+ t n = ψ χt n , ê+ t n , (9.5)
and the error attached to χ+ t n has covariance P + t n :

P + t n = (I -K n H e χ ( χt n ))P t n .
When the error is defined as the classical vector difference, the issue of the update function ψ does not have to be raised as adding the computed error to the estimated state looks quite natural. In the non-linear case the answer is much less clear as shown by the different possibilities discussed in the next section.

The issue of the update method

All the steps of the classical EKF but one have been straightforwardly transposed to the case of a non-linear error variable. The only problem arises with the computation of the new update. Assume an estimate e + t n of the error variable has been obtained after the observation step. An updated estimate χ+ t n has to be computed out of e + t n and χt n . The most natural way, but which will turn out to be the least relevant (see Proposition 27 and Figure 9.5), is the following: Definition 11 (Implicit update). Given an estimate χt n and estimated error e + t n , the implicit update consists in choosing as a new estimate χ+ t n = ψ χt n , ê+ t n the one which best fits the error, i.e. solving for χ+ t n the equation:

e + t n = ε( χ+ t n , χt n ).
This method can look interesting, but has the major drawback of relying on the secondorder terms of the error function. A consequence for our SLAM example as introduced in Chapter 8 is that the obtained EKF gives different results if the whole system (car and features) is shifted by a vector U ∈ R 2 . Indeed, assume a correction e + t n = e θ e X has been computed and is applied to two copies of the system separated by the shift U. We use the notations introduced in 8 and skip the parts of the equations related to p. Without the shift the implicit update is obtained solving:

θ + n -θn R( θ + n -θn )X + t n -Xn = e θ e X .
It gives:

X+ n = R(e θ )(
Xn + e X ). The same computation replacing Xn with Xn +U gives:

X+ n = R(e θ )( Xn + e X ) + R(e θ )U,
where we would like to have:

X+ n = R(e θ )( Xn + e X ) +U.
Note that the difference can be as large as wanted if U is chosen large enough. We showed the following result: Proposition 27. An EKF for the SLAM problem described in 8.4 relying on the nonlinear variable (8.10) gives different results if the general system is shifted.

This disturbing property makes the implicit update unacceptable in practice. Note that the issue is typically non-linear: it does not occur for linear system although the classical update is a linear form of implicit update. Thus, we introduce here two better choices: Definition 12 (First-order update). Given an estimate χt n and estimated error e + t n , the first-order update consists in choosing as a new estimate the one which best fits the first-order expansion of the error, i.e. solving for χ+ t n the equation:

e + t n = De χtn ( χ+ t n -χt n ),
with De χtn being defined by the first-order expansion:

ε( χt n + δ χ, χt n ) ≈ De χtn δ χ.
This method makes our EKF SLAM translation-invariant but we can do better, as the sequel will show, using a more sophisticated update that we introduce here and that is inspired by the exponential update we introduced in [START_REF] Barrau | Intrinsic filtering on lie groups with applications to attitude estimation[END_REF]:

Definition 13 (exponential update). Given an estimate χt n and estimated error e + t n , let χs denote the solution of the differential equation:

d ds χs = Dx( χs )e + t n ,
with initialization χ0 = χt n and where Dx is defined by the first-order expansion:

ε(χ t , χ t + Dx(χ t )δ e) ≈ δ e.
It is the first-order expansion of the true state about χ t w.r.t. a variation of the error about 0.

The exponential update consists then in choosing as a new estimate χ+ t n = χ1 (solution of the equation for s = 1).

Another way to see the exponential update is the following: Proposition 28. Assume the implicit or first-order update has been chosen, but instead of applying directly the formula χ+ t n = ψ χt n , e + t n , the error is divided by n then the operation χ → ψ χ, e + tn n is iterated n times. For n → ∞ the new estimate obtained is independent from the mapping ψ (implicit or first-order) and is precisely the one resulting from the exponential mapping. This is the reason why we propose this terminology: exponential update is the infinite iteration of an elementary update.

Note that solving a differential equation at each update is in practice not required as in most situations a close-form solution is available. This is due to the relation between error variables and Lie groups emphasized in the next chapters. For our SLAM problem, the reader can verify that the exponential update, for e + t n =   e θ e X e p   , is equivalent to:

θ + t n = θt n + e θ , Figure 9
.5 shows the difference between the three updates. Section 9.4 emphases an interesting property of the exponential update, confirming figure 9.5 and making it possibly decisive in SLAM applications. But we first summarize the ideas proposed so far.

Non-linear error and false observability

This section generalizes the results obtained in Chapter 8. The chosen error variable is shown to determine the observability properties of the system linearized on a trajectory involving updates (i.e. jumps). Remark 16 explains again the idea illustrated on figure 8.5: the error variable basically is a local basis, and two bases turning differently when the estimate "jumps" lead to two different linearized systems. Note also that, the matrices A e x and H e x are expressed in this basis, that is, they take as inputs vectors whose components are coordinates in this basis. Similarly, P t is defined in this basis which depends on the estimate χt . This is the key feature of all this chapter (and actually of all this document): the error variable is not just a parameterization allowing to do the computations of the EKF, it is the choice of a correspondence between first-order perturbations about two different points χ 1 and χ 2 . As well, the update function is a way to translate a perturbation about a given point χ 1 as a new point χ 2 . Geometric interpretations of Section 9.5 will try to show that any attempt of an intrinsic definition of the EKF on a smooth manifold boils down to the same questions: -How to map the tangent plan about one point to the tangent plan about another point ? -How to map the tangent plan about one point to the state space.

We are now ready to prove the main result of this section, that characterizes a set of non-observable directions expressed at each point in the basis defined by a given non-linear error (see Remark 16) and then proves that the information decreases indeed along those directions for any EKF based on the corresponding modified nonlinear. 9.5 -We consider here a car making relative range and bearing observations of a feature. After a few observations, the position of the feature is known in the reference frame of the car. Thus, if the heading is updated then the estimate of the feature should follow a circle around the car. In the case of SLAM, only the exponential update has this behavior Next section explores another issue: the ability of the filter to include certain information, that is, to handle singular covariance matrices. This is not at all a degenerated situation due to numerical error, as some variables of a system can come to be known with high precision while others are still unknown.

Nonlinear exponential update and singular information

In a filtering problem, one can be confronted to the issue of an information becoming extremely precise. It happens for instance if a robot observes a large number of times the same set of features with centered noise (with heading and range information). As the features don't move, the knowledge of the local map in the robot frame eventually becomes exact. This kind of information takes the form of a function of the state h(χ) to be known, that is: h(χ t ) ≡ c,t t 0 .

If an EKF has worked properly during the observation phase, its absolute confidence in the value of h(χ) is encoded by a first-order relation:

He χtn P t n HeT χtn = 0, (9.6)
where He χt is defined by: h(χ) ≈ h( χ) + He χ e + •(e). (9.7) e = ε(χ t , χt ) being the non linear error associated with the covariance matrix P t . He χ is the classical Jacobian of h if the error is linear. Two properties seem essential to ensure then a sound behavior of the filter:

1. No update can change the relation H χtn P t n HT χtn = 0. Indeed, this would mean the new observation made the filter "forget" the value of h. Worse, as rank P + t n rank P t n , the filter would still consider some directions as certain. But not the right ones. 8.10), depending on the update method used. A static car (black cross) maps its environment with no information about its heading. Four features, arranged as a square around the car, have been observed a large number of times. Thus, their positions are known up to the heading of the car to which they are highly correlated. We assume here this correlation is correctly encoded by the covariance matrix P t . The blue circles indicate the estimated positions of the features. Then, an accurate heading observation is provided to the system, producing a large counter-clockwise variation of the estimated heading. Because of the correlation encoded by P t between heading and features positions, those move in a way coherent with the heading update, i.e., they "turn" with the car. In other words, the car has accurate information in its own reference frame, which has no reason to be modified by a heading update: the estimated local map should turn as information is obtained regarding the general orientation of the system. On the left plot, the red circles are the new features estimates if a first-order update is used. They turned counter-clockwise as wanted, but only up to the first order. Then they followed straight lines, tangentially to the circle centered on the car. As a consequence, the estimated map underwent a more complicated transformation than the simple rotation we expected. On the right plot, the red circles are the new features estimates if an exponential update is used. The result is very different: the transformation is this time a true rotation. The positions of the estimates in the estimated reference frame of the car are the same as before the update, only the general orientation has changed. This property is the preservation of a local map mentioned at the end of Section 9.4. Note that a classical EKF encounters in this situation the exact same problem as the EKF using non-linear error variable but first-order update. Figure 9.9 -EKF on the sphere: the updated state and covariance matrix are both defined in the tangent plan to the previous estimate. Thus, defining a mapping is necessary to bring them back to the state space.

No update can change the value of h( χt

n ) Algorithm 5 EKF with linear error variable (θ -θ , X -X, p -p) R is known, Q =   q 2 ω 0 0 0 0 Q v   are known parameters θ 0 = 0, x 0 = (0, 0), P 0 =   σ 2 cap 0 0 0 0 0 0 0 0   for i = 2 to N do xi = xi-1 + ∆t.R( θi-1 )v i θi = θi-1 + ∆t.ω i F =       1 0 0 0 0 -∆t sin( θ )v 1 -∆t cos( θ )v 2 1 0 0 0 ∆t cos( θ )v 1 -∆t sin( θ )v 2 0 1 0 0 0 0 0 1 0 0 0 0 0 1       P i = F.P i-1 .F T + Q if Observation Y i available then H = 0 1 -1 0 R( θ ) T ( p -x), -R( θ ) T , R( θ ) T S = HPH T + R K = P i H T S -1 e + = K Y i -R( θ ) T ( p -x) dθ = e + 1 ,

A geometric interpretation

The reason why the question of the error variable chosen for SLAM has not been studied before is probably that working in a vector space gives the illusion of a "natural" way to associate perturbations about a point χ 1 with perturbations about another point χ 2 (the vector translation). As a toy, yet illustrative example, we now assume the state to be estimated lives on a sphere (as can be the case when one tries to estimate a direction of motion in the 3D space for instance). Recall the update step of the conventional EKF is defined by:

χ+ t n = χt n + ê+ t n = χt n + K n Y n -h( χt n , 0) , P + t n = (I -K n H( χt n ))P t n
Its implementation is usually considered as straightforward, but we see it is actually not. On the sphere, no addition is defined, so the update step which is inherently an addition, is thus naturally performed in the tangent plane to χ, which is a vector space, as illustrated on Figure 9.9. The updated state χ+ t n has a covariance matrix P + t n attached to it, and both are now defined in the tangent plan to χt n . In order to define the updated state χ+ t n as an element of the sphere (that is, the state space) with a covariance matrix attached to it (that is, in geometric terms an uncertainty ellipsoid defined in the tangent plane, see Fig. 9.9), at least two additional tools are needed: a function ψ mapping the new estimate from the tangent plane at χt n to the sphere, then a correspondence between vectors tangent to χt n and those tangents to χ+ t n allowing to define the new uncertainty ellipsoid. The latter is equivalent to having a basis of the tangent plane at χ+ t n in which P + t n is defined. Defining this new basis is precisely the sense to be given to the design of an error variable. The crucial issue is that this choice does have an influence on the filter. If the sphere (or a part of it) was parameterized, the problem would become 2dimensional and the issues raised above would apparently disappear: a classical EKF could be implemented straightforwardly, and a filter devised using Euler angles would be wholly different from a filter based on e.g. stereographic projection. Of course, the problems are only hidden. The choice of a parameterization implicitly defines a choice of mapping ψ and a basis around any point (the one inherited from differentiation with respect to each coordinate). This is the basic influence of the coordinate system on the results of an EKF. But there is no reason a priori to assume a given parameterization Figure 9.10 -Influence of a measurement on the trusted covariance matrix P, depending whether the system is linearized at p = p 1 or p = p 2 (the position of the car is assumed known for simplicity). Assume the position p 1 of the landmark is measured in the robot's frame. Considering the linearized system variables at p = p 1 , the measurement yields an information about the projection of p onto the vertical axis x 2 , and neither the heading θ nor the projection of p onto the horizontal axis x 1 are observed, but both variables become correlated (a future observation of the heading will yield information about the position of the landmark in the x 1 direction). In a similar way, considering the system linearized around p = p 2 we see a measurement will yield an information gain in the projection of p onto z 2 , and will infer a correlation of the projection of p 2 over z 1 . defines a good mapping and good local bases. For different non-linear systems, the issue of mapping and local bases still exists, and we believe is the major cause of the failure of EKF SLAM. We think the new EKF-like SLAM algorithm introduced previously is based on the best possible such choices. We close this chapter with a visual explanation of false observability in SLAM (Figure 9.10 then 9.11) illustrating again the problem of bad shift of a vector.

Link with Invariant Kalman filtering on Lie groups

We described until now the properties we can expect from an EKF based on a non-linear error variable. But the most difficult issue is the choice of this error variable. This question will not be fully answered, but Remark 17 below suggests that the situations studied in Part I can give a hint. Remark 17. The linearized propagation and observation matrices used in the IEKF are the same as those computed on any trajectory of the system. Thus, the observability properties of the linearized system are precisely the first-order observability properties of Figure 9.11 -Mechanism of metric SLAM inconsistency illustrated. The position of the car is assumed known for simplicity, but the heading is not. Each time a relative position to landmark measurement is available, the system is linearized on the current estimate. Refer to Figure 9.10 for the probabilistic consequences of one update. Here, this operation is repeated and creates false heading uncertainty reduction. We let x 1 , x 2 , z 1 , z 2 denote the projections of p over the different plotted directions.(b) A new measurement is provided. (c) The system is linearized on the previous estimate. As explained on Fig. 9.10, after the update the Kalman covariance P encodes a reduced variance of x 2 and a correlation between x 1 and the heading. (d) As a consequence, the projection of p over the axis z 2 is (weakly) correlated with the heading, as z 2 is not orthogonal to x 2 . (e) Now, a new estimate is computed through the Kalman state update. (f) A new relative measurement is available. (g) The system is linearized on the last estimate: as explained on Fig. 9.10, the variable z 2 is directly observed. As P encodes a correlation between z 2 and θ , the information obtained along z 2 reduces the variance of θ after the update. But as only relative measurements are available, no information should be ever gained on the orientation θ with respect to the fixed frame.

the true system. In particular, false observability cannot occur.

But finding the good error variable from scratch is difficult for the reason explained in remark 18: Remark 18. For the systems described in Chapter 4, an EKF built upon their multiplicative error variable is not exactly an IEKF. The difference is the innovation term. Considering for example the observation has the form χ t n b the innovation term is:

• z n = Y nχ t n b for the EKF based on the error variable χ -1

t n χt n .

• z n = χ -1 t n Y nb for the IEKF. Yet, the resulting covariance matrices and estimates are strictly equal: the modification of the innovation term is automatically compensated by a modification of the gain. But the consequence of this difference is that the matrix H depends on the estimate in the first case and the independence of the Riccati equation from the estimate is not visible.

As a consequence of this remark, finding a good error variable without starting from one of the perfect cases described in Chapter 4 is out of reach. Fortunately, most of navigation problems take the generalized form (5.1) and inherit some properties of the system they are based on as shown by Theorem 20 below: Theorem 20. If the simplified system:

d dt χ t = f u t (χ t ), Y t n = χ t n b,
has a space V verifying the hypotheses of Theorem 18 for any sequence of inputs (as in SLAM for instance) then these directions are still non-observable in the full system and are preserved by the imperfect IEKF.

Proof. We have to verify that V 0 p,1 is stable through the flow and is in ker H. We have:

F = F χ F Θ,χ 0 F Θ ξ 0 p,1 = F χ ξ 0 p,1 , thus V 0 p,1
is stable. We have also:

H ξ 0 p,1 = Dh[H χ , H Θ ] ξ 0 p,1 = DhH χ ξ = 0.
Thus Theorem 18 applies and V is a non-observable subspace preserved by the filter.

Example 6. The SLAM problem (8.5)-(8.6) studied in chapter 9 has the form (5.1) where the core system is the simplified car studied in section 4.3.1 with observation (4.21). Thus the non-observability of general rotations and translations is preserved by an EKF using right-invariant error variable.

Example 7. Consider following the monocular SLAM problem:

d dt R t = αR t ω t , d dt x t = β R t (v t + b), d dt p t = 0, Y = angle(R T t (x t -p t )).
The rotations and translations of the whole system are non-observable, as well as a general scaling of x t , p and β . An EKF based upon the error variable

e t = Rt R -1 , ( xt -Rt R -1 t x t ), ( pt -Rt R -1 t p t ), α α , β β , ( b -b) ,
preserves the non-observability of global translations and rotations, but not the nonobservability of the scaling factor.

Conclusion

We detailed in this chapter the difficulties related to the design of an EKF relying on a non-linear error variable, and gave a physical meaning to these methods (in an EKF context at least): choosing an error variable is choosing a local basis around each point of the system. This choice is unimportant as long as the linearization trajectory is continuous. If "jumps" in the estimate occur, due to the updates, different error variables produce different observability properties. A result preventing the classical problem of false observability to occur was derived. The exponential update, inspired by the theory of Lie groups, was introduced and proved to ensure novel global properties, namely preservation of some deterministic side information about the state. The latter will be "experimentally" verified in the next chapter.

Chapter 10

Illustration by a tutorial example

Chapter abstract This Chapter clarifies the articulation between the methods described previously using the problem described in Section 4.3.1 (GPS-aided odometry for the non-holonomic car). Departing from the IEKF, we remove its specificities one by one to obtain 3 algorithms ranging from the IEKF to the conventional EKF. The superiority of the IEKF over the EKF even for this extremely simple low-dimensional and barely non-linear example is puzzling, both in terms of convergence and (especially) in terms of "rationality" of the behavior. Beyond proving the benefits of the IEKF, this tutorial example has been chosen to provide the reader with a physical/geometrical understanding of the limits of the EKF in high-performance navigation.

Introduction

In this chapter we study a navigation problem involving perfect knowledge of the dynamics (the process noise Q is turned off in the true world and in the filter parameters). This artificial situation is of decisive importance as a limit case of high-accuracy navigation. The IMUs used on aircrafts and ships give almost perfect propagation for several minutes, and even some cheap odometers can also achieve very accurate estimation of the trajectory on hundreds of meters. Regarding Kalman filtering, it means the gains can be large at the beginning if the prior information is inaccurate, but then decrease to zero. It does not prevent convergence of the estimate in the linear case but it is very different if the system is non-linear. In the example presented here the EKF converges for no initialization, except if the error is zero from the beginning. Although it manages to make the error decrease, the convergence is slowed down by non-linearities, and eventually not fast enough to counterbalance the decrease of the gains: the error stabilizes at a nonzero value. This extreme situation does not occur on true systems as the process noise Q is never zero. But the convergence speed can be so deteriorated that the result does not match the requirements of the system anymore. In particular, this is what happens during the transitory phase of an inertial navigation, known as alignment, where the algorithm starts with no information about the state of the system. Complicated procedures mentioned in Chapter 5 are then required to ensure the EKF starts with a good first guess of the state of the system. Note that the solution consisting in adding artificial process noise (Q) also greatly degrades performance. Besides, we consider only EKF-like methods. Any attempt to explicitly use the fact that the dynamics is exact would be irrelevant here as what we are interested in is the limit behavior of these methods when the process noise Q goes to zero. This chapter is organized as follows. Section 10.2 introduces the system, Section 10.3 derives the equations of the classical EKF, Section 10.4 those of the IEKF, Section 10.5 discusses the differences between these filters and proposes to test intermediary methods allowing to understand what is the most important difference between EKF and IEKF. One of them is discussed in the same section, the other one in Section 10.6. All the methods are compared in Section 10.7, and finally the results displayed are explained in Section 10.8 through theoretical arguments.

Perfect dynamics

The system we consider is the one described in Section 4.3.1 where the noise on the dynamics has been put to zero:

d dt θ t = u t v t , d dt x 1 t = cos(θ t )v t , d dt x 2 t = sin(θ t )v t , (10.1)
where θ t is the heading of the car, x 1 t , x 2 t are the components of the position vector, u t is (a function of) the steering angle and v t is the velocity computed through odometry. The GPS observation takes the form:

Y n = x 1 t x 2 t +V n , (10.2)
where V n is an i.i.d. Gaussian noise with covariance matrix R. We focus thus on the behavior of the EKF in the degenerated case where the initial position of the car is perfectly known but its heading is not. This is modeled by a singular initial covariance matrix (the first variable is heading, the two others are position):

P 0 =   π/2 0 0 0 0 0 0 0 0   .
Of course, after a few time steps the position becomes also uncertain due to the initial heading error. The next sections describe different filters for this dynamics. The results of EKF and IEKF in terms of position error are displayed on Figure 10.1. They show that as soon as the initial heading error is non-zero, the EKF can improve the position error but never bring it to zero as opposed to the IEKF (left plot). We verified this letting the filter run long enough (right plot). Although the observation covariance matrix R is non-zero we fed the filter with perfect observations (whitch is a more favorable situation for the EKF than using noisy data) to obtain clean curves.

Figure 10.1 -Influence of singular information. Left plot : position error of the EKF for GPS and perfect odometry, with initial position known and initial heading unknown. We see the estimate is never converging, whatever be the initial heading error (except exactly zero). Right plot: after a very long time the results of the EKF have stabilized at a nonzero value while those of the IEKF converge even with maximum initial heading error.

Classical EKF equations

The EKF with classical error variable e

t =   θ t -θt x 1 t -x1 t x 2 t -x2 t 
 for our system is defined by the following propagation and update steps:

Propagation: d dt θt = u t v t , d dt x1 t = cos( θt )v t , d dt x2 t = sin( θt )v t , t n-1 < t < t n , d dt P t = A t P t + P t A T t , t n-1 < t < t n , (10.3)
with

A t =   0 0 0 -sin( θt )v t 0 0 cos( θt )v t 0 0   .
Note that Equation (10.3) contains no process noise (usually denoted by Q t ). It has been put to zero here to study the extreme case of a very accurate dynamics.

Update:

The update step consists of the following operations:

• Computation of the gains:

K n = P t n H T HP t n H T + R -1 ,
with H = 0 1 0 0 0 1 .

• Computation of the innovation:

z = Y - x1 t n x2 t n .
• Computation of the new estimate:

  θt n + ( x1 t n ) + ( x2 t n ) +   =   θt n x1 t n x1 t n   + K n z.
• Update of the covariance matrix:

P + t n = (I -K n H) P t n .

IEKF equations

The Left-invariant IEKF for our system uses the error variable:

  θ -θ R(θ ) T x1 t -x 1 t x2 t -x 2 t   .
The algorithm is defined by the propagation and update steps derived in Section 4.3.1 and recalled here:

Propagation: d dt θt = u t v t , d dt x1 t = cos( θt )v t , d dt x2 t = sin( θt )v t , t n-1 < t < t n d dt P t = A t P t + P t A T t , t n-1 < t < t n , (10.4)
with

A t =   0 0 0 0 0 u t v t -v t -u t v t 0   .
Again, the equation contains no process noise.

Update:

The update step consists of the following operations:

• Computation of the gains:

K n = PH T HP t H T + R -1 ,
with H = -0 1 0 0 0 1 .

• Computation of the innovation:

z = R( θ ) T Y n - x1 t n x2 t n
.

• Computation of the new estimate:

R( θ + ) x+ 0 1×2 1 = R( θ ) x 0 1×2 1 exp(-K n z)
where exp : R 3 → R 3 is the exponential map of SE(2) defined in Chapter 2. • Update of the covariance matrix:

P + t n = (I -K n H) P t n .

Differences between the two filters

There are essentially three differences between the EKF and IEKF described here:

• The definition of the innovation is different.

• The computation of the new estimate is different.

• The error variable is different.

An interesting question now is to study the influence of each of these differences on the results. To isolate them, we build in the next section a third filter: an EKF having the same (non-linear) error variable as the IEKF, but using the implicit update instead of the exponential update of the IEKF. But before, we settle the issue of the innovation term by Proposition 29 below.

Proposition 29. Applying a linear function to the innovation term of an EKF before computing the gains does not change the results of the filter.

Proof.

= P t n H e n T S n -1 = P t n H e n T S -1 n L -1 n = K n L -1
n and we finally obtain K n z n = K n z n . Thus the updated state is going to be the same (regardless of the update method). The updated covariance matrix is defined as (I -K n H e n ) P n = I -K n L -1 n L n H e n P n and this step is also unchanged by the use of z n .

Non-linear EKF using implicit update

We give here the equations of an "intermediate" filter. It uses the error variable of the

IEKF   θ -θ R(θ ) T x1 t -x 1 t x2 t -x 2 t 
 along with an implicit update. The algorithm is defined by the following propagation and update steps:

Propagation: d dt θt = u t v t , d dt x1 t = cos( θt )v t , d dt x2 t = sin( θt )v t , t n-1 < t < t n d dt P t = A t P t + P t A T t , t n-1 < t < t n , (10.5)
with

A t =   0 0 0 0 0 u t v t -v t -u t v t 0   .
Again, the equation contains no process noise.

Update:

The update step consists of the following operations:

• Computation of the gains:

K n = PH T HP t H T + R -1 ,
with H = -0 cos θt nsin θt n 0 sin θt n cos θt n .

• Computation of the innovation:

z = Y n - x1 t n x2 t n .
• Computation of the new estimate (implicit update):

θ + t n = θt n -êθ , x1 x2 + = x1 x2 -R θ + t n êx 1 êx 2 , with   êθ êx 1 êx 2   = K n z n .
• Update of the covariance matrix:

P + t n = (I -K n H) P t n .

Experimental comparison

The error variable used in the IEKF leads to a Riccati Equation independent from the estimated trajectory. As a consequence, a false estimate does not create false gain matrices. In particular, the observability properties which can be read on the covariance matrix P t are those of the true system. This is true independently from the chosen update function. A natural question here is whether a good error variable is sufficient to improve performance and the exponential update is a detail, or the exponential update is required also. From a theoretical point of view, propagating correctly singular information requires exponential update (see Section 9.4). This aspect will be developed in Section 10. non-linear error variable is compared with the same algorithm where only the update method has been changed from implicit to exponential update. Figure 10.3 displays the estimates given by the two filters: trajectories on the left plot and heading error on the right plot. A second clear improvement is obtained. The interesting conclusion we can draw from this comparison is that exponential is required for the EKF to fully benefit from a non-linear error variable. Next section explains the physical phenomenon behind the failure of the EKF, here and in most situations where an accurate navigation system has to be initialized.

Explanation

The specificity of a navigation using accurate dynamics and known initial position but unknown initial heading is that the true dimensionality of the problem is much less than the dimension of the state space. Regarding our car, the state space dimension is 3 (heading and the two components of position). But the only unknown being the initial heading, the set of possible states of the car is only a one-dimensional manifold at any time. For example, if the car drives a straight line from a known point, its position at any time is on a circle centered on the starting point. If the estimate leaves this manifold (the circle) its probability to reach it again is zero. We will see in the sequel (figure 10.5) that the EKF brings the estimate "outside the circle",i.e., too far from the starting point. The EKF with the right non-linear error but implicit update, to the opposite, brings the estimate too close to the starting point. But in both cases, nothing can bring them back to the right distance. What we will show in this section using Theorem 19, is that the IEKF keeps the estimate in the manifold of physically possible states (the circle). It means the search space of the IEKF is one-dimensional and contains the true state. In this situation, having the update steps pushing to the right side is almost sufficient to converge. Figures 10.4

0 = 0 0 , d dt b t = - 0 -u t v t u t v t 0 b t - v t 0 .
The relation

R(θ t )b t + x 1 t x 2 t = 0 0 , (10.6)
is always verified by the true state of the car, but also by the estimate of the IEKF. Thus, these two variables are restricted in a one-dimensional space which changes at each time step. In some sense, the IEKF understands it has to work in a reduceddimension search space, what the classical EKF does not do.

The states verifying Equation (10.6) constitute, at a given time step, a one-dimensional manifold containing the true state regardless of the true value of the initial heading ( note that this manifold varies over time). Proposition 30 means the estimate of the IEKF is confined inside this manifold. In the specific case of an odometer indicating straight line and constant velocity, the estimate of the IEKF always verifies the property "the starting point is exactly behind the car, at a distence proportional to the time gone by". This links the position and heading, the remaining uncertainty being in what direction the car is going. Figures 10.4 and 10.5 illustrate this result, but also show it is neither verified for EKF using linear error variable nor the proper non-linear error variable if it is not going along with an exponential update. This is extremely important. The relevant search space if the odometer indicates straight line is a circle and an EKF-like method involving singular covariance matrix cannot afford to leave it. Indeed, singularity of P t makes the states accessible to the filter one-dimensional at each step and nothing can ensure a priori that the true state is within this space, or even close to this space. For the IEKF, it is ensured by Proposition 30. We give now its proof.

Proof. The perfect knowledge of the initial position takes the following form:

x 1 0 x 2 0 = 0 0 .
we have: and uncertainty ellipsoid (they are superposed on the plot so only one of them is visible). This confirms what was told throughout Chapter 8: in the absence of update all the error variables are equivalent. The position uncertainty is mainly due to heading, thus the ellipsoids are stretched tangentially to the circle. But from the first update, the filters have very different reactions. The classical EKF (blue) moves its estimate tangentially to the circle and keeps an ellipsoid encoding the uncertainty of the system linearized on the last estimate before the update. The EKF having the right error variable but implicit update has a more complicated evolution, but still leaves the circle. Yet, its uncertainty ellipsoid is consistent with its new estimate as the non-linear error variable made it "turn" properly.

d dt R(θ t )b t + x 1 t x 2 t = R(θ t ) -u t v
The IEKF finally, stays on the circle and has an uncertainty ellipsoid making sense with the physics of the problem. Here actually, it is not exactly on the circle as the uncertainty over initial position authorizes updates in any direction. But it simply confirms that the nice properties derived in this chapter for the limit case of a known initial position give a good idea of what happens on a true system.

Conclusion

This chapter illustrated on a very simple example a phenomenon at play in high-precision navigation: the set of likely values of the true state reduces, due to the information obtained, to the neighborhood of a non-linear sub-manifold of the state space. The estimate of an EKF not using both a relevant error variable and the exponential update introduced in Chapter 9 can very easily leave this region, and never reach it again. This issue makes the IEKF decisive in some applications like those presented in Chapter 5.3. In order to make sure the choice of the error variable was not the only ingredient making the performance of the IEKF, we implemented this method without the exponential update. The results obtained allow to conclude that the benefits of the exponential update are not simply theoretical. Chapter 11

Conclusion of the thesis

This work investigated the role of the error variable in EKF and proved it to be of decisive importance for several classical problems related to navigation.

A new class of dynamical systems defined on a Lie group G has been characterized, considered as "perfect" in the sense that an error variable having autonomous equation can be built. It contains in particular linear equations, left-invariant dynamics, rightinvariant dynamics, combinations of both, and some other equations appearing in inertial navigation. The algebraic logarithm of the error is shown to have linear evolution for these systems, which is also a novel result. This property is leveraged to prove the stability of the IEKF in this case even with changing gains. The hypotheses (first-order uniform observability) are the same as those required for linear systems, which contrasts with most previous results on the convergence of the EKF. The theory is applied to a long sequence of classical navigation problems and brings each time a stability guarantee.

The success of this approach led to a general study of non-linear errors in the EKF. The main question "What does choosing a non-linear error variable physically means ?" has been answered by the interpretation of the error as 1) a local basis and 2) an operation making the covariance matrix of an EKF "turn" after each update. This suggests in particular that there is no reason to approach EKF on linear spaces differently from EKF on manifolds. The classical problem of "false observability" in SLAM has been revisited, linked to the general topic of this work and proved to be solvable by the mere choice of a non-linear error variable. The implications of such a result in practice constitute probably the subject on which we will next devote our best efforts. The questions raised by the idea of an EKF based on a non-linear error variable have been discussed. This led to the introduction of the "exponential update" inspired by the theory of Lie groups. The issue of global behavior of the EKF, i.e., with large estimation errors, has been approached not in terms of global convergence, but in terms of preservation of physical constraints. To the question "My robot has good information of its close environment, is the EKF able to preserve it after an update ?" the answer is "yes" for linear systems and "No" in general. But novel properties have been derived regarding this issue, when non-linear error variable and exponential update are jointly used. Their possibly decisive importance has been illustrated through simulations.

These reflections having focused on the value of the estimate in the context of EKF, the properties of the whole p.d.f. of the estimation error have then been tackled in a sim-plified framework. This required to discuss stochastic differential equations on Lie groups, whose rigorous meaning is non-trivial. Some sampling-based methods have been proposed, either to reduce a complicated system to a simpler one where the present theory applies, or to improve the fidelity of the covariance matrices used by Kalman-like approaches. These ideas are transpositions of existing methods: the Rao-blackwellized particle filter and Ensemble Kalman filter respectively. The study of fixed-gains filters has been related to the theory of Harris chains, which allowed to prove, under proper conditions that the p.d.f. of the estimation error converges to a limit distribution not depending on the initial density. This novel result allowed designing and optimizing highly non-linear gains for an outlier-resistant observer.

Much time was also devoted to implementation and tests for application to commercial navigation systems. Methods derived from the results summed up above were shown to allow substantial improvement in terms of simplicity of the development phase, final performance and operational flexibility. The results displayed in this document include real-data experiments, comparison with tried and tested softwares, and a campaign of realistic simulations. A program developing a navigation system for military purposes already shifted to this technology.

Further work will focus on SLAM applications as a priority. Due to the large amount of time devoted to inertial navigation, the properties derived for EKF SLAM have been the subject of minimalist simulations. They were only intended to illustrate the main result: non-linear error variables solve false observability issues. It gives a reasonable argument to come back to EKF SLAM, it does not prove the method can compete with state-of-theart algorithms involving batch optimization or particle sampling. There is still much work before answering this question, especially as our approach can be combined with one of them, for example through a back-and-forth smoothing [START_REF] Auroux | Back and forth nudging algorithm for data assimilation problems[END_REF] based on non-linear error variable. Anyway, SLAM is a household where real experiments prevail : no applicative conclusion can be drawn before having achieved extensive simulation and real-data testing, which will be part of our future work.

Proof. The Riccati equation computed about the true trajectory reads:

d
T + N 1 + η -1 χ -1 N 2 χ -T η -T -1 HP(t)
Controlling the perturbation is easy: let L ξ : x → e -ξ x and A ξ : R dim g → R dim g , x → Ad e -ξ x.

As these functions are continuous and equal to I d for ξ = 0 there exists a real ε > 0 depending only on k such that ||ξ || ≤ ε ensures

1 k N 2 L T ξ N 2 L ξ kN 2 and 1 k Q 2 A T ξ Q 2 A ξ kQ 2 . It ensures consequently 1 k Q 1 +Ad χ -1 Q 2 Ad T χ -1 Q 1 +Ad χ-1 Q 2 Ad T χ-1 k Q 1 +Ad χ -1 Q 2 Ad T χ -1 and 1 k N 1 + χ -1 N 2 χ -T N 1 + χ-1 N 2 χ-T k N 1 + χ -1 N 2 χ -T
, and a mere look at the definitions of the constants of Theorem 6 yields the modified constants.

The inequality 1 k Pt 0 +s ≤ P t 0 +s ≤ k Pt 0 +s follows from the matrix inequalities above on the covariance matrices, by writing the Riccati equation verified by kP t and 1 k P t and using simple matrix inequalities. Lemma 6. [first-order control of growth] Under the same conditions as in Lemma 5 (including 1 k Pt 0 ≤ P t 0 ≤ k Pt 0 ) and ||ξ t 0 +s || bounded by the same ε for s ∈ [0, 2M∆T ] (i.e. over 2M time steps, where M is defined as in Theorem 6), there exists a continuous function l 1 depending only on k ensuring ||ξ t 0 +s || ≤ l 1 (||ξ t 0 ||) for any s ∈ [0, 2M∆T ] and l 1 (x) = O(x).

Proof. Using Lemma 5 and then Theorem 6 we know there exist two constants γ min > 0 and γ max > 0 such that γ max I P t γ min I. The non-linear rest r t n (ξ ) introduced in (4. ).

Lemma 2.

Let G be a locally compact Lie group provided with a right-invariant distance d, and C ⊂ G be a measurable compact set endowed with the σ -algebra Σ. Consider a homogeneous Markov chain (η + n ) n 0 defined by the relation

η 0 ∈ C, η + n+1 = q z (η + n )
where z ∈ Z is some random variable belonging to a measurable space. Let 0 denote a specific point of Z . Let Q : (C, Σ) → R + , denote the transition kernel of the chain, that is Q(x,V ) = P(q z (η + n ) ∈ V | η + n = x). We assume that 1. there exist real numbers α, ε > 0 such that for any x ∈ C and V ⊂ B o (q 0 (x), α), the latter denoting the open ball center q 0 (x) and radius α, we have Q(x,V ) = P(q z (η

+ n ) ∈ V | η + n = x) ε µ G (V )
where µ G is the right-invariant Haar measure.

2. q 0 admits a fixed point x 0 ∈ C, i.e. q 0 (x 0 ) = x 0 .

Let U 0 = {x 0 }. Define the sets (U n ) n 0 recursively by U n = {x ∈ G, d(x,U n ) < α 2 } ∩ C and U n+1 = q -1 0 (U n ). If there exists an integer N 0 such that U N = C, then the p.d.f. of η + n converges for the T.V. norm and its limit does not depend on the initialization.

More prosaically, z denotes the cumulative effect of the model and observation noise, and it is assumed that 1-the noiseless algorithm has some global convergence properties and 2-there are sufficiently many small enough noises so that there is an ε chance for η + n to jump from x to any neighborhood of the noiseless iterate q 0 (x) with a multiplicative factor corresponding to the neighborhood's size. It then defines a sequence of sets, by picking a fixed point of q 0 , dilating it, and taking its pre-image. Dilatation and inversion are then reiterated until the whole set C is covered. In this case, forgetting of the initial distribution is ensured.

Proof. We will demonstrate the property trough three intermediate results:

1. There exists a sequence ε 1 , • • • , ε N such that: P(η 1 ∈ U n |η 0 ∈ U n+1 ) > ε n > 0.

2. The first hitting time of U 1 is a.s. finite.

(η +

n ) n≥0 is an aperiodic recurrent Harris chain with A = U 1 and B = U 0 .

The conclusion will then immediately follow from Theorem 21. We give first a qualitative explanation of the approach. We have by assumption a sequence of sets (U n ) n 0 . Intermediate result 1) states that starting from a set U n+1 the chain has a non-vanishing chance to jump to the "smaller" set U n at the next time step. Intermediate result 2) states that starting from U n+1 , because of the non-vanishing property 1), the chain cannot avoid U n forever and eventually reaches U n , U n-1 and finally U 1 . Intermediate result 3) brings out the fact that, A = U 0 being totally included in the ball of radius α centered on any of its points, the chain can go from any given point of its pre-image B = U 1 to any area of A with controlled probability, which is the last property needed to obtain a recurrent aperiodic Harris chain.

1. Consider the closure U n of U n . For any x ∈ U n the set B o (x, α) ∩U n is open and nonempty (as d(x,U n ) α 2 by definition of U n ) so µ G (B o (x, α) ∩U n ) > 0. The function f : x → µ G (B o (x, α) ∩U n ) > 0 is continuous (since µ G is a regular and positive measure) on the compact set U n , so it admits a minimum m n > 0. We get: P(η 1 ∈ U n |η 0 ∈ U n+1 ) ≥ P(η 1 ∈ B o (q 0 (η 0 ), α) ∩ U n |η 0 ∈ U n+1 ) = P(η 1 ∈ B o (q 0 (η 0 ), α) ∩ U n |q 0 (η 0 ) ∈ U n ) ≥ εm n using the fact that, by assumption (1), Q(x,V ) ≥ ε µ G (V ) for x ∈ C and V ⊂ B o (q 0 (x), α). We set ε n = εm n to prove intermediate result 1).

2. We will prove by descending induction on n the property P n : P(τ n < ∞) = 1. P N is obvious as U N = C. Now assume P n+1 is true for n 1. Due to homogeneity, we can construct a strictly increasing sequence of stopping times (ν p ) p 0 such that: ∀p 0, η ν p ∈ U n+1 . But P(η ν p +1 ∈ U n |η ν p ∈ U n+1 ) > ε n by intermediate result 1). Thus letting T k denote the event {∀i k, η i / ∈ U n } = {k < τ n }, we have for any n 0: P(T ν p +1 ) = P(T ν p )P(T ν p +1 |T ν p ) P(T ν p )(1-ε n ). Thus P(T ν p+1 ) P(T ν p +1 ) P(T ν p )(1-ε n ). A quick induction and a standard application of Borel-Cantelli Lemma prove there exists a.s. a rank p such that T ν p is false. In other words, P n is true. In particular, P 1 is true: the first hitting time of U 1 is a.s. finite.

Define

A = U 1 and B = U 0 = B o (x 0 , α
2 ) ∩ C . For any V ⊂ B and x ⊂ A we have V ⊂ B o (q 0 (x), α), thus Q(x,V ) ε µ G (V ) by assumption [START_REF] Aghannan | On invariant asymptotic observers[END_REF]. Thus (η + n ) n 0 verifies property 2) of Definition 15. As A = U 1 the intermediate result 2) shows that (η + n ) n 0 verifies the property 1) of Definition 15. As there exists a.s. a rank k such that η k ∈ U 1 by intermediate result 2), there exists an integer M such that P(η M ∈ U 1 ) > 0.

As U 1 ∩ U 0 ⊂ B o (q(x), α) for any x ∈ U 1 (by definition of U 0 and U 1 ) Assumption (1) gives immediately P(η M+1 ∈ U 1 ∩ U 0 ) > P(η M ∈ U 1 )ε µ G (U 1 ∩ U 0 ). Using only the definition of U 0 and U 1 , and Assumption (2), we see easily that both contain x 0 . Thus the set U 0 ∩ U 1 is non-empty and open. We have then µ G (U 1 ∩ U 0 ) > 0 and P(η M+1 ∈ U 1 ∩ U 0 ) > 0. By an obvious induction on m, P(η m ∈ U 1 ∩ U 0 ) > 0 for any m > M. As U 1 ∩ U 0 ⊂ A we get P(η m ∈ A) > 0 for any m > M and the property iii) of Definition 15 is also verified. We finally obtain that (η + n ) n 0 is an aperiodic recurrent Harris chain.

Using Theorem 21 we can conclude that the density of η + n converges in T.V. norm to its unique equilibrium distribution for any initialization.

Building upon the previous results, the proof of Theorem 11 is as follows. The Markov chain defined by equations (6.10) and (6.11) with fixed ϒ can be written under the form η n+1 = q z (η n ) = q 1,z • q 2,z (η n ) with z = (W n ,V n ) ∈ G × R p , q 1,z (x) = ϒW n xϒ -1 and q 2,z (x) = xK(h(x,V n )) -1 . Let 0 = (I d , 0). We will show that (η n ) n 0 has all the properties required in Lemma 2.

Let α, ε, ε be defined as in 6. i=1 Q n has a finite subcover and there exists an integer N 1 such that Q N 1 = K, i.e. ϒ N 1 Kϒ -N 1 = U N 1 . As K is a subgroup of G (see above) containing ϒ we obtain K ⊂ U N 1 . Now we have to prove that the sets (U n ) n 0 eventually cover the whole set C. For any x ∈ G we have h(q n 0 (x), 0) → h(I d , 0). Thus there exists a rank n such that ∀k n, q k 0 (x) ∈ U N (otherwise we could extract a subsequence from (q n 0 (x)) n 0 which stays at a distance > α 2 from K, as G is compact we could extract again a convergent subsequence and its limit q ∞ 0 (x) would be outside K, thus we would have h(q n 0 (x)) → h(I d , 0)). Here we can define as in the proof of Theorem 11 the sets D n = {x ∈ C, ∀k n, q k 0 (x) ∈ U N 1 } (note that U 1 has been replaced by U N 1 in this definition of D n ). As for almost any x ∈ G there exists a rank n such that x ∈ D n we have µ G (D n ) → µ G (C) and as in the proof of Theorem 11 the sets U n eventually cover the set C.

The second part of the property (invariance to left multiplication) is easier. Consider the error process ζ n = Υη n . The propagation and update steps read:

ζ n+1 = Υη n+1 = ΥϒW n η + n ϒ -1 = ϒ ΥW n η + n ϒ -1 L = ϒW n Υη + n ϒ -1 L = ϒW n ζ + n ϒ -1 ζ + n+1 = Υη n+1 K n+1 (h(η n+1 ,V n+1 )) -1 L = Υη n+1 K n+1 (h( Υη n+1 ,V n+1 )) -1 L = ζ n+1 K n+1 (h(ζ n+1 ,V n+1 )) -1
where we have used the property:

h(η n+1 ,V n+1 ) L = η -1 n+1 h(I d ,V n+1 ) L = η -1 n+1 h( Υ,V n+1 ) L = h( Υη n+1 ,V n+1 )
We see that the law of the error process is invariant under left multiplication by Υ, thus the asymptotic distribution π inherits this property.

B.2 Proofs of the results of Section 6.4.2

B.2.1 Proof of Proposition 18

We define the continuous process γt : R → G as follows:

Title

Abstract: The present thesis explores the use of non-linear state errors to devise extended Kalman filters (EKFs). First we depart from the theory of invariant observers on Lie groups and propose a more general yet simpler framework allowing to obtain non-linear error variables having the novel unexpected property of being governed by a (partially) linear differential equation. This result is leveraged to ensure local stability of the invariant EKF (IEKF) under standard observability assumptions, when extended to this class of (non-invariant) systems. Real applications to some industrial problems in partnership with the company SAGEM illustrate the remarkable performance gap over the conventional EKF. A second route we investigate is to turn the noise on and consider the invariant errors as stochastic processes. Convergence in law of the error to a fixed probability distribution, independent of the initialization, is obtained if the error with noise turned off is globally convergent, which in turn allows to assess gains in advance that minimize the error's asymptotic dispersion. The last route consists in stepping back a little and exploring general EKFs (beyond the Lie group case) relying on a non-linear state error. Novel mathematical (global) properties are derived. In particular, these methods are shown to remedy the famous problem of false observability created by the EKF if applied to simultaneous localization and mapping (SLAM), which is a novel result.

Keywords: Non-linear estimation, extended Kalman filtering, Navigation, SLAM, Geometric methods

Lemma 2 .

 2 Consider the system (3.13) and let χt denote a particular solution. Consider the condition ∀a, b ∈ G, g u (ab) = ag u (b) + g u (a)b. (3.14)

3

 3 are two random noises and b 1 , b 2 are two vectors known in the fixed reference frame, for instance the gravity vector and earth magnetic field observed in the vehicle frame by an accelerometer. Here, the space Y is R 6 , the action of G on Y is defined by R • y 1 y 2 = Ry 1 Ry 2 and the observation is of the right-invariant kind.

Figure 5 . 1 -

 51 Figure 5.1 -Static alignment of an IMU planted in a car at stand with engine running.The blue line is the heading estimate of the IEKF, the green line the estimate of the commercial software. The vertical line is a consequence of the three-steps procedure: although already computed during the coarse alignment phase, the heading estimate is displayed here only from the beginning of the fine alignment. The transition from a memorized value creates this artifact. We see that the IEKF converges to the same value as the classical method although starting very far (120 degrees) from the true value.

Figure 5 . 3 -

 53 Figure 5.3 -In-flight alignment: comparison of an EKF and IEKF with same parameters. They are equivalent for small initial errors, but the convergence of the EKF deteriorates much faster with the increase of the initial error. It makes it useless during the transitory phase, to the contrary of the IEKF.

Figure 5 . 4 -

 54 Figure 5.4 -Simultaneous alignment of an IMU and estimation of some physical parameters. A set of 1000 random values has been sampled and corresponding increments generated by a proven simulator. The 3σ envelopes of the errors on the physical parameters are displayed over time, together with the true errors for the 1000 simulations. We see the filter gives consistent estimates although the initial attitude errors are possibly large.

  Assume there exists a C 2 function E : G → R ≥0 with bounded sublevel sets, a global minimum at I d , and a continuous and strictly positive function u : G → R >0 such that: ∀x ∈ G, k(x) = u(x)[x -1 .grad E (x)]. If the condition ∀x ∈ G, | ∂ k ∂ x | 1 (for the operator norm associated to the left-invariant metric) is verified, for any initial value γ 0 such that I d is the only critical point of E in the sublevel set {x ∈ G | E(x) ≤ E(γ 0 )} we have:

Figure 6 . 1 -

 61 Figure 6.1 -Artificial horizon example. Top plot: off-line optimization of the parameters of the asymptotic invariant filter (axis x → λ ; axis y → k ; axis z → Root Mean Square Error).The highest values are not displayed to improve visualization. The performance is optimal for k = 0.1202, λ = 0.0029. Bottom plot: error evolution of three artificial horizons : the invariant filter with optimized gain functions (black), a MEKF ignoring outliers (red), and a MEKF where the observation noise covariance matrix has been numerically adjusted to minimize the RMSE (dashed line). We see that the MEKF cannot filter the outliers and at the same time be efficient in the absence of outliers, contrarily to the proposed invariant filter.

Figure 6 . 2 -

 62 Figure 6.2 -Evolution over time of the coefficients of the gain matrix for MEKF, IEKF, USQUE and IEnKF. The coefficients of the MEKF and USQUE have an erratic evolution, whereas those of the IEKF are convergent, allowing to save much computation power using asymptotic values. Moreover, in this case, only 4 of the 18 coefficients are not equal to zero for the IEKF, allowing sparse implementation.

. 1 )Proposition 20 .

 120 where f verifies relation(3.13) and b is an unknown element of the Lie algebra g. A nice separation principle is verified: Let b ∈ B denote a particular value of b and χt a corresponding particular solution of Equation (7.1). If (χ t , b) is another couple of solutions, η L t = χ -1 t χt the left-invariant error of the χ component and ∆b = b -b the linear error of the b component we have:

Figure 8 . 3 -

 83 Figure 8.3 -Basic mechanism of false observability: propagation step. The estimate changes, but the Riccati equation propagates the covariance matrix so that the nonobservable directions are mapped to non-observable directions. No false observability is created here, unlike in the update step described on Figure 8.4.

Figure 8 . 4 -

 84 Figure 8.4 -Basic mechanism of false observability: update step. The estimate moves to X+t n+1 and the covariance is updated. But all the computations performed to update P n+1 rely on a linearization at Xt n+1 . Thus, the filter cannot "guess" the non-observable direction around X+ t n+1 . This is the moment where false observability is created.

  χ+

  t n , χt n ) = (De χ+ tn ) -1 De χtn . (8.15)

Remark 16 .

 16 The nonlinear error ε( χ, χ) induces a basis of dimension n about any point χ given by the columns of the Jacobian matrix Dε( χ, .)| χ -1 . The non-linear error is expressed in this coordinate system in the sense that up to second order terms we have δ ε := ε( χ, χ + δ x) = 0 + Dε( χ, .)| χ δ x and thus δ x = Dε( χ, .)| χ

Figure 9 . 1 -

 91 Figure 9.1 -The relative observations correlate the heading of the car with the position of the feature. If the heading is updated due to new information, the position of the feature is updated in a coherent way up to the first order.

Figure 9 . 2 -

 92 Figure 9.2 -If the update is large, implicit and first-order updates give different results. But none of them moves the feature along a circle as would be expected.

Figure 9 . 3 -

 93 Figure 9.3 -If the update takes into account only an half of the error but is performed twice sequentially, the results of the two update methods come closer to each other, and closer to the circle.

Figure 9 . 4 -

 94 Figure 9.4 -If the error is divided by n instead of 2, the update functions ψ become equivalent when n becomes large. The limit value is equal to the exponential update.

Figure

  Figure 9.5 -We consider here a car making relative range and bearing observations of a feature. After a few observations, the position of the feature is known in the reference frame of the car. Thus, if the heading is updated then the estimate of the feature should follow a circle around the car. In the case of SLAM, only the exponential update has this behavior

Figure 9 . 7 -

 97 Figure 9.7 -Coefficient of the information matrix corresponding to heading for classical (left plot) and modified (right plot) error equation. Theorem 18 is illustrated: the information decreases over time as expected if the proper non-linear error variable is chosen.

  dx = (e + 2 ; e + 3 ) T , d p = (e + 4 ; e + 5 ) T θ + i = θi + dθx+ i = xi + dx p+ i = pi + d p P + i = (I -KH)P i end if end forHeading measurement and Linear update: the estimated features move tangentially to a circle centered on the car. But they follow straight lines and the distances are modified.Heading measurement exponential heading update: the estimated features follow the circle centered on the car and distances are preserved.

Figure 9 . 8 -

 98 Figure 9.8 -Preservation of local maps by an EKF SLAM algorithm based on error variable(8.10), depending on the update method used. A static car (black cross) maps its environment with no information about its heading. Four features, arranged as a square around the car, have been observed a large number of times. Thus, their positions are known up to the heading of the car to which they are highly correlated. We assume here this correlation is correctly encoded by the covariance matrix P t . The blue circles indicate the estimated positions of the features. Then, an accurate heading observation is provided to the system, producing a large counter-clockwise variation of the estimated heading. Because of the correlation encoded by P t between heading and features positions, those move in a way coherent with the heading update, i.e., they "turn" with the car. In other words, the car has accurate information in its own reference frame, which has no reason to be modified by a heading update: the estimated local map should turn as information is obtained regarding the general orientation of the system. On the left plot, the red circles are the new features estimates if a first-order update is used. They turned counter-clockwise as wanted, but only up to the first order. Then they followed straight lines, tangentially to the circle centered on the car. As a consequence, the estimated map underwent a more complicated transformation than the simple rotation we expected. On the right plot, the red circles are the new features estimates if an exponential update is used. The result is very different: the transformation is this time a true rotation. The positions of the estimates in the estimated reference frame of the car are the same as before the update, only the general orientation has changed. This property is the preservation of a local map mentioned at the end of Section 9.4. Note that a classical EKF encounters in this situation the exact same problem as the EKF using non-linear error variable but first-order update.

8 .Figure 10 . 2 -

 8102 Figure 10.2 -Result of an EKF tracking the car, with linear and non-linear error variables. The true trajectory of the car is a straight line (red), going from the bottom to the top of the left plot. The initial position estimates are the true initial position, but the headings are directed to the left instead of the top (90 degrees initial error). We see the non-linear error variable improves the results, especially regarding the heading estimation, although the exponential update recommended in Chapter 9 is not yet used on this figure.

Figure 10 . 3 -

 103 Figure 10.3 -Result of an EKF tracking the car, with non-linear error variable, depending if the exponential update recommended in Chapter 9 is used or not. The true trajectory of the car is a straight line (red), going from the bottom to the top of the left plot. The initial position estimates are the true initial position, but the headings are directed to the left instead of the top (90 degrees initial error). We see the introduction of the exponential update here brings a new dramatic improvement of the performance.

Proposition 30 .

 30 and 10.5 illustrate this result: the distance between the IEKF estimate and the starting point is the same as for the true state, and the estimate has always the starting point exactly in its back, as the true space has. Proposition 30 below gives a mathematical sense to these considerations. Its proof relies on Theorem 19 of Chapter 9. Let b t be defined by the differential equation b

Figure 10 . 4 -

 104 Figure 10.4 -Illustration of Proposition 30. The true trajectory of the car is a straight line (red), going from the bottom to the top of each plot. The estimates over time of several filters are displayed. Their initial position is the true one, but their heading is directed to the left (90 degrees initial error). Between two updates, the position is propagated using the odometer, which indicates straight line. Each time an update is performed, the estimate jumps. The initial position error covariance matrix is zero: the filters know exactly their initial position. Top left and bottom left: position of the different estimates over time.As the odometer indicates straight line and the initial position is known, the estimated heading and (thus) the propagation of the estimate should always be radial with respect to the starting point. The same plots are reproduced on the top and bottom right, but for the estimate of the EKF using non-linear error variable and exponential update, the trajectory of the estimate between consecutive updates has been extended (black lines) to show it always reaches the starting point. This is obviously not verified by the other filters.

Figure 10 . 5 -

 105 Figure10.5 -Illustration of Illustration of Proposition 30. Difference between the distance of each estimate to the starting point, and the odometric distance. As the odometer is not noisy it should always be zero. We see that the IEKF ensures this property on the whole estimation process but the other filters do only until the first update.

Figure 10 . 6 -

 106 Figure10.6 -Estimates and uncertainty ellipsoids for the three filters considered in this chapter. The circle is the set of possible states given the measures of the odometer. Up to the small uncertainty over initial position we added for this example, the true state has to be on this circle. Left plot: before the first update all the filters give the same results. Right plot: after the first update, both classical EKF (blue) and EKF using the right non-linear error variable (purple) but not the exponential update have left the circle. The EKF having non-linear error variable keeps an uncertainty ellipsoid consistent with its new position, to the opposite of the classical EKF which put itself in a very bad situation. The EKF with proper non-linear error variable and exponential update (IEKF) sticks naturally to the circle and moves its uncertainty ellipsoid in accordance with the update of the state.

  [START_REF] Bonnabel | Symmetry-preserving observers. Automatic Control[END_REF]) is defined by exp(ξ) exp(L n (e ξ bb -Hξ )) = exp((I -L n H)ξ + r t n (ξ )). The Baker-Campbell-Hausdorff (BCH) formula gives r t n (ξ ) = O(||ξ ||.||L n ξ ||) but L n isuniformly bounded over time by γ max ||H|| δ 3 as an operator. Thus ||r n || is uniformly dominated over time by a second order: there exists a continuous function lk (depending only on k and on the true trajectory) such that lk (x) = O(x

4 . 1 .

 41 Let α such as B o (I d , α ) ⊂ ϒB o (I d , α 2 )ϒ -1 and ε = ε |Ad ϒ -1 |ε, where |Ad ϒ -1 |is the determinant of the adjoint operator on g. For any obtain Q ∞ = K. As K is compact (as a closed subset of a compact) the open cover ∪ ∞

  ∀n ∈ N, γn = γ n ∀n ∈ N, ∀t ∈ [n, n + 1[, d dt γt = -γt k( γn ) Titre Résumé : Cette thèse étudie l'utilisation de variables d'erreurs non linéaires dans la conception de filtres de Kalman étendus (EKF). La théorie des observateurs invariants sur les groupes de Lie sert de point de départ au développement d'un cadre plus général mais aussi plus simple, fournissant des variables d'erreur non linéaires assurant la propriété nouvelle et surprenante de suivre une équation différentielle (partiellement) linéaire. Ce résultat est mis à profit pour prouver, sous des hypothèses naturelles d'observabilité, la stabilité de l'EKF invariant (IEKF) une fois adapapté à la classe de systèmes (non-invariants) introduite. Le gain de performance remarquable par rapport à l'EKF classique est illustré par des applications à des problèmes industriels réels, réalisées en partenariat avec l'entreprise SAGEM. Dans une seconde approche, les variables d'erreurs sont étudiées en tant que processus stochastiques. Pour les observateurs convergeant globalement si les bruits sont ignorés, on montre que les ajouter conduit la variable d'erreur à converger en loi vers une distribution limite indépendante de l'initialisation. Ceci permet de choisir des gains à l'avance en optimisant la densité asymptotique. La dernière approche adoptée consiste à prendre un peu de recul vis-àvis des groupes de Lie, et à étudier les EKF utilisant des variables d'erreur non linéaires de façon générale. Des propriété globales nouvelles sont obtenues. En particulier, on montre que ces méthodes permettent de résoudre le célèbre problème de fausse observabilité créé par l'EKF s'il est appliqué aux questions de localisation et cartographie simultanées (SLAM).

Mots clés :

  Estimation non linéaire, Filtrage de Kalman étendu, Navigation, Localisation et Cartographie Simultanées, Méthodes géométriques.

  The evolution of e t reads now:

	d dt	e

t = -ω t e t + e t ω t , e + t n = e t n K n

  d dh Φ t (ψ h (Id)) = DΦ t | Id ξ 0 and d dh g u s (Φ s (ψ h (Id))) = Dg u s DΦ s | Id ξ 0 we get the final desired result d dt DΦ t | Id = Dg u t DΦ t | Id , letting F t be equal to DΦ t | Id .

  The matrix function f ω t ,u t is neither left or right invariant, nor a combination of both, but verifies condition (3.8). The demonstration is straightforward.

	IEKF equations for the left-invariant output (4.42)

.46) Proposition 14.

  )where W n is a random variable with values in G and whose law depends on the values taken by υ t for t ∈ [t n ,t n+1 ] and on the law of w t for t ∈ [t n ,t n+1 ], and ϒ n and Ω n are elements of G which only depend on the values taken respectively by υ t and ω t for t ∈ [t n ,t n+1 ].

  the two processes have the same law at time t n+1 , i.e. χ n+1 and W t n+1 ϒ t n+1 χ t n Ω t n+1 have the same law. Letting ϒ n = ϒ t n+1 , Ω n = Ω t n+1 and W n = W t n+1 we obtain the result. Note that, the variable W t depends also only on the law of w t for t ∈ [t n ,t n+1 ].

	Remark 9. In many practical situations (for instance examples 6.2.2 and 6.2.2), the
	Langevin noise w t is isotropic and we have thus ϒ -1 t w t ϒ t

L = w t .

Table 6 .

 6 1 -Artificial horizon: experiment parameters Standard Deviation of the model noise 0.01 • = 1.75 • 10 -4 rad Standard Deviation of the regular observations 0.1 • = 1.75 • 10 -3 rad

	Standard Deviation of the outliers	30 •
	Probability of the outliers to occur	0.01

  1 t ) t≥0 and (Θ 2 t ) t≥0 of (7.2) and let two Invariant Extended Kalman Filters run in parallel to filter the subsystem (7.3)-(7.4) where Θ t is considered as an input.

	Proposition 21. Let (Θ 1 t ) t≥0 and (Θ 2 t ) t≥0 be two solutions of (7.2). The equations
	(7.3)-(7.4) define then two different systems, one associated to Θ 1 t and the other to
	Θ 2

t . If these systems are filtered by two IEKFs ( χi t ) i=1,2 of the form:

Table 7 .

 7

	Frequency of the GPS observations	10Hz
	Variance of the regular GPS observations	1m 2
	Variance of the GPS bias	100m 2

1 -Parameters of the simulation Duration 1000s Variance of the gyroscope noise 10 -8 rad/s 2 Variance of the accelerometer noise 10 -6 m 2 /s 5 Probability of a GPS bias jump 10 -3 s -1

Table 7 .

 7 2 -Results of the simulation : RMSE of the error

	Number of particles	1	10	30	100	1000
	RMSE (m)	105,62 96,31 75,73 70,37 66,59
	Computation time (s)	1,29	2,61	5,64 15,59 150,52

  .2) Note that we used notations letting appear the dependency in the estimate χt of matrices A e χ , A e w , H e χ , H V χ . As previously, one can use the linear Kalman theory to derive an estimate ê+ t n of the error as follows. Assuming an uncertainty matrix P t n-1 of e t at time t n-1 is available:

	Propagation:
	d dt	P t = A e χ ( χt )P t + P t A e χ ( χt ) T + A e w ( χt )Q n A e w ( χt ) T ,	t n-1 < t < t n ,	(9.3)
	and P t represents an approximation to the covariance of the error variable according
	to the Kalman filter.
	Update:
		S n = H e χ ( χt n )P t n H e χ ( χt n

  The innovation term z n is a function of the error e t n and the observation noise V n .It defines the matrices H en and H V n through the first-order expansion z n = H e n e n + H V V n . If a different innovation term is defined as z n = L n z n with L n being any invertible matrix possibly depending on the estimate Xt n and having the dimension of z n , matrices

	H e n and H V n become H e n = L n H e n and H V n = L n H V n . The covariance matrix of z n becomes S n = H e n P n H e n T + H V n RH V n T = L n S n L T n with S n the covariance matrix of z n . The new gains
	are K n

  t Jb t -v t 0 + u t v t R(θ t )J b t + R(θ t )

					v t 0	=	0 0	,
	where J =	0 -1 1 0	. This means that, due to deterministic dynamics, this constraint on
	the initial state propagates over time to become R(θ t )b t +	x 1 t t x 2	= 0. We denote now this
	known function by ht (θ t , x 1 t , x 2 t ) = R(θ t )b t +	x 1 t t x 2	and show the two following properties are
	verified for any time t > 0:

  dt P(t) = A t P(t) + P(t)A t + Q 1 + Ad χ -1 Q 2 Ad T χ -1 P+ (t) = P(t) -P(t)H T H P(t)H T + N 1 + χ -1 N 2 χ -T -1 H P(t)This formulation allows to cover both cases of left and right observations. The Riccati equation computed on the estimated trajectory is obtained replacing χ t with χt . Recalling the error η t and the properties of the Ad, the idea of the proof is simply to rewrite the Riccati equation computed about χt as a perturbation of the Riccati equation computed about χ t :d dt P(t) = A t P(t) + P(t)A t + Q 1 + Ad η -1 Ad χ -1 Q 2 Ad T χ -1 Ad T

	η -1

P + (t) = P(t) -P(t)H T HP(t)H

  2 ) and ||r t n (ξ )|| ≤ lk (||ξ ||) for any n such that t n ≤ 2M∆T . Now we can control the evolution of the error using lk . The propagation step is linear, thus we have the classical result d dt V t (ξ t ) < 0. It ensures ||ξ t+s || < γ max γ min ||ξ t || as long as there is no update on [t,t + s]. At each update step we have V + t n (ξ + t n ) 1/2 = V + t n [I -L n H]ξ t n +

							2
	As 1-λ 2 2-1 λ 2	-→ λ → 1 2 √	-+∞ there exists λ 0 such that: M	θ u	≥ α(2 -1 λ 2 0	)	θ u	and the result is
	true for β 1 = α(2 -1 λ 2 0

ζ T P -1 t ζ dζ ,(4.16)where[ζ , ζ + dζ ] is a volume element.

Acknowledgements

Most people involved in this work being French-speaking, I'm shifting to this language in the present section.

Ces travaux de thèse n'auraient pas rencontré le même succès sans l'implication de mon directeur de thèse Silvère Bonnabel. Peu de doctorants ont eu comme moi la possibilité de discuter à tout moment avec leur encadrant, ni bénéficié d'une aide aussi conséquente pour rédiger leurs publications.

Mon responsable au sein de Sagem, Xavier Bissuel, a également joué un rôle essentiel dans ces recherches. Son expérience a été précieuse pour mettre en relation la théorie développée dans ce document avec les applications industrielles les plus prometteuses.

Emmanuel Robert et son équipe, en particulier Thierry Perrot et Olivier Guitton, m'ont fourni une aide précieuse dans la mise en oeuvre de mes algorithmes sur des produits commerciaux. Thierry Perrot a de plus contribué à la rédaction de ce document par ses relectures attentives.

Le stagiaire que j'ai eu la joie d'encadrer, Thomas Bernal, a également fournit un travail remarquable qui m'a fait économiser un temps précieux.

La collaboration entre Sagem et l'Ecole des Mines sur les techniques de filtrage non linéaire, sans laquelle cette thèse n'aurait pas eu lieu, a été initiée par MM. Erwan Salaun, Daniel Duclos, Yann Le Guilloux, Xavier Bissuel et Emmanuel Robert.

Je tiens aussi à remercier Sébastien Marcade et Loic Davain pour avoir proposé d'appliquer les méthodes de navigation que j'ai conçues aux projets dont ils étaient en charge. Ce sont eux qui rendu possible la transition d'un travail purement théorique vers une mise en oeuvre concrète profitable d'un point de vue industriel.

Enfin, je suis reconnaissant à toutes les personnes ayant fréquenté le centre de robotique de l'Ecole des Mines de Paris, en particulier le bureau V2O (Manu Alibay, Florent

Algorithm 1 Invariant Extended Kalman Filter

Returns at each time step χn , P n such that χ n ≈ exp(ξ n ) χn , where ξ n is Gaussian and var(ξ n ) = P n . χ0 and P 0 are inputs. H ξ , H V defined by:

Set n = 0. loop Compute the value M t n+1 solving the equation

The process noise covariance Q w n = Var(Ad ϒ n w n ) is equal to M t n+1 if w n is isotropic or if υ t ≡ 0, merely set

We obtained in Section 7.4.2 that P(φ -1 (χ -1 t n χt n ) = ξ | bn , Ỹn ) ≈ N (ξ , P t n , 0). We can extract the relation P(x t n = x| b j n , Ỹn ) ≈ N (x, R j t n HP t n H T R jT t n , x j t ) that will prove useful in the sequel.

Computation of P(Y n | b j n-1 , Ỹn-1 ) Let J n be the event "the bias jumps between t n and t n+1 ". This event is independent of χ t n+1 . We let p J denote its prior probability, and J its opposite. If J occurs, according to (7.13), Y n is the sum of three independent Gaussian vectors: x t n , b t n and V n . Thus

t n H T P t n HR t n + σ 2 b I 3 + r 2 I 3 , x j t n ). If J occurs, Y n is the sum of two independent Gaussian, x t n and V n , and a known vector b t n . Thus P(Y n | J, b j n-1 , Ỹn-1 ) = N (Y n , R T t n H T P t n HR t n + r 2 I 3 , x j t n + b j t n ). We can conclude introducing the quantities Π 1 and Π 2 :

t n H T P t n HR t n + r 2 I 3 ,

Computation of

). If J occurs using the fact b n is a Gaussian and Y nx j n a noisy measurement of it, the linear Kalman equations yield:

If J occurs the bias does not change :

To sample from this law (see Algorithm 4) we first choose if the bias jumps (for instance comparing Π 1 to a uniform variable). Then, if it is the case, we use a Cholesky decomposition

where X is sampled following a standard three dimensional normal law. Note that a direct Cholesky decomposition of the variance R jT t n L b n R t n would be a bad choice as the computation would have to be done for each particle.

Algorithm 4 Invariant Rao -Blackwellization

The prior estimation χ0 and variance P 0 are supposed known.

Update the weight:

Remedying EKF-SLAM inconsistency

Chapter abstract In this chapter, we first recall one of the main reasons why, although very well suited to loop closures and as such very appealing, the EKF SLAM has been essentially abandoned. It is known as "EKF-SLAM inconsistency" [START_REF] Bailey | Consistency of the ekf-slam algorithm[END_REF][START_REF] José A Castellanos | Limits to the consistency of ekf-based slam 1[END_REF][START_REF] Huang | Convergence and consistency analysis for extended kalman filter based slam[END_REF][START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF], that is, the over-optimism of the EKF covariance matrix due to linearizations. Then, we show the problem can be fully solved by choosing to linearize a certain nonlinear state error variable inspired by Chapter 4. To our best knowledge, this result is entirely novel.

Introduction

The problem of a robot discovering a new environment, and trying to map it while estimating its own position on the map, is known as "Simultaneous Localization and Mapping". The probabilistic formulation of this problem introduced in [START_REF] Smith | Estimating uncertain spatial relationships in robotics[END_REF], and its resolution using Extended Kalman filtering recommended by the authors of the paper, has been a great success. This is due to the impossibility to neglect the correlations between the different variables [START_REF] José A Castellanos | Building a global map of the environment of a mobile robot: The importance of correlations[END_REF]. They have to be encoded one way or another and to this end, the Kalman filtering framework is difficult to avoid. Yet, further work [START_REF] Bailey | Consistency of the ekf-slam algorithm[END_REF][START_REF] José A Castellanos | Limits to the consistency of ekf-based slam 1[END_REF][START_REF] Huang | Convergence and consistency analysis for extended kalman filter based slam[END_REF][START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF] showed the limits of the EKF SLAM: the non-linearity of the problem makes the filter inconsistent, in the sense that the error covariance matrix it provides is extremely optimistic, or in other words the estimation error is far beyond its (alleged) the 3-σ envelope. This has been related to a problem known as "false observability": the filter believes it has enough information to improve its estimate of some variables being actually non-observable. For instance, a robot using only relative measurements of unknown landmarks eventually considers its heading is known whereas an overall rotation of the system yields the entire observations sequence unchanged ans thus cannot be distinguished. To circumvent this drawback some work proposed to chose a linearization point forcing the non-observable subspace of the system to keep the right dimension [START_REF] Guoquan | Analysis and improvement of the consistency of extended kalman filter based slam[END_REF][START_REF] Guoquan | Observability-based rules for designing consistent ekf slam estimators[END_REF]. But in general, EKF was mostly abandoned in favor of two kinds of approaches: batch optimization and particle methods deriving from FastSlam. FastSLAM [START_REF] Montemerlo | Fastslam: A factored solution to the simultaneous localization and mapping problem[END_REF] is a sampling-based estimation method that carries a set of hypothetical trajectories along with weights. It performs data association jointly with

Equivalence of the two approaches

We provided two ways to solve the problem of false observability of global translations and rotations in the SLAM problem. The second one is the most intuitive but could look artificial. The first one inspires more confidence as the procedure is exactly the same as in the classical EKF, only the definition of the error changes, and the problem being non-linear there is no reason why the linear error χ t -χt should prevail, on the contrary we advocate throughout the present thesis that it may be an inappropriate choice. But the physical sense of changing the error variable is not necessarily obvious. The answer is Proposition 26: choosing an error variable means precisely choosing a way to make the confidence ellipsoids "turn". Proposition 26. Solutions proposed in Sections 8.4 and 8.5 are equivalent in the following sense:

Let us denote by Qn , Pn and (Q n , P n ) the covariance matrices appearing in Equation (8.11) and (8.12) respectively, and by De χ the Jacobian matrix of the non-linear error defined by the first-order expansion:

If Q n and Qn are coherent regarding the definition of the non-linear error e n , i.e., linked by the relation:

then the solutions of Riccati equations (8.11) and (8.12) are always linked by the relations:

Proof. While no update is performed the equivalence is obvious: the same differential equation is simply written in two different bases. Consider the update step. The Jacobian matrices corresponding to the linear and non-linear variable are denoted respectively by H n and Hn . We write the update of P n and Pn in the information form:

We want to show that if we have Pn = De χn P n De χn T before the update then we want to obtain after the update:

Theorem 18. Consider a non-linear error e t and the matrices A e χ (χ) and H e χ (χ) defined by (9.1) and (9.2). Assume that a linear subspace

• Any EKF based on the error e t , and with state error covariance matrix P t , is such that the information along those directions always decreases i.e., for any ξ ∈ V if we let ξ 0 = ξ , d dt ξ t = A e χ ξ t the information along this direction ξ T t (P t ) -1 ξ t monotonically decreases.

Proof. The result is trivial: if a vector space V is in ker H e χ for any χ and stable by F e χ for any χ then it is true in particular on the estimated trajectory and V is a non-observable subspace of the linearized system and Proposition 22 can be applied.

The EKF defined in this section has been applied to the problem of a car navigating using range-and-bearing measurements (Algorithm 6), and compared to the classical EKF (Algorithm 5). The latter is known to fail even in this convenient situation [START_REF] Simon | A counter example to the theory of simultaneous localization and map building[END_REF], which is confirmed by the results displayed on Figure 9.6 where the uncertainty ellipsoids are over-optimistic. The figure also shows that the EKF with modified error variable does not encounter the same problem as could be expected from the properties previously derived. Figure 9.7 is also interesting, as it precisely illustrates the results of this chapter. The information over the non-observable direction (rotation of the full system) is displayed and the difference between the two filters is striking: this quantity is decreasing only if the proper error variable is chosen.

Algorithm 6 EKF with error variable

ω and Q v are known parameters

Note that these properties are verified in the linear case: a Kalman filter runs perfectly with singular covariance matrix, the estimate being confined inside a subspace of the state space. They also turn out to be verified in some non-linear situations, but require the use of an exponential update as we are about to prove. Theorem 19. Assume we have built an EKF based on a non linear error ε(., .) through the equations (9.3), (9.4), (9.5), such that for a given function h and a time t n the two following relations are verified:

where He is defined by (9.7). Then, if any new measurement is processed through an EKF update (9.4), (9.5) where ψ is exponential (see Definition (13)), the same relations hold after the update:

Proof. Property 1 is verified as follows. For χ ∈ R n , as P t n is symmetric, He χ P t n H e χ = 0 implies He χ P t n = 0. Thus:

Property 2 is obtained using simply definition 13. With the same notations we have:

An immediate implication is preservation of a local map. Assume the software of the car has been able to build a very accurate local map in the reference frame of the car, when a large heading update is applied (due to loop closure for example). A desirable property of the EKF SLAM would be the local map to stay unchanged in the frame of the car, and the non-zero directions of its covariance P t to be still aligned with the level sets of the local map. To show this is verified for the EKF SLAM with error variable (8.10), all we need is to check according to Theorem 19 that the "Jacobian" of the function h(θ , X, p) = R(θ ) T (p -X) w.r.t. the error variable e has the same kernel on any point of the state space. We have:

with (e θ , e p , e X ) the components of e (we recall the first-order expansions e p = δ p -J pδ θ and e X = δ X -J Xδ θ ). Thus H e χ = R( θ ) T [0 2,1 , -I 2 , I 2 ] has a kernel independent from χ. Plot 9.8 displays the result of a heading update after acquisition of a local map, for an EKF SLAM based on a linear error variable and on error (8.10).

• ht (θ t , x 1 t , x 2 t ) = (0, 0) T .

• He t P t ( He t ) T = 0, where He t is defined by the first-order expansion:

e t being the error variable of the IEKF.

The proof is in two steps: propagation and update. But first, we need the expression of He t . We have:

Thus:

. we compute each term:

We we conclude this study plotting the position estimates and 50% uncertainty ellipsoids at the moment of the first update for all the three filters (Figure 10.6). To avoid having totally flat ellipsoids we added a small uncertainty on the initial position. The result is a mere confirmation of the results of this chapter but gives a good visual interpretation of the benefits of IEKF. On both plots a circle has been drawn, centered on the initial position and having the traveled distance (given by the odometer) as radius. The left plot is obtained just before the first update. All the filters give the exact same estimate Appendix A

Proofs of the results of Chapter 4 A.1 Proof of theorem 7 A.1.1 Review of existing linear results

Consider a linear time-varying Kalman filter and let Ψ t t 0 denote the flow of the error variable ξ s . It is proved in [START_REF] Deyst | Conditions for asymptotic stability of the discrete minimumvariance linear estimator[END_REF] that if the parameters of the Riccati equation verify conditions (1) -( 5) then there exist γ max > 0 and γ min > 0 such that γ max I P t γ min I. This pivotal property allows to prove the solution of the linear error equation Ψ t s ξ s verifies for V (P, ξ ) = ξ T P -1 ξ :

where β 3 only depends on α 1 , α 2 , β 1 , β 2 , δ 1 , δ 2 , δ 3 , N. Of course, the proof given in [START_REF] Deyst | Conditions for asymptotic stability of the discrete minimumvariance linear estimator[END_REF] holds if the inequalities are only verified on an interval [0, T ]. We will also use the direct consequence:

A.1.2 Preliminary lemmas

The proof of Theorem 7 is displayed in the next subsection. It relies on the final Lemma 8, which is proved step by step in this section through lemmas 5, 6 and 7. The time interval between two successive observations will be denoted ∆t. P(t) will denote the Kalman covariance about the true state trajectory.

Lemma 5. [modified constants for closeby trajectories]

If the conditions (1) to (5) are satisfied about the true trajectory, then for any k > 1 there exists a radius ε such that the bound ∀s ∈ [0,t], ||ξ t 0 +s || < ε ensures the conditions (1) to (5) are also verified about the estimated trajectory, with the modified constants

Proof. The result stems from the decomposition (4.19) as:

(triangular ineq.)

As we have ( lk

, we obtain the result squaring the inequality and using V t 0 +s Ψ t 0 +s t 0 ξ t 0 +s ≤ V t 0 ξ t 0 ≤ γ max γ min ||ξ t 0 || to control the crossed terms.

Lemma 8.

[final second order growth control] Under the same conditions as in Lemma 5 (including 1 k Pt 0 ≤ P t 0 ≤ k Pt 0 ) and for ||ξ t 0 +s || bounded by the same ε for s ∈ [0,t], there exist two functions l k

) and a constant β k ensuring the relation:

where n max is the last update before t 0 +s ( i.e. n max = max{n,t n ≤ t 0 +s}), J is the maximum number of successive sequences of M updates in [t 0 + M∆t,t n max ] (i.e. J = max{ j,t n max -jM ≥ t 0 } -1) and n 0 = n max -JM. If t 0 + s = t n max the last term can be removed.

Proof. For l k 1 we choose the same function as in Lemma 7. There is nothing more to prove for s < 2M∆t. Let s ≥ 2M∆t. We have V t 0 +s (ξ t 0 +s ) -V + t 0 (ξ + t 0 ) = V t 0 +s (ξ t 0 +s ) -

. The first and third terms are upper bounded using Lemma 7. The second term is controlled as follows:

And we conclude using (see [START_REF] Deyst | Conditions for asymptotic stability of the discrete minimumvariance linear estimator[END_REF]):

for a β k depending only on the modified constants of Lemma 5. The last inequality is obtained using Ψ t n + t 0 = (P + n P -1 n )Ψ t n t 0 and an obvious recursion over M time steps. We finally set β = β k ( γ min γ max δ 1 ) M . Remark 19. The control we have obtained on ξ t 0 +s is verified if ||ξ t 0 +s || is already in a ball of radius ε over the whole interval [t 0 ,t 0 +t]. We now prove the result holds assuming only that ξ t 0 is sufficiently small.

A.1.3 Proof of theorem 7

Applying Lemma 8 with t 0 + s = t n max gives for 1 k Pt 0 ≤ P t 0 ≤ k Pt 0 and ||ξ t 0 +s || < ε on [0,t] :

There exist K > 0 and ε > 0 such that for x < ε , we have

which finally ensures

A.2 Proof of proposition 8

Only conditions (1) and ( 5) are non-trivial. Let Φ denote the flow of the dynamics. We

and ( 1) is verified. The difficult part of ( 5) is the lower bound. Denoting Cov(V n ) by N we will show:

That is to say that we want a lower bound on the quadratic form:

We decompose Φ

To simplify the writing we introduce the norms ||x|| 2 N = x T N -1 x and the associated scalar product < ., . > N . There exists α > 0 such that ∀x ∈ R 2 , ||x|| N ≥ α||x||. For any

N and for λ ∈ ]0, 1] we have: We first introduce here the basic notions about Harris Chains that we will need, to prove the stochastic convergence properties of the invariant filters. 3. There exists an integer N 0 such that: ∀x ∈ A, ∀n N, P(X n ∈ A|X 0 = x) > 0.

The somewhat technical property 2 means that any given area of B can be reached from each point of A with non-vanishing probability.

Theorem 21. [Harris, 1956] A recurrent aperiodic Harris chain admits a unique stationary distribution ρ ∞ and the density of the state X n converges to ρ ∞ in T.V. norm for any distribution of X 0 .

Proof. See [START_REF] Durrett | Probability: theory and examples[END_REF], Theorems 6.5 and 6.8

In other words, if for any initialization we can ensure that the process will come back with probability 1 to a central area A where mixing occurs, we have a convergence property. The following technical result is a cornerstone to demonstrate the theorems of Subsection 6.4.1.

x ∈ C and V ⊂ B o (q 0 (x), α ) = B o (I d , α )q 0 (x) we have:

))

))×

))

i.e. P(q z (x) ∈

2 ) we have:

As W n is independent from the other variables we obtain:

And finally:

As q 0 has I d as a fix point we only have to verify that the sets U n as defined in Lemma 2 eventually cover the whole set C. It suffices to consider for any n 0 the set D n = {x ∈ C, ∀k n, q k 0 (x) ∈ U 0 }. As we have q n 0 (x) → I d almost-everywhere on C we get: µ

So all the conditions of Lemma 2 hold and we can conclude convergence in T.V. norm to a stationary distribution which doesn't depend on the prior, provided that its support lies in C. Theorem 11 is proved.

Corollary 2 follows directly from the case C = G. Theorem 12 can be proved as follows. Let C = G. Let K = {x ∈ G, h(x, 0) = h(I d , 0)}. Note that the left-right equivariance assumption ensures that K is a subgroup of G. First we show that there exists an integer N 1 such that K ⊂ U N 1 . As we have ∀x ∈ K q 0 (x) = ϒxϒ -1 the sequence of sets Q n = ϒ -n U n ϒ n ∩ K is growing and we have: proving in the Lie algebra that k( γt ), k( γn ) 0. As u is a positive function we have thus u( γt ) -1 γt k( γt ), -γt k( γn ) 0 immediately proving grad E ( γt ), d dt γt 0. The latter result being true for every t ∈ R\N and the function E( γt ) being continuous, it decreases and thus converges on R ≥0 . As we have supposed the sublevel sets of E are bounded (and closed as E is continuous), γt is stuck in a compact set. Let γ ∞ an adherence value of γn . By continuity of E we have E( γ∞ ) = E( γ∞ exp(-k( γ∞ )). Thus the function t → E( γ∞ exp(-tk( γ∞ ))) is decreasing on [0, 1] and has the same value at 0 and 1. Thus it is constant, its derivative at 0 is null proving γ ∞ = I d . ( γn ) n 0 being confined in a compact set and having I d as unique adherence value we finally get γ n = γn → I d .

B.2.2 Proof of Proposition 17