Robust Landmark Estimation for SLAM in Dynamic OutdoorEnvironment

Atsushi SAKAI, Teppei SAITOH and Yoji KURODAMeiji University, Department of Mechanical Engineering,1-1-1 Higashimita, Tama-ku, Kawasaki, Kanagawa, Japan

Email: ee53063@isc.meiji.ac.jp

Abstract— In this paper, we propose techniques which makeSLAM accurate and practical in dynamic outdoor environment.In order to achieve the objective, stable feature detection bya laser range finder and efficient data management are intro-duced. The stable feature detection is used to select static andcharacteristic landmarks, it is possible to estimate every positionof landmark accurately in the dynamic environment. The datamanagement is introduced in order to decrease computationaltime and localization error of SLAM. Furthermore, for esti-mating landmark and robot’s position accurately, unscentedtransformation based sampling technique is also adopted. As aresult, it is shown that the computing time and the maximumposition error of SLAM decreases by 80% compared withtypical landmark estimation, and position error of SLAM withunscented transformation becomes more accurate comparedwith FastSLAM2.0 in the dynamic outdoor environment.

Index Terms— FastSLAM, Landmark estimation, Data asso-ciation, Unscented Transformation.

I. INTRODUCTION

IN recent years, NASA’s Mars Exploration Rovers (MERs)carried out various scientific discoveries in the surface of

the Mars [1]. In the future, robots will operate in various en-vironments, not only other planets but also such as undersea,underground, and so on.

An important problem at the time of a exploration bya robot is estimation of localization and map. In the en-vironment (e.g. other planets,underground), it is difficult toacquire globally accurate position (e.g. GPS) and informationof ambient surrounding. In order for a robot to operateautonomously in the environment, it is required that technol-ogys of the estimation of robot’s position and map accurately.

The Simultaneous Localization and Mapping, also knownas SLAM, has gotten a lot of attention in the mobile roboticsliterature [2]. The problem of SLAM is building a map of anenvironment from landmark measurements obtained from amoving robot. But a robot motion involves error, the mappingproblem must induce a robot localization problem. To solvethis problems, various solutions to the SLAM problem havebeen studied, such as using the Extended Kalman Filter(EKF), the information filter, and the Graph SLAM [3][4][5].

In these algorithms, FastSLAM algorithm is one of thealgorithms which succeed at doing accurate SLAM inoutdoor environments[6]. The FastSLAM algorithm is asolution to stochastic SLAM that is based on a particlefilter to approximate the ideal recursive the Bayesian filter.FastSLAM algorithm has two advantages: First, the time-complexity of SLAM is small;hence the appelation ”Fast”.

Because FastSLAM is separated the mapping problem andthe localization problem. Second, FastSLAM algorithm isrobust to failure of data association. If observations are asso-ciated correctly in some particles and incorrectly the others,the incorrect particles will receive lower importance weightand being removed in future re-sampling step. Because ofthese advantages, FastSLAM made precise and robust SLAMpossible. Besides, in order to do SLAM with more accurate,the new algorithm FastSLAM2.0 was proposed as developedalogorithm of FastSLAM [7]. In FastSLAM2.0, a proposaldistribution relies not only on the motion estimation (as isthe case in FastSLAM), but also on the most recent sensormeasurement when proposing a new robot pose. The tech-nique enable to estimate the true distribution. Alternatively,because the sampling accuracy is improved by this technique,the SLAM can be calculated with a few particles. The time-complexity of SLAM is more small than using FastSLAM.FastSLAM2.0 can do SLAM with high accuracy and high-speed.

However, using FastSLAM algorithm, it is difficult toexcute SLAM with high accuracy and high speed in theenvironment where complicated like outdoor environmentand have two or more moving objects. For example, fail-ures of data association increase in outdoor environmentcompared with indoor environment. This is attributed tospurious landmark from sensor noise and dynamic objects.For this reason, error of localization and map increase, thenthe calculation of SLAM may diverge. Besides, it is knownthat calculation time of FastSLAM increase logarithmicallyobserved landmarks increase [6]. The more a robot operatea long distance, the more the calculation cost of SLAMincrease. In consequence, the SLAM can not be done onthe fly. For these reason, with holding only static andcharacteristic landmarks on a map, failure of data associationmust be prevented and the calculation time must be reduced.

