The Invariant Extended Kalman filter as a stable observer

# The Invariant Extended Kalman filter as a stable observer

Axel Barrau, Silvère Bonnabel A. Barrau and S. Bonnabel are with MINES ParisTech, PSL Research University, Centre for robotics, 60 Bd St Michel 75006 Paris, France.[axel.barrau,silvere.bonnabel]@mines-paristech.fr
###### Abstract

We analyze the convergence aspects of the invariant extended Kalman filter (IEKF), when the latter is used as a deterministic non-linear observer on Lie groups, for continuous-time systems with discrete observations. One of the main features of invariant observers for left-invariant systems on Lie groups is that the estimation error is autonomous. In this paper we first generalize this result by characterizing the (much broader) class of systems for which this property holds. Then, we leverage the result to prove for those systems the local stability of the IEKF around any trajectory, under the standard conditions of the linear case. One mobile robotics example and one inertial navigation example illustrate the interest of the approach. Simulations evidence the fact that the EKF is capable of diverging in some challenging situations, where the IEKF with identical tuning keeps converging.

## 1 Introduction

The design of non-linear observers is always a challenge, as except for a few classes of systems (e.g., [15]), no general method exists. Of course, the grail of non-linear observer design is to achieve global convergence to zero of the state estimation error, but this is a very ambitious property to pursue. As a first step, a general method is to use standard linearization techniques, such as the extended Kalman filter (EKF) that makes use of Kalman equations to stabilize the linearized estimation error, and then attempt to derive local convergence properties around any trajectory. This is yet a rare property to obtain in a non-linear setting (see, e.g., [1]), due to the fact that the linearized estimation error equation is time varying, and contrarily to the linear case it generally depends on the unknown true state we seek to estimate. The EKF, the most popular observer in the engineering world, provides an “off the shelf” candidate observer, potentially able to deal with the time-varying nature of the linearized error equation, due to its adaptive gain tuning through a Riccati equation. However, the EKF does not possess any optimality 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 unadapted 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 [11, 25, 24, 10]) rely on the highly non-trivial assumption that the eigenvalues of the Kalman covariance matrix computed about the estimated trajectory are lower and upper bounded by strictly positive scalars. To the authors’ knowledge, only a few papers deal with the stability of the EKF without invoking this assumption [19]. It is then replaced by second-order properties whose verification can prove difficult in practical situations. This lack of guarantee is also due to the fact the filter can diverge indeed in a number of applications. Note that, beyond the general theory, there are not even that many engineering examples where the EKF is proved to be (locally) stable.

The present paper builds upon the theory of symmetry preserving observers [8, 7] and notably the theory of invariant Kalman filtering [6, 9, 23, 5] in a purely deterministic context. As such, it is a contribution to the theory of non-linear observer design on Lie groups that has lately attracted considerable interest, notably for attitude estimation, see, e.g., [22, 26, 3, 17, 20, 16, 18]. The detailed contributions and organization of the paper are as follows.

In Section 2, we recall the main contribution of [8, 7] is to evidence the fact that for left-invariant systems on Lie groups, non-linear observers may be designed in such a way that the left-invariant estimation error obeys an autonomous equation, a key property for observer design on Lie groups. We show here this property of the error equation can actually be obtained for a much broader class of systems, and we characterize this class. Very surprisingly, it turns out that, up to a suitable non-linear change of variables, the error evolution (in the absence of measurements) obeys a linear differential equation.

In Section 3, we focus on the invariant extended Kalman filter (IEKF) [6] when applied to the broad class of systems of Section 2. We consider continuous-time models with discrete observations, which best suits navigation systems where high rate sensors governing the dynamics are to be combined with low rate sensors [14]. We change a little the IEKF equations to cast them into a matrix Lie group framework, more handy to use than the usual abstract Lie group formulation of [6]. We then prove, that under the standard convergence conditions of the linear case [13], applied to the linearized model around the true state, the IEKF is an asymptotic observer around any trajectory of the system, a rare to obtain property. This way, we produce a generic observer with guaranteed local convergence properties under natural assumptions, for a broad class of systems on Lie groups, whereas this property has so far only been reserved to specific examples on Lie groups. This also allows putting on firm theoretical ground the good behavior of the IEKF in practice, as already noticed in a few papers, see e.g., [3, 5, 2].

