arXiv:1510.06263v3 [cs.RO] 2 Sep 2016

An EKF-SLAM algorithm with consistency
properties
Axel Barrau, Silvère Bonnabel∗
September 5, 2016

Abstract
In this paper we address the inconsistency of the EKF-based SLAM algorithm
that stems from non-observability of the origin and orientation of the global reference frame. We prove on the non-linear two-dimensional problem with point
landmarks observed that this type of inconsistency is remedied using the Invariant
EKF, a recently introduced variant of the EKF meant to account for the symmetries
of the state space. Extensive Monte-Carlo runs illustrate the theoretical results.

1

Introduction

The problem of simultaneous localization and mapping (SLAM) has a rich history over
the past two decades, which is too broad to cover here, see e.g. [18, 19]. The extended
Kalman filter (EKF) based SLAM (the EKF-SLAM) has played an important historical
role, and is still used, notably for its ability to close loops thanks to the maintenance of
correlations between remote landmarks.
The fact that the EKF-SLAM is inconsistent (that is, it returns a covariance matrix
that is too optimistic, see e.g., [3], leading to inaccurate estimates) was early noticed
[28] and has since been explained in various papers [14, 2, 24, 26, 27, 23]. In the
present paper we consider the inconsistency issues that stem from the fact that, as only
relative measurements are available, the origin and orientation of the earth-fixed frame
can never be correctly estimated, but the EKF-SLAM tends to “think” it can estimate
them as its output covariance matrix reflects an information gain in those directions of
the state space. This lack of observability, and the poor ability of the EKF to handle it,
is notably regarded as the root cause of inconsistency in [26, 23] (see also references
therein). In the present paper we advocate the use of the Invariant (I)-EKF to prevent
covariance reduction in directions of the state space where no information is available.
The Invariant extended Kalman filter (IEKF) is a novel methodology introduced in
[10, 11] that consists in slightly modifying the EKF equations to have them respect the
∗ A. Barrau is with SAFRAN TECH, Groupe Safran, Rue des Jeunes Bois - Chateaufort, 78772 Magny Les Hameaux CEDEX. S. Bonnabel are with MINES ParisTech,
PSL Research University, Centre for robotics, 60 Bd St Michel 75006 Paris, France
[axel.barrau,silvere.bonnabel]@mines-paristech.fr

geometrical structure of the problem. Reserved to systems defined on Lie groups, it
has been mainly driven by applications to localization and guidance, where it appears
as a slight modification of the multiplicative EKF (MEKF), widely known and used in
the world of aeronautics. It has been proved to possess theoretical local convergence
properties the EKF lacks in [8], to be an improvement over the EKF in practice (see
e.g., [5, 4, 17, 31] and more recently [8] where the EKF is outperformed), and has been
successfully implemented in industrial applications to navigation (see the patent [7]).
In the present paper, we slightly generalize the IEKF framework, to make it capable to handle very general observations (such as range and bearing or bearing only
observations), and we show how the derived IEKF-SLAM, a simple variant of the
EKF-SLAM, allows remedying the inconsistency of EKF-SLAM stemming from the
non-observability of the orientation and origin of the global frame.

1.1

Links and differences with previous literature

The issue of EKF-SLAM inconsistency has been the object of many papers, see [28,
14, 2, 27] to cite a few, where empirical evidence (through Monte-Carlo simulations)
and theoretical explanations in various particular situations have been accumulated.
In particular, the insights of [2, 27] have been that the orientation uncertainty is a
key feature in the inconsistency. The article [27], in line with [28, 14, 32, 2], also
underlines the importance of the linearization process, as linearizing about the true
trajectory solves the inconsistency issues, but is impossible to implement in practice as
the true state is unknown. It derives a relationship that should hold between various
Jacobians appearing in the EKF equations when they are evaluated at the current state
estimate to ensure consistency.
A little later, the works of G.P. Huang, A.I. Mourikis, and S. I. Roumeliotis [24,
26, 23] have provided a sound theoretical analysis of the EKF-SLAM inconsistency as
caused by the EKF inability to correctly reflect the three unobservable degrees of freedom (as an overall rotation and translation of the global reference frame leave all the
measurements unchanged). Indeed, the filter tends to erroneously acquire information
along the directions spanned by those unobservable transformations. To remedy this
problem, the above mentioned authors have proposed various solutions, the most advanced being the Observability Constrained (OC)-EKF. The idea is to pick a linearization point that is such that the unobservable subspace “seen” by the EKF system model
is of appropriate dimension, while minimizing the expected errors of the linearization
points.
Our approach, that relies on the IEKF, provides an interesting alternative to the
OC-EKF, based on a quite different route. Indeed, the rationale is to apply the EKF
methodology, but using alternative estimation errors to the standard linear difference
between the estimate and the true state. Any non-linear error that reflects a discrepancy
between the true state and the estimate, necessarily defines a local frame around any
point, and the idea underlying the IEKF amounts to write the Kalman Jacobians and
covariances in this frame. We notice and prove here that an alternative nonlinear error
defines a local frame where the unobservable subspace is everywhere spanned by the
same vectors. Using this local frame at the current estimate to express Kalman’s covariance matrix will be shown to ensure the unobservable subspace “seen” by the EKF

system model is automatically of appropriate dimension.
We thus obtain an EKF variant which automatically comes with consistency properties. Moreover, we relate unobservability to the inverse of the covariance matrix (called
information matrix) rather than on the covariance matrix itself, and we derive guarantees of information decrease over unobservable directions. Contrarily to the OC-EKF,
and as in the standard EKF, we use here the latest, and thus best, state estimate as the
linearization point to compute the filter Jacobians.
In a nutshell, whereas the key fact for the analysis of [26] is that the choice of
the linearization point affects the observability properties of the linearized state error
system of the EKF, the key fact for our analysis is that the choice of the error variable
has similar consequences. Theoretical results and simulations underline the relevance
of the proposed approach.
Robot-centric formulations such as [15], and later [22, 30] are promising attempts
to tackle unobservability, but they unfortunately lack convenience as the position of
all the landmarks must be revised during the propagation step, so that the landmarks’
estimated position becomes in turn sensitive to the motion sensor’s noise. They do not
provably solve the observability issues considered in the present paper, and it can be
noted the OC-EKF has demonstrated better experimental performance than the robocentric mapping filter, in [26]. In particular, the very recent papers [22, 30] propose to
write the equations of the SLAM in the robot’s frame under a constant velocity assumption. Using an output injection technique, those equations become linear, allowing to
prove global asymptotic convergence of any linear observer for the corresponding deterministic linear model. This is fundamentally a deterministic approach and property,
and as the matrices appearing in the obtained linear model are functions of the observations, the behavior of the filter is not easy to anticipate in a noisy context: The
observation noise thus corrupts the very propagation step of the filter.
Some recent papers also propose to improve consistency through local map joining,
see [39] and references therein. Although appealing, this approach is rather oriented
towards large-scale maps, and requires the existence of local submaps. But when using submap joining algorithm, “inconsistency in even one of the submaps, leads to an
inconsistent global map” [25]. This approach may thus prove complementary, if the
IEKF SLAM proposed in the present paper is used to build consistent submaps. Note
that, the IEKF SLAM can also be readily combined with other measurements such as
the GPS, whereas the submap approach is tailored for pure SLAM.
From a methodology viewpoint, it is worth noting our approach does not bring to
bear estimation errors written in a robot frame, as [15, 22, 30, 39]. Although based on
symmetries as well, the estimation errors we use are slightly more complicated.
Finally, nonlinear optimization techniques have become popular for SLAM recently, see e.g., [16] as one of the first papers. Links between our approach, and those
novel methods are discussed in the paper’s conclusion.

1.2

Paper’s organization

The paper is organized as follows. In Section 2, the standard EKF equations and EKFSLAM algorithm are reviewed. In Section 3 we recall the problem that neither the

origin nor the orientation of the global frame are observable, but the EKF-SLAM systematically tends to “think” it observes them, which leads to inconsistency. In Section
4 we introduce the IEKF-SLAM algorithm. In Section 5 we show how the linearized
model of the IEKF always correctly captures the considered unobservable directions.
In Section 6 we derive a property of the covariance matrix output by the filter that can
be interpreted in terms of Fisher information. In Section 7 simulations support the theoretical results and illustrate the benefits of the proposed algorithm. Finally, the IEKF
theory of [8] is briefly recapped in the appendix, and the IEKF SLAM shown to be an
application of this theory indeed. The equations of the IEKF SLAM in 3D are then
also derived applying the general theory.

2
2.1

The EKF-SLAM algorithm
Statement of the general standard EKF equations

Consider a general dynamical system in discrete time with state Xn ∈ RN associated to
a sequence of observations (Yn )n>0 ∈ R p . The equations are as follows:
Xn = f (Xn−1 , un , wn ),

(1)

Yn = h(Xn ) +Vn ,

(2)

where f is the function encoding the evolution of the system, wn is the process noise,
un an input, h the observation function and Vn the measurement noise.
The EKF propagates the estimate X̂n−1|n−1 obtained after the observation Yn−1 ,
through the deterministic part of (1):
X̂n|n−1 = f (X̂n−1|n−1 , un , 0)

(3)

The update of X̂n|n−1 using the new observation Yn is based on the first-order approximation of the non-linear system (1), (2) around the estimate X̂n , with respect to the
estimation errors en−1|n−1 , en|n−1 defined as:
en−1|n−1 = Xn−1 − X̂n|n−1 ,

en|n−1 = Xn − X̂n|n−1

(4)

Using the Jacobians Fn = ∂∂ Xf (X̂n−1|n−1 , un , 0), Gn = ∂∂ wf (X̂n−1|n−1 , un , 0), and Hn =
∂h
∂ X (X̂n|n−1 ), the combination of equations (1), (2) and (3) yields the following firstorder expansion of the error system
en|n−1 = Fn en−1|n−1 + Gn wn ,
Yn − h(X̂n|n−1 ) = Hn en|n−1 +Vn ,

(5)
(6)


