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 filtering
Axel Barrau and Silvere Bonnabel
Annual Reviews of Control, Robotics, and Autonomous Systems,
Vol 1, pp: 237-257, 2018.
1
Invariant Kalman filtering
Axel Barrau1and Silv`
ere Bonnabel2
Contents
1 INTRODUCTION 3
1.1 Extended Kalman filtering (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 Groupaffinesystems ..................................... 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 benefits of the IEKF . . . . . . . . . . . . . . . . . 16
6.2 Experimentalresults ..................................... 17
7 CONCLUSION 18
8 SUMMARY POINTS 19
2
Abstract
The Kalman filter, or more precisely the extended Kalman filter (EKF), is a fundamental engineering
tool that is pervasively used in control, robotics, and for various estimation tasks in autonomous systems.
The recent field of Invariant extended Kalman filtering, 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 field 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 field. This review aims to provide an accessible introduction
to the methodology of invariant Kalman filtering, 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 finding simple to implement and robust filters for localization, navigation,
and SLAM, notably for autonomous vehicle guidance.
1 INTRODUCTION
The goal of a filter 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 filtering 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 filters in practice for control, robotics, and autonomous
systems. The extended Kalman filter (EKF) appeared in the 1960s with the advent of computers, and was
first 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 filters, 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 filtering (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 filter (UKF). In the aerospace industry it remains the reference filter.
1.1 Extended Kalman filtering (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 filter (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 defined. 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 defined 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 first-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 first-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 filter equations ensure that the updated error en|n=Xn−ˆ
Xn|nsatisfies 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 filter to inconsistencies and sometimes even divergence.
Algorithm 1 Extended Kalman Filter (EKF)
Choose an initial estimate ˆ
X0|0and uncertainty matrix P
0|0.
loop
Define Fn,Gnand Hnthrough (5) and (6).
Define 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 filtering problem defined 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 filter, 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 defined 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 filter (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 defined 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
filtering (IEKF). Section 4 is concerned with the mathematical guarantees that come with the IEKF. Sec-
tion 5 presents some real industrial applications to the field 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 defined 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 figure 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 oversimplification.)
Gthat satisfies γ(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 specific role. It is
denoted gand its dimension ddefines 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→Gdefined 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-fixed 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 infinitesimal 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 infinitesimal rotation, but ξis a vector of the body frame whereas
ζis expressed in the earth-fixed frame. It is easy to prove indeed that ζ=Rξ.
6
2.2 Uncertainty representation on matrix Lie groups
To define 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 define the probability distribution χ∼NL(¯
χ,P)as the probability
law of the random variable χ∈Gdefined as
χ=¯
χexp(ξ),ξ∼N(0,P),(8)
where N(., .)is the classical Gaussian distribution in Euclidean space Rd- identified 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 define 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 defined 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 filtering in [7, 16, 19]. They are sometimes referred to as concentrated Gaussians on
Lie groups. Note that, χin (8) and (9) is a well defined random variable of G. Computing its distribution
on Gis not trivial though, but for the purposes we pursue in invariant filtering it is absolutely not necessary.
Several approaches to filtering 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 specific 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 filtering 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 filtering consists of finding the path that best fits 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 filtering, 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 filter.
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 affine 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 filtering, 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 affine systems in [7]. Indeed if the group is merely
a vector space with addition as the group composition law, we recover the affine 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 affine 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 defined in a different context in [3].
Theorem 1 (Fundamental property of invariant filtering).We have the equivalence:
f satisfies (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 first 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 (flawless) 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 satisfied. 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 fsatisfies 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 define an alternative filter, 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 defined 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 defined. 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 fsatisfies (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 first-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 defined by
ηn−1|n−1=exp(ξn−1|n−1),ηn|n−1=exp(ξn|n−1).(17)
Let Fn∈Rd×dbe the defined by ∀ξ∈Rd
,g(exp(ξ),un) = exp(Fnξ). This matrix exists thanks to Theorem
1, and it is not defined as through first-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 first 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 first-order Taylor expansion in ξ∈Rdarbitrary,
allows defining 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 filter 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
Define Hnas in (19), and Fnthrough the expansion g(exp(ξ),un) = exp(Fnξ).
Define 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 defined similarly to
(19), through a first-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
defined 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 defined 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% confidence 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 confidence 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 difficult to some practitioners. This is one of the main reasons why the unscented extension of the
Kalman filter (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 field 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 filter’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 filter 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 filter. Indeed if the estimate is not sufficiently close to the true state,
the gain will be erroneous, and the correction applied by the filter (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 fsatisfies 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 filter 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 filter 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 significant 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 first 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 filter
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 filter 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 filtering 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 first applications of the EKF in the 1960s, and a driver for
scientific 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 filtering. The Euroflir 410
(Fig. 5) is the first commercial product implementing invariant Kalman filtering. 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: Euroflir 410 (left), last generation of ultra-long-range electro-optical system commercialized
by Safran. Its navigation system includes the first 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 fires.
Fig. 6 is obtained on real in-flight experimental data for hybrid INS-GPS navigation. The initial head-
ing error has been set deliberately to an extreme value (90◦) to obtain experimental confirmation 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 Euroflir 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 affine. 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-affine 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 filtering problem,
and the extended Kalman filter (EKF) based SLAM (the EKF-SLAM) one was of the first algorithm to be
used in this field. It was mostly abandoned though, due to inconsistencies of its estimates. However, it was
recently proved in [8] that invariant Kalman filtering 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-flight 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 benefits 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 filter’s output covariance matrix P
n|nto
correctly reflect 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 reflect 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 configuration 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 fixed 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 configuration, 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 confidence 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 first noticed in [13]. As proved in [8] this defines 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
configurations of Fig. 8 (b), so {Ψα(ˆ
Xn|n−1),α∈R}defines 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)defines
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 reflects
the unobservability there, that is, the confidence ellipsoid defined 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 confidence 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 filtering approach for SLAM. The
idea is roughly to formulate the problem of finding 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 finds 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
filter 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 filters: 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 difficult 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 field of navigation, which is not yet the case for optimization based
filtering techniques. In particular, the Invariant EKF has been already industrially implemented on a flying
device presented in Section 5 and invariant Kalman filtering 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 filter’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 field 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 filter
of [43].
Ten years ago, invariant Kalman filtering 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 filtering 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
filtering, 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
filters 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 filtering.
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 finding
simple to implement and robust filters for localization, navigation, and SLAM, notably for autonomous
vehicle guidance.
8 SUMMARY POINTS
1. Invariant Kalman filtering is a recent methodology to design extended Kalman filters (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 filters. 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 filter 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 filter
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
filter 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 filter. 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 filtering 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 filter 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 filter 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 Pflimlin. Nonlinear complementary filters 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 filter 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 filter as a practical tool for
aerospace and industry. 1985.
[46] SK Ng and PE Caines. Nonlinear filtering 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 filtering with observation in a manifold: Reduction to a
classical filtering 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 filter 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 Defined 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 filtering 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
