Odometry Error Covariance Estimation for Two Wheel Robot Vehicles

Transcription

1 Odometry Error Covariance Estimation for Two Wheel obot Vehicles indsay KEEMAN Intelligent obotics esearch Centre Department of Electrical and Computer Systems Engineering Monash University Technical eport MECSE ASTACT This technical report develops a simple statistical error model for estimating position and orientation of a mobile robot using odometry. Once the errors are characterised, other sensor data can be combined sensibly in the estimation of position, using the Extended Kalman Filter [Kleeman, 99 #; Jazwinsi, 97 #7]. A closed form error covariance matrix is developed for (i) straight lines and (ii) constant curvature arcs and (iii) turning about the centre of axle of the robot. Other paths can be composed of short segments of constant curvature arcs without great loss of accuracy. The model assumes that wheel distance measurement errors are exclusively random zero mean white noise. Systematic errors due to wheel radius and wheel base measurement are ignored, since these can be removed by calibration. Previous wor on developing odometry covariance relies on incrementally updating the covariance matrix in small times steps. The approach taen here integrates the noise theoretically over the entire path length to produce simple closed form expressions, allowing efficient covariance matrix updating after the completion of path segments.

2 . Introduction Odometry is the measurement of wheel rotation. When odometry measurements are nown as a function of time from two wheels on a common axle, the position and orientation of the centre of the axle can be determined as a function of time. Practical implementations of this approach where both time and wheel increments are discretised are used as one estimate of a mobile robot's position and orientation [Wang, 988 #87]. With time, however, errors accumulate in odometry localisation, due to wheel slippage and measurement error in rotation and distance between wheels, and non-continuous sampling of wheel increments. It is therefore necessary to use in addition some external referencing of position to correct for accumulating odometry errors. This technical report develops a simple statistical model for the errors in estimation of position and orientation of a mobile robot using odometry. Once the errors are characterised, other sensor data can be combined sensibly in the estimation of position, using the Extended Kalman Filter [Kleeman, 99 #; Jazwinsi, 97 #7]. A closed form error covariance matrix is developed for (i) straight lines and (ii) constant curvature arcs and (iii) turning about the centre of axle of the robot. Other paths can be composed of short segments of constant curvature arcs without great loss of accuracy. The model assumes that errors are exclusively random zero mean white noise in the measurement of distance travelled by each wheel. Systematic errors due to wheel radius and wheel base measurement are ignored, since these can in theory be removed by calibration. Previous wor on developing odometry covariance relies on incrementally updating the covariance matrix in small times steps. The approach taen here integrates the noise theoretically over the entire path length to produce simple closed form expressions, allowing efficient covariance matrix updating after the completion of path segments. Figure - Wheel Geometry.

3 . Odometry Error Assumptions The odometry error of a wheel incurred in one unit of travel is assumed to be randomly distributed with zero mean and to be independent of the odometry error incurred in the next or previous unit of travel. The variance of the cumulative error is then the sum of the variance of each independent segment and is therefore proportional to the distance travelled. The error variances for the left and right wheels σ and σ can then be expressed as: σ = d σ = d () where d and d are the distances travelled by each wheel, and and are constants. 3. Straight ine Motion The robot wheels are controlled to travel at the same speed as measured by wheel odometry with the aim of following a straight line path. In reality errors in odometry mean that the path deviates randomly from a straight line. As shown in figure, there are three errors in the path: (i) the error in distance travelled along the line, es, (ii) the error perpendicular to the line, ep, and (iii) the error in heading angle e. Each of these errors is characterised in terms of the basic odometry errors of each wheel. Initially, the errors are respectively es(), ep) and e () with an initial covariance matrix given by: P () sp s σ ( ) σ ( ) σ ( ) s sp s σ ( ) σ ( ) σ ( ) sp p p σ ( ) σ ( ) σ ( ) p () desired straight line e s e p e Figure - Definition of Errors in Straight ine Path - the robot path is shown as the dashed line. elow, the effect of propagating these errors and the introduction of new errors is derived for a straight line motion. The problem is tacled by breaing up the trajectory of the robot into discrete small segments where the robot travels a distance of d. The segments are indexed so that the robot angle error at the end of the th segment is denoted by e (). The errors in odometry of the left and right wheels are denoted by e and e, and the 3