where the second order terms, that is, terms of order O kek2 , kwk2 , kekkwk have
been removed according to the standard way the EKF handles non-additive noises in
the model (see e.g., [35] p. 386). Using the linear Kalman equations with Fn , Gn , Hn ,
the gain Kn is computed, and letting zn = Yn − h(X̂n|n−1 ), an estimate en|n = Kn zn of

the error Xn − X̂n|n−1 accounting for the observation Yn is computed, along with its
covariance matrix Pn|n . The state is updated accordingly:
X̂n|n = X̂n|n−1 + Kn zn

(7)

The detailed equations are recalled in Algorithm 1. The assumption underlying the
EKF is that through first-order approximations of the state error evolution, the linear Kalman equations allow computing a Gaussian approximation of the error en|n ∼
N (0, Pn|n ) after each measurement, yielding an approximation of the sought density P(Xn |Y1 , · · · ,Yn ) ≈ N (X̂n|n , Pn|n ). However, the linearizations involved induce inevitable approximations that may lead the filter to inconsistencies and sometimes even
divergence.
Algorithm 1 Extended Kalman Filter (EKF)
Choose an initial uncertainty matrix P0 and estimate X̂0
loop
Define Fn , Gn and Hn through (5) and (6).
Define Qn as Cov(wn ) and Rn as Cov(Vn ).
Propagation

X̂n|n−1 = f X̂n−1|n−1 , un , 0
Pn|n−1 = Fn Pn−1|n−1 FnT + Gn Qn GTn
Update

zn = Yn − h X̂n|n−1
Sn = Hn Pn|n−1 HnT + Rn ,
Kn = Pn|n−1 HnT Sn−1
Pn|n = [I − Kn Hn ]Pn|n−1
X̂n|n = X̂n|n−1 + Kn zn
end loop

2.2

The considered SLAM problem

For simplicity’s sake let us focus on the standard “steered” bicycle (or unicycle) model
[20]. The state is defined as:

Xn = θn , xn , p1n , . . . , pKn ,
(8)
where θn ∈ R denotes the heading, xn ∈ R2 the 2D position of the robot/vehicle, pnj ∈ R2
the position of unknown landmark j (landmarks or synonymously features, constitute
the map). The equations of the model are:
θn = θn−1 + ωn + wω
n,
xn = xn−1 + R(θn−1 )(vn + wvn ),
pnj

=

j
pn−1
,

1≤ j≤K

(9)

where ωn ∈ R denotes the odometry-based estimate of the heading variation of the vehiv
cle, vn ∈ R2 the odometry-based indication of relative shift, wω
n and wn their associated
noises, and R(θ ) is the matrix encoding a rotation of angle θ :


cos(θ ) − sin(θ )
R(θ ) =
.
sin(θ ) cos(θ )
Note that a forward Euler discretization of the continuous time well-known unicycle
equations leads to vn ∈ R2 having its second entry null. More sophisticated integration
methods or models including side slip may yet lead to non-zero values of both entries
of vn so we opt for a more general model with vn ∈ R2 . The covariance matrix of the
noises will be denoted by

 ω   ω T 
wω
wn
wn
n
Qn = Cov  wvn  = E  wvn   wvn   ∈ Rl×l
02K×1
02K×1
02K×1


with l = 3 + 2K. A general landmark observation in the robot’s frame reads:


 
h̃ R(θn )T p1 − xn +Vn1


..
Yn = 

.


h̃ R(θn )T pK − xn +VnK

(10)

(11)

where Yn ∈ R2K (or RK for monocular visual SLAM) is the observation of the features
at time step n, and Vn the observation noise, and h̃ is any function.
Remark 1. Only a subset of the features is actually observed at time n. However, to
simplify the exposure of the filters’ equations, we systematically assume in the sequel
that all features are observed.
We let the output noise covariance matrix be
 1
Vn
 .. 
Rn = Cov  .  .

(12)

VnK

Remark 2. Note that, the observation model (11) encompasses
usual range and
  theq

y1
y21 + y22 , arctan 2 (y2 , y1 ) .
bearing observations used in the SLAM problem by letting h̃
=
 y2
y
If we choose instead the one dimensional observation h̃ 1 = arctan 2 (y2 , y1 ) we rey2
cover the 2D monocular SLAM measurement. Note also we do not provide any specific
form for the noise in the output: this is because the properties we are about to prove
are related to the observability and thus only depend on the deterministic part of the
system, so they are in fact totally insensitive to the way the noise enters the system.

2.3

The EKF-SLAM algorithm

We merely apply here the methodology of EKF to the SLAM problem described in
Section 2.2. The first-order expansions (5), (6) applied to equations (9), (11) yield:


1 
01,2 01,2K
Fn = R θ̂n−1|n−1 JvTn
I2
02,2K  ,
02K,1
02K,2 I2K
 1 1


∇h · Hn
1
01,2  01,2K
(13)


..


Gn = 02,1 R θ̂n−1|n−1
02,2K , Hn = 

.
02K,1
02K,2
02K,2K
∇hK · HnK



T 
T
T k
,
R θ̃
p̂n|n−1 − x̂n|n−1
−R θ̃
Hnk = −JR θ̃


i
T h k
0 −1
with θ̃ = θ̂n|n−1 , J =
and ∇hk denotes the Jacobian of h̃ computed at R θ̂n|n−1
p̂n|n−1 − x̂n|n−1 ∈
1 0
R2 . The obtained EKF-SLAM algorithm is recaped in Algorithm 2.
Algorithm 2 EKF SLAM
Choose an initial uncertainty matrix P0 and estimate X̂0
loop
Define Fn , Gn and Hn as in (13).
Define Qn , Rn as in (10) and (12).
Propagation
θ̂n|n−1 = θ̂n−1|n−1 + ωn

x̂n|n−1 = x̂n−1|n−1 + R θ̂n−1|n−1 vn
j
j
p̂n|n−1
= p̂n−1|n−1
for all 1 ≤ j ≤ K
Pn|n−1 = Fn Pn−1|n−1 FnT + Gn Qn GTn
Update  h

i
h̃ R(θ̂n|n−1 )T p̂1n|n−1 − x̂n|n−1




..
zn = Yn − 

.
 h

i
T
K
h̃ R(θ̂n|n−1 ) p̂n|n−1 − x̂n|n−1
Sn = Hn Pn|n−1 HnT + Rn ,
Kn = Pn|n−1 HnT Sn−1
Pn|n = [I − Kn Hn ]Pn|n−1
X̂n|n = X̂n|n−1 + Kn zn
end loop

3

Observability issues and consistency of the EKF

In this section we come back to the general framework (1), (2). The standard issue of
observability [21] is fundamentally a deterministic notion so the noise is systematically

turned off.
Definition 1 (Unobservable transformation). We say a transformation φ : RN → RN of
the system
(1)-(2) is unobservable if for any initial conditions X01 ∈ RN and X02 =

1
φ X0 the induced solutions of the dynamics (1) with noise turned off, i.e., Xn =
f (Xn−1 , un , 0) yield the same output at each time step n > 0, that is:
h(Xn1 ) = h(Xn2 ).
It concretely means that (with all noises turned off) if the transformation is applied
to the initial state then none of the observations Yn are going to be affected. As a
consequence, there is no way to know this transformation has been applied. In line with
[29, 24, 26] we will focus here on the observability properties of the linearized system.
To that end we define the notion of non-observable (or unobservable) shift which is
an infinitesimal counterpart to Definition 1, and is strongly related to the infinitesimal
observability [21]:
Definition 2 (Unobservable shift). Let (Xn )n>0 denote a solution of (1) with noise
turned off. A vector δ X0 ∈ RN is said to be an unobservable shift of (1)-(2) around X0
if:
∀n > 0, Hn δ Xn = 0,
where Hn is the linearization of h at Xn and where δ Xn is the solution at n of the
linearized system δ Xn = Fn δ Xn−1 initialized at δ X0 , with Fn denoting the Jacobian
matrix of f (·, un , 0) computed at Xn−1 .
In other words (see e.g. [26]), for all n ≥ 0, δ Xn lies in the kernel of the observability matrix between steps 0 and n associated to the linearized error-state system model,
i.e., δ X0T [H0T ; (H1 F1 )T ; · · · ; (Hn Fn · · · F1 )T ] = 0.
The interpretation is as follows: consider another initial state shifted from X0 to
X0 + δ X0 . Saying that δ X0 is unobservable means no difference on the sequence of observations up to the first order could be detected between both trajectories. Formally,
this condition reads: h(Xn + δ Xn ) = h(Xn ) + ◦ (δ Xn ), i.e., Hn δ Xn = 0. An estimation
method conveying its own estimation uncertainty as the EKF, albeit based on linearizations, should be able to detect such directions and to reflect that accurate estimates
along such directions are beyond reach.

3.1

Considered unobservable shifts

In the present paper we consider unobservability corresponding to the impossibility to
observe the position and orientation of the global frame [29, 24]. The corresponding
shifts have already been derived in the literature.

Proposition 1. [26] Let X̂ = θ̂ , x̂, p̂ be an estimate of the state. Only one feature is
considered, the generalization of the proposition to several features is trivial. The firstorder perturbation of the estimate corresponding
 to
 an infinitesimal rotation of angle


1
0 −1
δ α of the global frame consists of the shift  J x̂  δ α, with J =
. In the
1 0
J p̂

same way, the first-order perturbation of the estimate corresponding toan infinitesimal

0
translation of the global frame of vector δ u ∈ R2 consists of the shift δ u.
δu
Proof. When rotating the global frame the heading becomes:
θ̂ → θ̂ + δ α.
The position of the robot becomes:



cos(δ α) − sin(δ α)
0
x̂ →
x̂ ≈ x̂ + δ α
sin(δ α) cos(δ α)
1


−1
x̂.
0

The position of the feature becomes:



cos(δ α) − sin(δ α)
0
p̂ →
p̂ ≈ p̂ + δ α
sin(δ α) cos(δ α)
1


−1
p̂.
0

