Towards Model-Aided Navigation of Underwater Vehicles ∗

This paper reports the development and preliminary experimental evaluation of a model-aided inertial navigation system (INS) for underwater vehicles. The implemented navigation system exploits accurate knowledge of the vehicle dynamics through an experimentally validated mathematical model, relating the water-relative velocity of the vehicle to the forces and moments acting upon it. Together with online current estimation, the model output is integrated in the navigation system. The proposed approach is of practical interest both for underwater navigation when lacking disparate velocity measurements, typically from a Doppler velocity log (DVL), and for systems where the need for redundancy and integrity is important, e.g. during sensor dropouts or failures, or in case of emergency navigation. The presented results verify the concept that with merely an addition of software and no added instrumentation, it is possible to considerably improve the accuracy and robustness of an INS by utilizing the output from a kinetic vehicle model. To the best of our knowledge, this paper is the ﬁrst report on the implementation and experimental evaluation of model-aided INS for underwater vehicle navigation.


Introduction
Deciding which sensor outfit to include in an underwater navigation system is important both from a performance and cost perspective.A typical sensor outfit may consist of standard components such as compass, pressure sensor, and some class of inertial navigation system (INS).In addition, various sources of position aiding may be available, for instance long baseline (LBL) or ultra short baseline (USBL) acoustics, terrain-based techniques, and surface GPS.For an extensive survey on sensor systems and underwater navigation the reader should refer to Kinsey et al. (2006) and references therein.
In practice, a submersible does not have continuous position updates, hence a navigation solution based solely on INS, and in particular low-cost INS, will have an unacceptable position error drift without sufficient aiding.While most high-end systems also incorporate a Doppler velocity log (DVL) in their sensor suite in order to limit the drift, this additional expense is not always feasible for low-cost systems.Even when a DVL unit is included, situations may also occur where it fails to work or measurements are discarded due to decreased quality.In either case, in the absence of DVL measurements, alternative velocity information is required to achieve an acceptable low drift navigation solution between position updates.One possibility is to utilize mathematical models describing the vehicle dynamics, in conjunction with online sea current estimation.
The purpose of this paper is twofold.First, with the aim of providing model-based velocity measurements, an experimentally validated kinetic vehicle model is presented.Second, the potential use of such a model as a mean for aiding the INS of an underwater vehicle is investigated, and the effectiveness of the integrated navigation system is evaluated on experimental data.
To date, the use of model-based state estimators for underwater navigation has primarily focused on applying purely kinematic models, i.e. models describing the vehicle motion without the consideration of the masses or forces that bring it about.State estimators based on kinetic underwater vehicle models are rare.Model-based nonlinear deterministic observers utilizing the knowledge of the vehicle dynamics together with disparate measurements are proposed in Kinsey and Whitcomb (2007); Refsnes et al. (2007).Both papers evaluate their observer using experimental data.As for model-aided INS, some simulation studies have been reported for aerial vehicles (Bryson and Sukkarieh, 2004;Koifman and Bar-Itzhack, 1999;Vasconcelos et al., 2006).To the best of our knowledge however, no results have been reported through simulations or experiments, where the output from a kinetic vehicle model is used to aid the INS of an underwater vehicle.
Note that as studied herein, the integration of vehicle models in underwater navigation systems is of particular interest for systems without a DVL unit.Other important implications involve systems (also having a DVL) where the need for redundancy and integrity is important, e.g. during sensor dropouts or sensor failures, or in case of emergency navigation.
The remainder of this paper is organized as follows.Section 2 presents the mathematical vehicle model utilized in this paper.The integrated navigation system with model aiding included is described in Section 3, including a brief discussion on assumptions applied during development.Section 4 and 5 describe the experimental setup and experimental results, where in particular, the solutions from the navigation systems with and without model aiding in place are compared.

Modeling
The steps involving development and validation of the finite-dimensional mathematical vehicle model utilized in this paper have been rigorously treated in Hegrenaes et al. (2007a).For an extended review and historical recap of work related to modeling of underwater vehicles the reader should refer to the same paper and references therein.The main results are presented in the following.

