Consistency of SLAM-EKF Algorithms for Indoor Environments

Consistency of SLAM-EKF Algorithmsfor Indoor Environments

Diego Rodriguez-Losada & Fernando Matia &

Luis Pedraza & Agustin Jimenez & Ramon Galan

Received: 5 March 2007 /Accepted: 11 July 2007 /Published online: 29 August 2007# Springer Science + Business Media B.V. 2007

Abstract The solution to the Simultaneous Localization And Mapping (SLAM) problemusing an Extended Kalman Filter (EKF) is probably the most extended in the literaturedespite the recently reported inconsistency of its estimation. There has been an importantlack of successful SLAM-EKF implementations for indoor environments that could buildmonolithic large maps with features conveying angular information. In this paper weanalyze the source and factors of the SLAM-EKF inconsistency in indoor environments(where the landmarks contain angular information) and we review current existingapproaches presenting novel solutions to this problem that let us build indoor largemonolithic feature based maps.

Keywords Mobile robots . Simultaneous Localization AndMapping . Extended KalmanFilter . Consistency . Linearization errors . Indoor environments

1 Introduction

A mobile robot requires an internal representation (map) of the environment to perform itstask. If true autonomy is desired, the robot has to automatically build that map whilelocalizing itself in it. This is known as the Simultaneous Localization And Mapping(SLAM) problem.

The probabilistic approach has dominated the solution to the SLAM problem [1]. Manydifferent stochastic mapping algorithms are found in the literature, from occupancy gridmapping [2] to the recent MonteCarlo particle filter based algorithm [3]. ExpectancyMaximization [4, 5], hybrid approaches [6] and even topological SLAM [7] are also othersuccessful solutions to the problem. Since the seminal paper [8] the Extended Kalman Filter(EKF) [9] has probably been the most extended approach to SLAM. They introduced a

J Intell Robot Syst (2007) 50:375–397DOI 10.1007/s10846-007-9171-8

D. Rodriguez-Losada (*) : F. Matia : L. Pedraza :A. Jimenez : R. GalanIntelligent Control Group, Universidad Politecnica de Madrid, UPM,C/Jose Gutierrez Abascal, 2, 28006 Madrid, Spaine-mail: [email protected]

framework for handling uncertain spatial relationships, which enables a mobile robot toincrementally build a map of its environment using an EKF for estimating the map,represented by a Gaussian distribution.

The SLAM-EKF solution has many interesting theoretical advantages: (1) it can beapplied incrementally, (2) it is able to close loops correctly because it maintains the full(Gaussian) posterior and (3) it can partially handle dynamic environments. Its maindrawback is its computational complexity O(n2), where ‘n’ is the number of features, whichlimits its real time application to small-medium size environments. Nevertheless, a lot ofrecent research has shown that this computational cost can be reduced as in [10, 11] or evenwithout loosing the optimality of the filter [12–15]. Information Filters have also beenrecently used [16] for this purpose.

A problem that has been broadly avoided in the SLAM-EKF literature is the inconsistencyand divergence of the estimation due to non-negligible linearization errors. It is widelyassumed that the linear assumption is correct, as in the properties of the SLAM-EKFalgorithm described in [17–19], which are known to be the strongest proofs of convergencein the SLAM domain. Initially, the work of [20] stated that the estimation of the SLAM-EKF is inconsistent even in simple scenarios, proving that those properties do not hold inthe real case. The topic has recently attracted more attention as it can be seen in [21–24].We have realized in our implementations [25–27] that this problem is even worse if thelandmarks contain angular information. This is the case of indoor environments, wherewalls are typically modeled as segments. The source and factors of this inconsistency willbe described in Section 2. Section 3 is an overview of existing solutions and also presentsour novel approaches, that let us build large monolithic feature based maps of indoorenvironments as described in Sections 4 and 5. Section 6 summarizes our conclusions.

2 Source and Factors of Indoor SLAM-EKF Inconsistency

2.1 Indoor SLAM-EKF Formulation

Indoor environments are typically composed by straight walls that clearly define landmarksthat convey angular information. Let us suppose that every feature F in the environment(assumed as bidimensional) can be parameterized as a three dimensional vector containingits position and angle, as opposed to most approaches that only consider the landmarkposition:

xF ¼ x y θ½ �T ð1Þ

At a certain time step k, the state vector x (k) is defined by ‘n’ map features, includingthe robot (let it be the first one)

x kð Þ ¼ xTR kð Þ xTF2kð Þ � � � xTFn

kð Þ� �T ð2Þ

As its actual value is unknown, this vector is the stochastic variable that will beestimated by the EKF and approximated by a Gaussian distribution with mean bx k kjð Þ andcovariance P k kjð Þ:

x kð Þ � N bx k kjð Þ;P k kjð Þð Þ ð3Þ

376 J Intell Robot Syst (2007) 50:375–397

When the robot performs an incremental movement u (k+1), it is assumed as Gaussianwith mean bu k þ 1ð Þ (the actual value provided by odometry) and covariance Q (k+1)

u k þ 1ð Þ � N bu k þ 1ð Þ;Q k þ 1ð Þð Þ ð4ÞUnder the Markov assumption, the only moving object is the robot, so the prediction

step of the EKF is defined as:

x k þ 1ð Þ ¼ f x kð Þ; u k þ 1ð Þð Þ

f ¼$ xR k þ 1ð Þ ¼ fR xR kð Þ; u k þ 1ð Þð Þ ¼ xR kð Þ � u k þ 1ð ÞxFi k þ 1ð Þ ¼ xFi kð Þ ;8i ¼ 2; . . . ; nf g

�

The estimate is computed as:

bxR k þ 1 kjð Þ ¼ bxR k kjð Þ � bu k þ 1ð ÞbxFi k þ 1 kjð Þ ¼ bxFi k kjð Þ 8i ¼ 2; . . . ; nf gP k þ 1 kjð Þ¼Fx k þ 1ð ÞP k kjð ÞFT

x k þ 1ð Þ þ Fu k þ 1ð ÞQ k þ 1ð ÞFTu k þ 1ð Þ

where Fx k þ 1ð Þ ¼ @f@x

��bx k kjð Þbu kþ1ð Þ;Fu k þ 1ð Þ ¼ @f

@u

��bx k kjð Þbu kþ1ð Þ

Then, the robot performs an observation bz k þ 1ð Þ (the actual value provided by the sensor).The probability of the theoretical noise-free measurement z (k+1) is also supposed to beGaussian with mean bz k þ 1ð Þ and covariance R (k+1)