Stacking these results we obtain the first-order variation of the full state vector (regarding the rotation only, the effect of infinitesimal translation being trivial to derive):
     
1
θ̂
θ̂
 x̂  →  x̂  +  J x̂  δ α.
(14)
J p̂
p̂
p̂

Proposition 2. [26] The shifts of Proposition 2 that correspond to infinitesimal rotations, are unobservable shifts of (9)-(11) in the sense of Definition 2.
The intuitive explanation is clear [26]: “if the robot and landmark positions are
shifted equally along those vectors, it will not be possible to distinguish the shifted
position from the original one through the measurements.”

3.2

Inconsistency of the EKF

This section recalls using the notations of the present paper, a result of [24]. It shows
the infinitesimal rotations defined in Proposition 2 are not, in general, unobservable
shifts of the system linearized about the trajectory estimated by the EKF. Indeed, applying Definition 2 to (13) in the case of a single feature (the generalization being
straightforward) with δ X0 = (1, J x̂0|0 , J p̂0|0 )T and δ Xn = Fn · · · F1 δ X0 yields the condition for an infinitesimal rotation of the initial state to be unobservable for the linearized
system. This condition writes Hn δ Xn ≡ 0 and boils down to have for any n > 0 (see
[24]):
"
#
n−1


∇hn · J · R(θn|n−1 )T − p̂n|n−1 − p̂0|0 + ∑ x̂i|i − x̂i|i−1 = 0
i=1

T

p̂n|n−1 − x̂n|n−1 . For examwhere ∇hn is the Jacobian of h̃ computed at R θ̂n|n−1
ple, if h̃ is invertible the condition boils down to
"
#
 n−1

∀n > 0, − p̂n|n−1 − p̂0|0 + ∑ x̂i|i − x̂i|i−1 = 0.
(15)
i=1

We see the quantities involved are the updates of the state. As they depend on the noise,
there is a null probability for the condition to be respected, and it is always violated in
practice.
But the point of the present paper is to show that the problem is related to the
(arbitrary in a non-linear context) choice to represent the estimation error as the linear
difference X − X̂, not to an inconsistency issue inherent to EKF-like methods applied to
SLAM. By devising an EKF-SLAM based on another estimation error variable, which
in some sense amounts to change coordinates, the false observability problem can be
corrected. The qualitative reason why this is sufficient is related to the basic cause of
false observability: a given fixed shift may or may not be observable depending on the
linearization point X̂, as proved by Proposition 2. It turns out that the latter property
is not inherently related to the SLAM problem: it is in fact a mere consequence of
the errors’ definition (4). Defining those errors otherwise can dramatically modify the
condition (15). This is the object of the remainder of this article.

4

A novel EKF-SLAM algorithm

Building upon the theory of the Invariant (I)EKF on matrix Lie groups, as described
and studied in [8], we introduce in this section a novel IEKF for SLAM. In Appendix
A.1-A.2 the general theory of the IEKF is recalled and slightly extended to account for
the very general form of output (11), and the algorithm derived herein is shown to be
a direct application of the theory. To spare the reader a study of the Lie group based
theory, we attempt to explain in simple terms the IEKF methodology on the particular
SLAM example throughout the present section.
Consider the model equations (9) with state Xn given by (8). Exactly as the EKF,
the IEKF propagates the estimated state obtained after the observation Yn−1 of (11)
through the deterministic part of (9) i.e., θ̂n|n−1 = θ̂n−1|n−1 + ωn , x̂n|n−1 = x̂n−1|n−1 +

j
j
R θ̂n−1|n−1 vn , p̂n|n−1
= p̂n−1|n−1
for all 1 ≤ j ≤ K. To update the predicted state
X̂n|n−1 using the observation Yn we use a first order Taylor expansion of the error system. But, instead of considering the usual state error X − X̂, we rather use the (linearized) estimation error defined as follows


θn − θ̂n|n−1

 xn − x̂

n|n−1 − θn − θ̂n|n−1

 J1x̂n 
 p1 − p̂1

n
n|n−1 − θn − θ̂n|n−1 J p̂n|n−1 
(16)
ξn|n−1 = 


..



.
 K 
K
K
pn − p̂n|n−1 − θn − θ̂n|n−1 J p̂n|n−1

and ξn|n is analogously defined. For close-by X, X̂, this represents an error variable
in the usual sense indeed, as ξ = 0 if and only if X̂ = X. As in the standard EKF
methodology, let us see how this alternative estimation error is propagated through a
first-order approximation of the error system. Using the propagation equations of the
filter, and (9), we find


wω
n
v
−wω J x̂
 n n−1|n−1 + R(θ̂n−1|n−1 )wn 


ω
1
−wn J p̂n−1|n−1

(17)
ξn|n−1 = ξn−1|n−1 + 


..


.


K
−wω
J
p̂
n
n−1|n−1
ω 2
where terms of order O(kξn−1|n−1 k2 ), O(kwω
n kkξn−1|n−1 k), and O(kwn k ) have been
neglected as in the standard the EKF handles non-additive noises [35]. To derive (17)
we have used the equalities ∀θ , θ̂ ∈ R, w ∈ R2 :

R(θ )w = R(θ̂ )w + O(|θ̂ − θ | kwk)

(18)
2

R(θ ) − R(θ̂ ) − (θ − θ̂ )JR(θ̂ ) = O(|θ − θ̂ | )

(19)

Note that, the odometer outputs ωn , vn have miraculously vanished. This is in fact a
characteristics - and a key feature - of the IEKF approach.
Let us now compute the first-order approximation of the observation error, using
the alternative state error (16). Define Hn as the matrix, depending on X̂n|n−1 only, such
that for all ξn|n−1 ∈ R2K+3 defined by (16), the innovation term

i
 h
 
 
h̃ R(θ̂n|n−1 )T p̂1n|n−1 − x̂n|n−1
h̃ R(θn )T p1 − xn


 


..
..
−




.
.


h

i


T
K
h̃ R(θn ) p − xn
h̃ R(θ̂n|n−1 )T p̂K
− x̂n|n−1
n|n−1

is equal to Hn ξn|n−1 +O(kξn|n−1 k2 ). Using that R(θ )T (p−x)−R(θ̂ )T ( p̂− x̂) = R(θ )T [(p−
x) − R(θ − θ̂ )( p̂ − x̂)], and R(θ )T ξ = R(θ̂ )T ξ + O(kξ k2 ) from (18), we see that Hn is
defined as in (21) below. Thus the linearized (first-order) system model with respect to
alternative error (16) writes
ξn|n−1 = Fn ξn−1|n−1 + Gn wn ,
Yn − h(X̂n|n−1 ) = Hn ξn|n−1 +Vn

(20)

v T
T
with wTn = (wω
n , (wn ) , 01×2K ) , and



01,2K

 −J x̂
02,2K 
R θ̂n−1|n−1
n−1|n−1


−J p̂1
02
02,2K 
,
n−1|n−1
Fn = I2K+3 , Gn = 

..
..
.. 


.
.
. 

K
−J p̂n−1|n−1
02
02,2K


 
T
02,1 −I2 I2 02,2(K−1)
∇h̃1 · R θ̂n|n−1
T

 2
02,1 −I2 02,2 I2 02,2(K−2) 
∇h̃ · R θ̂n|n−1
,

Hn = 
..



.


T
K
02,1 −I2 02,2(K−1) I2
∇h̃ · R θ̂n|n−1
1

01,2

(21)


T  k
where ∇h̃k is the Jacobian of h̃ computed at R θ̂n|n−1
p̂n|n−1 − x̂n|n−1 . As in the
standard EKF methodology, the matrices Fn , Gn , Hn allow to compute the Kalman gain
Kn and covariance Pn . Letting zn be the standardly defined innovation (see Algorithm 3
just after “Update”), ξn|n = Kn zn is an estimate of the linearized error ξn|n−1 accounting
T ).
for the observation Yn , and Pn|n is supposed to encode the dispersion E(ξn|n ξn|n
The final step of the standard EKF methodology is to update the estimated state
X̂n|n−1 thanks to the estimated linearized error ξn|n = Kn zn . There is a small catch,
though: ξ being not anymore defined as a mere difference X − X̂, simply adding ξn|n
to X̂n|n would not be appropriate. The most natural counterpart to (7) in our setting,
would be to choose for X̂n|n the values of (θn , xn , · · · , p1n , pKn ) making the right member
of (16) equal to the just computed ξn|n . However, the IEKF theory recalled in Appendix
A.2, suggests an update that amounts to the latter to the first order, but whose nonlinear structure ensures better properties [8]. Thus, the state is updated as follows
X̂n|n = ϕ(ξn|n , X̂n|n−1 ) = ϕ(Kn zn , X̂n|n−1 ), with ϕ defined by


  
δθ
θ̂ + δ θ
θ̂


 δx   
 1   x̂   R(δ θ )x̂1 + B(δ θ )δ x 1 
 R(δ θ ) p̂ + B(δ θ )δ p 
δ p   1 
ϕ 

 , p̂  = 


 ..  
..


 .  · · ·
.
K
K
K
K
p̂
δp
R(δ θ ) p̂ + B(δ θ )δ p


where B(α) =

sin(α)
α
1−cos(α)
α

− 1−cos(α)
α
sin(α)
α

(22)

!
. Algorithm 3 recaps the various steps of the

IEKF SLAM.

5

Remedying EKF SLAM consistency

In this section we show the infinitesimal rotations and translations of the global frame
are unobservable shifts in the sense of Definition 2 regardless of the linearization points

Algorithm 3 IEKF SLAM
The state is defined by X = (θ , xT , (p1 )T , · · · , (pK )T )) ∈ R3+2K . Pick an initial uncertainty matrix P0 and estimate X̂0 .
loop
Define Fn , Gn and Hn as in (21).
Define Qn , Rn as in (10) and (12).
Propagation
θ̂n|n−1 = θ̂n−1|n−1 + ωn

x̂n|n−1 = x̂n−1|n−1 + R θ̂n−1|n−1 vn
j
j
p̂n|n−1
= p̂n−1|n−1
for all 1 ≤ j ≤ K
Pn|n−1 = Fn Pn−1|n−1 FnT + Gn Qn GTn
Update  h