Preliminaries
In cases where a vehicle operates in a limited geographical area, it is common to apply a flat Earth approximation when describing its location.Let {m} denote a local Earth-fixed coordinate frame where the origin is fixed at the surface of the WGS-84 Earth ellipsoid, and the orientation is north-east-down (NED).Similarly, let {w} denote a reference frame where the origin is fixed to, and translates with the water (due to current).The current is assumed irrotational, hence {w} does not rotate relative to {m}.The frame {b} is a body-fixed frame where the axes coincide with the principal axes of the vehicle.The origin is located at the vehicle center of buoyancy.A general expression of the vehicle position can now be written as where p m wb ∈ R 3 is the vector from the origin of {w} to the origin {b}, decomposed in {m}, and R where Ṙm w equals zero due to the assumption of irrotational current.Multiplying both sides of (2) with R b m finally gives the velocity relationship Analogous to the linear velocities, their angular counterparts are given as ω m mb and ω b mb := R b m ω m mb .For navigation purposes, two additional reference frames are common.The Earth-centered Earth-fixed (ECEF) coordinate frame is denoted {e}.The frame {l} denotes a wander azimuth frame, defined such that it has zero angular velocity relative to the Earth about its z-axis.The initial orientation is NED and its origin is directly above the vehicle at the surface of the Earth ellipsoid.Note that {m} is fixed relative to {e}, and that R b l ≈ R b m for operations in limited geographical areas, far from the poles.In light of the new frames, (3) may be restated as The correspondence between the variables above and the SNAME (1950) notation is shown in Table 1.

Kinetic Vehicle Model
As shown in Fossen (2002) a general expression of the rigid body equations of motion can be written as where M RB is the rigid body inertia matrix, C RB is the corresponding matrix of Coriolis and centripetal terms, and τ RB is a generalized force vector of external forces and moments.For 3 DOF motion in the horizontal plane (surge, sway, and yaw), the generalized force and velocity vectors are The difficulty in modeling an underwater vehicle arises when expressing the right hand-side of (5).One possibility is to linearly decompose τ RB as where the generalized hydrostatic force τ S is known in its exact form.The generalized hydrodynamic force τ H arises from the reaction between the surrounding fluid and the submerged vehicle in motion.The last generalized force component τ consists of forces and moments from propulsion and control surfaces.
The HUGIN 4500 autonomous underwater vehicle (AUV) is used as a case study in this paper.Its bare hull is a body of revolution, and it has a cruciform tail fin configuration that is top-bottom, port-starboard symmetric.A 3 DOF kinetic model for this vehicle can, after adding up the contributions in (6), be written as A description and complete expressions for the various terms are given in Hegrenaes et al. (2007a).Note the difference between ν and ν r = [u r , v r , r] , denoting generalized Earth-relative (inertial) and generalized water-relative velocity, respectively.For (7) one must decide upon using either ν or ν r as the velocity state.As discussed in Hegrenaes et al. (2007a), a reasonable assumption at low vehicle angular rates or small current amplitudes is that ν ≈ νr .This yields the final model where for simplicity we used As seen from (8), the term c(ν, ν r ) depends on both ν and ν r .If there is no current then ν = ν r .Also, only the translational part of ν and ν r differ since the current is irrotational by assumption.The inertial velocity can be calculated from (4), once the current velocity and the water-relative velocity are known.This implies that the current must be measured or estimated.In the integrated navigation system studied in this paper, the current is included as a state in the Kalman filter (KF).
The equation in (8) can be solved using a standard numerical integration routine in order to recover the state.That is, model-based measurements of the waterrelative velocity in surge and sway, as well as the yaw rate, can be attained from control inputs, attitude and current.Both control inputs and the vehicle attitude are usually measured.Also note that (8) was derived assuming negligible coupling from heave, and roll and pitch rate.This is a reasonable assumption for normal operations with the HUGIN 4500 AUV.
The model in (8) is a typical grey-box model where the vehicle behavior is described by a set of nonlinear differential equations with unknown parameters.For the model considered herein, the parameters were found from semi-empirical relationships, open-water test, and from navigation data collected by the HUGIN 4500.More information on the steps involved for identifying the parameters is found in Hegrenaes (2006); Hegrenaes et al. (2007a,b).

Model-Aided Underwater Navigation
Navigation systems built upon inertial principles, time of flight acoustics, velocity logs, and global positioning systems are all common.As pointed out in Kinsey et al. (2006), none of these techniques are perfect however, and in practice a combination of them is usually employed.This section reports the concept and development of an integrated model-aided INS for underwater navigation.

Traditional INS
The key components of any INS consist of an inertial measurement unit (IMU) and a set of equations imple-

INS KF
Aiding sensors mented in software.The navigation equations take the gyro and accelerometer measurements from the IMU as inputs and integrate them to velocity, position and orientation.The general solution of the navigation equations does not require any information on the dynamics of the vehicle in which the IMU is installed.
Since an INS is a diverging system, it requires an aiding system to limit the growth of its errors.Classically, aiding is accomplished using external measurements, e.g.position from acoustics and velocity from a DVL.A coarse schematic diagram of a traditional aided INS is shown in Figure 1, where the input to the KF is the difference between the aiding sensor output and that of the INS.The KF output includes estimates of the accumulated errors in the navigation equations, which are used for resetting the INS and for obtaining the best possible estimate of the true vehicle state (position, velocity and orientation).Besides modeling the INS errors, additional states may also be included in the KF, for instance colored noise in the aiding sensors.

