Robotica

http://journals.cambridge.org/ROB
Additional services for Robotica:
Email alerts: Click here
Subscriptions: Click here
Commercial reprints: Click here
Terms of use : Click here

Towards features updating selection based on the covariance matrix of

the SLAM system state
Fernando A. Auat Cheein, Fernando di Sciascio, Gustavo Scaglia and Ricardo Carelli
Robotica / Volume 29 / Issue 02 / March 2011, pp 271 - 282
DOI: 10.1017/S0263574710000111, Published online: 31 March 2010

Link to this article: http://journals.cambridge.org/abstract_S0263574710000111

How to cite this article:
Fernando A. Auat Cheein, Fernando di Sciascio, Gustavo Scaglia and Ricardo Carelli (2011). Towards features updating
selection based on the covariance matrix of the SLAM system state. Robotica, 29, pp 271-282 doi:10.1017/
S0263574710000111
Request Permissions : Click here

Downloaded from http://journals.cambridge.org/ROB, IP address: 200.83.129.91 on 24 Nov 2013

Robotica (2011) volume 29, pp. 271282. Cambridge University Press 2010
doi:10.1017/S0263574710000111

Towards features updating selection based on the covariance

matrix of the SLAM system state
Fernando A. Auat Cheein , Fernando di Sciascio, Gustavo Scaglia
and Ricardo Carelli
Instituto de Automatica, National University of San Juan, San Juan, Argentina
(Received in Final Form: March 4, 2010. First published online: March 31, 2010)

SUMMARY
This paper addresses the problem of a features selection
criterion for a simultaneous localization and mapping
(SLAM) algorithm implemented on a mobile robot. This
SLAM algorithm is a sequential extended Kalman filter
(EKF) implementation that extracts corners and lines from
the environment. The selection procedure is made according
to the convergence theorem of the EKF-based SLAM.
Thus, only those features that contribute the most to the
decreasing of the uncertainty ellipsoid volume of the SLAM
system state will be chosen for the correction stage of
the algorithm. The proposed features selection procedure
restricts the number of features to be updated during the
SLAM process, thus allowing real time implementations
with non-reactive mobile robot navigation controllers. In
addition, a Monte Carlo experiment is carried out in order
to show the map reconstruction precision according to the
KullbackLeibler divergence curves. Consistency analysis
of the proposed SLAM algorithm and experimental results
in real environments are also shown in this work.
KEYWORDS: SLAM; Feature selection; Mobile robot.

1. Introduction
This article concerns the problem of the feature-based
simultaneous localization and mapping (SLAM) algorithm
applied to a mobile robot. The correction stage of the SLAM
is restricted to the features of the environment that are
most significant for the consistency and convergence of the
algorithm. This restriction decreases the computational cost
of the algorithm, making it more suitable for non-reactive
control applications. It is also presented a map divergence
analysis when compared the obtained SLAMs map with
respect to other maps of the same environment.
The mobile robotic field is composed by three main areas:
navigation, localization and mapping. The knowledge of the
robot about its position inside an environment and the way the
vehicle interacts with it defines the autonomy of the robot.
A mobile robot is said autonomous if there is no external
supervision in order to accomplish, efficiently, the robotic
task. To reach full autonomy, the robot has to know its
position within the environment (localization), to model the
* Corresponding author. E-mail: fauat@inaut.unsj.edu.ar

http://journals.cambridge.org

Downloaded: 24 Nov 2013

environment (mapping) and to navigate through it.1,2 The

SLAM algorithm comes up as a solution to the problem of
localization and mapping within different environments.3
The SLAM algorithm recursively corrects the pose
position and orientation of the vehicle using environmental
information while the environment is modeled using the
estimated pose information4,5 at the same time. The SLAM
considers the pose of the robot and the parameters that model
the environment as part of the same system state, therefore,
increasing its size as the knowledge about the environment
grows. Several solutions to the SLAM problem have been
proposed in the literature over the last 20 years, mainly
depending on the robotic task. The extended Kalman filter
(EKF) is one of the most used filters as a SLAM solution.6 It
models the environment and the vehicle as Gaussian random
variables. Due to the fact that the EKF works with linearized
models of the environment and the mobile robot motion,
the errors of the models are not strictly minimized.3 The
EKFSLAM processing time is bigger than other approaches
such as the information filter (IF) or the unscented Kalman
filter6 (UFK). In order to deal with non-Gaussian models,
the particle filter (PF) has shown a better performance
when dealing with bigger amount of data and with nonGaussian environments. Despite of this, the EKFSLAM is
widely used in robotics because of its simplicity and its easy
programming for practical robotics issues.79
The integration of the SLAM with control strategies
presents an incipient field of research.3,10,11 Although
reactive-based control is implemented in most of the SLAMcontrol applications, plan-based control integrating the map
derived by the SLAM remains under study. One of the key
problems in the fusion of SLAM with plan-based control
strategies in real time implementations is that the SLAM is
not a constant time algorithm. If the environment is modeled
by features, e.g. lines, corners, corridors, trees, furrows, the
SLAM system state increases its size as the robot acquires
more features from the environment.
The increment of the size of the SLAM system states
represents an increment of the processing time needed to
execute the SLAM algorithm. Furthermore, the processing
time of the EKFSLAM increases with the number of
features with appropriate association that will be used
during the correction stage of the algorithm. Several attempts
to restrict the processing time of the correction stage of
the SLAM algorithm can be found in the literature. For

IP address: 200.83.129.91

272

Towards features updating selection based on the covariance matrix of the SLAM system state

example, in ref. [12], a modification of the SLAM correction

