Localization of a Drifting Underwater Vehicle Using a Terrain-Based Particle Filter

In this paper we present a terrain-aided particle ﬁlter to localize a freely drifting underwater vehicle. The vehicle is a bottom imaging Lagrangian ﬂoat used for habitat classiﬁcation, monitoring and ﬁsh abundance. During operation the vehicle captures down looking images at a controlled altitude above the bottom. Direct navigation information is often, but not always, recorded with a ultra short baseline (USBL) acoustic systsem. The presented methodology provides an al ternate method for georeferencing when USBL is unavailable. The implemented particle ﬁlter utilizes a background bathymetry map and visual odometry as a motion mode. The particle ﬁlter is implemented using the Robot Operating System (ROS) and Orocos Bayesian Filtering Library (BFL). The Grid Map package is used to store and retrieve the bathymetryic data. Results using data collected on ﬁeld deployments in the Hawaiian islands in 2018 show the method is able to eﬀectively utilize the terrain information and produce drift trajectories which closely match the recorded USBL data. Utilizing the method allows the ﬂoat system to be deployed with minimal ship-side support while still maintaining the georeferencing critical to the end use of the collected images.

Kalman filters are known to diverge when the initial state estimate is far from the actual solution or the actual errors violate the unimodal assumption.
A better approach for this application will be a particle filter that does not need to assume a unimodal error distribution and is very amenable to terrain added navigation problems. Particle filters can be a better approach for treating non linear problems [5] [6]. Several applications of particle filters are described that tackle non-linearities [4] [9]. Particle representations can be implemented quite easily to approximate a wide array of distributions, but can require a large number of particles needed to achieve a desired accuracy. This can be computationally demanding [12].
The remainder of this paper describes the development of a particle filter approach appropriate for a drifting underwater vehicle. Section 1.2 describes the float instrument in more detail and discusses the sensors available to the filter. Section 1.3 describes the extended Kalman filter and particle filter implementations. Results for several variants of the particle filter are presented in Section 1.4. Section 1.5 and 1.6 discuss the results further in the context of typical float operations.

Lagrangian Float System
The Lagrangian imaging float, Figure 1, is a small drifting instrument designed to image the seafloor with minimal cost and operational complexity [8].
The float is able to drift over varying bathymetry (down to 100 meters depth) while automatically maintaining a constant altitude above the bottom [10].

Sensor measurements
The particle filtering method uses measurements of depth, altitude, drift speed over the bottom and direct position measurements provided by acoustic tracking.
These measurements occur asynchronously and are incorporated into the filter as they occur in a causal manner.

Depth and altitude
The water depth is determined as a combination of the float's depth and altitude above the seafloor. Depth is obtained from a Keller American PA33X pressure sensor with a full scale range of 11 dbar. The sensor has a resolution of 2 millimeters, an acurracy of 5 centimeters and reports data at 5 Hz. The altimeter is a 200 kHz Airmar tranducer that provides data with 3 centimeter resolution at 1 Hz.

Position measurements
Direct underwater position measurements are made with an Evologics S2C R 18/34 Ultra Short Baseline (USBL) tracking system with a horizontally omnidirectional transducer beam pattern. These measurements are used to estimate the X and Y position for the float when underwater. The depth estimate from the USBL is generally noise and is not used in the filtering. The USBL system is range limited and can often be occluded by the bottom terrain or ship orientation. As such, it is common for the USBL data to drop out for minutes at a time, or simply be unavailable for some deployments when the float is used far from support vessel.
Additional information is collected when the float is on the surface and with a separate surface buoy. The float GPS and Iridium locating system will log the GPS at the start and end of the dive when it is still on the surface. During shallow operations, in less than 40 meters water depth, a surface buoy can be attached to the float with a thin floating tether. A GPS tracker on this buoy will give an approximate location of the float, but will be separated from the float by up to a tether length. Typically the surface buoy is pulled thought the water with a relatively consistent tether offset determined by the wind, current and depth of the float.  Visual odometry The camera system mounted on the Float takes down-looking overlapping images. The images are processed to produce displacement estimates between sequential image pairs. The camera images are strobe illuminated and taken at a fixed frame rate of 3 seconds per pair and typically overlap. In some instances the overlap will be too small and a displacement measurement will not be possible.
The images from each dive are batch processed as an initial step to produce a time series of velocity and displacement estimates. A sample sequence is shown in  The current odometry method uses standard single viewpoint camera calibrations [3] and a FFT-based approach that has been robust to images or varying quality taken over a range of altitudes [2]. The FFT method has proven to be robust to lower quality images that sometimes suffer from low contrast and backscatter caused by the background ambient lighting during daytime operations and water turbidity. The camera system also has a three axis magnetometer to measure heading and rotate the image into the world reference frame as a preprocessing step.
Prior to use by the particle filter odometry time series is first smoothed to obtain an average velocity. This process is done to compensate for the unsteady surge motion of the float as it drifts near the bottom. The surge motion has motion period consistent with the surface wave period, as is typically less the 15 seconds.
A filter time constant of 120 seconds produces more steady motion estimates that integrate well and avoid the surge motion.