In Section 4 we consider a mobile robotics example, where a unicycle robot (or simplified car) tries to estimate its position and orientation from GPS position (only) measurements, or alternatively landmarks range and bearing measurements. On this example of engineering interest, the IEKF is proved to converge around any trajectory using the results of the paper, which is a contribution in itself. Simulations indicate the IEKF is always superior to the EKF and may even outperform the latter in challenging situations.

In Section 5 we consider the highly relevant problem of an unmanned aerial vehicle (UAV) navigating with accelerometers and gyrometers, and range and bearing measurements of known landmarks. Although the system is not invariant in the sense of [8, 7], it is proved to fit into our framework so that the autonomous error equation property of [7] holds, a fact never noticed before to our best knowledge (except in our preliminary conference paper [4]). The IEKF is shown to converge around any trajectory using the results of the paper, which is a contribution in itself. Moreover, it is shown to outperform the EKF which even diverges when, as in high precision navigation, the user has way more trust in the inertial sensors than in the landmark measurements.

The main contributions can be summarized as follows:

• The class of systems, for which the key result of [7] about the (state) error equation autonomy holds, is completely characterized, and actually shown to be much broader than left-invariant systems.

• The autonomy of the error equation is proved to come with a very intriguing property: a well-chosen non-linear function of the non-linear error is proved to obey a linear differential equation.

• In turn, this property allows proving that, for the introduced class of systems, the IEKF used in a deterministic context possesses powerful local convergence guarantees that the standard EKF lacks.

• Two examples of navigation illustrate the applicability of the results, and simulations indicate indeed the IEKF is always superior to the EKF, and may turn out to literally outperform the latter when confronted with some challenging situations - the EKF being even capable to diverge.

## 2 A special class of multiplicative systems

### 2.1 An introductory example

Consider a linear (deterministic) system . Consider two trajectories of this system, say, a reference trajectory and another one . The discrepancy between both trajectories satisfies the linear equation . This is a key property for the design of linear convergent observers, as during the propagation step, the evolution of the error between the true state and the estimate does not depend on the true state’s trajectory.

Consider now the following non-linear standard model of the two-dimensional non-holonomic car. Its state is defined by three parameters : heading and position . The velocity is given by an odometer, the angular velocity is measured by a differential odometer or a gyrometer. The equations read (see, e.g., [12]):

 ddtθt=ωt,ddtx1t=cos(θt)ut,ddtx2t=sin(θt)ut.

Now consider a reference trajectory and a second trajectory with different initial conditions but same inputs. The exact propagation of the “error” , , satisfies:

 ddtΔθt=0, (1) ddtΔx1t=[cos(θt)−cos(¯θt)]ut, ddtΔx2t=[sin(θt)−sin(¯θt)]ut,

where we let . We see the time derivative of is not a function of only: it also involves and individually. Moreover, the equation is non-linear. These features, characteristic of non-linear systems, make the design of observers way more complicated in the non-linear case. Now, let us introduce the following non-linear error, where denotes the planar rotation matrix of angle (see definition of below):

 ξt:= (2) with J2=(0−110) and a(s)=sin(s)1−cos(s),

which is equal to 0 indeed if and only if both trajectories coincide. We are about to prove by elementary means a surprising property that will be generalized by Theorem 2.

###### Proposition 1.

Contrarily to the linear error obeying (1), the alternative non-linear error (2) obeys the following linear and autonomous equation although the system, and the error, are totally non-linear:

 ddtξt=⎛⎜⎝00000ωt−ut−ωt0⎞⎟⎠ξt. (3)
###### Proof.

We will use the notations . Since we have as in the linear case above, only the two last terms of change over time. Moreover, commutes with , and and . Thus we have:

 ddt δxt=−J2ωtδxt +12δθt[a(δθt)I2−J2]R(−¯θt)[R(θt)−R(¯θt)](ut,0)T = −J2ωtδxt = −J2ωtδxt+δθtJ2(ut,0)T = −(ωtJ2)δxt+δθt(0,−ut)T.