4 distance between wheels by as shown in Figure. The error introduced in after a segment d of distance is denoted by e. Therefore e ( ) = e ( ) e ( ) = e( ) e ( i) (3) e() i e() i = e ( ) From equation (3) and the assumption that odometry errors are independent, with respect to time, and with respect to wheels, the variance of the angle, σ is given by σ( ) σ( ) d = (4) where d is the total distance travelled and is given by d. The perpendicular distance error is given approximately by e ( ) e ( ) de ( ) (5) p p where the angle error is assumed to be small and constant throughout the segment. This approximation is good when angle errors are ept small and the segment size is small enough so that new angle errors introduced into the segment are insignificant compared to the current angle error. From equations (3) and (5) and rearranging e ( ) = e ( ) d e ( j ) p p j= = e ( ) d e ( ) e ( i) p j= j = e ( ) de ( ) d ( h) e ( h) p h= = e ( ) de ( ) d ge ( g) p g= (6) From the assumptions of independence of errors and equation (6), the variance of the perpendicular distance error, σ p is given by 4

5 σp σp dσp d σ d g ( ) = ( ) ( ) ( ) ( ) d g= 3 ( d) ( ) = σp( ) dσp ( ) d σ( ) g g= 3 3 ( d) ( ) ( ) 3( ) ( ) = σp( ) dσp ( ) d σ( ) 6 (7) Note that the expression for the sum of integer squares can be proven by mathematical induction on. In the limit as d approaches zero and d is the total distance of travel d, the expression in equation (7) reduces to σ = σ ( ) dσ ( ) d σ ( ) p p p dd ( ) 3 (8) The variance of the perpendicular error is now seen as dependent on the cube of distance rather than just distance as is the case for the angle error. This can be understood by observing that the perpendicular errors accumulate from an ever increasing angle error translated into perpendicular distance error by the movement. The error in distance straight along the path for the th segment, e s () is given by e( ) e( ) es( ) = es( ) cos( e ( )) e( ) e( ) es ( ) s = e ( ) e ( i) e ( i) (9) where the approximation is valid for small heading angle errors. The corresponding variance is then σs σs d ( ) = ( ) 4 () The off-diagonal covariances are now evaluated. From equations (3) and (9) 5

6 σ s = Eee s e e () i e () i es e i e ( ) i = e () i e() i s i = = E = σ ( ) E = σs ( ) d ( ) () () i () and from equations (9) and (6) σ = E e ( ) e ( i) e ( i) ep ( ) de( ) d h e h i ( ) ( ) = h= d = σsp ( ) dσs ( ) E ( i) e i e i () () d = σsp ( ) dσs ( ) ( i) d d d ( ) = σ sp ( ) dσs ( ) 4 sp s In the limit as approaches infinity d σ = σ ( ) dσ ( ) 4 sp sp s () (3) From equations (9) and (3) σ e ( h) e ( h) ( ) p = E ep( ) de( ) d( h) e h= σp dσ E d = ( ) ( ) ( i) e( i) e( i) d = σp ( ) dσ( ) ( i) d d d ( ) = σp ( ) dσ( ) e () i e() i (4) In the limit as approaches infinity 6

7 σ = σ ( ) dσ ( ) p p dd (5) In summary, the standard deviation of angle error and distance along the path increase with the square root of distance while the perpendicular standard deviation increase as a power.5 with distance. The constants of proportionality all can be related bac to the basic odometry error statistics on each wheel. If both wheels have the same error statistics, then only the perpendicular distance error and angle errors are correlated with a constant correlation coefficient of σ σ σ = p p 4. otation About the Centre of Axle of the obot The robot wheels are controlled to travel in opposite directions at equal speed as measured by the odometry, with the aim of turning through an anti-clocwise angle α at one position with respect to the centre of the axle. The starting angle with respect to the X-axis is assumed to be β, as shown in figure 3. Errors in odometry mean that the point of turning moves from its starting position and the total angle turned is not exactly α. Final angle Major axis Minor axis α Starting angle β X-axis Figure 3 - Definition of Starting and Final Angles, and Major and Minor Axes. Again small segments of motion are considered. Each segment now is a change of angle =α/. The error in angle, eα is examined first. e( ) e( ) eα( ) = eα( ) = e ( ) e () i e () i α (6) The variance of this error is given by σα( ) = σα( ) ( ) α = σα ( ) ( ) (7) 7

