UM-MEAM-90-09 Algorithms for Fusing Absolute and Incremental Position Measurements for:.:\Mobile Systems L. Feng Y. Fainman Y. Koren Mobile Robot Laboratory Department of Mechanical Engineering and Applied Mechanics

( ~"tip Al v, ". * 41

Algorithms for Fusing Absolute and Incremental Position Measurements for Mobile Systems L. Feng Y. Fainman Y. Koren Mobile Robot Laboratory Department of Mechanical Engineering and Applied Mechanics The University of Michigan Ann Arbor, MI 48109-2125 Abstract In mobile system navigation, there are two kinds of position measuring methods: absolute and incremental. Each of the two position measuring methods has its advantages and its deficiencies. In this report, the sources of position measuring errors for each method have been analyzed. Four algorithms based on statistical analysis have been developed to fuse the absolute and incremental position measurements information to increase the positioning accuracy and reliability. The algorithms have also been evaluated by both computer simulation and experiments.

1 Introduction The navigation of mobile systems relies on the accurate position measurements. There are basically two types of position measuring methods, incremental and absolute. The incremental methods (e.g., encoders) are simple to use, and the position information is always available. However the positioning accuracy is affected by the terrain (slippage, uneven surface, etc.), and by the vehicle (difference in wheel diameter, wheel misalignment, and unbalanced load on the vehicle). The errors also accumulate over distance [1, 2]. On the other hand, the absolute position measuring method is not affected by the terrain and vehicle. A landmark based absolute position measuring system is simple to implement and operates fast, but the position information is not always available, the accuracy is limited by the resolutions of the devices and the relative position between the mobile system and the landmark [3-5]. We need both incremental and absolute position measuring systems in order to navigate the robot accurately and reliably. A effective way to increase the measurement accuracy and reliability is to weighted average multiple measurements for the same quantity [6, 7], in our case, the position coordinates. In this report, we will introduce several algorithms to fuse the absolute position measurement and the incremental measurement to get more accurate and reliable position estimate. In the next section, we will analyze the positioning errors for each method. In Section 3, we will develop the fusion algorithms. In Section 4, we will provide computer simulation and experimental results. The final conclusions are given in Section 5.

.2 Error Analysis In this section, we will analyze the error sources associated with both absolute and incremental positioning methods. The precision of landmarkbased absolute position measurement depends on errors caused by the measurement system and the relative position between the mobile system and the landmark. The accuracy of the encoder-based incremental position measurement accuracy is affected by imperfections of the mechanical system, as well as environmental factors. 2.1 Absolute Position Measurement Errors In our study, we used circular landmarks. A camera mounted on the mobile system take a picture of the landmark, the differences in both shape and size between the image and the real landmark will provide enough information to determine the relative position of the mobile system to the landmark. In this study, we only discuss the two-dimension case in which the axis of the camera lens lies in a plane perpendicular to the plane of the landmark and the center of the camera lens is at the same height as the center of the landmark, as shown in Fig. 1. In this case, we need only two coordinates to determine the relative position of the mobile system with respect to the landmark. The relative coordinates of the mobile system to a circular landmark (see Fig. 2) are determined by fR Ay

forV 2 r-2-' cos a = 2Ar R (1) 2AR where r and Ca are the polar coordinates of the mobile system with respect to the landmark, AX and Ay are the axes of the ellipse in the image plane, R is the radius of the circular landmark, and f is the focal length of the TV camera lens. The derivation of Eq.( 1) is given in Appendix A. The measurement error is mainly determined by the digitization error, which occurs due to the limited resolution of the TV camera and other devices. It is dominated by the lowest resolution of these devices. The minimum measurement error due to digitization can be determined from the size of the detected image and the detector resolution. For example, for a TV camera that permits the resolution images the size of 512x512 pixels, this error is on the order of 1/512 = 0.002. But if the image does not occupy the full TV frame, the measurement error is larger. The measurement error is also affected by a sensitivity function, that is defined as the ratio of the change in the image parameters to the change in the position of the mobile system. The sensitivity is not fixed, and it depends on the relative location of the mobile system with respect to the landmark, namely, the distance r and the angle of view a. In general, r and a are functions of Ax and Ay (see Eq.( 1)); therefore, the positioning error of the distance Ar and the orientation Aac can be expressed by Or Or Ar = A9A + O A,/A AOA + A

Aa = -SAA + oA AAy (2) where AA, and AA, are the measurement errors of the two axes of the ellipse and are determined by the digitization error. In real situations, the digitization error is a function of the relative position and the resolution of the devices. The coefficients or ar oa and a0 are the inverse of the OAA,' A OA' 04t aAy sensitivities, which, in turn, are functions of the relative position of the mobile system to the landmark. From Eq.( 2), we observe that the measurement error increases with an increase in the digitization error and a decrease in sensitivities. The sensitivities can be determined from Eq.( 1) which may be rewritten in the following form: fRr cos a r2 - R2 sin2 c Ay f(3) r The sensitivity functions of the relative position of the mobile system with respect to the landmark are derived from Eq.( 3): aAX -frR sin a(r2 - R2 - R2 cos2 a) OCr (r2 - R2 sin2 a)2 aA_ -fR cos a(r2 + R2 sin2 a) Or (r2 - R2 sin2 (4))2 = 0 Oa~~~~~~~

OAy -fR Or r2 From Eq.( 4), we can observe that all the sensitivity functions decrease with an increase in r. Moreover, when r > R, i.e., the distance between the mobile system and the landmark is much greater than the radius of the circular landmark (this is usually the practical situation), the first two functions become aA, -f R sin a Oac r OA4 -fR cosa (5) Or r We can see that if r is kept constant, the sensitivity with respect to a increases with an increase in a, and the sensitivity with respect to r decreases with an increase in ar. Substituting Eq.( 5) into Eq.( 2) yields the approximated positioning error: r2 r Ar - AAx -AA fR cos a fR fR sin a Assuming we have constant digitization errors AA. and AAy, we see that for a mobile system moving away from the landmark under a certain orientation, the measurement errors Ar and Aa increase due to the increase in r. If we keep a fixed distance between the mobile system and the landmark, but change the angle of view a, the measurement error Ar increases with an increase in ca, and Aa decreases with the increase in a. At a = 0~, there is

no information on a; therefore, an error in a tends to infinity, i.e., Aa — 4 o. Similarly, at a = 900, Ar -- oo. To improve the measurement accuracy, we will introduce a statistical model for the digitization error by assuming the image parameters are random variables that possess a normal distribution. The probability model will provide a reliability measure for the measurements and will allow us to employ statistical techniques to improve the navigation of the mobile system. 2.2 Measurement Reliability The reliability measure is a function of the digitization error and the sensitivity. The errors in determining the coordinates of the mobile system depend on the digitization error and the sensitivity, according to Eq.( 2). By assuming that Ax and Ay are normally distributed independent random variables, we then know the linear combination of Gaussian variables is also a Gaussian random variable, and the variances of the coordinates are related to the variances of A, and Ay by [6] Or Or 2 (r )2. +( 2 2 2 Oa 2 2 a 2 2 a = (OA )2A. + (A) O'A (7) where a'2 and a 2 are the variances of the two axes of the ellipse, and ar2 and a2 are the variances of the coordinates of the mobile system. The values of the coordinate variances ar2 and a2 can be used as measures of reliability.

2.3 Incremental Position Measurement Errors The positioning accuracy of incremental positioning is affected by factors of the mobile system and the environment. For wheeled mobile systems equipped with incremental incremental encoders, position errors exist due to the different diameters of the wheels, misaligned wheels and asymmetric vehicle loads. The effect of these factors is that the mobile system moves on a curved path when it is instructed to move along a straight line path. In addition, the floor conditions may be different for different wheels, slippage may occur for one or more wheels. The positioning errors caused by these factors can not be detected by the encoders, and the errors accumulate over distance. Thus an absolute position measuring method becomes essential for accurate and reliable navigation. The effect of these errors will be included in the computer simulation. 3 Algorithms for Improved Performance In this section, we will show how to take advantages of both absolute and incremental positioning information to fulfill the two basic requirements for navigation, accuracy and reliability. As mentioned earlier, these two requirements can be achieved by averaging multiple measurements of the same quantity. However navigation is a real-time process, the absolute position measurements need processing time. It is not practical to have several absolute position measurements at the same spot without stopping the mobile system unless the mobile system has several measuring devices. However the incremental position measurements are rather accurate over short dis

tance, we can use this information to project the previous absolute position measurements to the current position. In the following, we will develop algorithms that use the present and previous absolute position measurements using landmarks as well as the incremental position measurements from the encoders to improve the accuracy and reliability of navigation. When the mobile system is instructed to move, measurements are taken at different points along the trajectory. At each point, a new estimate of the current position is obtained based on new as well as previous measurements. Each previous measurement is projected to the present vehicle location according toe following equation: n-1 x(n) =xi) + E Ax+l (8) 3=, where xi') is the landmark measurement at the i-th point, Ax'+' is the increment from the j-th point to the (j+l)-th point, which is obtained from the readings of the on-board encoders, and x(n!") is the projection to the n-th point from the measurement at the i-th point. In developing the navigation algorithms, we will consider two errors, the absolute landmark measurement error and the incremental position measurement error. The measurement error is caused by the limited resolution of the devices and is sensitive to the location of the mobile system. In order to increase the robustness and positioning accuracy of the mobile system, we will use algorithms based on a weighted average method [7]

(n) = Z, (9) i:=1 Wi where x(n) is the new estimate of the current position at the n-th point, x2 is the predicted n-th point obtained from the i-th measurement according to Eq.( 8), and wi is the weight that gives the optimal estimate (see Sec. 3.1). The weighted average is used to fuse the information from both the new measurements and the projections from previous measurements (see Eq.( 8)) to get a better position estimation. Eq.( 9) can be also rewritten in the recursive form: (n) (n) = + wn( - _ n$[) (10) where ((n)l is the projection from the last estimate, and x" is the new measurement. In our discussion, x(n) will include two variables, f(n) and a(n). To counteract for the incremental position measurement error, we must realize that the errors caused by the imperfection of the mobile system itself and the environmental factors accumulate over distance. In order to avoid the buildup of large errors, we use a moving window to include only the most recent measurements into the weighted average process (see Sec. 3.2). In Sec. 3.3, we will discuss the situation where both absolute and incremental position measrement errors present. 10

3.1 Compensating for the Absolute Position Measurement Error due to Sensitivity If the incremental positioning errors are small, the major source of error will be the absolute positioning error. We will develop algorithms based on the weighted average of Eq.( 9) to compensate for the absolute measurement error based on different optimization criteria. 3.1.1 Algorithm with Weights Based on Sensitivity The sensitivity of measuring the landmark parameters depends on the location of the mobile system, as shown in Eq.( 4). In general, the accuracy of the measurement increases with an increase in sensitivity, i.e., a measurement obtained in a high sensitivity region is more reliable and should be assigned a larger weight. From Eq.( 2), we can see that if we assume a constant digitization error, the measurement error depend only on sensitivities. Thus, selecting the weights of the weighted average based on sensitivities might be an appropriate approach under the assumption of a constant digitization error (an assumption that is hard to justify). The sensitivities given in Eq.( 4) are used as the weights in Eq.( 9). Since aA = 0 and -8 > 0,A the dominant sensitivity factors become and 0A a -- si ar tiy Or accordingly. We take wu = %9 and tr = A, and the estimated location of the mobile system is calculated using the following equations: -(n) Sl=1 (Wr)irT E?- (w,1)i 11

c(n) - Si (Wa)ia (11) where f(n) and a(n) are the estimated distance and the orientation of the mobile system, respectively; (wr)i and (wa)i are the sensitivity functions determined from Eq.( 4) and evaluated at the i-th point, and rt and and are the projections for the coordinates at the n-th point, which are defined by equations similar to Eq.( 8). Eq.( 11) can be expressed in the following recursive form: 7(f) = (n + (wr)nr$?)r 1 + (Wr)i C(n) = +(n) (Wor)n(an) -_ (n-) (12) where r(n) and ac(n) are the new measurements, and r(n)l and (n-) are the projections from the last estimate. An evaluation of this algorithm and comparisons to those presented below are given in Section 4. 3.1.2 Minimizing the Variance of the New Estimate The algorithm described by Eq.( 11) is not based on an optimization procedure. In contrast, our second algorithm is based on minimizing the variance of the new estimate. Assuming that XIn) in Eq.( 9) are independent random variables, we can determine the variance of every new estimate by [7] [~ = E,-~(w,~ (13) 12

where Ua(n) is the variance of the projection of r from the i-th point, and n) is the variance of the new estimate of the distance at the n-th point. In order to minimize the variance, we must satisfy the relation r)i = 0. (14) (w,.)i Substituting Eq.( 13) into Eq.( 14) and solving the resultant equation, we obtain the optimum weights [7]: 1 (W,)i C (15) A similar relation can be obtained for the second coordinate (i.e., angle of view a ): (w,)i oc;-. This result can also be obtained by applying a a(n) Kalman filter to this simple situation [8]. 3.1.3 Minimizing the Mean Square Error of the New Estimate The third algorithm is based on minimizing the mean square error of the new estimate. The mean square error at the n-th point can be expressed by n J - ( -(")- r(n))2 (16) i=l where f(n) is the estimate for the n-th point given by Eq.( 9), and r(n) is the projection to the n-th point from the i-th point. To minimize the error, we have to satisfy the following equation: 13

-= 0 (17) OWi where i = 1,...,n - 1. Substituting Eq.( 16) and Eq.( 9) into Eq.( 17), we obtain Wi(rn)- r)) - 0 (18) i-=1 where j = 1, 2,..., k-1, k + 1,..., n - 1. Assuming a certain value for the k-th weight, wk, we can rewrite the last equation as [ 0 r(n) (n) (n) (n) r(n) (n) (n) (n) (n) () (n) w: -r2 0 r3 -r2... r -.11W2 9 - r$ rn) r... 0 Wn] I (n) (n) I Wkr2 - (19) rn) (n) By solving this system of linear equations at each point, we find the optimal weights for minimizing the mean square error of the new estimate at this point. To avoid the trivial solution of this homogeneous linear system, we must first pick one weight. The weight in the highest sensitivity region is assigned a weight of 1, or Wk = 1. The derivation of Eq.( 19) is given in Appendix B. 14

