Attitude Filter for Intervention-AUVs working inTandem with Autonomous Surface Craft ?

M. Morgado, P. Batista, P. Oliveira, and C. Silvestre

Instituto Superior Técnico, Universidade Técnica de LisboaAv. Rovisco Pais, 1, 1049-001, Lisbon, Portugal{marcomorgado,pbatista,pjcro,cjs}@isr.ist.utl.pt

Abstract: This paper presents the design, analysis, and performance evaluation of an attitude filter for anIntervention-Autonomous Underwater Vehicle (I-AUV) working in tandem with an Autonomous SurfaceCraft (ASC). In the proposed framework, an ASC aids an I-AUV to determine its attitude by providing aninertial reference direction vector through acoustic modem communications, which is measured in body-fixed coordinates by an Ultra-Short Baseline (USBL) device. The specificity of the intervention missionto be carried out, near structures that distort the Earth magnetic field, renders on-board magnetometersunusable for attitude determination. The solution presented herein includes the estimation of rate gyrosbiases, yielding globally asymptotically stable error dynamics under some mild restrictions on the vehicleteam configurations. The feasibility and performance of the proposed architecture is assessed resorting tonumerical simulations with realistic sensor noise.

Keywords: Underwater navigation, attitude estimation, Kalman filter, GAS, cooperative navigation

1. INTRODUCTION

Enabling Autonomous Underwater Vehicles (AUVs) with thecapability of performing precision-demanding underwater inter-ventions involves the design and implementation of several keyoperational systems. Among other components, such as agilerobotic actuators, thrusters, buoyancy control systems, etc., thedesign of precise navigation systems and sensor packages standsout as one of the most fundamental aspects, see Hayato andUra [2004]. This paper presents a novel sensor-based approachto the design of globally asymptotically stable (GAS) attitudefilters with application to a team of two autonomous vehiclescommissioned on a typical survey and intervention task.

Intervention-AUVs (I-AUVs) — De Novi et al. [2009] — area new class of autonomous robotic vehicles that are capable ofgoing beyond typical survey-only tasks and successfully interactwith underwater subjects to perform, for instance, collectionof sediment samples, deep-water oil well repairs, deactivationof underwater mines, etc. without human interaction. Whenoperating in open water scenarios, the attitude of the vehicleis typically estimated from the fusion of information fromseveral sensors such as accelerometers, rate gyros, DopplerVelocity Loggers (DVL), acoustic positioning systems, andfrom flux-gate magnetometers or digital compasses. Typicaltargets, such as large metal structures, ship wrecks, metallicdrills, and other Man-made objects, are known for having strongmagnetic signatures which cause space anomalies and magneticdistortions (Wynn and Bono [2002]), thus rendering the on-board magnetometers unsuitable for heading determination, butlargely adopted for the detection of buried objects (Kumar et al.[2005]) and underwater structures investigation, see Pei et al.[2010], Ioannidis [1977].? This work was partially supported by Fundação para a Ciência e a Tecnologia(ISR/IST plurianual funding) and by the EU Project TRIDENT (Contract No.248497). The work of Marco Morgado was supported by PhD Student Scholar-ship SFRH/BD/25368/2005 from the Portuguese FCT POCTI programme.

This paper addresses the design of an AUV attitude determina-tion system that relies upon a team of two vehicles, comprisingof an Autonomous Surface Craft (ASC) and an AUV in a tandemconfiguration, which cooperatively navigate the surroundings ofa subject of interest. The proposed framework falls within thescope of the European project TRIDENT — Sanz et al. [2010] —in which both vehicles are able to exchange information throughacoustic modem communications and, among other sensorpackages, are also suited with Ultra-Short Baseline (USBL)acoustic positioning devices and acoustic pingers / transponders.The USBL is composed of a small calibrated array of acousticreceivers that measures very accurately the Direction-of-Arrival(DOA) of the acoustic waves, given the proximity of the sensorsin the receiving array.