8 To determine the covariance matrix of errors in position of the centre of axle, two orthogonal axes are first defined as shown in figure 3. The major axis is defined to be at an angle of βα/ and the minor axis at the angle of βα/ π/. The errors of the axle centre are derived with respect to these axes, since these components will be shown to have zero covariance. As the robot rotates, it is only the common mode or sum of wheel increments that contribute to the axle centre error. These errors are always in the direction or opposite direction to that at which the robot faces at any instant. As the angle moves from α/ to α/ with respect to the major axis, the major axis error e M () is given by e( ) e( ) em( ) = em( ) cos ( ) e() i e() i = em ( ) cos ( i ) (8) The variance of the major axis component is given by σm( ) = σm( ) cos ( i) (9) 4 In the limit as approaches zero, equation (9) becomes an integral as follows: α/ α/ ( ) σm( ) = σm( ) cos ( ) d 8 ( ) sin = σ M ( ) 8 4 ( ) sinα α = σ M ( ) 6 α/ α/ () The minor axis error, e m () is derived in a similar way: e( ) e( ) em( ) = em( ) sin ( ) e() i e() i = em ( ) sin ( i ) () Again taing limits introduces an integral: We have ignored the second order effect of angle error in cos (). 8

9 α/ α/ σm( ) = σm( ) ( ) sin ( ) d sin = σm( ) ( ) 4 α sinα = σm( ) ( ) α/ α/ () To show that the major and minor components have covariance of, the expectation of e M e m is examined. Note that E[ e ( i) e ( i)] Ee [ ( i) e( j)] =, i j (3) Ee [ ( i) e( j)] =, i j = Taing expectations of equation (8) multiplied by equation (), and taing the limit: α/ α/ α/ α/ σmm ( ) = σmm ( ) cos( )sin( ) d 4 ( ) cos = σmm ( ) 8 4 = σ ( ) mm (4) That is, the covariance contribution between the major and minor axis components is zero. Symmetry arguments could be applied to achieve the same result, and in fact this was the method for choosing the axes in the first place. The covariance between the angle and the major and minor axes are now investigated: σ αm ( ) = E eα( ) e() i e() i em ( ) = σ ( ) E e () i e ()cos i () i αm = σ ( ) cos ( i) αm e() i e() i cos ( i ) (5) In the limit as approaches zero, the summation becomes an integral as follows: 9

10 α/ σα M( ) = = σα M( ) cosd 4 α/ σα M ( ) sin( α ) Similarly, it follows that α/ σα m( ) = σα m( ) sind 4 α/ = σ ( ) αm That is, there is no correlation between the angle and minor axis errors. (6) (7) 5. Extension to Arbitrary adius Arcs In this section we consider the case of the wheeled vehicle traversing an arc of finite radius. Figure 4 shows the propagation of an error in the direction of motion after traversing an arc of angle. The result is a perpendicular error of e s sin and a straight ahead error of e s cos. e s cos e s e s sin Actual centre e s Nominal centre Figure 4 Propagation of straight error in arc move of angle. The propagation of a perpendicular error e is shown in Figure 5. The result is a perpendicular error of ep cos and a straight ahead error of e p sin. The propagation of straight and perpendicular errors can be seen to be a rotation through the angle. The propagation of an angle error e through the same arc is shown in Figure 6 and is much more complex in the geometry. p

11 e p e p sin e p cos Actual centre Nominal centre e p Figure 5 - Propagation of perpendicular error in arc move of angle. ( sin ( / ))sin ( e / ) e sin ( / ) Figure 6 - Propagation of angle error in arc move of angle and radius. e On close consideration of the small triangle in the upper bubble, the error length is 4sin( / )sin( e / ) which is at an angle of e to the radial direction at the end point. For small angle errors and angular increments, the perpendicular error is approximated by 4sin( / )sin( e / ) cos(( e ) ) sin( / ) e (8)