i
h̃ R(θ̂n|n−1 )T p̂1n|n−1 − x̂n|n−1




..
zn = Yn − 

.
 h

i
T
K
h̃ R(θ̂n|n−1 ) p̂n|n−1 − x̂n|n−1
Sn = Hn Pn|n−1 HnT + Rn ,
Kn = Pn|n−1 HnT Sn−1
Pn|n = [I − Kn Hn ]Pn|n−1
Use (22) to compute X̂n|n = ϕ(Kn zn , X̂n|n−1 ).
end loop
used to compute the matrices Fn and Hn of eq. (21), a feature in sharp contrast with
the usual restricting condition (15) on the linearization points. In other words we show
that infinitesimal rotations and translations of the global frame are always unobservable shifts of the system model linearized with respect to error (16) regardless of the
linearization point, a feature in sharp contrast with previous results (see Section 3.2 and
references therein).

5.1

Main result

We can consider only one feature (K = 1) without loss of generality. The expression
of the linearized system model has become much simpler, as the linearized error has
the remarkable property to remain constant during the propagation step in the absence
of noise, since Fn = I3+2K in (20)-(21). First, let us derive the impact of first-order
variations stemming from rotations and translations of the global frame on the error as
defined by (16), that is, an error of the following form


(θ − θ̂ )
ξ =  x − x̂ − (θ − θ̂ )J x̂  ,
(23)
p − p̂ − (θ − θ̂ )J p̂
Proposition 3. Let X̂ = (θ̂ , x̂, p̂)T be an estimate of the state. The first-order perturbation of the linearized estimation error defined by (23) around 0, corresponding to an




1
infinitesimal rotation of angle δ α of the global frame, reads 02,1  δ α. In the same
02,1
way, an infinitesimal translation of the global frame with vector δ u ∈ R2 implies a
first-order perturbation of the error system (23) of the form (0, δ u, δ u)T .
Proof. According to Proposition 1, an infinitesimal rotation by an angle δ α  1 of
the true state corresponds to the transformation θ → θ + δ α. x → x + δ αJx and p →
p + δ αJ p. Regarding ξ of eq (23) it corresponds to the variation
 
δα
ξ → ξ + 02,1  + O(δ αkξ k) + O(δ α 2 ).
02,1
This direction of the state space is “seen” by the linearized error system as the vector
(δ α, 0, 0, 0, 0)T . Similarly, a translation of vector δ u of the global frame yields the
transformation θ → θ , x → x + δ u, p → p + δ u. The effect on the linearized error ξ
of (23) is obviously the perturbation (0, δ u, δ u)T neglecting terms of order δ ukξ k.
We can now prove the first major result of the present article: the infinitesimal
transformations stemming from rotations and translations of the gobal frame are unobservable shifts for the IEKF linearized model.
Theorem 1. Consider the SLAM problem defined by equations (9) and (11), and the
IEKF-SLAM algorithm 3. Let δ X0 denote a linear combination of infinitesimal rotations and translations δ X0R , δ X01 , δ X02 of the whole system defined as follows
 
 
0
0
 
0
1
1
 
 


δ X02 = 
δ X0R = 02,1  ,
δ X01 = 
1 .
0 ,
0
1
02,1
1
0
Then δ X0 is an unobservable shift of the linearized system model (20)-(21) of the IEKF
SLAM in the sense of Definition 2, and this whatever the sequence of true states and
estimates (Xn , X̂n|n , X̂n|n−1 ): the very structure of the IEKF is consistent with the considered unobservability.
Proof. Note that Definition 2 involves a propagated perturbation δ Xn , but as here Fn is
I5 : we have ∀n > 0, δ Xn = δ X0 . Thus, the only point to check is: Hn (δ X0 ) = 0, i.e.,
T

02,1 −I2 I2 δ X0 = 0. This is straightforward replacing δ X0 with
∇h̃ · R θ̂n|n−1
alternatively δ X01 , δ X02 and δ X0R .
We obtained the consistency property we were pursuing: the linearized model correctly captures the unobservability of global rotations and translations. As a byproduct,
the unobservable seen by the filter is automatically of appropriate dimension.

5.2

Interpretation and discussion

The standard EKF is tuned to reduce the state estimation error X̂ − X defined through
the original state variables X, X̂ of the problem. Albeit perfectly suited to the linear
case, the latter state error has in fact absolutely no fundamental reason to rule the
linearization process in a non-linear setting. The basic difference when analyzing the
EKF and the IEKF is that
• In the standard EKF, there is a trivial correspondence between a small variation
of the true state and a small variation of the estimation error (4). But the global
rotations of the frame make the error vary in a non-trivial way as recalled in
Section 3.
• In the IEKF approach, the effect of a small rotation of the state on the variation
of the estimation error (16) becomes trivial as ensured by Proposition 3. But the
error is non-trivially related to the state, as its definition explicitly depends on
the linearization point X̂.
Many consistency issues of the EKF stem from the fact that the updated covariance
matrix Pn|n is computed before the update, namely at the predicted state X̂n|n−1 , and
thus does not account for the updated state’s value X̂n|n , albeit supposed to reflect the
covariance of the updated error. This is why the OC-EKF typically seeks to avoid linearizing at the latest, albeit best, state estimate, in order to find a close-by state such
that the covariance matrix resulting from linearization preserves the observability subspace dimension. The IEKF approach is wholly different: the updated covariance Pn|n
is computed at the latest estimate X̂n|n−1 , which is akin to the standard EKF methodology. But it is then indirectly adapted to the updated state, since it is interpreted as
the covariance of the error ξn|n . And contrarily to the standard case, the definition of
this error depends on X̂n|n . More intuitively, we can say the confidence ellipsoids encoded in Pn|n are attached to a basis that undergoes a transformation when moved over
from X̂n|n−1 to X̂n|n , this transformation being tied to the unobservable directions. This
prevents spurious reduction of the covariance over unobservable shifts, which are not
identical at X̂n|n−1 and X̂n|n .
Finally, note the alternative error (23) is all but artificial: it naturally stems from the
Lie group structure of the problem. This is logical as the considered unobservability
actually pertains to an invariance of the model (9)-(11), that is the SLAM problem, to
global translations and rotations. Thus it comes as no surprise the Invariant approach,
that brings to bear invariant state errors that encode the very symmetries of the problem,
prove fruitful (see the appendix for more details).

6

IEKF consistency and information

Our approach can be related to the previous work [26]. Indeed, according to the latter
article, failing to capture the right dimension of the observability subspace in the linearized model leads to “spurious information gain along directions of the state space
where no information is actually available” and results in “unjustified reduction of the

covariance estimates, a primary cause of filter inconsistency”. Theorem 1 proves that
infinitesimal rotations and translations of the global frame, which are unobservable in
the SLAM problem, are always “seen” by the IEKF linearized model as unobservable
directions indeed, so this filter does not suffer from “false observability” issues. This
is our major theoretical result.
That said, the results of the latter section concern the system with noise turned off,
and pertain to an automatic control approach to the notion of observability as in [26].
The present section is rather concerned with the estimation theoretic consequences of
Theorem 1. We prove indeed, that the IEKF’s output covariance matrix correctly reflects an absence of “information gain” along the unobservable directions, as mentioned
above, but where the information is now to be understood in the sense of Fisher information. As a by-product, this allows relating our results to a slightly different approach
to SLAM consistency, that rather focuses on the Fisher information matrix than on the
observability matrix, see in particular [38, 1].

6.1

The general Bayesian Fisher Information Matrix

The exposure of the present section is based on the seminal article [36]. See also [38, 1]
for related ideas applied to SLAM. Consider the system (1) with output (2). Define the
collection of state vectors and observations up to time n:
X̃n = (X0T , · · · , XnT )T ,

Ỹn = (Y0T , · · · ,YnT )T ,