Conventional attitude estimation algorithms are typically derivedon a particular representation, e.g. rotation matrix, quaternions,Euler angles, Euler angle-axis, etc., see Crassidis et al. [2007]for a recent thorough survey. The kinematic models, whichare exact and resort to the integration of the angular velocityfrom three-axial rate gyros, are employed on the chosen attituderepresentation. These angular velocity sensors are howeveraffected by unknown slowly time-varying biases. The adoptedframework in the classical strategies is illustrated in Fig. 1,and naturally enforces the derived solutions to inherit thechosen attitude representation drawbacks, such as singularities,unwinding phenomena and/or topological limitations to achieveglobal asymptotic stabilization.

Solutions based on the Extended Kalman Filter (EKF) have beenwidely adopted in the literature, see Bar-Itzhack and Oshman[1985] and Shuster [1989] for instance. Traditional attitudefiltering solutions typically convert first the available sensorvector information to an attitude representation and then filterthe noisy attitude measurements. It is also well known that due tolinearization of the nonlinear dynamics and observations models,EKF-like solutions are not able to achieve GAS properties. In

RawAttitude

Computation

Vector measurements

(gravity, magnetometers,etc.)

Attitude Filterauxiliary

kinematics/dynamics models

Aiding sensors(rate gyros)

Prior Attitude estimate

(Euler angles, rotation matrix,

quaternion, etc.)

FilteredAttitude estimate

Auxiliary parameters(rate gyros biases)

Fig. 1. Classical filtering structure

AttitudeComputation

Vector measurements

(gravity, magnetometers,etc.)

Aiding sensors(rate gyros)

FilteredAttitude estimate

(Euler angles, rotation matrix,

quaternion, etc.)

Auxiliary parameters(rate gyros biases)

Filtered vectors

System dynamics: Motion kinematics / Motion dynamics

Vector measurements

Filter

Fig. 2. Proposed filtering structure

order to tackle convergence issues, new trends have been recentlyadopted by the scientific community, with the design of nonlinearobservers (Mahony et al. [2008]).

The solution proposed herein makes use of the frameworkpresented in Batista et al. [2009] and Batista et al. [2011b],directly including the USBL direction vector measurements inthe system dynamics, while the exact kinematics are propagatedusing the angular velocity provided by the three-axis rategyro. The derived solution, illustrated in Fig. 2, includes theestimation of the rate gyros biases, and yields GAS errordynamics. Adopting a Lyapunov state transformation, thatpreserves observability properties of the original system andallows for the nonlinear dynamics to be regarded as Linear Time-Varying (LTV), the overall system is shown to be uniformlycompletely observable, under some mild restrictions on therelative positions of the work team vehicles. A Kalman filterdesign with GAS error dynamics follows from the obtainedobservability results, and an optimal attitude determinationalgorithm is applied to the filtered vector estimates to yieldthe final attitude estimate.

The paper is organized as follows: Section 2 sets the problemframework and definitions. The proposed filter design and maincontributions of the paper are presented in Section 3, wherethe filter structure is brought to full detail, and an extensiveobservability analysis is carried out. Simulation results andperformance evaluations are discussed in Section 4, and finallySection 5 provides some concluding remarks and comments onfuture work.

2. PROBLEM FRAMEWORK

In order to set the problem framework, let {I} denote a localinertial frame, {BASC} the coordinate frame attached to thebody of the ASC, and {BAUV } the coordinate frame attached tothe body of the AUV. The relative positions of both body framesare given by

ASC (s) : BASCru(t) = RTs (t)(pu(t)� ps(t)),AUV (u) : BAUV rs(t) = RTu(t)(ps(t)� pu(t)), (1)

where BASCru(t) 2 R3 represents the position of the AUV inthe ASC body-fixed frame, BAUV rs(t) 2 R3 is the position ofthe ASC represented in the AUV body-fixed reference frame,Rs(t) 2 SO(3) and Ru(t) 2 SO(3) are the rotation matricesthat represent the attitude of the ASC and the AUV body-

