Theoretical Results on On-line Sensor Self-Calibration - CiteSeerX

Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems October 9 - 15, 2006, Beijing, China

Theoretical Results on On-line Sensor Self-Calibration Agostino Martinelli

Jan Weingarten

Roland Siegwart

Autonomous System Laboratory ETHZ, Zurich, Switzerland Email: [email protected]

Autonomous System Laboratory EPFL, Lausanne, Switzerland Email: [email protected]

Autonomous System Laboratory ETHZ, Zurich, Switzerland Email: [email protected]

an analytical solution for the case of holonomic vehicles in section IV. Finally, conclusions are presented in section V.

Abstract— In this paper we derive theoretical results for the problem of on-line sensor calibration for a mobile robot. We consider the case of the odometry sensor. A first series of results regards the problem of understanding if a given system (consisting of a robot with several sensors) contains the necessary information to perform the on-line self calibration of one of its sensors. We consider several cases corresponding to different odometry systems and different types of robot sensors. Finally, we also consider the problem of maximizing the calibration accuracy and we formulate this problem as an optimal control problem. For the special case of a holonomic vehicle, we derive an analytical solution, i.e. we find the best trajectory which maximizes the calibration accuracy.

II. T HE S YSTEMS We consider a mobile robot moving in a 2D-environment. The configuration of the robot in a global reference frame can be characterized through the vector X = [xR , yR , θR ]T where xR and yR are the cartesian robot coordinates and θR is the robot orientation. The dynamics of this vector are described by the following non-linear differential equations: ⎡ x˙ R = v cos θ ⎢ (1) X˙ = f (X, u ˆ) = ⎣ y˙ R = v sin θ ˙θR = ω

I. I NTRODUCTION Several strategies have been developed to perform the online sensor self-calibration. In order to do this, the mobile robot is equipped with at least one other sensor. In many cases, an Extended Kalman Filter has been introduced to simultaneously estimate the robot configuration and the parameters characterizing the systematic error of a sensor (i.e., to solve the Simultaneous Localization and Auto Calibration (SLAC) problem). The SLAC problem bas been investigated in [5], [6], [10], [12], [13] both for indoor and outdoor environments. Although in most cases the strategies proposed perform well, the following two questions remain open: •

•

where v and ω are the linear and the rotational robot speed, respectively. The link between these velocities and the robot controls u ˆ depends on the considered robot system drive. We will consider separately the case of a differential drive and of a synchronous drive. In order to characterize the systematic odometry error we adopt for the former the model introduced in [3] and for the latter the model introduced in [11]. For the differential drive we have: v=

Given a system consisting of a mobile robot with several sensors, does the system contain the necessary information to perform the calibration of one sensor and/or to perform SLAC? What is the best robot trajectory which maximizes the calibration accuracy?

ω=

δR vˆR − δL vˆL δb b

(2)

where vˆR and vˆL are the control velocities (i.e. u ˆ = [ˆ vR , vˆL ]T ) for the right and the left wheel, b is the nominal value for the distance between the robot wheels and δR , δL and δb characterize the systematic odometry error due to an uncertainty on the wheels diameters and on the distance between the wheels. For the synchronous drive we have

An answer to the second question regarding the odometry sensor for a mobile robot with differential drive could generalize the off-line method usually adopted to calibrate the odometry, the U M Bmark [3]. This method requires that the robot moves along a square path in both clockwise and counterclockwise direction. In this paper we consider the case of the odometry sensor and we give an answer to the previous two questions for several systems. In section II we define these systems. In section III we answer the first question by carrying out an observability analysis which takes into account the system non linearities. Regarding the second question, we derive

1-4244-0259-X/06/$20.00 ©2006 IEEE

δR vˆR + δL vˆL 2

v = δv vˆ

ω=ω ˆ + δω vˆ

(3)

where vˆ and ω ˆ are the control linear and rotational velocities (i.e. u ˆ = [ˆ v, ω ˆ ]T ) and δv and δω characterize the systematic odometry error due to an uncertainty on the wheel diameters and/or to a not perfect alignment among the wheels. The equations (2) and (3) do not take into account the nonsystematic odometry error. Our robot is also equipped with one or several exteroceptive sensors able to provide observation

43

y

β

θR

R

Robot

D

Landmark

through the four observations previously introduced have the local distinguishability property. We remark that our systems are affine in the input variables, i.e. the function f in (1) has the following structure (both for the differential and synchronous drive):

xR

f (X, u ˆ) = Fig. 1.

