towards model-aided navigation of underwater vehicles · underwater navigation has primarily...

Modeling, Identification and Control, Vol. 28, No. 4, 2007, pp. 113–123

Towards Model-Aided Navigation ofUnderwater Vehicles∗

Øyvind Hegrenæs1, 2 Oddvar Hallingstad1, 2 Kenneth Gade 3

1University Graduate Center at Kjeller (UNIK), NO-2027 Kjeller, Norway. E-mail: {hegrenas,oh}@unik.no

2Department of Engineering Cybernetics, Norwegian University of Science and Technology (NTNU), NO-7491Trondheim, Norway.

3Norwegian Defence Research Establishment (FFI), NO-2027 Kjeller, Norway. E-mail: [email protected]

Abstract

This paper reports the development and preliminary experimental evaluation of a model-aided inertialnavigation system (INS) for underwater vehicles. The implemented navigation system exploits accurateknowledge of the vehicle dynamics through an experimentally validated mathematical model, relating thewater-relative velocity of the vehicle to the forces and moments acting upon it. Together with online currentestimation, the model output is integrated in the navigation system. The proposed approach is of practicalinterest both for underwater navigation when lacking disparate velocity measurements, typically from aDoppler 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 verifythe concept that with merely an addition of software and no added instrumentation, it is possible toconsiderably improve the accuracy and robustness of an INS by utilizing the output from a kinetic vehiclemodel. To the best of our knowledge, this paper is the first report on the implementation and experimentalevaluation of model-aided INS for underwater vehicle navigation.

Keywords: Inertial navigation; Kalman filtering; Model aiding; State estimation; Underwater vehicles.

1 Introduction

Deciding which sensor outfit to include in an underwaternavigation system is important both from a performanceand cost perspective. A typical sensor outfit may consistof standard components such as compass, pressure sen-sor, and some class of inertial navigation system (INS).In addition, various sources of position aiding may beavailable, for instance long baseline (LBL) or ultra shortbaseline (USBL) acoustics, terrain-based techniques,and surface GPS. For an extensive survey on sensorsystems and underwater navigation the reader should

∗Published in Proceedings of the 15th International Sympo-sium on Unmanned Untethered Submersible Technology(UUST’07), Durham, NH, USA, August 19-22, 2007.

refer to Kinsey et al. (2006) and references therein.In practice, a submersible does not have continuous

position updates, hence a navigation solution basedsolely on INS, and in particular low-cost INS, will havean unacceptable position error drift without sufficientaiding. While most high-end systems also incorporate aDoppler velocity log (DVL) in their sensor suite in orderto limit the drift, this additional expense is not alwaysfeasible for low-cost systems. Even when a DVL unitis included, situations may also occur where it fails towork or measurements are discarded due to decreasedquality. In either case, in the absence of DVL mea-surements, alternative velocity information is requiredto achieve an acceptable low drift navigation solutionbetween position updates. One possibility is to utilize

ISSN 1890–1328 c© 2007 Norwegian Society of Automatic Controldoi:10.4173/mic.2007.4.3

Hegrenæs et al., “Towards Model-Aided Navigation of Underwater Vehicles”

mathematical models describing the vehicle dynamics,in conjunction with online sea current estimation.

The purpose of this paper is twofold. First, with theaim of providing model-based velocity measurements,an experimentally validated kinetic vehicle model ispresented. Second, the potential use of such a modelas a mean for aiding the INS of an underwater vehicleis investigated, and the effectiveness of the integratednavigation system is evaluated on experimental data.

To date, the use of model-based state estimators forunderwater navigation has primarily focused on ap-plying purely kinematic models, i.e. models describingthe vehicle motion without the consideration of themasses or forces that bring it about. State estimatorsbased on kinetic underwater vehicle models are rare.Model-based nonlinear deterministic observers utiliz-ing the knowledge of the vehicle dynamics togetherwith disparate measurements are proposed in Kinseyand Whitcomb (2007); Refsnes et al. (2007). Both pa-pers evaluate their observer using experimental data.As for model-aided INS, some simulation studies havebeen 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 resultshave been reported through simulations or experiments,where the output from a kinetic vehicle model is usedto aid the INS of an underwater vehicle.