fixed reference frames respectively, with respect to the inertialframe {I}, pu(t) 2 R3 and ps(t) 2 R3 are the positions ininertial coordinates of the origins of the body-fixed referenceframes of the AUV and the ASC, respectively, and the operator(·)T denotes the usual matrix transpose operation. The time-derivative of the rotation matrix Ru(t) 2 SO(3) verifiesṘu(t) = Ru(t)S [!(t)], where !(t) 2 R3 is the angularvelocity of {BAUV } with respect to {I}, expressed in the AUVbody-fixed coordinates, and S [!(t)] is the skew-symmetricmatrix that represents the cross product, such that S [!] a = !⇥a and verifies (S [!])T = �S [!]. Further assume that theangular velocity readings provided by the on-board rate gyrosare corrupted by biases b! 2 R3

!m(t) = !(t) + b!(t), (2)that are assumed to be constant, that is, ḃ!(t) = 0, or can beconsidered slowly time-varying if modelled as a random walkḃ!(t) = ⌘! , where ⌘! is white Gaussian noise.

2.1 USBL direction vector observation

In order to extract attitude information from the on-boardsensors, the first vector observation is provided by the USBLas a DOA vector measurement of the acoustic waves emittedby the ASC. The USBL computes this DOA from the Time-Difference-Of-Arrival (TDOA) of the acoustic waves arrivingat the hydrophones, specially placed at known locations on thereceiving array (Vickery [1998]). As opposed to typical acousticrelative positioning systems, for attitude estimation purposes theUSBL can be considered to provide measurements of the DOAat a higher rate. Either from bypassing the regular interrogation-reply scheme or transmitting a known specially coded signalat a higher rate, the system can increase the update rate of theUSBL direction measurements. Previous work by the authorsfocused on the development of a USBL with pseudo-noise codedspread-spectrum signals can be found in Morgado et al. [2010].

Denoting the position of the ASC in the AUV coordinates simplyby rs(t) := BAUV rs(t), and differentiating (1) in time yields

ṙs(t) = �S [!(t)] rs(t) + vr(t), (3)where vr(t) 2 R3 is the relative velocity between both vehicles,given by

vr(t) = RTu(t) Ivs(t)� vu(t), (4)where vu(t) 2 R3 is the AUV velocity expressed in theAUV body-fixed reference frame, and Ivs(t) 2 R3 is theASC velocity expressed in inertial coordinates, which satisfiesIṗs(t) = Ivs(t).

The DOA of the acoustic waves being emitted by the ASC andarriving at the AUV USBL system is given by

ds(t) =rs(t)

⇢(t), (5)

where ⇢(t) 2 R is the distance between the ASC and the AUV,i.e., ⇢(t) = krs(t)k. The direction vector ds(t) 2 S(2) isunitary by definition and is considered to be measured by theUSBL device installed on-board the AUV.

Differentiating (5) in time yields

ḋs(t) =ṙs(t)⇢(t)� rs(t)⇢̇(t)

⇢2(t). (6)

The time derivative of the range ⇢(t) in (6) is simply given by

⇢̇(t) =1

⇢(t)rs(t)

Tṙs(t). (7)

Taking into account that ds · (! ⇥ ds) = 0 and substituting (3)and (7) in (6) yields

ḋs(t) = �S [!(t)]ds(t) +vr(t)

⇢(t)� ds(t)dTs (t)

vr(t)

⇢(t). (8)

Exploiting the cross-product multiplication property that assertsS [a]S [a] = aaT � kak2I3, and the fact that kds(t)k = 1 bydefinition, lets simplify (8) to

ḋs(t) = �S [!(t)]ds(t)� S2 [ds(t)]u(t), (9)where

u(t) =vr(t)

⇢(t)(10)

is regarded as the system input.

2.2 Gravity reading vector observation

The second vector observation is drawn from the gravity vectorpresent in the accelerometers readings. As carefully discussed inMahony et al. [2008], for sufficient low frequency bandwidths,the gravitational field dominates the accelerometer readings inbody-fixed coordinates. The gravity vector is a locally constantvector quantity in inertial coordinates, thus making it a feasiblevector measurement for attitude estimation purposes. Thus, thegravity vector is represented in AUV body-fixed coordinatesas g(t) = Ru(t)T Ig, where Ig 2 R3 is the gravity vector ininertial coordinates, and its time derivative is given by

ġ(t) = �S [!(t)]g(t) (11)

3. FILTER DESIGN

3.1 Combined dual vector observer