stage is proposed, decreasing the computational time of the
algorithm.
The feature extraction procedure must be accurate, robust
and reliable. Many works found in the literature use these
characteristics of the feature extraction procedure to select
the features to be used by the SLAM algorithm. Thus, the
feature selection criterion depends on the feature extraction
procedure which in turn depends on the type of sensor
used by the robot. In refs. [4, 5] the authors present a
feature-based SLAM that extracts trees from the environment
using a Laser range sensor and a clustering algorithm. They
discard the elements of the environment that the cluster does
not give a reliable detection. In refs. [13, 14] the authors
present a visual-based SLAM that uses principal component
analysis (PCA) to extract features from the environment and
to discard redundant information. For indoor applications,
the most widely used features are geometric primitives that
can be associated with elements of the environments such
as walls associated with lines and corners. In ref. [8]
the authors present a clustering algorithm to extract lines
from range measurements. Thus, if the quality of a line, e.g.
the length of the associated wall, or the number of points
used by the cluster does not exceed a certain threshold,
that line is discarded and it will not be used during the
SLAM algorithm. Other algorithms have been proposed in
the scientific literature to extract lines. The work of15 uses the
Hough transformation to obtain lines from the environment
with the corresponding computational cost. The fusion of the
information provided by two or more exteroceptive sensors
in order to obtain a robust feature extraction procedure was
also implemented. In refs. [16, 17] the authors present the
fusion of a monocular video with a range laser in order to
extract lines, corners and planes from the environment. They
use fuzzy algorithms to determine the features and those
features with the lowest covariance are then selected.
In the works presented before, the features selecting
procedure is performed during the feature extraction or
detection process.18 Once the features are selected, they are
used by the SLAM algorithm. On the other hand, other works
attempted to restrict the number of features to be used during
the correction stage of the SLAM19,20 independently of the
feature extraction procedure. In ref. [21], a threshold of the
entropy of the feature calculated from the covariance matrix
of the SLAM system state is taken into account for choosing
the features to be used in the SLAM; in ref. [9], the SLAM
uses only the two last features for the correction. All these
works do not evaluate before the updating stage how the
feature will affect a priori the SLAM process.
In this work we propose an EKFSLAM with feature
updating selection criterion for choosing whether a feature
should be used in the correction stage of the SLAM.
The selection criterion is based on the evaluation of the
eigenvalues of the covariance sensibility ratio of the SLAM
system state. Thus, we restrict the correction stage to those
features with correct association that presents the smallest
sum of eigenvalues of the covariance ratio affected by the
observed feature. In that way, the correction is made in
all directions of the estimation process. Furthermore, the
EKFSLAM configuration used in this paper corresponds

http://journals.cambridge.org

Downloaded: 24 Nov 2013

to a sequential algorithm.6 The Jacobian matrices of the

observation models in the sequential EKFSLAM, are sparse
matrices which reduces the computational cost of the SLAM.
Real time experimental results along with consistency tests
and computational costs analysis are shown in this work.
The experimental results shown were carried out for both
the classical sequential EKFSLAM and the EKFSLAM
with feature updating selection proposed in this work. In
addition, a KullbackLeibler divergence test is carried out
in order to show that the map built by the EKFSLAM
with feature updating selection presents a smaller divergence
value with respect to the classical sequential EKFSLAM
when compared with a digitalized architectonic draw of the
real environment used for the experiments.
This paper is organized as follows: Section 2 presents
both the classical sequential EKFSLAM and the EKF
SLAM with features updating selection proposed in this
work. Section 3 shows the consistency tests of the SLAM
algorithm and the experimental results carried out at the
facilities of the Instituto de Automatica. Section 4 shows the
KullbackLeibler divergence curves of the different maps
built by the SLAM algorithms and Section 5 shows the
conclusions of this work.

2. Methods
In this section, the classic sequential EKFSLAM is
presented along with the EKFSLAM algorithm with
restriction over the number of features to be updated proposed
in this paper.
2.1. EKFSLAM algorithm
The EKF equations for the SLAM problem solution are
stated in Eqs. (1)(5). By hypothesis, all variables involved
in the estimation process are considered as Gaussian random
variables. Thus, the SLAM system state and the features
model have Gaussian distributions.
Prediction stage
t = f (t1 , ut ),

(1)

Pt = At Pt1 ATt + Wt Qt1 WtT .

(2)

Correction stage

1
Kt = Pt HtT Ht Pt HtT + Rt ,
t = t + Kt (zt h(t )),
Pt = (I

Kt Ht )Pt .

(3)
(4)
(5)

In Eqs. (1) and (2) the prediction stage of the EKF is stated,
where t is the predicted system state at time t; ut is the input
control commands and t is the corrected system state at time
t; f describes the evolution of the elements of . Pt and Pt are
the predicted and corrected covariance matrices respectively,
at time t; At is the Jacobbian matrix of f with respect to the
SLAM system state ( ) and Qt is the covariance matrix of the
noise associated with the process, whereas Wt is its Jacobbian
matrix. Equations (3)(5) represent the correction stage of
the EKFSLAM. Kt is the Kalman gain of the algorithm

IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state
at time t; Ht is the Jacobian matrix of the measurement
model (h in Eq. (4)) and Rt is the covariance matrix of the
actual measurement zt . In Eq. (4), (zt h(t )) is called the
innovation process6 and takes place when the data association
procedure has reached an appropriate matching between the
observed feature and the predicted one (h(t )). Both, the
process model (f in Eq. (1)) and the observation model are
non-linear expressions. In this work, the features extracted
from the environment correspond to lines and corners, either
concave or convex.
The SLAM system state is composed by both the vehicles
degrees of freedom and the map, represented by the
parameters that define the features. The map is considered
as stationary. Thus, the evolution computed in Eq. (1)
corresponds to the robots movements. More information
about this topic can be found in ref. [4, 5]. The mobile robot
used in this work is a non-holonomic unicycle type Pioneer
R

3DX, built by ActivMedia . Figure 1 shows a scheme of
the vehicle and Eq. (6) represents their kinematic equations.
R

The robot has a range sensor laser -built by SICK which
acquire 181 measurements, from 0 to 180 , within a 30
m range. The models of the features are shown in Eqs. (7)
and (8), and Fig. 2 shows the geometrical representation of
each variable used in (7) and (8). More information about
features extraction and the SLAM algorithm implemented in
this work can be found in refs. [3, 8]:

xt1
cos(t1 ) a sin(t1 )
xt
yt = yt1 + t sin(t1 ) a cos(t1 )
t
t1
0
1

ut
+ t ,
(6)


z
zcorner,t = hi [Poserobot,t , pi,t , wt ] = R
z



(xt xe )2 + (yt ye )2
wR


=
+
,
(7)

yt ye
w
arctan
t
xt xe


z
zline,t = hi [Poserobot,t , pi,t , wt ] =
z

Fig. 2. Feature extraction.



w
r xt cos() yt sin()
+
.
t
w

Downloaded: 24 Nov 2013

(8)

In Eq. (6), xt , yt and t are the coordinates of h the point

of control in Fig. 1; of t is the discrete time Gaussian
process noise associated with the robots model, ut is the
linear velocity command applied to the vehicle and t is the
angular velocity command. In Eqs. (7) and (8), w signals
wR , w , w , w represent the Gaussian measurement
noises associated with the observation models (zline,t and
zcorner,t ).
In this work, a sequential EKF algorithm was implemented
as in ref. [6]. In this algorithm, the correction iteration
is executed with one innovation at a time. This algorithm
implies that the Jacobian matrix of the observation model,
H, be a sparse matrix as shown in ref. [6]. Along with the
map generated by the SLAM process, a secondary map
was also implemented. This secondary map, storages the
beginning and ending points of the segments associated with
lines in the environment. It is also maintained and updated
according to the evolution of the SLAM system state. The
secondary map allows the environment reconstruction for
feasible navigation purposes. More details about this map
can be found in ref. [23].
2.2. Classical sequential EKFSLAM
The classical sequential EKFSLAM was previously
presented in ref. [6]. It has the property of sequentially
updating the SLAM system state one feature at a time. Thus,
the Jacobbian matrices of the observation model are sparse
which reduce the processing time of the algorithm. This is
due to the fact that the entries of the Jacobbian matrix of the
observation model that do not correspond to the observed
feature will be considered as zero. Algorithm 1 summarizes
the classical sequential EKFSLAM.
Let Nt be the set of observed features at time t
Let Mt Nt be the set of features with correct
matching
for i = 1 : #Mt

1
Kt = Pt HtT Ht Pt HtT + Rt
t = t + Kt (zt h(t ))

Fig. 1. Schematic of the mobile robot used.

http://journals.cambridge.org

273

IP address: 200.83.129.91

(i)
(ii)
(iii)
(iv)
(v)

274

Towards features updating selection based on the covariance matrix of the SLAM system state

Pt = (I Kt Ht )Pt
Mt = Mt {zi }
Pt := Pt , t := t
end for
Algorithm 1 Classical sequential EKFSLAM

(vi)
(vii)
(viii)
(ix)

Algorithm 1 represents the correction stage of the classical

sequential EKF-SLAM. Thus, the prediction stage of the
EKF along with the computing of the robots movements is
not shown. After observing the environment, those features
that present a correct data association with respect to the
predicted ones, will be used for the EKF correction sentence
(i) and (ii). In this paper, the data association method used
is based on the Mahalanobis distance.2,24 The for loop in
sentences (iii)(ix) shows the sequentially part of the EKFSLAM. For a given feature zt , the Kalman gain, the SLAM
system state updating and the posterior covariance of the
SLAM system state are carried out in sentences (iv)(vi).
Sentences (vii) implies that the zt feature is extracted from
the set of features previously found in (ii) and will no longer
be used during the correction stage. The SLAM system
state updated in (v) and the posterior covariance in (vi) will
become, respectively, the predicted SLAM system state and
the prior covariance matrix of the SLAM in the next iteration
of the algorithm sentence (ix). The for loop repeats until
all features with correct association were used.
2.3. Sequential EKFSLAM with features
updating selection
The features updating selection procedure proposed in this
paper does not alter the sequential execution of the EKF
SLAM presented in Section 2.2. On the contrary, the features
selection procedure bounds the correction stage to those
features with correct data association that will cause
only a meaningful correction of the SLAM system state.
In order to determine which features will be more relevant
for the SLAM system state, let us considerer the two
convergence theorems presented in ref. [25].
Theorem 1. Let Pt and Pt be the prior and posterior
SLAM system state covariance respectively. Then,
|Pt | |Pt |.

zi Mt , zopt : argz min(|Pt |) argz min(|I Kt Ht |).

(10)
In Eq. (10), Mt is the set of features with appropriate
association as was previously shown in Algorithm 1.
Equation (10) also provides the tool for choosing the features
(zopt ) that are more significant for the SLAM. Thus, those
features with the smallest values of |I Kt Ht | will be
used in the correction stage. Instead of processing the
entire determinant of (I Kt Ht ), we will calculate their
eigenvalues. Due to the fact that (I Kt Ht ) is a matrix
which dimensions vary according to the number of features of
the map, the calculation of their eigenvalues could demand
higher processing time than the classical sequential EKF
SLAM. To avoid this situation, lets considerer that Ht is a
sparse matrix of the form:
Ht = [Hv,t
1 Hz,t
2]

(11)

as was stated in Section 2.1. In Eq. (11), the Jacobbian of the

observation model (Ht ) is composed by the Jacobbian of the
observation with respect to the vehicles degrees of freedom
(Hv,t ) and the Jacobbian of the observation with respect to
the features parameters on the position of the feature (Hz,t );

1 and
2 are null block matrices.
The Kalman gain, Kt , can also be calculated in the form
of Eq. (11). Thus,

 T
T
T
T
K
1
Kz,t
K
2
.
Kt = Kv,t

(12)

Using Eqs. (11) and (12), the expression (I Kt Ht ) can be

calculated as

Iv

(I Kt Ht ) =

I
1
Iz
I
2

Theorem 2. Let Pt,M be the covariance matrix of the features

