RELATIVE POSITION ESTIMATION FOR INTERVENTION-CAPABLE AUVS BY FUSING VISION AND INERTIAL MEASUREMENTS

Transcription

1 RELATIVE POSITION ESTIMATION FOR INTERVENTION-CAPABLE AUVS BY FUSING VISION AND INERTIAL MEASUREMENTS Anreas Huster an Stephen M. Rock Aerospace Robotics Lab, Stanfor University Abstract This paper proposes a position estimator that fuses monocular vision with accelerometer an gyro measurements to generate a irect, relative, 6DOF position estimate between an Autonomous Unerwater Vehicle (AUV) an a stationary object of interest. This type of estimator is useful for intervention-capable AUVs, which nee to control their position relative to objects in their environment in orer to accomplish tasks like stationkeeping, vision-base object moeling/reconstruction, an manipulation. Various vision-only systems have been use to estimate relative position, but these are often ifficult to implement in real unerwater environments. The system we propose relies on vision to generate relative position information, but also fuses inertial rate sensors to reuce the amount of information that nees to be extracte from the vision system. The result is a system that is simpler an more robust than a vision-only solution. However, the use of inertial rate sensors introuces several issues. The rate measurements are subject to biases, which nee to be estimate to prevent the accumulation of unboune rift as the measurements are integrate. The observability of state estimates requires sufficient camera motion. Finally, the estimation problem contains significant nonlinearities. The paper compares two non-linear estimation techniques, the an the two-step estimator, in the context of this estimation problem an presents a simulation of a simplifie problem to highlight the avantages of the two-step estimator. 1 Introuction This paper escribes a position sensing system for intervention-capable AUVs. This is a new class of AUV, with unique sensing requirements, that will be able perform various autonomous manipulation tasks, such as placing sensors an retrieving samples. These are complex tasks compose of moeling, planning an execution phases uring which the AUV requires accurate control (or at least knowlege) of its position an orientation relative to the object of interest. If the AUV cannot lock its position (e.g., by thrusting into the sea floor) uring an autonomous manipulation task, it requires a real-time estimate of the relative 6DOF (egrees of freeom) position between the AUV an the object of interest. This paper proposes a sensing strategy that enables AUV control relative to objects in its environment. Some AUVs alreay perform relatively simple intervention tasks, like sampling of volcanic glass with the Autonomous Benthic Explorer (ABE) [11 an ocking with the Oyssey AUV [3, 9. More complex intervention tasks, such as grasping of unknown an unprepare objects, are much more ifficult to implement. Several position sensing systems have been use to control AUVs. Sonar beacons eploye an surveye prior to the AUV mission, as well as Inertial Navigation Systems (INS) couple with perioic position fixes to correct for accumulate rift (e.g., from GPS), have been use for AUV navigation. Simultaneous Localization an Mapping (SLAM) is useful when no external position measurements are available [2, 1. Several visionbase positioning systems have been emonstrate for control of AUVs relative to planar surfaces [6, 7, 8. However, autonomous manipulation tasks require a irect 6DOF position measurement of the object of interest. Vision-base techniques (e.g., binocular stereo or structure from motion) provie relative position information an are often use for manipulation tasks. But most vision-only 6DOF positioning algorithms are ifficult to implement in real unerwater environments because they require a large number of trackable image features or rely on feature corresponences in multiple cameras. Unerwater manipulation tasks may occur in environments with only a small number of goo visual features, given the constraints on lighting an visibility impose by real unerwater environments. Robust tracking of features an reliable corresponences are ifficult to achieve. The system we propose relies on vision to generate relative position information, but also fuses inertial rate sensors to reuce the amount of information that nees to be extracte from the vision system. The result is a

2 system that is simpler an more robust than a vision-only solution. Our system requires only a bearing measurement to a fixe point (usually associate with the object to be manipulate) which can be obtaine by tracking a single visual feature. Compare to the vision-only algorithms, tracking only the best feature in a monocular vision system can be much faster an more reliable. The measurements from monocular vision an inertial rate sensors complement each other well. The motion of the camera between successive images generates a baseline for range computations by triangulation. Inertial rate sensors, whose acceleration an angular rate measurements can be integrate to obtain vehicle velocity, position an orientation, can account for the 6DOF motion of the AUV along this baseline. When these measurements are fuse, the relative position between the AUV an the object can be compute. A key benefit of this system is that, after initialization, the inertial rate sensors continue to maintain a useful estimate of relative position uring vision rop-outs (e.g., occlusions, lack of corresponence). Furthermore, both inertial rate sensors (for navigation) an monocular vision systems (for science purposes) are alreay common AUV sensors. However, the use of inertial rate sensors introuces a major issue into the esign of the system. Like other ea-reckoning sensors, inertial rate sensors suffer from bias an ranom noise errors, which lea to unboune rift in the integrate quantities. While more expensive sensors are associate with less rift, we envision the use of low-cost inertial sensors, which are subject to significant rift errors. Therefore, an estimator to resolve the relative AUV position as well as the sensor biases an rift errors is require. Observability of the states to be estimate is a critical issue because the problem is non-linear an observability epens on the motion of the camera. During camera translation irectly towars or away from the feature, the estimator has no new information with which to improve its range estimate. Only camera motions transverse to the feature irection provie new information for the range estimate. As a result, the estimator requires sufficient transverse camera motion in orer to prouce useful position estimates. In some cases, extra camera maneuvers are require to improve observability. This presents an interesting conflict between trajectories which are esigne to complete the manipulation task an special camera maneuvers require to ensure estimator observability. The estimation problem contains two significant nonlinearities. The first is relate to the rotational egrees of freeom of the AUV an the secon is cause by the camera s projection of the three-imensional worl onto the 2D image plane. As a result, the ynamics an measurement equations are non-linear an epen on the actual Figure 1: Relative Position Estimator Scenario state of the system. In fact, the estimator exploits the non-linearity of the problem to observe the range to the feature. As motion of the AUV moifies the system state, the measurement equations change, an new measurements (i.e., bearings to the object from new viewpoints) make the range to the object observable. Various methos exist to hanle non-linear estimation problems. In this paper, we consier estimators base on extensions to the popular Kalman Filter. The most wiely use is the Extene Kalman Filter (), which linearizes the ynamics an measurement equations of non-linear systems in orer to take avantage of the Kalman Filter equations. Although the works very well for a wie range of applications, it comes with no guarantees an can lea to very poor performance. An alternative to the is the two-step estimator [5, which reformulates the problem so that all the measurement equations become linear an all of the non-linearities appear in the ynamics. At the expense of ae complexity, the two-step estimator can prouce much better estimates. This paper compares the ability of the an the two-step estimator to fuse monocular vision an inertial rate sensors an to estimate relative position. We will erive an an two-step estimator for a simplifie (2D) problem an present a simulation that examines the accuracy of the state an covariance estimates an the tenency of the estimators to iverge. The simulation inicates that in the context of this problem, the two-step estimator performs much better. The rest of the paper is organize as follows. Section 2 efines the estimation problem in terms of the system states, ynamics, measurements, an noise moels. Section 3 shows how to construct an an a two-step estimator to solve the 2D version of this problem. Section 4 presents an compares the results from a simulation of both estimators. Section 5 states our conclusions an outlines future work.

3 Process Noise w r Trajectory Generator Disturbance Dynamics x x e u u w Measurement Noise v a,ω v s p C + - AUV Control AUV x Camera q x ˆx ek f inertial rate sensors z a,ω z s O ˆx two Figure 3: Geometry of the Estimation Problem Two-Step Estimator Figure 2: Block Diagram with AUV Controller, Sensors, an Estimators 2 Problem Statement This section efines the geometry of the estimation problem, the sensor measurements use to upate the state estimate, the coorinates that represent the state of the system, an the ynamics associate with these states. Figure 1 shows an intervention-capable AUV which has extene its manipulator arm to grasp a stationary object in the environment. For the grasping task to succee, the AUV requires a relative position measurement of the object in AUV coorinates. A camera on the front of the AUV is tracking the object an inertial rate sensors are reporting the AUV s acceleration an angular velocity. The block iagram in Figure 2 shows how the AUV, its controller, sensors, estimators an the environment interact. The camera measurement z s an inertial rate measurements z a an z ω, along with the control commans u are passe to an estimator, which computes a state estimate ˆx. The ifference x e between the esire trajectory x an the state estimate is use by the controller to generate the next u. The control comman an any isturbances u w rive the AUV ynamics to generate anewstatex. In general, x is not available for AUV control. The iagram shows a ashe line to inicate that this truth measurement can only be use for simulations. 2.1 Geometry The essential geometry of the estimation problem is represente in Figure 3. Frame O inicates the inertial reference frame an p inicates the position of the stationary feature. Let q be the position of the camera, which is attache to the boy frame C. In principle, the camera an the inertial sensors can be mounte anywhere on the AUV as long as their relative position an orientation are known. To simplify the iscussion, we assume that all the sensors are colocate at q. R oc is the rotation matrix from the camera to the inertial frame an C ω is the associate rotational velocity expresse in the camera frame. We use a preceing superscript C to inicate that a vector is resolve in Frame C instea of Frame O. The position of the feature as seen by the camera is r = p q. We assume that the feature is stationary in the inertial frame, so ṗ = p =. Therefore, ṙ = q an r = q. Because of this assumption, a measurement of AUV acceleration q in inertial space is useful for estimating the relative feature position r. 2.2 Measurements The vision measurement z s provies the projection of C r onto the image plane, an is moele as follows: [ [ sx C r z s = = x / C r z s C y r y / C + v r s z where v s is zero-mean Gaussian noise N (,R s ). For simplicity, we assume that the camera measurement is scale so the effective focal length is 1. With a slight abuse of terminology, we will refer to C r z as the range to thefeatureantos x an s y as the bearing. The optical axis of the camera is aligne with the z-axis of Frame C. While vision processing for unerwater environments remains a challenging problem, our current research oes not expan on it. Instea, we assume that a robust point-feature can be tracke, an focus instea on integrating this type of measurement into a position estimator. The accelerometer measurement z a inclues the acceleration C q of the AUV, the acceleration ue to gravity,

4 asensorbiasb a, an sensor noise. z a = C q + R co g + b a + v a We assume that v a is N (,R a ) noise. R co = R T oc is the rotation matrix from inertial to camera coorinates an g = [ g T is the acceleration ue to gravity in inertial coorinates. The gyro measurement inclues the rotational velocity of the AUV,sensorbiasb ω, an sensor noise v ω moele by N (,R ω ). 2.3 States z ω = C ω + b ω + v ω The state of the system is compose of the coorinates that are use to escribe AUV position an to express the AUV ynamics an sensor measurements. These inclue the position q, velocity q, an acceleration q, acceleration isturbance a w, a quaternion λ to represent orientation, angular velocity ω, as well as the inertial rate sensor biases b a an b ω. 2.4 Dynamics We assume simple 1/s 2 ynamics for vehicle position an orientation. In inertial coorinates, these are: t q = q t q = q = a w + R oc (λ)u 1 t a w = w 1 t λ = E (λ)ω t ω = R oc(λ)u 2 + w 2 t b a = w 3 t b ω = w 4 u = w = [ u1 u 2 w 1 w 2 w 3 w 4 The eterministic control input u is compose of the translational component u 1 an the rotational component u 2, both of which are expresse in AUV coorinates. The isturbances (e.g., ocean currents) an sensor biases (e.g., temperature epenence) are riven by the process noise w, which is zero-mean Gaussian noise N (,Q). We have chosen arbitrarily simple ynamics to moel the isturbances, sensor biases, an sensor noise. For best performance, these moels shoul be matche to the actual AUV, the environment in which it operates, an the specifications of the sensors that are use. However, engineering trae-offs are routinely applie to simplify the moeling task an the estimator esign. For example, the Kalman Filter equations, which will be introuce in the following section, require all of the noise sources to be moele as white Gaussian noise processes (which we have one). Our objective in this paper is to highlight significant ifferences between two estimation techniques an while the choice of these ynamics will affect the results, these ifferences are not central to our objective. 3 Estimator Design The Kalman Filter is a wiely use optimal estimator for linear systems with white Gaussian noise moels. It upates the state an covariance estimate recursively in real time as new measurements are generate. The Extene Kalman Filter (), which is commonly applie to problems in which some of the ynamics or measurement equations are non-linear, provies a solution which is easy to implement, but fails to generate goo results in the context of this problem. The linearizes ynamics an measurements aroun the current state estimate to take avantage of the Kalman Filter equations, which accept only linear equations. However, linearization can introuce estimation errors. These are especially problematic in this case because estimation requires motion, which itself has to be estimate. Any biases that the estimates accumulate can continue to propagate to future estimates. As a result, the often iverges, generates a poor covariance estimate, an results in large estimation errors. Enforcing linear measurement equations by choosing appropriate coorinates to represent the system state provies an interesting solution to this problem. By using linear measurements, the Kalman measurement upate equations require no approximations an introuce no bias errors ue to linearization. This is the efining characteristic of the two-step estimator, which is escribe in etail in [5. In the Bearings-Only Tracking community, a similar concept has been use to provie improve tracking estimates of targets observe with a bearing-only sensor [1. The cost of this metho appears in the system ynamics, which are now non-linear an more complex. Our

5 implementation uses a Runge-Kutta integration to propagate the moifie state estimate an covariance forwar in time. The buren of the non-linearity has shifte from the measurement upate, where is causes biases, to the time upate, where it can be hanle more accurately with computational methos. A consequence of this approach is that the system is automatically partitione into instantaneously unobservable (e.g., range) an observable (e.g., bearing) states, which helps to prevent ill-conitione covariance matrices. Of course, the purpose of the estimator is to make all states observable by incorporating camera motion. In this section, we erive the an two-step estimator equations for a 2D version of the problem. In two imensions, the representation of rotation is much simpler (only one angle require an no singularities), the overall number of states is reuce, but most of the important aspects of the 3D problem are retaine. The state vector x for the 2D problem has eleven entries as follows: x = q x q x a w,x b a,x q z q z a w,z b a,z λ in ω b ω We use q instea of r to simplify the presentation. The estimate is still a relative measurement because we can choose p in the algorithm. If we choose p =, then r = q. We represent rotation with [ [ λc cos(θ) λ = = λ s sin(θ) [ λc λ R oc (λ) = s λ s λ c t λ = E (λ)ω = [ λs λ c ω At any time, only one of the λ-coorinates is inclue in the state vector. The other one can be compute from the constraint 1 = λ 2 c +λ 2 s an knowlege of its sign. The sign of the coorinate with the larger magnitue oes not change rapily because its magnitue is greater than or equal to.5. This shoul be the exclue coorinate. If the absolute value of the exclue coorinate becomes less than.5, the two coorinates nee to be exchange to ensure that the sign of the exclue component is well efine. To hanle this bookkeeping complexity, we introuce a convention that allows us to write the estimator equations without ealing with separate cases. If λ c < λ s,then If λ s < λ c,then λ in = λ c λ out = λ s λ 1 = λ s λ in = λ s λ out = λ c λ 1 = λ c The ynamics of the 2D system can now be expresse in matrix form: ẋ = A(λ)x + B u (λ)u + B w w A(λ) = 1 λ 1 λ c λ s B u (λ) = λ s λ c B w =

6 3.1 Equations The Kalman Filter [4 consists of a time-upate, which avances the estimates forwar in time (ˆx i 1 to x i ), an a measurement upate which incorporates the current measurements into the estimate ( x i to ˆx i ). Associate with these estimates are covariance matrices: Time Upate M x,i = E x i x T i P x,i = E ˆx i ˆx T i The time upate for the is compute using a iscrete-time formulation, where T is the sample interval. where = [ ax a z r z / C rz 2 λ c λ s 1 r x / C rz 2 λ s λ z 1 a z /λ 1 a x /λ 1 C rx 2+C rz 2 rz 2 λ ([ ) aw,x = R co (λ) + g a w,z T x i = Φ i ˆx i 1 + Γ u,i u i + Γ w,i w i M x,i = Φ i P x,i 1 Φ T Q i + Γ w,i T ΓT w,i The matrices Φ i, Γ u,i,anγ w,i can be compute from A(ˆλ i 1 ), B u (ˆλ i 1 ), B w an T using a zero-orer hol technique Measurement Upate The measurement equations in Section 2.2 can be expresse as z i = z s,i z a,i + u z + v i = f (x i )+u z + v i z ω,i u z = [ u T 1 T where v i is N (,R) white Gaussian noise an f (x) = C r x / C r z R co (a w + g)+b a ω + b ω [ C r x = R r co (p q) z R = R s R a. R ω For the, we nee to linearize f (x) using a firstorer Taylor expansion about x. This is base on an assumption that higher-orer terms are small. f (x) = f ( x)+c ( x)(x x) C (x) = f x Finally, we can write the measurement upate equations: L i = M x,i C ( x i ) T ( R +C ( x i )M x,i C ( x i ) T ) 1 ˆx i = x i + L i (z i f ( x i ) u z ) P x,i = (I L i C ( x i ))M x,i 3.2 Two-Step Estimator Equations Before eriving the two-step estimator equations, we nee to efine a moifie state vector y that leas to linear measurement equations. y = v x = C q x s x v x a x b a,x C r z v z a z b a,z λ in ω b ω v z = C q z [ ax = R a co (λ) z ([ aw,x ) + g a w,z We can construct an invertible function F to relate the x an y states: y = F (x) x = F 1 (y)

7 The two steps of this estimator refer to computing y using the Kalman Filter measurement upate equations in a first step an then using F 1 to compute x in a secon step. The measurement equations from Section 2.2 can be expresse in terms of y: Time Upate z s = s x + v [ s ax + b z a = a,x + u a z + b 1 + v a a,z z ω = ω + b ω + v ω The ynamics for y, erive accoring to the rules in [5, are as follows: ẏ = Ã(y,u) Ṗ y = Ã T Ã y P y + P y y y + BQ B T y (s x v z v x ) + ( s 2 C r z x + 1) ω a x + [ λ c λ s g + vz ω + u 1,x a z ω v z s C x r z ω Ã(y,u) = a z + [ λ s λ c g vx ω + u 1,z a x ω λ 1 ω u 2 λ c λ s 1 B(λ) = λ s λ c For the two-step estimator, we perform the time upate (ŷ i 1 to ȳ i an P y,i 1 to M y,i ) with a Runge-Kutta numerical integration technique Measurement Upate Because the coorinates of y are chosen so that the measurement equations are linear, the measurement upate (ȳ i to ŷ i an M y,i to P y,i ) is ientical to the Kalman Filter. C = L i = M y,i C T ( R +CM y,i C T ) 1 ŷ i = ȳ i + L i (z i Cȳ i u z ) P y,i = (I L i C)M y,i 4 Simulation Results We have use a numerical simulation to compare the 2D versions of the an the two-step estimator. This section escribes the simulation an presents the results. The ata are erive from a Monte Carlo simulation with 1 runs. Each run is base on the same reference trajectory, but uses ranom initial conitions, isturbances, an measurement noise. The control commans u an sensor measurements z s,a,ω are provie as inputs for the estimators, which compute estimates of the true system state (see Figure 2). The sample interval was set to T =.1s an the uration of each experiment is 2s. To simplify the comparison between the two estimators, we avoi closing the loop on an estimate state by using the true state x of the system to compute control commans. Using the output from one of the two estimators to control the AUV might a an avantage or hanicap over the other estimator. Our simulation oes not capture the effect of closing the loop aroun the estimators nor oes it examine the effect of using ifferent reference trajectories. These are topics we hope to aress in future work. The goal of the simulation is to highlight generic properties of the an two-step estimators in the context of fusing vision an inertial measurements to estimate relative position. Therefore, we use simple noise moels with arbitrary parameters. While the results will epen on these choices, our experiments show that some generic properties are present in all useful cases. For this simulation, we chose uncorrelate process noise for which each component has a variance σ 2 w =.1. The sensor noise is also uncorrelate with T

8 1.8 Reference Actual Feature Actual z (m).6.4 [ g = 1 Range (m) x (m) Figure 4: A 2D Simulation showing the Actual an Reference Trajectory an the Visual Feature Figure 6: Range Estimate Desire AUV Position (ra) Z Accelerometer Bias (m/s 2 ) Actual Figure 5: Desire AUV Position Expresse as an Angle along an Arc. The otte line was consiere, but the soli line was actually use. variance σ 2 v =.1 for each component. Both R an Q are iagonal matrices with.1 along their iagonal. Figure 4 shows a 2D worl with a visual feature, reference trajectory, an actual AUV trajectory for one run of the simulation. The initial conitions for the AUV are rawn from a zero-mean Gaussian istribution, except for the rotation, which is centere on θ = π/4 to point the camera towars the feature. (Note that the optical axis of the camera is aligne with the z-axis of Frame C.) The variance of all of the components in the initial conition is σ 2 x,t= =.1, except for the position components σ 2 q,t= =.2 an the rotation σ2 θ,t= =.5. The short lines on the actual trajectory inicate the optical axis (viewing irection) of the camera at 5 secon intervals. The esign of an optimal esire trajectory for the AUV is a problem that we have not yet stuie, so we chose an a hoc trajectory for this simulation. The esire AUV position lies on an arc of raius 1m aroun the feature. It rotates the AUV through a quarter turn while Figure 7: Z-Accelerometer Bias Estimate observing the feature, which helps with the ientification of both accelerometer biases. Figure 5 shows the esire AUV position as an angle along this arc. The otte line woul start the AUV at the mipoint of the arc, move it towars the lower right sie, then to the upper left sie, an finally back to the mipoint using a sine wave trajectory. However, we actually use the soli line, which generates better results because it has greater variation of AUV velocity an acceleration. Figures 6 to 8 show the range, z-accelerometer bias, an theta estimates for both estimators along with the truth values for one run. These three coorinates are important to consier because they are not irectly observable an among the most ifficult to estimate. For example, the theta estimate epens primarily on the component of the accelerometer measurement ue to the gravity vector, so if the estimate accelerometer bias is wrong, this error propagates to the theta estimate. The theta estimate is use in the implicit triangulation performe by the estimator, so goo range estimates epen on the quality of the theta estimate. We will compare the two estimators base on three criteria: (1) how often the algorithm faile, (2) the qual-

9 Theta (ra) Actual Figure 8: Theta Estimate ity of the state estimate, an (3) their ability to preict estimation error with the covariance matrix. During the simulation, both the an the two-step estimator iverge on some of the runs. When the estimate range became too small (less than.1m) ortoo large (greater than 5m), the run was marke as a failure. Both algorithms encounter a singularity when the estimate range approaches zero. A range greater than 5m is unrealistic, if only because of unerwater visibility constraints. Table 1: Outcomes of the Monte Carlo Simulation 1 Runs Success Failure Two-Step Success Two-Step Failure 4 29 Table 1 shows that in 686 of the 1 runs, both algorithms succeee in proucing a useful estimate without iverging, an in 29 runs, both algorithms faile. A failure is not necessarily catastrophic, because both estimators can easily reset themselves when they have iverge (which they can etect using the rules above). However, an estimator which nees to reset less frequently is more esirable. The table also shows that the succeee only in 4 cases when the two-step estimator faile. On the other han, the two-step estimator succeee 281 times when the faile. These numbers inicate that the two-step estimator is much more robust than the. We use the 686 Monte Carlo runs for which both estimators succeee for the remaining part of the analysis. Figures 9 to 11 show plots of the stanar eviation of the estimation error for range, z-accelerometer bias, an theta. In all cases, the stanar eviation is significantly reuce for the two-step estimator for most of the 2s experiment. At t = 2s, the stanar eviation of Range SDev (m) Figure 9: Monte Carlo Stanar Deviation for Range Z Accelerometer Bias SDev (m/s 2 ) Figure 1: Monte Carlo Stanar Deviation for Z- Accelerometer Bias Theta SDev (ra) Figure 11: Monte Carlo Stanar Deviation for Theta

10 Range SDev (m) Actual SDev Estimate SDev Figure 12: Comparison of Stanar Deviation for the Range coorinate of the Two-Step Estimator Range SDev (m) Actual SDev Estimate SDev Figure 13: Comparison of Stanar Deviation for the Range coorinate of the the is at least twice as large for each of the three coorinates, which highlights a clear avantage of the two-step estimator. The covariance estimate generate by each algorithm is also a useful quantity in the control of the AUV. For example, when the stanar eviation of the range is large, the algorithm inicates that the range estimate is uncertain. In that case, the trajectory generator shoul back up the AUV to avoi a possible collision an shoul start a maneuver to improve the observability of the range estimate. In this last comparison, we examine the quality of the covariance estimate for each algorithm. The actual stanar eviation σ is compute from the estimation error ( x = x ˆx), which can only be compute uring a simulation when a truth measurement is available. It is an ensemble average over N runs. σ(t)= 1 N N x 2 i (t) i=1 The estimate stanar eviation ˆσ is an ensemble average of the quantity that the algorithm reports in the covariance matrix. ˆσ(t)= 1 N N i=1 σ i (t) Figure 12 compares these two quantities for the range estimate generate by the two-step estimator an Figure 13 shows the equivalent comparison for the. Whereas ˆσ is a useful preictor for σ for the two-step estimator, this is not the case for the, whereˆσ is much smaller than σ. This failure of the to generate an accurate covariance estimate is tightly relate to the inflate estimation errors plotte in Figures 9 to 11. The simulation results in this section have shown a clear avantage in performance of the two-step estimator over the. This was emonstrate by the number of failures ue to ivergence, by the size of the estimation errors, an the accuracy of the covariance estimate. 5 Conclusions We have propose a relative position estimator for intervention-capable AUVs. These AUVs will be use to execute autonomous manipulation tasks uring which the AUV s position relative to a target object nees to be controlle. While a variety of position sensing systems have been use for AUV control, this application poses a unique sensor requirement: a robust, 6DOF, irect estimate of the relative position between the AUV an a stationary visual feature. Our goal is to emonstrate this position estimator on an intervention-capable AUV. All of the assumptions an constraints that have shape the esign of the estimators are erive from such an AUV mission. However, this work is still in progress, an we have not reache the AUV testing phase. At this time, we have efine a sensor strategy that merges vision an inertial rate sensors, iscusse its key avantages, an ientifie some of the challenges of implementing this technique. Our initial work has focuse on eveloping an estimator to fuse measurements from these two sensors. We have compare two estimator esigns, the popular an the two-step estimator, in the context of a simplifie 2D version of this problem, an have conclue that the two-step estimator provies critical avantages at a cost of extra esign complexity an some computational expense. Our future work will focus on four main topics. First, the algorithm nees to be expane to the 3D scenario. Secon, we have not iscusse the impact of specific isturbance ynamics an sensor noise moels, an have instea assume arbitrary moels. More realistic moeling of these processes will improve the performance

11 of an operational system. Thir, we currently use a hoc trajectories to generate sufficient sensor motion. The issue of blening optimal maneuvers to improve observability with the execution of useful tasks (e.g., manipulation) remains to be explore. Finally, we expect to emonstrate the estimator as part of an AUV manipulation task. References [1 Vincent J. Aiala an Sherry E. Hammel. Utilization of moifie polar coorinates for bearingsonly tracking. IEEE Transactions on Automatic Control, AC-28(3), March [2 Hans Jacob S. Feer, John J. Leonar, an Christopher M. Smith. Aaptive mobile robot navigation an mapping. The International Journal of Robotics Research, 18(7):65 668, July [3 Michael D. Feezor, Paul R. Blankinship, James G. Bellingham, an F. Yates Sorrell. Autonomous unerwater vehicle homing/ocking via electromagnetic guiance. In Oceans 97 MTS/IEEE Proceeings, volume 2, pages , Halifax, October [9 Hanumant Singh, Martin Bowen, Franz Hover, Phillip LeBas, an Dana Yoerger. Intelligent ocking for an autonomous ocean sampling network. In Oceans 97 MTS/IEEE Proceeings, volume 2, pages , Halifax, October [1 Stefan B. Williams, Paul Newman, Gamini Dissanayake, an Hugh Durrant-Whyte. Autonomous unerwater simultaneous localisation an map builing. In IEEE International Conference on Robotics an Automation, volume 2, pages , San Francisco, CA, April 2. IEEE. [11 Dana R. Yoerger, Albert M. Braley, Marie-Helene Cormier, William B. F. Ryan, an Barrie B. Walen. High resolution mapping of a fast spreaing mi ocean rige with the autonomous benthic explorer. In Proceeings of the 11th International Symposium on Unmanne Untethere Submersible Technology, Durham, NH, August Autonomous Unersea Systems Institute. [4 Arthur Gelb, eitor. Applie Optimal Estimation. The M.I.T. Press, Cambrige, Massachusetts, [5 N. Jeremy Kasin. The two-step optimal estimator an example applications. In Robert D. Culp an Eileen M. Dukes, eitors, Guiance an Control 2, volume 14 of Avances in the Astronautical Sciences, pages 15 34, San Diego, CA, 2. American Astronautical Society, Univelt Incorporate. [6 J.-F. Lots, D. M. Lane, an E. Trucco. Application of 2 1/2 D visual servoing to unerwater vehicle station-keeping. In Oceans 2 MTS/IEEE,Provience, RI, September 2. [7 R. L. Marks, M. J. Lee, an S. M. Rock. Visual sensing for control of an unerwater robotic vehicle. In Proceeings of IARP Secon Workshop on Mobile Robots for Subsea Environments, Monterey, May IARP. [8 S. Negaharipour, X. Xu, an L. Jin. Direct estimation of motion from sea floor images for automatic station-keeping or submersible platforms. IEEE Journal of Oceanic Engineering, 24(3):37 382, July 1999.