Roboticahttp://journals.cambridge.org/ROB

Additional services for Robotica:

Email alerts: Click hereSubscriptions: Click hereCommercial reprints: Click hereTerms of use : Click here

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

Fernando A. Auat Cheein, Fernando di Sciascio, Gustavo Scaglia and Ricardo Carelli

Robotica / Volume 29 / Issue 02 / March 2011, pp 271 - 282DOI: 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 updatingselection 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

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

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

Towards features updating selection based on the covariancematrix of the SLAM system stateFernando A. Auat Cheein, Fernando di Sciascio, Gustavo Scagliaand Ricardo CarelliInstituto de Automatica, National University of San Juan, San Juan, Argentina(Received in Final Form: March 4, 2010. First published online: March 31, 2010)

SUMMARYThis paper addresses the problem of a features selectioncriterion for a simultaneous localization and mapping(SLAM) algorithm implemented on a mobile robot. ThisSLAM algorithm is a sequential extended Kalman filter(EKF) implementation that extracts corners and lines fromthe environment. The selection procedure is made accordingto the convergence theorem of the EKF-based SLAM.Thus, only those features that contribute the most to thedecreasing of the uncertainty ellipsoid volume of the SLAMsystem state will be chosen for the correction stage ofthe algorithm. The proposed features selection procedurerestricts the number of features to be updated during theSLAM process, thus allowing real time implementationswith non-reactive mobile robot navigation controllers. Inaddition, a Monte Carlo experiment is carried out in orderto show the map reconstruction precision according to theKullbackLeibler divergence curves. Consistency analysisof the proposed SLAM algorithm and experimental resultsin real environments are also shown in this work.

KEYWORDS: SLAM; Feature selection; Mobile robot.

1. IntroductionThis article concerns the problem of the feature-basedsimultaneous localization and mapping (SLAM) algorithmapplied to a mobile robot. The correction stage of the SLAMis restricted to the features of the environment that aremost significant for the consistency and convergence of thealgorithm. This restriction decreases the computational costof the algorithm, making it more suitable for non-reactivecontrol applications. It is also presented a map divergenceanalysis when compared the obtained SLAMs map withrespect to other maps of the same environment.

The mobile robotic field is composed by three main areas:navigation, localization and mapping. The knowledge of therobot about its position inside an environment and the way thevehicle interacts with it defines the autonomy of the robot.A mobile robot is said autonomous if there is no externalsupervision in order to accomplish, efficiently, the robotictask. To reach full autonomy, the robot has to know itsposition within the environment (localization), to model the

* Corresponding author. E-mail: fauat@inaut.unsj.edu.ar

environment (mapping) and to navigate through it.1,2 TheSLAM algorithm comes up as a solution to the problem oflocalization and mapping within different environments.3

The SLAM algorithm recursively corrects the pose position and orientation of the vehicle using environmentalinformation while the environment is modeled using theestimated pose information4,5 at the same time. The SLAMconsiders the pose of the robot and the parameters that modelthe environment as part of the same system state, therefore,increasing its size as the knowledge about the environmentgrows. Several solutions to the SLAM problem have beenproposed in the literature over the last 20 years, mainlydepending on the robotic task. The extended Kalman filter(EKF) is one of the most used filters as a SLAM solution.6 Itmodels the environment and the vehicle as Gaussian randomvariables. Due to the fact that the EKF works with linearizedmodels of the environment and the mobile robot motion,the errors of the models are not strictly minimized.3 TheEKFSLAM processing time is bigger than other approachessuch as the information filter (IF) or the unscented Kalmanfilter6 (UFK). In order to deal with non-Gaussian models,the particle filter (PF) has shown a better performancewhen dealing with bigger amount of data and with non-Gaussian environments. Despite of this, the EKFSLAM iswidely used in robotics because of its simplicity and its easyprogramming for practical robotics issues.79

The integration of the SLAM with control strategiespresents an incipient field of research.3,10,11 Althoughreactive-based control is implemented in most of the SLAM-control applications, plan-based control integrating the mapderived by the SLAM remains under study. One of the keyproblems in the fusion of SLAM with plan-based controlstrategies in real time implementations is that the SLAM isnot a constant time algorithm. If the environment is modeledby features, e.g. lines, corners, corridors, trees, furrows, theSLAM system state increases its size as the robot acquiresmore features from the environment.

The increment of the size of the SLAM system statesrepresents an increment of the processing time needed toexecute the SLAM algorithm. Furthermore, the processingtime of the EKFSLAM increases with the number offeatures with appropriate association that will be usedduring the correction stage of the algorithm. Several attemptsto restrict the processing time of the correction stage ofthe SLAM algorithm can be found in the literature. For

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

272 Towards features updating selection based on the covariance matrix of the SLAM system stateexample, in ref. [12], a modification of the SLAM correctionstage is proposed, decreasing the computational time of thealgorithm.

