1

Joint Positioning and Navigation Aiding System

for Underwater RobotsR. Sousa, A. Alcocer, P. Oliveira, R. Ghabcheloo, and A. Pascoal

renato.lisboa@ist.utl.pt, {alexblau, pjcro, reza, antonio}@isr.ist.utl.pt

Instituto Superior Tecnico (IST) and Institute for Systems and Robotics (ISR), Lisbon, Portugal

Abstract— This paper proposes a new joint positioning andnavigation aiding system for underwater robots. In the scenarioadopted, a submersed target carries a pinger that emits acousticsignals periodically, as determined by a low precision clock. Thetarget is tracked from the surface by a set of buoys equipped withacoustic hydrophones/projectors, GPS receivers, and electroniccircuitry that measures the times of arrival of the acoustic signalsemitted by the pinger. Once an estimate for the underwatertarget position is computed, the buoys transmit this informationback to the target indirectly, without resorting to an acousticmodem. This is done by encoding the information in the relativetimes of emission of acoustic pulses, as if they were emittedby a set of buoys remaining in fixed positions agreed upon inadvance with the target. The times of arrival of these pulsesare used by the target as aiding data to be fused with othermotion data available onboard, in a local navigation system.The scheme proposed allows for the compensation of the: i)displacement experienced by the surface buoys, which are notrequired to be moored, ii) clock bias due to the low accuracyclock onboard the target, and iii) motion of the target during thetime interval it takes to complete a tracking-positioning cycle, aspredicted by a dynamic target model implemented in the localpositioning system. Simulation results are presented to assess theperformance of both the positioning tracker and an onboardsimplified integrated navigation system.

I. INTRODUCTION

In the last decades, the marine community has witnessed

fast paced developments in underwater technology. The use of

sophisticated ROVs (Remotely Operated Vehicles) and AUVs

(Autonomous Underwater Vehicles) is now reality in a number

of underwater operations that are extremely difficult to be

carried out by humans or require tedious and time-consuming

activities. The operation of these types of underwater robots

poses considerable technical challenges, namely in what con-

cerns the determination of their position and velocity. In the

case of AUVs, practical operations require that the following

tasks be executed efficiently: i) computation of the position and

velocity of the robot at a surface support vessel, for security

purposes and to help scientists to follow up the missions being

executed, and ii) onboard computation of estimates of the robot

motion variables (linear and angular positions and velocities),

based on measurements from its motion sensor suite. The first

task is done by a positioning system, while the latter requires

Research supported in part by projects RUMOS of the FCT (ContractNo. PDCT/MAR/55609/2004), GREX / CEC-IST (Contract No. 035223) andFREESUBNET RTN of the CEC, and by the FCT-ISR/IST plurianual fundingprogram (through the POS-Conhecimento Program initiative in cooperationwith FEDER).

the implementation of a navigation system, both resorting to

nonlinear estimation techniques. Due to the presence of a

number of disturbances acting on sensor measurements, the

navigation system requires the use of external aiding data. In

currently existing solutions these two types of systems do not

work together, leading to an increase in the number of sub-

systems to install, calibrate, and operate and bringing added

complexity to the task of checking acoustic compatibility.

A number of technical solutions exist to the problem of

computing the position of an underwater vehicle, the most

common being Short Baseline Systems (SBL) and Ultra-

Short Baseline Systems (USBL), see [11]. These classical

positioning systems require the use of a dedicated support

vessel, where the acoustic sources (pingers) and receivers (hy-

drophones) are installed and calibrated. A more recent solution

is the GPS Intelligent Buoy System - GIB [10], that brings the

concept of aided positioning to the underwater world. This

system consists of a synchronized network of hydrophones

attached to surface buoys that are equipped with differential

GPS (DGPS) receivers. Before a mission is executed, the clock

of the target is synchronized with those of the buoys and,

when submerged, the target emits periodically a signal that

is received by each of the hydrophones. The buoys transmit

via radio the time of arrival (TOA) of the acoustic signals to

a control station. The position and velocity of the target is

then computed using a triangulation algorithm. This system

has the advantage of being portable and can be deployed in a

short period of time (a few hours, tipically) [1]. An acoustic

underwater communication link between the control station

and the robot must then be used to transmit the estimated

position of the target to the target itself, to be used as an

aiding source of information by the robot navigation system.

This major limitation of the GIB system motivated the work

presented in this paper, aimed at the study of a joint positioning

and navigation aiding system capable of dispensing with the

need for an extra acoustic communication system.

Currently, a number of techniques exist for reliable 3-D

navigation systems of underwater robots. Examples include