of the SLAM system state. Then,
lim |Pt,M | = 0.

Kv,t
K
1 

Kz,t Hv,t

Hz,t

2 ,

K
2

Taking into account Theorem 1 above and Eq. (5), the

following holds,

|Pt | = |I Kt Ht ||Pt |
|Pt |
= |I Kt Ht | 1.

|Pt | |Pt |
|Pt |
(9)
Equation (9) represents the covariance sensibility ratio of
the SLAM system state. Thus, according to Eq. (9), the
smaller |I Kt Ht |, the higher the correction. This is so,
because the determinant of the covariance matrix of the
SLAM system state (|Pt |) can be seen as a measure of the

http://journals.cambridge.org

volume of the covariance ellipse associated to the system

state. Considering also that |Pt | does not depend on the
observed feature, according to Eq. (9) can be stated that

Downloaded: 24 Nov 2013

Iv Kv,t Hv,t
K
1 Hv,t
=
Kz,t Hv,t
K
2 Hv,t

I
1

Kv,t Hz,t
K
1 Hz,t
Iz Hz,t
K
2 Hz,t

I
2
(13)

In Eq. (13), I is the identity matrix; Iv , I

1 , Iz , I
2 are identity
block matrices with the dimensions of Kv,t Hv,t , K
1
1 ,
Kz,t Hz,t and K
2
2 respectively, and
is a general null
block matrix.

IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state
Up to this point, it can be seen that the determinant
|I Kt Ht | can be calculated directly from Eq. (13). Thus,


Iv Kv,t Hv,t
Kv,t Hz,t



 K H
I
1 K
1 Hz,t


1 v,t

|I Kt Ht | = 
,

Iz Hz,t


 Kz,t Hv,t


 K
2 Hv,t

K
2 Hz,t I
2 


Iv Kv,t Hv,t
Kv,t Hz,t 

.
(14)
=
Kz,t Hv,t
Iz Kz,t Hz,t 
Eq. (14) shows the reduced form of |I Kt Ht |. If one of the
eigenvalues of Eq. (14), tends to zero faster than the others,
then the entire determinant will tend to zero. This could cause
that the posterior covariance matrix of the SLAM system
state, Pt , be corrected faster in the direction associated with
that eigenvalue than in the rest of the directions. Instead
of using Eq. (14) for choosing the significant features as
stated previously in this paper we propose to observe the
eigenvalues evolution of (I Kt Ht ). Nevertheless, the last
does not contradict Eq. (14), instead, the SLAM algorithm
will be able to choose the direction of the correction based
on the eigenvalues information. Thus, the determinant of Eq.
(14) provides lesser information than the observation of the
eigenvalues.
In order to calculate the eigenvalues of Eq. (13), the
following theorem is necessary.
Theorem 3. Let S nn be a non-singular matrix of the
form

A L1 B
S =
1 Ir
2 ,
C L2 D
where Ir is an identity block matrix with dimension r < n;
A, B, C, D, L1 , L2 are non-empty block matrices;
1 and

2 are null block matrices. Then, the following statement

holds,