Model-Aided INS
As mentioned above, a necessity to restrain the INS drift is the integration of external aiding sensors.Standard components such as compass and pressure sensor are almost always included, where the latter effectively binds the vertical geographical drift, i.e. drift along the z-axis of {m}, or more precisely {l} (recall Section 2).For navigation in the geographical horizontal plane the situation is more complicated, and to date, the main aiding methods involve time of flight acoustic positioning and Doppler sonar velocity measurements.
A DVL may or may not be part of the sensor suite, and even when it is, situations will occur where it fails to work or measurements are discarded due to decreased quality.In either case, in the absence of DVL measurements, alternative velocity information is required to achieve an acceptable low drift navigation solution between position updates.As for the acoustics, measurements may be available often or only sporadically.Both measurements are crucial for the INS performance.
As is experimentally validated in Section 5, the output from an INS with neither position nor velocity measurements in place, rapidly becomes useless.This leads back to the question addressed in this paper -can the output from a kinetic vehicle model improve the accuracy and robustness of an INS?
The basic idea and concept of using a vehicle model for aiding an INS is illustrated in Figure 2, where the output from the kinetic model is treated analogously to that of an external aiding sensor.The model-aided INS clearly resembles the traditional INS in Figure 1, and both systems may share many of the same aiding sensors.As implemented herein, the DVL unit in the traditional INS is merely replaced by the vehicle model, after doing necessary modifications in the KF.A modelaided INS utilizing both external velocity measurements and model output is subject to ongoing research.Note that the integration of a vehicle model in the navigation system does not require any additional instrumentation.A more detailed outline of the navigation systems is shown in Figure 3, differing only in the velocity aiding.This is illustrated with a switch.The traditional INS with DVL serves as the basis when later evaluating the traditional and model-aided INS in Section 5.

Measurement and Process Equation
A DVL measures the vehicle velocity relative to the bottom, hence it is unaffected by the current.In contrast, the translational velocity calculated by the vehicle model is relative to the water.Consequently, in order to better make use of this velocity estimate for navigation purposes, the current must be accounted for.
In accordance to Figure 2 and conventional KF notation, the general input to the KF is given as where the accent (•) denotes a calculated variable, in this case from the INS.For the velocity we then get where the discrete time index k is dropped for simplicity.
As is standard for INS, the calculated velocity is žvel = vl eb , which ideally implies that z vel = ṽl eb , where the accent (•) denotes a measured quantity.In case of using the output from the vehicle model this is not the case, and the best we can to is to let which after substitution in (10) yields the expression The variables vl eb and Řl b stem from the INS, vb wb is given by the vehicle model, and vl ew can, for instance, be calculated from empirical tide or current tables.If the current was measured it could be used in place of vl ew .In this paper we assume that vl ew = 0, which is to say that our best a priori guess of the current is zero.It does not mean that the true current is zero.Since the model does not include the water-relative velocity in heave as a state, this model output will be assumed to be zero.The inclusion of a depth sensor in the navigation system is presumed to compensate for this simplification.
A true variable is given as the sum of its calculated value and a corresponding error (similarly for a measured quantity), that is, Replacing the current velocity and the vehicle model velocity in (12) with their errors and true values yields which after some manipulation and first order approximations leads to the final expression of the measurement equation associated with the vehicle model where the variable e l lb is a measure of the calculation error in Řl b (Gade, 1997).The variables in (15) are all calculated by the INS or included in the KF process equation.In this work, we assume that the vehicle model output error δv b wb can be modeled as white noise.A more advanced error description is to be implemented in further work.As for the current δv l ew , it is modeled as the sum of colored noise and white noise.The colored noise is implemented as a 1. order Markov process driven by white noise Gelb (1974).The vector entries of δv b wb are assumed uncorrelated.Similarly for δv l ew .Finally note that the KF estimate of δv l ew is also an estimate of the true current, since vl ew = 0 by assumption, and consequently, v l ew = δv l ew .

Experimental Setup
Navigation data collected by the field-deployed HUGIN 4500 AUV are used for evaluating the performance of the model-aided INS proposed in Section 3.An overview of vehicle particulars is given subsequently, followed by a description of the conducted experiments.

