Closed-loop one-way-travel-time navigation using low-grade odometry for autonomous underwater vehicles

This paper extends the progress of single beacon one-way-travel-time (OWTT) range measurements for constraining XY position for autonomous underwater vehicles (AUV). Traditional navigation algorithms have used OWTT measurements to constrain an inertial navigation system aidedbyaDopplerVelocityLog(DVL).ThesemethodologieslimitAUVapplicationstowhereDVL bottom-lock is available as well as the necessity for expensive strap-down sensors, such as the DVL. Thus, deep water, mid-water column research has mostly been left untouched, and vehicles that need expensive strap-down sensors restrict the possibility of using multiple AUVs to explore a certain area. This work presents a solution for accurate navigation and localization using a vehicle ' s odometry determined by its dynamic model velocity and constrained by OWTT range mea-surementsfromatopsidesourcebeaconaswellasotherAUVsoperatinginproximity.Wepresent a comparison of two navigation algorithms: an Extended Kalman Filter (EKF) and a Particle Fil-ter(PF). Both of these algorithms also incorporate a water velocity bias estimator that further enhancesthenavigationaccuracyandlocalization.Closed-looponlineﬁeldresultsonlocalwaters aswellasareal-timeimplementationoftwodaysﬁeldtrialsoperatinginMontereyBay,California during the Keck Institute for Space Studies oceanographic research project prove the accuracy of this methodology with a root mean square error on the order of tens of meters compared to GPS position over a distance traveled of multiple kilometers.


INTRODUCTION
Accurate absolute positioning is fundamental to all robots-both to ensure realtime closed-loop control and to provide position measurements co-located with other observations. While the global positioning system (GPS) provides meter-scale absolute positioning of terrestrial, aerial, and sea-surface robots, the rapid attenuation of GPS in water precludes its use on submerged robots. Long baseline (LBL), 1 ultra-short baseline (USBL) 2 or single transponder methods 3  One-way-travel-time (OWTT) navigation provides a solution that enables simultaneous positioning of multiple robots from a single acoustic beacon by constraining strap-down navigation systems, and, when coupled with increasingly mature autonomous surface vessels (ASVs), provides a framework for navigating multiple submerged AUVs from one or more ASVs or in fact from an AUV which periodically surfaces. 6,7 Further, as will be shown in this work, the method is adaptable to low-quality odometry, avoiding the need for expensive and high-power inertial and acoustic systems. As such, the method is well suited to equipping the emerging class of low-cost micro-AUVs 8,9 and long-duration systems such as underwater gliders 10 or long-range AUVs 11 with bounded error navigation. Also, it enables bounded navigation for AUV operations in the mid-water column either during descents in deep water or to provide spatially dense surveys of fine scale physical or chemical features. 12 It is additionally well suited to aiding navigation in under-ice studies which face the added challenge that upward facing DVLs measure the AUV speed relative to a noninertial frame-i.e., translating and rotating ice. 13 Fundamental to OWTT ranging is a synchronized time base across all platforms. Highly accurate and synchronized clocks, such as a chipscale atomic clock (CSAC) 14 or a temperature compensated crystal oscillator (TXCO), 15 on both the beacon and the receiving vehicle are needed to accurately measure the one-way-travel-time of the acoustic packet. While CSACs initially promised an extremely stable time reference for relatively little power, recent manufacturing and aging issues have tempered those expectations. 16 Using this synchronized time, the time of launch and transmission location are encoded in the transmitted packet such that any receiving vehicle can compute a range from a known location with which to constrain its own navigation solution.
As the received range only constrains the navigation solution, the observability of the system is an important area of consideration, especially for single beacon ranging. 17 Prior efforts have proven observability of single beacon ranging through analysis of the Fisher Information Matrix 18 and which trajectories are observable. 17 Other work has examined optimal trajectories of a team of AUVs to enhance observability and thus improve the navigational accuracy. 19 Other works have investigated the use of multiple cooperating vehicles which share varying amounts of information to further constrain their navigation. [20][21][22] A significant challenge in using inter-vehicle ranges for position estimates is preventing overconfidence in the solution through the sharing of correlated covariance information or double counting. 23 Prior work has largely dealt with this challenge by preserving prior measurement information and extending the state with each new measurement which has limited scalability and suitability for real-time processing. Approximations to this approach have been presented which compress prior measurements 24 or which avoid transmitting the correlation terms of the covariance matrix. 25 Despite the aforementioned advantages, successful postprocessing of experimental results using OWTT range measurements are rare. Salient examples include centralized 15 and decentralized 26 filters, approximate filters for bandwidth limited communications, 25 cooperative navigation with kayaks 23 and cooperative ranging and bathymetric navigation. 27 Rarer are results that incorporate odometry from non-DVL sources such as the recent work with underwater glider tracking over short distances 21 and long distances under ice. 28 Completely absent are presentations of experimental validations of closed-loop solutions.
This work builds upon prior efforts and presents for the first time experimental results of closed-loop OWTT navigation using multiple AUVs. Further, a direct comparison between model velocity aided odometry and DVL aided odometry is presented, illustrating the flexibility of the method. The methodology was developed based on postprocessing of field data collected during the Keck Institute for Space Studies (KISS) "Satellites to the Seafloor" project conducted in Monterey Bay, CA in September, 2016. 29 With this data an Extended F I G U R E 1 Diagram of the one-way-travel-time navigation process where the autonomous underwater vehicle (AUV) transits in frame (a) with its estimation uncertainty growing over time. In frame (b) the AUV receives a range which reduces its uncertainty and updates its location estimate Kalman Filter (EKF) and Particle Filter (PF) were developed each with a bias estimator. Subsequently the EKF solution was demonstrated in closed-loop feedback into the navigation solution during field trials near Woods Hole, MA.
The remainder of this paper is organized as follows: Section 2 discusses the EKF, PF and bias estimator algorithms. Section 3 describes the vehicle and experimental set-up. Section 4 presents the results from the post-processed field trials in Monterey Bay, CA and closedloop field trials near Woods Hole, MA and Section 5 presents our conclusions.