The joint probability distribution of the (n + 1)N vector X̃n and of the np vector Ỹn is
p(Ỹn , X̃n ) = p(X0 )Πni=1 p(Yi | Xi )p(Xi | Xi−1 )
The Bayesian Fisher Information Matrix (BIFM) is defined as the following Nk × Nk
matrix based upon the dyad of the gradient of the log-likelihood:
J(X̃n ) = E([∇X̃n log p(Ỹn , X̃n )][∇X̃n log p(Ỹn , X̃n ]T )
and note that, for the SLAM problem it boils down to the matrix of [26]. This matrix is
of interest to us as it yields a lower bound on the accuracy achievable by any estimator
used to attack the filtering problem (1)-(2). Indeed let Jn be defined as the inverse of
the N × N right-lower block of [J(X̃n )]−1 . This matrix provides a lower bound on the
mean square error of estimating Xn from past and present measurements Ỹn and prior
p(X0 ). Indeed, for any unbiased estimator T : Rnp → RN :
E([T (Ỹn )][T (Ỹn )]T )  Jn−1
where A  B means A − B is positive semi-definite. Jn−1 is called the Bayesian or
Posterior Cramér-Rao lower bound for the filtering problem [36]. Most interestingly,
in the case where f and h are linear, the prior distribution is Gaussian, and the noises
are additive and Gaussian, we have
−1
Jn = Pn|n

where Pn|n is the covariance matrix output by the Kalman filter. Thus, in the linear
−1
Gaussian case, Pn|n
reflects the statistical information available at time n on the state Xn .
−1
By extension in the SLAM literature Pn|n
is often simply referred to as the information
matrix, in non-linear contexts also, e.g. when using extended information filters [37].

6.2

Application to IEKF-SLAM consistency

In the last section, we have recalled that in the linear Gaussian case, the inverse of
the covariance matrix output by the Kalman filter is the Fisher information available
to the filter (this is also stated in [3] p. 304). In the light of those results, it is natural
−1
to expect from any EKF variant, that the inverse of the output covariance matrix Pn|n
reflect an absence of information gain along unobservable directions indeed. If the
filter fails to do so, the output covariance matrix will be too optimistic, that is, inconsistent, and wrong covariances yield wrong gains [3]. The following theorem shows
the linearized system model of the IEKF allows ensuring the desired property of the
covariance matrix. It is our second major result.
Theorem 2. Consider the SLAM problem defined by equations (9)-(11) and the IEKFSLAM Algorithm 3. Let δ X0 denote a linear combination of infinitesimal rotations and
translations δ X0R , δ X01 , δ X02 of the whole system, as defined in Theorem 1. δ X0 is thus
an unobservable shift. If the matrix Pn|n output by the IEKF remains invertible, we have
at all times:
−1
−1
(δ X0 )T Pn|n
(δ X0 ) ≤ (δ X0 )T Pn−1|n−1
(δ X0 ) .

(24)

Proof. As Fn = I2K+3 in (21), the unobservable shifts remain fixed i.e. δ Xn = δ X0 . At
the propagation step we have:
−1
δ X0T Pn|n−1
δ X0 = δ X0T Pn−1|n−1 + Gn Qn GTn

−1

δ X0

−1
6δ X0T Pn−1|n−1
δ X0 .

as Gn Qn GTn is positive semi-definite. And at the update step (see the Kalman Information Filter form in [3]) we have:
h
i
−1
−1
−1
δ X0
H
δ X0 = δ X0T Pn|n−1
δ X0T Pn|n
δ X0 = δ X0T Pn|n−1
+ HnT R−1
n
n
−1
as Hn δ X0 = 0 as shown in the proof of Theorem 1. Thus (δ X0 )T Pn|n
(δ X0 ) is nonincreasing over time n. Note that, the proof evidences that if Pn|n is not invertible, the
results of the theorem still hold, writing the IEKF in information form.

Our result essentially means the linearized model of the IEKF has a structure which
guarantees that the covariance matrix at all times reflects an absence of “spurious”
(Bayesian Fisher) information gain over directions that correspond to the unobservable
rotations and translations of the global frame.

Figure 1: Simulated trajectory : the displayed loop is driven 10 times by a robot able
to measure the relative position of the landmarks lying in a range of 5 m around him.
Velocity is constant (1m/s) as well as angular velocity (9 deg/s).

7

Simulation results

In this section we verify in simulation the claimed properties on the one hand, and
on the other we illustrate the striking consistency improvement achieved by the IEKF
SLAM. To that end, we propose to consider a similar numerical experiment as in the
sound work [26] dedicated to the inconsistency of EKF and the benefits of the OCEKF. The IEKF is compared here to the standard EKF, the UKF, the OC-EKF, and the
ideal EKF, which is the - impossible to implement - variant of the EKF where the state
is linearized about the true trajectory.

Figure 2: Evaluation of the consistency of the five filters using the NEES indicator (for
the entire 3-DoF pose) over 50 runs, in the experimental setting described in Section
7.1. We see the indicator stays around 1 for IEKF SLAM and OC-EKF SLAM over
the whole time interval, as expected from a consistent estimation method. The “ideal”
EKF, where the system is linearized on the true value of the state, yields similar results.
To the opposite, we see the EKF is inconsistent, and the UKF also.

7.1

Simulation setting

The simulation setting we chose is (deliberately) similar to the one used in [26] (Section 6.2). The vehicle (or robot) drives a 15m-diameter loop ten times in the 2D plane,
finding on its path 20 unknown features as displayed on Figure 1. The velocity and
angular velocity are constant (1 m/s and 9 deg/s respectively). The relative position of
the features in the reference frame of the vehicle is observed once every second (where
h̃ of (11) is the identity). The standard deviation σ of the velocity measurement on
each wheel of the vehicle is 2% of the velocity. This yields the standard √
deviation σv
of the resulting
linear
velocity
and
σ
of
the
rotational
velocity:
σ
=
(
2/2)σ and
ω
v
√
σω = ( 2/a)σ , where a = 0.5m is the distance between the drive wheels (see [26]).
The features are visible if they lie within a sensing range of 5 m, in which case they
are observed with an isotropic noise of standard deviation 10 cm. The initial uncertainty over position and heading is zero - which will prove a condition not sufficient to
prevent failure of the EKF. Each time a landmark is seen for the first time, its position

Figure 3: Evaluation of the performance of the proposed IEKF SLAM algorithm, in
terms of RMS of the vehicle heading and position error (in rad and m respectively).
We see the results are very similar to those of the OC-EKF and ”ideal” EKF, the latter
being impossible to implement for real (the system is linearized at the true state). These
results are much better than those of the UKF and classical EKF.

Figure 4: Inconsistency illustrated on a single run. The plotted EKF and UKF heading
errors (top plots) do not remain in the 99% uncertainty envelope computed by the
filter. Filters whith theoretical properties regarding non-observable directions (IEKF,
OC-EKF and ideal EKF) remedy this problem.

is initialized in the earth frame using the current estimated pose of the robot, the associated uncertainty is set to a very high value compared to the size of the map, then
a Kalman update is performed to correlate the position of the new feature withe the
other variables. Each second, all visible landmarks (i.e. those in a range of 5m) are
processed simultaneously in a stacked observation vector.
Five algorithms are compared:
1. The classical EKF, described in Algorithm 2.
2. The proposed IEKF SLAM algorithm described in Algorithm 3.
3. The ideal EKF as defined in [26], i.e., a classical EKF where the Riccati equation
is computed at the true trajectory of the system instead of the estimated trajectory.
Although not usable in practice, the latter is a good reference to compare with,
as it is supposed to be an EKF with consistent behavior.
4. The OC-EKF described in [26], which is so far the the only method that guarantees the non-observable subspace has appropriate dimension.
5. The Unscented Kalman Filter (UKF), known to better deal with the non-linearities
than the EKF.
Before going further, the next subsection introduces the NEES indicator used in the
simulations to measure the consistency of these methods.

7.2

The NEES indicator

Classical criteria used to evaluate the performance of an estimation method, like Root
Mean Squared (RMS) error do not inform about consistency as they do not take into
account the uncertainty returned by the filter. This point is addressed by the Normalized
Estimation Error Squared (NEES), which computes the average squared value of the
error, normalized by the covariance matrix of the EKF. For a sample (Xi )i=1,p of error
values having dimension d, each of them with a covariance matrix Pi of size d × d, the
NEES is defined by:
1 p T −1
NEES =
∑ Xi Pi Xi .
p × d i=1
If each Xi is a zero mean Gaussian with covariance matrix Pi , then for large p we have
NEES ≈ 1. The case NEES > 1 reveals an inconsistency issue: the actual uncertainty
is higher than the computed uncertainty. This situation typically occurs when the filter
is optimistic as it believes to have gained information over a non-observable direction.
The NEES indicator will be used, along with the usual RMS, to illustrate our solution
to SLAM inconsistency in the sequel.

7.3

Numerical results

Figure 2 displays the NEES indicator of the vehicle pose estimate (heading and position) over time, computed for 50 Monte-Carlo runs of the experiment described in

Section 7.1. As expected, the profile of the NEES for classical EKF, ideal EKF and
OC-EKF is the same as in the previous paper [26] which inspired this experimental
section. Note that we used here a normalized version of the NEES, making its swing
value equal to 1. We see also that the result is similar for OC-EKF SLAM and ideal
EKF SLAM: the NEES varies between 1 and 1.7, in contrast to the EKF SLAM and
UKF SLAM which exhibit large inconsistencies over the robot pose We see here that
the IEKF remedies inconsistency, with a NEES value that remains close to 1. Note
that, it performs here even better than OC-EKF and ideal EKF (whose results are very
close to each other), in terms of consistency. The basic difference between IEKF and
these filters lies in using or not the current estimate as a linearization point. Uncertainty
directions being very dependent from the estimate, what Figure 2 suggests is that they
may not be correctly captured if computed on a different point. The other aspect of
the evaluation of an EKF-like method is performance: regardless of the relevance of
the covariance matrix returned by the filter (i.e. consistency), pure performance can
be evaluated through RMS of the heading and position error, whose values over time
are displayed in Figure 3. They confirm an expectable result: solving consistency issues improves the accuracy of the estimate as a byproduct, as wrong covariances yield
wrong gains [3].
Selecting a single run, we can also illustrate the inconsistency issue in terms of
covariance and information. Figure 4 displays the heading error for EKF, UKF, IEKF,
OC-EKF and ideal EKF SLAM, and the 99% envelope returned by each filter. This
illustrates both the false observability issue and the resulting inconsistency of EKF and
UKF: the heading uncertainty is reduced over time while the estimation error goes
outside the 99% envelope. To the opposite, the behavior of IEKF, OC-EKF and ideal
EKF is sound. Figure 5 shows the map and the landmarks 99% uncertainty ellipsoids:
Similarly, both EKF and UKF fail capturing the true landmarks ’positions within the
99% ellipsoids whereas the three over filters succeed to do so. Finally, Figure 6 displays
the evolution of the information over a shift corresponding to infinitesimal rotations as
−1
defined in Theorem 2, that is the evolution over time of the quantity δ XnT Pn|n
δ Xn .
The theorem is successfully illustrated: the latter quantity is always decreasing for the
IEKF, ideal EKF, OC-EKF but not for the EKF and the (slightly better to this respect)
UKF.

8

Conclusion

This work evidences that the EKF algorithm for SLAM is not inherently inconsistent
- at least regarding inconsistency related to unobservable transformations of the global
frame - but the choice of the right coordinates for the linearization process is pivotal.
We showed that applying the recent theory of the IEKF - an EKF (slight) variant - leads
to provable properties regarding observability and consistency. Extensive Monte-Carlo
simulations have illustrated the consistency of the new method and the striking improvement over EKF, UKF, OC-EKF, and more remarkably over the ideal EKF also,
which is the - impossible to implement - variant of the EKF where the system is linearized about the true trajectory.
Note that, the IEKF approach may prove relevant beyond SLAM to some other

problems in robotics as well, such as autonomous navigation (see [4]), and in combination with controllers, notably for motion planning purposes, see [17]. In [9] the
IEKF has proved to possess global asymptotic convergence properties on a simple localization problem of a wheeled robot, which is a strong property. The IEKF has also
been patented for navigation with inertial sensors [7].
Nowadays nonlinear optimization based SLAM algorithms are becoming popular
as compared with EKF SLAM, see e.g. [16] for one of the first papers on the subject. We yet anticipate a simple EKF SLAM with consistency properties will prove
useful to the research community, the EKF SLAM having been abandoned in part due
to its inconsistency. The general EKF has proved useful in numerous industrial applications, especially in the field of guidance and navigation. It has the benefits of being
1-recursive, avoiding to store the whole trajectory and 2-suited to on-line real-time applications. Moreover the aerospace and defense industry has developed a corpus of
experience for its industrial implementation and validation. And the IEKF is a variant
that, being in every respect similar to EKF, retains all its advantages, but which possesses additional guaranteed properties. Note also that, all the improvements of the
EKF for SLAM such as e.g., the SLAM of [33] and sparse extended information filters
[37], can virtually be turned into their invariant counterpart.
The high dimensional optimization formulation of the SLAM problem being prone
to local minima, having an accurate initial value (i.e. a small initial estimation error) is
very critical [39]. The IEKF SLAM algorithm proposed in the present paper may thus
be advantageously used to initialize those methods in challenging situations.
Besides, we anticipate our approach based on symmetries could help improve (at
least first order) optimization techniques for SLAM. To understand why, assume by
simplicity the sensors to be noise free. Then, moving a candidate trajectory along
unobservable directions will not change the cost function, and an efficient optimization
algorithm should account for this. And when a gradient descent algorithm is used,
only a first-order expansion of the cost function is considered. Our Lie group approach
will allow defining steepest descent directions in a alternative geometric way, that will
“stick” to the unobservable directions, and the corresponding update will move along
the (Lie group) state space in a non-linear yet relevant way. This issue is left for future
work, but a thorough understanding of the interest of the invariant approach for the
EKF, is a first step in this direction.

Acknowledgements
The authors would like to thank Cyril Joly for his advice.

A

IEKF theory, applications to 2D and 3D SLAM

In this section we provide more details on the IEKF theory on matrix Lie groups, and
show how the underlying Lie group structure of the SLAM problem has been used
indeed to build the IEKF SLAM Algorithm 3. We also provide the IEKF equations for
3D SLAM. For more information on the IEKF see [8] and references therein.

A.1

Primer on matrix Lie groups

A matrix Lie group G is a subset of square invertible N̄ × N̄ matrices MN̄ (R) verifying
the following properties:
IN̄ ∈ G,

∀g ∈ G, g−1 ∈ G,

∀a, b ∈ G, ab ∈ G

where IN̄ is the identity matrix of RN̄ . If γ(t) is a curve over G with γ(0) = IN̄ , then
its derivative at t = 0 necessarily lies in a subset g of MN̄ (R). g is a vector space and
it is called the Lie algebra of G. It has same dimension dim g as G. Thanks to a linear invertible map denoted by Lg : Rdim g → g, one can advantageously identify g to
Rdim g . Besides, the vector space g can be mapped to the matrix Lie group G through
the classical matrix exponential expm . Thus, Rdim g can be mapped to G through
the Lie exponential map defined by exp(ξ ) := expm (Lg (ξ )) for ξ ∈ Rdim g . This
map is invertible for small ξ , and we have (exp(ξ ))−1 = exp(−ξ ). The well-known
Baker-Campbell-Hausdorff (BCH) formula gives a series expansion for the product
exp(ξ ) exp(ζ ). In particular it ensures exp(ξ ) exp(ζ ) = exp(ξ + ζ + T ), where T is of
the order O(kξ k2 , kζ k2 , kξ kkζ k). For any g ∈ G, the adjoint matrix Adg ∈ Rdim g×dim g
is defined by g exp(ζ )g−1 = exp(Adg ζ ) for all ζ ∈ g. We now give explicit formulas
for two groups of particular interest for the SLAM problem.
A.1.1

Group of direct planar isometries SE(2)

This famous
matrices, i.e.,
 group
 in robotics
 can be defined
 using homogeneous
 
G = 
R(θ ) x
α
αJ u
SE(2) := g =
; θ ∈ R, x ∈ R2 . Let ζ =
, then Lse(2) (ζ ) =
,
01,2 1
u
01×2 0

where α ∈ R and u ∈ R2 . We have g = se(2) = Lse(2) (ζ ), ζ ∈ R3 . The Lie expo!


sin(α)
− 1−cos(α)
R(α) B(α)u
α
α
. We
nential writes exp(ζ ) =
where B(α) = 1−cos(α)
sin(α)
01,2
1
α
α


1
01×2
have Adg =
.
−Jx R(θ )
A.1.2

Group of multiple direct spatial isometries SEK+1 (2)

We now introduce a simple extension of SE(2), inspiring from preliminary remarks in
[13, 8]. For θ ∈ R and x, p1 , · · · , pK ∈ R2 , consider the map Ψ : R2K+3 → MK+3 (R)
defined by


R(θ ) x p1 · · · pn
 01,2


Ψ : (θ , x, p1 , · · · , pK ) 7→ 
(25)
 ...

IK+1
01,2
and let G ⊂ R(K+3)×(K+3) be defined by

G = Ψ(θ , x, p1 , · · · , pK ); θ ∈ R, x, p1 , · · · , pK ∈ R2

and denote it by G = SEK+1 (2).Note
 that, we recover SE(2) for K = 1, i.e., SE(2) =
α


αJ
x p1 · · · pn
x
 1
 01,2

 

SE1 (2). Letting α ∈ R and ζ =  p  yields LseK+1 (2) (ζ ) = 
..


 .. 
0
.
(K+1)×(K+1)
 . 
01,2
pK

and g = seK+1 (2) = LseK+1 (2) (ζ ); ζ ∈ R2K+3 . It turns out, by extension of the
SE(2) results, that there exists a closed form for the Lie exponential exp = expm ◦LseK+1 (2)
that writes exp(ζ ) = Ψ(α, B(α)x, B(α)p1 , · · · , B(α)pK )) with B(α) =

