Results for Outdoor-SLAM Using Sparse Extended Information Filters
Yufeng Liu and Sebastian Thrun
School of Computer Science
Carnegie Mellon University
Pittsburgh, PA 15213
yufeng@cs.cmu.edu , thrun@cs.cmu.edu
Abstract
In [13], a new algorithm was proposed for efficiently solving
the simultaneous localization and mapping (SLAM) problem.
In this paper, we extend this algorithm to handle data associ-
ation problems and report real-world results, obtained with an
outdoor vehicle. We find that our approach performs favor-
ably when compared to the extended Kalman filter solution
from which it is derived.
1 Introduction
This paper investigates a scalable algorithm for the simultane-
ous mapping and localization (SLAM) problem, and evaluates
it in the context of outdoor navigation. The SLAM problem is
the problem of acquiring a map of an unknown environment
with a moving robot, while simultaneously localizing the robot
relative to this map [2, 5]. The SLAM problem addresses sit-
uations where the robot lacks a global positioning sensor, and
instead has to rely on a sensor of incremental ego-motion for
robot position estimation (e.g., odometry, inertial navigation).
Such sensors accumulate errors over time, making the problem
of acquiring an accurate map challenging. The SLAM prob-
lem has attracted immense attention in the past a few years [4].
This paper addresses computational issues in performing real-
world SLAM. The classical SLAM solution, based on the ex-
tended Kalman filter (EKF) [8, 9, 12, 11], scales quadratically
with the number of landmarks in the map. As a result, prac-
tical implementations of this approach are limited to a few
hundred landmarks [2]. This deficiency has long been rec-
ognized and has spurred a flurry of research on more efficient
SLAM algorithms. One thrust of research involves the de-
velopment of hierarchical techniques, which decompose large
maps into smaller, computationally more manageable sub-
maps [1, 3, 6, 15]. Such techniques are more efficient than the
quadratic time EKF, but most of them still require quadratic
time for maintaining global consistency between multiple sub-
maps. Additionally, the consistency is difficult to maintain as
the vehicle crosses boundaries between different sub-maps.
In this paper, we follow a different approach. In a recent work-
shop paper [13], we proposed a SLAM algorithm that requires
constant time for updating, yet still maintains global consis-
tency. This approach is based on the information form of the
Kalman filter [7, 10], known as extended information filter
(EIF). EIFs are mathematically identical to EKF, yet they rep-
resent map estimates by sets of pairwise constraints between
landmarks. In practice, these constraints are usually sparse.
This insight led us to define the sparse extended information
filter, or SEIF. SEIFs can be updated in constant time, which
is significantly faster than the quadratic update time of EKFs.
Meanwhile it maintains a globally consistent estimate of the
robot pose and the map. However, the original paper [13] only
provided theoretical results and did not analyze the perfor-
mance of SEIFs using real-world data. It also did not provide a
method for handling data association problems that commonly
occur in real-world settings.
This paper describes SEIFs, our extension to handle data asso-
ciation problems, and empirical results. Because SEIFs are ap-
proximations of EKFs, an important question is the accuracy
of this approximation. This paper presents first experimental
results that compare SEIFs with EKFs using both simulated
and real-world data sets. We find that empirically, SEIFs are
highly accurate approximations to EKFs. Our empirical com-
parison utilizes a benchmark data set recorded with an outdoor
vehicle [2]. On the computational end, we find that SEIFs are
significantly more efficient as is predicted, and their efficiency
makes them scalable to much larger maps than EKFs can han-
dle.
2 Sparse Extended Information Filters
2.1 Preliminaries
Let us begin the disposition by highlighting the intuition be-
hind our approach. The standard approach for solving feature-
based SLAM problems is based on the extended Kalman filter
(EKF) [11, 2]. Figure 1 shows the result of EKF mapping in
an environment with 50 landmarks. The normalized covari-
ance of the EKF is the correlation matrix, which is visualized
in Figure 1a. Each of the two axes lists the robot pose (x-y
location and orientation) followed by the x-y-locations of the
50 landmarks. Dark entries indicate strong correlations. It is
known that in the limit of SLAM, all x-coordinates and all
y-coordinates become fully correlated [2]. The checkerboard
appearance of the correlation matrix illustrates this fact.
p. 1
Figure 1: Typical snapshot of EKFs applied to the SLAM problem:
(a) a correlation matrix (normalized covariance) and (b)
the normalized inverse covariance, or information matrix.
This plot illustrates the basic insight of SEIFs: Correla-
tion matrices are dense, whereas their normalized inverses
are naturally sparse.
(a) (b)
The key insight that motivates our approach is shown in Fig-
ure 1b. Shown there is the inverse covariance matrix (also
known as information matrix [7, 10]), normalized just like the
correlation matrix. Elements in this normalized information
matrix can be thought of as constraints, or links, between the
locations of different landmarks: The darker an entry is in the
display, the stronger the link is. As this depiction suggests, the
normalized information matrix appears to be naturally sparse:
It is dominated by a small number of strong links between
geographically nearby landmarks, and possesses a large num-
ber of links whose values, when normalized, are near zero.
Furthermore, link strengths are related to distances between
landmarks: Strong links are found only between geometrically
nearby landmarks. The more distant two landmarks are from
each other, the weaker their link is.
SEIFs exploit this structure by maintaining a sparse infor-
mation matrix, in which only nearby landmarks are linked
through a non-zero element. The resulting network structure
is illustrated in the right panel of Figure 2, where disks cor-
respond to landmarks and dashed arcs to links, as specified
in the information matrix visualized on the left. Shown also
is the robot, which is linked to a small subset of landmarks
only. This subset of landmarks are called active landmarks and
drawn in black. Storing a sparse information matrix requires
linear space. More importantly, updates can be performed in
constant time regardless of the number of landmarks in the
map. The resulting filter is a sparse extended information fil-
ter, or SEIF.
2.2 Information State
Let x
t
denote the pose of the robot at time t, and y
n
with
1 ≤ n ≤ N the location of the n-th landmark, with N being
the total number of landmarks in the environment (a quantity
that is estimated during mapping). The robot pose x
t
and the
set of all landmark locations Y together constitute the state of
the environment:
ξ
t
=
x
t
y
1
. . . y
N
T
(1)
Figure 2: Illustration of the network of landmarks generated by our
approach. Shown on the left is a sparse information ma-
trix, and on the right a map in which entities are linked
when the corresponding elements in the information ma-
trix are non-zero. As argued in the paper, the fact that not
all landmarks are connected is a key structural element of
the SLAM problem, and at the heart of our constant time
solution.
As is common in SLAM literature, SEIFs present the posterior
by a multi-variate Gaussian over the state ξ
t
. Such a Gaus-
sian can be represented by a mean µ
t
and a covariance Σ
t
,
or equally by the so-called natural parameters of the Kalman
filter:
b
t
= µ
T
t
Σ
−1
t
(2)
H
t
= Σ
−1
t
(3)
The EKF representation using the information vector b
t
and
the information matrix H
t
is known as the information form
of the EKF, or extended information filter (EIF). The mean µ
t
and covariance Σ
t
are easily recovered from the information
form:
Σ
t
= H
−1
t
(4)
µ
t
= Σ
t
b
T
t
(5)
The information matrix H
t
was already discussed above, and
an example was shown in Figure 1b. Sparse EIFs, or SEIFs,
are EIFs whose information matrix H
t
is sparse. Put differ-
ently, each row and each column in H
t
contains only a limited
number of non-zero elements, and the limit does not depend
on the size of the matrix N . Sparseness is achieved by an up-
date rule that occasionally removes links from the posterior so
as to maintain sparseness, as described further below.
2.3 Measurement Updates
One of the key update steps in SLAM involves the incorpora-
tion of a measurement (a landmark sighting). The measure-
ment at time t is denoted z
t
. In [13], it is assumed the index
of this landmark can be sensed without error—a classical as-
sumption known in SLAM as “known data association,” nec-
essary for maintaining Gaussian estimates. For now, let us
adopt this assumption; further below, we will discuss our ap-
proach for estimating the landmark identity during the estima-
tion process.
Figure 3 illustrates the effect of measurements on the infor-
mation matrix H
t
. Suppose the robot senses landmark y
1
,
p. 2
(a) (b)
Figure 3: The effect of measurements on the information matrix
and the associated network of landmarks: (a) Observing
y
1
results in a modification of the information matrix ele-
ment H
x
t
,y
1
. (b) Similarly, observing y
2
affects H
x
t
,y
2
.
Both updates can be carried out in constant time.
as illustrated in Figure 3a. This observation links the robot
pose x
t
to the location of y
1
. The strength of the link is
given by the level of noise in the measurement. Updating
EIFs based on this measurement involves the manipulation of
the off-diagonal element H
x
t
,y
1
and its symmetric counterpart
H
y
1
,x
t
that link together x
t
and y
1
. Additionally, the diago-
nal elements H
x
t
,x
t
and H
y
1
,y
1
are also updated. These up-
dates are additive: Each observation of a landmark y increases
the strength of the total link between the robot pose and this
very landmark, thus the total information in the filter. Fig-
ure 3b shows the incorporation of a second measurement of
a different landmark, y
2
. In response to this measurement,
the EIF updates the links H
x
t
,y
2
, H
y
2
,x
t
, H
x
t
,x
t
, and H
y
2
,y
2
).
As this example suggests, measurements introduce links only
between the robot pose x
t
and observed landmarks. Measure-
ments never generate links between pairs of landmarks, or be-
tween the robot and unobserved landmarks.
Incorporating measurements into the information filters natu-
rally requires constant time. The canonical update rule is the
following:
H
t
=
¯
H
t
+ C
t
Z
−1
C
T
t
(6)
b
t
=
¯
b
t
+ (z
t
− h(µ
t
) + C
T
t
µ
t
)
T
Z
−1
C
T
t
(7)
Here h is the measurement function that maps state ξ
t
into
measurement z
t
. The measurement noise is Gaussian with co-
variance Z. Finally, the matrix C
t
is the gradient of the mea-
surement function h with respect to the state vector ξ, taken at
ξ = µ
t
:
C
t
= ∇
ξ
h(µ
t
) (8)
In general filter applications, such an update may require more
than constant time. In SLAM, however, each measurement in-
volves only a single landmark (or a limited number of land-
marks, regardless of N). For this reason, C
t
is zero except for
a limited number of elements. With such a sparse matrix C
t
,
constant time updates can be implemented.
2.4 Motion Updates
Figure 4a illustrates an information matrix and the associated
network before the robot moves. The robot is linked to two
(previously observed) landmarks. If robot motion was free
of noise, this link structure would not be affected by robot
(a) (b)
Figure 4: The effect of motion on the information matrix and the
associated network of landmarks: (a) before motion, and
(b) after motion. If motion is non-deterministic, motion
updates introduce new links (or reinforce existing links)
between any two active landmarks, while weakening the
links between the robot and those landmarks. This step
introduces links between pairs of landmarks.
motion. However, the noise in robot actuation weakens the
link between the robot and all active landmarks. Hence H
x
t
,y
1
and H
x
t
,y
2
are decreased by certain amounts. This decrease
reflects the fact that the noise in robot motion causes a loss
of information about the relative positions of the landmarks
with respect to the robot. Not all of this information is lost,
however. Some of it is shifted into between-landmark link
H
y
1
,y
2
, as illustrated in Figure 4b. This reflects the fact that
even though the motion induces a loss of information of the
robot relative to the landmarks, no information is lost between
individual landmarks. Robot motion, thus, has the effect that
landmarks that were indirectly linked through the robot pose
become linked directly.
Motion updates in the information form of the Kalman fil-
ter are usually not achievable in constant time. However, as
proven in [13], the update can be performed in constant time
if the information matrix H
t
is sparse. The equations for the
general case are as follows:
Ψ
t
= I − S
x
(I + [S
T
x
A
t
S
x
]
−1
)
−1
S
T
x
H
0
t−1
= Ψ
T
t
H
t−1
Ψ
t
∆H
t
= H
0
t−1
S
x
[U
−1
t
+ S
T
x
H
0
t−1
S
x
]
−1
S
T
x
H
0
t−1
¯
H
t
= H
0
t−1
− ∆H
t
¯
b
t
= b
t−1
− µ
T
t−1
(∆H
t
+ H
0
t−1
− H
t−1
) +
ˆ
∆
T
t
¯
H
t
(9)
Here S
x
is the canonical projection matrix from the full state
to the robot pose coordinates. These equations are mathe-
matically exact (not just approximations). They may appear
quite complicated, but they sure are constant time update rules.
However, they may cause violations to the sparseness con-
straints by adding new links (non-zero elements) in the infor-
mation matrix H
t
. The removal of some links is a key approx-
imation step in SEIFs, which enables them to maintain sparse
information matrices.
2.5 Sparsification
SEIF’s sparsification technique is illustrated in Figure 5.
Shown there are the situations before and after sparsification.
The removal of a link in the network corresponds to setting
an element in the information matrix to zero. However, this
p. 3
(a)
(b)
Figure 5: Sparsification: A landmark is deactivated by eliminating
its link to the robot. To compensate for this change in
information state, links between active landmarks and/or
the robot are also updated. The entire operation can be
performed in constant time.
requires the manipulation of some links between the robot and
other active landmarks. The resulting network is only an ap-
proximation to the original one, and its quality depends on the
strength of the link before its removal.
H
0
t
= S
x,Y
+
,Y
0
S
T
x,Y
+
,Y
0
H
t
S
x,Y
+
,Y
0
S
T
x,Y
+
,Y
0
b
0
t
= b
t
S
x,Y
+
,Y
0
S
T
x,Y
+
,Y
0
L
0
t
= [S
x
(S
T
x
H
t
S
x
)
−1
S
T
x
+ S
Y
0
(S
T
Y
0
H
t
S
Y
0
)
−1
S
T
Y
0
−S
x,Y
0
(S
T
x,Y
0
H
t
S
x,Y
0
)
−1
S
T
x,Y
0
]H
0
t
˜
H
t
= H
t
− H
0
t
L
0
t
˜
b
t
= b
t
− b
0
t
L
0
t
S
Y
0
S
T
Y
0
+ (µ
T
t
˜
H
t
− b
t
)S
x,Y
+
S
T
x,Y
+
(10)
To implement these equations, it is necessary to subdivide the
set of landmarks into three subsets: the set of active land-
marks Y
+
that contain a non-zero link to the robot pose in
the information matrix H
t
; the set of passive landmarks Y
−
;
and finally the set of landmarks Y
0
that are active before the
sparsification step, but passive afterwards. Our sparsification
equations have the effect of removing links between Y
0
and
the robot pose — a step necessary if the number of land-
marks linked to the robot exceeds a given sparsity threshold.
By doing so, the number of between-landmark links also re-
mains limited. Consequently, the sparsification step ensures
the sparseness of the information matrix H
t
in SEIFs. We note
that this step is approximate. The question as to whether this
approximation affects SEIF’s performance in practice has not
been addressed previously.
2.6 Amortized Map Recovery
Finally, SEIFs offer an algorithm that also requires constant
time for recovering the means µ
t
from the information form.
The recovery of the means might be interesting if one would
like to visualize the map: The information form contains
the map only implicitly, and the obvious recovery via Equa-
tions (4) and (5) would require cubic time. More impor-
tantly, the means of the robot pose and active landmarks’
locations are required in several of the above update steps,
(8), (9) and (10). SEIFs use an amortized iterative method,
similar to the Jacobi method or the slightly different Gauss-
Seidel method, to gradually recover µ
t
. One sufficient con-
dition for this kind of method to converge is the positive def-
inite condition which the H
t
matrix satisfies. The fact that
H
t
is diagonally dominant also makes these methods con-
verge fast. To describe the algorithm in detail, let us write
H
t
in four blocks H
t
=
A B
B
T
C
and accordingly have
µ
t
= (µ
A
µ
C
), b
t
= (b
A
b
C
). Block A should include the
components of H
t
that correspond to robot pose, active land-
marks’ positions and possibly a constant number of other ele-
ments, such as the locations of those landmarks linked to ac-
tive landmarks. To recover µ
t
in one step, one needs to solve
two equations
Aµ
A
+ Bµ
C
= b
A
B
T
µ
A
+ Cµ
C
= b
C
(11)
Following the idea of iterative methods mentioned above, only
part of µ
t
, i.e. µ
A
is updated by µ
A
= A
−1
(b
A
− Bµ
C
) in
one step. Since matrices A and B have only a limited number
of nonzero elements, an update shall be carried out in constant
time. Iterations are performed whenever components of µ
t
are
inquired. If some components of µ
A
are changed by signifi-
cant amounts, for example, when a loop is being closed, extra
steps may be taken to update those components together with
landmarks linked directly with them. Further, such extra steps
can be buffered to work out when the computer is idling, thus
the computing power is better utilized. As the robot explores
the environment, active landmarks change, so all the compo-
nents of µ
t
get chances to be updated. This amortized map re-
covery introduces additional error to the system. However ex-
periments show that the error is insignificant when compared
to the advantages of SEIF in other aspects.
3 Data Association
3.1 Recovering Data Association Probabilities
Finally, practical domains are characterized by data associ-
ation problems. Data association problems arise when in-
dividual landmarks in the environment cannot be identified
uniquely based on sensor measurements alone. The data as-
sociation problem is pervasive in real-world SLAM problems.
However, the original publication [13] did not address this im-
portant problem.
Our mechanism for handling the data association problem uses
a maximum likelihood estimator, together with a thresholded
χ
2
test. In particular, our approach selects the landmark that
best explains a measurement. If we write n
t
as the landmark
index of the landmark seen at time t, the maximum likelihood
estimator determines
n
∗
t
= argmax
n
p(n
t
|z
t
, u
t
)
p. 4
xx
tt
yy
nn
Figure 6: The combined Markov blanket of landmark y
n
and robot
x
t
is sufficient for calculating the posterior probability
of the landmark locations, conditioning away all other
landmarks. This insight leads to a constant time method
for recovering the approximate probability distribution
p(x
t
, y
n
|z
t−1
, u
t
).
= argmax
n
p(n
t
|z
t
) p(ξ|z
t−1
, u
t
)
|
{z }
¯
H
t
,
¯
b
t
(12)
where z
t
= z
1
, ...., z
t
and u
t
= u
1
, ...., u
t
. If the expression
inside the argmax is smaller than a threshold α, that is, none
of the landmarks in the map explains the measurement with
a minimum required probability, the landmark is considered
new and the filter is grown accordingly. By using this test, the
resulting SEIF gradually builds up a network of landmarks,
while nearby landmarks are connected by links. This approach
is commonly used in the context of EKFs [2]. In EKFs, calcu-
lating p(n
t
|z
t
, u
t
) is easy since it is straightforward to extract
the mean and the covariance of a landmark position together
with the robot pose from the full state estimate. The mean and
the covariance define a probability density p(x
t
, y
n
|z
t−1
, u
t
)
which are then used to calculate the probability p(n
t
|z
t
, u
t
).
In SEIFs, the situation is more complicated: Recovering the
covariance of a landmark location and the robot pose in the
naive way would require inverting a large matrix, which is a
O(N
3
) operation. However, we can once again exploit the
sparseness of the information matrix to obtain a high fidelity
approximation of the necessary covariances.
Suppose we would like to calculate the probability distribution
of the n-th landmark y
n
and the robot pose x
t
. The idea is to
do so by conditioning on all state variables outside the Markov
blankets of these variables. The Markov blanket of the robot
pose x
t
is simply the set of all active landmarks. Likewise, the
Markov blanket of landmark y
n
is the set of all landmarks (and
possibly the robot pose) directly connected to this landmark in
the SEIF. Figure 6 illustrates the situation. Shown there is a
landmark, a robot pose, the Markov blanket of the landmark
(inside the square box) and the set of all active landmarks (in
black). All other variables are not considered during this op-
eration, since they do not assert a direct influence on the robot
pose or landmark in question.
Mathematically, we do the following approximation, where
Y
+
n
is the combined Markov blanket:
p(x
t
, y
n
|z
t−1
, u
t
)
=
Z
p(x
t
, y
n
|Y
+
n
, z
t−1
, u
t
) p(Y
+
n
|z
t−1
, u
t
)dY
+
n
≈
Z
p(x
t
, y
n
|Y
+
n
) p(Y
+
n
|Y
−
n
)dY
+
n
(13)
Here
Y
−
n
= Y \{x
t
, y
n
}\Y
+
n
(14)
is the set of all state variables not included in the Markov blan-
ket Y
+
n
, and also excluding y
n
and x
t
. This approximation
ignores a residual uncertainty in remote state variables. How-
ever, we found that empirically it approximates the true poste-
rior probability needed for data association with double-digit
accuracy in most cases.
Apart from the mathematical reasoning, the operation in ma-
trix form is simple. The distribution p(x
t
, y
n
|z
t−1
, u
t
) is ap-
proximated by a Gaussian with covariance
Σ
t:n
= (S
T
x
t
,y
n
,Y
+
n
H
t
S
x
t
,y
n
,Y
+
n
)
−1
(15)
This calculation is constant time, since it involves a matrix
whose size is independent of N.
In our experiment, we found this approximation to work sur-
prisingly well. In the results reported further below using real-
world data, the average relative error in estimating likelihoods
is 3.4 × 10
−4
. Association errors due to this approximation
were practically non-existent.
3.2 Map Management
Our exact mechanism for building up the map is closely re-
lated to standard procedures in the SLAM community [2]. Due
to fake landmark detections caused by moving objects, addi-
tional care has to be taken to filter out those interfering mea-
surements. For any detected object that can not be explained
by existing landmarks, a new landmark candidate is generated
but not put into SEIF directly. Instead it is added into a waiting
list with a weight representing its probability of being a use-
ful landmark. In the next measurement step, the newly arrived
candidates are checked against all candidates in the waiting
list; reasonable matches increase the weight of corresponding
candidates. Candidates that are not matched lose weight be-
cause they are more likely to be a moving object. When a
candidate has its weight above a certain threshold, it joins the
SEIF network of landmarks.
We notice that data association violates the constant time prop-
erty of SEIFs. This is because when calculating data associa-
tions, multiple landmarks have to be tested. If we can ensure
that all plausible landmarks are already connected in the SEIF
by a short path to the set of active landmarks, it would be fea-
sible to perform data association in constant time. In this way,
the SEIF structure naturally facilitates the search of the most
p. 5