In the second to last equality we used the relation . Equation (3) is proved. ∎

The present section provides a novel geometrical framework - encompassing this example - to characterize systems on Lie groups for which such a property holds. In turn, such a property will simplify the convergence analysis of non-linear observers, namely the IEKF, due to the implied similarities with the linear case.

### 2.2 Systems on Lie groups with state trajectory independent error propagation property

Let be a matrix Lie group whose Lie algebra is denoted and has dimension . We consider a class of dynamical systems:

 ddtχt =fut(χt), (4)

where the state lives in the Lie group and is an input variable. Consider two distinct trajectories and of (4). Define the left-invariant and right-invariant errors and between the two trajectories as:

 ηLt=χ−1t¯χt(left invariant), (5)
 ηRt=¯χtχ−1t(right invariant). (6)

The terminology stems from the invariance of e.g., (5) to (left) multiplications for .

###### Definition 1.

The left-invariant and right-invariant errors are said to have a state-trajectory independent propagation if they satisfy a differential equation of the form .

Note that, in general the time derivative of is a complicated function depending on and both and in a way that does not boil down to a function of , see for instance eq (1) above. The following result allows characterizing the class of systems of the form (4) for which the property holds.

###### Theorem 1.

The three following conditions are equivalent for the dynamics (4):

1. The left-invariant error (5) is state trajectory independent

2. The right-invariant error (6) is state trajectory independent

3. For all and we have (in the tangent space at ):

 fut(ab)=fut(a)b+afut(b)−afut(Id)b, (7)

where denotes the identity matrix. Moreover, if one of these conditions is satisfied we have

 ddtηLt =gLut(ηLt)wheregLut(η)=fut(η)−fut(Id)η, (8) ddtηRt =gRut(ηRt)wheregRut(η)=fut(η)−ηfut(Id). (9)
###### Proof.

Assume we have for a certain function and any , where and are solutions of (4). We have:

 gut(χ−1t¯χt)=ddt(χ−1t¯χt)=−χ−1t[ddtχt]χ−1t¯χt+χ−1tddt¯χt =−χ−1tfut(χt)ηLt+χ−1tfut(¯χt), i.e. gut(ηLt)=−χ−1tfut(χt)ηLt+χ−1tfut(χtηLt). (10)

This has to hold for any and . In the particular case where we obtain:

 gut(ηLt)=fut(ηLt)−fut(Id)ηLt. (11)

Reinjecting (11) in (10) we obtain:

 fut(χtηLt)=fut(χt)ηLt+χtfut(ηLt)−χtfut(Id)ηLt.

The converse is trivial and the proof is analogous for right-invariant errors. ∎

###### Remark 1.

The particular cases of left-invariant and right-invariant dynamics, or the combination of both as follows, verify (7). Let . We have indeed:

 fvt,ωt(a)b+afvt,ωt(b)−afvt,ωt(Id)b =(vta+aωt)b+a(vtb+bωt)−a(vt+ωt)b =vtab+abωt=fvt,ωt(ab).
###### Remark 2.

In the particular case where is a vector space with standard addition as the group composition law, the condition (7) boils down to and we recover the affine functions. We thus see the class of system introduced here appears as a generalization of the linear case.

In the next section we show that the dynamics of the form (4) with additional property (7) have striking properties generalizing those of linear systems.

### 2.3 Log-linear property of the error propagation

In the sequel, we will systematically consider systems of the form (4) with the additional property (7), i.e. systems on Lie groups defined by

 ddtχt =fut(χt), (12) where ∀(u,a,b)fu(ab) =afu(b)+fu(a)b−afu(Id)b.

For such systems, Theorem 1 proves that the left (resp. right) invariant error is a solution to the equation where is given by (8) (resp. (9)). We have the following novel and striking property.

###### Theorem 2.