the inverted USBL, as detailed in [8] and in references therein,

terrain- or landmark-relative navigation systems [9], and Long

Baseline Systems (LBL) [7]. These techniques have some

limitations. Namely, for correct operation of LBL and inverted

USBL systems careful installation and accurate calibration of

a pinger/transponder must be performed. To use terrain- or

landmark-based navigation systems, a bathymetric map of the

area of operation is required. Note that the electromagnetic

978-1-4244-2620-1/08/$25.00 ©2008 IEEE

2

signals used by the GPS satellites attenuate rapidly in the wa-

ter, thus precluding their direct use in underwater applications

[5].

This paper describes a new joint positioning and aiding

navigation system (JPN) that provides both positioning data

and navigation aids simultaneously. Inspired by the GIB ap-

proach, this system is composed by three segments: a) Surface

Segment: consists of a synchronized net of hydrophones and

pingers attached to surface buoys that are equipped with DGPS

receivers; b) Underwater Segment: the submerged target that

emits periodically a signal that is received by each of the

hydrophones installed on the surface buoys, which in turn

transmit via radio the time of arrival of the signals (TOA)

to a control station positioned on a support vessel or on

land; and c) Control Segment: using an Extended Kalman

Filter (EKF), the control station computes the positions and

the timing compensations among the buoys (synchronized at

the surface resorting to accurate GPS timing signals) and the

target. Resorting to the estimate of the position of the target,

the control station computes the instant at which each buoy

must send a signal that will be received by the target and used

as an external aiding signal by its onboard navigation system.

Note that to access this external navigation aiding data no

communication link is required, thus avoiding the requirement

of operation of a difficult and at times very unreliable system.

In our opinion, this solution has great potential to substantially

improve the operationality of the original GIB architecture.

The paper is organized as follows: Section II details a

dynamical target model, specially suited for AUV survey

missions, to be used by the JPN system. Section III describes

in detail the Surface Segment, namely the sub-systems to be

installed in the surface buoys and their operation. The Control

Segment is described in Section IV where the positioning

system is implemented, supported on the design of an EKF.

Section V describes the navigation system that is at the core

of the Underwater Segment and in Section VI the results of a

set of simulated experiments are summarized. Finally, Section

VII draws some conclusions on the proposed architecture and

outlines future work.

II. TARGET MODEL

The main purpose of the JPN system is to provide sup-

port to underwater robot missions. The types of trajectories

executed by the robot are instrumental in the development of

a dynamical model that describes the target movement. For

instance, when considering typical AUV bathymetric surveys,

the following constraints can be assumed:

• the magnitude of the target velocity is constant;

• the mission is composed by a set of straight paths or

curves of constant radius, i.e. trimming trajectories;

• the target trajectories are normally in the horizontal plane.

To match these assumptions, the Near Planar Constant-Turn

Model (NCT) [6] is adopted is this work. For other types of

missions and robots, other constraints could be considered.

Some basic notation is introduced next. Let

pt =[

xt yt zt]T

,

Fig. 1. AUV interchaging data with a bouy, real position during themission at left and nominal position on the right. Linear and angularquantities definition.

be the position of the target in an inertial reference frame

I = Oxyz , expressed as a column vector, verifying

pt = vt,

where vt is the target linear velocity, in the inertial frame. In

the same reference frame, it verifies

pt = v

t = at,

where at is the target acceleration and where the explicit time

dependence was omitted. In this model it is assumed that the

velocity vector is always aligned with longitudinal direction

of the target, as depicted in Fig. 1. Given a continuous-time

variable u(t), u(tk) denotes its value at discrete instants of

time tk = kT, k ∈ N0, and T denotes the sampling interval.

Throughout this work, the time instant T ∗ (k) + δt will be

denoted as tk + δt, for the sake of compactness. It is now

possible to define the continuous time kinematic model for

the target as (see [6] for a detailed derivation)

at = −ω2vt + w, (1)

where ω is the turn rate (assumed to be known and constant)

and w is Gaussian white noise. The dynamical model for the

target state

x =[

xt xt xt yt yt yt zt zt zt]T

, (2)

is given by

x(t) = A(ω)x(t) + Bw(t),

where

A(ω) = diag{[

C(ω) C(ω) C(ω)]}

,

B = diag{[

D D D]}

,

C(ω) =

0 1 00 0 10 −ω2 0

, and D =

001

,

3

the Gaussian white noise w(t) =[

wx(t) wy(t) wz(t)]T

has power spectral density S = diag{[

Sx Sy Sz

]}. The

corresponding discrete time model can be obtained using the

