Robust ModelAided Inertial Localization for Autonomous Underwater Vehicles
Abstract
This paper presents a manifold based Unscented Kalman Filter that applies a novel strategy for inertial, modelaiding and Acoustic Doppler Current Profiler (ADCP) measurement incorporation. The filter is capable of observing and utilizing the Earth rotation for heading estimation with a tactical grade IMU, and utilizes information from the vehicle model during DVL drop outs. The drag and thrust modelaiding accounts for the correlated nature of vehicle model parameter error by applying them as states in the filter. ADCPaiding provides further information for the modelaiding in the case of DVL bottomlock loss. Additionally this work was implemented using the MTK and ROCK framework in C++, and is capable of running in realtime on computing available on the FlatFish AUV.
The IMU biases are estimated in a fully coupled approach in the navigation filter. Heading convergence is shown on a realworld data set. Further experiments show that the filter is capable of consistent positioning, and data denial validates the method for DVL dropouts due to very low or high altitude scenarios.
I Introduction
Autonomous Underwater Vehicles (AUVs) have found applications in a variety of underwater exploration and monitoring tasks including highresolution, georeferenced optical/acoustic ocean floor mapping and measurements of water column properties such as currents, temperature and salinity [17]. An advantage of AUVs over other methods of ocean observation is the autonomy and decoupling from a surface vessel that a selfcontained robot provides.
The ability to georeference, or to compute the absolute position in a global reference frame, is essential for AUVs for the purposes of path planning for mission requirements, registration with independently navigated information, or revisiting a previous mission. Georeferenced navigation is often achieved by initializing the navigation solution to the GPS while on the surface and, once submerged, relaying on velocity measurements from a Doppler Velocity Log (DVL). When the water depth is less than the range of the DVL (a 300kHz DVL has a range of 200m), the DVL has continuous bottom lock throughout the mission. The DVL sensor provides measurements of the seafloor relative velocity of the AUV. By combining this information with an appropriate heading reference, the observations are placed in the global reference frame and integrated to facilitate underwater dead reckoning. The result is accuracies of 22m per hour (2) in position error growth attainable during diving and 8m per hour error growth (2) is possible if coupled with a navigationgrade ($100K) Inertial Measurement Unit (IMU) [13].
In cases where the seafloor depth is greater than the DVL bottom lock range, transitioning from the surface to the seafloor presents a localization problem [7], since both GPS and DVL are unavailable in the midwater column. Traditional solutions include rangelimited Long Base Line (LBL) acoustic networks requiring deployment, Ultra Short Base Line (USBL) navigation requiring a dedicated ship, or single range navigation from an acoustic beacon attached to a ship [16] or an autonomous surface vehicle (ASV) [9]. In addition to requiring dedicated infrastructure, acoustic positioning also suffers from multipath returns and the need to accurately measure the sound speed profile through the water column. Acoustic methods typically give accuracy at 1km ranges [8] [10].
IMUs provide a strap down navigation capability through providing body accelerations and rotation rates without external aiding such as GPS, acoustic ranging, or DVL velocities. However, IMUs quickly accumulate position errors, with an unaided tactical grade IMU ($10K) drifting at 100km per hour, and a navigation grade IMU drifting at 1km per hour [14]. There also exists cases where DVL bottomlock is not possible, when the altitude is very low, such as in inspection or docking scenarios.
In [5], a modelaiding Inertial Navigation System (INS) is applied with watertrack from the DVL. Comparatively, the novel contributions of the work presented in this paper are as follows:

Utilizing and validating through experiment a manifold based Unscented Kalman Filter (UKF) which can observe and utilize the Earth rotation for heading estimation,

Incorporating and validating a novel drag and thrust modelbased aiding, which accounts for the systematic uncertainty in vehicle parameters by incorporating them as states in the UKF and