Considering the real-time requirements of the navigation problem, we adopt the recursive least-squares algorithm for Eq.( 19)[9], which solves the least-squares problem approximately by using a recursive relation at each point: ((n) (in) + n(rn ) - n- +'Pn-1 Pi = Pi-1 l + aP (20) where r(n) is the new measurement, r(n) is the projection from the last estimate, r(') is the new estimate, ai is the weight, and Pi is a variable gain, which can start with an arbitrary positive value. Equations similar to Eqs.( 19) and ( 20) can be obtained for a. 3.2 Compensating for the Incremental Positioning Errors In the algorithms discussed in Sec. 3.1, we used the previous absolute measurements and the encoder readings to estimate the new positions. However, as discussed in Sec. 2.3, there could be a large accumulation of errors in the encoder readings due to the mechanical and terrain factors (e.g., different sized tire, slippery floors, or misaligned wheels). To reduce the effect of incremental error accumulation, we adopted the strategy of introducing a forgetting factor. With this strategy, the effect of the previous measurements on the current position estimate diminishes over the distance. In this study, we use the weighted average of the current measurement and the projected 15

estimate from the last measurement: -(n)._r~nn + (W(n) n-1) + A,n-1) -~n) ((w; ) n + ( _)I_~ (Wu)n + (t)n (21) where r(") is the new estimate at the n-th point, r(n) is the new measurement at the n-th point, Arnr1 is the increment of variable r from the (n-1)-th point to the n-th point, (wI)n is the weight for the new measurement and (i)n_-1 is the weight for the projection from the last estimate. Similar equations can be obtained for a. To show the effect of the forgetting factor, we substitute 7(n-l) r(n-2),..., into (n), which yields n) (Wtr)nr~nn) _.(Wt)n —/'n — 1 ( w4)n +) (,)_ (w) + (,)_n(n-1) Wr-')n-l (Wr)n-lrn-l1 + ((,) r r ((;).n + (t,4)n-.)((wu)n-1 + (tW,)n-2) AT,-12r/~ n-1 +((, (Wr,)n)- (r_)n-2.Arn-2 ((w,)n + (;r)n-_)((W, )n-1 + (f;)._2) (2'rn)n-1(Wr), n — 2''- (Wr )n-k(Wr)n-kr(n-k) ((Wr)n + (WLr)n-i)((Wr)n-1 + (Wtr)n-2)... ((Wr)n-k + (Wtr)ni(k+l)) r ** )n-k(tV)n-(k+l)rn-k -' n-2 r r n-(k+l)'v((Wr)n + (tr)n-1)((Wr)n-_1 + (rV)n-_2)... ((Wr)n-k + (iVr)n-(k+l)) +... (22) We can observe from Eq.( 22) that the new estimate, obtained from the 16

weighted average, contains information from all the previous measurements, and the effect of the increment that contains the mobile system error is multiplied by a number smaller than one, so its effect diminishes with each step. 3.3 Compensating for both Absolute and Incremental Positioning Errors In this section, we will treat the general problem with both measurement and mobile system errors. We will use a weighted average for two successive measurements and select the weights such that the resultant new estimate will have the minimum variance. If we assume that the random variables r and Ar are independent, we have.n W (ntr l (f(n —1)' /rn (+ (tr)( + inn- l (23) )( )2. + (Ar )l (24) w ). n+ (i,)_ where ak2n) is the variance of the new estimate, 2(n is the variance of the r-(") rn new measurement, a.2_,) is the variance of the estimate at the previous position, and,2 rn is the variance of the incrementArn1. To minimize the variance of the new estimate, we compute the derivative of an2 with respect to (t' )_,- using Eq.( 24): 17

aevv7 -0. (25) We solve Eq.( 25) for ()'r)n-1 and obtain (w'r)n(n) ()n-l a= 2 2( (26) By substituting Eq.( 26) into Eq.( 24) and Eq.( 23), we obtain (c2l1) + ar2 n )2() (27) 2 r-( - Arn2 da rn (27) =(n) 2(. +, rn + 2 (2) f(n) -(u.) + )r(n) + + Arnr 1) (28) A-n-1) +.7 &r (2_) -- rn We can see from Eq.( 28) that the weights are still inversely proportional to the variance. When the new absolute measurement is more accurate (i.e., a(n) < a.n-1~ + ar2 ) a larger weight is put on the new absolute measurement. Otherwise, a larger weight is put on the projection from the last estimate. In the ideal situation, i.e., r2 = 0, (k = 0,,..., n), all the measurements are of the same accuracy, ua2(k), (k = 0, 1,..., n), and the best estimate should be the arithmetic average. We can show that under these assumptions, Eq.( 28) indeed will reduce to the simple arithmetic average, as detailed in Appendix C. 18

4 Simulations and Experimental Results We have performed computer simulations and experimental evaluations of the proposed hybrid opto-electronic navigation system using the different algorithms introduced in Sec. 3. The results are summarized below. 4.1 Computer Simulations The objective of this computer simulation is to verify the effectiveness of the algorithm developed in Section 3.3 when both measurement and mobile system errors are present. In the simulation, the mobile system moves along a straight path starting from (500, 2000) with incremental steps of 200. Measurements are taken at each step. At the target point, a new estimate of the current position is obtained using the new as well as the previous measurements with the weighting factors of the different algorithms. The data from the encoders of the mobile system are also used. The final correction is then made toward the target based on the new position estimation. We introduced a slippage error of 10% for two simulation cases: (A) a mobile system error occurs only at the second step; and (B) a mobile system error occurs only at the 10th step, where the total number of steps is 12. We have chosen these two cases to demonstrate the effectiveness of the algorithm in controlling errors that occur at the beginning of the route and at its end. The results of the computer simulation for the two cases are summarized in Figs. 3A and 3B, respectively. In each case, errors at the target point are compared for three navigation strategies: (a) motion based on the new absolute measurement; (b) motion based on the algorithm that minimizes the

variance of the new estimate without using the forgetting factor ( Sec.3.3); and (c) motion based on the algorithm in (b) but using the forgetting factor. From the results in Fig. 3, we observe that (1) the algorithm based on minimizing the variance of the new estimate with forgetting factor gives the best performance; (2) the mobile system error has a significant effect on the position estimation; and (3) the forgetting factor is an effective way of compensating for error accumulation. 4.2 Experimental Evaluation We have also conducted experiments to evaluate the different position estimation algorithms developed in Sec. 4.1. In our experiments, a mobile TV camera (with a resolution of 256x256 and a lens focal length of f = 16mm) was used at each measurement point on a desired trajectory to acquire an image of a circular landmark of radius R = 107mm. The camera usually recorded an image of an ellipse. The parameters of the ellipse were determined from the image. The parameters of the ellipse were then used to determine the real position of the mobile system by employing the different algorithms without introducing any incremental positioning errors. There are several error sources involved in our experiments: camera positioning errors (e.g., the positioning accuracy of the mobile TV camera is on the order of 1 mm), parameter measuring errors (e.g., digitization error), and calibration errors. The path taken in our experiment is a straight line path from (990, 2196)

to (1705, 1475). We have plotted the position estimation error of two points on the trajectory to compare the three algorithms. The results from computer simulation and experiment are represented in Figs. 4A and 4B, respectively, along with the results of the absolute measurement. From Fig. 4, we can observe that (1) the algorithm based on minimizing the variance of the new estimate gives the best performance, and (2) all algorithms give a more accurate estimate of position than the absolute measurement. 4.3 Discussion of the Results The computer simulation and the experimental results were found to be consistent. In the case of selecting the weights by minimizing the variance of the new estimate, we have considered both the sensitivity and the digitization errors by assuming a certain statistical error model. In most cases, it gives the best result. In the case of weighted average based on the sensitivity, we select weights based on sensitivity without explicitly using the digitization error model. A forgetting factor is introduced to compensate for the mobile system errors. From the resu-lts in Figs. 3 and 4, we can conclude the following: 1) Each of the three navigation algorithms improve the navigation performance of the mobile system. This improvement will be even further enhanced when the distance traveled is increased. 2) The algorithm based on minimizing the variance of the new estimate with the forgetting factor performs the best in the presence of both absolute and incremental positioning errors.