z k þ 1ð Þ � N bz k þ 1ð Þ;R k þ 1ð Þð Þ ð7ÞThe matching of this measurement against the existing map can be done [28] with an

implicit measurement equation h:

h z k þ 1ð Þ; x k þ 1ð Þð Þ ¼ 0 ð8ÞThis equation can be particularized for every feature Fi of the map using the composition

operators [8], and linearized around the current known values bz k þ 1ð Þ;bx k þ 1 kjð Þ.h k þ 1ð Þ ¼ �xFi k þ 1ð Þ � xR k þ 1ð Þ � z k þ 1ð Þ ¼ 0

h z k þ 1ð Þ; x k þ 1ð Þð Þ ’ h bz k þ 1ð Þ;bx k þ 1 kjð Þð ÞþHx k þ 1ð Þ x k þ 1ð Þ � bx k þ 1 kjð Þð Þ½ � þHz k þ 1ð Þ z k þ 1ð Þ �bz k þ 1ð Þð Þ½ �

Hx k þ 1ð Þ ¼ @h@x

��bx kþ1 kjð Þbz kþ1ð Þ; Hz k þ 1ð Þ ¼ @h

@z

��bx kþ1 kjð Þbz kþ1ð Þ

The actual value for the estimate bh (known as measurement vector) is typically differentfrom zero, and its covariance is:

bh k þ 1ð Þ ¼ bh bz k þ 1ð Þ;bx k þ 1 kjð Þð Þ 6¼ 0S kþ 1ð Þ ¼ Hx k þ 1ð ÞP k þ 1 kjð ÞHT

x k þ 1ð Þ þHz k þ 1ð ÞR k þ 1ð ÞHTz k þ 1ð Þ ð10Þ

This pairing can be tested with a confidence value α with a Mahalanobis test:

bhT k þ 1ð ÞS�1 kþ 1ð Þbh k þ 1ð Þ < X 2

dim bh kþ1ð Þ� �

;αð11Þ

ð5Þ

ð6Þ

ð9Þ

J Intell Robot Syst (2007) 50:375–397 377

If the pairing is accepted, the correction step can be computed as follows:

W kþ 1ð Þ ¼ P k þ 1 kjð ÞHTx kþ 1ð ÞS�1 kþ 1ð Þ

bx k þ 1 k þ 1jð Þ ¼ bx k þ 1 kjð Þ �W kþ 1ð Þbh k þ 1ð ÞP k þ 1 k þ 1jð Þ ¼ I�W kþ 1ð ÞHx kþ 1ð Þ½ �P k þ 1 kjð Þ

If the measurement does not satisfy 11, then it is considered as a new feature and isadded to the map, augmenting it with a new feature Fn+1 as described in the followingequations:

x k þ 1ð Þ ¼ g x k þ 1ð Þ; z k þ 1ð Þð Þg ¼$ xFi k þ 1ð Þ ¼ xFi k þ 1ð Þ;8i ¼ 1; . . . ; nf g

xFnþ 1 k þ 1ð Þ ¼ gFnþ 1xR k þ 1ð Þ; z k þ 1ð Þð Þ ¼ xR k þ 1ð Þ � z k þ 1ð Þ

�

Thus, the new estimate is:

bxFi k þ 1 k þ 1jð Þ ¼ bxFi k þ 1 kjð Þ ;8i ¼ 1; . . . ; nf gbxFnþ1 k þ 1 k þ 1jð Þ ¼ bxR k þ 1 kjð Þ �bz k þ 1ð ÞP k þ 1 k þ 1jð Þ ¼ GxP k þ 1 kjð ÞGT

x þGzR k þ 1ð ÞGTz

where Gx ¼ @g@x

��bx;bz ; Gz ¼ @g@z

��bx;bzThere are few approaches that can rigorously handle the geometric symmetry of

segments within the EKF. We have used the SPMap approach [29] which is probably thebest existing framework for handling this problem. The complete formulation of ourimplementation of this solution can be found at [25, 26].

2.2 Source of Inconsistency

The estimation of the EKF at time step k is said to be consistent if the difference betweenthe estimated P(k|k) and the true P(k) covariance matrices is positive semidefinite.

It is widely assumed that the EKF linear assumption is correct, as in the consistency andconvergence properties proofs of the SLAM-EKF algorithm of [17–19], which are known tobe the strongest proofs of convergence in the SLAM domain [1]. Despite these interestingtheoretical properties there has been an important lack of successful implementations forlarge indoor environments with features containing angular information.

It is a well known problem that linearizations of the EKF can lead to divergence of thefilter [30], but until [20] this problem had not been reported. Julier and Uhlman [20]analyzed how the estimation could become inconsistent after hundreds of time steps. Manyrecent researchers [21–24] have shown that this problem arises in fact much earlier, evenbefore the computational cost becomes an issue. All of them use a range-bearing sensor todetect point features. But in indoor environments, the most common features are walls, thatcan be modeled as segments or point oriented landmarks, containing angular information.We have realized in our implementations [25, 26] that this problem can be even worse inthis case. This section analyzes the source and factors of this inconsistency.

It can be easily derived from 9 that the measurement equation is non-linear in the robotangle, thus the robot angular estimation will be used as the main variable for studying theconsistency. Let’s imagine a static robot located at the origin, with just one degree of

ð12Þ

ð13Þ

ð14Þ


freedom (rotation) with initial angular uncertainty PqR 0 0jð Þ. The initial estimated state of thesystem is:

bx 0 0jð Þ ¼ 0P 0 0jð Þ ¼ PθR 0 0jð Þ½ �

As the robot is static, the prediction step 5 for every time step is simplified to:

bx k þ 1 kjð Þ ¼ bx k kjð ÞP k þ 1 kjð Þ ¼ P k kjð Þ ð16Þ

A single point-oriented landmark is available, and an observation of this landmark andits covariance is:

bz k þ 1ð Þ ¼ bx z k þ 1ð Þ byz k þ 1ð Þ bθz k þ 1ð Þ� �T

; R k þ 1ð Þ ¼Rx 0 00 Ry 00 0 Rθ

24

35 ð17Þ

The estimation state, once the feature is initialized in the map, can be represented for anytime step ‘k’ by:

bx k kjð Þ ¼ bθR k kjð Þ bxF k kjð Þ byF k kjð Þ bθF k kjð Þ� �T

P k kjð Þ ¼PθR k kjð Þ PθR xF k kjð Þ PθR yF k kjð Þ PθRθF k kjð Þ

PxF k kjð Þ PxFyF k kjð Þ PxF θF k kjð ÞPyF k kjð Þ PyF θF k kjð Þ

PθF k kjð Þ

2664

3775