Combining (9) and (11) yields8><

>:

ḋs(t) = �S [!(t)]ds(t)� S2 [ds(t)]u(t)ġ(t) = �S [!(t)]g(t)y1(t) = ds(t)y2(t) = g(t)

, (12)

where y1(t) and y2(t) are the outputs of the system: y1(t) =ds(t) is measured by the USBL device and y2(t) = g(t) isobtained from the low-frequency content in the accelerometerreadings.

Considering that the rate gyros readings (2) are corrupted byconstant biases in (12) yields

8>>>>><

>>>>>:

ḋs(t) = �S [!m(t)]ds(t) + S [b!(t)]ds(t)�S2 [ds(t)]u(t)

ġ(t) = �S [!m(t)]g(t) + S [b!(t)]g(t)ḃ!(t) = 0y1(t) = ds(t)y2(t) = g(t)

. (13)

Using the cross-product property ! ⇥ a = �a ⇥ ! and theconsidered outputs, (13) can be regarded as a linear time-varying(LTV) system, even though it is still nonlinear (see Lemma 1 inBatista et al. [2011a]). Thus, it comes⇢

ẋ(t) = A(t)x(t) +B(t)u(t)y(t) = Cx(t) , (14)

where x(t) = [dTs (t) gT (t) bT!(t)]

T is the system state,

A(t) =

"�S [!m(t)] 03⇥3 �S [y1(t)]03⇥3 �S [!m(t)] �S [y2(t)]03⇥3 03⇥3 03⇥3

#,

B(t) = [�S2 [y1(t)] 03⇥3 03⇥3]T ,

and

C =

I3 03⇥3 03⇥3

03⇥3 I3 03⇥3

�.

The observability analysis of the system (14) is addressed in thetheorem that follows.Theorem 1. If the AUV is never exactly below the ASC, suchthat the gravity vector and the USBL direction vector are notparallel, then the LTV system (14) is uniformly completelyobservable (UCO) — Sastry and Desoer [2002] — that is, thereexist positive constants ↵1, ↵2, and � such that ↵1I � W(t, t+�) � ↵2I, for all t � t0, where t0 is some initial time instant,and W(t, t + �) is the observability Gramian associated withthe pair (A(t),C) on [t, t+ �].

Proof Omitted due to space limitations, but similar to [Batistaet al., 2009, Theorem 1].Remark 2. Notice that, by definition, the ASC (surface craft) isnever below the AUV (underwater vechicle), thus in practice,what Theorem 1 states is that if the AUV does not navigatedirectly under the ASC, then the LTV system is uniformlycompletely observable.Remark 3. Theorem 1 provides only sufficient conditions forthe system to be UCO. The system might still be UCO evenif the vectors are parallel, requiring however some persistentexcitation conditions on angular motion of the vehicle (Batistaet al. [2009]).

Albeit other filtering solutions such as H1 could be devised,the design of a Kalman Filter with globally asymptoticallystable error dynamics follows simply by including state andobservation disturbances n

x

(t) and ny

(t)⇢ẋ(t) = A(t)x(t) +B(t)u(t) + n

x

(t)y(t) = Cx(t) + n

y

(t) , (15)

which are considered to be uncorrelated additive white Gaussiannoise (AWGN) with covariance matrices given by

E[nx

(t)nx

(⌧)T ] = Q(t)�(t� ⌧),E[n

y

(t)ny

(⌧)T ] = R(t)�(t� ⌧),respectively. Additional states could be added to model the obser-vation and process disturbances as colored noise, by modellingn

x

(t) and ny

(t) as outputs of stable linear time-invariant filters.Nonetheless, the resulting Kalman filter equations are standard(Gelb [1974]) and therefore omitted.

3.2 Attitude estimation

The filtering architecture presented so far allows for the AUV tofilter the noisy gravity and USBL direction vector measurementsand obtain accurate body-fixed estimates of two vectors whileestimating the rate gyros biases with an optimal filter withGAS properties. In this section, an attitude solution is derivedfrom the two proposed filtered vector measurements, exploitingthe tandem vehicle formation and the ability to exchangeinformation through acoustic modems.