Note that as studied herein, the integration of vehiclemodels in underwater navigation systems is of partic-ular interest for systems without a DVL unit. Otherimportant implications involve systems (also having aDVL) where the need for redundancy and integrity isimportant, 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 uti-lized in this paper. The integrated navigation systemwith model aiding included is described in Section 3, in-cluding a brief discussion on assumptions applied duringdevelopment. Section 4 and 5 describe the experimentalsetup and experimental results, where in particular, thesolutions from the navigation systems with and withoutmodel aiding in place are compared.

2 Modeling

The steps involving development and validation of thefinite-dimensional mathematical vehicle model utilizedin this paper have been rigorously treated in Hegrenæset al. (2007a). For an extended review and historicalrecap of work related to modeling of underwater vehiclesthe reader should refer to the same paper and referencestherein. The main results are presented in the following.

2.1 Preliminaries

In cases where a vehicle operates in a limited geograph-ical area, it is common to apply a flat Earth approxi-mation when describing its location. Let {m} denote alocal Earth-fixed coordinate frame where the origin isfixed at the surface of the WGS-84 Earth ellipsoid, andthe orientation is north-east-down (NED). Similarly, let{w} denote a reference frame where the origin is fixedto, and translates with the water (due to current). Thecurrent is assumed irrotational, hence {w} does notrotate relative to {m}. The frame {b} is a body-fixedframe where the axes coincide with the principal axes ofthe vehicle. The origin is located at the vehicle center ofbuoyancy. A general expression of the vehicle positioncan now be written as

pmmb = pm

mw + pmwb

= pmmw + Rm

w pwwb, (1)

where pmwb ∈ R3 is the vector from the origin of {w} to

the origin {b}, decomposed in {m}, and Rmw ∈ SO(3)

is the coordinate transformation matrix from {w} to{m}. The velocity of {b} relative to {m}, representedin {m}, is given as vm

mb := pmmb, or decomposed in {b}

as vbmb := Rb

mvmmb. The interpretation of the other

variables follows directly. Taking the time derivative ofboth sides of (1) yields

pmmb = pm

mw + Rmw pw

wb + Rmw pw

wb

= pmmw + Rm

w pwwb, (2)

where Rmw equals zero due to the assumption of irrota-

tional current. Multiplying both sides of (2) with Rbm

finally gives the velocity relationship

vbmb = Rb

mvmmw + vb

wb. (3)

Analogous to the linear velocities, their angular coun-terparts are given as ωm

mb and ωbmb := Rb

mωmmb.

For navigation purposes, two additional referenceframes are common. The Earth-centered Earth-fixed(ECEF) coordinate frame is denoted {e}. The frame{l} denotes a wander azimuth frame, defined such thatit has zero angular velocity relative to the Earth aboutits z-axis. The initial orientation is NED and its originis directly above the vehicle at the surface of the Earthellipsoid. Note that {m} is fixed relative to {e}, andthat Rb

l ≈ Rbm for operations in limited geographical

areas, far from the poles. In light of the new frames,(3) may be restated as

vbeb = Rb

l vlew + vb

wb. (4)

The correspondence between the variables above andthe SNAME (1950) notation is shown in Table 1.

114


Table 1: NomenclatureDescription Variable Entries*

Local vehicle position pmmb (x, y, z)

Earth-relative linear velocity vbmb = vb

eb (u, v, w)

Water-relative linear velocity vbwb (ur, vr, wr)

Current velocity vlmw = vl

ew (ulc, v

lc, w

lc)

Vehicle angular velocity ωbmb = ωb

eb (p, q, r)

External forces on vehicle fb (X,Y, Z)

External moments on vehicle mb (K,M,N)

Attitude (roll, pitch, yaw) Θ (φ, θ, ψ)

* Based on SNAME notation.

2.2 Kinetic Vehicle Model

As shown in Fossen (2002) a general expression of therigid body equations of motion can be written as

MRBν + CRB(ν)ν = τRB , (5)

where MRB is the rigid body inertia matrix, CRB is thecorresponding matrix of Coriolis and centripetal terms,and τRB is a generalized force vector of external forcesand moments. For 3 DOF motion in the horizontalplane (surge, sway, and yaw), the generalized force andvelocity vectors are τRB = [X, Y,N ]> and ν = [u, v, r]>.

The difficulty in modeling an underwater vehiclearises when expressing the right hand-side of (5). Onepossibility is to linearly decompose τRB as