If the robot is static, it is clear that the robot estimation must remain constant toguarantee consistency, that is, for every time step it must hold:

bθR k þ 1 k þ 1jð Þ ¼ bθR k þ 1 kjð ÞPθR k þ 1 k þ 1jð Þ ¼ PθR k þ 1 kjð Þ ð19Þ

From the correction step of the EKF 12 it can be deduced that the consistency can beguaranteed [20] if the Kalman gain for the robot angle is equal zero:

WqR kþ 1ð Þ ¼ 0 ð20ÞDeveloping this expression with some simple algebra, the following equalities are

obtained, where time indices are omitted for clarity:

PθR � PθRθFð Þ bxz sin bθF � bθR�

�byz cos bθF � bθR� h i

� PθR xF þ byFPθR θFð Þ cosbθF � PθRyF � bxFPθRθF

� �sinbθF ¼ 0

PθR � PθR θFð Þ bxz cos bθF � bθR�

þbyz sin bθF � bθR� h i

þ PθRxF þbyFPθRθFð Þ sinbθF � PθRyF �bxFPθRθF

� �cosbθF ¼ 0

PθR � PθRθF ¼ 0

ð15Þ

ð18Þ

ð21Þ


At time step k=1, the first observation bz 1ð Þ is used to initialize the landmark in thesystem state as detailed in 14 leading to:

bx 1 1jð Þ ¼ 0 bxz 1ð Þ byz 1ð Þ bθz 1ð Þ� �T ¼ 0 bxF 1 1jð Þ byF 1 1jð Þ bθF 1 1jð Þ

� �T

P 1 1jð Þ ¼PθR 0 0jð Þ �PθR 0 0jð ÞbyF 1 1jð Þ PθR 0 0jð ÞbxF 1 1jð Þ PθR 0 0jð Þ

PθR 0 0jð Þby2F 1 1jð Þ þ Rx �PθR 0 0jð ÞbxF 1 1jð ÞbyF 1 1jð Þ �PθR 0 0jð ÞbyF 1 1jð ÞPθR 0 0jð Þbx2F 1 1jð Þ þ Ry PθR 0 0jð ÞbxF 1 1jð Þ

PθR 0 0jð Þ þ Rθ

2664

3775

It can be seen that the condition above 21 is satisfied at time step k=2, considering fromEq. 22 that:

PθRθF 1 1jð Þ ¼ PθR 1 1jð ÞPθRxF 1 1jð Þ ¼ �byF 1 1jð ÞPθRθF 1 1jð ÞPθRyF 1 1jð Þ ¼ bxF 1 1jð ÞPθRθF 1 1jð Þ

Then, the consistent correction step ensures that:

PθR 2 2jð Þ ¼ PθR 1 1jð Þ; PθRxF 2 2jð Þ ¼ PθRxF 1 1jð ÞPθRyF 2 2jð Þ ¼ PθRyF 1 1jð Þ; PθRθF 2 2jð Þ ¼ PθRθF 1 1jð Þ ð24Þ

But in the general case of a non-zero innovation, it is also true that:

bxF 2 2jð Þ 6¼ bxF 1 1jð ÞbyF 2 2jð Þ 6¼ byF 1 1jð ÞbθF 2 2jð Þ 6¼ bθF 1 1jð Þð25Þ

So at time step k=3, the estimation is guaranteed to be inconsistent because:

PθR θF 2 2jð Þ ¼ PθR 2 2jð ÞPθRxF 2 2jð Þ ¼ �byF 1 1jð ÞPθRθF 1 1jð Þ 6¼ �byF 2 2jð ÞPθRθF 2 2jð ÞPθRyF 2 2jð Þ ¼ bxF 1 1jð ÞPθRθF 1 1jð Þ 6¼ bxF 2 2jð ÞPθRθF 2 2jð Þ

It is proved how the estimation of the SLAM-EKF when using oriented-point landmarks,becomes inconsistent in just three time steps even in the simplest scenario of a static robotwith angular uncertainty. It can also be seen that the term PqRqF is involved in the equations,showing that the correlation between the robot and the feature angle plays an importantrole. It must be highlighted that segments extracted from a laser scanner have a very lowangular uncertainty and become very highly correlated with the robot angle. As shown, theupdates done on the feature estimated position generate the errors that cause inconsistency.The magnitude of these updates depends on many factors as the sensors uncertainty, thatwill be analyzed in the next section.

2.3 Characterization

2.3.1 Static Robot

As the evolution of the estimation is analytically intractable, and it depends on theparticular stochastic values of the observations, simulations are used in the followingexperiments. The particular values of the noise applied to the observations in the

ð22Þ

ð23Þ

ð26Þ


simulations at each time step ‘k’ are exactly the same along variations of each parameterconsidered in the same experiment, to achieve repeatability. In the next simulation, thelandmark is located at position (x=1,y=1,θ=0), and it is observed with measurementnoise with covariance Rx ¼ Ry ¼ Rq ¼ 0:0024 (m2, rad2). The evolution of the estima-tion along 500 time steps for different initial robot angular covariance values PqR ¼0; 2; 4; 6; 8; 10f g � 10�3 is shown in Fig. 1.As mathematically derived, the covariance quickly begins to decline (inconsistency) and

spurious corrections are done to the robot angle (divergence). The evolution is random, but it canbe seen alongmultiple experiments that a larger initial covariance produce a larger inconsistency.

It can also be seen that a lower measurement noise can also produce higherinconsistencies, as previously stated for the generic EKF by [30]. Consider now sensors“Sensor2” and “Sensor3” that use the following respective covariance values: R ¼ 6e� 4(m2, rad2), R ¼ 14e� 4 (m2, rad2). The results of the simulation are shown in Fig. 2,showing that higher decrements of covariance and larger spurious correction to the robotangle are produced.

The sensor range is also important, because the further the landmark is, the worse thelinearization and larger the inconsistency are. If the following landmark position values areused z1=[1 1 0]T, z2=[2 2 0]T and z3=[3 3 0]T, the angle estimations obtained arerepresented in Fig. 3.

It was noted in the experiments, that the inconsistency in the first three time steps isindependent from the observation covariance and the feature position. Now, the experimentof Fig. 1 is repeated with a little variant: after the third observation the feature is deletedfrom the map, so the next observation has to initialize the feature again, and the cycle isrepeated every three observations. This deletion can be done without jeopardizing theestimation as stated in [10] if a subsequent observation of that feature is followed by thereinitialization of that feature in the state estimation. This algorithm is a suboptimalapproach that in the linear case should also produce a consistent and convergent estimation.