Particle filter implementation
. The filter presented here has been implemented using the Orocos Bayesian Filter Library (BFL) within the Robot Operating System (ROS). The overall organization of the method is shown in Figure 5a. The data for each float dive are stored in a rosbag. Replaying the rosbag controls the processing rate. On a typical laptop computer the method can run at 10x speed. The data are routed through an uncertainy pre-processor which adds uncertainty values to the ROS messages that will be used by the particle filter. The data messages are handled within the filter by measurement functions that run asynchronously. Each measurement function returns a likelihood of the measurement that is used by the filter to resample particles. The bathymetric data is handled independent of the rosbag and stored using the GridMap utility. GridMap is used to provide a depth lookups for the particle filter in the terrain resampling step. The particles are displayed using RViz after they have been transformed to the world coordinate frame.
The particle filter has been tested against a conventional extended Kalman filter (EKF), that was also implemented in ROS (Figure 5b). This filter uses the robot localization ROS package and does not incorporate the bathymetry data.
The particle filter is implemented as a two dimensional filter estimating the X and Y position of each particle. The state of each particle, includes the location in a local coordinate frame and respective velocities. The initial distribution of particles is specified by an initial location and a Gaussian distribution parametrized by σ p . The depth of particles are assigned the most Similarly, the EKF uses a two dimensional state vector.

Motion models
The particle filter assumes that the motion of the float at any time is governed by a random walk. This is implemented by perturbing the velocity of each particle by a random sample drawn from a normal distribution. This distribution is zero mean with variance σ v . This motion step occurs every second.
The prediction step of the EKF implementation is parameterized by a process noise Q v on the state velocity.

Measurement functions
The measurement functions for the particle filter are defined for the USBL position, visual odometry and measured bathymetry. Each function is implemented to return a likelihood of the specific measurement given the state of each particle.
These functions have been implemented using Gaussian likelihoods.

USBL
The USBL position measurement function is characterized as zero mean with constants σ x and σ y . This level of uncertainty matches the spread of measurements from calibration tests where the float was fixed on the seafloor and the ship made measurements from different bearings and ranges.

Bathymetry
The terrain likelihood function is implemented as a lookup for each particle using the background bathymetry data stored in GridMap ( Figure 6). GridMap loads the data once at the start of the algorithm and the subsequent access is computationally efficient. For the cases tested here the map was stored with 10x10 meter grid cells. The terrain measurements are run a fixed rate, typically 10 seconds. Although the altimeter reports more frequently the slow motion of the float over the bottom, the grid cell size do not require a faster update rate. This rate could be changed if a smaller or larger grid size is used.
The likelihood of a terrain measurement is defined as a Gaussian parameterized by σ t . To derive a reasonable value for σ t the distribution of errors between the float's water depth estimate and the bathymetry map water depth at 3000 USBL positions across several dives was determined (Figure 7). This distribution captures the spread of the best case error that can be expected between the float and the background bathymetry. This distribution is not exactly zero mean, which is likely attributed some non-tidal constant offset. The incorporated data cover a range of times and tidal levels.

Visual odometry
The visual odometry measurement function is defined as a velocity error between an individual particle's velocity and the measured odometry velocity. This is defined as a Gaussian likelihood parameterized by σ o (t). The time dependence is included because uncertaintiy values are calculated for each image measurement.
This value is determined from the shape of the image matching correlation peak and an altitude scale factor that relates pixel values true scale motion.

Results
To evaluate the filtering method the EKF and several version of the particle filter were run. This specific float deployment was chosen from a number of possible dives because it has a section of good USBL data, a long USBL hiatus, varied bathymetry and odometry data with typical image overlap and surge motions.
The specific parameters used for the initial conditions, measurement uncertainties, process noise and are shown in Table 1.

EKF results
The EKF was run to establish a best case performance baseline when using both USBL and visual odometry. A comparison case using only visual odometry was also run to provide insight for operational scenarios where continuous USBL  Figure 8 shows a very good correspondence with the USBL measurements when they are available. During the 30 minute gap in USBL data the trajectory follows the direction correctly but over estimates the motion. When the USBL returns the trajectory is pulled back to the USBL measurements. The corresponding position uncertainty is shown in Figure 9. This clearly shows the loss of confidence during the USBL gap and the smaller periodic increases in uncertainty between the regular USBL fixes. Figure 9: Position uncertainty for the EKF using both USBL and visual odometry measurements. Figure 10 shows a position track create by integrating only the image displacements over time. In general this trajectory replicates the USBL tracking in direction but is prone to the inherent integration error over time and steadily deviates from the starting point.