τRB = τS + τH + τ , (6)

where the generalized hydrostatic force τS is known inits exact form. The generalized hydrodynamic forceτH arises from the reaction between the surroundingfluid and the submerged vehicle in motion. The lastgeneralized force component τ consists of forces andmoments from propulsion and control surfaces.

The HUGIN 4500 autonomous underwater vehicle(AUV) is used as a case study in this paper. Its barehull is a body of revolution, and it has a cruciformtail fin configuration that is top-bottom, port-starboardsymmetric. A 3 DOF kinetic model for this vehicle can,after adding up the contributions in (6), be written as

MRBν + CRB(ν)ν = τ −MAνr −CA(νr)νr−d(νr)νr − l(νr)− g(Θ). (7)

A description and complete expressions for the vari-ous terms are given in Hegrenæs et al. (2007a). Notethe difference between ν and νr = [ur, vr, r]>, denot-ing generalized Earth-relative (inertial) and generalizedwater-relative velocity, respectively.

For (7) one must decide upon using either ν or νr

as the velocity state. As discussed in Hegrenæs et al.

(2007a), a reasonable assumption at low vehicle angularrates or small current amplitudes is that ν ≈ νr. Thisyields the final model

Mνr = τ − c(ν,νr)− d(νr)νr − l(νr)− g(Θ), (8)

where for simplicity we used

M := MRB + MA

c(ν,νr) := CRB(ν)ν + CA(νr)νr.

As seen from (8), the term c(ν,νr) depends on both νand νr. If there is no current then ν = νr. Also, onlythe translational part of ν and νr differ since the currentis irrotational by assumption. The inertial velocity canbe calculated from (4), once the current velocity andthe water-relative velocity are known. This implies thatthe current must be measured or estimated. In theintegrated navigation system studied in this paper, thecurrent is included as a state in the Kalman filter (KF).

The equation in (8) can be solved using a standardnumerical integration routine in order to recover thestate. That is, model-based measurements of the water-relative velocity in surge and sway, as well as the yawrate, can be attained from control inputs, attitude andcurrent. Both control inputs and the vehicle attitudeare usually measured. Also note that (8) was derivedassuming negligible coupling from heave, and roll andpitch rate. This is a reasonable assumption for normaloperations with the HUGIN 4500 AUV.

The model in (8) is a typical grey-box model wherethe vehicle behavior is described by a set of nonlineardifferential equations with unknown parameters. Forthe model considered herein, the parameters were foundfrom semi-empirical relationships, open-water test, andfrom navigation data collected by the HUGIN 4500.More information on the steps involved for identifyingthe parameters is found in Hegrenæs (2006); Hegrenæset al. (2007a,b).

3 Model-Aided UnderwaterNavigation

Navigation systems built upon inertial principles, timeof flight acoustics, velocity logs, and global positioningsystems are all common. As pointed out in Kinseyet al. (2006), none of these techniques are perfect how-ever, and in practice a combination of them is usuallyemployed. This section reports the concept and develop-ment of an integrated model-aided INS for underwaternavigation.

3.1 Traditional INS

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

115


INS KF

Aidingsensors

z δz−

z

δx

x

Reset

x

Figure 1: High-level outline if traditional aided INS.

mented in software. The navigation equations take thegyro and accelerometer measurements from the IMUas inputs and integrate them to velocity, position andorientation. The general solution of the navigation equa-tions does not require any information on the dynamicsof the vehicle in which the IMU is installed.

Since an INS is a diverging system, it requires an aid-ing 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 INSis shown in Figure 1, where the input to the KF is thedifference between the aiding sensor output and thatof the INS. The KF output includes estimates of theaccumulated errors in the navigation equations, whichare used for resetting the INS and for obtaining thebest possible estimate of the true vehicle state (position,velocity and orientation). Besides modeling the INSerrors, additional states may also be included in theKF, for instance colored noise in the aiding sensors.

3.2 Model-Aided INS