sin(α)
α
1−cos(α)
α

− 1−cos(α)
α
sin(α)
α

The Ad is also easily derived by extension of SE(2), but to save space, we only display
it once: AdΨ(θ̂ ,x̂, p̂1 ,··· , p̂K ) is defined as the matrix Gn of eq (21).

A.2

Statement of the general IEKF equations

This section is a summary of the IEKF methodology of [8, 6]. Let G be a matrix Lie
group. Consider a general dynamical system χn ∈ G ⊂ RN̄×N̄ on the group, associated
to a sequence of observations (Yn )n>0 ∈ R p , with equations as follows :
χn = χn−1 exp(wn )Γn

(26)

Yn = h(χn ) +Vn ,

(27)

where Γn ∈ G is an input matrix which encodes the displacement according to the
evolution model, wn ∈ Rdim g is a vector encoding the model noise, h : G → R p is the
observation function and Vn ∈ R p the measurement noise.
The IEKF propagates an estimate obtained after the previous observation Yn−1
through the deterministic part of (26):
χ̂n|n−1 = χ̂n−1|n−1 Γn

(28)

To update χ̂n|n−1 using the new observation Yn , one has to consider an estimation error
that is well-defined on the group. In this paper we will use the following right-invariant
errors
−1
−1
ηn−1|n−1 := χn−1 χ̂n−1|n−1
, ηn|n−1 := χn χ̂n|n−1
(29)
which are equal to IN̄ when χ̂ = χ. The terminology stems from the fact they are invariant to right multiplications, that is, transformations of the form χ̂n|n−1 7→ χ̂n|n−1 g, χn 7→
χn g with g ∈ G. Note that, one could alternatively consider left-invariant errors but it
turns out to be less fruitful for SLAM.

!
.

A.2.1

Linearized error equations over the group G

The IEKF update is based upon a first-order expansion of the non-linear system associated to the errors (29) around IN̄ . First, compute the full error’s evolution
−1
ηn|n−1 = χn χ̂n|n−1
−1
= χn−1 exp(wn )Γn Γ−1
n χ̂n−1|n−1
−1
= χn−1 exp(wn )χ̂n−1|n−1
−1
−1
χ̂n−1|n−1 exp(wn )χ̂n−1|n−1
= χn−1 χ̂n−1|n−1

= ηn−1|n−1 exp(Adχ̂n−1|n−1 wn )
Note that the term Γ has disappeared ! This is a key property for the successes of the invariant filtering approach [12, 8]. To linearize this equation we define ξn−1|n−1 , ξn|n−1 ∈
Rdim g around IN̄ through
ηn−1|n−1 = exp(ξn−1|n−1 ),

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

(30)

As in the standard non-additive noise EKF methodology [35] all terms of order kξn−1|n−1 k2 , kwn k2 , kwn kkξn−1|n−1 k,
are assumed small and are neglected. Using the BCH formula, and neglecting the latter
terms, we get
exp(ξn|n−1 ) = exp(ξn−1|n−1 + Adχ̂n−1|n−1 wn )
Using the local invertibility of exp around 0dim g×1 , we get the following linearized
error evolution in Rdim g :
ξn|n−1 = Fn ξn−1|n−1 + Gn wn ,

(31)

where Fn = Idim g and Gn = Adχ̂n−1|n−1 .
To linearize the output error, we now slightly adapt the IEKF theory [8] to account
for the general form of output (11). Note that, Yn − h(χ̂n|n−1 ) = h(χn ) − h(χ̂n|n−1 ) +
Vn = h(exp(ξn|n−1 )χ̂n|n−1 )−h(χ̂n|n−1 )+Vn . As ξn|n−1 is assumed small, and as exp(0dim g×1 ) =
IN̄ , a first-order Taylor expansion in ξ ∈ Rdim g arbitrary, allows definit Hn as follows
h(exp(ξ )χ̂n|n−1 ) − h(χ̂n|n−1 ) := Hn ξ + O(kξ k2 )
A.2.2

