Content uploaded by Silvère Bonnabel

Author content

All content in this area was uploaded by Silvère Bonnabel on Oct 15, 2020

Content may be subject to copyright.

The following document is the preprint for the paper:

Invariant Kalman ﬁltering

Axel Barrau and Silvere Bonnabel

Annual Reviews of Control, Robotics, and Autonomous Systems,

Vol 1, pp: 237-257, 2018.

1

Invariant Kalman ﬁltering

Axel Barrau1and Silv`

ere Bonnabel2

Contents

1 INTRODUCTION 3

1.1 Extended Kalman ﬁltering (EKF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2 Motivation for the use of geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.3 Outline ............................................ 5

2 LIE GROUPS AND PROBABILITY 5

2.1 MatrixLieGroups ...................................... 5

2.2 Uncertainty representation on matrix Lie groups . . . . . . . . . . . . . . . . . . . . . . . 7

3 INVARIANT KALMAN FILTERING 7

3.1 Groupafﬁnesystems ..................................... 7

3.2 InvariantEKFmethodology ................................. 8

3.3 Geometricinsight....................................... 10

3.4 Unscentedversion....................................... 11

4 THEORETICAL CONVERGENCE GUARANTEES 12

4.1 The Invariant EKF as a non-linear stable observer . . . . . . . . . . . . . . . . . . . . . . 12

4.1.1 The conventional EKF as a non-linear observer . . . . . . . . . . . . . . . . . . . 12

4.1.2 The invariant EKF as a non-linear observer . . . . . . . . . . . . . . . . . . . . . 12

4.2 Non-linear observers on Lie groups . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5 INDUSTRIAL APPLICATION: DRONE NAVIGATION 13

6 APPLICATION TO SIMULTANEOUS LOCALIZATION AND MAPPING 14

6.1 EKF-based SLAM inconsistency and beneﬁts of the IEKF . . . . . . . . . . . . . . . . . 16

6.2 Experimentalresults ..................................... 17

7 CONCLUSION 18

8 SUMMARY POINTS 19

2

Abstract

The Kalman ﬁlter, or more precisely the extended Kalman ﬁlter (EKF), is a fundamental engineering

tool that is pervasively used in control, robotics, and for various estimation tasks in autonomous systems.

The recent ﬁeld of Invariant extended Kalman ﬁltering, aims at using the geometric structure of the

state space and the dynamics to improve the EKF, notably in terms of mathematical guarantees. The

methodology essentially applies in the ﬁeld of localization, navigation, and simultaneous localization

and mapping (SLAM). Albeit recent, its remarkable robustness properties have already motivated a real

industrial implementation in the aerospace ﬁeld. This review aims to provide an accessible introduction

to the methodology of invariant Kalman ﬁltering, and to allow the reader to gain insight into the relevance

of the method, as well as what the important differences with the conventional EKF are. This should be

of interest to readers intrigued by the application of mathematical theories to practical applications, and

also to readers interested in ﬁnding simple to implement and robust ﬁlters for localization, navigation,

and SLAM, notably for autonomous vehicle guidance.

1 INTRODUCTION

The goal of a ﬁlter is to estimate the state of a dynamical system, by combining an evolution model and some

sensor measurements that bring partial information about the state. Unfortunately, models are inherently

inaccurate and sensors are subject to noises that corrupt the measurements. The idea of ﬁltering is to

explicitly include both latter sources of uncertainty in the model, and to compute the best estimates of

the state that can be inferred from the available information. Even though the mathematical theory is now

well understood, it is still a challenge to design ﬁlters in practice for control, robotics, and autonomous

systems. The extended Kalman ﬁlter (EKF) appeared in the 1960s with the advent of computers, and was

ﬁrst implemented by NASA in the Apollo program to estimate in real time the trajectory of the space

capsule. The past two decades have also witnessed the burst of particle ﬁlters, with great advances both

in theory and in practice. However, they rely on extensive numerical computations that are not always

suited to real time on-board implementations, and there are theoretical caveats especially when process

noise is low (typically for static parameter estimation). The robotics community has also more recently

turned to optimization based techniques for ﬁltering (the problem is typically formulated as nonlinear least

squares) but the computation demands are extensive and robustness to erroneous initializations is not yet

clearly established. As robots and many control systems are real-time, the amount of computation is limited

and the EKF is still a very widespread tool in control and robotics along with its more recent variant the

unscented Kalman ﬁlter (UKF). In the aerospace industry it remains the reference ﬁlter.

1.1 Extended Kalman ﬁltering (EKF)

Consider a general dynamical system in discrete time whose state is described by the vector variable Xn∈

Rd. We associate a sequence of observations (Yn)n>0∈Rpwhich are measurement data returned by sensors.

The trusted evolution model is:

Xn=f(Xn−1,un,wn),(1)

where fis the function encoding the evolution of the system, wnis the (unknown) process noise, that is a

centered random variable with covariance matrix Qn, the vector un∈Rma control input, and the observation

consists of partial measurements of the state at time n:

Yn=h(Xn) + Vn,(2)

with hthe observation function and Vnthe (unknown) measurement noise that accounts for sensors’ limita-

tions.

The extended Kalman ﬁlter (EKF) computes in real time an approximation ˆ

Xn|nto the best state

estimate given the observations. Let Y1:ndenote the collection of past measurements Y1,Y2,· ·· ,Ynand u1:n

3

be similarly deﬁned. To be more precise, the EKF represents the belief P(Xn|u1:n,Y1:n), which assigns a

probability to each possible value of the true state in the light of all the information collected so far, by a

mean ˆ

Xn|nand covariance matrix P

n|n. Indeed the rationale is to use the following Gaussian approximation:

P(Xn|u1:n,Y1:n)≈N(ˆ

Xn|n,P

n|n). To compute the mean and covariance, the EKF uses a two step procedure.

Step 1 - Propagation: The estimate ˆ

Xn−1|n−1obtained after the observationYn−1, is propagated through

the deterministic part of (1):

ˆ

Xn|n−1=f(ˆ

Xn−1|n−1,un,0)(3)

To compute the associated covariance, introduce the estimation errors deﬁned as

en−1|n−1=Xn−1−ˆ

Xn−1|n−1,en|n−1=Xn−ˆ

Xn|n−1.(4)

The key idea underlying the EKF is to linearize the error system through a ﬁrst-order Taylor expansion of the

non-linear functions fand hat the estimate ˆ

Xn−1|n−1. Indeed, using the Jacobians Fn=∂f

∂X(ˆ

Xn−1|n−1,un,0),

Gn=∂f

∂w(ˆ

Xn−1|n−1,un,0), and Hn=∂h

∂X(ˆ

Xn|n−1), the combination of equations (1), (2) and (3) yields the

following ﬁrst-order expansion of the error system:

en|n−1=Fnen−1|n−1+Gnwn,(5)

Yn−h(ˆ

Xn|n−1) = Hnen|n−1+Vn,(6)

where the second order terms, that is, terms of order O||e||2

,||w||2

,||e||||w||have been removed, see e.g.,

[52]. P

n−1|n−1is an approximation to the true covariance E(en−1|n−1eT

n−1|n−1), and it is propagated through

the linearized model (5) so that P

n|n−1=FnP

n−1|n−1FT

n+GnQnGT

nis an approximation of E(en|n−1eT

n|n−1),

and we have P(Xn|u1:n,Y1:n−1)≈N(ˆ

Xn|n−1,P

n|n−1).

Step 2 - Measurement update: To account for the measurement Yn, we let zn=Yn−h(ˆ

Xn|n−1), and

znis called the innovation. Assuming that en|n−1∼N(0,P

n|n−1)and the approximation (6) to be exact,

the linear Kalman ﬁlter equations ensure that the updated error en|n=Xn−ˆ

Xn|nsatisﬁes en|n∼N(0,P

n|n)

where

ˆ

Xn|n=ˆ

Xn|n−1+Knzn,and P

n|n= [I−KnHn]P

n|n−1(7)

with Kn, called the Kalman gain, computed in Algorithm 1. Of course the belief after update N(ˆ

Xn|n,P

n|n)

is only an approximation to P(Xn|u1:n,Y1:n), due to the linearizations. In practice, those linearizations may

lead the ﬁlter to inconsistencies and sometimes even divergence.

Algorithm 1 Extended Kalman Filter (EKF)

Choose an initial estimate ˆ

X0|0and uncertainty matrix P

0|0.

loop

Deﬁne Fn,Gnand Hnthrough (5) and (6).

Deﬁne Qnas Cov(wn)and Rnas Cov(Vn).

Propagation

ˆ

Xn|n−1=fˆ

Xn−1|n−1,un,0

P

n|n−1=FnP

n−1|n−1FT

n+GnQnGT

n

Measurement update

Compute zn=Yn−hˆ

Xn|n−1,Sn=HnP

n|n−1HT

n+Rn,Kn=P

n|n−1HT

nS−1

n

P

n|n= [I−KnHn]P

n|n−1

ˆ

Xn|n=ˆ

Xn|n−1+Knzn

end loop

4

1.2 Motivation for the use of geometry

The user who is facing the ﬁltering problem deﬁned by system (1)-(2) with fand htwo non-linear maps,

is free to choose a different coordinate system to design an EKF. For instance, in radar tracking, one can

choose a frame attached to the target, or attached to the radar, but also range and bearing as an alternative to

Cartesian coordinates. In general, it is unclear what the best coordinates for EKF design are. However, for

some systems, there is a choice of coordinates that is natural. For instance, when facing a linear Gaussian

system, the EKF boils down to the linear Kalman ﬁlter, which is optimal, and it would be nonsense to work

with alternative coordinates that would make the system non-linear. In this paper we advocate that for a

large class of systems deﬁned on matrix Lie groups, the machinery of geometry provides coordinates

that are unarguably more suited to the problem. In those cases, the Invariant EKF theory is useful, as

the original problem is often formulated using coordinates that do not mach the group structure, leading to

degraded performance of the conventional EKF.

For systems on Lie groups, the Invariant extended Kalman ﬁlter (IEKF) was originally introduced in

[11] and continued in [15, 44, 4, 16, 7]. The complete methodology along with the convergence properties

of the IEKF can be found in the recent paper [7]. More generally, the use of Lie groups for state estimation

dates back to the 1970s [18, 56, 28] and has recently spanned a range of applications and a rich stream of

theoretical results, see e.g. [43, 53, 10, 9, 16, 40, 41].

In the robotics community, following the work of [22], it has increasingly been recognized that using

probability distributions properly deﬁned on Lie groups is of importance, notably for pose estimation, see

e.g. [5, 21, 22, 23, 58, 42]. Moreover, the use of the IEKF over the conventional EKF has recently been

shown to solve the inconsistency issues of the EKF based simultaneous localization and mapping (SLAM)

in [8].

1.3 Outline

Section 2 consists of geometry preliminaries. Section 3 reviews the methodology of Invariant Kalman

ﬁltering (IEKF). Section 4 is concerned with the mathematical guarantees that come with the IEKF. Sec-

tion 5 presents some real industrial applications to the ﬁeld of inertial navigation. Section 6 reviews the

inconsistency of EKF-based SLAM and the interest of IEKF-based SLAM.

The presentation of the present article is freely inspired by the very tutorial paper [6], that uses methods

rooted in differential geometry to improve Monte Carlo schemes.

2 LIE GROUPS AND PROBABILITY

2.1 Matrix Lie Groups

In this section we provide the reader with the bare minimum of Lie group theory that is required to introduce

the Invariant EKF methodology1. A matrix Lie group Gis a subset of square invertible N×Nmatrices

MN(R)verifying the following properties:

IN∈G,∀χ∈G,χ−1∈G,∀χ1,χ2∈G,χ1χ2∈G,

where INis the identity matrix of RN. The subset Gis generally not a vector space, and can thus be viewed

as a curved space (see Fig. 1). To every point χ∈G, one can associate a vector space TχGcalled the

tangent space at χ, and deﬁned as all the matrices that write d

dt γ(0)where γ:R→Gis a smooth curve of

1General Lie groups: All the results carry over to general abstract Lie groups, but the (less general) matrix Lie groups are well

suited to tutorial and computational purposes, and encompass all the applications discussed.

5

Figure 1: Gis a curved space. Left and right multiplications offer two ways to identify the tangent space

TχGat χwith the tangent space at Identity TING, called the Lie algebra g. In turn, the application ξ7→ ξ∧

provides a linear bijection between the Euclidean space Rdand g. (Remark: Lie groups are homogeneous

spaces, which somehow “look the same everywhere”. As such, the ﬁgure may be slightly misleading, since

the curved surface representing Gseems irregular. Yet, it seems to us that representing a Lie group by, e.g.,

a sphere, would be an oversimpliﬁcation.)

Gthat satisﬁes γ(0) = χ. The elements of this space are called tangent vectors.

The tangent space TINGat the identity INis called the Lie algebra, and plays a very speciﬁc role. It is

denoted gand its dimension ddeﬁnes a dimension for the group Gitself, where dis generally much smaller

than the dimension N2of the ambiant space. We have g⊂MN(R), but there always is an invertible map

Rd→gthat allows identifying gto Rd. For ξ∈Rdwe denote ξ∧∈gthe corresponding element of g, and

we recall that ξ7→ ξ∧is a linear map. There are then two canonical ways to identify Rdand the tangent

space TχGat any χ∈G: through left and right multiplications, and they are generally different. Indeed

for any ξ∈Rdthe vectors χ(ξ∧)and (ξ∧)χboth are tangent vectors at χ. Of course they are different due

to non-commutativity of matrix multiplication.

The usual matrix exponential map expm:g→Gconstitutes a bijection from a neighborhood V⊂g

of 0 to a neighborhood of the identity INin G. In this paper, we will call the Lie exponential map the

map exp : Rd→Gdeﬁned by exp(ξ) = expm(ξ∧), that is a bijection in a neighborhood of 0 ∈Rd, with

exp(ξ∧)−1=exp(−ξ∧). Moreover, χexp(·)and exp(·)χprovide two distinct bijections between a neigh-

borhood of 0 in Rdand a neighborhood of χin G.

The Baker-Campbell-Hausdorff (BCH) formula gives a series expansion for the image in gof the

product on G:BCH(ξ,ζ) = exp−1(exp(ξ)exp(ζ)). In particular it ensures exp(ξ)exp(ζ) = exp(ξ+ζ+

T), where Tis of the order O(kξk2

,kζk2

,kξkkζk).

Example 1. Consider the group of rotation matrices G=SO(3), that describes the orientation (attitude)

of a body in space. It is the subset of matrices R of M3(R)such that RRT=I3and det(R) = 1. Each R ∈G

can be viewed as the rotation that maps vectors expressed in the body frame to vectors expressed in the

earth-ﬁxed frame. We have d =3, and for any ξ∈R3,ξ∧∈R3×3is the skew symmetric matrix associated

with the “cross product” operator ζ→ξ×ζ. The Lie algebra is the set of skew symmetric matrices. Any

tangent vector U ∈TRG at R represents an inﬁnitesimal shift of R in SO(3). It can be written as U =Rξ∧

for some vector ξ∈R3or alternatively as U =ζ∧R for some ζ∈R3. Both ξand ζcan be seen as angular

velocity vectors that represent the same inﬁnitesimal rotation, but ξis a vector of the body frame whereas

ζis expressed in the earth-ﬁxed frame. It is easy to prove indeed that ζ=Rξ.

6

2.2 Uncertainty representation on matrix Lie groups

To deﬁne random variables on Lie groups, we cannot apply the usual approach of additive noise for χ∈G

as Gis not a vector space. In contrast, we deﬁne the probability distribution χ∼NL(¯

χ,P)as the probability

law of the random variable χ∈Gdeﬁned as

χ=¯

χexp(ξ),ξ∼N(0,P),(8)

where N(., .)is the classical Gaussian distribution in Euclidean space Rd- identiﬁed to the Lie algebra

gand P∈Rd×dis a covariance matrix. In (8), the original Gaussian vector ξof the Lie algebra is moved

over by left multiplication to be centered at ¯

χ∈G, and we similarly deﬁne the distribution χ∼NR(¯

χ,P)

for right multiplication of ¯

χthrough the random variable

χ=exp(ξ)¯

χ,ξ∼N(0,P).(9)

In (8) and (9), ¯

χis deterministic and can take any value, whereas Pis the covariance of the small, noisy

perturbation ξ. We stress that we have deﬁned these probability density functions directly in the vec-

torspace Rdand that both NL(., .)and NR(., .)are not Gaussian distributions. These kinds of distributions

introduced to our best knowledge in [58, 23, 22] were advocated and studied in [5] for pose estimate and

leveraged for Kalman ﬁltering in [7, 16, 19]. They are sometimes referred to as concentrated Gaussians on

Lie groups. Note that, χin (8) and (9) is a well deﬁned random variable of G. Computing its distribution

on Gis not trivial though, but for the purposes we pursue in invariant ﬁltering it is absolutely not necessary.

Several approaches to ﬁltering for systems possessing a geometric structure have been developed in

previous literature. For stochastic processes on Riemannian manifolds [36] some results have been derived,

see e.g., [46]. The speciﬁc situation where the process evolves in a vector space but the observations belong

to a manifold has also been considered, see e.g. [28, 48] and more recently [49]. For systems on Lie groups

powerful tools to study the ﬁltering equations - such as harmonic analysis [57, 47, 55] - have been used,

notably in the case of bilinear systems [56] and estimation of the initial condition of a Brownian motion

in [27]. A somewhat different but related approach to ﬁltering consists of ﬁnding the path that best ﬁts the

data in a deterministic setting. It is thus related to optimal control theory where geometric methods have

long played an important role [18]. A certain class of least squares problems on the Euclidean group has

been tackled in [32], see also [59].

3 INVARIANT KALMAN FILTERING

In invariant Kalman ﬁltering, the state space is assumed to be a matrix Lie group. As a result it comes with

the non-linear machinery of Subsection 2.1. However, we emphasize that it is the coupling of the choice of

the Lie group structure and the dynamical model used in (1) that allows derivation of properties of the ﬁlter.

Originally only dynamics that were invariant to the group action were considered - hence the name, the

IEKF is rooted in [14, 12] - and the class of dynamics to be considered has recently been much extended,

as presented in Subsection 3.1. The remainder of the section is as follows. Subsection 3.2 is devoted to the

methodology of the IEKF, Subsection 3.3 to the geometric vision that underlies it, and Subsection 3.4 to its

recent unscented version.

3.1 Group afﬁne systems

This section reviews some results of [7], that are derived in the continuous time context, and we present

here their (novel) discrete-time counterpart. Consider a deterministic dynamical system on a matrix Lie

group G:

χn=f(χn−1,un)(10)

7

with fa smooth map on the group, un∈Rm, or possibly un∈G(in which case we use a capital Un). In

invariant Kalman ﬁltering, function fis required to verify

∀χ,ν∈G,∀u f (χν,u) = f(χ,u)f(IN,u)−1f(ν,u).(11)

Systems that satisfy this condition are called group afﬁne systems in [7]. Indeed if the group is merely

a vector space with addition as the group composition law, we recover the afﬁne functions, as (11) then

becomes f(a+b,u) = f(a,u)−f(0,u) + f(b,u)which implies f(x,u)is necessarily afﬁne if it is smooth.

Note that, the continuous version of condition (11) introduced in paper [7] is similar to the notion of linearity

on a group deﬁned in a different context in [3].

Theorem 1 (Fundamental property of invariant ﬁltering).We have the equivalence:

f satisﬁes (11) ⇔There exists a map g such that ∀χ,ν,u,f(υ,u)−1f(χ,u) = g(υ−1χ,u)

Moreover, in this case, for each u ∈Rmthere exists F ∈Rd×dsuch that ∀ξ∈Rd

,g(exp(ξ),u) = exp(Fξ),

that is, the function g(·,u)is wholly encoded in a simple matrix F.

This result is pivotal as will be explained in the sequel. Note that, the fact that g(·,u)may be described

by a mere matrix Fis not a ﬁrst order approximation. It is true at all orders, and this is a quite surprising

result.

Example 2. Let us continue our simple example of rotation matrices. Consider a motion R0,R1,··· on

SO(3)with Rn=Rn−1Un. The matrix Un∈SO(3)represents the relative rotation undergone by a body in

space between time steps n −1and n, and can be measured by (ﬂawless) gyroscopes: it can thus be viewed

as an input. The dynamics can be cast into the form (10) letting f (R,U) = RU. Thus f (χν,U) = χνU=

f(χ,U)f(I3,U)−1f(ν,U)so condition (11) is satisﬁed. And we have:

f(ν,U)−1f(χ,u) = (νU)−1χU=U−1ν−1χU=U−1(ν−1χ)U.

We see that the last term is a function of ν−1χand U indeed, as predicted by the theorem! This has been

long known in the SO(3)case, and was heavily exploited for attitude observer design, see e.g. [43, 53, 12,

10, 9, 16, 40, 41]. Moreover, it is well known from Lie group theory, see e.g. [5], that for any ξ∈Rd, we

have U−1exp(ξ)U=exp(AdUξ)where AdUξ=Uξin the SO(3) case. Letting F =U , we see this agrees

again with the second part of Theorem 1.

3.2 Invariant EKF methodology

This section is a summary of the IEKF methodology of [7]. Consider a general dynamical system χn∈G

associated to a sequence of observations (Yn)n>0∈Rpas follows :

χn=f(χn−1,un)exp(wn)(12)

Yn=h(χn) +Vn,(13)

where un∈Rdis a control input, wn∈Rdis a (unknown) vector encoding the process noise, Vn∈Rp

is the (unknown) measurement noise, and where fsatisﬁes condition (11). The (left) invariant EKF

(IEKF)2computes in real time an approximation to the posterior using the uncertainty representation (8),

that is, it computes at each step two parameters ˆ

χn|n∈Gand P

n|n∈Rd×d, and makes the approximation

P(χn|u1:n,Y1:n)≈NL(ˆ

χn|n,P

n|n).

2Left versus right: For simplicity of exposure the IEKF theory is presented using left-invariant error η=χ−1ˆ

χand uncertainty

representation (8). Swaping the role of left and right multiplications we can similarly deﬁne an alternative ﬁlter, the Right-Invariant

EKF.

8

Step 1 - Propagation: The IEKF propagates an estimate obtained after the previous observation Yn−1

through the deterministic part of (12), i.e., by setting wn=0:

ˆ

χn|n−1=f(ˆ

χn−1|n−1,un)(14)

To compute the associated covariance, introduce the left-invariant estimation errors deﬁned as

ηn−1|n−1:=χ−1

n−1ˆ

χn−1|n−1,ηn|n−1:=χ−1

nˆ

χn|n−1.(15)

This error is indeed invariant to left multiplications (χ,ˆ

χ)7→ (Γχ,Γˆ

χ)for any group element Γ∈G, and

right-invariant errors can be analogously deﬁned. This is a natural way of measuring the discrepancy

between the true state χand the estimate ˆ

χin a Lie group context where the usual linear error ˆ

χ−χis not

even an element of G. As fsatisﬁes (11), Theorem 1 yields:

ηn|n−1= [ f(χn−1,un)exp(wn)]−1f(ˆ

χn−1|n−1,un) = exp(−wn)g(ηn−1|n−1,un)(16)

where we have also used that exp(wn)−1=exp(−wn). Along the lines of the EKF, we want to linearize

the error system through a ﬁrst-order Taylor expansion of the non-linear functions fand hat the estimate

ˆ

χ. However, in contrast to (4), the estimation errors (15) are elements of G, that is, square matrices, and not

vectors. But when χand ˆ

χare close, the invariant error η=χ−1ˆ

χis close to the identity matrix IN. Using

that the Lie exponential map provides a bijection between a neighborhood of Rdand a neighborhood of IN,

the estimation error can be locally approximated by an element of Rd, that is, we let ξn−1|n−1,ξn|n−1∈Rd

be deﬁned by

ηn−1|n−1=exp(ξn−1|n−1),ηn|n−1=exp(ξn|n−1).(17)

Let Fn∈Rd×dbe the deﬁned by ∀ξ∈Rd

,g(exp(ξ),un) = exp(Fnξ). This matrix exists thanks to Theorem

1, and it is not deﬁned as through ﬁrst-order approximation, as in the conventional EKF case. Accord-

ing to the BCH formula we have exp(−wn)g(exp(ξn−1|n−1),un) = exp(Fnξn−1|n−1−wn)up to terms of

order kξn−1|n−1k2

,kwnk2

,kξn−1|n−1kkwnk. Recalling (16) and (17), this means that up to the ﬁrst order

exp(ξn|n−1) = exp(Fnξn−1|n−1−wn)so we get the following linearized equation, which is the Lie group

counterpart of (5)

ξn|n−1=Fnξn−1|n−1−wn.(18)

Remark 1. Despite the fact that the state belongs to a non-linear space, and is a matrix and not a vector, the

Lie exponential map allowed us to linearize the error system in Rdas in the conventional EKF methodology.

Moreover, note that Fndepends on unbut not on ˆ

χn−1|n−1. This is in contrast with the conventional EKF

where Fnin (5) depends on ˆ

Xn−1|n−1. This is a consequence of requirement (11) and Thm. 1, and it will

play an important role in the sequel.

We have exp(ξn|n−1) = ηn|n−1=χ−1

nˆ

χn|n−1, which implies that χn=ˆ

χn|n−1exp(−ξn|n−1). Resorting

to the uncertainty representation (8), and using (18), we have just proved that if P(χn−1|u1:n−1,Y1:n−1)≈

NL(ˆ

χn−1|n−1,P

n−1|n−1)then we have approximately the propagated distribution P(χn|u1:n,Y1:n−1)≈NL(ˆ

χn|n−1,P

n|n−1)

where P

n|n−1=FnP

n−1|n−1FT

n+Qn, and where Qn=Cov(wn) = Cov(−wn).

Step 2 - Measurement update To account for the new measurement we let zn=Yn−h(ˆ

χn|n−1)be the

innovation. It is a vector of Rp. We have zn=h(χn)−h(ˆ

χn|n−1)+Vn=h(ˆ

χn|n−1exp(ξn|n−1))−h(ˆ

χn|n−1)+

Vn. As ξn|n−1is assumed small, and as exp(0) = IN, a ﬁrst-order Taylor expansion in ξ∈Rdarbitrary,

allows deﬁning Hnas follows

h(ˆ

χn|n−1exp(ξ)) −h(ˆ

χn|n−1):=Hnξ+O(kξk2)(19)

9

Now that we have obtained a linearized system in Rdakin to (5)-(6), the conventional Kalman theory

can be applied to derive the Kalman gain Kn, and the updated covariance matrix P

n|n. The term Knznis a

corrective shift computed on the linearized system. Thus it should act on the linearized error ξn|n−1. As

our estimation errors on the group are of the form exp(ξ) = χ−1ˆ

χ, that is, χ=ˆ

χexp(ξ), an approximation

to the best estimate of χnafter observation Ynwhich is consistent with (15), is obtained through the Lie

group counterpart ˆ

χn|n=ˆ

χn|n−1exp(Knzn)of the linear update (7). The equations of the ﬁlter are detailed

in Algorithm 2.

Algorithm 2 Invariant Extended Kalman Filter (IEKF)

Choose initial ˆ

χ0|0∈Gand P

0|0∈Rd×d=Cov(ξ0|0)

loop

Deﬁne Hnas in (19), and Fnthrough the expansion g(exp(ξ),un) = exp(Fnξ).

Deﬁne Qnas Cov(wn)and Rnas Cov(Vn).

Propagation

ˆ

χn|n−1=f(ˆ

χn−1|n−1,un)

P

n|n−1=FnP

n−1|n−1F−1

n+Qn

Measurement update

Compute zn=Yn−hˆ

χn|n−1,Sn=HnP

n|n−1HT

n+Rn,Kn=P

n|n−1HT

nS−1

n

P

n|n= [I−KnHn]P

n|n−1

ˆ

χn|n=ˆ

χn|n−1exp(Knzn)

end loop

Remark 2. Many systems of interest are such that the observation (13) is of the form Yn=χnb+Vnwhere

b∈RNis a known vector. This is the case in the framework of [7]. Indeed, this means that one measures

some combinations of entries of the matrix entries, i.e., partial measurements of the state. In this case,

we use an alternative innovation ˜zn=ˆ

χ−1

n|n−1Yn−b, which is equal to η−1

n|n−1b−b+ˆ

χ−1

n|n−1Vn. Then, since

η−1

n|n−1b=exp(−ξn|n−1)b, the linearized output ˜

Hncorresponding to innovation ˜znis deﬁned similarly to

(19), through a ﬁrst-order expansion where we don’t consider the noise Vn, i.e., ˜zn=exp(−ξ)−1b−b:=

˜

Hnξ+O(kξk2). We then see that, along the lines of Remark 1, the matrix ˜

Hndoes not depend on ˆ

χn|n−1

either! Note that, the choice of the alternative innovation above is mostly tutorial though: a rigorous

application of Algorithm 2 to the present case would in fact lead to entirely identical estimates. This

may be shown as follows. On one hand we have ˜zn≈˜

Hnξ+ˆ

χ−1

n|n−1Vn=˜

Hnξ+ˆ

Vnwhere we have let

ˆ

Vn=ˆ

χ−1

n|n−1Vn, which is a noise having covariance matrix ˆ

Rn=ˆ

χ−1

n|n−1Rnˆ

χ−T

n|n−1. On the other we have

deﬁned zn=Yn−ˆ

χn|n−1b≈Hnξ+Vn.But we also have zn=ˆ

χn|n−1ˆ

χ−1

n|n−1Yn−b=ˆ

χn|n−1˜zn, proving

that Hn=ˆ

χn|n−1˜

Hn. The gain in Algorithm 2 is deﬁned as the Kalman gain for the linearized innovation

zn≈Hnξ+Vn, that is,

Kn=P

n|n−1HT

nHnP

n|n−1HT

n+Rn−1

,

For the alternative innovation ˜znof [7] the Kalman gain for the linearized system ˜zn≈˜

Hnξ+ˆ

Vnwrites

˜

Kn=P

n|n−1˜

HT

n˜

HnP

n|n−1˜

HT

n+ˆ

Rn−1=P

n|n−1˜

HT

nˆ

χT

n|n−1HnP

n|n−1HT

n+Rn−1ˆ

χn|n−1

and we see that ˜

Kn˜

Hn=KnHnand we also see that ˜

Kn˜zn=P

n|n−1HT

nHnP

n|n−1HT

n+Rn−1(ˆ

χn|n−1˜zn) = Knzn

and thus the two approaches yield exactly the same updates.

3.3 Geometric insight

Table 1 compares the main features of both EKF and IEKF methodologies. The IEKF features admit a

geometric interpretation illustrated by Figure 2.

10

Table 1: Differences between conventional EKF and Invariant EKF

EKF EKF IEKF IEKF

Name Expression Nature Expression Nature

State Xnvector of Rdˆ

χnmatrix of G⊂RN×N

Uncertainty repres. N(ˆ

X,P)Gaussian in RdNL(ˆ

χ,P)random matrix, see (8)

Linearized error en|nvector of Rdξn|nvector of Rd

Non-linear error en|nvector of Rdηn|n=exp(ξn|n)matrix of G⊂RN×N

Covariance matrix Pmatrix of Rd×dPmatrix of Rd×d

Correction term Knznshift in RdKnznshift in Rd

Figure 2: In the IEKF methodology the covariance matrix Prepresents a dispersion in Rd(illustrated by

the red 99% conﬁdence ellipsoid on the left plot). In turn, using the bijection ξ7→ ξ∧, it corresponds

to a dispersion in the Lie algebra TING=g(central plot). By adopting the uncertainty representation (8)

we implicitly posit that the left-invariant error χ−1ˆ

χis the exponential of a Gaussian. This means we

implicitly use left multiplication to move the ellipsoid over from TINGto Tˆ

χn|nG(right plot). Thus, the

choice of a particular non-linear estimation error rules the way conﬁdence ellipsoids “turn” when moved

over in G. Using (9) instead, that is right-invariant errors (and thus right multiplication ξ7→ ξ∧ˆ

χn|n), or

more generally the original coordinates in which the system is modeled, we would get a quite different

ellipsoid in Tˆ

χn|nG!

3.4 Unscented version

When applying Algorithm 2 to a particular system, matrices Fn,Hnmust be computed, which might prove

a little difﬁcult to some practitioners. This is one of the main reasons why the unscented extension of the

Kalman ﬁlter (UKF) of [37] has become a very popular alternative to the EKF. In [19] an unscented ver-

sion of the Invariant EKF was proposed, which spares the practitioner a computation of the Jacobians for

systems modeled using discrete time dynamics.

There have been other attempts to use (partially) the Lie group structure for UKF design, see [25, 24]

for symmetry-preserving observer design based on the unscented transform, [42] which uses the Lie group

structure of SE(3)for a drone navigation application, and [30] which exploits related ideas.

11

4 THEORETICAL CONVERGENCE GUARANTEES

Over the past decade there has been large body of literature devoted to deterministic non-linear observer

design on Lie groups in the control community. We partially review this ﬁeld in Subsection 4.2. But

prior to that, we review the convergence properties of the invariant EKF (IEKF), when used as a candidate

non-linear observer.

4.1 The Invariant EKF as a non-linear stable observer

Consider the model (1)-(2) and turn the noise off. This yields a deterministic system of the form Xn=

f(Xn−1,un),Yn=h(Xn). A stable non-linear observer is a dynamical system of the form ˆ

Xn=ˆ

f(ˆ

Xn−1,un,Yn)

where ˆ

fis designed to achieve asymptotic convergence of the estimation error, that is, ˆ

Xn−Xn→0 when

n→∞. Although no noise perturbs the system, the state Xnis assumed unknown and the system’s equations

only yield partial information about it. As a result, designing stable non-linear observers is often a great

challenge.

4.1.1 The conventional EKF as a non-linear observer

When facing a non-linear observer problem, any EKF can readily be used as a candidate asymptotic ob-

server, by choosing arbitrary matrices Qn,Rn, that are then viewed as tuning parameters. Unfortunately,

there are not many guarantees that the EKF when used as an observer will asymptotically converge. The

main results [17, 51] rely on strong assumptions about the ﬁlter’s behavior, and the EKF can actually fail

to converge sometimes, even for small initial estimation errors. To understand why, consider the block

diagram of Figure 3 that illustrates the architecture of the EKF (Algorithm 1).

System

Innovation zn=Yn−h(ˆ

Xn|n−1)

Extended Kalman ﬁlter Output map

Gain computation

Input unMeasurement Yn

ˆ

Xn|n−1

h(ˆ

Xn|n−1)

Knˆ

Xn−1|n−1,ˆ

Xn|n−1

un

Figure 3: Architecture of the EKF (Algorithm 1). The gain depends on the estimated state.

The important point is that the computation of the gain Knrelies on the matrices Fn,Hnintroduced at

(5)-(6), since the system is linearized at the estimate. This creates a loop between the estimate and the gain

computation that can destabilize the ﬁlter. Indeed if the estimate is not sufﬁciently close to the true state,

the gain will be erroneous, and the correction applied by the ﬁlter (7) will amplify the estimation error. This

positive feedback may lead to divergence indeed.

4.1.2 The invariant EKF as a non-linear observer

The IEKF does not suffer from this drawback. Indeed, consider the system (12) with noise turned off, that

is, χn=f(χn−1,un)and assume fsatisﬁes condition (11). As in Remark 2, assume also that the observation

(with noise turned off) writes Yn=h(χn) = χnbwith b∈RNa known vector. As emphasized by Remarks 1

and 2, neither Fnnor Hnthen depend on the estimates. This makes the gain computation independent from

12

System

Alternative innovation zn=ˆ

χ−1

nYn−b

Invariant extended Kalman ﬁlter Output map

Gain computation

Input unMeasurement Yn

ˆ

χn|n−1

h(ˆ

χn|n−1)

Kndoes not exist anymore

un

Figure 4: Architecture of the IEKF (Algorithm 2): the gain does not depend on the estimates.

the estimate, and spares the ﬁlter potentially harmful positive feedback. Indeed, as illustrated by the block

diagram of Figure 4, the inner loop of the EKF has disappeared. 3

The independence of the gain computation to the trajectory means that if the system were linearized

at the (unknown) true state, the gain Knwould be identical to the one obtained linearizing at the estimate!

Thanks to this strong property, stability of the IEKF as an observer could be proved in [7]. Although the

guaranteed convergence properties are only local, this is in sharp contrast with the conventional EKF where

no such guarantees exist.

4.2 Non-linear observers on Lie groups

There has been a huge body of research devoted to nonlinear observers on Lie groups over the past decade,

especially for attitude estimation. Although the literature is too broad to be covered here, we can point to

a few signiﬁcant references. Early developments on estimation on Lie groups were covered in Section 2.2.

In the early 2000s, there are essentially two streams of research that have bolstered the developments of

observers on Lie groups. The ﬁrst one was initiated by Aghannan and Rouchon [2] and seeks to design

non-linear observers that share the symmetries of the original system. The theory was formalized and de-

veloped in [14], and applied to estimation on Lie groups in [12]. At the same time, the complementary ﬁlter

on SO(3)for attitude estimation was introduced in [43]. It makes extensive use of left-invariant errors on

SO(3)and of the autonomy properties of the error equation. Due to its simplicity and its global conver-

gence guarantees, it has become a very renowned attitude estimator, and has proved useful for quadrotor

unmanned aerial vehicles (UAVs) control. Observers that are akin to the complementary ﬁlter on SO(3)for

attitude estimation include [53, 10, 9, 16, 40, 41], and similar ideas have been applied to pose estimation,

e.g. in [50]. The idea to use similar techniques for noisy (instead of deterministic) systems on Lie groups

and the subsequent theory of invariant Kalman ﬁltering is slightly more recent, see e.g. [11, 44, 4, 25, 16, 7].

5 INDUSTRIAL APPLICATION: DRONE NAVIGATION

Hybrid inertial navigation has been amongst the ﬁrst applications of the EKF in the 1960s, and a driver for

scientiﬁc breakthroughs and reformulations that led from the original work of Kalman to a widespread in-

dustrial tool, see e.g., [45]. The company Safran Electronics & Defense (formerly known as Sagem), which

is n◦1 in Europe for inertial navigation systems (INS), chose to invest in invariant ﬁltering. The Euroﬂir 410

(Fig. 5) is the ﬁrst commercial product implementing invariant Kalman ﬁltering. It is notably embedded in

the recent drone Patroller, which is a long-endurance UAV used for military purposes (the French army has

3Technical note: In the general IEKF theory of [7], there are in fact situations where the gain may partially depend on the trajectory,

but in a way that is not harmful.

13

Figure 5: Euroﬂir 410 (left), last generation of ultra-long-range electro-optical system commercialized

by Safran. Its navigation system includes the ﬁrst commercial implementation of an Invariant EKF. It is

embedded in particular in the drone Patroller (right).

recently purchased fourteen Patrollers) but also for various tasks such as detection of forest ﬁres.

Fig. 6 is obtained on real in-ﬂight experimental data for hybrid INS-GPS navigation. The initial head-

ing error has been set deliberately to an extreme value (90◦) to obtain experimental conﬁrmation that the

convergence is not affected, and this whatever the actual trajectory followed (a fact which agrees with the

insights of Section 4 indeed). The situation is wholly different when using a conventional EKF as shown

by Fig. 7, also obtained on real experimental data. We see conventional and invariant EKF share a similar

behavior for small angles (around 5◦) indeed, but EKF rapidly deteriorates for larger angles while IEKF

keeps converging almost as fast.

Remark 3. Commercial navigation systems such as the one equipping the Euroﬂir 410 have to model a

very high-dimensional state which includes sensor bias, scale factors and several other uncertainty sources,

so that the state space is not a Lie group and the dynamics not strictly group afﬁne. However, the IEKF

methodology can easily be adapted and employed, by appending the other variables to the state, and treat-

ing them along the lines of the EKF methodology. Its success stems from the fact that navigation systems

are clearly much “closer” to some group-afﬁne system indeed, than to a linear system, as assumed by the

conventional EKF methodology.

6 APPLICATION TO SIMULTANEOUS LOCALIZATION AND MAP-

PING

The problem of simultaneous localization and mapping (SLAM) has a rich history in robotics and au-

tonomous navigation over the past two decades, see e.g. [26, 29]. It can be posed as a ﬁltering problem,

and the extended Kalman ﬁlter (EKF) based SLAM (the EKF-SLAM) one was of the ﬁrst algorithm to be

used in this ﬁeld. It was mostly abandoned though, due to inconsistencies of its estimates. However, it was

recently proved in [8] that invariant Kalman ﬁltering resolves the inconsistency issues of the EKF.

The Invariant EKF-SLAM algorithm was introduced and studied in [8] and preliminary ideas date back

to [13]. In [61] some complementary properties of the Invariant EKF-SLAM were derived, and it was

successfully applied to visual-inertial SLAM in [60].

14

Figure 6: Heading estimated by an Invariant EKF based inertial navigation system started in-ﬂight with

extremely large initial error (90 degrees). Due to the the properties highlighted in Section 4, convergence is

not perturbed despite a large initial error and a complicated trajectory.

Figure 7: Heading error (milliradians) for invariant EKF (left) and conventional EKF (right) for GPS-INS

hybrid navigation. The ”small angles” hypothesis is crucial to EKF, but not to IEKF.

15

6.1 EKF-based SLAM inconsistency and beneﬁts of the IEKF

The issue of EKF-SLAM inconsistency has been the object of many papers, see [38, 20, 35] to cite a few,

where empirical evidence and theoretical explanations in various particular situations have been accumu-

lated. In this context, inconsistency refers to the inability of the ﬁlter’s output covariance matrix P

n|nto

correctly reﬂect the true error dispersion E(Xn−ˆ

Xn|n)(Xn−ˆ

Xn|n)T. In particular, it was proved the orienta-

tion uncertainty is a key feature in the inconsistency, and this derives from the linearization process. A little

later, [33, 34] have provided a complementary sound theoretical analysis of the EKF-SLAM inconsistency

insisting on the EKF’s inability to correctly reﬂect the unobservabilities inherent to the SLAM problem.

(a) (b)

Figure 8: (a) In the SLAM problem a robot (triangle) is moving and makes measurements relative to some

landmarks. The goal is to simultaneously estimate the robot’s trajectory and the position of the landmarks,

i.e., the map. (b) The problem is unobservable as a global rotation of the robot and the landmarks is

impossible to observe: the robot only makes relative measurements of landmarks. Thus it can’t distinguish

between the left and the right conﬁguration of picture (b).

Although all the results below carry over to 3D SLAM, we focus on the 2D case for simplicity. In

the SLAM problem in 2D, a robot moves and observes some ﬁxed features (or landmarks) of the environ-

ment. At time n, let xn∈R2be the position of the robot , θnits orientation, with respect to a global frame,

and let p1,p2,· ·· ,pK∈R2be the positions of the Klandmarks in the global frame, see Fig. 8 (a). The

state is the vector Xn= (xn,θn,p1,·· · ,pK)and the goal is to estimate it from observations of landmarks

relative to the robot’s frame through sensors (Lidars, cameras) attached to the robot. The collection of

landmarks p1,p2,·· · ,pKconstitutes the map, hence the name SLAM. Let R(α)denote a planar rotation

of angle αand consider the transformation on the state space Ψα:(xn,θn,p1,·· · ,pK)7→ (R(α)xn,θn+

α,R(α)p1,·· · ,R(α)pK). As simply illustrated on Fig. 8 (b), the state Xn= (xn,θn,p1,··· ,pK)and the

state Ψα(Xn)are undistinguishable, that is, departing from either one or the other conﬁguration, whatever

the motions of the robot, it is impossible for it to tell that its own position, orientation, and the landmarks’

positions are actually different in both cases.

The main source of inconsistency of the EKF-SLAM derives from this unobservability, as illustrated

by Figure 9. The linearity of the update step (7) in the original variables combined with the uncertainty

representation N(ˆ

Xn|n,P

n|n−1)results in a mere translation of the conﬁdence ellipsoid at the update step,

that does not match with unobservablities of the system. In contrast, the SLAM state space can be endowed

with a Lie group structure as ﬁrst noticed in [13]. As proved in [8] this deﬁnes local coordinates such that

the unobservable directions become independent of the linearization point. As a result, an EKF-SLAM

based on those coordinates, namely an Invariant EKF SLAM, computes beliefs that always match with the

unobservable directions. And this is what allowed to prove consistency properties in [8].

16

(a) (b)

Figure 9: Difference between the conventional EKF and invariant EKF update steps with respect to un-

observable directions. (a) EKF-SLAM:Ψα(ˆ

Xn|n−1)and ˆ

Xn|n−1correspond to the two undistinguishable

conﬁgurations of Fig. 8 (b), so {Ψα(ˆ

Xn|n−1),α∈R}deﬁnes a continuous curve of states undistinguishable

from ˆ

Xn|n−1in the state space. In terms of linearized model, this means Ψ0

α(Xn):=d

dαΨα(Xn)deﬁnes

a direction in the state space at Xnalong which no information can ever be gained through measurement

data. The problem with the EKF is as follows. The covariance P

n|nis computed with local information

(that is, the linearized model) at the propagated step ˆ

Xn|n−1. Even if the updated belief correctly reﬂects

the unobservability there, that is, the conﬁdence ellipsoid deﬁned by P

n|nis very elongated in the unob-

servable direction Ψ0

α(ˆ

Xn|n−1), it is then moved over to updated state ˆ

Xn|nthrough a simple translation.

But Ψ0

α(ˆ

Xn|n−1)6=Ψ0

α(ˆ

Xn|n)which means the updated belief N(Xn|n,P

n|n)is elongated in the direction

Ψ0

α(ˆ

Xn|n−1)that does not match with the actual unobservable direction Ψ0

α(ˆ

Xn|n)at ˆ

Xn|n.(b) IEKF-SLAM:

In the coordinates induced by the Lie group structure of the state space, the unobservable directions Ψ0

α(ˆ

X)

become independent of ˆ

X! The effect, back in the original variables, is that conﬁdence ellipsoids “turn” in

a way that matches with unobservable directions.

6.2 Experimental results

The smoothing and mapping (SAM) approach is an alternative to the ﬁltering approach for SLAM. The

idea is roughly to formulate the problem of ﬁnding the most likely robot’s trajectory and map as a big least

squares problem that involves all the past states and the past measurements up to current time step. The

method ﬁnds its roots in [54] to our best knowledge. The problem can be solved incrementally, and one of

the most commonly used such algorithm is the iSAM of [39], that can be implemented using the package

available at https://svn.csail.mit.edu/isam. At each time step, it returns the maximum likelihood (MaxLik)

value of the state given all past the measurements. When it converges, iSAM can be viewed as an optimal

ﬁlter and provides a reference estimate. This is why over the past recent years, the SLAM community has

gradually turned to optimization based techniques.

The conventional EKF, described in Algorithm 1, the Invariant EKF which is a right multiplication

based version of Algorithm 2, and the iSAM, have been tested on the publicly available real Victoria Park

dataset described in [31]. It is a large scale experiment, in which a vehicle drives through a Park where

(hundreds of) trees constitute the landmarks. Estimated trajectories, as well as the GPS measured trajec-

tory, are displayed in Figure 10. The SLAM algorithms use the odometers, steering angle, and laser sensors’

measurements, whereas GPS measurements are solely used for evaluation purposes. Table 2 displays the

error with respect to GPS measurements for various tunings of covariance matrices (as noise characteristics

are not precisely known, various choices are possible indeed).

The results are very instructive regarding robustness of the ﬁlters: the results of IEKF and iSAM are

almost insensitive to retained tuning parameters (RMSE stays between 5 and 11 m) while the EKF is very

17

sensitive to tuning parameters. This can be a problem as the tuning of noise matrices when no ground truth

is available is always difﬁcult in practice, see e.g. [1]. Moreover, IEKF and iSAM results are always very

close to each other. This is interesting, as there already is a large corpus of experience and method in the

industry for EKF implementation in the ﬁeld of navigation, which is not yet the case for optimization based

ﬁltering techniques. In particular, the Invariant EKF has been already industrially implemented on a ﬂying

device presented in Section 5 and invariant Kalman ﬁltering may thus open up for novel robust industrial

SLAM algorithms.

(a) (b)

Figure 10: (a) The utility car used for the experiment is equipped with a Sick laser range and bearing

sensor, linear variable differential transformer sensor for the steering and back wheel odometer. Image

courtesy of J. Guivant and E. Nebot, Australian Centre for Field Robotics. (b) Trajectory (in meters) of the

car estimated by several SLAM algorithms and GPS measurements (Robocentric mapping ﬁlter’s estimates

were removed to improve readability).

7 CONCLUSION

The use of differential geometry and more precisely continuous symmetries for estimation emerged in the

non-linear automatic control ﬁeld decades ago. Attitude estimation, essentially for the control of unmanned

aerial vehicles (UAVs), has been a strong driver for the development of Lie-group based estimators over

the past ten years, the most popular attitude estimator probably being the non-linear complementary ﬁlter

of [43].

Ten years ago, invariant Kalman ﬁltering was introduced in [11] and has motivated many developments.

The invariant EKF has since been mathematically shown to come with convergence properties, it has been

implemented industrially for drone navigation, and it has recently been shown to resolve a major issue of the

EKF when applied to simultaneous localization and mapping (SLAM). SLAM is an important application,

that is sometimes considered as the “holy grail” of robotics, as it allows making a robot truly autonomous

[29]. Invariant Kalman ﬁltering opens up for novel implementations, as the EKF is a proven algorithm in

terms of implementation and validation, but previous attempts to apply EKF to SLAM were not satisfactory.

This review has aimed to provide an accessible introduction to the methodology of invariant Kalman

ﬁltering, and to allow the reader to gain insight into the relevance of the method, as well as what the

18

iSAM

σV

σ1 4 8

1 6.56 m 9.61 m 10.50 m

10 5.52 m 5.99 m 6.29 m

IEKF

σV

σ1 4 8

1 6.50 m 9.07 m 10.84 m

10 5.95 m 6.42 m 6.45 m

EKF

σV

σ148

1 6.54 m 123.01 m 34.5 m

10 6.32 m 10.04 m 13.2 m

Table 2: Position error (in meters) on the Victoria Park dataset for several noise tuning parameters. Pa-

rameter σ(in %) encodes model noise and σV(in m) observation noise. The trajectories returned by the

ﬁlters are rotated and translated to match at best the GPS measurements. The residual errors in meters are

reported. Note that, results achieved by IEKF and iSAM are very close, and weakly sensitive to (large) vari-

ations in the tuning parameters, contrarily to the EKF. This is another argument advocating the robustness

of Invariant Kalman ﬁltering.

important differences with the conventional EKF are. This should be of interest to readers intrigued by

the application of mathematical theories to practical applications, and also to readers interested in ﬁnding

simple to implement and robust ﬁlters for localization, navigation, and SLAM, notably for autonomous

vehicle guidance.

8 SUMMARY POINTS

1. Invariant Kalman ﬁltering is a recent methodology to design extended Kalman ﬁlters (EKFs) based

on alternative coordinates dictated by geometry.

2. The invariant EKF comes with convergence, stability, and robustness properties that the conventional

EKF lacks. It is yet reserved for a class of systems on Lie groups, or systems that are close to this

class.

3. The invariant EKF is particularly suited to localization and navigation of autonomous vehicles, and

has been successfully implemented in an industrial product of Safran Electronics & Defense, which

is the n◦1 company in Europe for inertial navigation systems.

4. The invariant EKF has recently been proved to resolve the inconsistencies of the EKF for the im-

portant application of SLAM in robotics. Although the standard EKF is the historical algorithm for

SLAM, it had mostly been abandoned due to its inconsistencies.

References

[1] Pieter Abbeel, Adam Coates, Michael Montemerlo, Andrew Y Ng, and Sebastian Thrun. Discrimina-

tive training of kalman ﬁlters. In Robotics: Science and systems, volume 2, page 1, 2005.

[2] Nasradine Aghannan and Pierre Rouchon. On invariant asymptotic observers. In Decision and

Control, 2002, Proceedings of the 41st IEEE Conference on, volume 2, pages 1479–1484, 2002.

19

[3] Victor Ayala and Juan Tirao. Linear control systems on Lie groups and controllability. In Proceedings

of symposia in pure mathematics, volume 64, pages 47–64. AMERICAN MATHEMATICAL SOCI-

ETY, 1999.

[4] Martin Barczyk and Alan F Lynch. Invariant observer design for a helicopter uav aided inertial navi-

gation system. IEEE Transactions on Control Systems Technology, 21(3):791–806, 2013.

[5] T. D. Barfoot and P. T. Furgale. Associating Uncertainty with Three-Dimensional Poses for Use in

Estimation Problems. IEEE Transactions on Robotics, 30(3):679–693, 2014.

[6] Alessandro Barp, Francois-Xavier Briol, Anthony D Kennedy, and Mark Girolami. Geometry and

dynamics for markov chain monte carlo. Annual Reviews of Statistics, 2018.

[7] A. Barrau and S. Bonnabel. The Invariant Extended Kalman Filter as a Stable Observer. IEEE

Transactions on Automatic Control, 62(4):1797–1812, 2017.

[8] Axel Barrau and Silvere Bonnabel. An EKF-SLAM Algorithm with Consistency Properties. Arxiv

preprint, 2015.

[9] Pedro Batista, Carlos Silvestre, and Paulo Oliveira. A GES attitude observer with single vector obser-

vations. Automatica, 48(2):388–395, 2012.

[10] Jan Bohn and Amit K Sanyal. Unscented State Estimation for Rigid Body Motion on SE(3). In

Decision and Control (CDC), 2012 IEEE 51st Annual Conference on, pages 7498–7503. IEEE, 2012.

[11] S. Bonnabel. Left-invariant extended Kalman ﬁlter and attitude estimation. In 2007 46th IEEE

Conference on Decision and Control, pages 1027–1032, 2007.

[12] S. Bonnabel, P. Martin, and P. Rouchon. Non-Linear Symmetry-Preserving Observers on Lie Groups.

IEEE Transactions on Automatic Control, 54(7):1709–1713, 2009.

[13] Silvere Bonnabel. Symmetries in observer design: Review of some recent results and applications to

EKF-based SLAM. In Robot Motion and Control 2011, pages 3–15. Springer, 2012.

[14] Silv`

