EKF SLAM updates in O(n) with Divide and Conquer SLAM

L.M. Paz, P. Jensfelt, J.D. Tardós and J. Neira

n 1 join, 1 resulting map

Abstract— In this paper we describe Divide and Conquer
SLAM (D&C SLAM), an algorithm for performing Simulta-
n/2 n/2 2 joins
neous Localization and Mapping using the Extended Kalman
Filter. D&C SLAM overcomes the two fundamental limitations n/4 n/4 n/4 n/4 4 joins
of standard EKF SLAM: 1- the computational cost per step is
reduced from O(n2 ) to O(n) (the cost full SLAM is reduced
from O(n3 ) to O(n2 )); 2- the resulting vehicle and map
estimates have better consistency properties than standard 4p
EKF SLAM in the sense that the computed state covariance
2p 2p 2p m/2 joins
adequately represents the real error in the estimation. Unlike
many current large scale EKF SLAM techniques, this algorithm
p p p p p p m loc al maps
computes an exact solution, without relying on approximations
or simplifications to reduce computational complexity. Also,
estimates and covariances are available when needed by data Fig. 1. Binary tree representing the hierarchy of maps that are created
association without any further computation. Empirical results and joined in D&C SLAM. The leaves of the tree are the sequence of
show that, as a bi-product of reduced computations, and with- local maps of minimal size (p) that the algorithm computes with normal
out losing precision because of approximations, D&C SLAM EKF SLAM. The intermediate nodes represent the maps resulting from
has better consistency properties than standard EKF SLAM. intermediate joining steps that are carried out, and their final size.
Both characteristics allow to extend the range of environments
that can be mapped in real time using EKF. We describe the
algorithm and study its computational cost and consistency Kalman Filter, for SLAM updates. The Sparse Extended
properties.
Information Filter (SEIF) algorithm [17] approximates the
I. INTRODUCTION information matrix by a sparse form that allows O(1) updates
and O(n) computations of the state vector x. Nontheless,
Simultaneous Localization and Mapping (SLAM) consists data association becomes more difficult when covariance
in building a map of an unknown environment by traversing it matrix is not available, and the approximation can yield
using a vehicle with an onboard sensor, while simultaneously overconfident estimations of the state [6]. This overconfi-
determining the vehicle location within the map. In the dence is overcome by the Exactly Sparse Extended Infor-
Extended Kalman Filter solution to SLAM (EKF SLAM), mation Filter (ESEIF) [18] with a strategy that produces an
this problem is stated as a stochastic estimation process, in exactly sparse Information matrix with no introduction of
which a move-sense-update cycle is carried out. At every inaccuracies through sparsification. The Thin Junction Tree
step, the EKF is used to obtain the state vector estimate x̂ Filter algorithm [14] works on the Gaussian graphical model
containing the vehicle pose and n feature locations, along represented by the Information matrix, and achieves high
with the estimated error covariance matrix P. scalability by working on an approximation where weak links
The EKF solution to SLAM has been used successfully are broken. The Treemap algorithm [8] is a closely related
in small scale environments. However, the O(n2 ) cost of technique.
updating the covariance matrix at each step limits the use of Most of these algorithms focus on computational is-
EKF SLAM in large environments [3], [9]. This has been sues, ignoring another important limitation of standard EKF
subject of much interest in research. Early improvements SLAM that has gained attention recently: the effect of
include Postponement [12], the Compressed EKF filter [9], linearizations in the consistency of the final vehicle and
and Local Map Sequencing [16]. These algorithms work on feature estimates. Given that SLAM is a nonlinear problem,
local areas of the stochastic map and are essentially constant the Kalman Filter, designed for linear systems, is extended
time most of the time, although they require periodical by linearizing around the current estimate. This introduces
O(n2 ) updates. More recently, researchers have pointed out errors in the estimation process that can render the result
the approximate sparseness of the information matrix, the inconsistent, in the sense that the computed state covariance
inverse of the full covariance matrix P, that suggests using P does not represent the real error in the estimation [11], [4],
the Extended Information Filter, the dual of the Extended [1]. Among other things, this shuts down data association,
L.M. Paz, J.D. Tardós and J. Neira are with the Departamento which is based on contrasting predicted feature locations
de Informática e Ingenieria de Sistemas, Centro Politécnico Superior, with observations made by the sensor. Thus, important
Universidad de Zaragoza, Zaragoza, Spain {linapaz, jneira, processes in SLAM like loop closing are crippled. The
tardos}@unizar.es
P. Jensfelt is with the Royal Institute of Technology (KTH), Stockholm, Unscented Kalman Filter (UKF) [10] avoids linearization
Sweden [email protected] via a parametrization of means and covariances through
selected points to which the nonlinear transformation is Algorithm 1 dc slam:
applied. Unscented SLAM has been shown to have improved sequential implementation using a stack.
consistency properties [13]. Graphical SLAM [7] works on stack = new()
the Gaussian graphical model, and handles non-linearities m0 = ekf slam()
and reversible data associations. These solutions however stack = push(m0 , stack)
ignore the computational complexity problem. {
In this paper we propose Divide and Conquer SLAM Main loop: postorder traversing of the map tree.
(D&C SLAM), an EKF SLAM algorithm that addresses both }
the computational problem and the consistency problem. It repeat
is based on the idea of Local Map Sequencing proposed in mk = ekf slam()
[16]. In Local Map Sequencing, instead of working on a while ¬ empty(stack) and then
single absolute map, a sequence of local independent maps size(mk ) ≥ size(top(stack)) do
of equal constant size are generated as the vehicle traverses m = top(stack)
the environment, and then joined at fixed intervals during the stack = pop(stack)
process to produce the final absolute map. For local maps of mk = join(m, mk )
fixed size, it was shown in [16] that the computational cost end while
could be reduced by a large constant factor, but was still stack = push(mk , stack)
O(n2 ) in every map joining step. The algorithm proposed until end of map
here works with a binary tree of local maps of different {
sizes (see fig. 1), so that the number of map joining steps is Wrap up: join all maps in stack for full map recovery.
minimized and map joining can be performed in an amortized }
way. We show that the computational complexity at each while ¬ empty(stack) do
step is reduced from O(n2 ) to O(n), with a total cost m = top(stack)
for full SLAM of O(n2 ) compared to O(n3 ) for standard stack = pop(stack)
EKF SLAM. Furthermore, map joining is known to exhibit mk = join(m, mk )
better consistency properties than full EKF SLAM [15], [5]. end while
Here we show that the parametrization which the proposed return (mk )
algorithm carries out on the environment map produces
always vehicle and map estimates with better consistency
properties than those provided by standard EKF SLAM. of double size, which in turn will be joined into m/4 local
Since the algorithm works with the Kalman Filter form and maps of quadruple size, until the final map of size n will be
no approximations or simplifications are required, central the result of joining 2 maps of size n/2.
processes such as data association can be carried out as in Carrying out this process sequentially amounts to travers-
standard EKF SLAM with no further processing. ing the binary tree in postorder, and can be easily imple-
This paper is organized as follows: section II contains a mented using a stack of maps (see Algorithm I).
description of the proposed algorithm, and a study of its
computational cost. In section III we study the consistency
A. Computational Complexity of full D&C SLAM
properties of D&C SLAM in the simulated experiments. We
have used a simulated experiment because Monte Carlo runs Without loss of generality, consider performing SLAM in
allow to gather statistically significant evidence about the an environment where the density of features is uniform. At
consistency properties of the algorithms being compared. every step k, the onboard sensor of limited range provides
Finally in section IV we draw the main conclusions of this a set of measurements. During exploration, a fraction will
work and discuss future directions of research. Appendix correspond to features already in the map, and the rest will
I contains the mathematical details of the improved map correspond to new features that should be included in the
joining process that we use in this work. map. While we carry out a straightforward trajectory (before
loop closing, see fig. 2), the map will grow in size in
II. THE DIVIDE AND CONQUER SLAM proportion to k. Thus, the cost of an update will be O(k 2 ),
ALGORITHM and the final cost of full EKF SLAM will be cubic on the
The central idea of D&C SLAM is very simple: instead of total number of steps.
doing Local Map Sequencing, building a sequence of local Assume mapping such an environment using D&C SLAM,
maps of some fixed size (see [15] for a discussion on how starting with local maps of some fixed maximal size of p
to decide this size for a given sensor and environment), and features. If the total size of the environment requires m
then joinining them sequentially to form the complete map, such maps to be covered fully, the total number of map
D&C SLAM joins local maps in a binary tree fashion (see features will be at most n = p m. The cost of building
fig. 1). Standard EKF SLAM is carried out up to a fixed each map of p features will be O(p3 ). Considering only the
maximal (small) size p. For a given environment requiring higher order term, each local map will cost K1 p3 . Thus, the
m such local maps, they will be joined into m/2 local maps computational cost of these m local maps will be K1 p3 m.
 This means that D&C SLAM performs SLAM with a total