12 and the tangential error is approximated by 4sin( / )sin( e / )sin(( e ) ) This gives the following iterative relationships on errors: ep( ) ep( )cos es( )sin e ( )sin( / ) e( ) e( ) es( ) ep( )sin es( )cos e ( ) = e ( ) e ( ) = e ( ) e ( i) (9) We define the complex error E() as follows: E( ) ep( ) j es( ) (3) This allows rewriting of the first two iterative expressions of (9) as: j E ( ) = e E ( ) e( ) sin( / ) j e( ) (3) where e( ) e( ) ed ( ) (3) After iterating on (3) we have: d j ji E ( ) = e E( ) e e( i) sin( / ) j e( i) jα ji ji = e E( ) sin( / ) e e ( ) e ( g) j e e ( i) i g= j α g j α e i = e E( ) e ( ) j sin( / ) e ( g) e j j d g= d e j i e ( i) d (33) where α j j i e sin( / ) e = sin( / ) j e α j α e as and α = fixed. j In (34) the limit as is evaluated using 'Hopital's ule. ewriting equation (33) and simplifying with the real and imaginary versions of the limits in (34) gives: (34) When the numerator and demominator of a limit approach zero they can be replaced by their derivatives with respect to the limiting variable.

13 e ( ) e ( )cos α e ( )sin α e ( )( cos α) e ( g) cos(( g) ) e ( i )cos( i ) s s p g= e ( ) e ( )sin α e ( )cos α e ( )sin α e ( g)sin(( g) ) e ( i )sin( i ) p s p e( ) = e( ) e () i or in matrix form: g= d d. (35) e() = Φ e( ) Ψ( i ) en( i) cosα sin α ( cos α) = cos( i ) cos( i ) where Φ sinα cosα sin α, Ψ( i )= sin( i ) sin( i ), en( i) (36) = e () i ed () i The error covariance matrix is then T P( ) = Cov( e( ) ) = E[ e( )e( ) ] Τ =ΦP( ) Φ Ψ( i ) Q( i) Ψ( i ) (37) Τ where s s s s Ee i Ee ied i Q( i) = ( ) ( ) [ ( )] [ ( ) ( )] Ee ie i Ee i s s s s d d = [ ( ) ( )] [ ( )] ( ) ( ) 4 (38) and s is defined to be the distance travelled by the left wheel with an angle change of. Therefore the angle subtended by the left wheel is given by s = ( ) (39) s = = similarly s, The radius of curvature is assumed to be negative for arcs with a centre to the right of the heading direction. In the limit as approaches zero and approaches infinity, we can evaluate (37) as the integral: 3

14 α Τ Τ P() =ΦP( ) Φ sign( α) Ψ( ) Q Ψ( ) d (4) where sign(x) is for x> and - for x<. The integral evaluates to the following: α sp p p ρs ρsp ρs Ψ( ) Q Ψ( ) Τ d = ρ ρ ρ ρs ρp ρ s s α α ρs = 6α 8 α α sin sin sin s s 4sinα α sinα α α ρ p = sin s s s s 4 4 αs s ρ = 3 4 sp = s s cosα cos α 4 cos α ρ 6 sinα ρs = s s α sinα s s ρp = α s s s s cos. (4) It has been shown by the author that in the limit as the arc approaches a straight line (ie approaches infinity, α approaches zero with α=d constant) the results of equation (4) approach the straight line results presented in Section Example Calculation of the Odometry Covariance Matrix In this section an example robot path is used to illustrate the results of the previous sections. The path shown in Figure 7 consists of a straight line, turn on the spot, an arc of radius twice the wheel base and an arc of radius /4, where is.5 m. The covariance matrix is calculated after each move. To facilitate the calculations, we require a transformation matrix T() which represents a rotation of the X-Y coordinate system through an angle. Suppose we represent our robot position and orientation with x=[x y ] T, then T() is given by: T( ) = cos sin sin cos Note that the inverse and transpose of T are the equal. (4) 4