method introduced in [3] to yield

x(tk+1) = f(x(tk), w(tk)) = H(ω, T )x(tk) + w(tk), (3)

where H = diag{[

F(ω) F(ω) F(ω)]}

,

the disturbance covariance matrix Q(ω, T ) =diag

{[SxJ(ω, T ) SyJ(ω, T ) SzJ(ω, T )

]},

F(ω, T ) =

1 sin ωTω

1−cos ωTω2

0 cos ωT sin ωTω

0 −ω sinωT cos ωT

, and

J(ω, T ) =264 6ωT−8 sin ωT+2 sin 2ωT4ω5

2 sin4 ωT/2

ω4−2ωT+4 sin ωT−sin 2ωT

4ω3

2 sin4 ωT/2

ω42ωT−sin 2ωT

4ω3sin

2 ωT2ω2

−2ωT+4 sin ωT−sin 2ωT4ω3

sin2 ωT

2ω22ωT+sin 2ωT

4ω

375 .

This discrete time model will be used both in the Control

Segment and in the Underwater Segment of the JPN system,

as detailed in the next sections.

III. SURFACE SEGMENT

The positioning system to be studied in this paper borrows

from the key concepts introduced in the GIB system. In the

setup adopted, the target carries a pinger that emits periodically

a signal (every T seconds). This signal will be received by

a net of N synchronized hydrophones attached to surface

buoys equipped with DGPS receivers. Each buoy records the

Time of Arrivals (TOA) of the acoustic signals emitted by

the pinger, together with its instantaneous GPS coordinates

(given by the DGPS receiver). The resulting data pack is then

transmitted to the control station, where the target position

is computed. At this stage, a key difference relative to the

GIB system can already be noticed: the target does not need

to be synchronized with the buoys. This is possible because

the control station, when computing the target location, will

estimate the de-synchronization factor (Υ) between the buoys

and the target, i.e. the clock bias of the target.

ΥΥΥrb1(tk) rb

2(tk) . . . rbN (tk)

ttk tk+1 tk+2

st(tk) st(tk+1) st(tk+2)

Fig. 2. Reception delay on the emission at time tk . The signal is”really” emitted at time sk and received at the buoys at surface at timesrb1(tk), . . . , rb

N (tk).

Every st(tk) = tk +Υ, k ∈ N0 seconds, the pinger emits

a signal that will be received by each buoy at instant

rbi (tk) = st(tk)+di(s

t(tk))/vsound +npi (tk), i = 1, . . . , N

where vsound is the velocity of sound in water (assumed to

be constant and known apriori) and npi (tk) is stationary, zero-

mean, Gaussian white noise with constant standard deviation

σi. The disturbances npi (tk) and np

j (tk) are assumed to be

independent for i 6= j and the distance di(tk) between the

target and the buoy i is given by

di(tk) = ‖pbi (tk) − pt(tk)‖2, i = 1, . . . , N

where pbi (tk) =

[xb

i (tk) ybi (tk) zb

i (tk)]T

is the position

of the buoy i and pt(tk) is the target position, both at time

tk. See in Fig.2 a logic diagram relating these quantities.

It is assumed that the velocities of the buoys the targets are

small when compared with vsound. Next, the buoys transmit

to the control station their position pbi (tk) and the result

of rbi (tk) − tk = Υ + di(s

t(tk))/vsound + npi (tk), which

would correspond to the time-of-flight (TOF) of the signals

under ideal conditions, i.e. in the absence of noise and with

the systems synchronized. At the control station, the model

adopted for the measurements is

zpi(r

bi (tk)) = zp

i (tk) = di(tk)/vsound+npi (tk)+δt, i = 1, . . . , N.

(4)

The measurements zpi(r

bi (tk)) correspond to the TOF of the

acoustic signal from pt(tk) to pbi (tk) computed at rb

i (tk). In

the model (4), δt is the estimate of Υ. The number of valid

measurements over an acoustic emission cycle varies from 0to N , depending on the conditions of the acoustic channel.

The set of 0 ≤ m ≤ N measurements can be represented in

compact form as

zp(tk) =(Cm

[zp

1(rb1(tk)) · · · zp

N (rbN (tk))

])T

,

where Cm : RN → R

m denotes the operator that extracts the

m columns in[

zp1(r

b1(tk)) · · · zp

N (rbN (tk))

]that are

actually available. It is also convenient to define

np(tk) =(Cm

[np

1(tk) · · · npN (tk)

])T

and

Rp = diag{Cm[

σ1 · · · σN

]}.

IV. CONTROL SEGMENT