Taking into account that krs(t)k = kru(t)k = ⇢(t) comes from(1) that the direction of the AUV measured on the ASC vehicleis given by

du(t) =ru(t)

⇢(t)= RTs (t)

pu(t)� ps(t)⇢(t)

. (16)

Now, letIdu(t) =

pu(t)� ps(t)⇢(t)

. (17)

Using (17) it is immediate to see that (16) can be written asdu(t) = RTs (t) Idu(t), and the ASC direction measured in theAUV coordinate frame comes as

ds(t) = RTu(t) Ids(t) = RTu(t)(� Idu(t)),

where Ids(t) = � Idu(t) is the natural inertial reference forthe AUV body-fixed USBL measurement. For vehicles workingin tandem, as in the case of the framework proposed herein, thisquantity is invariant to rotations and can be considered slowlytime-varying for attitude estimation purposes.

Since the ASC vehicle is navigating on the sea surface, andbenefits from the availability of GPS signals, it is considered tohave a complete navigation package, which provides accurateinertial velocity and attitude and heading estimates. Thus itis able to invert its own USBL measurements as R̂s(t)du(t)and send it to the AUV via the acoustic modem link in orderto aid the underwater vehicle attitude estimation algorithm.In the proposed architecture, depicted in Fig. 3, the surfacevehicle sends to the underwater vehicle, via acoustic modem,the estimates of its own inertial velocity IvASC(t), the directionvector Idu(t), and the range ⇢(t), which is computed fromeither the USBL tracking scheme or from the acoustic modemitself.

The optimal attitude estimation solution, in a least-squares sense,is obtained from the minimization of the cost functional

J(R) = 12

NX

i=1

kyi �RT Iyik2 , (18)

which is commonly known in the literature as the Wahba’sproblem (Wahba [1965]). For the case of more than one non-collinear vector observation, that is N � 2 in (18), there existsa closed-loop solution (Markley [2008]), based on the singularvalue decomposition (SVD).

Let

B =NX

i=1

ȳiIȳ

Ti , 2 R3⇥3,

where ȳi represents the normalized filtered estimates ȳi =yi/kyik, and Iȳi the corresponding inertial representations.Consider the SVD decomposition B = U⌃VT , where U andV are unitary matrices, and ⌃ is a diagonal matrix that containsthe singular values of B. Thus, the optimal solution for R thatminimizes (18), presented in Markley [2008] and provided herefor completeness, is computed as

RT = Uh 1 0 00 1 00 0 det(U) det(V)

iV

T . (19)

Based on the information received through the acoustic modem,the AUV computes the relative velocity vr(t) from (4) as

vr(t) = R̂Tu(t) Ivs(t)� vu(t), (20)where the underwater vehicle velocity vu(t) 2 R3 is measuredby the on-board DVL, and R̂u(t) 2 SO(3) is an a prioriestimate for the rotation matrix Ru(t), computed from theavailable noisy measurements. The received range ⇢(t) is usedwith (20) to compute the system input u(t) in (10), and drive thesensor-based vector measurement filter dynamics (15). Finally,the filtered attitude estimate is computed a posteriori, using theoptimal solution from (19) with the filtered vectors g(t) andds(t) from (13) and their respective inertial reference vectors:gravity Ig(t), which is assumed known, and Ids(t) providedthrough the acoustic modem as explained above.

Autonomous Surface Craft (ASC)

USBL

NavigationSystem

Acoustic Modem

Autonomous Underwater Vehicle (AUV)

Vector sensor measurements

Filter +

AttitudeComputation

Acoustic Modem

Gravity vector measurements

(accelerometer)

USBL

Known gravity vector

(in Inertial coordinates)

Ids

krsk

�

Bds

Bg

Ig

Slowly time-varying / invariant to rotations

Ivs

Fig. 3. Proposed tandem architecture