A B
Eig
Eig(S).
C D
C
] is
Thus, the set of eigenvalues of the sub-matrix [ BA D
included in the set of eigenvalues of S.

Proof. Applying the definition of eigenvalue, we have that

L1
B
A IA
Ir Ir

2
S I =
1
C
L2
D ID
with IA and ID in the dimensions of A and D respectively.
Then,




B
r A IA

|S I | = (S I ) 
C
D ID 





IA
B
r A
,

= (S I ) 
C D
ID 



A B
Eig(S).
Eig
C D

http://journals.cambridge.org

Downloaded: 24 Nov 2013

275

According to this, the eigenvalues of S are: Eig(S) =

C
Eig([ BA D
]) Ones, where Ones is a set of r eigenvalues
equal to the unity.
Corollary. Identical demonstration is performed if S were
of the form

A
1 B
S = L1 Ir L2 .
C
2 D
Thus, according to Eq. (13), let us suppose that S = I
Kt Ht , then because of Theorem 3 we have that


Eig

Iv Kv,t Hv,t
Kz,t Hv,t

Kv,t Hz,t
Iz Kz,t Hz,t


Eig(I Kt Ht ).
(15)

Considering that Iv is a 3 3 matrix (the robot has three

degrees of freedom) and Iz is a 2 2 matrix (every feature
is defined by two parameters, see Eqs. (7) and (8)), the final
calculation in (15) are the eigenvalues of a 5 5 matrix.
Furthermore, by inspection of the proof of Theorem 3, it can
Kv,t Hv,t
Kz,t Hv,t
]
be concluded that the eigenvalues of [ IvK
Iz Kz,t Hz,t
v,t Hz,t
are the only eigenvalues of (I Kt Ht ) affected by the
observation zi in Algorithm 1, because the rest of the
eigenvalues are equal to the unity.
Equation (15) also provides the feature selection criterion
method used in this work. Considering that the aim of this
paper is to update the correction stage of the EKFSLAM
with the most significant features, this approach is based on
observing the sum of eigenvalues of Eq. (15). Thus, from
a set of Mt features with correct association, those features
with the lowest sum of eigenvalues will be chosen for the
correction stage.
zi Mt , zopt : argz min(|Pt |) argz min




Iv Kv,t Hv,t
Kv,t Hz,t

Eig
. (16)
Kz,t Hv,t
Iz Kz,t Hz,t
The main difference of taking the sum of eigenvalues in
(15) instead of its determinant is that, if an eigenvalue tends
to zero, also does the determinant and the correction is not
guaranteed to happen in all directions.
The implementation algorithm with the approach
presented in this work is summarized in Algorithm 2.
Let Nt be the set of observed features at time t
Let Mt Nt be the set of features with correct
matching
Let MAX be the maximum number of features to be
used
for i = 1 : min(#Mt , MAX)
opt
find
 z :
argz min(|Pt |) argz min 

Iv Kv,t Hv,t
Kv,t Hz,t
Eig
Kz,t Hv,t
Iz Kz,t Hz,t

1
Kt = Pt HtT Ht Pt HtT + Rt
t = t + Kt (zt h(t ))

IP address: 200.83.129.91

(i)
(ii)
(iii)
(iv)
(v)
(vi)
(vii)

276

Towards features updating selection based on the covariance matrix of the SLAM system state

Pt = (I Kt Ht )Pt
(viii)
Mt = Mt {zopt }
(ix)
(x)
Pt := Pt , t := t
end for
(xi)
Algorithm 2 Sequential EKFSLAM with Feature
Selection
Algorithm 2 shows the correction stage of the EKF
SLAM. Once extracted the features of the environment and
determined those features with appropriate association, the
for loop of the correction stage begins. The maximum number
of features to be updated (MAX) is stated in sentence (iii). In
this work, MAX = 2. In this way, the two most significant
features will be used for the correction stage. Thus, the
limit of the for loop is governed by MAX or the number of
observed features with correct association (for the case when
#Mt < MAX). Sentence (v) shows the feature selection
criterion. The most significant feature is updated first. In
order to do so, zopt must be found from the set Mt . The rest
of the algorithm performs identically than the Algorithm 1
shown before.
The fact of updating only the most significant features in
the eigenvalues sense ensures consistency of the SLAM
see Theorem 1 and saves processing time as will be shown
in the following sections.

3. Experimental Results
In this section is shown the consistency test of the algorithm
proposed in this work in addition with the real time
experimental results carried out at the facilities of the Instituto
de Automatica of the National University of San Juan.
3.1. Consistency analysis
The consistency test was carried out within a simulated
environment considering that the true position of the mobile
robot is required. The environment was simulated using
R
R


the MobileSim software provided by ActivMedia . The
consistency results are shown in Fig. 3.
Figure 3(a) shows the simulated environment which is
composed by corners and lines. The mobile robot starts in
a known initial position and orientation inside the map, that
is, the SLAM system state, t , is previously initialized. In
the simulation, 0T = [ x0 y0 0 ] = [ 30 30 0.1 ]. In dashed
grey is the desired path to be travelled by the robot through
the environment. Figure 3(b) shows the map reconstruction
based on the SLAM system state parameters and the
secondary map maintained during the process.
The navigation of the mobile robot is governed by the
determination of local frontier points as it is shown in
ref. [10]. The local frontier points strategy finds nonoccupied points of the navigable space and directs the
robots movements to that point. The last is accomplished
by generating a kinematically plausible path from the robots
position to the frontier point. This exploration strategy causes
the actual travelled path by the mobile robot differs from the
desired one. Figures 3(ce) show the error of the estimated
pose of the mobile robot with respect to the real pose
obtained by simulation. In the three cases, the error is
bounded by two times the corresponding standard deviation.

http://journals.cambridge.org

Downloaded: 24 Nov 2013

Thus, the sequential EKFSLAM with features updating

selection algorithm is consistent. This simulation also shows
how the map is built after closing the loop by the mobile
robot.

3.2. Real time experimental results

Several experiments were carried out at the facilities of the
Instituto de Automatica of the National University of San
Juan. In order to check the performance of the proposed
algorithm, both the classical sequential EKFSLAM and
the EKFSLAM with features updating selection criterion
were implemented in parallel in the mobile robot. Thus, both
algorithms worked at the same time in order to share the
same mobile robots path for both SLAM strategies. The
exploration strategy is made by the determination of local
frontier points as was stated in Section 3.1. In addition,
all experiments were real time realizations. The odometric
information of the mobile robot was used for the exploration
strategy, although it was not used in the estimation
process.
Figure 4 shows an example of the map reconstruction
when applying both EKFSLAM algorithms. The initial
SLAM system state was set to 0T = [ 11 8 /2 ]. Figure 4(a)
shows the map reconstruction based on the classic sequential
EKFSLAM system state whereas Fig. 4(b) shows the
map reconstruction based on the EKFSLAM with feature
updating selection.
Since the correction stage of the SLAM with feature
selection approach is bounded by MAX the maximum
number of features to be updated, see Algorithm 2
the robot performs a higher number of measurements of
the environment, increasing the number of features to be
acquired and the possibility of revisiting past features. As
it can be seen, both maps of Figs. 4(a) and 4(b) are
consistent. Figure 4(c) shows the processing time demanded
by both algorithms. The classical sequential EKFSLAM
shows a quadratic evolution of its accumulated processing
time constrained to the unbounded number of features
to be updated; the EKFSLAM with features updating
selection also shows a quadratic evolution of the accumulated
processing time but with a slower growing with respect to
the classical sequential EKFSLAM processing time. This
is so because, as was stated before, the number of features
to be updated remains unchanged. Although Fig. 4(c) is a
consequence of the correction restriction and could apply to
any restriction policy, the fact of choosing the most significant
features prevents the algorithm to become inconsistent. In
addition, Fig. 4(d) shows the accumulated time that the robot
remains in an open loop situation. The sampled time of the
mobile robot used Pioneer 3DX must be a multiple of
0.1 s. Thus, if all the process demands, e.g., 0.15 s, then
the sampled time of the robot will be of 0.2 s. These 0.2 s
will be the time that the robot remains in the open loop
(the robot cannot change their driving commands). Thus, in
Fig. 4(d), the classical sequential EKFSLAM has reached
a maximum sampled time of 0.7 s whereas the EKF
SLAM with the features selection procedure proposed in
this work has remained constant to 0.2 s. The open loop
represents a compromise in the control strategy adopted

IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state

277

Fig. 3. Consistency test of the sequential EKFSLAM with the feature selection approach proposed in this paper. (a) Simulated environment
(solid black lines) with the desired path (dashed line); (b) map reconstruction of the environment. The grey points correspond to raw laser
data; solid black segments represent walls of the environment; light grey circles correspond to corners; black squares are the beginning and
ending points associated with the segments and the solid grey line is the path travelled by the mobile robot; (c) The error in the x-coordinate
of the mobile robot is bounded by two times its standard deviation; (d) The error for the y-coordinate; (e) The error for the orientation of
the mobile robot.

for the mobile robot driving because collisions must be

avoided.
Considering that the classical sequential EKFSLAM
corrects the system state with all the features with correct
association see Algorithm 1, as the number of features
with correct association grows, also grows the processing
time of the SLAM algorithm; then the robot will have
a variable sampling time. On the other hand, with the
feature selection approach, the computational cost remains

http://journals.cambridge.org

Downloaded: 24 Nov 2013

bounded and thus, the SLAM is more suitable for control

implementations.

4. Maps Divergence
The maps shown in Figs. 4(ab) represent the environment
built by the two SLAM algorithms implemented in this
work. Although in both cases the mobile robot navigates
the same environment, the number of features acquired by

IP address: 200.83.129.91

278

Towards features updating selection based on the covariance matrix of the SLAM system state

Fig. 4. Performance of the SLAM with the feature selection approach with respect to the classical sequential EKFSLAM. (a) Map built
using the classical sequential EKFSLAM system state. The path travelled by the robot is represented by a solid line; grey circles are
corners of the environment; solid black segments represent walls and black squares are the beginning and ending points of the segments;
(b) map built using the EFKSLAM feature updating selection system state. The path travelled by the robot is the same than the one shown
in (a); (c) accumulated processing time. The processing time of the feature selection approach remains quadratic but growing slower than
the classical sequential EKFSLAM. The SLAM with the feature selection approach consumes lesser time the classical EKF-SLAM; (d)
Accumulated open loop time. The robot remains more time under an open loop situation with the classical EKF-SLAM than with the
feature selection approach proposed in this paper.

each algorithm varies. Then, also varies the precision of the

maps geometrical information, e.g. number of walls and their
lengths. In order to get a measure of the discrepancy between
the obtained maps, in this work we have implemented the
KullbackLeibler divergence criterion.
The KullbackLeibler divergence26 is a pseudo-metric
that gives the divergence between two probability density
functions and it is presented in Eq. (17) for the continuum
case and in (18) for the discrete case. The reference functions
in Eqs. (17) and (18) are g(.), density function, and g(.), mass
function, respectively:
0T = [x0
Dp||q

0 ] = [ 30 30 0.1 ],




p(x)
.
=
p(x) log
q(x)

y0

(17)
(18)

In order to apply the KullbackLeibler divergence to the

maps built by the SLAM algorithms, it is first necessary to

http://journals.cambridge.org

calculate the probability associated to every point within the

map. That is, every point in the space travelled and mapped
by the robot should have a probability value associated with
it. The most widely used solution in the literature is to
grid the map.6 Nevertheless, the gridding process demands
high computational cost, directly related to the size of the
grid and the map, making it unsuitable for possible real
time applications. In addition, a probability value must be
calculated for each cell in the gridded map.
In this work we propose a Monte Carlo experiment for
generating a set of significant points covering the map and
then, calculate the probability value associated with each
point using the concept of sum of Gaussians.21 The following
section will explain the last in detail.

Downloaded: 24 Nov 2013

4.1. Monte Carlo experiment

The Monte Carlo experiment consists of generating
Euniformly distributed points within the map built by the
SLAM. In order to do so, the limits of the map must be

IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state
previously defined. For comparison purposes, the same set
E should cover all the maps that are going to be used in the
KullbackLeibler divergence. Thus, the limits of the map
are calculated according to the maximum and minimum
value occupied by a feature in the Cartesian coordinate
system in both maps. For example, if we use Figs. 4(a)
and 4(b), the minimum common x-coordinate (Xmax ) limit
between both maps is near 9 m; the maximum common xcoordinate (Xmin ) limit is near 19 m; the minimum common
y-coordinate (Ymax ) limit is near 6 m and the maximum
common y-coordinate (Ymin ) limit is near 27 m. Thus, the set
of points E should be uniformly generated within those limits.
The Monte Carlo experiment is then executed as shown in
Eqs. (19)(22):
i U (0, 1), i = 1, . . . #E,

(19)

mi = Xmin + (Xmax Xmin ) i ,

(20)

i U (0, 1), i = 1, . . . #E,

(21)

ni = Ymin + (Ymax Ymin ) i .

(22)

Equations (19) and (21) represent the independent uniformly

generation of points, where # E is the number of points
that will be used; the pair ( mi ni )T E represents a point
of the map bounded by the limits Xmin , Xmax , Ymin and
Ymax . Figure 5 shows how a partiality of the map shown in
Fig. 4(a) is affected by the Monte Carlo experiment. For this
realization were used 200 points (#E = 200). In addition,
the map in Fig. 4(b) has the same Monte Carlo points
distribution.
4.2. Sum of Gaussians
To calculate the probability of a point inside the built map
it is necessary to determine how a feature influences the
probability of that point. By considering that all features

Fig. 5. Monte Carlo points generation. A partiality of the map

shown in Fig. 4(a) is covered by the Monte Carlo points (solid grey
circles); the solid black lines are the segments associated to walls
of the environment; light grey circles are corners and black squares
are the beginning and ending points of the segments.

http://journals.cambridge.org

Downloaded: 24 Nov 2013

279

included in the SLAM system state are Gaussian random

variables, the probability of a point given all these features
can be estimated by a sum of Gaussians.27 Thus, if L is the
number of features contained in the SLAM system state, the
probability of point the pi = [ mi ni ]T is calculated as it is
shown in (23).
Probability of pi
=

L


k 

k=1

1
(2)2 |Pk |

12

 m   T
 m   
i
i
kx
Pk1
kx
ni
ni
ky
ky

. (23)

In Eq. (23), Pk is the covariance matrix of the kth feature of

the SLAM system state, determined as shown in Eq. (24); kx
and ky are the estimated mean values of the feature; mi and
ni are the Cartesian coordinates of the point and k is the
weight factor associated with the Gaussian distributions to
accomplish the probability distribution fusion:

v
1

..

t =
.
k

..
.

Pvv,t
.
..
Pt =
Pkv,t

T
Pkv,t
..
..
.
.
Pkk,t

..
.
.

..
.

(24)

In Eq. (24), v is the estimate of the robots pose; i , i =

1 . . . n is the estimate of the features parameters; Pvv,t is the
robots covariance; Pkv,t is the cross-correlation between the
robots covariance and the kth features covariance and Pkk,t
is the covariance matrix of the kth feature.
In this work, the environment is represented by lines and
corners, where corners are defined in the Cartesian space and
lines in the polar space see Eqs. (7) and (8). In order to
use Eq. (23) as an estimate of the probability of a point, it is
necessary to refer all distribution functions to a same space.
The appropriate transformations carried out can being seen in
ref. [23]. Thus, once the probabilities of all points of E with
respect to all features of the map are obtained, they must be
collapsed into one representative value for each point. To do
so, a weighted sum of Gaussians is proposed as was stated
in Eq. (23). The k factors in (23) are obtained according to
ref. [27]. The final result is always less than 1 and the sum
of all probability values is also less than 1.
Using the sum of Gaussians concept we have a
probabilistic representation of all the points of Ecovering
the map raised by the SLAM.
4.3. KullbackLeibler divergence
Applying the Monte Carlos points generation and the
calculation of the probability value associated with each
of these points by means of the sum of Gaussians method,
we can convert the map obtained by the SLAM algorithm
in a probability mass function. This probability mass
function has the Monte Carlos set of points (E) as its
domain. Then, converting the maps obtained by the SLAM
algorithms (see Figs. 4(a) and 4(b)) it is possible to calculate

IP address: 200.83.129.91

280

Towards features updating selection based on the covariance matrix of the SLAM system state
By inspection in Fig. 6 is possible to see that, in both
cases, after the experiment of 2 104 points, the mean
and variance of the experiment remain approximately in
the same value. That means, a set of #E = 2 104 points
is needed to obtain a divergence value between the maps
shown in Fig. 4. Furthermore, Fig. 7 shows the divergence
of the maps shown in Fig. 4 with respect to a digitalized
version of the facilities of the Instituto de Automatica. In
order to compute this divergence value, the features of the
real map (the digitalized architectonic draw of the Instituto
de Automatica) were considered as degenerated Gaussians
features.
Figure 7 also shows that the divergence of the EKF
SLAM with feature updating selection map with respect
to the real map remains smaller than the divergence of the
classical sequential EKFSLAM map with respect to the
real map, for all the Monte Carlo realizations. In both cases
in Fig. 6, from #E = 1 104 we can have an idea of the
divergence. The fact that the SLAM with feature selection
approach shown in this work presents a smaller value of
divergence with respect to the real map than the classical
sequential EKFSLAM can be interpreted as the map
obtained by the proposal of this work, acquires more accurate
geometrical information of the environment than the classical
implementation.

Fig. 6. Mean and Variance of the divergence after several Monte

Carlo realizations with different number of points generation. (a)
Divergence between the map built by the classical sequential EKFSLAM (considered as p(.) in Eq. (18)) and the map built by the
EKF-SLAM with feature updating selection approach (considered
as q(.) in Eq. (18)); (b) In this case, the map built by the EKF
SLAM with feature updating selection is considered as p(.) in Eq.
(18) whereas q(.) is the map built by the classical sequential EKF
SLAM.

the divergence between their associated probability mass

functions.
Figure 6 shows the divergence between the two maps
shown in Fig. 4. For this result, several Monte Carlo
realizations were carried out. For each number of points,
a hundred experiments were executed, obtaining in that way,
the mean and variance associated with the Monte Carlo
realization. Figure 6(a) shows the divergence of the map built
by the classical sequential EKFSLAM with respect to the
map built by the EKFSLAM with feature updating selection,
and Fig. 6(b) shows the divergence between the map built by
the feature selection criterion and the classical EKFSLAM
one (remember that we are using a pseudo-metric measure
for the divergence of maps thus being sensitive to the order
the comparison is performed).

http://journals.cambridge.org

Downloaded: 24 Nov 2013

5. Conclusions
In this paper, an EKFSLAM algorithm based on the
restriction in the number of features to be corrected was
presented. The correction restriction was based on selecting
the most significant features in the sense of the covariance sensibility ratio of the SLAM system state. Thus,
those features with the smallest sum of eigenvalues in the
sensibility expression of the SLAM covariance matrix (see
Eq. (9)) were chosen for the update stage of the EKF. The
last ensured that the estimation process will not turn into
inconsistency.
The restriction also implied that the computational cost of
SLAM process was bounded (when bounded the number
of features to be corrected, as was implemented in this
work) which reduced the open loop time of the mobile
robot. Thus, the smaller the open loop time the higher the
number of control actions to drive the robot and to map the
environment. The fact of implementing a SLAM algorithm
with bounded correction stage makes the algorithm more
suitable for control strategies implementations.
In order to define the precision of the map, the Kullback
Leibler divergence was carried out between the maps
obtained by both SLAM algorithms implemented in this
work and a digitalized architectonic draw of the environment.
For several Monte Carlo experiments, the map built by
the EKFSLAM with feature updating selection turned
to be less divergent than the map built by classical
sequential EKFSLAM when compared to the real map
of the environment. Thus, the map built by the SLAM
with updating restrictions presented in this paper is more
geometrically similar to the real map than the one built by
the classical sequential EKFSLAM. The last result ensures

IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state

281

Fig. 7. Divergence between the maps obtained by the SLAM algorithms and a digitalized architectonic draw of the Instituto of Automatica
(real map of the environment).

more reliable information for mobile robot maps planning

purposes.

Acknowledgments
The authors gratefully acknowledge Universidad Nacional de
San Juan of Argentina, CONICET (Argentina) and ANPCyT
for funding this research.

References
1. R. C. Arkin, Behavior-Based Robotics (MIT Press, Cambridge,
1998).
2. R. Siegwart and I. R. Nourbakhsh, Introduction to Autonomous
Mobile Robots (MIT Press, Cambridge, 2004).
3. J. Andrade-Cetto and A. Sanfeliu, Environment Learning for
Indoor Mobile Robots (Spinger Tracks in Advanced Robotics,
Springer, 2006).
4. H. Durrant-Whyte and T. Bailey, Simultaneous localization
and mapping (SLAM): Part I essential algorithms, IEEE
Robot. Autom. Mag. 13, 99108 (2006).
5. H. Durrant-Whyte and T. Bailey, Simultaneous localization
and mapping (SLAM): Part II state of the art, IEEE Robot.
Autom. Mag. 13, 108117 (2006).
6. S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics (MIT
Press, Cambridge, 2005).
7. J. E. Guivant and E. M. Nebot, Optimization of the
simultaneous localization and map-building algorithm for realtime implementation, IEEE Trans. Robot. Autom. 17, 242257
(2001).
8. A. Garulli, A. Giannitrapani, A. Rosi and A. Vicino, Mobile
robot SLAM for Line-Based Environment Representation,
Proceedings of the 44th IEEE Conference on Decision and
Control, pp. 20412046, Seville, Spain (2005).
9. J. Castellanos, R. Martinez-Cantin, J. Tardos and J. Neira,
Robocentric map joining: Improving the consistency of EKFSLAM, Robot. Auton. Sys. 55. 2129 (2007).

http://journals.cambridge.org

Downloaded: 24 Nov 2013

10. T. Tao, Y. Huang, F. Sun and T. Wang, Motion Planning for

SLAM Based on Frontier Exploration, Proceedings of the
IEEE International Conference on Mechatronics and Automation, pp. 21202125, Takamatsu, Kagawa, Japan (2007).
11. B. Xi, R. Guo, F. Sun and Y. Huang, Simulation Research
for Active Simultaneous Localization and Mapping Based
on Extended Kalman Filter, Proceedings of the IEEE
International Conference on Automation and Logistics,
pp. 24432448, Qingdao, China (2008).
12. J. Knight, A. Davison and I. Reid, Towards Constant Time
Slam Using Postponement, Proceedings IEEE/RSJ Int. Conf.
on Intelligent Robots and Systems, pp. 405413, Maui, Hawaii,
USA (2001).
13. N. Vlassis, R. Bunschoten and B. Krose, Learning TaskRelevant Features From Robot Data, Proceedings of the
IEEE International Conference on Robotics and Automation,
pp. 499504, Seoul, Korea (2001).
14. E. Brunskill and N. Roy, SLAM using Incremental Probabilistic PCA and Dimensionality Reduction, Proceedings of the
IEEE International Conference on Robotics and Automation,
pp. 342347, Barcelona, Spain (2005).
15. W. Z. Yu, H. X. Han, L. Xin, W. Min and Y. H. Cheng, A
Simultaneous Localization and Mapping Method Based on
Fast-Hough Transform, Inf. Technol. J. 7, 190194 (2008).
16. G. A. Borges, M. J. Aldon, V. H. Alcalde and L. R. Suarez,
Local Map Building for Mobile Robots by Fusing Laser
Range Finder and Monocular Video Images, IEEE Latin
American Robotic Symposium, pp. 16, Sao Luis-MA, Brazil
(2005).
17. S. Fu, H. Liu, L. Gao and Y. Gai, SLAM for Mobile
Robots using Laser Range Finder and Monocular Vision,
International Conference on Mechatronics and Machine Vision
in Practice, Xiamen, China (2008) pp. 9196.
18. S. Theodoridis and K. Koutroumbas, Pattern Recognition,
(Elsevier, San Diego, USA, 2006).
19. E. Eade and T. W. Drummond, Edge Landmarks in Monocular
SLAM, The 17th British Machine Vision Conference
(BMVC), pp. 588596, Edinburgh, UK (2006).
20. Y. J. Lee and J. B. Song, Autonomous Selection, Registration
and Recognition of Objects for Visual SLAM in Indoor

IP address: 200.83.129.91

282

Towards features updating selection based on the covariance matrix of the SLAM system state

Environments, The International Conference on Control,

Automation and Systems, pp. 668673, Seul, Korea (2007).
21. D. Zhang, L. Xie and M. D. Adams, Entropy based Feature
Selection Scheme for Real Time Simultaneous Localization
and Map Building IEEE Conference on Intelligent Robots
and Systems, pp. 649654, Edmonton, Canada (2005).
22. S. Fintrop, P. Jensfelt and H. I. Christensen, Attentional
Landmark Selection for Visual SLAM, Proceedings of the
International Conference on Intelligent Robots and Systems,
pp. 25822587, Biejing, China (2006).
23. F. A. A. Cheein, Simultaneous Localization and Mapping of a
Mobile Robot based on Uncertainty Zones Navigation Ph.D.
Thesis (San Juan, Argentina: National University of San Juan,
2009).

http://journals.cambridge.org

Downloaded: 24 Nov 2013

24. M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte and

M. Csorba, A solution to the simultaneous localization and
map building (SLAM) problem, IEEE trans. Robot. Autom.
17, 229241 (2001).
25. S. J. Julier and J. K. Uhlmann, A counter Example to the
Theory of Simultaneous Localization and Map Building,
IEEE International Conference on Robotics and Automation,
pp. 42384243, Seul, Korea (2001).
26. T. M. Cover and J. A. Thomas, Elements of Information
Theory, 2nd ed. (Wiley-Interscience, New York, USA,
2006).
27. A. S. Miralles and M. A. S. Bobi, Global Path Planning in
Gaussian Probabilistic Maps, J. Intell. Robot. Syst. 40, 89102
(2004).

IP address: 200.83.129.91