As mentioned above, a necessity to restrain the INS driftis the integration of external aiding sensors. Standardcomponents such as compass and pressure sensor arealmost always included, where the latter effectivelybinds the vertical geographical drift, i.e. drift alongthe z-axis of {m}, or more precisely {l} (recall Section2). For navigation in the geographical horizontal planethe situation is more complicated, and to date, themain aiding methods involve time of flight acousticpositioning 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 towork or measurements are discarded due to decreasedquality. In either case, in the absence of DVL mea-surements, alternative velocity information is requiredto achieve an acceptable low drift navigation solutionbetween position updates. As for the acoustics, mea-surements may be available often or only sporadically.

INS KF

Aidingsensors

Vehiclemodel

z δz−

z

δx

x

Reset

Resetx

Figure 2: High-level outline of model-aided INS.

Both measurements are crucial for the INS performance.As is experimentally validated in Section 5, the outputfrom an INS with neither position nor velocity measure-ments in place, rapidly becomes useless. This leads backto the question addressed in this paper – can the outputfrom a kinetic vehicle model improve the accuracy androbustness of an INS?

The basic idea and concept of using a vehicle modelfor aiding an INS is illustrated in Figure 2, where theoutput from the kinetic model is treated analogouslyto that of an external aiding sensor. The model-aidedINS clearly resembles the traditional INS in Figure 1,and both systems may share many of the same aidingsensors. As implemented herein, the DVL unit in thetraditional INS is merely replaced by the vehicle model,after doing necessary modifications in the KF. A model-aided INS utilizing both external velocity measurementsand model output is subject to ongoing research. Notethat the integration of a vehicle model in the navigationsystem does not require any additional instrumentation.A more detailed outline of the navigation systems isshown in Figure 3, differing only in the velocity aiding.This is illustrated with a switch. The traditional INSwith DVL serves as the basis when later evaluating thetraditional and model-aided INS in Section 5.

3.3 Measurement and Process Equation

A DVL measures the vehicle velocity relative to thebottom, hence it is unaffected by the current. In con-trast, the translational velocity calculated by the vehiclemodel is relative to the water. Consequently, in order tobetter make use of this velocity estimate for navigationpurposes, the current must be accounted for.

In accordance to Figure 2 and conventional KF nota-tion, the general input to the KF is given as

δzk = zk − zk, (9)

where the accent (·) denotes a calculated variable, inthis case from the INS. For the velocity we then get

δzvel = zvel − zvel, (10)

116


INS KF

Vehicle modelVelocity

measurements

Control

Compass

Pressuresensor

Positionmeasurements

Velocity−

Attitude−

Depth−

Position

Reset Current

−

Smoothing

Estimates

Figure 3: Block diagram of model-aided (and tradi-tional) INS. Additional velocity measure-ments are not included in this paper whenutilizing the output from the vehicle model,and the other way around when using veloc-ity measurements. This is illustrated with aswitch/selector. The position measurementsmay be available often or only sporadically.

where the discrete time index k is dropped for simplicity.As is standard for INS, the calculated velocity is zvel =vl

eb, which ideally implies that zvel = vleb, where the

accent (·) denotes a measured quantity. In case of usingthe output from the vehicle model this is not the case,and the best we can to is to let

zvel := Rlbv

bwb + vl

ew, (11)

which after substitution in (10) yields the expression

δz = Rlbv

bwb + vl

ew − vleb. (12)

The variables vleb and Rl

b stem from the INS, vbwb is

given by the vehicle model, and vlew can, for instance,

be calculated from empirical tide or current tables. Ifthe current was measured it could be used in place ofvl

ew. In this paper we assume that vlew = 0, which

is to say that our best a priori guess of the current iszero. It does not mean that the true current is zero.Since the model does not include the water-relativevelocity in heave as a state, this model output will beassumed to be zero. The inclusion of a depth sensor inthe navigation system is presumed to compensate forthis simplification.

A true variable is given as the sum of its calculatedvalue and a corresponding error (similarly for a mea-sured quantity), that is,

(·) = (·) + δ(·) or (·) = (·) + δ(·). (13)

Replacing the current velocity and the vehicle modelvelocity in (12) with their errors and true values yields

δz = Rlb(v

bwb − δvb

wb) + (vlew − δvl

ew)− vleb, (14)

which after some manipulation and first order approxi-mations leads to the final expression of the measurementequation associated with the vehicle model

δzvel = δvleb − S(vl

eb)ellb − Rl

bδvbwb − δvl

ew, (15)

where the variable ellb is a measure of the calculation

error in Rlb (Gade, 1997). The variables in (15) are all