15 (3.5,.5) E (3,) D (,) (,) C A Figure 7 - Example robot path with (x,y) coordinates mared (in metres). The left and right wheel paths are shown dotted. The covariance matrix results presented in Sections 3 and 5 are with respect to tangential coordinates for lines and arcs. Therefore coordinates need to be rotated by to convert to X-Y cartesian coordinates, where is the heading angle of the robot after the path segment. Thus if the covariance matrix P is with respect to tangential coordinates, then the cartesian covariance matrix, P is given by xy sp P = Cov[ T( ) e ( T( ) e ) T ] = Cov[ T( ) e e T T( ) T ] = T( ) P T( ) T (43) xy sp sp sp sp sp The results for straight line paths of Section 3 are summarised here in matrix form. The distance d is positive when moving in the direction of the robot and negative when the robot moves bacwards. The covariance after the straight line move is the addition of the propagation of the initial covariance P and the contribution of noise along the path itself, Q ( d) line : sp( ) 5

16 T P ( ) = Φ ( d) P ( ) Φ ( d) Q ( d) sp line sp line line where Φ ( d) = and Q line line ( d) = d d d( ) ( ) 4 4 d ( ) d ( ) d ( ) 4 3 ( ) d ( ) (44) The results are generated from MATA scripts which are included at the end of this section. Error ellipses are generated for X-Y errors at the end of each segment and due to the noise introduced along each segment and are plotted in Figures 8 and 9. Covariance matrices are symmetric and therefore, from linear algebra, can be diagonalised using an orthonormal basis (ie by changing coordinate system to these unity orthogonal vectors we achieve decoupling of the error contributions since their covariance matrix is diagonal). The basis vectors are the eigenvectors and these form the axes of the error ellipses. The lengths of the axes are the square root of the eigenvalues and correspond to standard deviations of the independent noise contribution in the direction of the eigenvector. For the purposes of displaying the results,only X-Y errors are shown. Error Ellipses corresponding to 5 standard deviations D to E E D A to C to D A C to C Figure 8 - Error Ellipses for each path segment and at the end of each segment. = = 3 m, =.5 m. 6

17 D to E Error Ellipses corresponding to 5 standard deviations C to D D E A to A C to C Figure 9 - Error Ellipses for each path segment and at the end of each segment. 3 3 = m = m =.5 m. Tables I and II list the covariance matrices corresponding to Figures 8 and 9. ocation TAE I = = 3 m, =.5 m A A to to C C P xy

18 C to D D D to E E ocation 3 3 TAE II = m =, =.5 m m P xy. 4 A A to to C C C to D D D to E

19 E Shown below is a MATA script UN.M and functions to evaluate the covariance matrices. Output from UN.M is also shown for different sets of l and r. ;********* start of UN.M ****** Pxy = T(-angle)*P*T(-angle)' l = e-3 [V,D] = eig(pxy(:, :)) r = e-3 sqrtd=sqrt(d) =.5 d = Pdiff = arc_cov(zeros(3,3), dl, dr, l, r, ); Pdiffxy = T(-angle)*Pdiff*T(-angle)' angle = [V,D] = eig(pdiffxy(:, :)) P=zeros(3,3); sqrtd=sqrt(d) P = line_cov(p,l, r,, d); %[V,D] = eig(p) Pxy = P a = pi/; [V,D] = eig(pxy(:, :)) dl= ra_to_dl(.5, a, ); sqrtd=sqrt(d) dr =ra_to_dr(.5, a, ); P = arc_cov(p, dl, dr, l, r, ); a = pi/; angle = angle a; dl= ra_to_dl(e-6, pi/, ); Pxy = T(-angle)*P*T(-angle)' dr =ra_to_dr(e-6, pi/, ); [V,D] = eig(pxy(:, :)) P = arc_cov(p, dl, dr, l, r, ); angle = angle a; Pxy = T(-angle)*P*T(-angle)' sqrtd=sqrt(d) Pdiff = arc_cov(zeros(3,3), dl, dr, l, r, ); Pdiffxy = T(-angle)*Pdiff*T(-angle)' [V,D] = eig(pxy(:, :)) [V,D] = eig(pdiffxy(:, :)) sqrtd=sqrt(d) sqrtd=sqrt(d) Pdiff = arc_cov(zeros(3,3), dl, dr, l, r, ); Pdiffxy = T(-angle)*Pdiff*T(-angle)' ;************ end of UN.M ************* [V,D] = eig(pdiffxy(:, :)) function Transf=t(theta); sqrtd=sqrt(d) Transf = eye(3); Transf(,) = cos(theta); a=-pi/; Transf(,) = sin(theta); dl= ra_to_dl(-, a, ); Transf(,) = -Transf(,); dr =ra_to_dr(-, a, ); Transf(,) = Transf(,); P = arc_cov(p, dl, dr, l, r, ) ;************************************ angle = angle a; function P=line_cov(P, l, r,, d); 9