ONE-WAY-TRAVEL-TIME NAVIGATION WITH LOW-GRADE ODOMETRY
One-way-travel-time navigation incorporates range measurements between a transmitter and a receiver by means of acoustic packet transmissions. Based on a pre-determined timing cycle, the source transmits an acoustic packet, encoded with its pose information and time-of-launch (TOL). The receiver decodes this acoustic packet information and records its time-of-arrival (TOA). Based on the acoustic packet's time of flight (TOF), and knowledge of the transmitting medium's sound velocity profile, a range is calculated between the transmitter and the receiver. The source's position information and calculated range is then incorporated into the receiver's state estimator to update its own navigation estimate. Since the range is calculated based on TOF of the acoustic packet, both the transmitter and the receiver must have highly synchronized clocks within an acceptable tolerance. For example, a timing error of less than 1 ms will result in a position error of less than 1.5 m with a sound speed of 1500 m/s. Using this information, a range from a source beacon collapses the vehicle's position area of uncertainty and updates its position estimate as illustrated in Figure 1.
To fuse the range measurement with the vehicle's position estimate, the standard EKF for one-way-travel-time navigation is extended to include a bias estimator and to take as its navigation update a model velocity based dead-reckoning solution. We also present a particle filter implementation based on the same inputs which also includes a bias estimator.

Tightly coupled OWTT extended Kalman filter
In this section, we present the EKF coupled with a bias estimator. This EKF algorithm is summarized below in Algorithm 1.

12: end loop
Since we consider attitude and depth to be adequately instrumented, the three-dimensional OWTT range is projected into the horizontal plane, and only position and velocity are considered.