A. Underlying Model

Supported on the linear dynamical model of the target

summarized in (1) and given the nonlinear observations in

(4), it is straightforward to derive an Extended Kalman

Filter (EKF), that will yield estimates of the target posi-

tion pt(tk), based on the measurements zp(tk). To explic-

itly address the fact that the target and the buoys are de-

synchronized, an extra state was added to the state vector

(2) δt (δt(tk+1) = δt(ttk)) that will be an estimate of Υ.

It is assumed that this state models a constant bias or a

very slow drift. Therefore, in this section the complete state

is x(t) =[

xt xt xt yt yt yt zt zt zt δt]T

and H(ω, T ) = diag{[

H(ω, T ) 1]}

. The Gaussian

white noise vector w(tk) will have a new element with

the same properties as the disturbances previously intro-

duced, with standard deviation σδ, leading to Q(ω, T ) =diag

{[Q(ω, T ) σ2

δ

]}. Following the by now classical so-

lution to EKF [4], we define the Jacobian matrices

Ap(tk) = A

p(xp(tk)) =

∂f(x, w)

∂x

∣∣∣∣∣xp(tk)

,

4

Lp(tk) = L

p(xp(tk)) =

∂f(x, w)

∂w

∣∣∣∣∣xp(tk)

,

Cp(tk) = C

p(xp(tk)) =

∂zp

∂x

∣∣∣∣∣xp(tk)

and

Dp(tk) = D

p(xp(tk)) =

∂zp

∂np

∣∣∣∣∣xp(tk)

.

where xp(tk) is the estimated value of the state vector x(tk),

available on the estimator. As the target model (3) is linear,

Ap

= Ap(tk) = H(ω, T ) and L

p= L

p(tk) = I10,

where In) denotes an identity matrix of size n × n. Further-

more, defining

Cp

i (tk) =

−xb

i (tk)−xp(tk)

di(tk)

00

−yb

i (tk)−yp(tk)

di(tk)

00

−zb

i (tk)−zp(tk)

di(tk)

001

, i = 1, . . . , N

with

di(tk) = ‖pbi (tk) − p

t(tk)‖2 and Dp

= Dp(tk) = Im,

it follows that

Cp(tk) =

[

Cm[

Cp

1(tk) · · · Cp

N (tk)]]T

.

Note that the dimensions of Cp(tk) and Dp vary according to

the number of valid measurements of tk.

B. Positioning System: Extended Kalman Filter Design

In the strategy adopted, the estimator runs at the same

sampling period as the interrogation period of the underwater

pinger T . While not all the valid measurements corresponding

to instant tk are available, the estimator performs a pure state

and covariance prediction cycle using the EKF setup already

mentioned, as follows:

xp−

(tk+1) = H(ω, T )xp(tk), (5)

Pp−

(tk+1) = ApPp(tk+1)A

pT + L

pQpL

pT, (6)

where xp−

and Pp−

(tk) are the state prediction and the error

prediction covariance, respectively. Once the measurements

at time instant tk are validated, and assuming that the state

vectors xp−

and Pp−

(tk) are available (stored in memory on

the control station), it is possible to go back to that initial

cycle time and perform the filter state and covariance update

cycle

xp+(tk+1) = x

p−

(tk+1) + Kp(tk+1)[zp(tk+1) − z

p(tk+1)],

where

Kp(tk+1) = Pp+(tk+1)C

pT(tk+1)

[

DpRpD

pT

]−1

and

Pp+(tk+1) = P

p−

(tk+1) − Pp−

(tk+1)CpT(tk+1)

[

Cp(tk+1)P

p−

(tk+1)CpT(tk+1) + D

pRpD

pT

]−1

Cp(tk+1)P

p−

(tk+1).

The estimate of zp(tk+1) is denoted as zp(tk+1), obtained from

the observation model (4) and xp−

(tk+1).Upon updating the state estimates, a new prediction cycle

begins. Using (5)-(6) this new cycle starts at xp+ and P

p+(tk+1)

and stops when a new set of observation is available. The

overall structure of the algorithm proposed is depicted in Fig.

3.

V. UNDERWATER SEGMENT

The major innovation of the JPN system is in its navigation

system. The system uses the surface buoys to send signals to

the underwater target. When the target receives these signals, it

computes their TOF (as if the buoys remained fixed at known

locations) and estimates its own location. The control station

coordinates the emission of the acoustic signals so that the

emission time in each buoy compensates for these well known

problems: i) the buoys cannot possible to be moored in a fixed

position, ii) the target timing device (the local clock) cannot be