20 %function P=line_cov(P, l, r,, d); C=eye(3); l = l*l; r = r*r; C(,) = d*(lr)/4; C(,) = d^*(r-l)/4/; C(,3) = d*(r-l)//; C(,) = C(,); C(,) = d^3*(lr)/3//; C(,3) = d^*(rl)///; C(3,) = C(,3); C(3,) = C(,3); C(3,3) = d*(lr)//; pheye(3); phi(,3) = d; P = phi*p*phi'c; function P=arc_cov(P, dl, dr, l, r, ); %function P=arc_cov(P, dl, dr, l, r, ); % update covariance matrix P for arc of left distance dl, and right dr % with robot parameters l, r, wheel base % Assumes that P is in tangential coordinates and is left in tangential % coordinates on return C=eye(3); = *(drdl)//(dr-dl); a = (dr-dl)/; sr = abs(*dr/(dr-dl)*r*r); sl = abs(*dl/(dr-dl)*l*l); sum = sr sl; diff = sr - sl; C(,) = sum**//*(3*a/- *sin(a)sin(*a)/4)sum/4*(a/sin(*a)/4) diff*/*(sin(a)-a/-sin(*a)/4); C(,) = sum*(*//*(3-4*cos(a)cos(*a))/4(cos(*a)- )/6)diff*/4/*(*cos(a)-cos(*a)-); C(,3) = sum*//*(asin(a))diff*sin(a)//; C(,) = C(,); C(,) = (a/- sin(*a)/4)*(sum*(*//.5)-diff*/); C(,3) = (sum*//-diff//)*(- cos(a)); C(3,) = C(,3); C(3,) = C(,3); C(3,3) = sum//*a; if (a < ) C = - * C; end % propagate P Pheye(3); Phi(,)=cos(a); Phi(,)=Phi(,); Phi(,) = sin(a); Phi(,3) = *(-cos(a)); Phi(,) = -sin(a); Phi(,3) = *sin(a); P = Phi*P*Phi'C; function dl = ra_to_dl(, a, ); %function dl = ra_to_dl(, a, ); %converts radius and angle to left distance dl dl = a*(*-)/; function dr = ra_to_dr(, a, ); %function dr = ra_to_dr(, a); %converts radius and angle to left distance dr

21 dr = a*(*)/; ;*********** esults from running UN.M run l =.e r =.e-3 D =.e-6 * = d =.67 angle =.e-3 * Pxy =.e-4 * P =.e-4 * Pxy =.e-4 * D =.e-4 * Pxy =.e-4 * D =.e-4 * Pdiffxy =.e-4 * D =.e-4 * Pdiffxy =.e-5 * D =.e-5 *

22 .9.9 Pxy =.e-4 * D =.e-3 * Pdiffxy =.e-5 * D =.e-6 * e-3 * diary run l =.e-3 r =.5 =.5 d = angle = Pxy =.e-3 * D =.e-3 * Pxy =.e-3 * D =.e-3 * Pdiffxy =.e-4 * D =.e-5 * P =

23 Pxy = D = Pdiffxy =.e-3 * D =.e-3 * Pxy = D = Pdiffxy =.e-4 * D =.e-5 * quit 59 flops. run l =.e-3 r =. =.5 d = angle = Pxy =.e-4 * D =.e-4 *