[Log-linear property of the error] Consider the left or right invariant error as defined by (5) or (6) between two arbitrarily far trajectories of (12), the superscript denoting indifferently or . Let and be defined as in Appendix A. Let be such that initially . Let be defined by . If is defined for by the linear differential equation in

 ddtξit=Aitξit, (13)

then, we have for the true non-linear error , the correspondence at all times and for arbitrarily large errors

 ∀t≥0ηit=exp(ξit).

The latter result, whose proof has been moved to the Appendix, shows that a wide range of nonlinear problems (see examples below) can lead to linear error equations provided the error variable is correctly chosen. We also see the results displayed in the previous introductory example of Section 2.1 are mere applications of the latter theorem, as the non-holonomic car example turns out to perfectly fit into our framework (see Section 4) and in eq (2) actually merely is the Lie logarithm of the left-invariant error. This will be extensively used in Section 3, and in the examples to prove stability properties of IEKFs.

## 3 Invariant Extended Kalman Filtering

In this section we first recap the equations of the Invariant EKF (IEKF), a variant of the EKF devoted to Lie groups space states, that has been introduced in continuous time in [6, 9]. We derive the equations in continuous time with discrete observations here, which has already been done in a restricted setting in [5], and we propose a novel matrix (Lie group) framework to simplify the design. We then show that for the class of systems introduced in Section 2, under observability conditions, and painless conditions on the covariance matrices considered here as design parameters, the IEKF is a (deterministic) non-linear observer with local convergence properties around any trajectory, a feature extremely rare to obtain in the field of non-linear observers, due to the dependency of the estimation error to the true unknown trajectory. The notions necessary to follow Section 3 are given in Appendix A.

### 3.1 Full system and IEKF general structure

We consider in this section an equation on a matrix Lie group of the form:

 ddtχt =fut(χt), (14)

with for all . This system will be associated to two different kinds of observations.

#### 3.1.1 Left-invariant observations

The first family of outputs we are interested in write:

 Y1tn =χtnd1,...,Yktn=χtndk, (15)

where are known vectors. The Left-Invariant Extended Kalman Filter (LIEKF) is defined in this setting through the following propagation and update steps:

 ddt^χt =fut(^χt),tn−1≤t

where the function is to be defined in the sequel using error linearizations. A left-invariant error between true state and estimated state can be associated to this filter:

 ηLt=χ−1t^χt. (18)

During the Propagation step, and are two trajectories of the system (14). Thus, the error (18) is independent from the true state trajectory from Theorem 1 and eq. (8) ! We have thus

 ddtηLt=gLut(ηLt),tn−1≤t

Consider now the following linear differential equation in :

 ddtξt=Autξt, (20)

where is defined by . Theorem 2 implies the unexpected result:

###### Proposition 2.

If is defined as a solution to the linear system (20) and is defined as the solution to the nonlinear error system (19), then if at time we have then the equality is verified at all times , even for arbitrarily large initial errors.

Besides, at the update step, the evolution of the invariant error variable (18) merely writes:

 (ηLtn)+=χ−1tn^χ+tn=ηLtnexp⎡⎢ ⎢⎣Ln⎛⎜ ⎜⎝(ηLtn)−1d1−d1...(ηLtn)−1dk−dk⎞⎟ ⎟⎠⎤⎥ ⎥⎦. (21)

We see that the nice geometrical structure of the LIEKF allows the updated error to be here again only a function of the error just before update , i.e. to be independent from the true state .

#### 3.1.2 Right-invariant observations

The second family of observations we are interested in have the form:

 Y1tn=χ−1tnd1,...,Yktn=χ−1tndk, (22)

with the same notations as in the previous section. The Right-Invariant EKF (RIEKF) is defined here as:

 ddt^χt =fut(^χt), (23) ^χ+tn =exp⎡⎢ ⎢⎣Ln⎛⎜ ⎜⎝^χtnY1tn−d1...^χtnYktn−dk⎞⎟ ⎟⎠⎤⎥ ⎥⎦^χtn. (24)

A right-invariant error can be associated to this filter:

 ηRt=^χtχ−1t. (25)

