Simultaneous Localization and Odometry Calibration for MobileRobot

Agostino Martinelli, Nicola Tomatis, Adriana Tapus and Roland Siegwart

Autonomous Systems Lab Swiss Federal Institute of Technology Lausanne (EPFL)EPFL, CH-1015 Lausanne, Switzerland e-mail: agostino.martinelli, nicola.tomatis,

adriana.tapus, roland.siegwart@epfl.ch∗

Abstract

This paper presents both the theory and the firstexperimental results of a new method which allowssimultaneously estimating of the robot configura-tion and the odometry error (both systematic andnon-systematic) during the mobile robot navigation.The estimation of the non-systematic componentsis carried out through an augmented Kalman filterwhich estimates a state containing the robot config-uration and the parameters characterizing the sys-tematic component of the odometry error. It usesencoder readings as inputs and the readings from alaser range finder as observations. The estimation ofthe non-systematic component is carried out throughanother Kalman filter where the observations are ob-tained by two subsequent robot configurations pro-vided by the previous augmented Kalman filter.

Key Words: Robot Navigation, Kalman filter,Odometry Learning

1 Introduction

Determining the odometry errors of a mobile robotis very important both in order to reduce them, andto know the accuracy of the state configuration esti-mated by using encoder data.

Odometry errors can be both systematic and non-systematic. In a series of papers Borenstein and col-laborators [3, 4, 5, 6, 7, 8, 19] investigated on pos-sible sources of both kind of errors. A review of allthe types of these sources is given in [8]. In the workby Borenstein and Feng [7], a calibration techniquecalled UMBmark test has been developed to calibratefor systematic errors of a mobile robot with a dif-ferential drive. Larsen et al. [11, 12] suggested analgorithm that uses the robot’s sensors to automati-cally calibrate the robot as it operates. In particular,

∗This work has been supported by the European projectRECSYS (Real-Time Embedded Control of Mobile Systemswith Distributed Sensing)

they introduced an augmented Kalman filter (AKF )which simultaneously estimates the robot configura-tion and the parameters characterizing the system-atic odometry error. This filter uses encoder readingsas inputs and vision measurements as observations.They referred to a mobile robot with a differentialdrive system.

Many investigations have been carried out on theodometry error from a theoretical point of view.Wang [18] and Chong and Kleeman [9] analyzed thenon-systematic errors and computed the odometrycovariance matrix Q for special kind of the robottrajectory. Kelly [10] presented the general solutionfor linearized systematic error propagation for anytrajectory and any error model. Martinelli [14] de-rived general formulas for the covariance matrix andalso suggested a strategy to estimate the model pa-rameters for a mobile robot with a synchronous drivesystem. This strategy is based on the evaluation ofthe mean values of some quantities (called observ-ables) which depend on the model parameters andon the chosen robot motion.

In a recent work Martinelli and Siegwart [16] sug-gested a method to estimate both systematic andnon-systematic odometry error of a mobile robot,during navigation. Concerning the systematic com-ponent, they adopted the same AKF introducedby Larsen et al. [11, 12] by considering also thecase of a synchronous drive. Concerning the non-systematic parameters, they introduced a new filter(the Observable Filter, OF ) where the state to be es-timated contains the parameters characterizing thenon-systematic error and the observations are pro-vided by the observables as defined in [14] and whichcan be evaluated by knowing two subsequent robotconfigurations.

In this paper the new method [16] is empiricallyvalidated by experimentation on a real differentialdrive robot. In section 2 we introduce the modelhere adopted to characterize the odometry error for

Proceedings of the 2003 IEEE/RSJIntl. Conference on Intelligent Robots and SystemsLas Vegas, Nevada · October 2003

0-7803-7860-1/03/$17.00 © 2003 IEEE 1499

a mobile robot with a differential drive. In section3 we summarize the AKF introduced by Larsen etal. [11, 12]. The OF is presented in section 4 anddiscussed for the specific case implemented in theexperiments. In particular the influence on the ac-curacy on the non-systematic parameter estimationsdue to the error on the systematic error evaluationis deeply investigated. In section 5 we show and dis-cuss the experimental results obtained with the fullyautonomous robot Donald Duck. Finally, some con-clusions are given in section 6

2 The odometry error model

A simple way to characterize the odometry error fora mobile robot with a differential drive system is ob-tained by modeling separately the error in the trans-lation of each wheel [9]. The actual translation ofthe right/left wheel related to the ith time step isassumed to be a gaussian random variable satisfyingthe following relation:

δρR/Li = δρ

R/L

i + νR/Li (1)

δρR/L