The feature extraction procedure must be accurate, robustand reliable. Many works found in the literature use thesecharacteristics of the feature extraction procedure to selectthe features to be used by the SLAM algorithm. Thus, thefeature selection criterion depends on the feature extractionprocedure which in turn depends on the type of sensorused by the robot. In refs. [4, 5] the authors present afeature-based SLAM that extracts trees from the environmentusing a Laser range sensor and a clustering algorithm. Theydiscard the elements of the environment that the cluster doesnot give a reliable detection. In refs. [13, 14] the authorspresent a visual-based SLAM that uses principal componentanalysis (PCA) to extract features from the environment andto discard redundant information. For indoor applications,the most widely used features are geometric primitives thatcan be associated with elements of the environments suchas walls associated with lines and corners. In ref. [8]the authors present a clustering algorithm to extract linesfrom range measurements. Thus, if the quality of a line, e.g.the length of the associated wall, or the number of pointsused by the cluster does not exceed a certain threshold,that line is discarded and it will not be used during theSLAM algorithm. Other algorithms have been proposed inthe scientific literature to extract lines. The work of15 uses theHough transformation to obtain lines from the environmentwith the corresponding computational cost. The fusion of theinformation provided by two or more exteroceptive sensorsin order to obtain a robust feature extraction procedure wasalso implemented. In refs. [16, 17] the authors present thefusion of a monocular video with a range laser in order toextract lines, corners and planes from the environment. Theyuse fuzzy algorithms to determine the features and thosefeatures with the lowest covariance are then selected.

In the works presented before, the features selectingprocedure is performed during the feature extraction ordetection process.18 Once the features are selected, they areused by the SLAM algorithm. On the other hand, other worksattempted to restrict the number of features to be used duringthe correction stage of the SLAM19,20 independently of thefeature extraction procedure. In ref. [21], a threshold of theentropy of the feature calculated from the covariance matrixof the SLAM system state is taken into account for choosingthe features to be used in the SLAM; in ref. [9], the SLAMuses only the two last features for the correction. All theseworks do not evaluate before the updating stage how thefeature will affect a priori the SLAM process.

In this work we propose an EKFSLAM with featureupdating selection criterion for choosing whether a featureshould be used in the correction stage of the SLAM.The selection criterion is based on the evaluation of theeigenvalues of the covariance sensibility ratio of the SLAMsystem state. Thus, we restrict the correction stage to thosefeatures with correct association that presents the smallestsum of eigenvalues of the covariance ratio affected by theobserved feature. In that way, the correction is made inall directions of the estimation process. Furthermore, theEKFSLAM configuration used in this paper corresponds

to a sequential algorithm.6 The Jacobian matrices of theobservation models in the sequential EKFSLAM, are sparsematrices which reduces the computational cost of the SLAM.Real time experimental results along with consistency testsand computational costs analysis are shown in this work.The experimental results shown were carried out for boththe classical sequential EKFSLAM and the EKFSLAMwith feature updating selection proposed in this work. Inaddition, a KullbackLeibler divergence test is carried outin order to show that the map built by the EKFSLAMwith feature updating selection presents a smaller divergencevalue with respect to the classical sequential EKFSLAMwhen compared with a digitalized architectonic draw of thereal environment used for the experiments.

This paper is organized as follows: Section 2 presentsboth the classical sequential EKFSLAM and the EKFSLAM with features updating selection proposed in thiswork. Section 3 shows the consistency tests of the SLAMalgorithm and the experimental results carried out at thefacilities of the Instituto de Automatica. Section 4 shows theKullbackLeibler divergence curves of the different mapsbuilt by the SLAM algorithms and Section 5 shows theconclusions of this work.

2. MethodsIn this section, the classic sequential EKFSLAM ispresented along with the EKFSLAM algorithm withrestriction over the number of features to be updated proposedin this paper.

2.1. EKFSLAM algorithmThe EKF equations for the SLAM problem solution arestated in Eqs. (1)(5). By hypothesis, all variables involvedin the estimation process are considered as Gaussian randomvariables. Thus, the SLAM system state and the featuresmodel have Gaussian distributions.

Prediction stage

t = f ( t1, ut ), (1)Pt = AtPt1ATt + WtQt1WTt . (2)

Correction stage

Kt = Pt H Tt(HtP

t H

Tt + Rt

)1, (3)