Moreover, Localization and mapping are the same prob-lems in SLAM. In order to create a more accurate map,we have to estimate robot’s position accurately. The particlefilter in the FastSLAM relies on the importance sampling.Therefore, the FastSLAM requires the design of proposal dis-tributions that can approximate the ture posterior reasonablywell. FastSLAM2.0 algorithm samples using the formulaof EKF so that particles are sampled with high accurately.Although, there are problems in it: The first is that, in EKF,a nonlinear model is approximated with Jacobian matrices.

If the nonlinearity of the model is high, the accuracy ofapproximation becomes bad. The second is that we haveto develop the expression for Jacobian matrices of motionand observation models. However, this derivation is difficultwhen each model is complicated.

Uhlmann et al proposed the Unscented filter as an alter-native to the EKF for recursive state estimation [8]. Thisapproach is used to avoid the analytical linerization basedon Taylor series expansion of both the motion and themeasurement model. Merwe et al introduced the UnscentedParticle Filter (UPF) algorithm [9]. This algorithm usesunscented transformation for a proposal distribution in thesampling step of a particle filter. Because of using unscentedtransform, they enabled to do precise estimations in thenonlinear models.

In this paper, we propose a set of methods of landmarkestimation for high-speed and accurately SLAM on the basisof FastSLAM algorithm. To estimate a landmark accuratelyand to decrease the time-complexity of SLAM, stable featuredetection and data management are important. We introducethat stable feature detection with a laser range finder andefficient data management. The feature detection and the datamanagement involved three techniques respectively. Firstly,we propose following methods in the feature detection:

1) Removing dynamic objects and sensor noise withdifference processing

2) Detecting feature points sparsely and evenly3) Adjusting a threshold value of data association with a

landmark densityTaking in these techniques, we show that detecting featurepoints becomes stable, then these feature points make easy todo data association. Secondly, we also propose these methodsin the data management:

1) Calculating landmark’s existence probability2) Using the landmark exclusivity for data association3) Predicting importance weights with observation range

Because of these techniques, we show that the computationaltime and a position error are decreased and data associationbecomes robust.

Furthermore, we take in a sampling method using un-scented transformation. Because of introducing it and thelandmark estimation we proposed, we show that the ourSLAM is more accurate than FastSLAM2.0 algorithm.

II. FASTSLAM ALGORITHM

SLAM is the problem of determining robot poses xt

and the position of all landmarks ! from measurementszt = z1, · · · · · · , zt and robot’s controls ut = u1, · · · · · · , ut.In probabilistic terms, this is expressed by the posteriorp(xt, ! | zt, ut), where we use the superscript to refer to aset of variables from time 1 to time t. If the correspondencesof landmark each time are known, the SLAM posterior is :

p(xt, ! | zt, ut, nt) (1)

where nt is the correspondence variables.

FastSLAM algorithm divides the problem of localizationand mapping, and calculates each probability. The posterior(1) be factored as follows:

p(xt, ! |zt, ut, nt)

= p(xt|zt, ut, nt)N!

k=1

p(!k |xt, ut, zt, nt)(2)

where k is a number of each landmarks, N is a total oflandmarks. The FastSLAM algorithm implements the robot’sposition estimator p(xt|zt, ut, nt) using a particle filter,and each landmark position estimators p(!k |xt, ut, zt, nt)using low dimension Kalman filters. Dividing problems oflocalization and mapping, the FastSLAM algorithm makesdecreasing the time-complexity possible. Furthermore, eachparticle in the particle filter has its own local map, FastSLAMis robust to a failure of data association.

FastSLAM2.0 was proposed as development algorithm ofFastSLAM. In FastSLAM2.0 algorithm, robot’s poses aresampled under a consideration of both the motion ut and themeasurement zt. The i-th particle is sampled by a followingprobability expression:

x[i]t ! p(xt|x[i]

t , zt, ut, ![i]) (3)

where x[i]t is the variable which is the i-th particle’s state,

![k] is the variable that the i-th particle has the landmarkmap. The accuracy of localization improved by the use ofthis technique. Because of it, the number of particles couldbe held down to a small number, and the time-complexity ofFastSLAM2.0 is small than FastSLAM.