(32)

Computing the Kalman gain

The gain is computed as for the standard EKF [35], but where the linearized error
system to be considered is (31)-(32).
A.2.3

Update

As in the standard theory, the Kalman gain matrix Kn allows computing an estimate
of the linearized error after the observation Yn through ξn|n = Kn zn , where zn = Yn −
h(χ̂n|n−1 ). Recall the state estimation errors defined by (29)-(30) are of the form
exp(ξ ) = χ χ̂ −1 , that is, χ = exp(ξ )χ̂. Thus an estimate of χn after observation Yn

which is consistent with (29)-(30), is obtained through the following Lie group counterpart of the linear update (7)
χ̂n|n = exp(Kn zn )χ̂n|n−1

(33)

The equations of the filter are detailed in Algorithm 4.
Algorithm 4 Invariant Extended Kalman Filter (IEKF)
Choose initial χ̂0|0 ∈ G and P0|0 ∈ Rdim g×dim g = Cov(ξ0|0 )
loop
Define Hn as in (32) and let Fn = Idim g and Gn = Adχ̂n|n−1 .
Define Qn as Cov(wn ) and Rn as Cov(Vn ).
Propagation
χ̂n|n−1 = χ̂n−1|n−1 exp(un )
Pn|n−1 = Fn Pn−1|n−1 Fn−1 + Gn Qn GTn
Update

zn = Yn − h χ̂n|n−1
Sn = Hn Pn|n−1 HnT + Rn ,
Kn = Pn|n−1 HnT Sn−1
Pn|n = [I − Kn Hn ]Pn|n−1
χ̂n|n = exp(Kn zn )χ̂n|n−1
end loop

A.3

Lie group based derivation of the 2D IEKF-SLAM

In this section we show step by step the IEKF-SLAM Algorithm 3 is a strict application
of Algorithm 4.
A.3.1

Underlying Lie group

The Lie group that underlies the SLAM problem, is G = SEK+1 (2) introduced in Appendix A.1.2. Let us apply the general theory of the IEKF to this group. To define the
Lie group counterpart χn of the state Xn defined by (8), we let χn = Ψ(Xn ). The model
equations (9) write
χn = χn−1 Γ̃n
v T
T
with Γ̃n = Ψ(ωn + wω
n , (vn + wn ) , 01×2 · · · , 01×2 ) . At the propagation step, the IEKF
propagates the estimate through the corresponding deterministic equations

χ̂n|n−1 = χ̂n−1|n−1 Γn
with Γn = Ψ(ωn , vTn , 01×2 · · · , 01×2 )T .

A.3.2

Right-invariant error (29)

−1
A simple matrix multiplication shows that ηn|n−1 = χn χ̂n|n−1
= Ψ(en|n−1 ), where


θn − θ̂n|n−1 

 xn − [R θn − θ̂
n|n−1  x̂n|n−1 ] 


 p1 − [R θ − θ̂
1
n
n|n−1 p̂n|n−1 ] 
n
en|n−1 = 


..




.

pKn − [R θn − θ̂n|n−1 p̂Kn|n−1 ]


(34)

and where en|n is defined analogously.
A.3.3

Linearized error

The linearized error is defined by (30), that is, ηn|n−1 = (exp(ξn|n−1 )). As terms of order O(kξn|n−1 k2 ) are to be neglected in the linearized equations, it suffices to compute
a first-order approximation of ξn|n−1 . First note that ξn|n−1 defined by (16) is a linear
approximation to en|n−1 , that is, en|n−1 = ξn|n−1 + O(kξn|n−1 k2 ). This readily implies
I2K+3 + LseK+1 (2) (ξn|n−1 ) + O(kξn|n−1 k2 ) = Ψ(en|n−1 )
Recalling I2K+3 + LseK+1 (2) (ξ ) + O(kξ k2 ) = expm (LseK+1 (2) (ξ )) := exp(ξ ) we see
(16) is a first approximation of ξn|n−1 as defined in (30) indeed.
A.3.4

Linearized error propagation

The way the linearized error (16) propagates has already been computed and consists
of (17), which is the same as the first equation of (20). It exactly matches what can be
expected from the general theory, that is, equation (31), recalling that Gn of (21) is the
map Adχ̂n−1|n−1 of the group G = SEK+1 (2) indeed.
A.3.5

Linearized output map

Note the general definition of Hn of (32) here boils down to the one of Section 4 as here
χ̂ = Ψ(X̂), exp(ξ )χ̂ = χ = Ψ(Xn ) and ξ is given by (16).
A.3.6

Estimate update

Referring to Appendix A.1.2 and the definition of exponential map of G = SEK+1 (2),
a simple matrix multiplication shows that exp(ξ )Ψ(X̂) = ϕ(ξ , X̂) with ϕ defined by
(22) indeed.

A.4

Equations of the IEKF-SLAM in 3D

Extending the group SEK+1 (2) to the 3D case, and applying the general IEKF theory
of Section A.2 , we derive in the present section an IEKF for 3D SLAM. Due to space

limitations an as it is not the primary object of the present paper we pursue extreme
brevity of exposure. See also [6, 8]. Note that, although the 3D SLAM equations
make use of rotation matrices, they are in fact totally intrinsic: When using quaternions
(recommended) or Euler angles (not recommended) they write the same as the group
SEK+1 (3) we introduce does in fact not depend on a specific representation of rotations.
A.4.1

3D SLAM model

The equations of the robot in 3D and in continuous time write:
d
d
Rt = Rt (ωt + wtω )× ,
xt = Rt (vt + wtv ),
dt
dt
d j
p = ptj , 1 ≤ j ≤ K
dt t

(35)

where Rt ∈ SO(3) is a rotation matrix that represents the robot’s orientation at time
t, ωt ∈ R3 denotes the angular velocity of the robot measured by a gyrometer or by
odometry (in combination with a unicycle model for a terrestrial vehicle), vt ∈ R3 the
velocity in the robot’s frame, and ptj ∈ R3 is the position of landmark j, and where
(b)× for b ∈ R3 denotes the skew symmetric matrix of R3×3 such that for any x ∈ R3
we have (b)× x = b × x. Finally wtω and wtv denote (resp.) the noise on angular and
linear velocities. Although the theory of IEKF could very well be applied directly to
this continuous time dynamics as in [8], we apply it here to a discretized model, to be
consistent with the rest of the article. Although exact discretization of the noisy model
on the group is beyond reach [6], letting ∆t be the time step, the following first-order
integration scheme is widely used:
Rn = Rn−1 expm [(wω
n )× ]Ωn ,
j
,
pnj = pn−1

xn = xn−1 + Rn−1 (vn + wvn ),

1≤ j≤K

(36)

where the increments Ωn , vn are obtained solving the noise-free initial conditions during the n-th time step with initial condition R = I3 , x = 0, and where the following
discrete noise
T
v T
T
wTn := ((wω
n ) , ( wn ) , 01×3K ) ,

Qn = E(wn wTn )

(37)

is obtained by integration of the corresponding white noises. Note that, this scheme is
accurate to first-order terms in ∆t. A general landmark observation in the car’s frame
reads:

  T 1

h̃ Rn p − xn +Vn1


..
Yn = 
(38)

.
 T K

K
h̃ Rn p − xn +Vn
where Yn ∈ R3K (or R2K for monocular visual SLAM) is the observation of the features
at time step n, and Vn the observation noise. We let the output noise covariance matrix
be R̃n ∈ R3K×3K (not to be confused with the rotation Rn ).

A.4.2

Underlying Lie group

The Lie group that underlies the problem is the group G = SEK+1 (3) that we introduce
as follows. For R ∈ SO(3) and x, p1 , · · · , pK ∈ R3 let


R x p1 · · · pn
 01,3


Ψ : (R, x, p1 , · · · , pK ) 7→ 
(39)
 ...

IK+1
01,3
and let G ⊂ R(K+4)×(K+4) be defined as

G = Ψ(R, x, p1 , · · · , pK ); R ∈ SO(3), x, p1 , · · · , pK ∈ R3



ω

(ω)× x p1 · · · pn
x
 1  0
 
1,3
and denote it by G = SEK+1 (3). We then have LseK+1 (2) ( p ) = 
.
 ..   ..
0(K+1)×(K+1)
 . 
0
1,3
pK

and g = seK+1 (3) = LseK+1 (3) (ζ ); ζ ∈ R3K+6 . For ζ ∈ R3K+6 , by extension of the
SE(3) results, we have the closed form:
exp(ζ ) = IK+4 + S +