EKF process and measurement model
In this EKF, the vehicle's state vector is composed of its horizontal position and velocity components: Here each x, y pair is the vehicle's position, and each u, v pair the vehi- In discrete time, the models follow the standard form where the state transition matrix F maps the prior state to the present with the addition of the noise term w. Here, the state transition matrix is based on a constant velocity model with sampling period, dt: For the measurements, z, the model velocity measurement along with the bias estimation is linear, and the OWTT measurement is nonlinear (more details on the bias estimation are presented in Section 2.3). The plant process noise, k , and measurement noise, k , are considered zero-mean, Gaussian white noise: For the measurement variance matrix, , the matrix is diagonal with the respective measurement (i.e., GPS, model velocity, or range) variance values: The plant process noise matrix, Q, is defined with a gain parameter,q: Assuming that the state estimation of position and velocity are independent, cross terms may be neglected and the matrix becomes a diagonal matrix.
The EKF updates the state estimate at each time step, k, with a prediction step and subsequent measurement update depending on availability of a measurement. 32 The state is predicted by applying the transition matrix, F, to the state estimate of the previous step: Subsequently, the predicted error covariance matrix, − , is determined by applying the plant transition matrix to the updated error covariance matrix of the previous step, + and the process noise covariance matrix, Q: With the availability of a measurement, the Kalman Gain matrix, K, is computed by being a function of the predicted error covariance, the respective measurement mapping matrix H, and the respective measurement noise covariance matrix R: The state estimate is then updated by adding the product of the Kalman Gain and the measurement innovation to the predicted state: Lastly, the error covariance is updated using the "Joseph form" of the Riccati equation, which ensures positive definiteness: 33

Range measurement and augmentation
For each OWTT range measurement, from either the surface beacon or another vehicle, the receiving vehicle's state vector and error covariance matrix, , is augmented to process the range measurement with the transmitting beacon's pose, x b , y b : The covariance matrix is augmented with the transmitting beacon's position uncertainty values, similar to the Naively Distributed Extended Kalman Filter (NEKF). 34 There, the authors show that a geometry that consists of a single surface transmitting node with multiple submerged transmitting nodes (which mirrors the geometry in our field experiments), the NEKF closely follows the 1 uncertainties of the benchmark Centralized EKF. A similar arrangement has also been termed the Egocentric Extended Kalman Filter (EEKF) which assumes independence among the vehicles ranging by neglecting the cross covariance terms. 25 Motivated by these prior results, the algorithmic simplicity, and low acoustic bandwidth necessary, we use a similar method during the transmitting of beacon state uncertainties. Here, the vehicles covariance v is augmented with the beacon's covariance b to form the augmented covariance matrix aug to process the range measurement: The v matrix is the vehicle's predicted 4 × 4 Covariance Matrix, − , at time, k, of the range measurement. The b matrix is a 2 × 2 diagonal matrix, in which the diagonals are both equal to the sum of the beacon's variance values requiring only a single value to be transmitted in the acoustic packet. Transmitting beacon uncertainties in this manner is more conservative than the NEKF or EEKF methods and thus minimizes the potential for overconfident solutions.
Once this augmentation process is complete, the measurement matrix, rng is determined to map the vehicle's state vector to the OWTT range computed by the product of the acoustic packet's TOF and sound speed. The OWTT range update is based upon the decentralized EKF presented prior. 35 Here, xb is the state vector of the beacon (either a surface beacon with access to GPS or another submerged AUV): The range between the vehicle at TOA and the beacon at TOL is found by computing the linear distance between their locations: Here, v xy and b xy are the position of the vehicle at TOA and beacon at TOL, respectively, and v rng is the noise in the range measurement that does not change with time. The range equation may then be rewritten in state vector form Here, v and b are matrices defined to capture the pose information of the vehicle and the beacon at TOA and TOL, respectively. * Since the range measurement is non-linear, a Jacobian matrix, evaluated at the vehicle's augmented predicted state becomes The Kalman Filter measurement update equations (13)(14)(15)

Loosely coupled OWTT particle filter
In this section, we present a particle filter (PF) which provides an update to the EKF based on the OWTT ranging. The PF is presented to provide a comparative method which better captures the nonlinearities in the range measurement update. Here, the PF also includes a bias estimator and takes the place of EKF's range measurement augmentation and bias estimator presented in Section 2.3. The PF algo- In the PF algorithm, the EKF's dead-reckoning solution since the last range update is used to propagate the prior particles i k−1 with the state difference Δ k . To account for the error growth in the deadreckoning solution, a proportionate amount of jitter k is applied during the particle propagation proportional to the distance traveled through the DR term: Here, the range time step is indicated by k, the number of particles by N and the particle index by i. The particle weightsw i k are then computed by comparing the range measurement k with the estimated range from each particle to the transmitting beacon's location b using a normal distribution: The normal distribution is defined by pf , estimated using a range dependent error term rng and the square root of the norm of the beacon's position estimate co-variance b : The raw particle weightsw i k are then normalized to w i k such that their sum is equal to one to form them into a probability distribution: To adjust the particle locations so they best capture the informative portion of the probability density function the particles are then resampled. While there are several options for re-sampling algorithms here we use systematic re-sampling outlined in Algorithm 3. 36 This method provides a fast and simple way to represent the probability density through evenly weighted particles, requiring the particle locations and weights as inputs and providing the re-sampled particles as outputs. The algorithm operates by first taking the cumulative sum of the particle weights, forming an increasing set of values from zero to one. A random number is then drawn from a uniform distribution on the interval [0,1] and the index of the cumulative sum found in which the random number is equal to the value of the cumulative sum. The particle at the index in the old set is then stored at i in the re-sampled set for output. In this way, particles with high weight are divided into many particles as they occupy a large portion of the cumulative sum and particles with small weights are discarded as they occupy a negligible portion of the cumulative sum.