t = t + Kt (zt h( t )), (4)Pt = (I KtHt )Pt . (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 inputcontrol commands and t is the corrected system state at timet; f describes the evolution of the elements of . Pt and Pt arethe predicted and corrected covariance matrices respectively,at time t; At is the Jacobbian matrix of f with respect to theSLAM system state ( ) and Qt is the covariance matrix of thenoise associated with the process, whereasWt is its Jacobbianmatrix. Equations (3)(5) represent the correction stage ofthe EKFSLAM. Kt is the Kalman gain of the algorithm

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state 273at time t; Ht is the Jacobian matrix of the measurementmodel (h in Eq. (4)) and Rt is the covariance matrix of theactual measurement zt . In Eq. (4), (zt h( t )) is called theinnovation process6 and takes place when the data associationprocedure has reached an appropriate matching between theobserved feature and the predicted one (h( t )). Both, theprocess model (f in Eq. (1)) and the observation model arenon-linear expressions. In this work, the features extractedfrom the environment correspond to lines and corners, eitherconcave or convex.

The SLAM system state is composed by both the vehiclesdegrees of freedom and the map, represented by theparameters that define the features. The map is consideredas stationary. Thus, the evolution computed in Eq. (1)corresponds to the robots movements. More informationabout this topic can be found in ref. [4, 5]. The mobile robotused in this work is a non-holonomic unicycle type Pioneer3DX, built by ActivMedia R . Figure 1 shows a scheme ofthe vehicle and Eq. (6) represents their kinematic equations.The robot has a range sensor laser -built by SICK R whichacquire 181 measurements, from 0 to 180, within a 30m range. The models of the features are shown in Eqs. (7)and (8), and Fig. 2 shows the geometrical representation ofeach variable used in (7) and (8). More information aboutfeatures extraction and the SLAM algorithm implemented inthis work can be found in refs. [3, 8]:

xtyt

t

=

xt1yt1

t1

+ t

cos(t1) a sin(t1)sin(t1) a cos(t1)

0 1

(

utt

)+ t, (6)

zcorner,t = hi[Poserobot,t , pi,t , wt ] =[zRz

]

=

(xt xe)2 + (yt ye)2

arctan(yt yext xe

) t

+

[wRw

], (7)

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

]

Fig. 1. Schematic of the mobile robot used.

Fig. 2. Feature extraction.

=[r xt cos() yt sin()

t]

+[ww

]. (8)

In Eq. (6), xt , yt and t are the coordinates of h the pointof control in Fig. 1; of t is the discrete time Gaussianprocess noise associated with the robots model, ut is thelinear velocity command applied to the vehicle and t is theangular velocity command. In Eqs. (7) and (8), w signals wR,w,w,w represent the Gaussian measurementnoises associated with the observation models (zline,t andzcorner,t ).

In this work, a sequential EKF algorithm was implementedas in ref. [6]. In this algorithm, the correction iterationis executed with one innovation at a time. This algorithmimplies that the Jacobian matrix of the observation model,H, be a sparse matrix as shown in ref. [6]. Along with themap generated by the SLAM process, a secondary mapwas also implemented. This secondary map, storages thebeginning and ending points of the segments associated withlines in the environment. It is also maintained and updatedaccording to the evolution of the SLAM system state. Thesecondary map allows the environment reconstruction forfeasible navigation purposes. More details about this mapcan be found in ref. [23].

2.2. Classical sequential EKFSLAMThe classical sequential EKFSLAM was previouslypresented in ref. [6]. It has the property of sequentiallyupdating the SLAM system state one feature at a time. Thus,the Jacobbian matrices of the observation model are sparsewhich reduce the processing time of the algorithm. This isdue to the fact that the entries of the Jacobbian matrix of theobservation model that do not correspond to the observedfeature will be considered as zero. Algorithm 1 summarizesthe classical sequential EKFSLAM.Let Nt be the set of observed features at time t (i)Let Mt Nt be the set of features with correct

matching (ii)for i = 1 : #Mt (iii)

Kt = Pt H Tt(HtP

t H

Tt + Rt

)1 (iv)t = t + Kt (zt h( t )) (v)

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

274 Towards features updating selection based on the covariance matrix of the SLAM system statePt = (I KtHt )Pt (vi)Mt = Mt {zi} (vii)Pt := Pt, t := t (viii)

end for (ix)Algorithm 1 Classical sequential EKFSLAM

Algorithm 1 represents the correction stage of the classicalsequential EKF-SLAM. Thus, the prediction stage of theEKF along with the computing of the robots movements isnot shown. After observing the environment, those featuresthat present a correct data association with respect to thepredicted ones, will be used for the EKF correction sentence(i) and (ii). In this paper, the data association method usedis based on the Mahalanobis distance.2,24 The for loop insentences (iii)(ix) shows the sequentially part of the EKF-SLAM. For a given feature zt , the Kalman gain, the SLAMsystem state updating and the posterior covariance of theSLAM system state are carried out in sentences (iv)(vi).Sentences (vii) implies that the zt feature is extracted fromthe set of features previously found in (ii) and will no longerbe used during the correction stage. The SLAM systemstate updated in (v) and the posterior covariance in (vi) willbecome, respectively, the predicted SLAM system state andthe prior covariance matrix of the SLAM in the next iterationof the algorithm sentence (ix). The for loop repeats untilall features with correct association were used.

2.3. Sequential EKFSLAM with featuresupdating selectionThe features updating selection procedure proposed in thispaper does not alter the sequential execution of the EKFSLAM presented in Section 2.2. On the contrary, the featuresselection procedure bounds the correction stage to thosefeatures with correct data association that will causeonly a meaningful correction of the SLAM system state.

In order to determine which features will be more relevantfor the SLAM system state, let us considerer the twoconvergence theorems presented in ref. [25].

Theorem 1. Let Pt and Pt be the prior and posteriorSLAM system state covariance respectively. Then,

|Pt | |Pt |.

Theorem 2. Let Pt,M be the covariance matrix of the featuresof the SLAM system state. Then,

limt |Pt,M | = 0.