calculated by the INS or included in the KF processequation. In this work, we assume that the vehiclemodel output error δvb

wb can be modeled as white noise.A more advanced error description is to be implementedin further work. As for the current δvl

ew, it is modeledas the sum of colored noise and white noise. The colorednoise is implemented as a 1. order Markov process drivenby white noise Gelb (1974). The vector entries of δvb

wb

are assumed uncorrelated. Similarly for δvlew. Finally

note that the KF estimate of δvlew is also an estimate

of the true current, since vlew = 0 by assumption, and

consequently, vlew = δvl

ew.

4 Experimental Setup

Navigation data collected by the field-deployed HUGIN4500 AUV are used for evaluating the performance ofthe model-aided INS proposed in Section 3. An overviewof vehicle particulars is given subsequently, followed bya description of the conducted experiments.

4.1 Vehicle Specifications

The Kongsberg Maritime HUGIN 4500 is the latestmember of the HUGIN AUV family. Figure 4 shows apicture from one of the sea-trials in September 2006.

The length of the vehicle is approximately 6.5 m andthe maximum diameter is 1 m. This gives a nominaldry mass of 1950 kg. Designed for large depths andlong endurance, the vehicle can operate for 60-70 hoursat depths down to 4500 meters. The cruising speed ofthe vehicle is about 3.7 knots or 1.9 m/s. The vehicle ispassively stable in roll and close to neutrally buoyant.

For propulsion, the vehicle is fitted with a singlethree-bladed propeller. A cruciform tail configurationwith four identical control surfaces is used for maneuver-ing. The vehicle can operate in either UUV (unmanned

117


Figure 4: The HUGIN 4500 AUV during sea-trial.

underwater vehicle) or AUV mode. In AUV mode thevehicle maneuvers without supervision, and indepen-dently of the mother ship. In UUV mode the vehicleoperates near the mother ship, hence enabling real-timesupervision. The data used in this paper were collectedwhile operating in UUV mode.

HUGIN 4500 is equipped with a traditional aidedINS. Some IMU specifications are listed in Table 2. InUUV mode the surface ship tracks the submersible withan ultra short baseline acoustic position system (USBL).By combining DGPS with USBL, a global position esti-mate can be obtained, which is then transmitted to theAUV. Additional navigation sensors include compass,pressure sensor, and Doppler velocity log (DVL). Pri-mary aiding sensors and some of their specifications arelisted in Table 3. A schematic outline of the integratednavigation system is shown in Figure 3. Readers arereferred to Gade (2004); Jalving et al. (2003a,b) foradditional information on the navigation system andnavigation system accuracy.

4.2 Experimental Description

During September and October 2006, several sea-trialswere conducted with the HUGIN 4500 in the vicinity of59◦ 29’ N, 10◦ 28’ E, in the Oslo-fjord, Norway. Morethan 60 hours of data were collected, of which roughly3 hours are utilized in this paper. The test area andthe horizontal vehicle trajectory are shown in Figure5. The vehicle followed a standard lawn-mover pattern,typical for a survey AUV like the HUGIN 4500. Dur-ing the entire run the vehicle was kept at a close toconstant depth at 140 meters. Note that no parts ofthe experimental data used herein were used during thedevelopment process of the vehicle model.

4.3 Data Post-Processing

The raw data collected by the HUGIN 4500 were post-processed before being utilized in this paper. The first

Longitude [deg]

Latitu

de

[deg

]

40 60 80 100 120 140 160 180

10.42 10.43 10.44 10.45 10.46 10.47 10.48 10.49 10.5

59.47

59.48

59.49

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

steps involved wild-point filtering of the position mea-surements. The HUGIN navigation system then re-processed the data to get real-time estimates from theKF (this is done using a true copy of the at-sea nav-igation system). The data were finally smoothed toenhance accuracy. All these steps were done usingNavLab (Gade, 2004) and without generating any ar-tificial data. The accuracy of the smoothed vehicleposition was estimated to be 0.75 meters (1σ) northand east. The experimentally proven accuracy of thenavigation system is thoroughly discussed in Section5.2.2 of Gade (2004). The smoothed data collectedwith the vehicle configuration described in Section 4.1serve as the basis for evaluating the performance of thetraditional and model-aided INS. NavLab is also usedduring the evaluation process in Section 5.