24 .74 Pxy =.e-4 * D =.e-4 * Pdiffxy =.e-5 * D =.e-6 * e-3 * P =.e-3 * Pxy =.e-3 * D =.e-3 * Pdiffxy =.e-4 * D =.e-4 * Pxy =.e-3 * D =.e-3 * Pdiffxy =.e-4 *

25 .e-3 * D =.e-6 * quit.65 6 flops Simulation esults Monte-Carlo simulations studies have been performed on the odometry error covariance estimation results for two main reasons. Firstly, there are approximations involved in the derivation of the results and the simulations will be able to verify that the approximations are reasonable. Secondly, the errors need to be eliminated from the theoretical derivations and the simulation can verify the correctness of the derivations. The simulation studies read a configuration file which specifies the left and right wheel movements defining the path, the wheel base, the wheel noise constants and and the sample frequency of the odometry position update. A noise free path is calculated first to determine the end point position and orientation. Statistics are calculated for errors in end points of paths where measurement noise is added to the wheel measurements on each sample. The error covariance matrix for all the samples is calculated and printed. Also the mean of the errors is calculated to determine any bias that has been introduced. A convenient form of the covariance matrix is also used whereby the square root of diagonal elements and the correlation coefficient of diagonal elements 3 are displayed. This allows easy comparison of the simulation results and theoretical results. An example output file corresponding to Figure 8 follows: Sampleate=., =.5, l=., r=., MonteCarlouns= time eftdist ightdist: number of samples: mean is: 3 The correlation coefficient is σ ij σσ i j and lies between - and. 5

26 -.9974e e e-5 covariance matrix is:.97987e e e e e e e e e-5 stand dev, correlation matrix is: ========== K theoretical cov matrix: 3.379e e e e e e e e e-5 stand dev, correlation matrix is: The results for 3 simulation runs are shown in Table III for the conditions matching Figure 8. The agreement between theory and simulation is within the statistical variation displayed by three runs. Theoretical results un ( samples) un ( samples) un 3 ( samples) TAE III - Monte-Carlo runs for = = 3 m, =.5 m Means Covariance Matrix Stand dev/ Corr Matrix -.997e-5 5.3e e-5.99e-5 -.7e-5-5.6e e e-5-4.5e-5 3.3e e e e e-5 4.7e e-5 4.7e e e e-5.76e e e e e e e e e e e e e e e e e e e e e e e e e

27 Three more runs were conducted with the conditions of Figure 9 and the results are summarised in Table IV: TAE IV - Monte-Carlo runs for =. m =., =.5 m Theoretical results un ( samples) un ( samples) un 3 ( samples) m Means Covariance Matrix Stand dev/ Corr Matrix -3.5e e e-5.374e e e-5.38e e e e e e e e e e e e e e e e e The theoretical results derived in Sections 3, 4 and 5 depend on the absolute values of the errors being small. To study the effect of larger errors, the noise variance has been increased to values of =. m =.. Table V summarises the results obtained. m TAE V - Monte-Carlo runs for =. m =., =.5 m Theoretical results m Means Covariance Matrix Stand dev/ Corr Matrix un ( samples) un ( samples) e

28 un 3 ( samples) Conclusion and Future Wor ased on a simple model for odometry errors in each wheel of a robot, results have been derived that describe the error statistics of robot trajectories in straight lines, arcs and turns. The results allow efficient computation of the odometry covariance matrix at the end of each path segment of the robot. Previous approaches rely on updating the covariance matrix after small time increments [Wang, 988 #87]. The model of odometry errors allows sensible fusing of localisation data from other sensors, since the relative errors of each can be assessed. A Kalman Filter can be employed in the merging process. The results of the covariance estimates for constant curvature paths can be applied to a varying curvature path by breaing the path up into small segments of approximately constant curvature. Simulation studies have been employed to confirm the mathematical correctness of the derivations and study the limitations of approximations employed in the derivations. Future wor will attempt to experimentally verify the results derived in this report using a mobile robot. A calibration technique for determining the error model parameters will be investigated. eferences 8