i = δρeR/Li δR/L (2)

νR/Li ∼ N(0, Kw|δρeR/L

i |) (3)

In other words, both δρRi and δρL

i are assumed gaus-sian random variables, whose mean values are givenby the encoder readings (respectively δρeR

i and δρeLi )

corrected for the systematic errors (which are as-sumed to increase linearly with the distance traveledby each wheel), and whose variances also increaselinearly with the traveled distance. Moreover, it isassumed that δρR

i and δρLi are uncorrelated. With

respect to the Chong-Kleeman model, only one pa-rameter (Kw) is here adopted to characterize boththe variances for the right and left wheel. The robottranslation and rotation are given by the followingrelations:

δρi =δρR

i + δρRi

2δθi =

δρRi − δρL

i

dδd(4)

where d is the estimated distance between the wheelsand δd characterizes the uncertainty on this estima-tion. Clearly, the robot translation and rotation arecorrelated accordingly to the equations (1-4). Theodometry error model here proposed is based on 4parameters. Three ( δR, δL and δd) characterize thesystematic components whereas the last one (Kw)characterizes the non-systematic components.

In section 3 and 4 we introduce the strategy to si-multaneously estimate all these parameters duringthe robot navigation.

3 Systematic Parameters Estimationduring Navigation

In order to estimate the parameters characterizingthe systematic error we adopt the same AKF in-troduced by Larsen et al. [11, 12]. This filter esti-mates a state (the augmented state) containing therobot configuration and the systematic parameters,through an extended Kalman filter (EKF ).

Let be X the robot configuration (X = [x, y, θ]T )and Xa the augmented state. We have

Xa = [x, y, θ, δR, δL, δd]T (5)

The state X evolves accordingly to the dynamicalequation Xi+1 = f(Xi, Ui) where Ui = [δρR

i , δρLi ]T .

The observation at the ith time step depends on thecurrent robot configuration and it is assumed to beaffected by an error wi with a gaussian distribution,zero-mean and covariance matrix Ri =< wiw

Ti >

zi = h(Xi, wi) (6)

A simple example for this function is obtained bydefine z as the vector containing all the distancesin several directions of observation from the robotconfiguration towards the landmarks (e.g. straightlines stored in a map a priori known). In this case thefunction h characterizes the measurement predictionof a laser range finder. In the experiments carried outin our lab and discussed in section 5, this functionwas not the previous one since, instead of using theraw data, we extracted the straight lines from thedata (see also [1]).

The dynamical equation for the augmented state Xa

is given by the equation:

Xai+1 = fa(Xai, Ui) (7)

The function fa, restrictly to the first three compo-nents, is obtained directly from the function f includ-ing the dependence on the systematic parameters inthe input Ui; concerning the last three componentsfa is the identity function since there is no evolutionin time for the error parameters.

In order to obtain the EKF equations for the aug-mented state (i.e. the equations of the AKF ), it isnecessary to compute the Jacobian Fa of the func-tion fa with respect to Xa and the Jacobian Ga ofthe function fa with respect to the vector ν, whichis [νR, νL]T (eq. (3)):

1500

Fa = ∇Xafa|Xa(i|i),UiGa = ∇νfa|Xa(i|i),Ui

where Xa(i|i) is the state estimated at the previoustime step and U i is the mean value of the vector Ui

previously defined. The computation of these matrixcan be found in [11, 12].

4 Non-Systematic Parameters Estima-tion during navigation

The non-systematic parameter Kw cannot be evalu-ated following the previous method. Indeed, by in-cluding Kw in the augmented state, the Kalman gainrelated to this parameter is null.

The OF suggested in [16] and here adopted is basedon the observables defined in [14]. The observablesare random variables related to a given robot motionwhose statistical properties (mean value and vari-ance) depend on the parameters characterizing theodometry error and on the robot trajectory in theodometry reference frame. Moreover, it is possibleto evaluate the observable mean value only by know-ing the actual initial and final configuration. Theseactual configurations are directly estimated by theAKF of above. The OF is an EKF whose esti-mated state contains the non-systematic parameters(in this case only the parameter Kw). Since the en-vironment is assumed to be homogeneous and sta-tionary the dynamical equation of this filter ([2, 13])is the identity:

Kwi+j = fK(Kwi) = Kwi (8)

where we use i + j instead of i + 1 to remark thatthe frequency of this second filter is not necessarilythe same of the previous one (AKF ).

The observational equation is the following:

zObsi+j = mObs(Kwi+j) + wObs

i+j (9)