Particle filter results
The particle filter was evaluated using several combinations of the USBL, terrain and visual odometry measurements. In each instance 5000 particles were used and the initial particle distribution was started at the location of the first USBL measurement. The trajectory of the particle filter was defined by the centroid of the particle cloud at each times step. The spread of the particle distribution was characterized by calculating the standard deviation of the distances between the centroid and the individual particles. This radial spread calculation does not cap- Figure 10: A sample trajectory estimated using only visual odometry.
ture the full shape of the particle distribution but does provide a single statistic to represent the overall consensus of the particle cloud along the trajectory.

Terrain only measurements
The trajectory using only terrain measurements in the particle filter is shown in Figure 11. This trajectory has quite a bit a variability but matches the general motion trend measured by the USBL. In this case the only particle motion in the filter is the non directional random walk. Figure 12 shows a sample particle cloud along the trajectory that follows the terrain shape. Figure 11: XY USBL measurments (red dots) and estimated particle filter trajectory (blue) using on the terrain information in the particle filter.
Terrain and visual odometry Figure 13 shows the trajectory when both terrain and visual odomoetry are included in the filter. This case produces a much more consistent trajectory that is closer to the USBL positions and more direct that in the terrain only case. This shows the value of the odometry data to constrain motion of the float.
Visual odometry and USBL Figure 15 shows the combination of odometry and direct USBL measurements.
In this case the filter does a better job of spanning the USBL gap than that EKF and generally follows the USBL fixes. Near the center of the trajectory the trajectory is pulled east of the USBL fixes. This could indicate a directional bias in the odometry data. Figure 12: Sample representation of the particle cloud using terrain navigation only.
Terrain, USBL and visual odometry Figure 16 shows the most constrained result where the filter uses all three measurements. In this case the float follows the USBL fixes where they are available. In the USBL gap the filter keeps the trajectory consistent with the USBL points with only a small correction when the USBL measurements return. Figure 13: XY USBL measurments (red dots) and estimated particle filter trajectory (blue) using visual odometry and terrain navigation

Summary of uncertainty
A comparison of the uncertainties for the EKF and all four particle filter cases is shown in Figure 17. The EKF results, reproduced here from Figure 8, shows the best case scenario when USBL measurements are available and a rapid growth in uncertainty during the USBL gap. The terrain only particle filter generally has higher uncertainty along the trajectory. This is expanded as it is never constrained by the direct position fix. The USBL and visual odometry particle filter behaves similarly to the EKF. In this case the uncertainty is generally low but increases during the USBL gap. The growth rate is lower than the EKF. The terrain and visual odometry particle filter has generally low uncertainty that is not significantly higher than the version using USBL information. Lastly, the filter using all three measurements has the lowest overall uncertainty and limited growth in uncertainty Figure 14: Image of sample particle distributions for the terrain and visual odometry filter along the trajectory shown over the background bathymetry. The shape of these distributions reflects the shape of the underlying bathymetry.
across the USBL gap. This suggests the additional terrain information effectively curbs the error growth associated with just the visual odometry.

Discussion
The particle filter test cases presented here capture the behavior of the filter when including different measurement sources. The combination of terrain measurements and visual odometry performed well relative to the EKF and particle filters incorporating direct USBL measurements. Some potential biases were ob- Figure 15: XY USBL measurments (red dots) and estimated particle filter trajectory (blue) using visual odometry and USBL positions.
served between the visual odometry and the USBL measurements. This could indicate a camera heading calibration offset. The terrain only filter performed reasonably but its performance could likely be improved by using tighter constraints on the bathymetry measurements and a better treatment of any potential offsets.
The non zero mean of the error histogram shown in Figure 7 indicates that a bias does exist and could be affecting the results. The next step should be a better tidal analysis to separate any time varying and static offsets. The majority of the testing for these results were done on this one sample dive. To further evaluate the parameter choices additional dives should be processed to see if the parameters still provide good results. Lastly, these filters did not include the additional surface GPS measurements available during regular operations. These could also be incorporated into the filter to provide additional constraints at the start and Figure 16: XY USBL measurments (red dots) and estimated particle filter trajectory (blue) using visual odometry, terrain and USBL navigation end of the dives.

Conclusions
This thesis has developed a terrain-based particle filter method for navigating an underwater camera system. The results indicate that the terrain information can effectively be used to estimate the drift track of the float. The combination of visual odometry and terrain information proves the most effect at producing a trajectory estimate close to the direct USBL position measurements. This result is encouraging as it provides an alternative approach to localizing the float images when USBL is not available. This method will help reduce the operating costs for the float by reducing the need for peripheral tracking and ship support. Future improvements to the approach could entail better characterization of the depth errors and the inclusion of the surface GPS and tracking buoy data. The Filter Madgwick block is used for turning magnetometer data into heading.
Robot Localization is the ROS package that solves the Extended Kalman Filter equations. Robot localization will provide the transformation from local coordinate to robot coordinates. Data from the bathymetry map are loaded into the GridMap package and displayed in RViz. The non-parametric particle filter approach, Figure   A.2, is similar in structure but replaces the robot localization package with the particle filter in the Bayesian Filter Library.