cost quadratic with the size of the environment, as compared
with the cubic cost of standard EKF SLAM.

B. Computational Complexity of D&C SLAM per step

Fig. 3 (top) shows the computational cost per step for
256 steps of D&C SLAM versus EKF SLAM in a sim-
ulated experiment in which the vehicle performs a 1m
motion at every step in a 3D environment of 1036 features.
The odometry of the vehicle has errors in each motion
step with a standard deviation of 10cm in the x direction
(the direction of motion), 5cm in y and z directions, and
(1deg, 0.5deg, 0.5deg) for Roll, Pitch and Yaw angles. We
simulate an onboard range and bearing sensor with a range
of 3m, so that 12 features are normally seen at every step.
The measurement error is 2% of the distance in range, and
0.5deg in bearing.
Fig. 2. Initial trajectory and map for the simulated 3D experiment. We can see that the computational cost of D&C SLAM
is very low for most steps compared with standard EKF
SLAM, except in those steps that are a power of 2. In
In the proposed algorithm, these m maps will be joined those cases, several map joinings may take place at the same
into m/2 maps of approximately double size (slightly less step to complete the map. This results in a slightly higher
when there are repeated features). Map joining is O(n2 ) on computational cost for D&C compared with EKF. All map
the final map size n. Again considering only the higher order joining operations are quadratic on the number of features
computational cost, each of these map joinings will thus on the resulting map. However, in D&C SLAM, the map to
have a K2 (p + p)2 computational cost, for a total cost of be generated at step k will not be required for joining until
K2 (2p)2 (m/2). These in turn will be joined into m/4 maps step 2 k. We can therefore amortize the cost O(k 2 ) of this
at a cost of K2 (4p)2 each, for a total cost of K2 (4p)2 (m/4). join by dividing it up between steps k to 2 k − 1 in equal
Continuing in this fashion, the total computational cost of O(k) computations for each step. We must however take into
this process will be: account all joinings to be computed at each step. If k is a
power of 2 (k = 2j ), j joinings will take place at step k,
log2 m