Taking into account Theorem 1 above and Eq. (5), thefollowing holds,{|Pt | = |I KtHt ||Pt ||Pt | |Pt |

|Pt ||Pt |= |I KtHt | 1.

(9)Equation (9) represents the covariance sensibility ratio of

the SLAM system state. Thus, according to Eq. (9), thesmaller |I KtHt |, the higher the correction. This is so,because the determinant of the covariance matrix of theSLAM system state (|Pt |) can be seen as a measure of the

volume of the covariance ellipse associated to the systemstate. Considering also that |Pt | does not depend on theobserved feature, according to Eq. (9) can be stated that

zi Mt, zopt : argz min(|Pt |) argz min(|I KtHt |).(10)

In Eq. (10), Mt is the set of features with appropriateassociation 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, thosefeatures with the smallest values of |I KtHt | will beused in the correction stage. Instead of processing theentire determinant of (I KtHt ), we will calculate theireigenvalues. Due to the fact that (I KtHt ) is a matrixwhich dimensions vary according to the number of features ofthe map, the calculation of their eigenvalues could demandhigher processing time than the classical sequential EKFSLAM. To avoid this situation, lets considerer that Ht is asparse 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 theobservation model (Ht ) is composed by the Jacobbian of theobservation with respect to the vehicles degrees of freedom(Hv,t ) and the Jacobbian of the observation with respect tothe 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 formof Eq. (11). Thus,

Kt =[KTv,t K

T

1 K

Tz,t K

T

2]. (12)

Using Eqs. (11) and (12), the expression (I KtHt ) can becalculated as

(I KtHt ) =

Iv

I1Iz

I2

Kv,t

K1Kz,t

K2

[Hv,t 1 Hz,t 2

],

=

Iv Kv,tHv,t Kv,tHz,t

K1Hv,t I1 K1Hz,t

Kz,tHv,t Iz Hz,t

K2Hv,t K2Hz,t I2

.

(13)

In Eq. (13), I is the identity matrix; Iv , I1, Iz, I2 are identityblock matrices with the dimensions of Kv,tHv,t , K11,Kz,tHz,t and K22 respectively, and is a general nullblock matrix.

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state 275Up to this point, it can be seen that the determinant

|I KtHt | can be calculated directly from Eq. (13). Thus,

|I KtHt | =

Iv Kv,tHv,t Kv,tHz,t

K1Hv,t I1 K1Hz,t

Kz,tHv,t Iz Hz,t

K2Hv,t K2Hz,t I2

,

=Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t

. (14)Eq. (14) shows the reduced form of |I KtHt |. If one of theeigenvalues of Eq. (14), tends to zero faster than the others,then the entire determinant will tend to zero. This could causethat the posterior covariance matrix of the SLAM systemstate, Pt , be corrected faster in the direction associated withthat eigenvalue than in the rest of the directions. Insteadof using Eq. (14) for choosing the significant features asstated previously in this paper we propose to observe theeigenvalues evolution of (I KtHt ). Nevertheless, the lastdoes not contradict Eq. (14), instead, the SLAM algorithmwill be able to choose the direction of the correction basedon the eigenvalues information. Thus, the determinant of Eq.(14) provides lesser information than the observation of theeigenvalues.

In order to calculate the eigenvalues of Eq. (13), thefollowing theorem is necessary.

Theorem 3. Let S nn be a non-singular matrix of theform

S = A L1 B1 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 statementholds,

Eig([

A B

C D

]) Eig(S).

Thus, the set of eigenvalues of the sub-matrix [AB

C

D] is

included in the set of eigenvalues of S.Proof. Applying the definition of eigenvalue, we have that

S I =A IA L1 B1 Ir Ir 2

C L2 D ID

with IA and ID in the dimensions of A and D respectively.Then,

|S I | = (S I )rA IA BC D ID

= (S I )r

[A B

C D

]

[IA

ID

] ,

Eig

([A B

C D

]) Eig(S).

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

B

C

D]) Ones, where Ones is a set of r eigenvalues

equal to the unity.

Corollary. Identical demonstration is performed if S wereof the form

S =A 1 BL1 Ir L2

C 2 D

.

Thus, according to Eq. (13), let us suppose that S = I KtHt, then because of Theorem 3 we have that

Eig([

Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t

]) Eig(I KtHt ).

(15)

Considering that Iv is a 3 3 matrix (the robot has threedegrees of freedom) and Iz is a 2 2 matrix (every featureis defined by two parameters, see Eqs. (7) and (8)), the finalcalculation in (15) are the eigenvalues of a 5 5 matrix.Furthermore, by inspection of the proof of Theorem 3, it canbe concluded that the eigenvalues of [ IvKv,tHv,tKv,tHz,t

Kz,tHv,tIzKz,tHz,t ]

are the only eigenvalues of (I KtHt ) affected by theobservation zi in Algorithm 1, because the rest of theeigenvalues are equal to the unity.

Equation (15) also provides the feature selection criterionmethod used in this work. Considering that the aim of thispaper is to update the correction stage of the EKFSLAMwith the most significant features, this approach is based onobserving the sum of eigenvalues of Eq. (15). Thus, froma set of Mt features with correct association, those featureswith the lowest sum of eigenvalues will be chosen for thecorrection stage.