3) The algorithm based on minimizing the variance of the new estimate is the most efficient in compensating for absolute positioning errors. 4) The experimental results are consistent with the computer simulation, considering the degree of randomness of the position estimation process. 5) The computer simulation and the experimental results show that the performances of the mobile system depend on the particular region passed by the mobile system as well as the locations where the measurements are made. 5 Conclusions In this report, we analyzed different error sources that affect the navigation of the mobile system. To assure robust and accurate operation of the mobile system, we developed several algorithms based on different optimization criteria. Computer simulations and experiments were conducted to evaluate the performance of different algorithms. The results of the computer simulation and the experiments are consistent. The best navigation performance of the mobile system is obtained with the algorithm based on minimizing the variance of the new estimate with a forgetting factor. Acknowledgment: This research is supported by CAMRSS (a NASA Center for Commercial Development of Space) and by the Department of Energy grant DE-FG02-86NE37969. Discussions with Dr. Johann Borenstein, The University of Michigan, are greatly appreciated.

6 References [1] J. Borenstein and Y. Koren, "Motion Control Analysis of a Mobile Robot," Journal of Dynamic Systems, Measurement, and Control, Vol. 109, pp. 73-79, June 1987. [2] J. Borenstein and Y. Koren, "A Mobile Platform for Nursing Robots," IEEE Trans. on Industrial Electronics, Vol. 32, pp. 158-165, May 1985. [3] M. R. Kabuka and A. E. Arenas, "Position Verification of a Mobile Robot Using Standard Pattern," IEEE J. of Robotics and Automation, Vol. RA-3, pp. 505-516, Dec. 1987. [4] I. Fikui, "TV Image Processing to Determine the Position of a Robot Vehicle," Pattern Recognition, Vol. 14, No. 6, pp. 101-109, 1981. [5] Y. Fainman, L. Feng and Y. Koren, "Estimation of Absolute Spatial Position of Mobile Systems by Hybrid Opto-Electronic Processor," Proceedings of 1989 IEEE Int. Conf. on Systems, Man and Cybernetics, Vol. 2, pp.651-657, Nov. 1989. [6] H. Stark and J. W. Woods, Probability, Random Processes, and Estimation Theory for Engineers, Chs. 2 and 3, Prentice-Hall, Englewood Cliffs, New Jersey, 1986. [7] L. G. Parratt, Probability and Experimental Errors in Science, Ch. 3, John Wiley & Sons, Inc., New York & London, 1961. [8] H. V. Poor, An Introduction to Signal Detection and Estimation, Ch.