with a cost O(22 ) + . . .+ O((2j )2 ). To carry out the last join
m i 2 in the step, the previous join j − 1 in the same step should
C = K 1 p3 m + K2 (2 p)
i=1
2i be complete. Thus if we wish to amortize all joins, we must
log2 m

wait until step k + k/2 for join j − 1 to be complete, and
m i 2
= K 1 p3 m + K 2 p2 (2 ) then start join j. For this reason, the amortized version of
i=1
2i this algorithm is carried out by dividing up the largest join
log2 m

at step k into steps k + k/2 to k + k − 1 in equal O(2 k)
= K 1 p3 m + K 2 p2 m 2i computations for each step. The next-to-largest join in the
i=1 step will be divided into steps k + k/4 to k + k/2 − 1 in
The sum is a geometric progression of the type: equal O(k) computations each, and so on. In this way, the
cost of D&C SLAM per step becomes linear with n in the
m

rm+1 − r amortized version.
ri = Fig. 3 (middle) shows the resulting amortized cost for 256
i=1
r−1
steps in this simple simulation. Note that at steps i = 2j , the
Thus, in this case: cost falls steeply. As said before, in these steps j joins should
be computed, but since join i required the map resulting
 
3 2 2log2 m+1 − 2 from join i − 1, all j joins are postponed. If at any moment
C = K1 p m + K2 p n during the map building process the full map is required for
2−1
3 2 another task, it can be computed in a single O(n2 ) step. D&C
= K1 p m + K2 p m (2 m − 2)
  SLAM can then continue normally with this single map in