Remark 4. The attitude solution computed from (19) is not welldefined for a set of parallel vectors. It might happen by accidentthat the estimated vectors ŷ1(t⇤) and ŷ2(t⇤) are parallel, thatis ŷ1(t⇤) ⇥ ŷ2(t⇤) = 0, or null for some time t⇤, as in thefiltering structure presented herein, there are no restrictionsimposed on the cross-product ŷ1(t)⇥ ŷ2(t). However, if suchhappens in practice during the initial convergence of the filterfor instance, an attitude solution can be computed directly fromthe vector sensor measurements. Nonetheless, sustained on theGAS properties of the error dynamics the filter is guaranteedto converge, and as it will be shown in Section 4, the proposedarchitecture exhibits very fast convergence.

4. SIMULATION RESULTS

The proposed solution is evaluated in this section resorting tonumerical simulations of two autonomous vehicles, operating ina typical survey and intervention scenario. The vehicles describea trajectory as depicted in Fig. 4, in which both the ASC and theAUV start heading north along the x-axis at the same speed, andthen the AUV identifies an intervention target and descends ona helicoidal trajectory while the ASC waits for it to completeits descent. When the AUV reaches the bottom, both vehiclestravel northbound before performing a coordinated 180-degreeturn and return to the origin of the mission. The motion ofboth vehicles is performed on an higher-layer of the missioncontrollers, coordinated via the on-board acoustic modems. TheASC, as it navigates on the surface, is affected by the sea stateand thus its nominal position and velocity are disturbed by themotion of the waves.

Both vehicles are equipped with acoustic modems and USBLpositioning devices, whereas the ASC USBL unit is mountedin a standard surface-mount scheme, whilst the AUV USBLunit is mounted in what is known as an inverted-USBL con-figuration (Vickery [1998]). Table 1 summarizes the sensor

Fig. 4. Vehicles trajectory

Table 1. Vehicles sensor packages

VehicleSensor package ASC (surface) AUV (underwater)Full navigationstate (INS)

Yes No

GPS Yes No: unavailable underwaterUSBL Yes: standard sur-

face mountYes: inverted configuration

DVL - Yes: with bottom-lockMagnetometer - No: unavailable due to strong

environmental magnetic dis-tortions during intervention

Rate gyros (included in INS) Yes: corrupted with biasesAccelerometers (included in INS) Yes: used as gravitometerAcoustic Modem Yes Yes

packages of each vehicle. The acoustic modems are also ableto provide range measurements between the two vehicles viahandshaking protocols, and, due to environmental restrictionssuch as a low underwater sound propagation velocity and harshmultipath conditions, have considerable bandwidth limitationsand message delivery delays. In particular, with a nominalunderwater sound speed of 1515 ms�1, and due to the distancesconsidered between the two vehicles (in the trajectory describedin Fig. 4) the messages sent from the ASC to the AUV areonly delivered after the propagation delay profiled in Fig. 5.The limited bandwidth of the acoustic modems is also taken intoconsideration in the simulations presented herein: the ASC needsto send messages to the AUV with a payload composed of theinertial reference direction vector Ids(t) and the ASC inertialvelocity Ivs. Even though message compression techniquescan be employed, the worst case scenario is to send withoutcompression three single-precision floating point numbers (32bit floats) for each vector quantity, which implies a payload of192 bits per message. Commercially available acoustic modemssuch as the Link-Quest R� UWM2000 have payload data ratesof 6600 bits per second (bps). Thus, in addition to deliverydelays and to keep the simulations realistic, the acoustic modemsare considered to successfully transmit only ten (10) messagesper second, which reserves a bandwidth of 1920 bps for therequired communications. The quantities being transmitted overthe acoustic link are, nonetheless, invariant to rotations of theAUV, and can be considered slowly time-varying for attitudeestimation purposes.

All the sensors installed on-board the AUV are considered tobe disturbed by additive white Gaussian noise, as described on

Fig. 5. Modem communications time delay profile

Table 2. AUV and ASC sensors noise characteristics

On-board Sensor AWGN standard deviationAUV Rate gyros 0.05 [deg s�1]AUV DVL velocity 1 [mm s�1]AUV USBL azimuth 0.5 [deg]AUV USBL elevation 0.5 [deg]AUV Accelerometer 1 [mg]AUV Range 1 [m]ASC AHRS yaw 0.1 [deg]ASC AHRS pitch 0.01 [deg]ASC AHRS roll 0.01 [deg]ASC Velocity 1 [mm s�1]ASC USBL filtered azimuth 0.05 [deg]ASC USBL filtered elevation 0.05 [deg]