Incorporating and validating the use of ADCP measurements in a novel form to further aid the estimation in cases of DVL bottomlock loss.
IMUs with low gyro bias uncertainty allow gyrocompassing by measuring the Earth rotation to estimate heading. The price range for navigation grade IMUs (as used in [5]) with a low bias uncertainty are typically in the $100K USD price range. In this paper, the KVH1750 IMU, in the $10K USD price range, is utilized. In order for this price range IMU to be utilized, the biases are estimated in a fully coupled approach in the navigation filter. Realworld experiments with the FlatFish AUV (Fig. 1) show that less than 1 (2) heading uncertainty is possible in the filter following an initialization within 15 of the true heading (possible from a magnetic sensor). Further experiments also show that the filter is capable of consistent positioning, and data denial validates the method for DVL dropouts due to very low or high altitude scenarios. Additionally this work was implemented using the MTK [6] and ROCK [1] framework in C++^{1}^{1}1The implementation is under open source license available on https://github.com/rockslam/slamuwv_kalman_filters, and is capable of running in realtime on computing available on the FlatFish AUV.
The work in this paper utilizes vehicle modelbased aiding and the ADCP sensor for further water current and vehicle velocity constraints. Modelaiding allows physics based constraints on the positioning, and the uncertainty in each parameter can be set to account for the systematic error associated with a system identification. Thus even a low accuracy system identification can still be used with this filter without resulting in filter overconfidence. Additionally, by modeling the vehicle parameters as timevarying, the model itself has become uncertain, as any small deviations in dynamics from the modeling equations can be absorbed by the timevarying parameters. ADCPaiding in cases where DVL dropout would occur, due to being higher altitude than the bottomlock range, also can aid the model by providing independent vehicle velocity constraints. ADCP also gives information regarding the surrounding water currents when there is a DVL dropout and the vehicle state estimation relies more on modelaiding. Generally, inertial navigation is achieved using errorstate filtering [5], but this is not necessary as is shown in this paper. This paper gives a more conceptually simplified approach, while also utilizing manifold methods [3] to represent attitude, which is more general than other methods.
Ii Modelaided Inertial filter design
Our filter design is conceptually simple, since we model all modalities in one filter and model the attitude of the vehicle as a manifold. We utilize an Unscented Kalman Filter (UKF) since it doesn’t require the Jacobians of the process or measurement models and can handle nonlinearities better than an Extended Kalman Filter [15].
The attitude of the vehicle is an element of , the group of orientations in . To directly estimate the attitude in the filter it can be either modeled by a minimal parametrization (Euler angles) or by a overparametrization (quaternion or rotation matrix). A minimal parametrization has singularities, i.e. small changes in the state space might require large changes in the parameters. An overparametrization has redundant parameters and needs to be renormalized as required. In both cases it requires special treatment in the filter, which leads to a conceptually more complex filter design. Representing the attitude as a manifold is a more general solution in which the filter operates on a locally mapped neighborhood of in [6].

Description  

Position of the IMU in the navigation frame  
Attitude of the IMU in the navigation frame  
Velocity of the IMU in the navigation frame  
Acceleration of the IMU in the navigation frame  
Inertia submatrix of the motion model  
Linear damping submatrix of the motion model  
Quadratic damping submatrix of the motion model  