kept synchronized with the clocks in the buoys at the surface,

and iii) the motion of the target during the time interval it

takes to complete a tracking-positioning cycle. These problems

are evident when the target tries to compute the TOF of the

acoustic signals received. If the buoys changed their position,

due to wind, current, and other disturbances or if they are not

synchronized with the target, the computation of the TOF to

the target is incorrect and will corrupt the estimates of the

target location. Next, a solution to this problem that is at the

core of the JPN system will be detailed. At this point, some

additional notation is needed.

Ideally, the time at which the buoys should emit their signals

is tk. To compensate for the problems previously outlined, a

simple solution is to make the buoys transmit the signals at

the new time instants sbi (tk), i = 1, . . . , N . This signal will

arrive at the target at rti(tk). In the target frame, the time

instants will be denoted as ttk (ttk = tk + Υ). Each buoy will

have a reference position pbi =

[

xbi yb

i zbi

]T

, i =

1, . . . , N recorded in the underwater target and in the control

station before the mission starts.

The algorithm proposed is now examined in detail. At the

target, (7) will be evaluated to determine the TOA of the

signal emitted by buoy i at time sbi (tk). Thus this quantity is

used to compensate for the undesired disturbances measured

at surface,

dpti(t

tk) = rt

i(ttk) − ttk, i = 1, . . . , N. (7)

First, using the estimates given by the positioning system, at

tk the control station predicts the state of the target at tk+1, as

5

rb1(tk) rb

2(tk) ... rbN (tk)

rb1(tk+1) rb

2(tk+1)

... rbN (tk+1)

rb1(tk+2) rb

2(tk+2)

... rbN (tk+2)

tk

tk

tk+1

tk+1

tk+1

tk+2

tk+2

tk+2

tk+3

tk+3

st(tk) st(tk+1)

st(tk+2)

st(tk+3)

xp

−

(tk)

xp

−

(tk+1)

xp

−

(tk+1)

xp

−

(tk+2)

xp

−

(tk+2)

xp

−

(tk+2)

xp

−

(tk+3)

xp

−

(tk+3)

xp

+(tk)

xp

+(tk+1)

Fig. 3. Control Segment: data processing in the positioning algorithm.

in (5), and then computes the distance between the predicted

target position and pbi as

di(tk+1) = ‖pbi − p

p(tk+1)‖2, i = 1, . . . , N.

To allow for the correct computation of the target position

at time instant tk+1, (7) must equal

dpi(tk+1) = dpi(tk+1)/vsound.

Because the target is moving, the control station estimates

the target state at time T ∗ (k + 1) + dpi(tk+1) that should

correspond to the position

xp(tk+1 + dpi(tk+1)) = H(ω, dpi(tk+1))x

p(tk+1).

where the target receives the signals. Assuming that the

buoys move very slowly, the distance between the buoys

positions pbi (tk+1) and x

p(tk+1+dpi(tk+1)) will be di(tk+1+dpi(tk+1)). Given the results available from the positioning

system implemented in the control station, the value for

dpti(tk+1) is known and the TOF of the signal emitted by

the buoys and received on the estimated position for the target

xp(tk+1+ dpi(tk+1)), is di(tk+1+ dpi(tk+1))/vsound. Finally,

taking into account the previous description, the emission time

at each buoy can be computed as

sbi (tk+1) = tk+1+dpi(tk+1)+δt−di(tk+1+dpi(tk+1))/vsound,

with the following interpretation: the emission time of the

signal on the buoy i, sbi (tk+1), is the subtraction of the desired

instant of arrival tk+1 + dpi(tk+1) + δt (where δt is the

positioning system state variable that estimates Υ) from the

TOF di(tk+1 + dpi(tk+1)/vsound, as depicted in Fig.4). In the

absence of noise, the target computation of (7) would result

in

dpti(t

tk+1) =

rti(t

tk+1)

︷ ︸︸ ︷

ttk+1 − Υ + dpi(tk+1) + δt−ttk+1 =

dpi(tk+1) − Υ + δt ≈ dpi(tk+1),

thus compensating for the undesired perturbations that can be

estimated and canceled out1.

tk tk+1 tk+2ttk+1

Υ

t

rti(t

tk+1)sb

i (tk+1)

di(tk+1+dpi(tk+1))vsound

dpi(tk+1)

Fig. 4. Emission and reception cycle for a signal, emitted at sbi (tk+1)

and received at the time instant rti(t

tk+1).

Finally, the observations from the robot target point of view

will be briefly outlined. When a signal arrives, the target

computes (7) and the result is fused in the target navigation

system as