= 2 K 2 p 2 m2 + K 1 p 3 − 2 K 2 p 2 m the stack. Fig. 3 (bottom) shows the total execution time of
Given that n = p m: both algorithms. Given that the computational cost per step
of D&C SLAM is lower than that of EKF SLAM most of
  the time, the total cost of D&C SLAM increases very slowly
C = 2 K 2 n2 + K 1 p2 − 2 K 2 p n compared to the total cost of EKF SLAM.
 III. CONSISTENCY IN D&C SLAM
Consistency analysis determines how well the covariance
matrix P represents the error in estimate x̂.
4.5
EKF
D&C If the ground truth solution x for the state variables is
available, a statistical test for filter consistency can be carried
4
out on the state estimate (x̂, P). The Normalized Estimation
3.5 Error Squared (NEES) is defined as:

3
D2 = (x − x̂)T P−1 (x − x̂) (1)
Time (s)

2.5
Consistency is then checked using a chi-squared test:
2

D2 ≤ χ2r,1−α (2)
1.5

1
where r = dim(x) and α is the desired significance level
(usually 0.05). If we define the consistency index of a given
0.5 estimation (x̂, P) with respect to its true value x as:

50 100 150 200 250 D2

Step CI = , (3)
χ2r,1−α
4 EKF
D&C
the estimation is consistent with ground truth when CI < 1,
3.5 and inconsistent (overconfident) when CI > 1.
We tested consistency of both standard EKF and D&C
SLAM algorithms by carrying out 20 Monte Carlo runs
3

2.5
on the simulated experiment. Figure 4, top left, shows the
evolution of the mean consistency index of the vehicle
Time (s)

2 orientation (roll angle) during all steps of the simulation.

We can see that the D&C estimate always has a lower
1.5
consistency index than the standard EKF estimate, and falls
out of consistency at a much lower rate. Fig. 4 (bottom,
1
left) shows the mean consistency index for all features in
0.5
the map. Again, the D&C feature estimates have always a
lower consistency index than those of standard EKF SLAM
50 100 150 200 250
and fall out of consistency at a much lower rate. Note that for
Step D&C SLAM, consistency is pointed out by triangles in those
EKF
D&C
steps which are a power of 2, when a full map is available
350 (although we compute consistency at every step).
Figures 4 (right) show the evolution of the mean absolute
300
roll error of the vehicle (top) and mean absolute lateral error
for all features (bottom). The 2σ bounds for the theoretical
250
(without noise) and computed (with noise) uncertainty of
both standard EKF and Divide and Conquer SLAM algo-
Time (s)

200
rithms are also drawn. We can see that the error increases at
150
a slower rate in the case of D&C SLAM; we can also see
that the main cause of inconsistency in the standard EKF
100 SLAM is the fast rate at which the computed uncertainty
falls below its theoretical value.
50 We carried out another simulated experiment to test the
D&C SLAM consistency when the vehicle moves along a
50 100 150 200 250 loop trajectory of 64 steps. The robot estimate was computed
Step
by joining all maps available in every step. Both D&C SLAM
Fig. 3. Cost per step of D&C v.s. EKF SLAM (top); Amortized cost per and EKF SLAM were executed with exactly the same data
step of D&C and EKF SLAM (middle). Total execution time of EKF .vs. (including random errors). Fig. 5 shows a typical situation:
D&C SLAM (bottom). EKF SLAM (top) is overconfident: errors are larger than
the computed covariances suggest, and thus loop closing is
not possible. D&C SLAM (bottom) computes estimates and
covariances that allow it to easily close the loop.
 3.5 0.7