Once again, Theorem 1 proves the evolution of the error does not depend on the state of the system. The analog of Proposition 2 is thus easily derived for the error (25) and we skip it due to space limitations.

At the update step, the evolution of the invariant error variable reads:

so that the error update does not depend on the true state either.

### 3.2 IEKF gain tuning

In the standard theory of Kalman filtering, EKFs are designed for “noisy” systems associated with the deterministic considered system. In a deterministic context, the covariance matrices and of the noises are left free to tune by the user, and are design parameters for the EKF used as a non-linear observer. Yet, in the spirit of [25], it is nevertheless convenient to associate a “noisy” system with the considered deterministic system consisting of dynamics (14) with outputs (15) or (22). The obtained error equations can be linearized, and the standard Kalman equations applied to make this error decrease. This way, the matrices and can be interpreted as covariance matrices. And in many engineering applications, the characteristics of the noises of the sensors are approximately known, so that the engineer can use the corresponding covariance matrices as a useful guide to tune (or design) the non-linear observer, that is here the IEKF. This provides him with (at least) a first sensible tuning of the parameter matrices, which is consistent with the trust he has in each sensor. Moreover, in the same spirit, the IEKF viewed as a non-linear observer remedies a common weakness shared by numerous non-linear observers on Lie groups, as it conveys an information about its own accuracy through the computed covariance matrix . Although it comes with no rigorous interpretation in a deterministic context, the information conveyed by may prove useful in applications.

Note that, in mobile robotics and navigation, the sensors are attached to the earth-fixed frame (e.g., a GPS) or to the body frame (e.g., a gyrometer). To interpret them as covariance matrices in the IEKF framework (see below) those matrices and may have to undergo a change of frame yielding trajectory dependent tuning matrices and , such as in the application examples in the sequel. This does not weaken the results, but however comes at the price of making the stability analysis a little more complicated.

#### 3.2.1 Associated “noisy” system

To tune the IEKF (16)-(17) or (23)-(24), we associate to the system (14) the following “noisy” system:

 ddtχt =fut(χt)+χtwt, (26)

where is a continuous white noise belonging to whose covariance matrix is denoted by (for a proper discussion on multiplicative noise for systems defined on Lie groups, see e.g. [5]).

In the same way, we associate to the family of left-invariant observations (15) the following family of “noisy” outputs:

 Y1tn =χtn(d1+B1n)+V1n , ... , Yktn=χtn(dk+Bkn)+Vkn, (27)

where the , are noises with known characteristics. To the family of right-invariant observations (22) we associate the following family of “noisy” outputs

 Y1tn=χ−1tn(d1+V1n)+B1n , ... , Yktn=χ−1tn(dk+Vkn)+Bkn. (28)

#### 3.2.2 Linearized “noisy” estimation error equation

As in a conventional EKF, we assume the error to be small (here close to as it is equal to if ) so that the error system can be linearized to compute the gains . By definition, the Lie algebra represents the infinitesimal variations around of an element of . Thus the natural way to define a vector error variable in is (see Appendix A):

 ηt=exp(ξt)=expm(Lg(ξt)). (29)

During the Propagation step, that is for , elementary computations based on the results of Theorem 1 show that for the noisy model (26) we have

 ddtηLt =gLut(ηLt)−wtηLt, (30) ddtηRt =gRut(ηRt)−(^χtwt^χ−1t)ηRt.

Defining by in the first case and (i.e. ) in the second case, and using the superscript to denote indifferently or we end up with the linearized error equation in :

 ddtξt=Aiutξt+^wt, (31)

where is defined by and where we have neglected terms of order as well as terms of order . The latter approximation, as well as the fact that can be considered as white noise, are approximations that would require a proper justification in a stochastic setting. However, they are part of the standard EKF methodology, see e.g., [21] and justifying them is not the object of the present paper. Besides, the emphasis is put here on deterministic properties of the observer.