4:
Find index of random number in cumulative sum

5:
Store particle at index for output

6: end for
The updated statêk may then be estimated by simply computing the average of the re-sampled particles locations: The updated state estimate is then fused back into the EKF's navigation solution which continues dead-reckoning on every speed estimate until the next range is received.

Velocity bias estimator
For dead-reckoning AUVs moving in mostly straight lines the primary source of error accumulation may be assumed to be due to unknown and varying ocean currents and modeled velocity estimation errors.
These errors present themselves as a persistent bias in the state update if they are changing much slower than the frequency of range measurements. To improve the dead-reckoning performance a velocity bias estimator may be written based on the state update bias and the time difference between ranges. With each range measurement, the EKF or PF output may be used to determine a velocity error mea- Here, Δt is the time between the current range measurement and the With thēand̄matrices equal to the identity matrices, the KF predicts the bias estimator co-variancē− through addition of the prior bias estimator covariance and the process noise covariance matrix̄: The bias estimator covariance prediction is then used to compute the co-variance innovation̄through addition of the EKF or PF covariance + for the velocity bias and the norm of the EKF or PF covariance for the time bias:̄k The measurement innovation̄is computed through the subtraction of the bias estimator state prediction from the velocity measurement in Eq. (29) and the quotient of the range error estimate and the sound speed:̄k The Kalman gain̄is then computed as the product of the predicted covariance and inverted covariance innovation.
Using these terms, the bias estimator's state and covariance are updated through application of the Kalman gain to the measurement innovation and by the sequential algebraic Riccati equation respectively:̂b ias,k =̂b ias,k−1 +̄k̄k The outputs of this KF are used to adjust the velocity estimates in the EKF and to adjust the received range estimates. Previous work has described a similar method to the one described here that estimates range-based averaged currents (RACs) for use with underwater gliders. 28 That approach estimates a new current estimate after a series of OWTT range measurements. Conversely, the bias estimator in this work conducts a new estimate sequentially after each OWTT measurement from any beacon.

EXPERIMENTAL CONFIGURATION
The vehicles used during all of the field trials were Ocean-Server, Inc. Iver-2 AUVs. Two of these vehicles, Iver-106 and Iver-107, were   for these dives such that the surfacing error between the last deadreckoned location and first good GPS location is minimized.

FIELD TRIALS
Two sets of field trials were used to perform this work.