EKF EKF theorical uncertainty
D&C EKF computed uncertainty
Bound EKF Error
3 0.6 D&C Theorical Uncertainty
D&C computed uncertainty
D&C Error

2.5 0.5

2 0.4

Error (rad)
Index

1.5 0.3

1 0.2

0.5 0.1

0 0
50 100 150 200 250 50 100 150 200 250
steps Steps
45
4 EKF EKF theorical uncertainty
D&C EKF computed uncertainty
Bound 40 EKF Error
D&C Theorical Uncertainty
3.5 D&C computed uncertainty
35 D&C Error

3
30

2.5
25
Error (m)
Index

2
20

1.5
15

1 10

0.5 5

0 0
50 100 150 200 250 50 100 150 200 250
steps Steps

Fig. 4. Mean consistency index (left) and Mean Absolute error (right) for standard EKF SLAM (black) and D&C SLAM (blue). Roll Robot error(top);
lateral error for all map features (bottom); in all cases, at all steps of the vehicle trajectory.

IV. CONCLUSIONS between two local maps. Depending on the type of trajectory,
the size and overlap between the maps will change. This will
In this paper we propose Divide and Conquer SLAM, a
require developing specialized data association algorithms to
computationally more feasible alternative to standard EKF
keep the amortized cost always linear.
SLAM. Its computational cost per step is O(n), as com-
We hope to demonstrate that D&C SLAM is the algorithm
pared to O(n2 ) for standard EKF SLAM. D&C SLAM
to use in all applications in which the Extended Kalman
is a simple algorithm to implement, and in contrast with
Filter solution is to be used.
many current efficient SLAM algorithms, all information
required for data association is available when needed with V. ACKNOWLEDGMENTS
no further processing. D&C SLAM computes the exact EKF This research has been funded in part by the Dirección
SLAM solution, both the estimate x̂ and covariance P, with General de Investigación of Spain under projects DPI2003-
no approximations, and with the additional advantage of 07986 and DPI2006-13578, the Swedish Foundation for
providing always a more precise and consistent vehicle and International Cooperation in Research and Higher Education
map estimate. (STINT), under project IG2003-2 060, and the Swedish
One of the main subjects of our future work will be Foundation for Strategic Research through the Centre for
to study D&C with other simulations experiments (explore Autonomous Systems.
and return, loop closing, lawn mowing) to show that the
amortized cost is kept linear. Also, future work will be to A PPENDIX I: M AP J OINING 2.0
carry out a large scale experiment where the advantages This appendix describes the map joining process used
and limits of D&C SLAM can be experimentally evaluated. in D&C SLAM, an improved version with respect to the
Data association is required to carry out the joining process original map joining 1.0 in [16]. The general idea is the
 second map). This results in having the last vehicle location
in the first map, Rj , be the base reference of the second map,
which allows maps to be joined into a full map in a three step
process of (1) joining; (2) update; and (3) transformation, as
it is explained next.
A. The Map Joining step
Consider two sequential local maps mi...j =
(x̂i...j , Pi...j ), mj...k = (x̂j...k , Pj...k ), with n features
F1 . . . Fn and m features G1 . . . Gm each:

   
xRi Rj xRj Rk
 xRi F1   xRj G1 
   
x̂i...j =  ..  ; x̂j...k =  ..  (4)
 .   . 
xRi Fn xRj Gm
In this approach, the joining
 − step allows
 to obtain a
stochastic map m− i...k = x̂i...k , P−i...k in the following
simple way:

x̂i...j
x̂− = (5)
i...k x̂j...k