III. LANDMARK ESTIMATION

In landmark based SLAM which uses LRF, the feature de-tection and the data association are very important problems.In this paper, we propose a set of techniques to solve theseproblems effectively.

A. Feature detectionIn FastSLAM, a feature point are served as a candidate

of a landmark. A camera can detect feature points using acolor distribution of a picture. However, a LRF can detecta feature point only using distance information between arobot and obstacles. This problem makes data associationwith LRF adversity. In this paper, we use the extremum ofthe observation value of LRF as feature points. This sectionproposes three techniques of the feature detection for doingit stably.

1) Removing dynamic objects and sensor noise with dif-ference processing: For accurate SLAM, it is necessary toremove dynamic objects and sensor noise from a measure-ment data of LRF. If they are reflected into a map, the time-complexity of SLAM and an error of map estimation wouldincrease.

We presents a technique of removing dynamic objects andsensor noise to piling up the LRF measurement data for twotimes. With a curent observation value and a previous, eachobservations on same angle are assciated. Then, a distance

between the couple of accosiated observations is calculated.If the distance longer than a threshold distance determinedin advance, it is removed. Because of this method, dynamicobjects and sensor noises are removed.

2) Detecting feature points sparsely and evenly: We useMaximum Likelihood Estimation (MLE) for data associ-ation [10]. MLE selects data associations by maximizinga importance weight of each landmarks. The importanceweight is calculated from the Mahalanobis distance betweena observation and a landmark, and the function of Gaussiansdistribution:

"j = |2"Qj |!12 exp("1

2(zt " zj)tQ!1

j (zt " zj)) (4)

where, the variable "j is importance weight, Qj is covarianceof landmark position, zj is predicted observation value, andsuperscript j is a index of landmarks. Importance weightsare calculated by (4) with respect to each observation. Thelandmark which has the most biggest importance weight inall landmarks is associated the observation. If all importanceweights to all landmarks are below a threshold determined inadvance, it is judged that the feature point is a new landmark.

MLE associates individually all landmarks on each featurepoint. Therefore, it is one of the robust data associationmethods in the outdoor environment where a dynamic objectexists. However, because data association of MLE usesMahalanobis distance between each landmarks and a obser-vation value, it tends to go wrong assciation where landmarksare dense.

We propose that it makes a distance of feature pointsalready detected into the conditions on the feature detection.If the distance between a observation and the most nearestfeature is longer than the threshold value, the observation isnot selected as the feature point. Put simply, in a adjacentplace where a lot of feature are detected, it makes featuresdetected sparsely. In a adjacent place where a few featuredetected, it makes features detected thickly. Because of this,feature points are detected sparsely and evenly, a failure ofdata association decreases.

3) Adjusting a threshold value of data association witha landmark density: In data association using the distancebetween a observation and a landmark, it is necessary toadjust a threshold of data association with a landmarkdensity. For example, when distance is long between onelandmark and another landmark which is the nearest it, onlythe object exists around there. That is, the landmark shouldbe associated even if some observation errors arise. On thecontrary, in the place where landmarks exists closely, thefailure of data association may be occured. In the situationlike thies, the data association should be done when theassociation is really absolute.

In this paper, we propose a method that a threshold valueof data association is adjusted with the distance betweentwo adjacent landmarks. The threshold value is calculatedby following formula:

Mth = Mc " K·dmin (5)

where the variable Mc is basic threshold of importanceweight, dmin is the distance between two adjacent land-marks, and K is a constant. The threshold value is adjustedwith respect to each observations.

B. Data ManagementIn this paper, three techniques are used for efficient data

association.1) landmark’s existence probability: Failures of feature

detection resulting from sensor noise or complicated envi-ronment becomes a problem in outdoor environment. There-fore, even if it is used methods of feature detection whichdescribed in the chapter 3-A, it could not prevent completelyspurious features from geting mixed in the map. To solvethe problem, we always have to execute the SLAM withremoving spurious landmarks.

Montemerlo et al calculated the probability of the land-mark’s existence using available r of two values to all thelandmarks that each particles hold [10]:

! = logp(r|zt, x

[k]t )

1 " p(r|zt, x[k]t )

(6)

where the variable ! the probability of the landmark’s exis-tence, it is updated in each observation. When the probabilitybecomes below a threshold, it judges that the landmark isa spurious, and removes it from a map. By this, only atrue landmark could be held on the map, and the numberof handing landmarks is decrease. As a result, the time-complexity becomes small and failure of data associationdecreases.

2) Using the landmark exclusivity for data association:When features are detected sparsely with the techniquementioned in the chapter 3.A.2, it can be figured out thateach feature points are individual objects. Therefore, severalobservations which observed in the same time should not beassociated simultaneously with one landmark.