Gravity in the navigation frame  
Gyro bias  
Accelerometer bias  
Bias in the ADCP measurements 
Table I shows the state vector of the filter as element of and gives a detailed description of the higher dimensional elements of the state vector. The navigation frame is NorthEastDown (NED). The body and IMU frames are xaxis pointing forward, yaxis pointing left and zaxis pointing up. In the filter design we consider the IMU frame not to be rotated with respect to the body frame.
Iia Inertial prediction equations
The following equations describe the prediction models for position, velocity, acceleration and attitude, applying a constant acceleration model for translation and a constant angular velocity model for rotation:
(1) 
(2) 
(3) 
(4) 
where is the position of the IMU in the navigation frame at time , is the velocity of the IMU in the navigation frame, is the acceleration of the IMU in the navigation frame, is the coordinate transformation from body to navigation frame, is the attitude of the IMU in the navigation frame, is the rotation rates in the body frame, is the bias in the gyro sensor and is the Earth rotation in the navigation frame. The operator in (4) is a manifold based addition, as defined in [6]. Equations (1) to (4) each have corresponding prediction noise added.
The accelerometer measurements are handled with an update equation on the acceleration state as follows:
(5) 
where is the specific force acting on the vehicle at time , is the bias in the accelerometer sensor and is the gravity vector in the navigation frame. The gravity state is modeled applying a constant gravity model in order to refine the theoretical gravity according to the WGS84 ellipsoid earth model starting with a small initial uncertainty. The acceleration state in the filter allows both the accelerometer and modelaiding to act on the filter in a consistent fashion, without resorting to virtual correlation terms when an acceleration state does not exist, such as in [5]. Accelerometer and gyro bias terms are modeled as a first order Markov process as follows:
(6) 
where is the expected rate change of the bias, is the mean bias value, and is a zeromean normally distributed random variable with
(7) 
where is a bound to the bias drift and is the measurement frequency. The accelerometer and gyro bias are assumed to be zero mean.
IiB Modelaiding update equations
In this section we show a modelaiding measurement function using a simplified vehicle motion model for which a subset of the parameter space is part of the filter state. This allows the filter to refine the parameters at runtime and to account for the systematic uncertainty in the vehicle parameters.
The nonlinear equations for motion [4] of a rigid body with 6 DOF can be written as
(8) 
where is the vector of forces and torques, is the vector of linear and angular velocities, is the inertia matrix including added mass, is the Coriolis and centripetal matrix, is the hydrodynamic damping matrix and is the vector of gravitational forces and moments given the rotation matrix from the body to the navigation frame .
(9) 
Equation (9) shows how the gravitational forces and moments are calculated given the weight , buoyancy , center of gravity and center of buoyancy of the vehicle, where is the unit vector .
We assume the Coriolis and centripetal forces as well as damping terms higher than second order are negligible for vehicles operating within lower speeds (typically below m/s). This allows us the define the measurement function for the forces and torques in the body frame from (8) as
(10) 
where is the linear acceleration, is the angular acceleration, is the linear velocity and is the angular velocity, all expressed in the bodyfixed frame at time . is the random noise of the force and torque measurement, with a standard deviation given by the thruster manufacturer.
can be computed given the acceleration in the navigation frame as
(11) 
where is the coordinate transformation matrix from navigation to body frame at time and is the position of the IMU in the body frame.
can be computed given the velocity in the navigation frame as
(12) 
where is the water current velocity surrounding the vehicle at time .
Equation (13) shows how the damping is defined given the linear and angular velocities at time .
(13) 
The linear damping matrix , the quadratic damping matrix and the inertia matrix are time dependent, since for all of them a sub matrix is part of the filter state. The filter states , , are defined by removing the rows 3 to 6 and columns 3 to 5 from the full damping and inertia matrices , , . In other words we model the terms of the matrices in the filter, where is the yaw. Because we expect them to have the major impact with respect to the horizontal accelerations and velocities, in case of an AUV operating with a stable roll and pitch. It would be easy to extended the filter states and add more model terms, however it is a tradeoff between the additional benefit, the computational complexity and potential filtering instability.
The damping and inertia state prediction models have a base time varying component, with a timescale of around one hour, modeled as in (6). The vehicle parameters are initialized using a prior system identification, with the means of the states set at these values in the first order Markov process equation. Since the vehicle parameters are states in the filter, the systematic uncertainty in their error can be accounted for, which acts like a bias rather than a noise. This allows even a low accuracy system identification, or very crude estimates of the parameter values, to still allow estimation without resulting in overconfidence, due to the bias error having a stronger and different effect to simply increasing the uncertainty in the vehicle model noise. This also allows the vehicle modeling to adapt to different scenarios, such as surfacing or changes to the vehicle following the system identification, while constraining their value range by utilizing the first order Markov process model. Although these parameters could have been modeled as a constant, by allowing the parameters to have a timevarying component it acts as a way to implement “model uncertainty”. In this way, the robustness of the filter improves as we no longer fully trust our model to be a perfect representation of the true dynamics, which is most definitely the case with applying a simplified and computationally tractable model for realtime usage to the realworld.
IiC ADCPaiding update equations
Given the 3D velocities output from the ADCP, the observation function for each ADCP measurement is
(14) 
where is the ADCP measured current vector in the i measurement cell, is the coordinate transform from navigation/world frame to ADCP/body frame at time , is the vehicle velocity in the world/navigation frame, is the water current velocity near the vehicle, is the water current velocity at the maximum ADCP range, is the bias in the ADCP measurement and is the random noise in the ADCP measurement, with a standard deviation given by the sensor manufacturer.
To reduce the state number of the filter, the vertical velocity of the water currents are not estimated. The ADCP measurement model is a depth dependent function with two water current states, which linearly interpolates between them. The states are located at the vehicle position, and at a water volume at end of the ADCP measurement range. The water velocity and the ADCP bias state prediction models have a base time varying component, with a timescale of around one hour for the water current and half an hour for the bias, modeled as in (6). In addition to this, the water velocity state will vary more given spatial motion through a water current vector field. This component scales the process model uncertainty of the water velocity state according to the vehicle velocity. In this way, if the vehicle is slowly traveling through the water current vector field, it can account for the spatial scale of the water currents, which can depend on the environment. For example, water currents near complex bathymetry or strong wind and tides can contribute to smaller spatial scale water current velocity changes, compared to the case of the midwater ocean [11].
Iii Results
All the experiments have been made using the FlatFish AUV [2] shown in Fig. 1. As relevant sensors for our experiments, the vehicle is equipped with a KVH 1750 IMU, a Rowe SeaProfiler DualFrequency 300/1200 kHz ADCP/DVL, a Paroscientific 8CDP700I pressure sensor, a ublox PAM7Q GPS receiver and six 60N Enitech ring thrusters. For heading evaluation purposes we also use a Tritech Gemini 720i Multibeam Imaging Sonar attached to the AUV. The data sets have been collected during the sea trails of the second phase of the FlatFish project close to the shore of Salvador (Brazil) during April 2017. Since the experiments took place in the open ocean in all data sets, a fiber optic tether was attached to the vehicle for safety reasons. As a result, even though the vehicle model parameters were estimated with a prior system identification, there would be a large error associated with the model given the tether, so there is 20% uncertainty in the parameter values. Nonetheless, the filter is robustly capable of accounting for this increase in the uncertainty of the vehicle model parameters. This allows the filter to adaptively change the parameters while keeping them in a constrained range through the use of the first order Markov process model. The filter is capable of running in 14 realtime with an integration frequency of 100 Hz on computing available on the FlatFish AUV.
Iiia Heading estimation experiment
In this data set we show that the filter is able to find its true heading without a global positioning reference, given an initial guess. The mission consists of a initialization phase on the surface followed by a submerged phase and the resurfacing. During the initialization phase the vehicle moves for around 8 minutes on a straight line in order to estimate its true heading and position by incorporating GPS measurements. In the submerged phase the vehicle changes its heading to face the target coordinate and follows a straight line for about 112 meters to reach it.
Fig. 2 shows six runs of the same data set in different filter configurations. Three GPSaided runs with a different initial heading distributed over 30, one with a close initial guess (black line), one with a 15 positive offset (cyan line) and one with a 15 negative offset (blue line). Due to the help of the GPS measurements the estimated headings converge in the first 5 minutes. The three runs not integrating a global position reference starting with the same heading distribution show that the filter is able to find its true heading by observing the rotation of the earth (gyro compassing), relying only on Inertial and velocities. After 15 minutes the GPSaided and the nonGPSaided estimated headings have converged with an uncertainty below 0.5 (1). The green crosses show multiple independent measurements of the expected vehicle heading based on landmarks (poles) visible in the multibeam imaging sonar on the vehicle. The average difference between the landmark based headings to the filter estimates is below 1. We expect the uncertainties of these measurements to be within 5 due to the uncertainties associated with the pole positions in surveyed maps and in the sonar images.
Using the GPSaided heading with a close initial guess shown in Fig. 2 (black solid line) as ground truth, we can have a closer look in Fig. 3 on the uncertainties and how the estimates improve. In Fig. 3 both filter configurations start with an offset of 15 to the ground truth and an initial uncertainty of 30 (1). The GPSaided heading estimate converges, as expected, quickly to the ground truth while staying in the 1 bound. For the heading estimate without global positioning reference we can see that the strong offset and high uncertainty in the beginning leads to a fast compensation in the correct direction with an overshot slightly exceeding the 1 bound. As the experiment progresses we can see that observing different orientations helps to estimate the gyroscope bias and therefore helps to detect the error between the expected rotation of the earth given the current orientation. We have shown that our filter is able to estimate its true heading by observing the rotation of the earth and that observations from different attitudes help to improve the process.
IiiB Repeated square path experiment
In this experiment we show how the filter performs when the vehicle travels a longer distance of 1 km without horizontal position aiding measurements, such as GPS. The vehicle was following a 5 times repeated square trajectory with an edge length of 50 meter for 1 hour. After resurfacing, the position difference to the GPS ground truth is within 0.5% of the traveled distance.
Starting with an initialization phase (same as in IIIA) on the surface, to estimate its heading and position using GPS measurements, the vehicle submerges to 10 m depth, performs the mission and surfaces at the end. The blue line in Fig. 4 shows the trajectory of the vehicle from minute 20 to minute 80 in the mission, i.e. 1 minute before submerging and 2 minutes after surfacing. The red dots are the GPS measurements including outliers.
The pose filter used on the vehicle at the time the data set was created was not aware of the drift and the initial error in heading. Our filter can correct the heading by observing the rotation of the earth and compensate for DVL dropouts utilizing the motion model. However during the mission a fiber optic tether was attached to the vehicle which represents an unmodeled source of error.
The blue line in Fig. 5 shows the position difference on the North/East plane with respect to the GPS measurements (including outliers). During the first 20 minutes of the mission the GPS measurements are integrated in the filter allowing initialization. After resurfacing (minute 78 and onward) the GPS measurements are not integrated allowing us to observe the difference to the ground truth. After traveling a distance of 1 km the position difference is withing 5 meter (0.5% of distance traveled) and in the 2 bound of the position uncertainty.
In the case that ADCP measurements are not available the filter will estimate the water currents only by the difference between the motion model based velocity and the DVL based velocity, as modeled in (12). Fig. 6 shows the estimated water current velocities in North and East direction during this experiment without the aiding of ADCP measurements. During the first 20 minutes the uncertainties of the water current velocities stay constant, since we apply the modelaiding measurements with an increased uncertainty in case the vehicle is surfaced. When the mission starts and the vehicle submerges (starting around minute 21) to a depth of 10 meters we can see that the estimated water flow changes to the one on the surface and that its velocity continuously increases during the 1 hour mission. The uncertainties reduce during this phase since we trust the model more when submerged. The impact of the tether attached to the vehicle is seen as an unmodeled but estimated drag, which changes depending on the direction the vehicle travels.
Fig. 7 shows the linear damping terms on the x and yaxis in the body frame of the vehicle and how they are refined during the mission. Because the vehicle travels during the mission mainly in the forward direction, the damping term on the xaxis is refined and the corresponding uncertainty reduces more compared to the yaxis damping term. The uncertainty reduction reaches a limit however due to observability, and the first order Markov process model ensures that the parameters become neither overconfident nor unconstrained. In this way, the model parameters can adapt with time to new conditions and implicitly represents some uncertainty in the model equations themselves.
IiiC Square path with ADCP
Filter measurements used 