When this experiment is carried out, it can be observed (Fig. 4) that the robot angularcovariance evolves irrespective of the observation noise or the feature pose. Its hyperbolicdecrease obeys to the following empirical equation that only depends on the initial robot angularcovariance PqR 0ð Þ, and can be considered as the intrinsic inconsistency of the estimation.

PqR kð Þ ¼ 3

k þ 3PqR 0ð Þ

ð27Þ

0 100 200 300 400 500-0.02

0

0.02

0.04

0.06

0.08

0.1

Time step

Angle

(rad)

Robot angle

00.0020.0040.0060.0080.01

0.01

0.002

0

0.004 0.006

0.008

0 100 200 300 400 5000

1

2

3

4

5

6

7

8

9

10x 10

-3

Time step

Covariance(r

ad2)

Robot angular covariance

00.0020.0040.0060.0080.01

Fig. 1 Inconsistency: static robot angle estimation with different initial angle covariance


It is empirically proved how inconsistency grows quadratically with the robot angularcovariance, and that it appears for every value greater than zero.

P0θR

kð Þ ¼ $PθR

$ k� P 2

θRkð Þ ð28Þ

2.3.2 Moving Robot

To see the behavior of the inconsistency when the robot moves, a reference estimation isneeded. This reference estimation can be obtained carrying out a stochastic change of theworld reference frame, trying to minimize the EKF linearization effects. Before adding anew feature to the estimation, a change of base to the current robot position estimation isaccomplished 29, as represented in Fig. 5.

xW2 kð Þ ¼$ xW2R kð ÞxW2W1

kð Þ �

¼ �xW1R kð Þ � xW1

R kð Þ�xW1

R kð Þ �

¼ 0�xW1

R kð Þ �

PW2 k kjð Þ ¼ 0 00 JRP

W1R k kjð ÞJTR

�with JR ¼ @x

W2W1

kð Þ@x

W1R kð Þ

��bx W1 k kjð Þ

ð29Þ

0 100 200 300 400 500-0.06

-0.05

-0.04

-0.03

-0.02

-0.01

0

0.01

0.02

0.03

Time step

Angle

(rad)

Robot angle

Sensor3Sensor2Sensor1

Sensor1

Sensor2 Sensor3

0 100 200 300 400 5003

4

5

6

7

8

9

10x 10

-3

Time step

Covariance(r

ad2)


Sensor3Sensor2Sensor1

Sensor2

Sensor3

Sensor1

Fig. 2 Inconsistency: static robot angle estimation with different sensor accuracy

0 100 200 300 400 500-0.02

0

0.02

0.04

0.06

0.08

0.1

0.12

0.14

Time step

An

gle

(ra

d)

Robot angle

z=[1 1 0]z=[2 2 0]z=[3 3 0]

z=[1 1 0]

z=[2 2 0]

z=[3 3 0]

0 100 200 300 400 5002

3

4

5

6

7

8

9

10x 10

-3

Time step

Co

va

ria

nce

(ra

d2

)

Robot angular covariance z=[1 1 0]z=[2 2 0]z=[3 3 0]

z=[1 1 0]

z=[2 2 0]

z=[3 3 0]

Fig. 3 Inconsistency: static robot angle estimations with different landmark positions


With this transformation, the robot covariance is stored, “frozen,” in the new worldreference frame W2. The feature is initialized in that frame W2 with a covariance equal tothe measurement one, and the EKF updates are computed with minimum robot angularcovariance, in a similar way to the robocentric mapping [21].

The following experiment is then defined: the robot moves at constant speed along the Xaxis observing features that are evenly spaced. Continuous exploration is then achieved,with a theoretically monotonic increasing robot covariance. The robot trajectories andcovariance are obtained from three different estimations: the pure odometry withoutmeasurement updates (odometry), the standard algorithm (global EKF), and the referenceestimation (local EKF) that minimizes linearization effects applying 29 before adding eachnew feature to the estimation.

The simulation parameters are defined as follows: the robot starts at the origin with zerouncertainty. The robot moves along the X axis 10 cm per time step, i.e. u k þ 1ð Þ ¼0:1 0 0½ �T, with an odometry noise Qx ¼ Qy ¼ Qq ¼ 0:000025 (m2, rad2). For eachmeter ‘i’ there is a feature Fi with xFi ¼ i 1 0½ �T, and the robot can only observe it frompositions from xR=[i−1 0 0]T to xR=[i 0 0]T (both included), due to the limited sensorrange. As the robot only observes one map feature at each time step, and it never revisitspast features, the current feature can be safely deleted from the estimation [10] when itbecomes out of range and just before adding the next one. The observation covarianceparameters used are Rx ¼ Ry ¼ Rq ¼ 0:0006 (m2, rad2), i.e. using “Sensor2,” and thesimulation is performed along 100 m.

Fig. 5 Stochastic base change tothe robot position

0 200 400 600 800 10000

0.001

0.002

0.003

0.004

0.005

0.006

0.007

0.008

0.009

0.01

Time step

Co

va

ria

nce

(ra

d2

)


Actual3/k

Fig. 4 Inconsistency: hyperbolicangular covariance decrease


The estimation of the two EKF algorithms is very similar during the first 30 m of thetrajectory, but after that, they start to diverge (Fig. 6). The local EKF shows a betterbehavior, being closer to the actual trajectory. The global EKF trajectory suffers spuriouscorrections due to inconsistent updates, and its behavior is even worse than the pureodometry.

The robot angular covariance of the odometry and the local EKF estimation growslinearly with different slopes (Fig. 6), but the standard global EKF estimation reaches anupper limit that depends on the experiment parameters, that is typically very low. In thiscase, it is only 0.006 rad2. The covariance in the Y-axis is one order of magnitude lower forthe standard global EKF algorithm than for the reference local EKF.

The Normalized Estimation Error Squared (NEES; 30) with a confidence level α can beused to check the inconsistency of the standard (global EKF) algorithm, while the referenceestimation (local EKF) remains consistent for the whole trajectory.

NEES ¼ bxR � xRð ÞTP�1R bxR � xRð Þ X 2

dim bxR� �;α

ð30Þ

Figure 7 shows how the local EKF NEES remains bounded, but the global standard EKFNEES continuously grows, showing the inconsistency of its estimation. The observableeffect on real data is that the loops are not properly closed. Figure 8 shows a typical runusing the data collected in Belgioioso Castle (Italy), where the observations performedwhen returning back to the origin are not properly paired against the map because anoptimistic (inconsistent) estimation. It is clear that this problem is not due to a lowodometry covariance. In that case, the resulting corrected trajectory would be similar to theodometry one, but it can be seen in the figure how those trajectories are clearly different.