zi Mt, zopt : argz min(|Pt |) argz min

(

Eig

([Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t

])). (16)

The main difference of taking the sum of eigenvalues in(15) instead of its determinant is that, if an eigenvalue tendsto zero, also does the determinant and the correction is notguaranteed to happen in all directions.

The implementation algorithm with the approachpresented in this work is summarized in Algorithm 2.

Let Nt be the set of observed features at time t (i)Let Mt Nt be the set of features with correct

matching (ii)Let MAX be the maximum number of features to be

used (iii)for i = 1 : min(#Mt,MAX) (iv)

find zopt : argz min(|Pt |) argz min(Eig

([Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t

]))(v)

Kt = Pt H Tt(HtP

t H

Tt + Rt

)1 (vi)t = t + Kt (zt h( t )) (vii)

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

276 Towards features updating selection based on the covariance matrix of the SLAM system statePt = (I KtHt )Pt (viii)Mt = Mt {zopt} (ix)Pt := Pt, t := t (x)

end for (xi)Algorithm 2 Sequential EKFSLAM with FeatureSelection

Algorithm 2 shows the correction stage of the EKFSLAM. Once extracted the features of the environment anddetermined those features with appropriate association, thefor loop of the correction stage begins. The maximum numberof features to be updated (MAX) is stated in sentence (iii). Inthis work, MAX = 2. In this way, the two most significantfeatures will be used for the correction stage. Thus, thelimit of the for loop is governed by MAX or the number ofobserved features with correct association (for the case when#Mt < MAX). Sentence (v) shows the feature selectioncriterion. The most significant feature is updated first. Inorder to do so, zopt must be found from the set Mt . The restof the algorithm performs identically than the Algorithm 1shown before.

The fact of updating only the most significant features inthe eigenvalues sense ensures consistency of the SLAM see Theorem 1 and saves processing time as will be shownin the following sections.

3. Experimental ResultsIn this section is shown the consistency test of the algorithmproposed in this work in addition with the real timeexperimental results carried out at the facilities of the Institutode Automatica of the National University of San Juan.

3.1. Consistency analysisThe consistency test was carried out within a simulatedenvironment considering that the true position of the mobilerobot is required. The environment was simulated usingthe MobileSim R software provided by ActivMedia R . Theconsistency results are shown in Fig. 3.

Figure 3(a) shows the simulated environment which iscomposed by corners and lines. The mobile robot starts ina known initial position and orientation inside the map, thatis, the SLAM system state, t , is previously initialized. Inthe simulation, T0 = [ x0 y0 0 ] = [ 30 30 0.1 ]. In dashedgrey is the desired path to be travelled by the robot throughthe environment. Figure 3(b) shows the map reconstructionbased on the SLAM system state parameters and thesecondary map maintained during the process.

The navigation of the mobile robot is governed by thedetermination of local frontier points as it is shown inref. [10]. The local frontier points strategy finds non-occupied points of the navigable space and directs therobots movements to that point. The last is accomplishedby generating a kinematically plausible path from the robotsposition to the frontier point. This exploration strategy causesthe actual travelled path by the mobile robot differs from thedesired one. Figures 3(ce) show the error of the estimatedpose of the mobile robot with respect to the real pose obtained by simulation. In the three cases, the error isbounded by two times the corresponding standard deviation.

Thus, the sequential EKFSLAM with features updatingselection algorithm is consistent. This simulation also showshow the map is built after closing the loop by the mobilerobot.

3.2. Real time experimental resultsSeveral experiments were carried out at the facilities of theInstituto de Automatica of the National University of SanJuan. In order to check the performance of the proposedalgorithm, both the classical sequential EKFSLAM andthe EKFSLAM with features updating selection criterionwere implemented in parallel in the mobile robot. Thus, bothalgorithms worked at the same time in order to share thesame mobile robots path for both SLAM strategies. Theexploration strategy is made by the determination of localfrontier points as was stated in Section 3.1. In addition,all experiments were real time realizations. The odometricinformation of the mobile robot was used for the explorationstrategy, although it was not used in the estimationprocess.

Figure 4 shows an example of the map reconstructionwhen applying both EKFSLAM algorithms. The initialSLAM system state was set to T0 = [ 11 8 /2 ]. Figure 4(a)shows the map reconstruction based on the classic sequentialEKFSLAM system state whereas Fig. 4(b) shows themap reconstruction based on the EKFSLAM with featureupdating selection.

Since the correction stage of the SLAM with featureselection approach is bounded by MAX the maximumnumber of features to be updated, see Algorithm 2 the robot performs a higher number of measurements ofthe environment, increasing the number of features to beacquired and the possibility of revisiting past features. Asit can be seen, both maps of Figs. 4(a) and 4(b) areconsistent. Figure 4(c) shows the processing time demandedby both algorithms. The classical sequential EKFSLAMshows a quadratic evolution of its accumulated processingtime constrained to the unbounded number of featuresto be updated; the EKFSLAM with features updatingselection also shows a quadratic evolution of the accumulatedprocessing time but with a slower growing with respect tothe classical sequential EKFSLAM processing time. Thisis so because, as was stated before, the number of featuresto be updated remains unchanged. Although Fig. 4(c) is aconsequence of the correction restriction and could apply toany restriction policy, the fact of choosing the most significantfeatures prevents the algorithm to become inconsistent. Inaddition, Fig. 4(d) shows the accumulated time that the robotremains in an open loop situation. The sampled time of themobile robot used Pioneer 3DX must be a multiple of0.1 s. Thus, if all the process demands, e.g., 0.15 s, thenthe sampled time of the robot will be of 0.2 s. These 0.2 swill be the time that the robot remains in the open loop(the robot cannot change their driving commands). Thus, inFig. 4(d), the classical sequential EKFSLAM has reacheda maximum sampled time of 0.7 s whereas the EKFSLAM with the features selection procedure proposed inthis work has remained constant to 0.2 s. The open looprepresents a compromise in the control strategy adopted

http://journals.cambridge.org Downloaded: 24 Nov 2013 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 laserdata; solid black segments represent walls of the environment; light grey circles correspond to corners; black squares are the beginning andending points associated with the segments and the solid grey line is the path travelled by the mobile robot; (c) The error in the x-coordinateof the mobile robot is bounded by two times its standard deviation; (d) The error for the y-coordinate; (e) The error for the orientation ofthe mobile robot.

for the mobile robot driving because collisions must beavoided.

Considering that the classical sequential EKFSLAMcorrects the system state with all the features with correctassociation see Algorithm 1, as the number of featureswith correct association grows, also grows the processingtime of the SLAM algorithm; then the robot will havea variable sampling time. On the other hand, with thefeature selection approach, the computational cost remains

bounded and thus, the SLAM is more suitable for controlimplementations.

4. Maps DivergenceThe maps shown in Figs. 4(ab) represent the environmentbuilt by the two SLAM algorithms implemented in thiswork. Although in both cases the mobile robot navigatesthe same environment, the number of features acquired by

http://journals.cambridge.org Downloaded: 24 Nov 2013 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 builtusing the classical sequential EKFSLAM system state. The path travelled by the robot is represented by a solid line; grey circles arecorners 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 shownin (a); (c) accumulated processing time. The processing time of the feature selection approach remains quadratic but growing slower thanthe 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 thefeature selection approach proposed in this paper.

each algorithm varies. Then, also varies the precision of themaps geometrical information, e.g. number of walls and theirlengths. In order to get a measure of the discrepancy betweenthe obtained maps, in this work we have implemented theKullbackLeibler divergence criterion.

The KullbackLeibler divergence26 is a pseudo-metricthat gives the divergence between two probability densityfunctions and it is presented in Eq. (17) for the continuumcase and in (18) for the discrete case. The reference functionsin Eqs. (17) and (18) are g(.), density function, and g(.), massfunction, respectively:

T0 = [x0 y0 0] = [ 30 30 0.1 ], (17)

Dp||q =

p(x) log

(p(x)q(x)

). (18)

In order to apply the KullbackLeibler divergence to themaps built by the SLAM algorithms, it is first necessary to

calculate the probability associated to every point within themap. That is, every point in the space travelled and mappedby the robot should have a probability value associated withit. The most widely used solution in the literature is togrid the map.6 Nevertheless, the gridding process demandshigh computational cost, directly related to the size of thegrid and the map, making it unsuitable for possible realtime applications. In addition, a probability value must becalculated for each cell in the gridded map.

In this work we propose a Monte Carlo experiment forgenerating a set of significant points covering the map andthen, calculate the probability value associated with eachpoint using the concept of sum of Gaussians.21 The followingsection will explain the last in detail.

4.1. Monte Carlo experimentThe Monte Carlo experiment consists of generatingEuniformly distributed points within the map built by theSLAM. In order to do so, the limits of the map must be

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

Towards features updating selection based on the covariance matrix of the SLAM system state 279previously defined. For comparison purposes, the same setE should cover all the maps that are going to be used in theKullbackLeibler divergence. Thus, the limits of the mapare calculated according to the maximum and minimumvalue occupied by a feature in the Cartesian coordinatesystem in both maps. For example, if we use Figs. 4(a)and 4(b), the minimum common x-coordinate (Xmax) limitbetween both maps is near 9 m; the maximum common x-coordinate (Xmin) limit is near 19 m; the minimum commony-coordinate (Ymax) limit is near 6 m and the maximumcommon y-coordinate (Ymin) limit is near 27 m. Thus, the setof points E should be uniformly generated within those limits.The Monte Carlo experiment is then executed as shown inEqs. (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 uniformlygeneration of points, where # E is the number of pointsthat will be used; the pair (mi ni )T E represents a pointof the map bounded by the limits Xmin, Xmax, Ymin andYmax. Figure 5 shows how a partiality of the map shown inFig. 4(a) is affected by the Monte Carlo experiment. For thisrealization were used 200 points (#E = 200). In addition,the map in Fig. 4(b) has the same Monte Carlo pointsdistribution.

4.2. Sum of GaussiansTo calculate the probability of a point inside the built mapit is necessary to determine how a feature influences theprobability of that point. By considering that all features

Fig. 5. Monte Carlo points generation. A partiality of the mapshown in Fig. 4(a) is covered by the Monte Carlo points (solid greycircles); the solid black lines are the segments associated to wallsof the environment; light grey circles are corners and black squaresare the beginning and ending points of the segments.

included in the SLAM system state are Gaussian randomvariables, the probability of a point given all these featurescan be estimated by a sum of Gaussians.27 Thus, if L is thenumber of features contained in the SLAM system state, theprobability of point the pi = [mi ni ]T is calculated as it isshown in (23).

Probability of pi

=L

k=1k

1(2)2 |Pk|

e 12

([mini

][

kxky

])TP1k

([mini

][

kxky

]). (23)

In Eq. (23), Pk is the covariance matrix of the kth feature ofthe SLAM system state, determined as shown in Eq. (24); kxand ky are the estimated mean values of the feature; miandni are the Cartesian coordinates of the point and k is theweight factor associated with the Gaussian distributions toaccomplish the probability distribution fusion:

t =

v1.

.

.

k.

.

.

Pt =

Pvv,t PTkv,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 therobots covariance; Pkv,t is the cross-correlation between therobots covariance and the kth features covariance and Pkk,tis the covariance matrix of the kth feature.

In this work, the environment is represented by lines andcorners, where corners are defined in the Cartesian space andlines in the polar space see Eqs. (7) and (8). In order touse Eq. (23) as an estimate of the probability of a point, it isnecessary to refer all distribution functions to a same space.The appropriate transformations carried out can being seen inref. [23]. Thus, once the probabilities of all points of E withrespect to all features of the map are obtained, they must becollapsed into one representative value for each point. To doso, a weighted sum of Gaussians is proposed as was statedin Eq. (23). The k factors in (23) are obtained according toref. [27]. The final result is always less than 1 and the sumof all probability values is also less than 1.

Using the sum of Gaussians concept we have aprobabilistic representation of all the points of Ecoveringthe map raised by the SLAM.

4.3. KullbackLeibler divergenceApplying the Monte Carlos points generation and thecalculation of the probability value associated with eachof these points by means of the sum of Gaussians method,we can convert the map obtained by the SLAM algorithmin a probability mass function. This probability massfunction has the Monte Carlos set of points (E) as itsdomain. Then, converting the maps obtained by the SLAMalgorithms (see Figs. 4(a) and 4(b)) it is possible to calculate

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

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

Fig. 6. Mean and Variance of the divergence after several MonteCarlo realizations with different number of points generation. (a)Divergence between the map built by the classical sequential EKF-SLAM (considered as p(.) in Eq. (18)) and the map built by theEKF-SLAM with feature updating selection approach (consideredas q(.) in Eq. (18)); (b) In this case, the map built by the EKFSLAM with feature updating selection is considered as p(.) in Eq.(18) whereas q(.) is the map built by the classical sequential EKFSLAM.

the divergence between their associated probability massfunctions.

Figure 6 shows the divergence between the two mapsshown in Fig. 4. For this result, several Monte Carlorealizations 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 Carlorealization. Figure 6(a) shows the divergence of the map builtby the classical sequential EKFSLAM with respect to themap built by the EKFSLAM with feature updating selection,and Fig. 6(b) shows the divergence between the map built bythe feature selection criterion and the classical EKFSLAMone (remember that we are using a pseudo-metric measurefor the divergence of maps thus being sensitive to the orderthe comparison is performed).

By inspection in Fig. 6 is possible to see that, in bothcases, after the experiment of 2 104 points, the meanand variance of the experiment remain approximately inthe same value. That means, a set of #E = 2 104 pointsis needed to obtain a divergence value between the mapsshown in Fig. 4. Furthermore, Fig. 7 shows the divergenceof the maps shown in Fig. 4 with respect to a digitalizedversion of the facilities of the Instituto de Automatica. Inorder to compute this divergence value, the features of thereal map (the digitalized architectonic draw of the Institutode Automatica) were considered as degenerated Gaussiansfeatures.

Figure 7 also shows that the divergence of the EKFSLAM with feature updating selection map with respectto the real map remains smaller than the divergence of theclassical sequential EKFSLAM map with respect to thereal map, for all the Monte Carlo realizations. In both casesin Fig. 6, from #E = 1 104 we can have an idea of thedivergence. The fact that the SLAM with feature selectionapproach shown in this work presents a smaller value ofdivergence with respect to the real map than the classicalsequential EKFSLAM can be interpreted as the mapobtained by the proposal of this work, acquires more accurategeometrical information of the environment than the classicalimplementation.

5. ConclusionsIn this paper, an EKFSLAM algorithm based on therestriction in the number of features to be corrected waspresented. The correction restriction was based on selectingthe most significant features in the sense of the covar-iance sensibility ratio of the SLAM system state. Thus,those features with the smallest sum of eigenvalues in thesensibility expression of the SLAM covariance matrix (seeEq. (9)) were chosen for the update stage of the EKF. Thelast ensured that the estimation process will not turn intoinconsistency.

The restriction also implied that the computational cost ofSLAM process was bounded (when bounded the numberof features to be corrected, as was implemented in thiswork) which reduced the open loop time of the mobilerobot. Thus, the smaller the open loop time the higher thenumber of control actions to drive the robot and to map theenvironment. The fact of implementing a SLAM algorithmwith bounded correction stage makes the algorithm moresuitable for control strategies implementations.

In order to define the precision of the map, the KullbackLeibler divergence was carried out between the mapsobtained by both SLAM algorithms implemented in thiswork and a digitalized architectonic draw of the environment.For several Monte Carlo experiments, the map built bythe EKFSLAM with feature updating selection turnedto be less divergent than the map built by classicalsequential EKFSLAM when compared to the real mapof the environment. Thus, the map built by the SLAMwith updating restrictions presented in this paper is moregeometrically similar to the real map than the one built bythe classical sequential EKFSLAM. The last result ensures

http://journals.cambridge.org Downloaded: 24 Nov 2013 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 planningpurposes.

AcknowledgmentsThe authors gratefully acknowledge Universidad Nacional deSan Juan of Argentina, CONICET (Argentina) and ANPCyTfor funding this research.

References1. 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 localizationand mapping (SLAM): Part I essential algorithms, IEEERobot. Autom. Mag. 13, 99108 (2006).

5. H. Durrant-Whyte and T. Bailey, Simultaneous localizationand 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 (MITPress, Cambridge, 2005).

7. J. E. Guivant and E. M. Nebot, Optimization of thesimultaneous localization and map-building algorithm for real-time implementation, IEEE Trans. Robot. Autom. 17, 242257(2001).

8. A. Garulli, A. Giannitrapani, A. Rosi and A. Vicino, Mobilerobot SLAM for Line-Based Environment Representation,Proceedings of the 44th IEEE Conference on Decision andControl, pp. 20412046, Seville, Spain (2005).

9. J. Castellanos, R. Martinez-Cantin, J. Tardos and J. Neira,Robocentric map joining: Improving the consistency of EKF-SLAM, Robot. Auton. Sys. 55. 2129 (2007).

10. T. Tao, Y. Huang, F. Sun and T. Wang, Motion Planning forSLAM Based on Frontier Exploration, Proceedings of theIEEE International Conference on Mechatronics and Auto-mation, pp. 21202125, Takamatsu, Kagawa, Japan (2007).

11. B. Xi, R. Guo, F. Sun and Y. Huang, Simulation Researchfor Active Simultaneous Localization and Mapping Basedon Extended Kalman Filter, Proceedings of the IEEEInternational Conference on Automation and Logistics,pp. 24432448, Qingdao, China (2008).

12. J. Knight, A. Davison and I. Reid, Towards Constant TimeSlam 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 Task-Relevant Features From Robot Data, Proceedings of theIEEE International Conference on Robotics and Automation,pp. 499504, Seoul, Korea (2001).

14. E. Brunskill and N. Roy, SLAM using Incremental Probabil-istic PCA and Dimensionality Reduction, Proceedings of theIEEE 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, ASimultaneous Localization and Mapping Method Based onFast-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 LaserRange Finder and Monocular Video Images, IEEE LatinAmerican Robotic Symposium, pp. 16, Sao Luis-MA, Brazil(2005).

17. S. Fu, H. Liu, L. Gao and Y. Gai, SLAM for MobileRobots using Laser Range Finder and Monocular Vision,International Conference on Mechatronics and Machine Visionin 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 MonocularSLAM, The 17th British Machine Vision Conference(BMVC), pp. 588596, Edinburgh, UK (2006).

20. Y. J. Lee and J. B. Song, Autonomous Selection, Registrationand Recognition of Objects for Visual SLAM in Indoor

http://journals.cambridge.org Downloaded: 24 Nov 2013 IP address: 200.83.129.91

282 Towards features updating selection based on the covariance matrix of the SLAM system stateEnvironments, The International Conference on Control,Automation and Systems, pp. 668673, Seul, Korea (2007).

21. D. Zhang, L. Xie and M. D. Adams, Entropy based FeatureSelection Scheme for Real Time Simultaneous Localizationand Map Building IEEE Conference on Intelligent Robotsand Systems, pp. 649654, Edmonton, Canada (2005).

22. S. Fintrop, P. Jensfelt and H. I. Christensen, AttentionalLandmark Selection for Visual SLAM, Proceedings of theInternational Conference on Intelligent Robots and Systems,pp. 25822587, Biejing, China (2006).

23. F. A. A. Cheein, Simultaneous Localization and Mapping of aMobile Robot based on Uncertainty Zones Navigation Ph.D.Thesis (San Juan, Argentina: National University of San Juan,2009).

24. M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte andM. Csorba, A solution to the simultaneous localization andmap building (SLAM) problem, IEEE trans. Robot. Autom.17, 229241 (2001).

25. S. J. Julier and J. K. Uhlmann, A counter Example to theTheory 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 InformationTheory, 2nd ed. (Wiley-Interscience, New York, USA,2006).

27. A. S. Miralles and M. A. S. Bobi, Global Path Planning inGaussian Probabilistic Maps, J. Intell. Robot. Syst. 40, 89102(2004).