Table 2, while the ASC navigation package provides measure-ments with an accuracy described by the AWGN also describedin Table 2. The Kalman filter gains were adjusted to allow for afast convergence with

R(t) = diag�10�313⇥1, 10

�313⇥1

�,

Q(t) = diag�10�513⇥1, 10

�513⇥1, 10

�713⇥1

�,

and maintain good steady-state performance when compared tothe raw attitude measurement, obtained by minimizing (18) withthe unfiltered vector measurements.

To illustrate the convergence of the filter, the filter is initializedwith the gravity vector rotated to an offset corresponding to 30,10, and 5 deg in yaw, pitch, and roll respectively. The USBLdirection vector estimate is initialized to one in the forwarddirection and zeros on the remaining, while the initial nominalvector is pointing in fact to the right and above of the AUV.The overall filtered attitude estimate is compared to the rawattitude measurements in Fig. 6. The linear acceleration v̇(t) ofthe vehicle present on the accelerometers readings is seen tocause some disturbances as this quantity was not modelled inthe original state space. This disturbance is expected, even inclassical attitude filters that use accelerometers to provide gravitymeasurements, when the linear acceleration of the vehiclealso affects the low-frequency content of the accelerometermeasurements.

The initial rate gyro bias estimate was set to zeros whilst thenominal bias was [5, �3, 4] deg s�1, and its time evolution alsoexhibits a fast convergence as evidenced in Fig. 7.

5. CONCLUSIONS

This paper presented a novel approach to the design of anattitude filter for an I-AUV, navigating in tandem with a surfacecraft. In the proposed framework, the ASC aids the I-AUVdetermine its attitude, by providing an inertial reference directionvector through acoustic modem communications, which ismeasured in body fixed coordinates by an Ultra-Short Baseline(USBL) device. Magnetometers are avoided in the presentedsolution, due to space anomalies and magnetic distortions

−100

−50

0

50

100

yaw e

rror [

deg]

−40

−20

0

20

40

pitc

h erro

r [de

g]

0 0.5 1

−100

−50

0

50

100

roll e

rror [

deg]

Initial conv. − t [s]

−2

−1

0

1

2

−0.5

0

0.5

50 100 150 200 250 300 350 400 450−0.5

0

0.5

Steady state response − t [s]

UnfilteredFiltered

Fig. 6. Attitude estimation error in ZYX-Euler angles

0 2 4−5

0

5

b erro

r [de

g/s]

Initial conv. − t [s]50 100 150 200 250 300 350 400 450

−0.1

−0.05

0

0.05

0.1

0.15

Steady state response − t [s]

xyz

Fig. 7. Rate gyro biases estimation errors

introduced by the intervention targets that may include Man-made objects, large metal structures, and ship wrecks thathave strong magnetic signatures. The filtering architecturepresented herein includes the estimation of rate gyros biases,which is a fundamental feature for open-loop integration ofthe vector kinematics. A modification of the system dynamicsallows for the nonlinear system to be regarded as an LTV,still being nonetheless nonlinear. An observability analysis isconducted using this modified system showing it to be uniformlycompletely observable, under some mild restrictions on thetandem vehicle configuration. This allows for the design of astandard Kalman filter with GAS error dynamics. The feasibilityof the proposed architecture was assessed resorting to numericalsimulations with realistic sensor noise, considering also adequatecommunications delays and limited bandwidth of the acousticmodems, and its performance was shown to yield satisfactoryresults. Future work on this subject will focus on the real-timeimplementation of the proposed architecture within the scope ofthe EU project TRIDENT.

REFERENCES

Bar-Itzhack, I. and Oshman, Y. (1985). Attitude determinationfrom vector observations: Quaternion estimation. Aerospaceand Electronic Systems, IEEE Transactions on, AES-21(1),128 –136. doi:10.1109/TAES.1985.310546.

Batista, P., Silvestre, C., and Oliveira, P. (2009). Sensor-based Complementary Globally Asymptotically Stable Filtersfor Attitude Estimation. In Proceedings of the 48th IEEEConference on Decision and Control, 7563–7568. Shanghai,China.