where zObs is the observable mean value as estimatedby the AKF , i.e. by knowing the robot configurationat the time step ith and (i + j)th, mObs(Kw) is themean value of the observable analytically computedby knowing the trajectory in the odometry frame,and wObs is a zero-mean random variable whose co-variance matrix contains both the covariance matrixof the observable and the error on the robot config-uration and on the systematic parameter estimation(given by the matrix Pa(i|i) and Pa(i + j|i + j)),since the observable mean value is estimated fromtwo subsequent robot configuration estimations ob-tained from the AKF and these estimations are af-fected by an error given by the matrix Pa. Observe

that zObs is an estimation of the observable meanvalue obtained only through one realization of therobot motion (since only one realization is obviouslyavailable). For this reason it is very important to in-clude in the covariance matrix of wObs the covariancematrix of the observable.

Let ∆θa and ∆θo be the robot orientation change be-tween the time i+ j and i, respectively estimated bythe AKF and the odometry corrected for the sys-tematic errors by using the systematic parametersestimated by the AKF at the (i + j)th. Finally, let∆θ be the actual orientation change. The observablewe adopt here (which is the same adopted in [16] forthe differential drive) is:

zObs = (∆θa − ∆θo)2 (10)

On the basis of the odometry error model introducedin section 2 we obtain for the mean value and vari-ance of this observable [17](we neglect the covariancebetween the error on the robot orientation obtainedby the AKF and the error on the orientation ob-tained with the odometry):

mObs(Kw) =KwS

b2+ σ2

θ + χ2 (11)

σ2Obs = 2× (12)

×[σ4

θ +K2

wS2

b4+ 2χ2

(σ2

θ +KwS

b2

)+ 2σ2

θ

KwS

b2

]

where:

• S = sR + sL, and sR and sL are the distancestraveled respectively by the right and left wheel,between the time i and i+ j as estimated by theodometry (i.e. sR/L =

∑i+jk=i

∣∣∣δρeR/Lk

∣∣∣)• b is the actual distance between the wheel,

namely b = δtdd, where we denote with δt

R, δtL

and δtd the actual (unknown) values of the sys-

tematic parameters

• σ2θ is the variance related to the robot ori-

entation estimated by the AKF , i.e. σ2θ =

Pa(i|i)[3, 3] + Pa(i + j|i + j)[3, 3], since the ori-entation is the third element in the augmentedstate (eq.(5))

• χ takes into account the uncertainty on the sys-tematic parameters and is explicitly given by thefollowing expression:

χ = χ(sR, sL) =

=sR(δdδ

tR − δt

dδR) − sL(δdδtL − δt

dδL)bδd

(13)

1501

In order to estimate Kw through the OF it is nec-essary that the first term on the right side in theequation (11) is larger than the other two. In partic-ular, for the term χ2 we do not have any estimationbut we can only compute an over bound, obtaining:

χ2 ≤ ε2

(bδd)2(|sR − sL| + S)2 = χ2

M (14)

where ε is the error on the systematic parameters(nearly the same value for all of them). As we ex-pected, the error on the systematic parameters (χ2)affects the observable mean value with the square ofthe traveled distance. On the other hand, the sec-ond term on the right side of the equation (11) isindependent of the traveled distance. Finally, thefirst term containing Kw, depends linearly on thedistance. Therefore, the best frequency for the OFis fixed by requiring that the linear component is thelargest. Clearly, as showed in Fig 1b this requirementcould not be satisfied (e.g. when the value of Kw isvery small). The value of S0 showed in the figurecorresponds to the S where χ2

M = σ2θ .

(a) (b)

Figure 1: The three components appearing in themean value expression (equation (11)) vs the distancetraveled by the robot between two subsequent OF up-dates. In the case showed in (b) it is not possible toestimate Kw for any S.

In the next section we show the experimental resultsobtained by choosing the value of S0 in order to fixthe frequency of the OF .

5 Results

For the experiments, a fully autonomous mobile ve-hicle has been used. Donald Duck (figure 2) is amobile robot with a differential drive. It is equippedwith wheel encoders, a 360 laser range finder and agrey-level CCD camera (not used here). It is con-nected via radio ethernet only for data visualizationvia web and data logging for statistical purposes.

We performed two set of experiments. In each ex-periment the robot moved along a distance of about300m in our laboratory. The two set of experiments

Figure 2: The autonomous robot Donald Duck. Itscontroller consists of a VME standard backplane witha Motorola PowerPC 604 microprocessor clocked at300 Mhz. Among its peripheral devices, the mostimportant are the wheel encoders, a 360 laser rangefinder and a grey-level CCD camera (not used here).