fk (X)ˆ uk

(5)

k=1

The considered observations

y = h (X)

M 

where M is the number of the control inputs (M = 2 for the differential and synchronous drive). We now want to remind some theoretical concepts by Hermann and Krener in [8]. We will adopt the following notation. We indicate the K th order Lie derivative of a field ψ along the vector fields vi1 , vi2 , ..., viK with LK vi1 ,vi2 ,...,viK ψ. Note that the Lie derivative is not commutative. In particular, in LK vi1 ,vi2 ,...,viK ψ it is assumed to differentiate along vi1 first and along viK at the end. Let us indicate with Ω the space spanned by all the Lie derivatives LK fi1 ,fi2 ,...,fiK h(X)|t=0 (i1 , ..., iK = 1, 2, ..., M and the functions fij are defined in (5)). Furthermore, we denote with dΩ the space spanned by the gradients of the elements of Ω. In this notation, the observability rank condition can be expressed in the following way: The dimension of the part of the system which is observable at a given X0 is equal to the dimension of dΩ. We found that the computation becomes significantly easier if, for the robot position, we adopt polar coordinates instead of cartesian ones. In these coordinates, the robot configuration is R = [D, χ, θR ]T with xR = D cos χ and yR = D sin χ. The dynamics defined in (1) become: ⎡ D˙ = v cos(θR − χ) ⎢ ⎢ χ˙ = v sin(θR − χ) (6) ⎣ D θ˙R = ω

(4)

where h is the observation function. We assume that a landmark exists in our environment and, without loss of generality, we fix the global reference frame on it. We consider the following four observations (see figure 1  for an illustration): 2) 1) the distance of the landmark (D = x2R + yR 2) the bearing of the landmark (β = π − θR + arctan 2(yR , xR )) 3) both D and β 4) both D and the absolute robot orientation (θR ). III. O BSERVABILITY P ROPERTIES In the previous section we introduced eight different robotic systems which are defined by the drive system (differential and synchronous) and by one of the four mentioned observations. In this section we want to investigate for each system the observability properties of the parameters characterizing the systematic odometry error (δR , δL and δb for the differential drive and δv and δω for the synchronous drive). In other words, we want to understand in which systems the calibration of the odometry can be performed. Furthermore, we want to investigate the observability properties of the robot configuration, namely we want to understand when the calibration can be performed simultaneously with the robot localization (the SLAC problem). In order to answer these questions we carry out an observability analysis which takes into account the system nonlinearities. Indeed, the observability analysis changes dramatically from linear to nonlinear systems [9]. First of all, in the nonlinear case, the observability is a local property. For this reason, in a nonlinear case the concept of the local distinguishability property was introduced by Hermann and Krener [8]. The same authors also introduced a criterium, the observability rank condition, to verify if a system has this property. This criterion plays a very important role since in many cases a nonlinear system, whose associated linearized system is not observable, has however the local distinguishability property. Regarding the localization problem this was proven in [1] and [2]. Note that it is the distinguishability property which implies that the system contains the information to have a bounded estimation error (actually, provided that the locality is large enough with respect to the sensor accuracy). We apply the observability rank condition as introduced by Hermann and Krener [8] to determine if the systems defined