Batista, P., Silvestre, C., and Oliveira, P. (2011a). Single RangeAided Navigation and Source Localization: observability andfilter design. Systems & Control Letters, 60(8), 665–673.

Batista, P., Silvestre, C., and Oliveira, P. (2011b). Vector-basedattitude filter for space navigation. Journal of Intelligent &Robotic Systems: Theory & applications, 64(2), 221–244.

Crassidis, J., Markley, F., and Cheng, Y. (2007). Survey ofnonlinear attitude estimation methods. Journal of GuidanceControl and Dynamics, 30(1), 12.

De Novi, G., Melchiorri, C., Garcia, J., Sanz, P., Ridao, P.,and Oliver, G. (2009). A New Approach for a Reconfig-urable Autonomous Underwater Vehicle for Intervention. InProceedings of the 3rd Annual IEEE International SystemsConference, 23–26. Vancouver, Canada.

Gelb, A. (1974). Applied Optimal Estimation. MIT Press.Hayato, K. and Ura, T. (2004). Navigation of an AUV for

investigation of underwater structures. Control EngineeringPractice, 12(12), 1551 – 1559.

Ioannidis, G. (1977). Identification of a Ship or Submarinefrom its Magnetic Signature. Aerospace and ElectronicSystems, IEEE Transactions on, AES-13(3), 327 –329. doi:10.1109/TAES.1977.308404.

Kumar, S., Skvoretz, D., Moeller, C., Ebbert, M., Perry, A.,Ostrom, R., Tzouris, A., Bennett, S., and Czipott, P. (2005).Real-Time Tracking Gradiometer for use in an autonomousunderwater vehicle for buried minehunting. In Proceedingsof the MTS/IEEE OCEANS 2005, 2108 – 2111 Vol. 3. doi:10.1109/OCEANS.2005.1640072.

Mahony, R., Hamel, T., and Pflimlin, J. (2008). NonlinearComplementary Filters on the Special Orthogonal Group.Automatic Control, IEEE Transactions on, 53(5), 1203–1218.

Markley, F. (2008). Optimal Attitude Matrix from Two VectorMeasurements. Journal of Guidance Control and Dynamics,31(3), 765.

Morgado, M., Oliveira, P., and Silvestre, C. (2010). Designand experimental evaluation of an integrated USBL/INSsystem for AUVs. In Proceedings of the 2010 IEEEInternational Conference on Robotics and AutomationICRA 2010, 4264 –4269. Anchorage, AK, USA. doi:10.1109/ROBOT.2010.5509597.

Pei, Y., Yeo, H., Kang, X., Pua, S., and Tan, J. (2010). Mag-netic gradiometer on an AUV for buried object detection.In Proceedings of the MTS/IEEE OCEANS 2010. doi:10.1109/OCEANS.2010.5664272.

Sanz, P., Ridao, P., Oliver, G., Melchiorri, C., Casalino, G.,Silvestre, C., Petillot, Y., and Turetta, A. (2010). TRIDENT:A Framework for Autonomous Underwater InterventionMissions with Dexterous Manipulation Capabilities. InProceedings of the 7th Symposium on Intelligent AutonomousVehicles IAV-2010. IFAC.

Sastry, S. and Desoer, C. (2002). The robustness of control-lability and observability of linear time-varying systems.Automatic Control, IEEE Transactions on, 27(4), 933–939.

Shuster, M. (1989). A simple Kalman filter and smoother forspacecraft attitude. Journal of the Astronautical Sciences, 37,89–106.

Vickery, K. (1998). Acoustic positioning systems. New concepts- The future. In Proceedings of the 1998 Workshop onAutonomous Underwater Vehicles, AUV’98. Cambridge, MA,USA.

Wahba, G. (1965). Problem 65 1: A least squares estimate ofspacecraft attitude. Siam Review, 7(3), 409.

Wynn, M. and Bono, J. (2002). Magnetic sensor operationonboard an AUV: magnetic noise issues and a linear systemsapproach to mitigation. In Proceedings of the MTS/IEEEOCEANS 2002, volume 2, 985 – 993.