5 Experimental Evaluation

This section evaluates the performance of the model-aided INS discussed in Section 3. The performance iscompared to the traditional aided INS. With exceptionof the tuning parameters associated with the vehiclemodel, all the KF parameters are identical. Depth sen-sor and compass are always included as aiding sensors.The compass is however given a large covariance and isconsequently weighted insignificantly in the KF. Theposition measurements are available either as loggedtopside at about 1/3 Hz, or as received onboard theAUV at about 1/30 Hz. External velocity measure-ments are absent. The position error is taken as thedifference between the local position in the basis data

118


Table 2: IMU specificationsModel Gyro Technology Gyro Bias Accelerometer Bias

IXSEA IMU90 Fiber optic ±0.05◦/h ±500 µg

Table 3: Primary navigation aiding sensorsVariable Sensor Precision Rate

Position Kongsberg HiPAP Range, Angle: < 20 cm, 0.12◦ Varying*Velocity RDI WHN 300 ±0.4%± 0.2 cm/s 1HzPressure Paroscientific 0.01 % full scale 1Hz

* Approximately 1/3 Hz. In real-time position updates are received at about1/30 Hz, from the surface vessel via an acoustic link.

and the local position estimated by the navigation sys-tem under consideration. The navigation systems areevaluated according to the following two cases:

5.0.1 Topside position fix with dropout

The scenario is best illustrated in Figure 6(a), where thevehicle starts at the same initial position as the basisdata. The real-time KF receives position measurementsat topside rate for about 83 minutes. The position aid-ing is then disabled for 30 minutes, before again beingenabled for the remaining of the survey. This experi-ment was done in order to evaluate the performanceof the two systems in the case position measurementsbecome unavailable.

5.0.2 Onboard position fix

Similar scenario as before, but with position measure-ments being available at onboard (AUV-side) rate, andwith no extraordinary dropouts. The position fix up-date rates for the entire run are shown in Figure 7(a).This experiment was done in order to evaluate the per-formance of the two navigation systems in the casewere position measurements are available at a reducedfrequency.

5.1 Navigation Performance - Case 1

During the first and last part of the survey, the model-aided INS and the traditional INS are found to performcomparably in terms of calculated position errors. Theposition uncertainties estimated by the model-aidedINS are slightly lower however, and less jagged. Forthe part without position aiding, the traditional INSbreaks down quickly, as can be seen in Figure 6(b) wherethe maximum Euclidian norm of the position error isclose to 700 meters. The model-aided INS continuesto perform excellent, and the maximum norm of theposition error is 6 meters. From Figure 6(d) this canbe seen to be well within the estimated one standarddeviation (1σ). The median of the estimated north

and east position uncertainties are 1.2 meters. Theestimated trajectory is shown in Figure 6(c), closelyfollowing the basis data. Overall the model-aided INSperforms excellent, and superior to the traditional INS.Note that the navigation accuracy obtained during timeslots without position aiding is limited to the accuracy ofthe KF estimated current. If the current does not varysignificantly throughout the time period where positionmeasurements are absent, the navigation accuracy willremain good.

5.2 Navigation Performance - Case 2

As can be seen in Figure 7(b), the two navigation sys-tems provide very different estimates of the positionuncertainties. A similar behavior was also observedwhen using position measurements at topside rate. Theposition uncertainties estimated by the model-aidedINS are clearly lower, and they appear more reliable.The estimates are also much smoother. The beneficialeffect of including the vehicle model for aiding the INS isapparent in Figure 7(c), where the tallest spikes for thetraditional INS correspond to approximately 60 secondssince receiving the preceding position measurement.

In terms of position errors the model-aided INS againperforms excellent, and well within one standard devia-tion (1σ) as seen in Figure 7(d). The traditional INSalso performs acceptable in terms of position errors,and comparable to the model-aided INS when the po-sition measurements appear frequently. As mentionedearlier, the drift without external position or velocityaiding is not linear, and the performance of the tradi-tional INS worsens when the position update frequencychanges from 1/30 Hz to 1/60 Hz. We conclude that themodel-aided INS performs better than the traditionalINS during time periods without position aiding, and itprovides better error covariance estimates throughout.

119


Logged from 06-Oct-2006 04:29:29 to 06-Oct-2006 07:20:28

