ArticlePDF Available

Abstract and Figures

The Kalman filter—or, more precisely, the extended Kalman filter (EKF)—is a fundamental engineering tool that is pervasively used in control and robotics and for various estimation tasks in autonomous systems. The recently developed field of invariant extended Kalman filtering uses 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 fields of localization, navigation, and simultaneous localization and mapping (SLAM). Although it was created only recently, 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 readers to gain insight into the relevance of the method as well as its important differences with the conventional EKF. This should be of interest to readers intrigued by the practical application of mathematical theories and those interested in finding robust, simple-to-implement filters for localization, navigation, and SLAM, notably for autonomous vehicle guidance. Expected final online publication date for the Annual Review of Control, Robotics, and Autonomous Systems Volume 1 is May 28, 2018. Please see for revised estimates.
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.
Invariant Kalman filtering
Axel Barrau1and Silv`
ere Bonnabel2
1.1 Extended Kalman filtering (EKF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Motivation for the use of geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 Outline ............................................ 5
2.1 MatrixLieGroups ...................................... 5
2.2 Uncertainty representation on matrix Lie groups . . . . . . . . . . . . . . . . . . . . . . . 7
3.1 Groupafnesystems ..................................... 7
3.2 InvariantEKFmethodology ................................. 8
3.3 Geometricinsight....................................... 10
3.4 Unscentedversion....................................... 11
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
6.1 EKF-based SLAM inconsistency and benefits of the IEKF . . . . . . . . . . . . . . . . . 16
6.2 Experimentalresults ..................................... 17
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.
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>0Rpwhich are measurement data returned by sensors.
The trusted evolution model is:
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 unRma 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-
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
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:
n|n). To compute the mean and covariance, the EKF uses a two step procedure.
Step 1 - Propagation: The estimate ˆ
Xn1|n1obtained after the observationYn1, is propagated through
the deterministic part of (1):
To compute the associated covariance, introduce the estimation errors defined as
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 ˆ
Xn1|n1. Indeed, using the Jacobians Fn=f
Xn1|n1,un,0), and Hn=h
Xn|n1), the combination of equations (1), (2) and (3) yields the
following first-order expansion of the error system:
Xn|n1) = Hnen|n1+Vn,(6)
where the second order terms, that is, terms of order O||e||2
,||e||||w||have been removed, see e.g.,
[52]. P
n1|n1is an approximation to the true covariance E(en1|n1eT
n1|n1), and it is propagated through
the linearized model (5) so that P
nis an approximation of E(en|n1eT
and we have P(Xn|u1:n,Y1:n1)N(ˆ
Step 2 - Measurement update: To account for the measurement Yn, we let zn=Ynh(ˆ
Xn|n1), and
znis called the innovation. Assuming that en|n1N(0,P
n|n1)and the approximation (6) to be exact,
the linear Kalman filter equations ensure that the updated error en|n=Xnˆ
Xn|nsatisfies en|nN(0,P
Xn|n1+Knzn,and P
n|n= [IKnHn]P
with Kn, called the Kalman gain, computed in Algorithm 1. Of course the belief after update 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
Define Fn,Gnand Hnthrough (5) and (6).
Define Qnas Cov(wn)and Rnas Cov(Vn).
Measurement update
Compute zn=Ynhˆ
n|n= [IKnHn]P
end loop
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.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:
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 γ:RGis 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.
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 gMN(R), but there always is an invertible map
Rdgthat 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:gGconstitutes a bijection from a neighborhood Vg
of 0 to a neighborhood of the identity INin G. In this paper, we will call the Lie exponential map the
map exp : RdGdefined 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(ξ,ζ) = exp1(exp(ξ)exp(ζ)). In particular it ensures exp(ξ)exp(ζ) = exp(ξ+ζ+
T), where Tis of the order O(kξk2
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ξ.
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
where N(., .)is the classical Gaussian distribution in Euclidean space Rd- identified to the Lie algebra
gand PRd×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(¯
for right multiplication of ¯
χthrough the random variable
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].
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:
with fa smooth map on the group, unRm, or possibly unG(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
Example 2. Let us continue our simple example of rotation matrices. Consider a motion R0,R1,··· on
SO(3)with Rn=Rn1Un. The matrix UnSO(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=U1ν1χU=U1(ν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 U1exp(ξ)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 χnG
associated to a sequence of observations (Yn)n>0Rpas follows :
Yn=h(χn) +Vn,(13)
where unRdis a control input, wnRdis a (unknown) vector encoding the process noise, VnRp
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|nGand P
n|nRd×d, and makes the approximation
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
Step 1 - Propagation: The IEKF propagates an estimate obtained after the previous observation Yn1
through the deterministic part of (12), i.e., by setting wn=0:
To compute the associated covariance, introduce the left-invariant estimation errors defined as
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|n1= [ f(χn1,un)exp(wn)]1f(ˆ
χn1|n1,un) = exp(wn)g(ηn1|n1,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 ξn1|n1,ξn|n1Rd
be defined by
Let FnRd×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(ξn1|n1),un) = exp(Fnξn1|n1wn)up to terms of
order kξn1|n1k2
,kξn1|n1kkwnk. Recalling (16) and (17), this means that up to the first order
exp(ξn|n1) = exp(Fnξn1|n1wn)so we get the following linearized equation, which is the Lie group
counterpart of (5)
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 ˆ
χn1|n1. This is in contrast with the conventional EKF
where Fnin (5) depends on ˆ
Xn1|n1. This is a consequence of requirement (11) and Thm. 1, and it will
play an important role in the sequel.
We have exp(ξn|n1) = ηn|n1=χ1
χn|n1, which implies that χn=ˆ
χn|n1exp(ξn|n1). Resorting
to the uncertainty representation (8), and using (18), we have just proved that if P(χn1|u1:n1,Y1:n1)
n1|n1)then we have approximately the propagated distribution P(χn|u1:n,Y1:n1)NL(ˆ
where P
n+Qn, and where Qn=Cov(wn) = Cov(wn).
Step 2 - Measurement update To account for the new measurement we let zn=Ynh(ˆ
χn|n1)be the
innovation. It is a vector of Rp. We have zn=h(χn)h(ˆ
Vn. As ξn|n1is assumed small, and as exp(0) = IN, a first-order Taylor expansion in ξRdarbitrary,
allows defining Hnas follows
χn|n1exp(ξ)) h(ˆ
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|n1. 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|n1exp(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|0Gand P
Define Hnas in (19), and Fnthrough the expansion g(exp(ξ),un) = exp(Fnξ).
Define Qnas Cov(wn)and Rnas Cov(Vn).
Measurement update
Compute zn=Ynhˆ
n|n= [IKnHn]P
end loop
Remark 2. Many systems of interest are such that the observation (13) is of the form Yn=χnb+Vnwhere
bRNis 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=ˆ
n|n1Ynb, which is equal to η1
n|n1Vn. Then, since
n|n1b=exp(ξn|n1)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(ξ)1bb:=
Hnξ+O(kξk2). We then see that, along the lines of Remark 1, the matrix ˜
Hndoes not depend on ˆ
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˜
Vnwhere we have let
n|n1Vn, which is a noise having covariance matrix ˆ
n|n1. On the other we have
defined zn=Ynˆ
χn|n1bHnξ+Vn.But we also have zn=ˆ
χn|n1˜zn, proving
that Hn=ˆ
Hn. The gain in Algorithm 2 is defined as the Kalman gain for the linearized innovation
znHnξ+Vn, that is,
For the alternative innovation ˜znof [7] the Kalman gain for the linearized system ˜zn˜
and we see that ˜
Hn=KnHnand we also see that ˜
χn|n1˜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.
Table 1: Differences between conventional EKF and Invariant EKF
Name Expression Nature Expression Nature
State Xnvector of Rdˆ
χnmatrix of GRN×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 GRN×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ˆ
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.
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(Xn1,un),Yn=h(Xn). A stable non-linear observer is a dynamical system of the form ˆ
where ˆ
fis designed to achieve asymptotic convergence of the estimation error, that is, ˆ
XnXn0 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
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).
Innovation zn=Ynh(ˆ
Extended Kalman filter Output map
Gain computation
Input unMeasurement Yn
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(χn1,un)and assume fsatisfies condition (11). As in Remark 2, assume also that the observation
(with noise turned off) writes Yn=h(χn) = χnbwith bRNa known vector. As emphasized by Remarks 1
and 2, neither Fnnor Hnthen depend on the estimates. This makes the gain computation independent from
Alternative innovation zn=ˆ
Invariant extended Kalman filter Output map
Gain computation
Input unMeasurement Yn
Kndoes not exist anymore
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].
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 n1 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.
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.
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].
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.
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
correctly reflect the true error dispersion E(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 xnR2be the position of the robot , θnits orientation, with respect to a global frame,
and let p1,p2,· ·· ,pKR2be 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(ˆ
n|n1)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].
(a) (b)
Figure 9: Difference between the conventional EKF and invariant EKF update steps with respect to un-
observable directions. (a) EKF-SLAM:Ψα(ˆ
Xn|n1)and ˆ
Xn|n1correspond to the two undistinguishable
configurations of Fig. 8 (b), so {Ψα(ˆ
Xn|n1),αR}defines a continuous curve of states undistinguishable
from ˆ
Xn|n1in the state space. In terms of linearized model, this means Ψ0
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|n1. 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|n1), it is then moved over to updated state ˆ
Xn|nthrough a simple translation.
But Ψ0
Xn|n)which means the updated belief N(Xn|n,P
n|n)is elongated in the direction
Xn|n1)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
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 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
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).
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
σ1 4 8
1 6.56 m 9.61 m 10.50 m
10 5.52 m 5.99 m 6.29 m
σ1 4 8
1 6.50 m 9.07 m 10.84 m
10 5.95 m 6.42 m 6.45 m
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.
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
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 n1 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.
[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.
[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.
[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,
[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.
[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,
[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.
... Within the context of navigation, recursive attitude estimation is addressed with nonlinear extensions of the original Kalman Filter (KF), such as the Error State KF (ESKF) [1], [2] or the Invariant KF (IKF) [3]- [5]. The aforementioned filters preserve the geometrical properties for the attitude estimates or, conversely, assure that the sequence of estimates remain in the corresponding manifold. ...
... The underlying concept relates to estimating a vector of perturbations around the state on the algebra associated with the target manifold. For threedimensional attitude systems, for instance, that manifold is SO(3), while for the rotation-translation pair is SE (3). When it comes to sensor integration, attitude systems typically include relative (i.e., angular rate) and absolute (i.e., observation models for the orientation) attitude information. ...
... Meanwhile, Barrau et al. [30], [31] introduced the invariant extended Kalman filter (InEKF), which shows the faster convergence of the observable states than the conventional extended Kalman filter [1] when an initial error exists by using Group-Affine property. Hartley et al. [32] discovered that the proprioceptive legged robot state estimation system is approximately group-affine and hence proposed the InEKF. ...
... In this equation, the error dynamics tainted by noise and the deterministic dynamics of the state propagation are separated and then discretized individually using g d (·) and f d (·). This approach approxi-mates the exact log-linearity directly derived from the discrete domain, which has been exploited in [31], [33]. However, it provides a useful way to get discrete-domain equations rooted in the continuous-domain group-affine property when it is challenging to obtain the discrete-domain group-affine propagation equation. ...
Full-text available
This work proposes an invariant smoother for legged robot state estimation with the measurement of an inertial measurement unit (IMU) and leg kinematics while assuming static foot contact. Because the proposed smoother is formulated with the residual functions with group-affine property, their Jacobians become independent from current state estimates. These state-independent Jacobians lead to better convergence properties in optimizing the cost in the smoother, especially under dynamic contact events. The proposed Slip Rejection method increases the uncertainty of static contact assumption when the robot has dynamic contact events. The estimated foot velocity, which is utilized to detect the dynamic contact events, is re-evaluated within the preserving time window. We also propose the Contact Loop method, a new measurement model asserting that foot position remains constant over multiple timesteps during stable contact. The proposed estimator is tested through online experiments, including indoor and 160 m-long outdoor experiments, and compared against state-of-the-art algorithms.
... The study of online estimation using Kalman filters and related techniques is a well-developed discipline with numerous introductory textbooks [7], [8]. Since its original formulation [2] in 1960, the Kalman filter has inspired many reformulations including sigma point (unscented) [9], ensemble [10], invariant [11] Kalman-type filters, and H ∞ filters [12]. In this work, we derive an extended Kalman filter for the gate set tomography estimation problem, which may be seen as a first step towards more sophisticated techniques. ...
... Currently IEKF has been used in a variety of integrated navigation systems [7,8,16,17]. The essence of the above two filtering algorithms used in SINS/GNSS can be understood as redefining the nonlinear error state which satisfies the linear differential equation to solve the linearization error problem in traditional SINS/GNSS based on KF [18,19]. However, the initialization of ECM corresponding to the transformed error state has not attracted enough attention. ...
Full-text available
The traditional Strapdown Inertial Navigation System (SINS)/Global Navigation Satellite System (GNSS) integrated system uses standard Kalman Filter (KF) to estimate the error states, which weakens the correlation between the different error components to directly initialed the Error Covariance Matrix (ECM) into diagonalization. This initialed method is also widely applied in State Transformation Extended Kalman Filter (STEKF) and Invariant Extended Kalman Filter (IEKF), which results in state estimation failing to achieve the optimal performance. To solve this problem, this paper first analyses the transformed relationship from traditional linear error state to the nonlinear error state redefined in STEKF and IEKF, and the strong correlation is found between the redefined error state components, namely the ECM in STEKF or IEKF no longer appears as a diagonal matrix. Then, aiming at the nonlinear error states, the transformed models of ECM in STEKF and IEKF are derived respectively, which establishes the theoretical basis for ECM initialization based on the error states correlation. Finally, the accuracy, feasibility and general applicability of the proposed method are verified by a boat-mounted field trial.
Due to the complexity and uncertainty of the actual working environments, relying solely on proprioceptive sensors to obtain accurate floating base and center of mass (CoM) estimates is of great significance for biped robots. In this paper, a biped locomotion state estimator aided by both CoM dynamics and leg forward kinematics is proposed. The main contribution of this estimator is the use of contact force measurements that are not considered in existing methods. Contact force measurements can be used to predict CoM motions and update the floating base estimates with CoM forward kinematics. Compared with the leg forward kinematics, the CoM dynamics prediction and the CoM forward kinematics update are more robust to contact slippage and highly dynamic motions. The simulation results show that the estimator proposed in this paper improves the estimation accuracy of the floating base in the slippage direction under various reference speeds. In addition, the introduction of CoM dynamics enables the filter to directly output the CoM state in the world coordinate system, avoiding the complicated cascade design of the existing CoM state estimators.
Dynamic state estimation, as opposed to kinematic state estimation, seeks to estimate not only the orientation of a rigid body but also its angular velocity, through Euler's equations of rotational motion. This paper demonstrates that the dynamic state estimation problem can be reformulated as estimating a probability distribution on a Lie group defined on phase space (the product space of rotation and angular momentum). The propagation equations are derived non-parametrically for the mean and covariance of the distribution. It is also shown that the equations can be approximately solved by ignoring the third and higher moments of the probability distribution. Numerical experiments show that the distribution constructed from the propagated mean and covariance fits the sample data better than an extended Kalman filter.
Full-text available
In this article, a tutorial introduction to visual-inertial navigation(VIN) is presented. Visual and inertial perception are two complementary sensing modalities. Cameras and inertial measurement units (IMU) are the corresponding sensors for these two modalities. The low cost and light weight of camera-IMU sensor combinations make them ubiquitous in robotic navigation. Visual-inertial Navigation is a state estimation problem, that estimates the ego-motion and local environment of the sensor platform. This paper presents visual-inertial navigation in the classical state estimation framework, first illustrating the estimation problem in terms of state variables and system models, including related quantities representations (Parameterizations), IMU dynamic and camera measurement models, and corresponding general probabilistic graphical models (Factor Graph). Secondly, we investigate the existing model-based estimation methodologies, these involve filter-based and optimization-based frameworks and related on-manifold operations. We also discuss the calibration of some relevant parameters, also initialization of state of interest in optimization-based frameworks. Then the evaluation and improvement of VIN in terms of accuracy, efficiency, and robustness are discussed. Finally, we briefly mention the recent development of learning-based methods that may become alternatives to traditional model-based methods.
Visual-inertial odometry (VIO) is the problem of estimating a robot's trajectory by combining information from an inertial measurement unit (IMU) and a camera and is of great interest to the robotics community. This article develops a novel Lie group symmetry for the VIO problem and applies the recently proposed equivariant filter. The proposed symmetry is compatible with the invariance of the VIO reference frame, leading to improved filter consistency. The bias-free IMU dynamics are group-affine, ensuring that filter linearization errors depend only on the bias estimation error and measurement noise. Furthermore, visual measurements are equivariant with respect to the symmetry, enabling the application of the higher order equivariant output approximation to reduce the approximation error in the filter update equation. As a result, the equivariant filter based on this Lie group is a consistent estimator for VIO with lower linearization error in the propagation of state dynamics and a higher order equivariant output approximation than standard formulations. Experimental results on the popular EuRoC and UZH FPV datasets demonstrate that the proposed system outperforms other state-of-the-art VIO algorithms in terms of both speed and accuracy.
Full-text available
Markov Chain Monte Carlo methods have revolutionised mathematical computation and enabled statistical inference within many previously intractable models. In this context, Hamiltonian dynamics have been proposed as an efficient way of building chains which can explore probability densities efficiently. The method emerges from physics and geometry and these links have been extensively studied by a series of authors through the last thirty years. However, there is currently a gap between the intuitions and knowledge of users of the methodology and our deep understanding of these theoretical foundations. The aim of this review is to provide a comprehensive introduction to the geometric tools used in Hamiltonian Monte Carlo at a level accessible to statisticians, machine learners and other users of the methodology with only a basic understanding of Monte Carlo methods. This will be complemented with some discussion of the most recent advances in the field which we believe will become increasingly relevant to applied scientists.
Full-text available
In this paper, we first prove that the output of the standard EKF based Visual-Inertial Navigation System is not invariant under \textit{the stochastic unobservable transformation}, where the transformation refers to the rotation about the gravitational direction and translation. Due to this reason, the filter fails to mimic the invariance of the original system and thereby can produce the inconsistent estimate. To address this issue, we propose a new EKF based algorithm, namely, RIEKF-VINS that has the expected invariance property via a novel uncertainty representation. We then adapt RIEKF-VINS into the multi-state constraint Kalman filter(MSCKF) framework to remedy its inconsistency. Moreover, both Monte Carlo simulation and real-world experimental tests validate the proposed method.
Full-text available
—In this paper, we investigate the convergence and consistency properties of an Invariant-Extended Kalman Filter (RI-EKF) based Simultaneous Localization and Mapping (SLAM) algorithm. Basic convergence properties of this algorithm are proven. These proofs do not require the restrictive assumption that the Jacobians of the motion and observation models need to be evaluated at the ground truth. It is also shown that the output of RI-EKF is invariant under any stochastic rigid body transformation in contrast to SO(3) based EKF SLAM algorithm (SO(3)-EKF) that is only invariant under deterministic rigid body transformation. Implications of these invariance properties on the consistency of the estimator are also discussed. Monte Carlo simulation results demonstrate that RIEKF outperforms SO(3)-EKF, Robocentric-EKF and the “First Estimates Jacobian” EKF, for 3D point feature based SLAM.
Conference Paper
This paper considers sigma point Kalman filtering on matrix Lie groups. Sigma points that are elements of a matrix Lie group are generated using the matrix exponential. Computing the mean and covariance using the sigma points via weighted averaging and effective use of the matrix natural logarithm, respectively, is discussed. The specific details of estimating landmark locations, and the position and attitude of a vehicle relative to the estimated landmark locations, is considered.
The problem of the estimation of a Markovian (state) process by use of observations on a second related process has been a focus of research for many years. There have also been Ouncan, 1977) some initial steps towards the formulation of such non-linear filtering problems when the processes take their values in a manifold. In this paper a general formulation of the non-linear filtering problem in Riemannian manifolds is given by use of the strong solutions of the stochastic differential equations for the state and observation processes in the orthonormal frame bundles of the state and observation process manifolds respectively. Then a general Bayes' formula for the conditional expectation of smooth functions of the state process is given. This is used to give a direct derivation of the Zakai equation for the general problem under consideration. An example is presented.
The theory of stochastic differential equations in a differentiate manifold has been established by many authors from different view-points, especially by R Lévy [2], F. Perrin [1], A. Kolmogoroff [1] [2] and K. Yosida [1] [2]. It is the purpose of the present paper to discuss it by making use of stochastic integrals.