Vehicle Specifications
The Kongsberg Maritime HUGIN 4500 is the latest member of the HUGIN AUV family.Figure 4 shows a picture from one of the sea-trials in September 2006.
The length of the vehicle is approximately 6.5 m and the maximum diameter is 1 m.This gives a nominal dry mass of 1950 kg.Designed for large depths and long endurance, the vehicle can operate for 60-70 hours at depths down to 4500 meters.The cruising speed of the vehicle is about 3.7 knots or 1.9 m/s.The vehicle is passively stable in roll and close to neutrally buoyant.
For propulsion, the vehicle is fitted with a single three-bladed propeller.A cruciform tail configuration with four identical control surfaces is used for maneuvering.The vehicle can operate in either UUV (unmanned underwater vehicle) or AUV mode.In AUV mode the vehicle maneuvers without supervision, and independently of the mother ship.In UUV mode the vehicle operates near the mother ship, hence enabling real-time supervision.The data used in this paper were collected while operating in UUV mode.
HUGIN 4500 is equipped with a traditional aided INS.Some IMU specifications are listed in Table 2.In UUV mode the surface ship tracks the submersible with an ultra short baseline acoustic position system (USBL).By combining DGPS with USBL, a global position estimate can be obtained, which is then transmitted to the AUV.Additional navigation sensors include compass, pressure sensor, and Doppler velocity log (DVL).Primary aiding sensors and some of their specifications are listed in Table 3.A schematic outline of the integrated navigation system is shown in Figure 3. Readers are referred to Gade (2004); Jalving et al. (2003a,b) for additional information on the navigation system and navigation system accuracy.

Experimental Description
During September and October 2006, several sea-trials were conducted with the HUGIN 4500 in the vicinity of 59 • 29' N, 10 • 28' E, in the Oslo-fjord, Norway.More than 60 hours of data were collected, of which roughly 3 hours are utilized in this paper.The test area and the horizontal vehicle trajectory are shown in Figure 5.The vehicle followed a standard lawn-mover pattern, typical for a survey AUV like the HUGIN 4500.During the entire run the vehicle was kept at a close to constant depth at 140 meters.Note that no parts of the experimental data used herein were used during the development process of the vehicle model.

Data Post-Processing
The raw data collected by the HUGIN 4500 were postprocessed before being utilized in this paper.The first steps involved wild-point filtering of the position measurements.The HUGIN navigation system then reprocessed the data to get real-time estimates from the KF (this is done using a true copy of the at-sea navigation system).The data were finally smoothed to enhance accuracy.All these steps were done using NavLab (Gade, 2004) and without generating any artificial data.The accuracy of the smoothed vehicle position was estimated to be 0.75 meters (1σ) north and east.The experimentally proven accuracy of the navigation system is thoroughly discussed in Section 5.2.2 of Gade (2004).The smoothed data collected with the vehicle configuration described in Section 4.1 serve as the basis for evaluating the performance of the traditional and model-aided INS.NavLab is also used during the evaluation process in Section 5.

Experimental Evaluation
This section evaluates the performance of the modelaided INS discussed in Section 3. The performance is compared to the traditional aided INS.With exception of the tuning parameters associated with the vehicle model, all the KF parameters are identical.Depth sensor and compass are always included as aiding sensors.
The compass is however given a large covariance and is consequently weighted insignificantly in the KF.The position measurements are available either as logged topside at about 1/3 Hz, or as received onboard the AUV at about 1/30 Hz.External velocity measurements are absent.The position error is taken as the difference between the local position in the basis data and the local position estimated by the navigation system under consideration.The navigation systems are evaluated according to the following two cases:

Topside position fix with dropout
The scenario is best illustrated in Figure 6(a), where the vehicle starts at the same initial position as the basis data.The real-time KF receives position measurements at topside rate for about 83 minutes.The position aiding is then disabled for 30 minutes, before again being enabled for the remaining of the survey.This experiment was done in order to evaluate the performance of the two systems in the case position measurements become unavailable.

Onboard position fix
Similar scenario as before, but with position measurements being available at onboard (AUV-side) rate, and with no extraordinary dropouts.The position fix update rates for the entire run are shown in Figure 7(a).This experiment was done in order to evaluate the performance of the two navigation systems in the case were position measurements are available at a reduced frequency.Note that the navigation accuracy obtained during time slots without position aiding is limited to the accuracy of the KF estimated current.If the current does not vary significantly throughout the time period where position measurements are absent, the navigation accuracy will remain good.