zni(r

ti(t

tk)) = zn

i (ttk) =‖pb

i − pt(ttk)‖2

vsound

+ nni (ttk), (8)

where nni (ttk) ) is stationary, zero-mean, Gaussian white noise

with constant standard deviation σi, where nni (ttk) and nn

j (ttk)are independent for i 6= j. The measurements zn

i(rti(t

tk))

correspond to the TOF of the acoustic wave from pbi to pt(ttk),

computed at rti(t

tk). In addition to the information embedded

in the signals emitted by the buoys, depth is also available

from a depth sensor that gives measurements of zt(ttk). The

model considered for this measurement is

znz (ttk) = zt(ttk) + nn

z (tk), (9)

where nnz (ttk) is zero-mean, Gaussian white noise, with con-

stant standard deviation σz . It is also assumed that nnz (ttk) is

independent of nni (ttk) i = 1, . . . , N .

As in the positioning system case, not all the signals sent

by the buoys are validated upon the reception at the target.

Depending on the acoustic conditions, the number of valid

measurements o varies from 0 to N (i.e., 0 ≤ o ≤ N ). This

can be expressed as

z′(ttk) =(Co

[zn

1(rt1(t

tk)) · · · zn

N (rtN (ttk))

])T

1Note that the material included until this point could have been explainedin the Control Segment. However, for the sake of readability of the paper itwas included in the current segment.

6

and the navigation system measurements vector is then

zn(ttk) =[

z′(ttk) znz (ttk)

]T

.

It is also convenient to define

n′(ttk) = Co[

np1(t

tk) · · · np

N (ttk)],

nn(ttk)) =[

n′(ttk) nnz (ttk)

]T

,

R′ = diag{Cn

[σ1 · · · σN

]}

and

Rn = diag{[

R′ σz

]}.

A. Underlying Model

The strategy adopted in the design of the navigation system

is similar to the one outlined for the positioning system in the

Control Segment. Resorting to the target model introduced in

(3), with the nonlinear measurements model as in (8) and (9),

an Extended Kalman Filter is used. An introduction to EKF

was already made in section III, so we will restrict here to the

presentation of the Jacobian matrices

An(ttk) = A

n(xn(ttk)) =

∂f(x, w)

∂x

∣∣∣∣∣xn(tt

k)

,

Ln(ttk) = L

n(xn(ttk)) =

∂f(x, w)

∂w

∣∣∣∣∣xn(tt

k)

,

Cn(ttk) = C

n(xn(ttk)) =

∂zn

∂x

∣∣∣∣∣xn(tt

k)

and,

and

Dn(ttk) = D

n(xp(ttk)) =

∂zn

∂nn

∣∣∣∣∣xn(tt

k)

,

where xn(ttk) is the estimated value of the state vector x(ttk).

As the target model (3) is linear it follows that

Ap

= Ap(ttk) = H(ω, T ) and L

p= L

p(ttk) = I9,

Furthermore, by defining

Cn

z (tak) =[

0 0 0 0 0 0 0 0 1]T

and,

Cn

i (ttk) =

−xb

i−xn(ttk)

ˆdn

i(ttk)

00

−yb

i−yn(ttk)

ˆdn

i(ttk))

00

−zb

i−zp(ttk)

ˆdn

i(ttk)

00

, i = 1, . . . , N

with

dni(t

tk) = ‖pb

i − pn(ttk)‖2

and

Dn

= Dn(ttk) = Io+1,

it follows that

Cn(tk) =

[

Co[

Cn

1 (ttk) · · · Cn

N (ttk)]

Cn

z (tak)]T

.

B. Navigation System: Extended Kalman Filter Design

The algorithm adopted for the navigation is similar to the

one developed for the positioning system. Therefore, it will

only be summarized in this section. At the time instant ttk+1,

the observations zn(ttk+1) are not all available, therefore an

a priori system state estimate and respective estimation error

covariance matrix are computed (predict step), using

xn−

(ttk+1) = H(ω, T )xn(ttk),

and

Pn−

(ttk+1) = An

Pn(ttk+1)An

T + Ln

QnLn

T.

Upon successful validation of the measurements, a filter state

and covariance update step is carried out, according to

xn+(ttk+1) = x

n−

(ttk+1) + Kn(ttk+1)[zn(ttk+1) − z

n(ttk+1)],

and

Pn+(ttk+1) = Pn

−(ttk+1) − Pn

−(ttk+1)C

nT(ttk+1)

[

Cn(ttk+1)P

n−

(ttk+1)Cn

T(ttk+1) + Dn

RnDn

T

]−1