In the case the goal is to perform only the calibration (i.e. we are not interested in estimating also the robot configuration) we can consider the dynamics of D and θ ≡ θR − χ: ⎡ D˙ = v cos θ ⎣ (7) v θ˙ = ω − sin θ D In the rest of this section we carry out the observability analysis separately for the differential and the synchronous drive. A. Differential Drive 1) Distance: Let us startby considering the case of the 2 . The dynamics of the observation h(X) = D = x2R + yR system can be characterized through the following equations: ⎡ ˙ vL ) cos θ D = A (ˆ vR + Bˆ ⎢ vL ) A (ˆ vR + Bˆ ⎢ θ˙ = C (ˆ (8) sin θ vL ) − vR − Bˆ ⎣ D A˙ = B˙ = C˙ = 0 44

where A = (5) we get:

δR 2 ,

B=

δL δR

and C =

1 δR b δb .

By comparing with

T  A f1 = A cos θ, C − sin θ, 0, 0, 0 D T  AB sin θ, 0, 0, 0 f2 = AB cos θ, −BC − D

of the system are characterized by the following equations: ⎡ ˙ vL ) cos(θR − χ) D = A (ˆ vR + Bˆ ⎢ vL ) vR + Bˆ ⎢ χ˙ = A (ˆ sin(θR − χ) ⎢ (12) D ⎢ ⎢˙ vR − Bˆ vL ) ⎣ θR = C (ˆ A˙ = B˙ = C˙ = 0

(9)

By comparing (12) with (5) we get: T  sin θ , C, 0, 0, 0 f1 = A cos θ, A D  T sin θ f2 = AB cos θ, AB , −BC, 0, 0, 0 D

By a direct computation it is possible to prove the independence of the gradients of the following Lie derivatives: L0 h, L11 h, L12 h, L211 h and L3111 h. This means that the dimension of dΩ is 5 and therefore the state [D, θ, A, B, C]T can be observed. Hence the system contains the necessary information to perform the self-calibration. 2) Bearing: Let us consider now the case of the bearing observation (β = π − θ). By defining Da = D A we get from the equation (8) ⎡ vR + Bˆ vL ) cos θ D˙ a = (ˆ ⎢ vL ) (ˆ vR + Bˆ ⎢ ˙ (10) vL ) − sin θ vR − Bˆ ⎢ θ = C (ˆ ⎣ Da B˙ = C˙ = 0

By a direct computation it is possible to prove the independence of the gradients of the following Lie derivatives: L0 hθR , L11 hθR , L12 hθR , L0 hD , L11 hD and L211 hD . This means that the dimension of dΩ is 6 and therefore it is possible to perform SLAC. B. Synchronous Drive We want to derive similar results for the case of a synchronous drive. The dynamics is now described by the following differential equations: ⎡ ˙ D = δv vˆ cos θ ⎢ ⎢ θ˙ = ω + δω vˆ − δv vˆ sin θ (14) ⎣ D δ˙v = δ˙ω = 0

The observation β does not provide any information to distinguish A from D. In other words, the best we can hope is that the state [Da , θ, B, C]T is observable. By comparing (10) with (5) we get:  T sin θ , 0, 0 f1 = cos θ, C − D T  B f2 = B cos θ, −BC − sin θ, 0, 0 D

(13)

1) Distance: By comparing (14) with (5) we get: T  δv sin θ, 0, 0 f1 = δv cos θ, δω − D

(11)

(15)

T

f2 = [0, 1, 0, 0]

By a direct computation it is possible to prove the independence of the gradients of the following Lie derivatives: L0 h, L11 h, L211 h and L212 h. This means that the dimension of dΩ is 4 and therefore the state [D, θ, δv , δω ]T can be observed. Hence, the system contains the necessary information to perform the self-calibration. 2) Bearing: We proceed similarly to the case of the differential drive. Let us define Dv = δDv . Then, we get from the equation (14): ⎡ D˙ v = vˆ cos θ ⎢ vˆ ⎢ ˙ (16) sin θ ⎢ θ = ω + δω vˆ − ⎣ Dv δ˙ω = 0

By a direct computation (as in the previous case) it is possible to prove the independence of the gradients of the following Lie derivatives: L0 h, L11 h, L12 h and L211 h. This means that the dimension of dΩ is 4 and therefore the state [Da , θ, B, C]T can be observed. 3) Bearing and Distance: The system contains the information to perform the self-calibration as proven in III-A.1. We want to see if at the same time the robot configuration [D, χ, θR ]T is observable. In other words, we want to see if it is possible to perform SLAC. We will prove that it is not possible. Indeed, from the equation (6), the vectors field fi defined in (5) will depend on θR and χ only through the difference θR − χ. On the other hand, also β depends on θR and χ through this difference. Finally, D is independent of them. This means that all the Lie derivatives will have T the gradients with the structure [q1 , q2 , q3 , q4 , q5 , q6 ] with q2 = −q3 . Therefore, the dimension of dΩ will be less or equal than 5. Since the previous derivation was obtained by using (6), which holds both for a differential and a synchronous drive, the result holds for both the drive systems. 4) Distance and Absolute Orientation: In this case the observation consists of hD = D and hθR = θR . The dynamics

The observation β does not provide any information to distinguish δv from D. In other words, the best we can hope is that the state [Dv , θ, δω ]T is observable. By comparing (16) with (5) we get:  T sin θ ,0 f1 = cos θ, δω − Dv (17) T

f2 = [0, 1, 0] 45