differed because in one case we added on both therobot wheels a small piece of tape in order to increaseslightly the wheel diameters and to test the accuracyof the implemented AKF . In all the cases the OFstarted to work only when the systematic parametererrors, as estimated by the covariance matrix of theAKF (Pa), were smaller than 5 10−4. This accuracywas always achieved after about 100m.

Concerning the AKF , we set the initial covariancematrix Pa as diagonal. Moreover, the variances cor-responding to the systematic parameters were setequal to (0.05)2 for all the three parameters. Fi-nally, the initial values was fixed equal to 1.0 for allof them.

Regarding the non-systematic parameter Kw, we setin the most of the experiments, the initial value equalto 0.01m. This value is very large. Indeed, it cor-responds to have a non-systematic error whose stan-dard deviation after 100m of navigation, is equal to1m for each wheel. Therefore, the AKF at beginningused nearly only the laser range finder to localize therobot.

Figures 3, 4 and 5 concern the systematic parame-ter results. Dotted line is adopted for the case withthe tape on the wheels. As expected, the values ofδR and δL increase with respect to the case withouttape. The variation is equal to about 0.01 corre-sponding to a diameter change of 0.4mm, since thewheel diameter is equal to 3.8cm. Fig. 5 shows alsoa change in the wheels base distance due to the tape.This change demonstrates that the point where thewheel touches the terrain is pushed out by the tape.

1502

0 50 100 150 200 250 3000.9

0.92

0.94

0.96

0.98

1

1.02

1.04

1.06

1.08

1.1

Figure 3: The δR parameter estimated by the AKFvs the distance traveled by the robot (unity m). Thedotted line refers to the case with the tape on thewheels

0 50 100 150 200 250 3000.9

0.92

0.94

0.96

0.98

1

1.02

1.04

1.06

1.08

1.1

Figure 4: The δL parameter estimated by the AKFvs the distance traveled by the robot (unity m). Thedotted line refers to the case with the tape on thewheels

0 50 100 150 200 250 3000.9

0.92

0.94

0.96

0.98

1

1.02

1.04

1.06

1.08

1.1

Figure 5: The δd parameter estimated by the AKFvs the distance traveled by the robot (unity m). Thedotted line refers to the case with the tape on thewheels

1

2

3

410

0 40 80 120 1600

-4

wK

200

Figure 6: The non-systematic parameter Kw as es-timated by the OF vs the distance traveled by therobot (unity m in both axis). The dotted line refersto the case with the tape on the wheels

Fig. 6 concerns the non-systematic parameter re-sults. Again, dotted line is adopted for the case withthe tape on the wheels. In this case the tape doesnot produce variation. The frequency of the OF waschosen accordingly to the considerations given in sec-tion 4. In particular, by imposing that the secondand third term in the equation (11) are equal, weobtain from equation (14):

S0 � σθb

ε(15)

which corresponds in our case to a value S0 � 15m(the error in the orientation as estimated by theAKF , i.e. the second term in equation (11), is aboutalways σθ � 0.025rad, ε � 5 10−4 and b = 0.3m).Observe that the frequency of the AKF is muchhigher (� 10cm). For this value of S0 we obtaina value of the first term in the equation (11) aboutof ten times larger than the other two. This meansthat we are in the situation showed in fig. 1a. Sim-ilar results for the estimated Kw were obtained bychanging the value of S0. In particular, we did manyexperiments in the range 10m ≤ S0 ≤ 30m obtaininga variation in Kw within the 20%. We also did othertrials by changing the initial value of Kw (always inthe range 0.0001m ≤ Kw ≤ 0.1m) obtaining againa slight variation in the results (within the 20%).The final estimated Kw showed in the fig. 6 areKw = (4.7 ± 1.6)10−5 and Kw = (5.4 ± 1.8)10−5

respectively with and without tape. This valueof Kw corresponds to have a non-systematic error

1503

whose standard deviation after 100m of navigation,is � 5cm for each wheel.

6 Conclusions

A new filter, the Observable filter, was implementedfor the estimation of the non-systematic odometryerror during the robot navigation. This filter is basedon the Observables (introduced in a previous work[14]) which provide the observations for an extendedKalman filter estimating a state containing the pa-rameters characterizing the non-systematic odome-try error. This new filter was used together with theaugmented Kalman filter (introduced by Larsen etal. [11, 12]) enabling the simultaneous estimation ofthe systematic and non-systematic odometry errorduring the robot navigation.

The experimental results show that it is possibleto estimate the systematic error with high accuracy(0.05% by moving the robot for 100m) and the non-systematic error with an accuracy of 30%. Observethat our experiments were carried out in an indoorenvironment with a very smooth floor and thereforethe non-systematic component is very low and verydifficult to be evaluated.