Inertial + ADCP  50.9 m (2)  22.0 m  
Inertial + model  45.7 m/s (2)  21.5 m  
Inertial + model + ADCP  32.3 m/s (2)  16.1 m 
This mission undergoes a 600 second initialization phase on the surface (as in IIIA), then 1000 seconds of data denial to show the performance of the filter in different scenarios. During the data denial phase, the vehicle completes a square trajectory, and surfaces at the corners. The ground truth trajectory is shown in Fig. 8. The ground truth is determined using Inertial, DVL, GPS, ADCP and modelaiding.
Since this mission also includes ADCP measurements interleaved with DVL, the ADCPaiding update is applied. During this mission, there are cases where the downward facing DVL drops out due to very low altitude (between 02m during the mission), and there is collision with the sandy bottom. Despite this challenging data set, the filter is capable of estimating the position of the vehicle, validated by the smooth trajectory without sudden corrections at the GPS measurements during the corner surfacing with Fig. 8.
With the full measurement filter (without data denial), the filter is able to handle DVL drop outs, which could be the case in lowaltitude scenarios such as inspection or docking, by letting the modelaiding fill in during these time periods. Data denial further validates the filter performance in DVL loss scenarios, as shown in Fig. 9. In cases of DVL bottomlock loss due to altitude being too high (simulated through datadenial), the ADCP and modelaiding combined gives the best solution, compared to either ADCP or modelaiding alone.
The position estimate differences compared to the ground truth for these data denials are consistent with the 2 uncertainty bounds, while remaining stable. At approximately 400 seconds following data denial, the filter with only ADCP and inertial measurements appears to slightly exceed the 2 bounds, due to a low altitude section with very little valid ADCP measurements, and some ADCP outliers are incorporated into the filter since the innovation gate increases due to inertialonly deadreckoning. The ground truth also increases in uncertainty at this stage due to the lack of DVL measurements, relying more on the modelaiding. Following further measurements, the filter recovers and is able to reduce the difference between the filter estimate and the ground truth. This is possible since the water current estimate will not vary significantly in this timescale, so that the vehicle can use this state when there are ADCP measurements available again to estimate the velocity and thus position of the vehicle.
The ADCPaiding typically performed worse in this case than the modelaiding, but this can be attributed to low altitude where there are very few valid ADCP measurements available. Nonetheless, incorporating these ADCP measurements into the modelaiding improved on the performance of either option. In addition to another source of velocityaiding information from the ADCP, it also allows an independent source of information regarding the water currents surrounding the vehicle, which is required to transform the water relative velocity of the vehicle model to the navigation frame position used in the filter.
The results are further quantitatively compared in Table II. The combination of the ADCPaiding and modelaiding results in a significant improvement compared to modelaiding alone, reducing position uncertainty from 45.7m (2) to 32.3m (2) during 1000 seconds of data denial.
Iv Conclusions
The filter designed and implemented in this paper would be appropriate for general AUV navigation, despite not using a navigation grade IMU. In comparison to [5], the primary insight to the design of this filter is the incorporation of the acceleration state, and adding many parameters as states to account for their correlated error, while modeling with a first order Markov process to constrain the change the filter can apply. The engineering design tradeoff is that adding too many states will unnecessarily add computational complexity and potential filtering instability.
This furthers the stateoftheart for robust filter design for INS, modelaiding and ADCP measurements, capable of realtime performance, consistency and stability as outlined in the experiments, while remaining conceptually simple. This paper has shown a manifold based UKF that applies a novel strategy for inertial, modelaiding and ADCP measurement incorporation. The filter is capable of observing and utilizing the Earth rotation for heading estimation to within 1 (2) by estimating the KVH 1750 IMU biases. The drag and thrust modelaiding accounts for the correlated nature of vehicle model parameter error by applying them as states in the filter. The usage of the modelaiding is validated through observing that the filter remains consistent and does not become overconfident or unstable in the realworld experiments, despite uncertain vehicle model parameters.
It is hypothesized that the usage of time varying first order Markov processes to model these parameters act as a way to implement “model uncertainty”, improving the robustness of the filter as we no longer fully trust our model to be a perfect representation of the true dynamics, which is most definitely the case with applying simplified and computationally tractable model for realtime usage to the realworld. ADCPaiding provides further information for the modelaiding in the case of DVL bottomlock loss. The importance of water current estimation is highlighted in underwater navigation in the absence of external aiding, justifying the use of the modelaiding and ADCP sensor. Through data denial, scenarios with no DVL bottom lock are shown to be consistently estimated. Additionally this work was implemented using the MTK and ROCK framework in C++, and is capable of running in 14 realtime on computing available on the FlatFish AUV.
Future work would include full spatiotemporal realtime ADCP based methods to more accurately model and observe the water current state around vehicle. This requires implementing a mapping approach, such as the work from [12] [11]. The primary source of bias uncertainty for the KVH 1750 IMU is due to temperature change. If the temperature of the IMU can be controlled, or this bias can be calibrated for with further experiments, then the performance can be further improved. Further heading evaluation will be possible with better ground truth, such as a visual confirmation or by utilizing an independent heading estimator such as an IXSea PHINS, so that a more accurate heading comparison can be undertaken. The error in alignments of sensors could also be further compensated, perhaps by adding states to the filter similar to the strategy for other systematic biases. Finally, further experiments and implementations in a variety of scenarios are planned to further test and refine the proposed filtering strategy.
Acknowledgement
The authors like to thank Shell and SENAI CIMATEC for the opportunity to test the presented work on FlatFish.
We also like to thank all colleagues in the FlatFish team for their support.
References
 [1] Rock, the Robot Construction Kit. http://www.rockrobotics.org.
 [2] Jan Albiez, Sylvain Joyeux, Christopher Gaudig, Jens Hilljegerdes, Sven Kroffke, Christian Schoo, Sascha Arnold, Geovane Mimoso, Pedro Alcantara, Rafael Saback, et al. Flatfisha compact subsearesident inspection auv. In OCEANS’15 MTS/IEEE Washington, pages 1–8. IEEE, 2015.
 [3] Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. Onmanifold preintegration theory for fast and accurate visualinertial navigation. IEEE Trans Robot, pages 1–18, 2015.
 [4] Thor I Fossen. Marine control systems: guidance, navigation and control of ships, rigs and underwater vehicles. Marine Cybernetics, 2002.
 [5] Øyvind Hegrenaes and Oddvar Hallingstad. Modelaided INS with sea current estimation for robust underwater navigation. IEEE Journal of Oceanic Engineering, 36(2):316–337, 2011.
 [6] Christoph Hertzberg, René Wagner, Udo Frese, and Lutz Schröder. Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds. Information Fusion, 14(1):57–77, 2013.
 [7] James C Kinsey, Qingjun Yang, and Jonathan C Howland. Nonlinear dynamic modelbased state estimators for underwater navigation of remotely operated vehicles. Control Systems Technology, IEEE Transactions on, (99):1–1, 2014.
 [8] J.C. Kinsey, R.M. Eustice, and L.L. Whitcomb. A survey of underwater vehicle navigation: Recent advances and new challenges. In Proceedings of the 7th Conference on Maneuvering and Control of Marine Craft (MCMCâ2006). IFAC, Lisbon. Citeseer, 2006.
 [9] J.C Kinsey, M.V. Jakuba, and C.R. German. A long term vision for longrange shipfree deep ocean operations: persistent presence through coordination of autonomous surface vehicles and autonomous underwater vehicles. In Workshop on Marine Robotics and Applications. Looking into the Crystal Ball: 20 years hence in Marine Robotics, Canary Islands, Spain, February 2013. Invited.
 [10] M. Mandt, K. Gade, and B. Jalving. Integrating DGPSUSBL position measurements with inertial navigation in the HUGIN 3000 AUV. In Proceedings of the 8th Saint Petersburg International Conference on Integrated Navigation Systems, Saint Petersburg, Russia, 2001.
 [11] Lashika Medagoda, James C Kinsey, and Martin Eilders. Autonomous underwater vehicle localization in a spatiotemporally varying water current field. In Robotics and Automation (ICRA), 2015 IEEE International Conference on, pages 565–572. IEEE, 2015.
 [12] Lashika Medagoda, Stefan B Williams, Oscar Pizarro, James C Kinsey, and Michael V Jakuba. Midwater current aided localization for autonomous underwater vehicles. Autonomous Robots, 40(7):1207–1227, 2016.
 [13] F. Napolitano, A. Chapelon, A. Urgell, and Y. Paturel. PHINS: The autonomous navigation solution. Sea Technology, 45(2):55–58, 2004.
 [14] D.H. Titterton and J.L. Weston. Strapdown inertial navigation technology. Peter Peregrinus Ltd, 2004.
 [15] Eric A Wan and Rudolph Van Der Merwe. The unscented kalman filter for nonlinear estimation. In Adaptive Systems for Signal Processing, Communications, and Control Symposium 2000. ASSPCC. The IEEE 2000, pages 153–158. Ieee, 2000.
 [16] S.E. Webster, R.M. Eustice, H. Singh, and L.L. Whitcomb. Advances in singlebeacon onewaytraveltime acoustic navigation for underwater vehicles. The International Journal of Robotics Research, page 0278364912446166, 2012.
 [17] Dana R Yoerger, Albert M Bradley, and Barrie B Walden. The Autonomous Benthic Explorer (ABE): A deep ocean AUV for scientific seafloor survey. Oceanographic, page 79, 1991.