By a direct computation it is possible to prove the independence of the gradients of the following Lie derivatives: L0 h, L11 h and L212 h. This means that the dimension of dΩ is 3 and therefore the state [Dv , θ, δω ]T can be observed. 3) Bearing and Distance: As mentioned before, the result obtained in III-A.3 holds both for differential and synchronous drive. 4) Distance and Absolute Orientation: In this case the observation consists of hD = D and hθR = θR . The dynamics of the system are characterized by the following equations: ⎡ D˙ = δv vˆ cos(θR − χ) ⎢ ⎢ χ˙ = δv vˆ sin(θ − χ) R ⎢ D (18) ⎢ ⎢˙ ⎣ θR = ω + δω vˆ δ˙v = δ˙ω = 0 By comparing (18) with (5) we get:  T δv f1 = δv cos θ, sin θ, δω , 0, 0 D

the estimation. Indeed, depending on the method, we have a well precise characterization for the estimation error. In this paper we consider the case of the Extended Kalman Filter (EKF ) since it has been widely used in previous works, both for SLAM and SLAC. In this case the characterization of the estimation error is provided by a covariance matrix P satisfying the Riccati differential equation [4]. This characterization can be adopted in a real implementation (where the system is actually time-discrete) since, starting from a discrete-time system, it is possible to derive an equivalent continuous-time system model as shown in [14]. The structure of the Riccati equation is: dP = F P + P F T + Q − f P H T R−1 HP (20) dt where F is the Jacobian of the dynamics with respect to the state to be estimated, Q characterizes the noise in the dynamics, R characterizes the observation error, H is the Jacobian of the observation with respect to the state to be estimated, f is the observation frequency. We remark that for all the systems considered in section III the solution of the Riccati differential equation can be obtained only numerically. Once we have a characterization for the estimation error, we must specify all the constraints of the problem. These constraints can be a fixed (or a maximum) length of the trajectory, a fixed (or a maximum) length of time to perform the estimation, a fixed (or a maximum) cost in terms of energy and special requirements on the control input. We remark that for all the systems introduced in section III it is not possible to obtain the solution analytically. In the next subsection we provide an analytical solution for a simpler system consisting of an holonomic vehicle.

(19)

T

f2 = [0, 0, 1, 0, 0]

By a direct computation it is possible to prove the independence of the gradients of the following Lie derivatives: L0 hθR , L11 hθR , L0 hD , L11 hD and L212 hD . This means that the dimension of dΩ is 5 and therefore it is possible to perform SLAC. We summarize the results of this section through the following theorems. They hold both for the differential and the synchronous drive. Theorem 1 It is possible to self-calibrate the odometry when the robot is equipped with a sensor able to provide its distance from a single landmark in the environment.

A. Holonomic Vehicle In this section we derive an analytical solution to the problem of finding the best trajectory for the case of a holonomic vehicle. The dynamics are described by the following differential equations: x˙ = δx vˆx (21) y˙ = δy vˆy

Theorem 2 It is possible to partially self-calibrate the odometry when the robot is equipped with a sensor able to provide the bearing of a single landmark in the environment. In particular, the calibration can be performed up to a scale factor. Theorem 3 It is not possible to perform SLAC when the observation consists of the bearing and the distance of a single landmark in the environment.

where vˆx and vˆy are the control inputs and δx and δy are two parameters which we want to estimate. More precisely, we want to perform SLAC (i.e. the simultaneous estimation of x, y, δx and δy ) and find the best trajectory (i.e. the best input vˆx and vˆy ) in order to minimize the error in the two parameters δx and δy . In particular, we require to have the same error in them. The vehicle is equipped with an exteroceptive sensor able to provide the full robot configuration (i.e. z = [x, y]T ) at a frequency f . Regarding the error model to characterize this exteroceptive sensor, we assume a Gaussian model without correlation between the two components (i.e., 2 × I2 ). Furthermore, we assume that the energy cost R = σobs depends quadratically on the vehicle speed (this is a suitable

Theorem 4 It is possible to perform SLAC when the observation consists of the distance of a single landmark in the environment and the absolute robot orientation. IV. O PTIMAL T RAJECTORIES FOR S ELF -C ALIBRATION Once we know that a given system contains the necessary information to perform the estimation of a set of parameters, the fundamental question arising is: what is the best choice for the control input in order to minimize the error in the estimation of these parameters? In order to answer the previous question it is necessary to specify first of all the method which is adopted to carry out

46

hypothesis for many cases). Finally, the solution has to satisfy the following requirements: 1) The length of time allowed for this estimation has to be equal to a given value T . Since we assumed a constant frequency f for the observations, this means that we fix the number of observations. 2) The total  2 cost has to be equal to a fixed value,