Several factors affect the consistency of the SLAM-EKF estimation in indoorenvironments. Furthermore, the inconsistency appears for every update with a robotangular covariance greater than zero, and it increases quadratically with the robot angularcovariance. Thus, in indoor environments with landmarks conveying angular information, itis not necessary to process hundreds of timesteps as [20] states to observe its effects, andvisible inconsistency can appear in a very short trajectory (few meters) if lookingbackwards (revisiting) is not accomplished, severe occlusions occur (indoor populatedenvironments) and/or large rotational movements (required to navigate indoor environ-ments) are performed. Conclusion: standard feature based SLAM-EKF is not applicable in

0 20 40 60 80 100-5

0

5

10

15

20

Robot trajectory

Y (

m)

0 20 40 60 80 100-0.1

0

0.1

0.2

0.3

X(m)

An

gle

(ra

d)

Global EKF

Global EKF

Local EKF

Local EKF

Odometry

Odometry

0 200 400 600 800 10000

0.2

0.4Robot Covariance

CovX

(m2)

0 200 400 600 800 10000

50

100

CovY

(m2)

0 200 400 600 800 10000

0.01

0.02

0.03

Time Step

CovA

ng(r

ad2)

Global EKF

Global EKF

Global EKF

Local EKF

Local EKF

Local EKF

Odometry

Odometry

Odometry

Fig. 6 Continuous exploration trajectories estimation


indoor environments, except in very small environments and with a restricted explorationbehavior.

3 Improving Consistency

There are several ways to reduce the effects of inconsistency. Some approaches try to limitthe robot angular covariance using absolute information (e.g. using a compass or GPS), butthe challenge is to use propioceptive sensors only (furthermore, both kinds of sensors don’twork well in indoor environments). Consistency might be possible by inflating thecovariance after each update, but it is not clear how to quantify adequate inflation, and inany case the SLAM convergence properties [18] would be violated. This section providesan overview and discussion of several methods that ameliorate the consistency, avoidingmathematical details and focusing on the concepts.

0 200 400 600 800 10000

5

10

15

20

25

30Robot pose NEES

Time Step

NE

ES

Global EKFLocal EKFOdometry

Local EKF

Odometry

Global EKF

Fig. 7 Continuous explorationrobot pose NEES

Fig. 8 Belgioioso Castle loop not closed because of inconsistency


3.1 Shape Constraints

In [31] pairs of features are randomly chosen at each time step, and a geometric(parallelism, orthogonality) relationship between them is tried to be established. If so, avirtual observation with large uncertainty is performed, and the angular uncertainty of themap (and the robot) is slightly reduced. With this approach, the angular uncertaintyreduction can be small enough to produce inconsistency if the robot uncertainty increaserate is high, a situation that commonly happens if the robot is moving fast in unexploredareas, there are many occlusions or a short sensor range.

Therefore, the novel stronger approach outlined in Fig. 9 is proposed. Every time one ormore segments are added to the map they are compared against all the previous ones,looking for a geometric relation (parallelism, orthogonality). After the pairings have beenestablished, an extra EKF update is computed without measurement noise. Thus, a greaterangular covariance reduction is achieved, with a much better consistency. This approachhas been proved to work perfectly in orthogonal and not so orthogonal indoor environments(see Section 4), but it will probably fail in environments with randomly oriented or curvedwalls.

The extracted segments from the laser data are compared with the existing map segmentsin the EKF data association, which includes a joint compatibility test [32]. Associatedsegments are used for the EKF update step and to the geometric update step (remember thatdue to the problem of symmetries and partiality in observations of segments, not all theinformation is updated through the EKF). The segments that are not paired in the dataassociation are new map segments that are adequately added to the map. At this point, theshape constraints are established between these recently added segments and the rest of themap. This association is done using the nearest neighbor approach to avoid too manypairings that would unnecessarily increase computation time. Also, not every previouslyexisting segment in the map is used in the association, but those with a minimum length of2 m, to avoid incorrect pairings. The constraint (parallelism, perpendicularity, colinearity)with the minimum Mahalanobis distance is chosen. With the resulting constraints, anupdate step similar to EKF is done. This process is equal to the proposed in [18] but with

Mk = Map Segments at time step k

OdometryData d=GetOdometryData();

EKFPredictionStep(d);

LaserData L= GetLaserData();

ObservedSegments S=ExtractSegments(L);

AssociatedSegments A S;

NonAssociatedSegments N S; //A ∪ N= S, A ∩N=Ø

(A,N)=JointCompatibilityDataAssociation(O, Mk)

Mk+1=EKFUpdateStep(Mk, A);

Mk+1=GeometricUpdateStep(Mk+1, A);

Mk+1 =AddNewSegments(Mk+1,N);

ShapeConstraintsAssociations C;

foreach s ( Mk+1 Mk) //recently added segments

C+=NearestNeigbourDataAssociation(s, Mk+1)

endfor

Mk+1 =EKFConstraintsUpdateStep(Mk+1, C);

Fig. 9 Pseudocode forSLAM-EKF applying shapeconstraints


the difference that no artificial measurement equation noise is added. Instead, the shapeconstraints are treated as perfectly known geometric relations.

3.2 Local Maps Fusion

As the Constrained Local Submap Filter [14] and the Local Map Sequencing algorithm [13]propose, the use of local maps can highly reduce the computational cost of the standardSLAM-EKF. The use of these local maps was also proven by [21] to reduce theinconsistency of the estimation. Nevertheless there are some problems for applying thismethod related to the inconsistency: divergence of global map estimation due to smallinconsistencies in local maps, and the singularity of the innovation covariance matrix ifshape constraints are applied to solve the first problem. As we describe in our previouswork [33], the first problem can be solved applying shape constraints in the local map(which are not necessary in the global one), and with a careful management of theinnovation covariance matrix. With this approach, we have proved [33] that good resultscan be obtained, both in computational savings and in consistency improvement. The onlydrawback of this approach is that shape constraints are required in the local map as inmethod explained in Section 3.1, thus also suffering a possible misbehavior in curvedenvironments.

3.3 Robocentric Mapping

Castellanos et al. [21] propose to use a map reference frame attached to the robot to reducethe linearization errors. Although this approach is stated to achieve a better consistency,there is a side effect when using it in indoor environments because of partiality inuncertainty. This previously unreported problem is the local deformations that appear whenperforming large corrections in the global map, produced by the geometric symmetries ofsegments [29]; segments cannot be displaced (corrected) by the filter in their longitudinaldirection due to the inexistence of stochastic information in that direction. Thus, theresulting map (as shown in Fig. 10) can be partially correct, as the stochastic information isproperly managed, but almost unusable because of the geometric information (segmentsymmetries) not handled by the filter.