5, Springer-Verlag, New York, 1988. [9] G. C. Goodwin, and K. S. Sin, Adaptive Filtering, Prediction, and Control, Ch. 3, Prentice-Hall, Englewood Cliffs, New Jersey, 1984.

A The Relationship Between Circular Landmark Image Parameters (Ar, Ay) and Relative Positions (r, a) in 2-D Case. E B 1/ ~ D Hi R C O R Bi Fig. 5a H Fig. 5b From Fig. 5a, since triangles AODE and AOCiDi are similar, we obtain A- =+RsnO; AOFB is similar to AOCB,, thus, I = r-Rs. Since A, = C2D +Ci, we obtain frR cos a A = 2- R2sin 2 (A.1) AOGH and AOGiHi of Fig. 5b are similar, so that A 2R, or fR Ay = (A.2) From Eqs. (A.1) and (A.2), we get Eq.( 1). 25

B Mathematical Development of the Algorithm Based on the Least- Squares Error The mean square error can be expressed as (n nn 2rin) J = ( - = -) - ))2 ((Wn E=r) i )2 i=1 1=1 Z'=i(W r)i To determine the minimum, we should set the derivative of J with respect to wk to zero: aJ _wk that is E(r )-,)) Wi o. i=1 What we have is a homogeneous linear system of equations; to avoid a trivial solution, we have to pick one weight first. Here we take the measurement in the highest sensitivity region as having a weight of 1. In order to achieve real-time operation, we can also use the recursive method to solve the equations approximately. C Special Case of Eq.( 28) If we assume that there is no error in the encoder or no error in the increment a2_rk = 0 (k = 0, 1,..., n), and all the absolute measurements are 26