T energy 2 x˙ + y˙ dt = E, where E is the allowed i.e. λ 0 energy cost and λ is a parameter which depends on the environment and the vehicle. The structure of the Riccati differential equation (20) associated to this problem is block diagonal. Therefore, its solution can be obtained by considering separately the x and y dimension and we have to solve twice a simpler problem whose dynamics are described by the following equations: ξ˙ = δˆ v (22) ˙δ = 0

=

U˙ = F U V˙ = −F T V + CU

ˆ − w(t) rΩ(t) ˆ (1 + rt)

where: 2 σ2 r = f σσ2 , rδ = f σ2δ , obs

obs



w(t) ˆ = ˆ Ω(t) =

t

0



t

0

vˆ(τ )dτ,

(26)

w(τ ˆ )dτ,

(27)

ˆ 2 (t) = Ω

 0

t

w ˆ2 (τ )dτ

(28)

Our goal is to minimize the error in the estimation of δ at the final time T , i.e. P22 (T ). This is equivalent to maximize −1 −1 P22 (T ) or to minimize −P22 (T ). From (24) and (25) we obtain: −1 ˆ 2 (T ) − 1 ˆ )2 − f Ω (T ) = αΩ(T −P22 2 σobs σδ2

 F =

and

 C=

0 0

vˆ 0

therefore, by introducing the Lagrange multiplier μ to take into account (30) and by neglecting the constant term in (29) we have to find the extrema of the following functional:

(23a)



−2 f σobs

0

f ˆ 2 ˆ − 2 Ω 2 (T ) + αΩ(T ) + μ σobs

(23b)

 0

T

 w(τ ˆ˙ )2 dτ − Eλ (31)

0 0

and by neglecting the constant term −μEλ we can consider the functional S  with the same extrema:

 (23c)

S  [w] ˆ =  T

The dynamics described in (22) do not take into account any noise. When this is considered, the structure of (23a) changes. In particular, the first equation in (23a) becomes U˙ = F U +QV , where Q characterizes the entity of the noise in the dynamics (see [4] for more details). In the following we do not consider this term. In order to have P (0) = P0 we set the following initial conditions on U and V : U (0) = P0 and V (0) = I, where I is the identity matrix 2 × 2. The solution of (23) is:  2  ˆ σ σδ2 w(t) (24) U= 0 σδ2 V =

(29)

rδ r . Furthermore, our solution has to where α = σ2 (1+rT ) δ satisfy the following constraint:  T E ≡ Eλ (30) w(τ ˆ˙ )2 dτ = λ 0

S[w] ˆ =

where



and

The observation is z = ξ + n where n is a zero mean 2 . Gaussian variable whose variance is σobs The initial covariance matrix for the state [ξ, δ]T characterizing the uncertainty on  2  the state at the initial time, is σ 0 P (0) = P0 = . We look for the solution of the 0 σδ2 Riccati differential equation by setting P = U V −1 (see [4] for the details). In these new variables the Riccati equation becomes:

ˆ

rδ Ω(t)  ˆ ˆ 1 + rδ Ω2 (t) − w(t) ˆ Ω(t)

1 + rt

0

 2  T f 2 2 ˙ μw ˆ − 2 w ˆ dτ + α wdτ ˆ σobs 0

(32)

We remark that, apart the last term, this is the action of a particle of mass 2μ in a harmonic potential field V = σ2f w ˆ2 . obs  Unfortunately, the last term implies that S does not satisfy the localization property (i.e. additivity over time, see [7]), so that the Euler-Lagrange differential equations cannot be directly adopted. However, it is possible to directly compute the first variation of S  (see [7] for the details) δS  =2δwˆ (T )w(T ˆ˙ ) − 2δwˆ (0)w(0) ˆ˙   T f ¨ +2 ˆ + αΩT δwˆ (τ )dτ −μw ˆ− 2 w σobs 0

(25)

47

(33)

where ΩT = Ω(T ) and δwˆ (.) is the variation of the input function. From (26) we have: w(0) ˆ =0

V. C ONCLUSIONS In this paper we analyzed the problem of on-line self calibration for autonomous vehicles from a theoretical point of view. We restricted our analysis to the odometry sensors and we gave an answer to the following two questions: • does the system contain the necessary information to perform the self-calibration? • what is the best trajectory in order to maximize the accuracy on the calibration? Regarding the first question we considered several mobile vehicles with differential and synchronous drive. We analyzed also the case when these systems contain the information to perform the simultaneous localization and odometry calibration (SLAC). Regarding the second question we provided an analytical solution for the case of holonomic vehicles.

(34)

Therefore, the variation δwˆ (0) = 0. On the other hand, since it is not fixed the value of w(T ), in order to have δS  = 0 we have to impose: w(T ˆ˙ ) = 0

(35)

Furthermore, we have to set to zero the integrand in (33) ¨ −μw ˆ−

f ˆ + αΩT = 0 2 w σobs

(36)

From (27) we have:  ΩT =

0

ACKNOWLEDGMENT T

w(τ ˆ )dτ

This work has been supported by the Swiss National Science Foundation No 200020-112152/1. The authors would like also to thank Antonio Bicchi for helpful discussions.

(37)

The differential equation (36) depends on the two unknown parameters μ and ΩT . Furthermore, since it is of the second order, its solution will depend on two parameters more. On the other hand, the equations (30), (34), (35) and (37) fix the values of these four parameters. In particular, we found two solutions for this system which correspond to the motion forth and back. We have:  w ˆ=±

2Eλ T × (ωT )2 − ωT sin ωT cos ωT

R EFERENCES [1] Bicchi A., Pratichizzo D., Marigo A. and Balestrino A., On the Observability of Mobile Vehicles Localization, IEEE Mediterranean Conference on Control and Systems, 1998 [2] Bonnifait P. and Garcia G., Design and Experimental Validation of an Odometric and Goniometric Localization System for Outdoor Robot Vehicles, IEEE Transaction On Robotics and Automation, Vol 14, No 4, August 1998 [3] Borenstein J., Feng L., “Measurement and correction of systematic odometry errors in mobile robots,” IEEE Transactions on Robotics and Automation, vol. 12, pp. 869–880, 1996. [4] W.L. Brogan, Modern Control Theory, Prentice-Hall, 1991 [5] Caltabiano D., Muscato G. and Russo F., “Localization and Self Calibration of a Robot for Volcano Exploration” International Conference on Robotics and Automation, pp. 586–591, New Orleans, USA, 2004. [6] E.M. Foxlin, Generalized Architecture for Simultaneous Localization, Auto-Calibration, and Map building, International Conference on Intelligent Robots and Syrtems, EPFL, Switzerland, October 2002. [7] I.M. Gelfand and S.V. Fomin, Calculus of Variations, Englewood Cliffs, 1963 [8] Hermann R. and Krener A.J., 1977, Nonlinear Controllability and Observability, IEEE Transaction On Automatic Control, AC-22(5): 728-740 [9] Isidori A., Nonlinear Control Systems, 3rd ed., Springer Verlag, 1995. [10] Larsen, T.D., Hansen K.L., Andersen N.A. and Ole Ravn, Design of Kalman filters for mobile robots; evaluation of the kinematic and odometric approach, Proceedings of the 1999 IEEE International conference on Control Applications, volume: 2 , 22-27 Aug. 1999, Pages:102l-1026. [11] Martinelli A, “The odometry error of a mobile robot with a synchronous drive system”, IEEE Trans. on Robotics and Automation Vol 18, NO. 3 June 2002, pp 399–405 [12] A. Martinelli, N. Tomatis, A. Tapus and R. Siegwart, Simultaneous Localization and Odometry Calibration for Mobile Robot, International Conference on Intelligent Robots and Systems, Las Vegas, Nevada, October 2003. [13] A. Martinelli, D. Scaramuzza and R. Siegwart, Automatic SelfCalibration of a Vision System during Robot Motion, International Conference on Robotics and Automation, Orlando, Florida, April 2006. [14] P.S. Maybeck, Stochastic Models, Estimation and Control, Academic Press, 1979, vol 141-1

(38)

× [cos (ω(t − T )) − cos (ωT )] and for the control speed:  vˆ = ±

2Eλ ω sin (ω(T − t)) ωT − sin ωT cos ωT

(39)

where ω satisfies the following equation: tan(ωT ) 1 =− ωT rT

(40)

This last equation provides the control speed which minimizes the estimation error in the parameter δ for the system whose dynamics are described by (22) and when SLAC is performed (i.e., the simultaneous estimation of ξ and δ) through an EKF . Furthermore, the minimization is carried out by considering all the control speeds satisfying (30). Regarding the original problem, whose dynamics are described through (21), the solution is obtained by setting the two √ directions to the same speed given in (39) but divided by 2. Indeed, in this case, instead of (30) the energy constraint regards both the speeds: 

T 0

  vˆx (τ )2 + vˆy (τ )2 dτ = Eλ

(41)

48