Nevertheless, using this technique together with a local maps fusion approach,Castellanos et al. [34] have developed an algorithm called Robocentric Map Joining thatachieves a much better result, both from the consistency and the computational cost pointsof view.

Fig. 10 Deformations in the map of Belgioiso Castle using single map robocentric mapping


3.4 Using a Tree of Transformations

Although the previous formulation using local maps represents a good balance between theconsistency of the estimation and the computational cost, small inconsistencies can alsoappear and shape constraints have to be applied in the local maps. We propose a novelsolution that uses a tree of transformations to define the map and the robot position.

This tree (Fig. 11) is created by the following process: the algorithm starts as regularSLAM-EKF. But the first time one or more new segments have to be added to the map (firstobservation of segments), instead of computing their estimated position in globalcoordinates, based on the robot estimation and the actual observation, a new referenceframe F1 is added at the current robot position inheriting the robot estimation referenced tothe world frame W. Thus, the new segments are initialized referenced to F1 with anestimation equal to the observation, and the robot pose estimation is referenced to F1, withan initial estimation equal to zero and without uncertainty.

The map is maintained as a tree of transformations, as represented in Fig. 11, containingm auxiliary frames, with frame Fa always referenced to frame Fa−1 being F0 the worldframe W. Every auxiliary frame Fa from F1 to Fm is used to reference from 1 to na segments.The robot is referenced to the last frame Fm. This process is done as outlined in Fig. 12every time new segments have to be added to the map.

The map state is 31, where the subscript indicates the feature and the superscript meansthe reference frame in which that particular feature is represented.

x ¼ xWF1

� Tx F1S1

� T� � � x F1

Sn1

� T� � �

x Fm�1Fm

� Tx FmS1

� T� � � x Fm

Snm

� Tx FmR

� �T �T

ð31ÞWith this map representation, the formulation of the map building algorithm has to be

rewritten, taking special care at the data association and EKF update step that now involvesvariable length measurement equations, jacobians with variable sparsity, and a computa-tional complexity that increases proportionally to the size of the loop that is being closed atthat moment. The overall computational cost is increased, because many new referenceframes are required, but the linearization errors are extremely minimized. We haveestimated from our experiments that the increase in the number of map features is about50%, with an associated quadratic computational cost increase. Although this increase doesnot allow the application of this method in real time, the computation time remainsreasonable. This solution presents a very good consistency, and does not suffer from theproblems of local deformations and it is able to correctly close large loops without applyingany shape constraints.

Fig. 11 The map as a treeof transformations


Furthermore, an optimistic (non-conservative) version of this method that neglects all thenon-diagonal covariance sub matrices, can achieve incredibly good results with very smallcomputational cost. Despite this approach does not guarantee convergence, the resultingmaps are practically indistinguishable from the maps of the full covariance matrixalgorithm.

4 Experiments

The presented SLAM-EKF algorithms have been fully implemented in an optimizedportable C++ kernel. Several experiments have been done in different real environments,and the resulting maps have often been reliably used for navigation purposes. Severalrobots have been used in the experiments, all of them equipped with an horizontallymounted SICK laser running at approximately 5 Hz. The segments from the raw laser dataare extracted at each scan with a split and merge recursive algorithm, followed by a LSQ fit.It has been used the Joint Compatibility Branch and Bound [32] algorithm to improve therobustness of the data association step. The robots were manually controlled to acquire thedata.

Belgioioso Castle (exhibition site, explored with a Pioner-AT in 17′ along 230 m) has 2large loops (∼100 m). The second one cannot be closed by regular SLAM-EKF because ofinconsistency (Fig. 8). The problem of local deformations if applying robocentric mappingalso produces a practically useless map, as shown for the first loop of this environment(Fig. 10).

Figure 13 shows that applying shape constraints leads to an excellent result (evaluatedon topological and visual appearance). A similar result is achieved using the local maps

Mk = Map at time step k

OdometryData d=GetOdometryData();

EKFPredictionStep(d);

LaserData L= GetLaserData();

ObservedSegments S=ExtractSegments(L);

AssociatedSegments A S;

NonAssociatedSegments N S; //A ∪ N= S, A ∩ N=Ø

(A,N)=JointCompatibilityDataAssociation(O, Mk)

Mk+1=EKFUpdateStep(Mk, A);

Mk+1=GeometricUpdateStep(Mk+1, A);

Fm= Last auxiliary frame in chain

R = Robot estimation, referenced to Fm

if (N- 0)

Fm+1 = R //add new frame

Mk+1= (Mk+1, Fm+1) //append frame to chain

R=0 //robot now referenced to Fm+1

foreach s∈N

Mk+1= (Mk+1, s) //append new segments referenced to Fm+1

endfor

endif

Fig. 12 Pseudocode for building the map using a tree of transformations


fusion algorithm. The latter achieves a much lower computation time 17″ (on a 2,4Gh PIV),compared to the 3′46″, with real time performance. Nevertheless it also relies on local shapeconstraints. The tree of transformations obtains a similar result without the shapeconstraints, but with a computation time of 20′. The auxiliary reference frames are alsorepresented. This reasonable time can also be reduced down to 19″ with the describedoptimistic approximation, with an indistinguishable result.

The second experiment was carried out at ETSII-UPM main building ground floor witha B21r. The exploration covered 595 m in 32′50″. Figure 14 shows the correctness and highorthogonality of the resulting map with the tree formulation, despite the lack of applicationof any kind of shape constraint. Although it is not represented in the figure, the slightcurvature of the map is congruent with the odometric trajectory curvature, as it should be. Itis important to highlight that the odometry of the B21r was not calibrated, with approximateerrors of 6%. The total odometry error in the first half of the trajectory (exploration ∼350 m)is about 100 m and 90°.

The shape constraints approach doesn’t necessarily require an orthogonal environment,as long as some structure is present. An exploration of 135 m along the loop of a largetriangular building at Carlos III University was performed. The data were collected with a

Fig. 13 Belgioioso Castle mapping: a Standard SLAM-EKF (parallelism constraints), b Relativeformulation (no shape constraints), c Optimistic simplification of the relative formulation (no shapeconstraints), d Local maps fusion with submaps (local maps parallelism constraints)


B21 at a very low rate of approximately 0.25 Hz, which also helps to increase the robotcovariance and the resulting potential inconsistency. Despite the shape of the environmentand the low data rate, the map is correctly built (Fig. 15) and the 120 m long loop issuccessfully closed, both using the shape constraints and the tree formulation (Table 1).