pmmb,y [m]

pm m

b,x

[m]

-500 0 500 1000 1500

-800

-600

-400

-200

0

200

400

600

800

1000

(a)


pmmb,y [m]

pm m

b,x

[m]

-500 0 500 1000 1500

-800

-600

-400

-200

0

200

400

600

800

1000

(b)


pmmb,y [m]

pm m

b,x

[m]

-500 0 500 1000 1500

-800

-600

-400

-200

0

200

400

600

800

1000

(c)


Err

or

inp

m mb,x

[m]

Time [m]

Err

or

inp

m mb,y

[m]

75 80 85 90 95 100 105 110 115 120 125

75 80 85 90 95 100 105 110 115 120 125

0

5

10

15

0

5

10

15

(d)

Figure 6: Traditional and model-aided INS evaluated according to case 1: (a) The red (solid) trajectory serves asbasis 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 withoutposition measurements corresponds to 30 minutes. (b) Real-time navigation solution obtained withtraditional INS shown in green (dashed). Other data as before. The system shows poor performancewithout position measurements. (c) Real-time navigation solution obtained with model-aided INSshown in green (dashed). Other data as before. The system shows excellent performance, also withoutposition measurements. The circles (red) indicate 75 and 125 minutes into the run. (d) The trueposition errors (assuming basis is correct) for the model-aided INS in north and east are shown inblue (solid). The corresponding estimated real-time KF position uncertainties (1σ) are shown in green(dashed).

120



Time [min]

Tim

ebet

wee

nA

UV

side

posi

tion

mea

sure

men

ts[s

]

20 40 60 80 100 120 140 160

25

30

35

40

45

50

55

60

(a)


SD

ofδp

m mb,x

[m]

Time [min]

SD

ofδp

m mb,y

[m]

0 20 40 60 80 100 120 140 160

0 20 40 60 80 100 120 140 160

0

2

4

6

0

2

4

6

(b)


SD

ofδp

m mb,x

[m]

Time [min]

SD

ofδp

m mb,y

[m]

56 58 60 62 64 66 68 70 72

56 58 60 62 64 66 68 70 72

0

2

4

6

0

2

4

6

(c)


δp

m mb,x

[m]

Time [min]

δp

m mb,y

[m]

0 20 40 60 80 100 120 140 160

0 20 40 60 80 100 120 140 160

0

1

2

3

0

1

2

3

(d)

Figure 7: Traditional and model-aided INS evaluated according to case 2: (a) The blue (solid) line shows thetime between position measurements, as received by the AUV. The basis trajectory is the same as inFigure 6(a). The circles indicate 54 and 72 minutes into the run. (b) The estimated real-time positionuncertainties (1σ) for the model-aided INS are shown in green (dashed). The estimated real-timeposition uncertainties (1σ) for the traditional INS are shown in blue (solid). (c) Magnified versionof Figure 7(b). The model-aided INS provides smoother estimates than the traditional INS. (d) Theestimated real-time position uncertainties (1σ) for the model-aided INS are shown in green (dashed).The true position errors (assuming basis is correct) for the model-aided INS in north and east shownin blue (solid).

121


6 Conclusions and Further Work

This paper reports the development of a model-aidedINS for underwater vehicle navigation. The navigationsystem is novel in that accurate knowledge of the vehicledynamics is utilized for aiding the INS, and the naviga-tion performance is experimentally evaluated using realAUV data. It is found that the error in the model-aidedINS position estimate is significantly lower than thatof the traditional INS throughout time segments whereposition and velocity measurements are absent. Themodel-aided INS also performs equally good or betterthan the traditional INS in cases with regular positionupdates, and the difference in performance increaseswith decreasing position update rate. The experimentalresults demonstrate that with merely an addition ofsoftware and no added instrumentation, it is possible toconsiderably improve the accuracy and robustness of anINS by utilizing the output from a kinetic vehicle model.To the best of our knowledge, the presented results arethe first report on the implementation and experimentalevaluation of model-aided INS for underwater vehiclenavigation. The conclusion has an important practicalconsequence, and the proposed approach shows promiseto improve underwater navigation capabilities both forsystems lacking disparate velocity measurements, andfor systems where the need for redundancy and integrityis important.

6.1 Further Work