Navigation Performance -Case 2
As can be seen in Figure 7(b), the two navigation systems provide very different estimates of the position uncertainties.A similar behavior was also observed when using position measurements at topside rate.The position uncertainties estimated by the model-aided INS are clearly lower, and they appear more reliable.The estimates are also much smoother.The beneficial effect of including the vehicle model for aiding the INS is apparent in Figure 7(c), where the tallest spikes for the traditional INS correspond to approximately 60 seconds since receiving the preceding position measurement.
In terms of position errors the model-aided INS again performs excellent, and well within one standard deviation (1σ) as seen in Figure 7(d).The traditional INS also performs acceptable in terms of position errors, and comparable to the model-aided INS when the position measurements appear frequently.As mentioned earlier, the drift without external position or velocity aiding is not linear, and the performance of the traditional INS worsens when the position update frequency changes from 1/30 Hz to 1/60 Hz.We conclude that the model-aided INS performs better than the traditional INS during time periods without position aiding, and it provides better error covariance estimates throughout.6 Conclusions and Further Work To the best of our knowledge, the presented results are the first report on the implementation and experimental evaluation of model-aided INS for underwater vehicle navigation.The conclusion has an important practical consequence, and the proposed approach shows promise to improve underwater navigation capabilities both for systems lacking disparate velocity measurements, and for systems where the need for redundancy and integrity is important.

Further Work
A more advanced error description of the vehicle model output may be implemented, and observability conditions for the vehicle model error and the sea current should be investigated.A model-aided INS utilizing both external velocity measurements and vehicle model output is of great practical interest, and should be implemented.This is subject to ongoing research.
m w ∈ SO(3) is the coordinate transformation matrix from {w} to {m}.The velocity of {b} relative to {m}, represented in {m}, is given as v m mb := ṗm mb , or decomposed in {b} as v b mb := R b m v m mb .The interpretation of the other variables follows directly.Taking the time derivative of both sides of (1

Figure 3 :
Figure 3: Block diagram of model-aided (and traditional) INS.Additional velocity measurements are not included in this paper when utilizing the output from the vehicle model, and the other way around when using velocity measurements.This is illustrated with a switch/selector.The position measurements may be available often or only sporadically.

Figure 5 :
Figure 5: Test area and outlier-filtered HUGIN position measurements, logged topside at 1/3 Hz.The square shows the start position.

-Case 1
During the first and last part of the survey, the modelaided INS and the traditional INS are found to perform comparably in terms of calculated position errors.The position uncertainties estimated by the model-aided INS are slightly lower however, and less jagged.For the part without position aiding, the traditional INS breaks down quickly, as can be seen in Figure 6(b) where the maximum Euclidian norm of the position error is close to 700 meters.The model-aided INS continues to perform excellent, and the maximum norm of the position error is 6 meters.From Figure 6(d) this can be seen to be well within the estimated one standard deviation (1σ).The median of the estimated north and east position uncertainties are 1.2 meters.The estimated trajectory is shown in Figure 6(c), closely following the basis data.Overall the model-aided INS performs excellent, and superior to the traditional INS.

Figure 6 :
Figure 6: Traditional and model-aided INS evaluated according to case 1: (a) The red (solid) trajectory serves as basis for evaluating the navigation systems.The red square shows the initial position used in the KF.The blue (o) data show wild-point filtered position measurements logged topside.The segment without position measurements corresponds to 30 minutes.(b) Real-time navigation solution obtained with traditional INS shown in green (dashed).Other data as before.The system shows poor performance without position measurements.(c) Real-time navigation solution obtained with model-aided INS shown in green (dashed).Other data as before.The system shows excellent performance, also without position measurements.The circles (red) indicate 75 and 125 minutes into the run.(d) The true position errors (assuming basis is correct) for the model-aided INS in north and east are shown in blue (solid).The corresponding estimated real-time KF position uncertainties (1σ) are shown in green (dashed).

Table 3 :
Primary navigation aiding sensors Hz.In real-time position updates are received at about 1/30 Hz, from the surface vessel via an acoustic link.
This paper reports the development of a model-aided INS for underwater vehicle navigation.The navigation system is novel in that accurate knowledge of the vehicle dynamics is utilized for aiding the INS, and the navigation performance is experimentally evaluated using real AUV data.It is found that the error in the model-aided INS position estimate is significantly lower than that of the traditional INS throughout time segments where position and velocity measurements are absent.The model-aided INS also performs equally good or better than the traditional INS in cases with regular position updates, and the difference in performance increases with decreasing position update rate.The experimental results demonstrate that with merely an addition of software and no added instrumentation, it is possible to considerably improve the accuracy and robustness of an INS by utilizing the output from a kinetic vehicle model.