In this paper, when some observations are identified as onelandmark by MLE, the observation of the highest importanceweight is only associated, and other observations are judgedas a new landmarks. As a result, failure of data associationcould be prevented.

3) Predicting importance weights with observation range:In order to deal with many landmarks on the fly, the time-complexity has to be reduced. However, using MLE fordata association, importance weights are calculated to alllandmarks in a map. The more the number of landmarks ina map increases, the more calculation increases. Especially,it is well known that the calculation time increases logarith-mically in proportion to the number of observed landmarkson FastSLAM algorithm [6]. Hence, the calculation time ofimportance weight is needed to be kept down much further.

We propose that the obsevation range which is determinedpreliminarily is used for decresing the computational time.Observable landmarks at the next step is presumued from theinformation of robot’s position and its observable range. Andthen, it is assumed that landmarks which are out of the rangemay not be observed, the landmark’s importance weights are

−20 0 20 40 60 80 100 120−20

0

20

40

60

80

100

120

X Displacement (m)

Y D

ispla

cem

ent (

m)

Ground TruthSLAMLandmarkEstimated Landmark

(A) Normal landmark estimation

−20 0 20 40 60 80 100 120−20

0

20

40

60

80

100

120

X Displacement (m)

Y D

ispla

cem

ent (

m)

Ground TruthSLAMLandmarkEstimated Landmark

(B) Proposed landmark estimation

Fig. 1. Simulation results of SLAM

not calculated. Thus, the futile calculation of the importanceweights would not be excuted.

IV. SAMPLING USING UNSCENTED TRANSFORMATION

The particle filter in the FastSLAM relies on the im-portance sampling and, as a result, requires the design ofproposal distributions that can approximate the true posteriorreasonably well. In this paper, the sampling technique usingthe unscented transformation which Merwe et al proposed istaken in [8].

The unscented transformation is a method for calculatingthe statistics of a random variable which undergoes a nonlin-ear transformation. It builds on the principle that it is easierto approximate a probability distribution than an arbitrarynonlinear function.

The procedure which is to estimate mean and covariancewith unscented transformation is following: Firstly, weightedsamples, called sigma-points, are deterministically chosen.Secondly, each sigma points is propagated through thenonlinear function. At the end, mean and covariance areestimated with these points. Equation (7) is the formulaof choosing sigma-points, (8) is the formula of calculatingweights to sigma-points.

x(0) = x