ere Bonnabel, Philippe Martin, and Pierre Rouchon. Symmetry-Preserving Observers.

IEEE Transactions on Automatic Control, 53(11):2514–2526, 2008.

[15] Silv`

ere Bonnabel, Philippe Martin, and Erwan Sala¨

un. Invariant Extended Kalman

Filter: theory and application to a velocity-aided attitude estimation problem. In

48th IEEE Conference on Decision and Control, pages 1297–1304, Shanghai, China, 2009.

[16] Guillaume Bourmaud, R´

emi M´

egret, Marc Arnaudon, and Audrey Giremus. Continuous-Discrete

Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions. Journal

of Mathematical Imaging and Vision, 51(1):209–228, 2015.

[17] M Boutayeb, H Rafaralahy, and M Darouach. Convergence analysis of the extended Kalman ﬁlter

used as an observer for nonlinear deterministic discrete-time systems. IEEE transactions on automatic

control, 42(4):581–586, 1997.

[18] R. W. Brockett. Lie algebras and Lie groups in control theory. In Geometric methods in system theory,

pages 43–82. Springer, 1973.

[19] M. Brossard, S. Bonnabel, and J.P. Condomines. Unscented Kalman Filtering on Lie Groups. In

Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, 2017.

[20] Jos´

e A Castellanos, Jos´

e Neira, and Juan D Tard´

os. Limits to the consistency of EKF-based SLAM.

2004.

20

[21] G. S. Chirikjian and M. Kobilarov. Gaussian Approximation of Non-linear Measurement Models on

Lie Groups. In Decision and Control (CDC), 2014 IEEE 53rd Annual Conference on, pages 6401–

6406. IEEE, 2014.

[22] Gregory S. Chirikjian. Stochastic Models, Information Theory, and Lie Groups, Volume 1: Classical

Results and Geometric Methods. Applied and numerical harmonic analysis. Birkh¨

auser, 2009.

[23] Gregory S Chirikjian. Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic

Methods and Modern Applications. Springer Science & Business Media, 2011.

[24] Jean-Philippe Condomines, C´

edric Seren, and Gautier Hattenberger. Pi-invariant unscented Kalman

ﬁlter for sensor fusion. In Decision and Control (CDC), 2014 IEEE 53rd Annual Conference on, pages

1035–1040. IEEE, 2014.

[25] Jean-Philippe Condomines, C´

edric Seren, Gautier Hattenberger, et al. Nonlinear state estimation using

an invariant unscented Kalman ﬁlter. In AIAA Guidance Navigation and Control Conference, pages

1–15, 2013.

[26] G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark, and M. Csobra. A solution to the si-

multaneous localisation and mapping (SLAM) problem. IEEE Trans. Robot. Automat., 17:229–241,

2001.

[27] T.E. Duncan. An estimation problem in compact Lie groups. Systems & Control Letters, 10(4):257–

263, 1988.

[28] Tyrone E Duncan. Some ﬁltering results in Riemann manifolds. Information and Control, 35(3):182–

195, 1977.

[29] Hugh Durrant-Whyte and Tim Bailey. Simultaneous localization and mapping: part i. Robotics &

Automation Magazine, IEEE, 13(2):99–110, 2006.

[30] J. Forbes and D. E. Zotnik. Sigma Point Kalman Filtering on Matrix Lie Groups Applied to the SLAM

Problem. In Geometric Sciences of Information (GSI), 2017, Springer LNCS, 2017.

[31] Jose Guivant, Eduardo Nebot, and Hugh Durrant-Whyte. Simultaneous localization and map building

using natural features in outdoor environments. In Intelligent Autonomous Systems, volume 6, pages

581–586, 2000.

[32] Y. Han and F.C. Park. Least squares tracking on the Euclidean group. Automatic Control, IEEE

Transactions on, 46(7):1127–1132, 2001.

[33] Guoquan P Huang, Anastasios I Mourikis, and Stergios I Roumeliotis. Analysis and improvement of

the consistency of extended Kalman ﬁlter based SLAM. In Robotics and Automation, 2008. ICRA

2008. IEEE International Conference on, pages 473–479. IEEE, 2008.

[34] Guoquan P Huang, Anastasios I Mourikis, and Stergios I Roumeliotis. Observability-based rules

for designing consistent EKF SLAM estimators. The International Journal of Robotics Research,

29(5):502–528, 2010.

[35] Shoudong Huang and Gamini Dissanayake. Convergence and consistency analysis for extended

Kalman ﬁlter based SLAM. Robotics, IEEE Transactions on, 23(5):1036–1049, 2007.

[36] Kiyosi Itˆ

o. Stochastic differential equations in a differentiable manifold. Nagoya Mathematical

Journal, 1:35–47, 1950.

[37] Simon J Julier and Jeffrey K Uhlmann. A New Extension of the Kalman Filter to Nonlinear Systems.

In AeroSense’97, pages 182–193, 1997.

21

[38] Simon J Julier and Jeffrey K Uhlmann. A counter example to the theory of simultaneous localization

and map building. In Robotics and Automation, 2001. Proceedings 2001 ICRA. IEEE International

Conference on, volume 4, pages 4238–4243. IEEE, 2001.

[39] Michael Kaess, Ananth Ranganathan, and Frank Dellaert. isam: Incremental smoothing and mapping.

IEEE Transactions on Robotics, 24(6):1365–1378, 2008.

[40] Alireza Khosravian, Jochen Trumpf, Robert Mahony, and Christian Lageman. Observers for invari-

ant systems on Lie groups with biased input measurements and homogeneous outputs. Automatica,

55:19–26, 2015.

[41] Taeyoung Lee. Global Unscented Attitude Estimation via the Matrix Fisher Distributions on SO(3).

In American Control Conference (ACC), 2016, pages 4942–4947. IEEE, 2016.

[42] G. Loianno, M. Watterson, and V. Kumar. Visual Inertial Odometry for Quadrotors on SE(3). In 2016

IEEE International Conference on Robotics and Automation (ICRA), pages 1544–1551, 2016.

[43] Robert Mahony, Tarek Hamel, and Jean-Michel Pﬂimlin. Nonlinear complementary ﬁlters on the

special orthogonal group. IEEE Transactions on automatic control, 53(5):1203–1218, 2008.

[44] Philippe Martin, Erwan Sala¨

un, et al. Generalized multiplicative extended Kalman ﬁlter for aided

attitude and heading reference system. In AIAA Guidance, Navigation, and Control Conference, page

8300, 2010.

[45] Leonard A McGee and Stanley F Schmidt. Discovery of the Kalman ﬁlter as a practical tool for

aerospace and industry. 1985.

[46] SK Ng and PE Caines. Nonlinear ﬁltering in Riemannian manifolds. IMA journal of mathematical

control and information, 2(1):25–36, 1985.

[47] W. Park, Y. Liu, Y. Zhou, M. Moses, G. S. Chirikjian, et al. Kinematic State Estimation and Motion

Planning for Stochastic Nonholonomic Systems Using the Exponential Map. Robotica, 26(4):419–

434, 2008.

[48] M. Pontier and J. Szpirglas. Filtering on manifolds. In Stochastic Modelling and Filtering, pages

147–160. Springer, 1987.

[49] Salem Said and Jonathan H Manton. On ﬁltering with observation in a manifold: Reduction to a

classical ﬁltering problem. SIAM Journal on Control and Optimization, 51(1):767–783, 2013.

[50] Glauco Garcia Scandaroli, Pascal Morin, and Geraldo Silveira. A nonlinear observer approach for

concurrent estimation of pose, imu bias and camera-to-imu rotation. In Intelligent Robots and Systems

(IROS), 2011 IEEE/RSJ International Conference on, pages 3335–3341, 2011.

[51] Yongkyu Song and Jessy W Grizzle. The extended Kalman ﬁlter as a local asymptotic observer for

nonlinear discrete-time systems. In American Control Conference, 1992, pages 3365–3369, 1992.

[52] Robert F Stengel. Optimal Control and Estimation. Courier Corporation, 1986.

[53] Abdelhamid Tayebi, Stephen McGilvray, A Roberts, and M Moallem. Attitude estimation and stabi-

lization of a rigid body using low-cost sensors. In Decision and Control, 2007 46th IEEE Conference

on, pages 6424–6429, 2007.

[54] Sebastian Thrun and Michael Montemerlo. The graph SLAM algorithm with applications to large-

scale mapping of urban structures. The International Journal of Robotics Research, 25(5-6):403–429,

2006.

22

[55] A. S. Willsky. Some estimation problems on Lie groups. In D.Q. Mayne and R.W. Brockett, editors,

Geometric Methods in System Theory, volume 3 of NATO Advanced Study Institutes Series, pages

305–314. Springer Netherlands, 1973.

[56] A. S. Willsky and S. I. Marcus. Estimation for bilinear stochastic systems. In Variable Structure

Systems with Application to Economics and Biology, pages 116–137. Springer, 1975.

[57] A.S. Willsky. Dynamical Systems Deﬁned on Groups: Structural Properties and Estimation. PhD

thesis, MIT Dept. of Aeronautics and Astronautics, May 1973.

[58] K Wolfe, Michael Mashner, and G. S. Chirikjian. Bayesian Fusion on Lie groups. Journal of Algebraic

Statistics, 2(1):75–97, 2011.

[59] Mohammad Zamani, Jochen Trumpf, and Robert Mahony. Minimum-energy ﬁltering for attitude

estimation. IEEE Transactions on Automatic Control, 58(11):2917–2921, 2013.

[60] Teng Zhang, Kanzhi Wu, Jingwei Song, Shoudong Huang, and Gamini Dissanayake. Convergence

and consistency analysis for a 3-d invariant-EKF SLAM. IEEE Robotics and Automation Letters,

2(2):733–740, 2017.

[61] Teng Zhang, Kanzhi Wu, Daobilige Su, Shoudong Huang, and Gamini Dissanayake. An invariant-

EKF VINS algorithm for improving consistency. arXiv preprint arXiv:1702.07920, 2017.

23