We are performing some experiments showing theusefulness of an odometry autocalibration in theframework of the SLAM problem. We want to re-mark that in the localization problem with a precisemap a priori known, and when a precise externalsensor is available, the localization error is very smallcompared to the odometry error (calibrated or not),since the localization task is nearly completely reliedto the external sensor. In the SLAM problem, how-ever, the odometry could play a very important roleespecially in solving the data association problem.

References

[1] K.O.Arras, N.Tomatis, B.T.Jensen and R.Siegwart,“Multisensor on-the-fly localization: Precision andreliability for applications”, Robotics and Au-tonomous Systems 34, pp. 131–143, 2001.

[2] Y.Bar-Shalom, T.E.Fortmann,, “Tracking and dataassociation, mathematics in science and engineering”,Vol 179, Academic Press, New York, 1988.

[3] Borenstein J., “Internal Correction of Dead-reckoningErrors with the Smart Encoder Trailer,” Interna-tional Conference on Intelligent Robots and Systems,vol. 1, pp. 127–134, 1994.

[4] Borenstein J., “The CLAPPER: A dual-drive MobileRobot with Internal Correction of Dead-reckoning Er-rors,” International Conference on Robotics and Au-tomation, vol. 3, pp. 3085–3090, 1995.

[5] Borenstein J., Feng L., “Correction of systematicodometry errors in mobile robots ,” International

Conference on Intelligent Robots and Systems, vol.3, pp. 569–574, 1995.

[6] Borenstein J., Feng L., “Measurement and correc-tion of systematic odometry errors in mobile robots,”IEEE Transactions on Robotics and Automation, vol.12, pp. 869–880, 1996.

[7] Borenstein J., Feng L., “UMBmark - A method formeasuring, comparing and correcting dead-reckoningerrors in mobile robots,” Technical Report UM-MEAM-94-22, University of Michigan.

[8] Borenstein J., “Experimental results from internalodometry error correction with the OmniMate mobilerobot,” IEEE Transactions on Robotics and Automa-tion, vol. 14, pp. 963–969, 1998.

[9] Chong K.S., Kleeman L., “Accurate Odometry andError Modelling for a Mobile Robot,” InternationalConference on Robotics and Automation, vol. 4, pp.2783–2788, 1997.

[10] Kelly A, “General Solution for Linearized System-atic Error Propagation in Vehicle Odometry,” Inter-national Conference on Inteligent Robot and Systems(IROS01) Maui, Hawaii, USA, Oct. 29 - Nov. 3, pag1938-1945, 2001

[11] T.D. Larsen, “Optimal Fusion of Sensors,” PhD the-sis, Department of Automation, Technical Universityof Denmark, Sept. 1998

[12] T.D. Larsen, M. Bak, N.A. Andersen and O. Ravn,“Location Estimation for Autonomously Guided Ve-hicle using an Augmented Kalman filter to Autocal-ibrate the Odometry,” FUSION98 Spie ConferenceLas Vegas, USA, July 1998

[13] J.J. Leonard, H.F. Durrant-Whyte, “Directed SonarSensing for Mobile Robot Navigation,” Kluwer Aca-demic Publishers, Dordrecht, 1992.

[14] Martinelli A, “The odometry error of a mobile robotwith a synchronous drive system”, IEEE Trans. onRobotics and Automation Vol 18, NO. 3 June 2002,pp 399–405

[15] Martinelli A, “Evaluating the Odometry Error ofa Mobile Robot,” International Conference on In-teligent Robot and Systems (IROS02) Lausanne,Switzerland, September 30 - October 4, 2002, Lau-sanne, Switzerland, pp 853–858

[16] Martinelli A and Siegwart R., “Estimating the Odom-etry Error of a Mobile Robot during Navigation,” ac-cepted at the European Conference on Mobile Robots(ECMR 2003), Warsaw, Poland, September 4-6, 2003

[17] Papoulis A., Probability, Random Variables, andStochastic Process McGRAW-HILL INTERNA-TIONAL EDITIONS, 1991

[18] Wang C.M., “Location estimation and uncertaintyanalysis for mobile robots,” International Conferenceon Robotics and Automation, pp. 1231–1235, 1988.

[19] Zhejun F., Borenstein J., Wehe D., Koren Y., “Ex-perimental evaluation of an Encoder Trailer for dead-reckoning in tracked mobile robots ,” IEEE Inter-national Symposium on Intelligent Control, pp. 571–576, 1995.

1504