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 http://www.annualreviews.org/page/journal/pubdates 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.
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 Groupafnesystems ..................................... 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>0Rpwhich are measurement data returned by sensors.
The trusted evolution model is:
Xn=f(Xn1,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 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-
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 ˆ
Xn1|n1obtained after the observationYn1, is propagated through
the deterministic part of (1):
ˆ
Xn|n1=f(ˆ
Xn1|n1,un,0)(3)
To compute the associated covariance, introduce the estimation errors defined as
en1|n1=Xn1ˆ
Xn1|n1,en|n1=Xnˆ
Xn|n1.(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 ˆ
Xn1|n1. Indeed, using the Jacobians Fn=f
X(ˆ
Xn1|n1,un,0),
Gn=f
w(ˆ
Xn1|n1,un,0), and Hn=h
X(ˆ
Xn|n1), the combination of equations (1), (2) and (3) yields the
following first-order expansion of the error system:
en|n1=Fnen1|n1+Gnwn,(5)
Ynh(ˆ
Xn|n1) = Hnen|n1+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
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
n|n1=FnP
n1|n1FT
n+GnQnGT
nis an approximation of E(en|n1eT
n|n1),
and we have P(Xn|u1:n,Y1:n1)N(ˆ
Xn|n1,P
n|n1).
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
n|n)
where
ˆ
Xn|n=ˆ
Xn|n1+Knzn,and P
n|n= [IKnHn]P
n|n1(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|n1=fˆ
Xn1|n1,un,0
P
n|n1=FnP
n1|n1FT
n+GnQnGT
n
Measurement update
Compute zn=Ynhˆ
Xn|n1,Sn=HnP
n|n1HT
n+Rn,Kn=P
n|n1HT
nS1
n
P
n|n= [IKnHn]P
n|n1
ˆ
Xn|n=ˆ
Xn|n1+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:
ING,χG,χ1G,χ1,χ2G,χ1χ2G,
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.
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 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
,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 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(¯
χ,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(χn1,un)(10)
7
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
result.
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 :
χn=f(χn1,un)exp(wn)(12)
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
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 Yn1
through the deterministic part of (12), i.e., by setting wn=0:
ˆ
χn|n1=f(ˆ
χn1|n1,un)(14)
To compute the associated covariance, introduce the left-invariant estimation errors defined as
ηn1|n1:=χ1
n1ˆ
χn1|n1,ηn|n1:=χ1
nˆ
χn|n1.(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|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
ηn1|n1=exp(ξn1|n1),ηn|n1=exp(ξn|n1).(17)
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
,kwnk2
,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)
ξn|n1=Fnξn1|n1wn.(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 ˆ
χ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ˆ
χ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)
NL(ˆ
χn1|n1,P
n1|n1)then we have approximately the propagated distribution P(χn|u1:n,Y1:n1)NL(ˆ
χn|n1,P
n|n1)
where P
n|n1=FnP
n1|n1FT
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(ˆ
χn|n1)+Vn=h(ˆ
χn|n1exp(ξn|n1))h(ˆ
χn|n1)+
Vn. As ξn|n1is assumed small, and as exp(0) = IN, a first-order Taylor expansion in ξRdarbitrary,
allows defining Hnas follows
h(ˆ
χn|n1exp(ξ)) h(ˆ
χn|n1):=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|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|n=ˆ
χ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
0|0Rd×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|n1=f(ˆ
χn1|n1,un)
P
n|n1=FnP
n1|n1F1
n+Qn
Measurement update
Compute zn=Ynhˆ
χn|n1,Sn=HnP
n|n1HT
n+Rn,Kn=P
n|n1HT
nS1
n
P
n|n= [IKnHn]P
n|n1
ˆ
χn|n=ˆ
χn|n1exp(Knzn)
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=ˆ
χ1
n|n1Ynb, which is equal to η1
n|n1bb+ˆ
χ1
n|n1Vn. Then, since
η1
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 ˆ
χn|n1
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|n1Vn=˜
Hnξ+ˆ
Vnwhere we have let
ˆ
Vn=ˆ
χ1
n|n1Vn, which is a noise having covariance matrix ˆ
Rn=ˆ
χ1
n|n1Rnˆ
χT
n|n1. On the other we have
defined zn=Ynˆ
χn|n1bHnξ+Vn.But we also have zn=ˆ
χn|n1ˆ
χ1
n|n1Ynb=ˆ
χn|n1˜zn, proving
that Hn=ˆ
χn|n1˜
Hn. The gain in Algorithm 2 is defined as the Kalman gain for the linearized innovation
znHnξ+Vn, that is,
Kn=P
n|n1HT
nHnP
n|n1HT
n+Rn1
,
For the alternative innovation ˜znof [7] the Kalman gain for the linearized system ˜zn˜
Hnξ+ˆ
Vnwrites
˜
Kn=P
n|n1˜
HT
n˜
HnP
n|n1˜
HT
n+ˆ
Rn1=P
n|n1˜
HT
nˆ
χT
n|n1HnP
n|n1HT
n+Rn1ˆ
χn|n1
and we see that ˜
Kn˜
Hn=KnHnand we also see that ˜
Kn˜zn=P
n|n1HT
nHnP
n|n1HT
n+Rn1(ˆ
χ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.
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 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ˆ
χ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(Xn1,un),Yn=h(Xn). A stable non-linear observer is a dynamical system of the form ˆ
Xn=ˆ
f(ˆ
Xn1,un,Yn)
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
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=Ynh(ˆ
Xn|n1)
Extended Kalman filter Output map
Gain computation
Input unMeasurement Yn
ˆ
Xn|n1
h(ˆ
Xn|n1)
Knˆ
Xn1|n1,ˆ
Xn|n1
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(χ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
12
System
Alternative innovation zn=ˆ
χ1
nYnb
Invariant extended Kalman filter Output map
Gain computation
Input unMeasurement Yn
ˆ
χn|n1
h(ˆ
χn|n1)
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 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.
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 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(ˆ
Xn|n,P
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].
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|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
α(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|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|n1)6=Ψ0
α(ˆ
Xn|n)which means the updated belief N(Xn|n,P
n|n)is elongated in the direction
Ψ0
α(ˆ
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
α(ˆ
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 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.
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
... In parallel, Lie group embeddings have allowed for a new class of filters, see [12,30,39], and in particular the Invariant Extended Kalman Filter (IEKF) [11], in its modern form [5], see [6] for an overview. The IEKF possesses convergence guarantees [5], resolves the inconsistency issues of the EKF for SLAM, see [4] and following work [14,29,33,40]. ...
... For inertial navigation, combining the IEKF with the Lie group of double spatial direct isometries SE 2 (3), or extended poses, introduced in [5], leads to powerful results. In particular, it has led to patented products, see [3,6], and improved legged robot state estimation [28,36]. Besides their convergence properties as observers, invariant filters also gracefully accommodate navigation systems' uncertainty, see [13]. ...
... This is because the information about the length is not a hard constraint for the optimisation algorithm. Neither is it for IS, but the latter's descent step based on the invariant filtering framework [6] inherently respects this information. ...
Preprint
In this paper we address smoothing-that is, optimisation-based-estimation techniques for localisation problems in the case where motion sensors are very accurate. Our mathematical analysis focuses on the difficult limit case where motion sensors are infinitely precise, resulting in the absence of process noise. Then the formulation degenerates, as the dynamical model that serves as a soft constraint becomes an equality constraint, and conventional smoothing methods are not able to fully respect it. By contrast, once an appropriate Lie group embedding has been found, we prove theoretically that invariant smoothing gracefully accommodates this limit case in that the estimates tend to be consistent with the induced constraints when the noise tends to zero. Simulations on the important problem of initial alignement in inertial navigation show that, in a low noise setting, invariant smoothing may favorably compare to state-of-the-art smoothers when using precise inertial measurements units (IMU).
... The main cause of loss of accuracy for EKF methods relative to optimisation-based methods is associated with the accumulation of linearisation errors. Recent advances in the theory of equivariant systems [6,7] have shown that exploiting the Lie group symmetries of a system can lead to improved filter designs such as the invariant EKF (IEKF) and the Equivariant Filter (EqF) [8] that minimize linearisation error. ...
... Equivariant observers are state estimators that exploit available Lie group symmetries of a given problem. Two key examples include the invariant EKF (IEKF) [6] and the Equivariant Filter (EqF) [8]. The success of equivariant observers in other robotics problems has led several authors to investigate their application to inertial navigation, SLAM, and VIO. ...
... For the EqF, however, this linearisation is dependent on the chosen symmetry group G, rather than on a chosen set of coordinates as in the EKF. In some special cases, the system dynamics are group affine with respect to the symmetry group G, and this results in an exactly linear propagation of the error coordinates [6]. Barrau and Bonnabel [20] showed that the bias-free IMU dynamics are group affine with respect to the action (10) of SE 2 (3). ...
Preprint
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 paper develops a novel Lie group symmetry for the VIO problem and applies the recently proposed equivariant filter. The symmetry is shown to be compatible with the invariance of the VIO reference frame, lead to exact linearisation of bias-free IMU dynamics, and provide equivariance of the visual measurement function. As a result, the equivariant filter (EqF) based on this Lie group is a consistent estimator for VIO with lower linearisation 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.
... Variants of the EKF, such as the UKF [260], Multi-State Constraint Kalman Filter (MSCKF) [261,262]-which incorporates poses of past frames to marginalize features from the state space. Iterated EKF [263], Cubature Kalman Filter (CKF) [264], fuzzy logic KF [265], covariance intersection KF [266] and invariant EKF based on the Lie group [267], have been proposed in the literature. ...
... Thus, it has been used for Radar Odometry (RO), since both misdetection and false feature association can be commonly encountered in RO [132]. A comparison between PHD filters and state-of-the-art EKFs [267] was conducted in [283], demonstrating robustness of the PHD filter. A C (Cardinalized)-PHD filter propagates the distribution of the number of targets (cardinality) in addition to the PHD function; this auxiliary information enables higher modelling accuracy [284]. ...
Article
Full-text available
Although Global Navigation Satellite Systems (GNSSs) generally provide adequate accuracy for outdoor localization, this is not the case for indoor environments, due to signal obstruction. Therefore, a self-contained localization scheme is beneficial under such circumstances. Modern sensors and algorithms endow moving robots with the capability to perceive their environment, and enable the deployment of novel localization schemes, such as odometry, or Simultaneous Localization and Mapping (SLAM). The former focuses on incremental localization, while the latter stores an interpretable map of the environment concurrently. In this context, this paper conducts a comprehensive review of sensor modalities, including Inertial Measurement Units (IMUs), Light Detection and Ranging (LiDAR), radio detection and ranging (radar), and cameras, as well as applications of polymers in these sensors, for indoor odometry. Furthermore, analysis and discussion of the algorithms and the fusion frameworks for pose estimation and odometry with these sensors are performed. Therefore, this paper straightens the pathway of indoor odometry from principle to application. Finally, some future prospects are discussed.
... After that, the prediction is corrected using the newly acquired location information. In contrast to the centralized Kalman filter employed in [8], in this work, we propose a distributed prediction method, which allows each node to employ its own predictor, such as Kalman filter [37], invariant Kalman filter [38], or Square-root unscented Kalman filter (SRUKF) [39]. Theoretically, the improved versions of Kalman filtering perform better than the original Kalman filter, but the complexity is also higher. ...
... In principle, further improvements in the location accuracy of the estimated trajectories and locations are expected to be obtained by improved versions of Kalman filtering [38,39]. These filtering techniques have the potential to provide more stable location predictions at lower resolutions and higher QPs but will come with additional computational complexity. ...
Article
Full-text available
A novel low-power distributed Visual Sensor Network (VSN) system is proposed, which performs real-time collaborative barcode localization, tracking, and robust identification. Due to a dynamic triggering mechanism and efficient transmission protocols, communication is organized amongst the nodes themselves rather than being orchestrated by a single sink node, achieving lower congestion and significantly reducing the vulnerability of the overall system. Specifically, early detection of the moving barcode is achieved through a dynamic triggering mechanism. A hierarchical transmission protocol is designed, within which different communication protocols are used, depending on the type of data exchanged among nodes. Real-Time Transport Protocol (RTP) is employed for video communication, while the Transmission Control Protocol (TCP) and Long Range (LoRa) protocol are used for passing messages amongst the nodes in the VSN. Through an extensive experimental evaluation, we demonstrate that the proposed distributed VSN brings substantial advantages in terms of accuracy, power savings, and time complexity compared to an equivalent system performing centralized processing.
... In this paper, we analyze the influence of the noise on the logarithmic invariant error propagation via the derivative of the matrix exponential. When the left Jacobians of the error are approximated to the identity by first-order linearization, our result boils down to results in [4], [11], [15], [17]. More about the linearization approximation in SE 2 (3) will be discussed in Section IV-B. ...
Preprint
Pose estimation is important for robotic perception, path planning, etc. Robot poses can be modeled on matrix Lie groups and are usually estimated via filter-based methods. In this paper, we establish the closed-form formula for the error propagation for the Invariant extended Kalman filter (IEKF) in the presence of random noises and apply it to vision-aided inertial navigation. We evaluate our algorithm via numerical simulations and experiments on the OPENVINS platform. Both simulations and the experiments performed on the public EuRoC MAV datasets demonstrate that our algorithm outperforms some state-of-art filter-based methods such as the quaternion-based EKF, first estimates Jacobian EKF, etc.
... While other KF extensions for nonlinear systems (e.g., Unscented or Cubature KFs) can be also applied for attitude estimations, their added complexity does not justify their use and, therefore, they will not be covered within this thesis. The latest contributions on attitude estimation are related to the framework of Invariant Kalman Filtering (IKF), described in the series of work by Barrau and Bonnabel [111]- [113]. The IKF leverages on the geometric structure of Lie groups to provide convergence guarantees for linear systems with group-affine dynamics. ...
Thesis
Full-text available
Navigation information is an essential element for the functioning of robotic platforms and intelligent transportation systems. Among the existing technologies, Global Navigation Satellite Systems (GNSS) have established as the cornerstone for outdoor navigation, allowing for all-weather, all-time positioning and timing at a worldwide scale. GNSS is the generic term for referring to a constellation of satellites which transmit radio signals used primarily for ranging information. Therefore, the successful operation and deployment of prospective autonomous systems is subject to our capabilities to support GNSS in the provision of robust and precise navigational estimates. GNSS signals enable two types of ranging observations: --code pseudorange, which is a measure of the time difference between the signal's emission and reception at the satellite and receiver, respectively, scaled by the speed of light; --carrier phase pseudorange, which measures the beat of the carrier signal and the number of accumulated full carrier cycles. While code pseudoranges provides an unambiguous measure of the distance between satellites and receiver, with a dm-level precision when disregarding atmospheric delays and clock offsets, carrier phase measurements present a much higher precision, at the cost of being ambiguous by an unknown number of integer cycles, commonly denoted as ambiguities. Thus, the maximum potential of GNSS, in terms of navigational precision, can be reach by the use of carrier phase observations which, in turn, lead to complicated estimation problems. This thesis deals with the estimation theory behind the provision of carrier phase-based precise navigation for vehicles traversing scenarios with harsh signal propagation conditions. Contributions to such a broad topic are made in three directions. First, the ultimate positioning performance is addressed, by proposing lower bounds on the signal processing realized at the receiver level and for the mixed real- and integer-valued problem related to carrier phase-based positioning. Second, multi-antenna configurations are considered for the computation of a vehicle's orientation, introducing a new model for the joint position and attitude estimation problems and proposing new deterministic and recursive estimators based on Lie Theory. Finally, the framework of robust statistics is explored to propose new solutions to code- and carrier phase-based navigation, able to deal with outlying impulsive noises.
... We notice that, the filters designed on matrix Lie groups that exploit a group-affine system dynamics structure tend to perform better as consistent estimators than the standard EKF and OCEKF counterparts. Future works may exploit the group-affine property for discrete-time systems [14] to extend DILIGENT-KIO to achieve autonomous error propagation. ...
Preprint
Extended Kalman filtering is a common approach to achieve floating base estimation of a humanoid robot. These filters rely on measurements from an Inertial Measurement Unit (IMU) and relative forward kinematics for estimating the base position-and-orientation and its linear velocity along with the augmented states of feet position-and-orientation, thus giving them their name, flat-foot filters. However, the availability of only partial measurements often poses the question of consistency in the filter design. In this paper, we perform an experimental comparison of state-of-the-art flat-foot filters based on the representation choice of state, observation, matrix Lie group error and system dynamics evaluated for filter consistency and trajectory errors. The comparison is performed over simulated and real-world experiments conducted on the iCub humanoid platform.
... This section reviews the theory and some basic properties of Lie groups used in this work. The following contents are chiefly based on [22,37] and partly on [15]. ...
Article
Full-text available
This paper investigates the numerical integration error calibration problem in Lie group sigma point filters to obtain more accurate estimation results. On the basis of the theoretical framework of the Bayes–Sard quadrature transformation, we first established a Bayesian estimator on matrix Lie groups for system measurements in Euclidean spaces or Lie groups. The estimator was then employed to develop a generalized Bayes–Sard cubature Kalman filter on matrix Lie groups that considers additional uncertainties brought by integration errors and contains two variants. We also built on the maximum likelihood principle, and an adaptive version of the proposed filter was derived for better algorithm flexibility and more precise filtering results. The proposed filters were applied to the quaternion attitude estimation problem. Monte Carlo numerical simulations supported that the proposed filters achieved better estimation quality than that of other Lie group filters in the mentioned studies.
Article
Video-based object detection plays an important role in the real world and scientific research. Compared with still images, video detection is more challenging due to occlusion, rare poses, high-speed movement, frames loss, etc. In order to improve the existing video stream detectors widely and with low coupling, a post-processing strategy, CFPP, is proposed in this work. The framework can establish a cross frame link based on deep learning, connect the proposals belonging to the same object, and improve the performance of the detector by optimizing the classification confidence and object coordinates. Furthermore, CFPP can connect the proposals in adjacent and non adjacent frames at the same time, which makes it exploit the context information of video stream more effectively than other post-processing strategies. Experiments shows that CFPP can improve the existing detectors (e.g. we improve the mAP of YOLOv4 on ImageNet VID dataset form 69.24% to 78.15%). In addition, experiments show that the designed framework can achieve better detection effect than other strategies in the case of high-speed moving object and frames loss.
Article
This article proposes a novel nonlinear state estimation framework for humanoid robots based on the dynamics of the center of mass (CoM) and dual-loop Kalman filter(DLKF). It effectively fuses the information from inertial measurement unit(IMU), joint encoders, and foot sensitive resistors (FSRs), and provides state estimates for various gait generation algorithms and dynamic balance controllers with CoM or divergence component of motion (DCM) feedback. Compared to the widely used linear models such as the linear inverted pendulum model (LIPM), the nonlinear dynamics of CoM effectively reduce the process error. However, the humanoid robot is a highly complex dynamic system with multiple links and joints, the dynamics of CoM are also a simplification of the whole body dynamics. As a result, it brings non-zero-mean, non-Gaussian, and correlated process error, which the conventional extend Kalman filter (EKF) cannot handle. To this end, the DLKF is adopted to compensate the process error, thus the estimator is robust to the modeling error caused by simplifications. Furthermore, the invariant extended Kalman filter (IEKF) is used for floating base kinematics estimation, and the force/torque (F/T) sensor which is difficult to integrate on smaller or cheaper robots due to the size and cost is not used in our framework. Finally, our nonlinear state estimation framework improves the accuracy of CoM and DCM estimation in a virtual environment simulation using our self-developed Defensor hydraulic humanoid robot.
Article
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.
Article
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.
Article
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.
Article
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.
Article
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.