Cn(ttk+1)P

n−

(ttk+1),

where

Kn(ttk+1) = Pn+(ttk+1)C

nT(ttk+1)

[

Dn

RnDn

T

]−1

.

VI. RESULTS

This section describes the results obtained from a series

of computer experiments aimed at validating the proposed

architecture and assessing its performance. In the simulations,

the buoys were initially placed on a square configuration,

with a side of 2 Km, and moved at an average velocity of

0.15 m/s, in the West-East direction. The target travels with

an approximately constant velocity of 1.6 m/s describing semi-

circumferences with a radii of 500 m in the horizontal plane.

Table I presents the initialization parameters for the simulation.

It is a well known fact that not all acoustic signals emitted

are correctly received [2]. To capture this phenomena random

variables with Bernoulli distribution were associated to the

success on the measurements detection for the positioning and

navigation systems, with parameters βp and βn, respectively.

A. Positioning system

The actual and estimated target trajectories are depicted in

Fig. 5 for the experiment described above. Figure 6 depicts the

error between the actual and the estimated trajectories, for the

x and y coordinates, where the error is 99% of the time under

5 m (two times the standard deviation of the distribution, when

in the presence of Gaussian disturbances). As the target moves

only in the horizontal plane, the error in z decreases with time.

7

TABLE I

SIMULATION PARAMETERS

T 1 sN 4

x(0)[

0 0.01 0 1000 1.6 0 300 0 0 0.01]T

xp(0)

[14 −0.12 0.01 984 0.74 0.0001 296 0 0 0.0017

]T

xn(0)

[109 −0.014 0.01 993 1.9 −0.0006 298 0 0

]T

P p(0) diag{[

502 0.52 0.0012 352 0.52 0.0012 32 0 0 0.012]}

Pn(0) diag{[

502 0.52 0.0012 352 0.52 0.0012 12 0 0]}

S diag{[

0.0012 0.0012 0]}

σi 0.0033, i = 1, . . . , Nσz 1

βp 0.1

βn 0.1

Other simulations, not present due to space limitations, have

shown that even with an initial error of 500 m in the x and/or ycoordinates, the system behavior is similar to the one reported

in this paper. The the number of valid observations during

600

800

1000

1200

1400

−10000

10002000

30004000

5000400

300

200

y[m]

Actual and estimated target tragectories

x[m]

z[m

]

Actual

EKF

Fig. 5. Simulated and estimated trajectories. The trajectories beginat the points marked with ∗.

0 500 1000 1500 2000 2500 3000 3500 4000−8

−6

−4

−2

0

2

4

6

8

time [s]

erro

r [m

]

Positioning system error

yxzΥ

Fig. 6. Error between actual and the estimated trajectories andbetween Υ and δt

the trajectory are shown in Fig. 7.

B. Navigation system

The results obtained with the Navigation System are de-

picted in Fig. 8, for the same experiment as described above.

In this figure, the nominal and estimated robot trajectories

are depicted. In Fig. 9 the error between the actual and the

estimated trajectories is shown. In a similar way as in the

positioning system, for the x and y coordinates the error is

under 5 m, 99% of the time. As the robot sensor package

includes also a depth sensor and a rategyro triad, the z

0 500 1000 1500 2000 2500 3000 3500 40000

1

2

3

4

time [s]

Val

idat

ed

Number of valid observations

Fig. 7. Number of valid observations in the positioning system.

coordinate and ω present small errors. Other experiments have

also shown that the EKF was able to provide estimates with

small errors, even for larger initial errors. The number of valid

600

800

1000

1200

1400

050010001500200025003000350040004500400

300

200

y[m]

Actual and estimated target tragectories

x[m]

z[m

]

Actual

EKF

Fig. 8. Simulated and estimated trajectories. The trajectories beginat the points marked with ∗.

observations during the trajectory is shown in Fig. 10. Note

that the performance of the navigation system does not degrade

at each reception cycle, given the good characteristics of the

EKF for sensor fusion. In the navigation system developed the

buoys do not need to be moored in a fixed position. However,

before mission deployment, the target is programmed with the

reference position for each buoy. In Fig. 11 the trajectories

of the target and of one of the buoys are shown, as well

as the buoy’s emission (compensated) delays ∆t, relative

to the reference time tk. The results obtained illustrate the

effectiveness of the proposed method in compensating for

the estimated disturbances: the clock bias at the target, the

target movement during the acoustic waves propagation, and

8

0 500 1000 1500 2000 2500 3000 3500 4000−20

−10

0

10

20

time [s]

erro

r [m

]

Navigation system error

x