of the same accuracy, i.e., a2(k = a2 (k = 0, 1,..., n), then from Eq.( 27), we have 2, 2 n-1) + a )no.2(rfn) -(2 -1) 2 O' (nn)) + %fl + Ur-n) -nn1) + a or O'rn) o.2 2 2 a2 2 1 (n-l) a2 1+2xl At the' beginning, we take cr-o) -.O2; then, we have a2 2 1 1+ 1+2x(n-1) )2 l+(n- 1) x 1 2 1 C02 l+nxl From Eq.( 26), we know (72.) 2 - (Wr)n anl a(n-l) (Wr)n_1 O(n) 2 1 + (n-1)x 1 27

In general, (W)= - - i = 1,2,..., n. From Eq.( 23), we get (r)n._ - 1l+(n-1)X1,'" ()(n) - (wnr )(-) +Arn r ) ) (Wr')nr + (17n-1) + - + 1+ )n 1+ () r) (0)n-~ (U(W ) )n-n r(n) r( n-) + arn If we substitute f(n"-) - (W')n-'r"- +(^)n-2(r —(w-2)+r2) ill h (n) in) r(n 1) + Arn r(n-2) + Arn-2 1+n l+n 1+n We can indeed see that under special assumptions Eq.( 28) gives the simple arithmetic average, which is an optimal estimation under the assumptions we made. 28

Camera Landmark Mobile System Fig. 1 A mobile system and a landmark Circular landmark with Ar Yim/ Ay Normal to the -cmera with lensdmark Xim TV-camera with lens Magnified image focal length f plane ( v Mobile system Fig. 2 Description of the relative spatial position of a mobile system circular landmark. 29

0 0 (a) (b) (c) (a) (b) (c) (A) (B) Fig. 3 Comparisons of computer simulation of the navigation errors for different error correcting criteria: (A) mobile system gives an error only at the second step; (B) mobile system gives an error only at the 10th step, where the total number of steps is 12. In each plot, from left to right, motion is based (a) only on the absolute measurement; (b) on minimizing the variance of the new estimate without using the forgetting factor; and (c) on minimizing the variance of the new estimate using the forgetting factor. compute simulation rmult experimental results X,~ g so 0...~... ~% ~~.. ~~..0 (A) (B) Fig.4 Comparisons of error from the real position for different position estimation algorithms: (A) computer simulation; (B) experiment. At each point, from left to right, the algorithms are based on (a) absolute measurement; (b) weighted average based on sensitivity; (c) least-squares method; and (d) minimizing the variance of the new estimate. 30

UNIVERSITY OF MICHIGAN 3 9015 02229 1499