A more advanced error description of the vehicle modeloutput may be implemented, and observability condi-tions for the vehicle model error and the sea currentshould be investigated. A model-aided INS utilizingboth external velocity measurements and vehicle modeloutput is of great practical interest, and should beimplemented. This is subject to ongoing research.

Acknowledgment

The first author acknowledges the financial supportfrom the Research Council of Norway, Kongsberg Mar-itime, and Kongsberg Defence & Aerospace, throughthe UNaMap program. Thanks also the Kongsberg Mar-itime HUGIN engineers and the crew of M/S SimradEcho for all their help during the experiments.

References

Bryson, M. and Sukkarieh, S. Vehicle model aided iner-tial navigation for a UAV using low-cost sensors. InAustralasian Conference on Robotics and Automation.Canberra, Australia, 2004 .

Fossen, T. I. Marine Control Systems: Guidance, Nav-igation and Control of Ships, Rigs and UnderwaterVehicles. Marine Cybernetics, 2002.

Gade, K. Integrering av treghetsnavigasjon i en au-tonom undervannsfarkost (in Norwegian). TechnicalReport FFI/RAPPORT-97/03179, Norwegian De-fence Research Establishment (FFI), 1997.

Gade, K. NavLab, a generic simulation and post-processing tool for navigation. European Jour-nal of Navigation, 2004. 2(4):21–59. (see alsohttp://www.navlab.net/).

Gelb, A. Applied Optimal Estimation. The MIT Press,1974.

Hegrenæs, Ø. Open-water propulsion test using full-scale propellers for the HUGIN AUV family. Tech-nical report, University Graduate Center, Kjeller,Norway, 2006.

Hegrenæs, Ø., Hallingstad, O., and Jalving, B. A com-parison of mathematical models for the HUGIN 4500AUV based on experimental data. In Proceedings ofthe IEEE International Symposium on UnderwaterTechnology (UT). Tokyo, Japan, 2007a .

Hegrenæs, Ø., Hallingstad, O., and Jalving, B. A frame-work for obtaining steady-state maneuvering charac-teristics of underwater vehicles using sea-trial data.In Proceedings of the 15th IEEE Mediterranean Con-ference on Control and Automation (MED). Athens,Greece, 2007b .

Jalving, B., Gade, K., Hagen, O., and Vestgard, K. Atoolbox of aiding techniques for the HUGIN AUVintegrated inertial navigation system. In Proceedingsof the MTS/IEEE Oceans Conference and Exhibition.San Diego, CA, 2003a .

Jalving, B., Vestgard, K., and Størkersen, N. Detailedseabed surveys with AUVs. In G. Griffiths, editor,Technology and Applications of Autonomous Under-water Vehicles, volume 2, pages 179–201. Taylor &Francis, 2003b.

Kinsey, J. C., Eustice, R. M., and Whitcomb, L. L.A survey of underwater vehicle navigation: Recentadvances and new challenges. In Proceedings of the7th IFAC Conference of Manoeuvring and Control ofMarine Craft (MCMC). Lisbon, Portugal, 2006 .

Kinsey, J. C. and Whitcomb, L. L. Model-based non-linear observers for underwater vehicle navigation:Theory and preliminary experiments. In Proceedingsof the IEEE International Conference on Roboticsand Automation (ICRA). Rome, Italy, 2007 .

122


Koifman, M. and Bar-Itzhack, I. Inertial navigation sys-tem aided by aircraft dynamics. IEEE Transactionson Control Systems Technology, 1999. 7(4):487–493.

Refsnes, J. E., Sørensen, A. J., and Pettersen, K. Y.A 6 DOF nonlinear observer for AUVs with exper-imental results. In Proceedings of the 15th IEEEMediterranean Conference on Control and Automa-tion (MED). Athens, Greece, 2007 .

SNAME. Nomenclature for treating the motion of asubmerged body through a fluid. Technical ReportTechnical and Research Bulletin No. 1-5, The Societyof Naval Architects and Marine Engineers, 1950.

Vasconcelos, J. F., Silvestre, C., and Oliveira, P. Embed-ded vehicle dynamics and LASER aiding techniquesfor inertial navigation systems. In Proceedings of theAIAA Guidance, Navigation, and Control Conference.Keystone, CO, 2006 .

123

towards model-aided navigation of underwater vehicles · underwater navigation has primarily...

Documents