y

z

Fig. 9. Error between actual and the estimated trajectories.

0 500 1000 1500 2000 2500 3000 3500 40000

1

2

3

4

time [s]

Validate

d

Number of valid observations

Fig. 10. Number of valid observations in the navigation system.

the buoys drift at surface. A simulation with the buoys fixed

−500 0 500 1000 1500−1000

0

1000

2000

3000

4000

5000

y [m]

Target and bouy tragectories

x [

m]

0 1000 2000 3000 4000−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

time [s]

del

ay [

s]

Buoy emission delay

Buoy

Target

begin

end

Fig. 11. Left: trajectories of the target (estimated by the positioningsystem) and of one of the buoys. The ∗ point is the buoy referenceposition. Right: signal emission delay relatively to tk

in the reference positions and with the systems synchronized

Υ = 0 (i.e. under ideal conditions) was performed. The system

was initialized with the same parameters used in the previous

experiments and the target performed the same trajectory. See

a summary of the results obtained in Tab. II, namely for the

error variance in the x, y and z coordinates under ideal and

disturbed conditions.

TABLE II

ERROR VARIANCE UNDER REALISTIC AND IDEAL CONDITIONS

Realistic conditions Ideal conditions

x 3.997 4.01

y 7.371 6.89

z 0.0031 0.0027

VII. CONCLUSION

This paper described a new joint positioning and aiding

navigation system (JPN) that provides both positioning data

and navigation aids simultaneously, composed by three seg-

ments: a) Surface Segment: consists of a synchronized net of

hydrophones and pingers attached to surface buoys that are

equipped with DGPS receivers; b) Underwater Segment: the

submerged target emits periodically a signal that is received by

each of the hydrophones at the buoys, which then transmit via

radio the time of arrival of the signals (TOA) to a control sta-

tion positioned on a support vessel or on land; and c) Control

Segment: using an Extended Kalman Filter (EKF), the control

station computes the position and the timing compensations

among the buoys (synchronized at surface using accurate GPS

timing signals) and the target. Resorting to the estimate of

the position of the target, the control station computes the

instant at which each buoy must send a signal such that it

will be received by the target and used as an external aiding

signal by the onboard navigation system. Simulation results

for the joint positioning and navigation system have shown

the viability of this approach as an improvement for the

operation of underwater vehicles. To complete the design of

the positioning system a Multiple Model Adaptive Estimator

will be designed to estimate the target angular velocity, from a

set of models chosen to describe the evolution of the dynamics

of AUVs in underwater surveying missions. In the near future

this architecture will be implemented and tested at sea during

the operation of the Infante AUV, property of the IST/ISR.

REFERENCES

[1] ACSA, ORCA. Trajectographe GIB Manuel Utilisateur, 1999.[2] A. Alcocer, P. Oliveira, and A. Pascoal. Study and implementation of

an ekf gib-based underwater positioning system. Control Engineering

Practice, 15:689–701, Junho 2006.[3] J. Farrell and M. Livstone. Exact calculations of discrete-time process

noise statistics forhybrid continuous/discrete time applications. Proceed-

ings of the 32nd IEEE Conference on Decision and Control, 1:857 –858, Dezembro 1993.

[4] A. Gelb. Applied Optimal Estimation. M.I.T. Press, 1974.[5] J. C. Kinsey and L. L. Whitcomb. In situ alignment calibration of attitude

and doppler sensors for precision underwater vehicle navigation: theoryand experiment. IEEE Journal of Oceanic Engineering, 32(2):286 –299, 2007.

[6] X. Rong Li and V. J. Jilkov. Survey of maneuvering target tracking.part i. dynamic models. Aerospace and Electronic Systems, IEEE

Transactions, 41:1255 – 1321, Outubro 2005.[7] P.H. Milne. Underwater Acoustic Positioning Systems. Gulf Publishing

Company, 1983.[8] M. Morgado, P. Oliveira, C. Silvestre, and J. F. Vasconcelos. Vehicle

dynamics aiding technique for usbl/ins underwater navigation system.IFAC Conference on Control Applications in Marine Systems CAMS07,2007.

[9] P. Oliveira. Mmae terrain reference navigation for underwater vehiclesusing pca. International Journal of Control, 80(7):1008–1027, 2007.

[10] H. G. Thomas. Gib buoys: an interface between space and depths of theoceans. Proceedings Of The 1998 Workshop on Autonomous Underwater

Vehicles, pages 181–184, 1998.[11] K. Vickery. Acoustic positioning systems. a practical overview of current

systems. Autonomous Underwater Vehicles, 1998.