Fig. 14 UPM ground floor mapping: a Standard SLAM-EKF with parallelism constraints, b Using relativeformulation (no shape constraints) and c Using local maps fusion with submaps, with local maps parallelismconstraints


The robot angular covariance obtained with the different algorithms has been analyzedin the following (Fig. 16) simple experiment: the robot was driven straight along the labmain corridor, returning back to the origin when reaching its end. The robot angularcovariance should increase along the way forth, and decrease in the way back.

Nevertheless, it can be seen in Fig. 17, how the standard SLAM-EKF algorithm presentslarge drops of the covariance in the exploration trajectory (i.e. inconsistency). Thecovariance drops when applying shape constraints doesn’t imply inconsistency, as long asglobal information is being used in these legitimate reductions. The estimation of the localmaps fusion algorithm presents a lower covariance than the relative formulation, but it hasto be considered that parallelism is applied in the local maps, so it doesn’t implyinconsistency either. The use of local maps improves the consistency of the estimation. It

Fig. 15 Carlos III university: a Shape constraints, b Tree formulation

Table 1 Experiments exploration and processing details

Exploration Processing

Environment Robot Distance

(m)

Time Loops Algorithm Shape

constraints

Features Time Real

time

UPM B21r 594 32′50″ 1×

100 m

Standard SLAM-EKF Paralelism 255 9′

53″

No

Relative Formulation No 551 2h3′ No

Local maps fusion Local

paralelism

276 27″ Yes

Belgioioso Pionner 228.2 16′27″ 2×

100 m

Standard SLAM-EKF Paralelism 227 3′

46″

No

Relative formulation No 498 20′

13″

No

Relative formulation

(optimistic)

No 492 29″ Yes

Local maps fusion Local

paralelism

247 17″ Yes

Carlos III B21 135 Unknown 1×

120 m

Standard SLAM-EKF Paralelism 117 1″ Yes

Relative formulation No 181 2″ No


can also be noted that the evolution of the angular covariance in this case closely resemblesthe relative formulation one. The optimistic simplification of the relative formulation is onlyslightly inconsistent. The decrease of the covariance when the robot returns back is delayedif using the local maps fusion, and its estimation presents a larger covariance than therelative formulation at some points of this stage, recovering a lower value after a fusion isperformed.

5 Case Studies

Among the presented solutions, the use of shape constraints has been proven to be veryconvenient for real applications [26] (Fig. 18) while the local maps fusion has beenexcellent for real time multirobot map building of large environments [29]. All mapspresented in Fig. 19 have been built in real time while exploring the environment, usingonly parallelism as shape constraint. In fact, the map of Fig. 19e was built in an “Exploreand Return” experiment [31] (but with a much larger environment), in which a remoteexploration of a populated building was performed with the only help of the map being

Fig. 16 Experiment at UPM-DISAM laboratory


0

0,005

0,01

0,015

0,02

0,025

0,03

0,035

0,04

1 51 101 151 201 251 301 351 401 451 501 551 601 651 701 751 801 851

Time step

Covariance(r

ad2)

Standard SLAM-

EKF

Standard SLAM-

EKF with

paralelism

constraints

Relative

formulation

Optimistic

relative

formulation

Local maps

fusion (local

paralelism)

exploration return

Fig. 17 Robot angular covariance with different algorithms


Fig. 18 Urbano robot (left)as a tour guide at PrincipeFelipe Museum and Guido™SmartWalker (right) guiding ablind woman at St. Mary’sHospital

Fig. 19 Maps built in real time while exploring the environment. With Guido: a St Mary’s Hospital forBlind Women (Dublin, Ireland). d Haptica´s office (Dublin, Ireland). With Urbano: b Indumatica Trade Fair(Madrid, Spain). c Exhibition at Principe Felipe Museum (Valencia, Spain). e Demo at UPM Collegebuilding (Madrid, Spain)


built, integrating path planning and reactive control with the real time SLAM process.Large loops are successfully closed in St. Mary’s Hospital (100 m long loop) Fig. 19a, andin Indumatica exhibition (60 m long loop) Fig. 19b. Although these environments are quiteorthogonal, it has been shown (Section 4) that this approach also obtains good results in notso orthogonal environments. The relative formulation is extremely interesting as areference, but it hasn’t been necessary for building maps for these applications.

6 Conclusions

Inconsistency is a structural problem of SLAM-EKF that makes it useless for indoorenvironments under the standard formulation, as it has been described in Section 2. Severalsolutions have been described in Section 3 that can reduce the linearization errors thatproduce the inconsistency. Some of them have been implemented for indoor environments,using the SPMap concepts to deal with the features model, and experimented in real largeenvironments with great success. The presented results are, up to our knowledge, the largestmaps built with a monolithic SLAM-EKF algorithm in indoor environments with featurescontaining angular information, both with the shape constraints solution and with the tree oftransformations.

It has also been proven that the shape constraints solution is very convenient for realapplications, enabling real time map building for Urbano tour guide robot [26] (even withteleoperated exploration as in [31]) and for Guido [25] robot for the frail blinded elderly.

Our current and future research includes developing more complex models for indoorenvironments with curved walls using B-Splines, as well as improved data association andnon-linear minimization algorithms for robust loop closing, under the local maps approachto handle the computational cost. We are also working on extending our algorithms for 3Ddata.

Acknowledgement This work is funded by the Spanish Ministry of Science and Technology (ROBINTproject DPI-2004-07907-C02) and supervised by CACSA whose kindness we gratefully acknowledge. Theauthors would specially like to thank W. Burgard and D. Haennel for the Belgioioso Castle raw data and L.Moreno for the Carlos III triangular building raw data. D. Rodriguez-Losada specially thanks G.Lacey andHaptica Ltd. for hosting his research visit and providing all material and human means to work with Guido.

References

1. Thrun, S.: Robotic mapping: A survey. In: Lakemeyer, G., Nebel, B. (eds.) Exploring ArtificialIntelligence in the New Millenium. Morgan Kaufmann (2002)

2. Elfes, A.: Sonar-based real-world mapping and navigation. IEEE J. Robot. Autom. 3(3), 249–265 (1987)3. Montemerlo, M., Thrun, S., Koller, D., Wegbreit, B.: FastSLAM: A factored solution to the simultaneous

localization and mapping problem. AAAI National Conference on Artificial Intelligence. Edmonton,Canada (2002)

4. Thrun, S., Fox, D., Burgard, W.: A probabilistic approach to concurrent mapping and localization formobile robots. Mach. Learn. 31, 29–53 (1998). Also in Auton. Robots 5, 253–271.