x(i) = x + ("

(N + #)P ) i = 1, ....., N (7)x(i) = x " (

"(N + #)P ) i = N + 1, ....., 2N

$(0)m =

#

n + #

$(0)c =

#

n + #+ (1 " % + &) (8)

$(i)m = $(i)

c =1

2(n + #)i = 1, ....., 2N

where x is the mean of robot’s position on previous step,# = %2(n + ') " n, n is state dimension, %,' are scalingparameter of choosing sigma-points, and & is a parameterwhich minimizes the effects from high order terms.

Comparing with EKF, the estimation with the unscentedtransformation does not need to develop the expressionfor Jacobian matrixes of models. This is a big advantagewhen using a complicated model. Additionally, as provedin the previous researches, the estimation of the mean andcovariance are accurate to the third order of the Taylorseries expansion of any nonlinear function, whereas the EKFestimation has a first order [13].

V. SIMULATION RESULTS

SLAM simulations was performed, which used our land-mark estimation . These simulations was done by MATLAB,the algorithm of SLAM was FastSLAM2.0 algorithm. MLEwas adopted for data association. This simulator makes thenoise of observation and input with respect to Gaussiandistribution.

Two kinds of simulations were done, which the one usedthe normal landmark estimation, the another took in ourlandmark estimation method. Same parameters(e.g. noise,robot’s speed, etc) were used in both simulation.

Fig. 1 shows the results of these simulations. (A) is resultof the simulation using the normal landmark estimation, (B)is result of simulation using our landmark estimation. Thesefigures show estimated trajectory of SLAM, Ground truth,true landmark position, and landmark position.

Fig. 2 shows the position error of these simulations as afunction of time. The figure indicates that our method allowsmaximum error of position to decrease by 80%. Becauseour methods makes possible to hold only true landmarks sothat spurious landmarks were removed. As a result, the truelandmark is updated frequently, the position of the landmarkis converged in the true position. Meanwhile, in normalestimation, because of not to remove spurious landmarks,error of the map increase as the failure of data association.Table I compares the number of landmarks at the end of thesimulation and the computational time. These simulationswas done by the system that the CPU is Intel Core2 Duo2.33GHz, the RAM is 3.25GB. Our method removed the

Fig. 2. Position Error

TABLE ICOMPUTATIONAL TIME

Number of Landmark Computational TimeNormal Estimation 627 643

Proposed Estimation 213 109

spurious landmark, so that the number of landmarks treatedwas reduced by more than 60%. As a result, the computa-tional time was reduced by more than 80%.

VI. RESULTS OF SLAM IN OUTDOOR ENVIRONMENT

We simulated feature detection and SLAM with a set ofsensor data that were taken in real outdoor environment.The experimental field were Tsukuba central park, Ibaraki.There were a number of other team ’s robots and manypassers in this driving course (Fig. 4). This experimental fieldhad objects static and dynamic objects. Additionally, Thisenvironment had many objects that are complicated shapes(e.g. grass, shrubbery, bicycle, building). Infant, which is oursystem for research of autonomic mobile robot (Fig. 3) andTable II is it’s specification, was used for getting the sensordata. The experiments are performed with the LRF of TOP-URG UTM-X001S made in Hokuyo denki co, the sensorsamples data at 10 Hz, and the robot was traveling at about0.5 m/s.

A. Feature detection

Fig. 5 shows detecting feature points with our methodsstated in chapter 3-A. Fig. 5 also shows robot’s position andLRF observations. This shows these features were detectedsparsely. In this simulation, the threshold of the least distancebetween a observation and the most nearest feature were10cm. As a result, feature points were detected with themore distance than 10cm each other. Moreover, Fig. 5shows dynamic objects are not detected with the differenceprocessing stated chapter in 3.A.1. These features were usedfor the our SLAM.

B. Results of SLAM

We simulated two SLAM algorithms, which the one wasFastSLAM2.0 with our landmark estimation, the anotherwas the one innovated the unscented transformation. These

Fig. 3. Infant

Fig. 4. Experiment field

TABLE IISPECIFICATION OF INFANT

Parameters [Unit] ValueLength [m] 0.9Width [m] 0.6Height [m] 1.1Tread [m] 0.5

Wheel base [m] 0.45Climbable angle [m] 4.9

Weight [m] 60Radius of wheel [m] 0.125

Maximum speed [km/h] 7Range of continuous drive [km] 20

−5 0 5 10 15 20 25−10

−5

0

5

10

15

X Displacement (m)

Y D

ispla

cem

ent (

m)

Robot PositionLRF ObservationsFeature points

Dynamic Objects

Fig. 5. Feature detection

0 20 40 60 80 100 120 140 160−40

−20

0

20

40

60

80

X Displacement (m)

Y D

ispla

cem

ent (

m)

Ground TruthFastSLAM2.0OdometryEstimated Landmark

(A) FastSLAM2.0

0 20 40 60 80 100 120 140 160

−20

0

20

40

60

80

X Displacement (m)

Y D

ispla

cem

ent (

m)

Ground TruthUnscented SLAMOdometryEstimated Landmark

(B) Unscented FastSLAM

Fig. 6. Results of SLAM in dynamic outdoor environment

SLAM simulations only used sensor data of wheel odometryand LRF observations.

Fig. 6 is results of SLAM simulations. (A) is the result ofSLAM with FastSLAM2.0, (B) is the result of our SLAMwith the sampling method of unscented transformation.These figures show the trajectory of wheel odometry, SLAM,Ground truth, and the feature map created with SLAM. Bothresults shows that the estimation of robot’s position is moreaccurately than the trajectory of odometry. This is attributedto ourlandmark estimation methods, it enabled to constructmap stably and accurately in dynamic environment.

Finally, the result of SLAM using unscented transforma-tion was compared with the result of SLAM using Fast-SLAM2.0. Fig. 6 shows that the SLAM using unscentedtransformation is more accurate about localization than Fast-SLAM2.0. Because the using unscented transformation ismore accurate than the use of EKF in the aspect of approx-imation accuracy of nonlinear function. For this reason, itwas shown that the accurately of localization was improvedby introducing the unscented transformation.

VII. CONCLUSION

In this paper, we proposed the landmark estimation forhigh-speed and accurate SLAM. In landmark estimation,three techniques were taken in respectively in the categoryof the feature detection and the data management. In thefeature detection, removing dynamic objects and sensornoise with difference processing, detecting feature pointssparsely and evenly, and adjusting a threshold value of dataassociation with a landmark density, were introduced. Byusing these techniques, it made possible to detect featurepoint stably in outdoor environment, we showed that accurateSLAM became possible under the complicated and dynamicenvironment. In the data management, calculating landmark’sexistence probability, using the landmark exclusivity fordata association, and predicting a importance weight withobservation range, are taken in. These techniques made itpossible to decrease the time-complexity of SLAM. We

showed that our method allowed maximum error of positionestimation and computational time to decrease by 80%.

We also proposed the sampling method using unscentedtransformation. With this method and our landmark estima-tion, we showed the our SLAM was more accurate thanFastSLAM2.0.

REFERENCES

[1] Li, R., K. Di, L.H. Matthies, R.E. Arvidson, W.M. Folkner and B.A.Archinal.“Rover Localization and Landing Site Mapping Technologyfor 2003 Mars Exploration Rover Mission.” Journal of Photogram-metric Engineering and Remote Sensing, Vol.70(1): 77-90, 2003.

[2] H. Durrant-Whyte and T. Bailey,“ Simultaneous Localisation andMapping (SLAM): Part I The Essential Algorithms,”Robotics andAutomation Magazine, vol. 13, pp. 99?110, 2006.

[3] G. Dissanayake, P. Newman, S. Clark, H.F. Durrant-Whyte,and M.Csorba. A solution to the simultaneous localisation and map building(SLAM) problem. IEEE Transactions of Robotics and Automation,2001.

[4] Y. Liu and S. Thrun. Results for outdoor-SLAM using sparse extendedinformation filters. Submitted to ICRA-03.

[5] S. Thrun and M. Montemerlo, “ The graphslam algorithm withapplications to large-scale mapping of urban structures,” IJRR, vol.25.

[6] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. Fast-SLAM:A factored solution to the simultaneous localization andmappingproblem. Proc. AAAI, 2002.

[7] M. Montemerlo, S. Thrun D. Koller, and B. Wegbreit. FastSLAM 2.0:An improved particle filtering algorithm for simultaneous localizationand mapping that provably converges. In Proc. of the Int. Conf. onArticial Intelligence (IJCAI), 2003.

[8] S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte, “ A newapproach for filtering nonlinear systems,”in Proc. Amer. Contr. Conf.,Seattle,WA, June 1995, pp. 1628?1632.

[9] R. Merwe, A. Doucet, N. Freitas and E. Wan,“The Unscented ParticleFilter”, Technical Report CUED/F-INFENG/TR 380, CambridgeUniversity Engineering Department, 2000.

[10] Avitzour, D. (1992) A maximum likelihood approach to data associa-tion.IEEE Transactions on Aerospace and Electronics Systems, 28, 2(Apr. 1992), 560?565.

[11] M.Montemerlo,S.Thrun,Simultaneous localization and mapping withunknown data association using FastSLAM.Proc. ICRA, 2003.

[12] S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics, MIT Press,2005.

[13] C.Kim, R.Sakthivel, and W.K.Chung, Unscented FastSLAM: A robustalgorithm for the simultaneous localization and mapping problem, inProc. IEEE Int. Conf. Robot. Autom., 2007, pp. 2439-2445.