− Pi..j 0
P̂i...k = (6)
0 Pj..k
Note that the elements in the second map are kept in their
own reference Rj instead of being referenced to reference
frame Ri as in map joining 1.0. This has the effect of
delaying the linearization process of converting all features
to base reference Ri until the update step has taken place,
and thus an improved estimation is used for this linearization.
This is the fundamental difference between map joining 1.0
and map joining 2.0
Fig. 5. Conditions on arriving at loop closing: EKF SLAM (top), D&C B. The update step
SLAM (bottom). Ground true trajectory in red, estimated trajectory in blue.
Data association is carried out to determine correspon-
dences between features coming from the first and second
same: in a sequential move-sense-update cycle, a local map map. This allows to refine the vehicle and environment fea-
is initialized at some moment i using the current vehicle ture locations by the EKF update step on the state vector. Let
location Ri as base reference, and thus the initial vehicle H be a hypothesis that pairs r features Ff1 . . . Ffr coming
location in the map is xRi Ri = 0 an also the initial vehicle from local map mi...j with features Gg1 . . . Ggr coming from
uncertainty PRi = 0. Standard EKF SLAM is carried out map mj...k . A modified ideal measurement equation for r
in a this move-sense-update fashion, until the map reaches a re-observed features expresses this coincidence:
certain size of n features F1 . . . Fn at step j. In this moment  
hf1 ,g1
the state vector x̂i...j will be:  .. 
hH (x̂−
i...k ) =  . =0 (7)
 
x̂Ri Rj hfr ,gr
 x̂Ri F1 
  where for each pairing:
x̂i...j = .. 
 . 
x̂Ri Fn hfr ,gr = xRi Ffr − xRi Rj ⊕ xRj Ggr .

with corresponding covariance matrix Pi...j . This map is Linearization yields:

then closed, and a new local map mj...k = (x̂j...k , Pj...k ) is
initialized in the same way (for simplicity, assume the sensor hH (x̂− − − −
i...k )  hH (x̂i...k ) + HH (xi...k − x̂i...k ) (8)
measurements at step j are used to update the first map, and
the vehicle motion from Rj to Rj+1 is carried out in the where:
 [6] R. Eustice, M. Walter, and J. Leonard, Sparse extended information
filters: Insights into sparsification, Proceedings of the IEEE/RSJ In-
∂hH
HH = |(x̂− ) ternational Conference on Intelligent Robots and Systems, Edmonton,
∂x−
i...k
i...k Alberta, Canada, August 2005.
 ∂h ∂hf1 g1
 [7] J. Folkesson and H. Christensen, Graphical SLAM - A Self-Correcting
f1 g1
0 ··· I 0 ··· Map, Proc. of the IEEE International Conference on Robotics and
 ∂xRi Rj ∂xRj Gg1
 Automation (ICRA’04), New Orleans, LA, USA, 2004.
 .. .. .. .. .. .. .. 
=  . . . . . . .  [8] U. Frese, ”Treemap: An O(logn) Algorithm for Indoor Simultaneous
  Localization and Mapping”, Autonomus Robots, 21(2) pp. 103-122,
∂hfr gr ∂hfr gr
∂xR R 0 I · · · 0 ··· ∂xRj Ggr 2006.
i j
[9] J. Guivant and E. Nebot, ”Optimization of the Simultaneous Local-
(9) ization and Map-Building Algorithm for Real-Time Implementation”,
IEEE Transactions on Robotics and Automation, 17(3) pp. 242-257,
 The update step allows to obtain a new estimate m+
i...k = 2001.
x̂i...k , P+
+
i...k by applying modified EKF update equations: [10] S. Julier J. and Uhlmann. ”A new extension of the Kalman
Filter to nonlinear systems”, In International Symposium on
Aerospace/Defense Sensing, Simulate and Controls, Orlando, FL.
1997.
x̂+
i...k = x̂− −
i...k − KhH (x̂i...k ) [11] S.J. Julier, J.K. Uhlmann, J.K. A Counter Example to the Theory of
P+
i...k = (I − KHH )P−
i...k
Simultaneous Localization and Map Building 2001 IEEE Int. Conf.
on Robotics and Automation, 2001, 4238-4243
[12] J. Knight, A. Davison and I. Reid, ”Towards Constant Time SLAM
where: using Postponement” IEEE/RSJ Int’l Conf on Intelligent Robots and
Systems, pp 406-412, 2001.
 
T −1
K = P− T −
i...k HH HH Pi...k HH
[13] R. Martinez-Cantin and J. A. Castellanos, ”Unscented SLAM for
large-scale outdoor environments”, 2005 IEEE/RSJ Int. Conference on
Intelligent Robots and Systems, IROS’05, Edmonton, Alberta, Canada,
C. The transformation step pp. 328-333.
A final step is carried out to transform all the elements [14] M. A. Paskin, Thin Junction Tree Filters for Simultaneous Localiza-
of x̂+
tion and Mapping, Proc. of the 18th Joint Conference on Artificial
i...k to the same base reference Ri and obtain the final Intelligence (IJCAI-03),San Francisco, CA. pp 1157–1164, 2003.
joined map mi...k = (x̂i...k , Pi...k ): [15] L. Paz and J. Neira, ”Optimal local map size for EKF-based
SLAM”, IEEE/RSJ Int. Conference on Intelligent Robots and Systems,
    IROS’06, Beijing, China.
x̂Ri Rk x̂+ +
Ri Rj ⊕ x̂Rj Rk [16] J. Tardós, J. Neira, P. Newman and J. Leonard, ”Robust Mapping
 x̂Ri F1   x̂+  and Localization in Indoor Environments using Sonar Data” Int. J.
   Ri F1 
   Robotics Research, 21, 311-330, 2002.
 .. ..
    [17] S. Thrun, Y. Liu, D. Koller, A.Y. Ng, Z. Ghahramani, and H.
 .   .  Durrant-Whyte, ”Simultaneous Localization and Mapping with Sparse
x̂i...k =  x̂Ri Fn = x̂+ 
   Ri Fn  Extended Information Filters”, The International Journal of Robotics
 x̂Ri G1   x̂+ + 
Ri Rj ⊕ x̂Rj G1
Research, 23, 693-716, 2004.
   
    [18] M. Walter, R. Eustice and J. Leonard, ”A Provably Consistent Method
  
.. ..
 .  .  for Imposing Sparsity in Feature-based SLAM Information Filters”,
Proc. of the Int. Symposium of Robotics Research (ISRR), 2004.
x̂Ri Gm x̂Ri Rj ⊕ x̂+
+
Rj Gm
 T
∂ x̂i...k + ∂ x̂i...k
Pi...k = Pi...k
∂ x̂+
i...k ∂ x̂+
i...k
 ∂x ∂xRi Rk

Ri Rk
∂xRi Rj 0 ∂xRj Rk 0
∂ x̂i...k  
=  0 I 0 0  (10)
∂ x̂+  ∂xRi E ∂xRi E

i...k
∂xRi Rj 0 0 ∂xRj E

Note again that this linearization is carried out once the

map has been refined in the previous update step, thus using
a better estimate.
R EFERENCES
[1] T. Bailey, J. Nieto, J. Guivant, M. Stevens and E. Nebot, ”Consistency
of the EKF SLAM Algorithm”, IEEE/RSJ International Conference on
Intelligent Robots and Systems, 2006.
[2] Y. Bar-Shalom, X. Rong Li and T. Kirubarajan, ”Estimation with
Applications to Tracking and Navigation”, Wiley InterScience, 2001.
[3] J. Castellanos, and J. Tardós, ”Mobile Robot Localization and Map
Building: A Multisensor Fusion Approach”. Boston, Mass. Kluwer
Academic Publishers. 1999
[4] J. Castellanos, J. Neira and J. Tardós, ”Limits to the Consistency of
EKF-based SLAM”, 5th IFAC Symposium on Intelligent Autonomous
Vehicles, 2004.
[5] J.A. Castellanos, R. Martinez-Cantin, J.D. Tardós and J. Neira, ”Robo-
centric Map Joining: Improving the Consistency of EKF SLAM”, to
appear in Robotics and Autonomous Systems.