5. Burgard, W., Fox, D., Jans, H., Matenar, C., Thrun, S.: Sonar-based mapping of large scale mobile robotenvironments using EM. Proceedings of the International Conference on Machine Learning (ICML’99).Bled, Slovenia (1999)

6. Gutmann, J.S., Konolige, K.: Incremental mapping of large cyclic environments. IEEE InternationalSymposium on Computational Intelligence in Robotics and Automation (CIRA), pp. 318–325.Monterey, California, USA (2000)


7. Choset, H., Nagatani, K.: Topological Simultaneous Localization and Mapping (SLAM): Toward exactlocalization without explicit localization. IEEE Trans. Robot. Autom. 17(2) (2001)

8. Smith, R., Self, M., Cheeseman, P.: Estimating uncertain spatial relationships in robotics. In: Lemmer,J.F., Kanal, L.N. (eds.) Uncertainty in Artificial Intelligence 2. Elsevier, New York (1988)

9. Kalman, R.E.: A new approach to linear filtering and prediction problems. Trans. ASME, J. Basic Eng.82, 34–45 (1960)

10. Dissanayake, G., Durrant-Whyte, H., Bailey, T.: A computationally efficient solution to the simultaneouslocalisation and map building (SLAM) problem. IEEE International Conference on Robotics andAutomation. Workshop W4: Mobile Robot Navigation and Mapping, pp. 1009–1014. San Francisco,USA (2000)

11. Leonard, J.J., Feder, H.J.S.: A computationally efficient method for large-scale concurrent mapping andlocalization. Ninth Int.Symposium on Robotics Research. Salt Lake City, Utah(1999)

12. Guivant, J., Nebot, E.: Optimization of the simultaneous localization and map-building algorithm forreal-time implementation. IEEE Trans. Robot. Autom. 17(3), 242–257 (2001)

13. Tardos, J.D., Neira, J., Newman, P., Leonard, J.J.: Robust mapping and localization in indoorenvironment using sonar data. Int. J. Rob. Res. 21(4), 311–330 (2002)

14. Williams, S.: Efficient solutions to autonomous mapping and navigation problems. PhD. Thesis.Australian Center for Field Robotics, University of Sidney (2001)

15. Knight, J., Davison, A., Reid, I.: Towards constant time SLAM using postponement. IEEE/RSJInternational Conference on Intelligent Robots and Systems, vol. 1, pp. 405–413. Hawaii, USA (2002)

16. Thrun, S., Liu, Y., Koller, D., Andrew, Y.N., Ghahramani, Z., Durrant-Whyte, H.: Simultaneouslocalization and mapping with sparse extended information filters. Int. J. Rob. Res. 23(7), 693–716(2004)

17. Csorba, M.: Simultaneous localisation and map building. PhD. Thesis. Robotics Research GroupDepartment of Engineering Science University of Oxford (1997)

18. Newman, P.: On the structure and solution of the simultaneous localisation and map building problem.PhD. Thesis. Australian Center for Field Robotics, University of Sidney (1999)

19. Dissanayake, G., Newman, P., Clark, S., Durrant-Whyte, H., Csorba, M.: A solution to the simultaneouslocalisation and map building (SLAM) problem. IEEE Trans. Robot. Autom. 17(3), 229–241 (2001)

20. Julier, S., Uhlmann, J.K.: A counter example to the theory of simultaneous localization and mapbuilding. IEEE Int. Conference on Robotics and Automation, vol. 4, pp. 4238–4243. Seoul, Korea(2001)

21. Castellanos, J.A., Neira, J., Tardos, J.D.: Limits to the consistency of EKF-based SLAM. Fifth IFACSymposium on Intelligent Autonomous Vehicles IAV’04. Lisbon, Portugal (2004)

22. Bailey, T., Nieto, J., Guivant, J., Stevens, M., Nebot, E.: Consistency of the EKF-SLAM Algorithm.IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2006, pp. 3562–3568(2006)

23. Martinez-Cantin, R., Castellanos, J.A.: Bounding Uncertainty in EKF-SLAM: The Robocentric LocalApproach. In IEEE International Conference on Robotics and Automation (ICRA), 2006, pp. 430–435(2006)

24. Huang, Shoudong, Dissanayake, G.: Convergence analysis for extended Kalman filter based SLAM. InIEEE International Conference on Robotics and Automation (ICRA’06), pp. 412–417 (2006)

25. Rodriguez-Losada, D., Matia, F., Jimenez, A., Galan, R., Lacey, G.: Implementing Map BasedNavigation in Guido, The Robotic SmartWalker. IEEE ICRA ’05, pp. 3401–3406. Barcelona, Spain

26. Rodriguez-Losada, D., Matia, F., Galan, R.: Building geometric feature based maps for indoor servicerobots. Elsevier: Robot. Auton. Syst. 54(7), 546–558 (2006) (31 July)

27. Rodriguez-Losada, D., Matia, F., Jimenez, A., Galan, R.: Consistency Improvement for SLAM – EKFfor Indoor Environments. Robotics and Automation, 2006 Proceedings 2006 IEEE InternationalConference on. ICRA’06, pp. 418–423 (2006)

28. Ayache, N., Faugeras, O.D.: Maintaining representations of the environment of a mobile robot. IEEETrans. Robot. Autom. 5(6) (1989)

29. Castellanos, J.A., Montiel, J.M.M., Neira, J., Tardos, J.D.: The SPmap: A probabilistic framework forsimultaneous localization and map building. IEEE Trans. Robot. Autom. 15(5), 948–953 (1999)

30. Bar-Shalom, Y., Li, X.R., Kirubarajan, T.: Estimation with Applications to Trackking and Navigation.Wiley InterScience (2001)

31. Newman, P., Leonard, J., Tardos, J.D., Neira, J.: Explore and Return: Experimental Validation of Real-time Concurrent Mapping and Localization. IEEE ICRA’02, pp. 1802–1809. Washington DC, USA(2002)


32. Neira, J., Tardos, J.D.: Data association in stochastic mapping using the joint compatibility test. IEEETrans. Robot. Autom. 17(6), 890–897 (2001)

33. Rodriguez-Losada, D., Matia, F., Jimenez, A., Galan, R.: Local map fusion for real-time indoorsimultaneous localization and mapping. Journal of Field Robotics 23(5), 291–309 (2006) (May, WileyInterscience)

34. Castellanos, J.A., Martinez-Cantin, R., Tardós, J.D., Neira, J.: Robocentric map joining: Improving theconsistency of EKF-SLAM. Robot. Auton. Syst. 55, 21–29 (2007)


Consistency of SLAM-EKF Algorithms for Indoor Environments

Documents