Monterey bay field trials
In these trials the Iver-2 AUVs were deployed from the R/V Shana Rae based out of Santa Cruz, CA, to locations determined to be interesting based on a 300 m gridded regional ocean model. The field trial data presented in the next section is from data gathered during deployments on September 1 and 2, 2016 with locations shown in Figure 4. For the data gathered on September 1, Iver-106 and Iver-107 were in the water performing the top and bottom half of a 3 km butter-fly pattern.
Because of logistical problems the data from Iver-107 was not logged, however, Iver-106 was still able to benefit from its acoustic transmissions. On September 2, Iver-106 and Iver-107 were deployed to perform a complete 3km butterfly pattern but in opposite directions.
On each day the topside transducer was dangled from the R/V Shana  Table 1. For the PF implementation the number of particles N, the dead-reckoning error growth rate DR , and the range dependent error rng are shown in Table 2.
For the bias estimator terms we use a value of one, for the velocity bias terms and for the clock bias term we use a value of 0.0001: Several different scenarios may be considered in post-processing the data to examine the utility of each method. The first scenario considered is that of simply re-navigating the raw data where the vehi-  Figure 6 for the data from Iver-106 and Iver107 on September 2, 2017.
During these trials, both vehicles were deployed in the upper left hand corners of Figure 6 from the R/V Shana Rae. The top left shows a period of sparse range measurements after which the 2 error ellipses are initially large but converge after several successful ranges. The bias estimator also shows its utility to provide a decent estimate of the velocity bias during the gap in range updates as the EKF and PF trajectories track the GPS-DR well the first two legs before having its propeller fouled with seaweed, necessitating its recovery. The other vehicle, Iver-107, was able to perform all four legs of its mission, requiring recovery due to its batteries being depleted. In re-navigating this data both the EKF and the PF are able to maintain convergence with errors dependent on the inter-vehicle geometry and success rate of receptions. The similarity between the two algorithms, their ability to handle gaps in received ranges and the ability of the bias estimator to improve the solution is illustrated in Figure 7.
The RMS error from each vehicle during this scenario is shown against the 2 bounds in Figure 8. Here the gap in ranges for Iver-106 from Figure 7 is seen around the 2500 s mark in the top frame. Additionally, after around 8300 s, Iver-106 is recovered. During this process TA B L E 3 RMS Errors for the EKF and PF during the sept 1 and 2 experiments for vehicle's Iver-106 and Iver-107 which both use the GPS readings during surfacings (All) or ignore GPS readings except during initialization (Init), make use of the bias estimation (Yes/No) and make use all available beacons (All) or only the topside beacon (Top)  The summary of all of the different scenarios are listed in Table 3.

Date Vehicle GPS Bias Beacons EKFe PFe DRe
In general, the PF implementation seems to slightly out-perform the EKF solution. In every case the PF improves on the DR error whereas the EKF improves on the error in every case except when the GPS is  A summary of the trajectories of Iver-106 and Iver-136 during these field trials are shown in Figure 10, the errors during the closed-loop results in Figure 11 and a summary of the RMS errors in Table 4.

Ashumet pond field trials
During these trials the vehicles were deployed from shore. Iver-136 was prepared and deployed first, followed by Iver-106 once These bad beacon location estimates had the effect of biasing Iver-136's closed-loop EKF estimate at the start of its mission as illustrated in Figure 10. In spite of this initial bias the closed-loop EKF solution managed to converge within 50 m of the GPS track after around 1500 s.
Iver-136's data was re-navigated with the EKF in post-processing to examine the effects of removing the two bad initial ranges. By removing these ranges the EKF solution remains much closer to the GPS measurements throughout. Additionally, the re-navigated EKF converges to the closed-loop solution after approximately 1800 s. The PF was also run in post-processing but allowed to take as inputs the two bad initial ranges. Here, the PF solution outperforms the EKF, converging = * 0.01 (40) and in the re-navigated PF the DR was reduced to 0.05:

CONCLUSIONS
To compute the one way travel time (OWTT) navigation updates two methods are presented, a tightly coupled extended Kalman filter (EKF) and a loosely coupled particle filter (PF  These methods were developed using data gathered during field trials with two Iver-2 Autonomous Underwater Vehicles (AUVs) and a topside transducer dangled from the R/V Shana Rae in Monterey Bay, CA on September 1 and 2, 2016. These trials were used to evaluate the short term and long term effectiveness of the EKF and PF, the utility of the bias estimator and the use of a topside only or all to all acoustic topology. In the short runs between GPS updates the PF was found to improve on the dead-reckoning solution whereas the EKF did not.
When the GPS was used only for initialization, both the EKF and the PF improve on the dead-reckoning performance significantly with the PF slightly outperforming the EKF in most cases. If the bias estimator is not used or when only the topside beacon is used, the EKF and PF perform worse than when the bias estimator is used and all beacons are used.
The EKF solution without the bias estimator was then used in The limited requirements of these methods make it directly applicable to a diverse set of underwater navigation problems. Most directly, the emerging class of micro-AUVs will directly benefit from the low cost requirements of the needed hardware enabling further democratization of the use of AUVs. Further, this method is applicable to navigational aiding of long duration platforms such as autonomous underwater gliders and long range AUVs due its low energy requirements. It also provides a natural path for scaling the navigational accuracy of vehicles which have DVLs and only turn them on when high-accuracy navigation is needed. This work also provides a bounded navigational aid for AUVs