Regarding the output, we consider for instance the case of left-invariant observations, and define through the exponential mapping (29), i.e. . Moreover, for let denote . The error update (21), when the LIEKF update (17) is fed with the “noisy” measurements (27) becomes

 (ηLtn)+ =χ−1tn^χ+tn =ηLtnexp⎡⎢ ⎢ ⎢⎣Ln⎛⎜ ⎜ ⎜⎝(ηLtn)−1d1−d1+^V1n+(ηLtn)−1B1n...(ηLtn)−1dk−dk+^Vkn+(ηLtn)−1Bkn⎞⎟ ⎟ ⎟⎠⎤⎥ ⎥ ⎥⎦. (32)

To linearize it we proceed as follows. For we have

 (ηtn)−1di−di+^Vin+(ηLtn)−1Bin =expm(Lg(ξtn))−1(di+Bin)−di+^Vin =(Id−Lg(ξ)tn)(di+Bin)−di+^Vin+O∥ξtn∥2 =−Lg(ξ)tndi+^Vin+Bin+O(∥ξtn∥2)+O(∥ξtn∥∥Bin∥),

using a simple Taylor expansion of the matrix exponential map. Expanding similarly equation (32) yields

 (33)

with terms of order . Neglecting them we finally get the following linearized error equation in

 ξ+tn=ξtn+Ln(Hξtn+^Vn+Bn), (34)

where , and are defined by

Now, let reflect the trusted covariance of the modified process noise , and the trusted covariance of the modified measurement noise . Note that, equations (31) and (34) mimick those of a Kalman filter designed for the following auxiliary linear system with discrete measurements: , . The standard Kalman theory thus suggests to compute through the Riccati equation

 ddtPt=AutPt+PtATut+^Qt,P+tn=(I−LnH)Ptn,with Sn=HPtnHT+^Nn, Ln=PtnHTS−1. (35)

### 3.3 Summary of IEFK equations

In a deterministic context, the IEKF equations can be compactly recapped as follows

 ddt^χt=fut(^χt),tn−1≤t

where the LIEKF (resp. RIEKF) is to be used in the case of left (resp. right) invariant outputs. The gain is obtained in each case through the following Riccati equation

 ddtPt=AutPt+PtATut+^Qt,Sn=HPtnHT+^Nn,Ln=PtnHTS−1,P+tn=(I−LnH)Ptn. (37)

As concerns the LIEKF, is defined by , and is defined by . The design matrix parameters are freely assigned by the user. When sensor noise characteristics are known, they can provide the user with a first sensible tuning of those matrices by considering the associated “noisy” system (26)-(27) of Section 3.2. In this case, the matrices can be interpreted in the following way: denotes the covariance of the modified process noise and the covariance matrix of the noise , and being defined as: , .

As concerns the RIEKF implementation, is defined by , and by . The design matrix parameters are freely assigned by the user. Considering the associated “noisy” system (26)-(28) of Section 3.2 they can be interpreted as follows. denotes the covariance of the modified process noise and the covariance matrix of the noise , and being defined as: , .

### 3.4 Stability properties

The aim of the present section is to study the stability properties of the IEKF as a deterministic observer for the system (14)-(15), or alternatively (14)-(22). We are about to prove the IEKF has guaranteed (local) stability properties, that rely on the error equation properties the EKF does not possess. The stability of an observer is defined as its ability to recover from a perturbation or an erroneous initialization:

###### Definition 2.

Let denote a continuous flow on a space endowed with a distance . A flow is an asymptotically stable observer of about the trajectory if there exists such that:

 d(x,~x)<ϵ⇒d(Xtt0(x),^Xtt0(~x))→0 when t→+∞.

Theorem 4 below is the main result of the paper . It is a consequence of Theorem 2 of Section 2. J. J. Deyst and C. F. Price have shown in [13] the following theorem, stating sufficient conditions for the Kalman filter to be a stable observer for linear (time-varying) deterministic systems.

###### Theorem 3 (Deyst and Price, 1968).

Consider the linear system , with and let denote the square matrix defined by . If there exist such that:

1. where

Then the linear Kalman filter tuned with covariance matrices and is an asymptotically stable observer for the Euclidean distance. More precisely there exist