1 − cos(||ζ ||) 2 ||ζ || − sin(||ζ || 3
S +
S
||ζ ||2
||ζ ||3

(40)

where S = LseK+1 (3) (ζ ). As easily seen by analogy with SE(3)


R


 (x)× R

AdΨ(R,x,p1 ,··· ,pK ) = 


..

.
K
(p )× R
A.4.3

03×3 · · · 03×3
R
..

.
R










(41)

Link with the dynamical model

Let the state be X = (R, x, p1 , · · · , pK ), and let χ ∈ G be χ = Ψ(X), and let X̂ and χ̂ be
their estimated counterparts. It is easily seen that up to terms that will disappear in the
linearization process anyway, the model (36) for the state is mapped through Ψ defined
at (39), to a model of the form (26).
A.4.4

Right-invariant error (29)

It writes η := χ χ̂ −1 = Ψ(RR̂T , x − RR̂T x̂, · · · , pK − RR̂T p̂K ).






A.4.5

Linearized error

Using the matrix logarithm, define ω̃ ∈ R3 as the solution of expm [(ω̃)× ] = RR̂T . Neglecting terms of order O(kω̃k2 ), we have x − RR̂T x̂ = x − x̂ − ω̃ × x̂. A first order identification as in Appendix A.3.3 thus yields ξ = (ω̃, x− x̂− ω̃ × x̂, · · · , pK − p̂K − ω̃ × p̂K )
as a vector that satisfies the definition exp(ξ ) = η up to terms of order O(kξ k2 ).
A.4.6

Linearized error propagation

As in the standard EKF theory, the IEKF propagates an estimate obtained after the
previous observation Yn−1 through the deterministic part of (36), or equivalently (26)
in matrix form. Thus, the propagation equation is given by (31) where Fn = I3K+6 and
Gn wn = Adχ̂n|n−1 (wn ) as a direct application of the theory.
A.4.7

Linearized output map

To apply Definition (32), simply note that RT p − R̂T p̂ = RT (p − RR̂T p̂) ≈ R̂T (p −
RR̂T p̂). Thus


T
03,1 −I3 I3 03,3(K−1)
∇h̃1 · R θ̂n|n−1


..
,
(42)
Hn = 
.




T
K
03,1 −I3 03,3(K−1) I3
∇h̃ · R θ̂n|n−1


where ∇h̃k is the Jacobian of h̃ computed at R̂Tn|n−1 p̂kn|n−1 − x̂n|n−1 .
The various steps are gathered in Algorithm 5.

References
[1] Juan Andrade-Cetto and Alberto Sanfeliu The effects of partial observability
when building fully correlated maps. IEEE Trans. Robot. Automat., 21:771–777,
2005.
[2] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot. Consistency of the
EKF-SLAM algorithm. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, pages 3562–3568. IEEE, 2006.
[3] Y. Bar-Shalom, X. R. Li, and T Kirubarajan. Estimation with Applications to
Tracking and Navigation. New York, Wiley, 2001.
[4] M. Barczyk, S. Bonnabel, J.-E. Deschaud, and François Goulette. Invariant EKF
design for scan matching-aided localization. Control Systems Technology, IEEE
Transactions on, 23(6):2440–2448, 2015.
[5] M. Barczyk and A. F. Lynch. Invariant observer design for a helicopter UAV aided
inertial navigation system. Control Systems Technology, IEEE Transactions on,
21(3):791–806, 2013.

Algorithm 5 IEKF SLAM: the 3D case
The state is defined by X = (R, x, p1 , · · · , pK ) ∈ SO(3) × R3+3K and the model is (36)(38). Pick an initial estimate X̂0 with covariance matrix P0 .
loop
Let Fn = I3K+6 , Hn as in (42), Gn = Adψ(X̂n−1|n−1 ) using (41)
Define Qn by (37). R̃n is the observation noise cov. matrix.
Propagation
R̂n|n−1 = R̂n−1|n−1 Ωn
x̂n|n−1 = x̂n−1|n−1 + R̂n−1|n−1 vn
j
j
p̂n|n−1
= p̂n−1|n−1
for all 1 ≤ j ≤ K
Pn|n−1 = Fn Pn−1|n−1 FnT + Gn Qn GTn
Update  h

i
h̃ RTn|n−1 p̂1n|n−1 − x̂n|n−1




..
zn = Yn − 

.
 h

i
T
K
h̃ Rn|n−1 p̂n|n−1 − x̂n|n−1
Sn = Hn Pn|n−1 HnT + R̃n ,
Kn = Pn|n−1 HnT Sn−1
Pn|n = [I − Kn Hn ]Pn|n−1
Compute χ̂n|n = exp(Kn zn )Ψ(X̂n|n−1 ) using (40) and let X̂n|n = Ψ−1 (χ̂n|n ).
end loop

[6] A. Barrau and S. Bonnabel. Intrinsic filtering on lie groups with applications to
attitude estimation. Automatic Control, IEEE Transactions on, 60(2):436 – 449,
2015.
[7] A.Barrau and S. Bonnabel. Alignment method for an inertial unit, 2014.
SAGEM/ARMINES. EP 2014/075439, WO/2015/075248.
[8] A. Barrau and S. Bonnabel. The invariant extended Kalman filter as a stable
observer. Automatic Control, IEEE Transactions on, Accepted and scheduled to
appear in the 2017 May issue. arXiv preprint arXiv:1410.1465, 2014.
[9] A. Barrau and S. Bonnabel. Navigating with highly precise odometry and noisy
GPS: a case study. In IFAC Symposium on Nonlinear Control Systems (NOLCOS).
Hal preprint hal.archives-ouvertes.fr/hal-01267244/document, 2016.
[10] S. Bonnabel. Left-invariant extended Kalman filter and attitude estimation. In
IEEE Conference on Decision and Control, pp. 1027-1032, 2007.
[11] S. Bonnabel, P. Martin, and E. Salaun. Invariant Extended Kalman Filter: Theory
and application to a velocity-aided attitude estimation problem. In IEEE Conference on Decision and Control, pp. 1297 - 1304, 2009.
[12] S. Bonnabel, Ph. Martin, and P. Rouchon. Symmetry-preserving observers. Automatic Control, IEEE Transactions on, 53(11):2514–2526, 2008.
[13] S. 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] J. A. Castellanos, J. Neira, and J. D. Tardós. Limits to the consistency of EKFbased SLAM. In IFAC Symposium on Intelligent Autonomous Vehicles, pp. 12441249. 2004.
[15] J. A. Castellanos, R. Martinez-Cantin, J. Tardos,and J. Neira. Robocentric map
joining: Improving the consistency of EKF-SLAM. Robotics and Autonomous
Systems, 55(1): 21- 29, 2007.
[16] F. Dellaert and M. Kaess Square root SAM: Simultaneous localization and mapping via square root information smoothing The International Journal of Robotics
Research, 25(12):1181–1203, 2006.
[17] S. Diemer and S. Bonnabel. An invariant linear quadratic Gaussian controller
for a simplified car. Robotics and Automation (ICRA), 2015 IEEE International
Conference on, 2015.
[18] G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark, and M. Csobra. A
solution to the simultaneous localisation and mapping (SLAM) problem. IEEE
Trans. Robot. Automat., 17:229–241, 2001.
[19] H. Durrant-Whyte and T. Bailey. Simultaneous localization and mapping: part i.
Robotics & Automation Magazine, IEEE, 13(2):99–110, 2006.

[20] H. F Durrant-Whyte. An autonomous guided vehicle for cargo handling applications. The International Journal of Robotics Research, 15(5):407–440, 1996.
[21] J.-P.l Gauthier and I. Kupka. Observability and observers for nonlinear systems.
SIAM Journal on Control and Optimization, 32(4):975–994, 1994.
[22] B. J. N. Gueirreiro, P. Batista, C. Silvestre and P. Oliveira. Globally asymptotically stable sensor-based simultaneous localization and mapping. Robotics, IEEE
Transactions on 29(6): 1380-1395, 2013.
[23] G. P. Huang, A. Mourikis, St. Roumeliotis. An observability-constrained sliding window filter for SLAM. In Intelligent Robots and Systems (IROS), 2011
IEEE/RSJ International Conference on, pages 65–72. IEEE, 2011.
[24] G. P. Huang, A. I. Mourikis, and S. 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.
[25] S. Huang, Z. Wang, and G. Dissanayake. Sparse local submap joining filter for
building large-scale maps. newblock In IEEE Transactions on Robotics, 24(5), pp
1121–1130, 2008.
[26] G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis. Observability-based rules
for designing consistent EKF SLAM estimators. The International Journal of
Robotics Research, 29(5):502–528, 2010.
[27] S. Huang and G. Dissanayake. Convergence and consistency analysis for extended Kalman filter based SLAM. Robotics, IEEE Transactions on, 23(5):1036–
1049, 2007.
[28] S. J. Julier and J. 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.
[29] K. W. Lee, W. S. Wijesoma, and J. I. Guzman. On the observability and observability analysis of SLAM. In Intelligent Robots and Systems (IROS), 2006
IEEE/RSJ International Conference on, pages 3569–3574. IEEE, 2006.
[30] P. Lourenço, B. J. N. Gueirreiro, P. Batista, C. Silvestre and P. Oliveira. Simultaneous localization and mapping for aerial vehicles: a 3-D sensor-based GAS
filter. Autonomous Robots 40: 881-902, 2016.
[31] Ph. Martin, E. Salaün. Generalized multiplicative extended kalman filter for aided
attitude and heading reference system. In Proc. AIAA Guid., Navigat., Control
Conf, pages 1–13, 2010.
[32] A. Martinelli, N. Tomatis, and R. Siegwart. Some results on slam and the closing
the loop problem. In Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ
International Conference on, pages 334–339. IEEE, 2005.

[33] L. M. Paz, J. D. Tardos and J. Neira. Divide and Conquer: EKF SLAM in O(n).
Robotics, IEEE Transactions on 24(5), 1107-1120, 2008.
[34] S.I. Roumeliotis and G.A. Bekey. Bayesian estimation and Kalman filtering: a
unified framework for mobile robot localization. In ICRA’00, pages 2985 – 2992,
2000.
[35] R.F. Stengel. Optimal Control and Estimation. Dover Books on Mathematics,
1994.
[36] P. Tichavsky, C. H. Muravchik and A. Nehorai. Posterior Cramér-Rao bounds
for discrete-time nonlinear filtering. Signal Processing, IEEE Transactions on,
46(5):1386–1396, 1998.
[37] S. Thrun, Y. Liu, D. Koller, A. Y. Ng,2. Ghahramani and H. Durrant-Whyte.
Simultaneous localization and mapping with sparse extended information filters.
In The International Journal of Robotics Research, 23(7-8): 693-716, 2004.
[38] Z. Wang and G. Dissanayake Observability analysis of SLAM using Fisher information matrix In ICARCV 2008, pages 1242–1247, 2008.
[39] L. Zhao, S. Huang, and G. Dissanayake. Linear SLAM: A linear solution to the
feature-based and pose graph SLAM based on submap joining. In Intelligent
Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on. IEEE,
2013.

Figure 6: Illustration of Theorem 2. Bottom plot is a zoom of the first time steps.
The information over an infinitesimal perturbation corresponding to a rotation of the
whole system is decreasing for the IEKF SLAM, which is a consistent behavior as this
perturbation is unobservable. Ideal EKF and OCEKF give similar results, but EKF and
UKF do not. The plot also confirms EKF and UKF SLAM tend to acquire spurious
information over this unobservable direction.

