Pos Mission T ajec o y Smoo hing o he Isu us AUV
Aníbal Ma os, Nuno C uz and Fe nando Lobo Pe ei a
Faculdade de Engenha ia da Uni e sidade do Po o
Ins i u o de Sis emas e Robó ica
R. D . Robe o F ias
4200-465 Po o
Po ugal
{anibal, nac uz, lp}@ e.up.p
Abs ac – In his pape we desc ibe an algo i hm ha
p oduces a pos mission es ima e o he spa ial e olu ion o
he Isu us AUV. To make his pos p ocessing possible, he
na iga ion sys em eco ds on he ehicle logging sys em all
he na iga ion da a ecei ed du ing he mission execu ion.
The da a comp ise he dep h o he ehicle, he ou pu s o he
il senso s and digi al compass, he angula eloci y o he
p opelle , as well as acous ic ange measu emen s o a se o
ansponde s. A e mission comple ion, he logged da a is
hen p ocessed o p oduce he es ima e o he e olu ion o he
ehicle. The algo i hm used o p ocess his da a is based on a
ixed in e al nonlinea s ochas ic smoo hing scheme and
p oduces an es ima e ha e ol es con inuously in ime. Fo
each ins an o ime, he pos mission posi ion es ima e is
based on all he in o ma ion collec ed du ing he mission, as
opposed o eal ime es ima es ha can only ake in o accoun
pas da a.
I. INTRODUCTION
Isu us is an AUV ope a ed and pa ially de eloped a he
Unde wa e Sys ems and Technology Labo a o y (LSTS)
om Po o Uni e si y. This ehicle has been pe o ming
se e al missions o he las ew yea s in di e en ope a ional
scena ios, mainly collec ing CTD and ba hyme ic da a. A e
he execu ion o each mission, he collec ed da a is p ocessed
acco ding o he inal pu pose o he mission. In almos all
he cases, i is necessa y o loca e he collec ed da a bo h in
ime and in space. I ime loca ion is a i ial ask ( he ehicle
ca ies a eal ime clock), he spa ial loca ion equi es
in o ma ion ela ed o he na iga ion sys em o he ehicle.
The main unc ion o he na iga ion sys em o an AUV is o
p o ide an es ima e o he ehicle posi ion in eal ime. In he
case o Isu us, he na iga ion sys em ecei es dep h da a
om a p essu e cell, a i ude da a om an in eg a ed se o il
senso s and digi al compass, and eloci y da a om an
encode coupled o he p opelle . Absolu e posi ioning da a is
p o ided by ime o ligh based ange measu emen s o a se
o acous ic ansponde s deployed in he ope a ion a ea. This
sys em employs a Kalman il e based p ocedu e o ob ain a
eal ime posi ion es ima e. This es ima e is compu ed by ime
in eg a ing he ehicle eloci y and co ec ing he in eg a ed
alue wi h ange measu emen s om he acous ic sys em.
Al hough his es ima e is accu a e enough o he guidance
and con ol o he ehicle, i is no well sui ed o spa ially
loca ing he da a collec ed by he AUV. In ac , he disc e e
na u e o he co ec ion mechanism gi es ise o
discon inui ies in he e olu ion o he es ima e, which is a
majo d awback o spa ially loca ing he collec ed da a.
This pape p esen s an algo i hm ha p ocesses he da a
collec ed by he ehicle na iga ion sys em du ing he
execu ion o a mission and p oduces an es ima e o he
ajec o y desc ibed by he ehicle mo e sui ed o spa ially
loca ing he oceanog aphic da a ga he ed. This es ima e
e ol es con inuously in ime and is compu ed using
echniques based on a nonlinea s ochas ic smoo hing
scheme.
The pape is o ganized as ollows. In sec ion II we desc ibe
he Isu us AUV. Then we desc ibe he Isu us na iga ion
algo i hm ha uses in eal ime he dead eckoning da a
oge he wi h absolu e posi ion in o ma ion. In sec ion IV we
p esen he pos mission ajec o y smoo hing algo i hm.
Finally, we p esen expe imen al esul s ha show ha he
pos -mission smoo hing algo i hm can g ea ly imp o e he
posi ion es ima e gi en by he on-boa d na iga ion sys em,
mainly when he ehicle mo es in u ns o when ange
measu emen s a e sca ce.
II. THE ISURUS AUV
Au onomous Unde wa e Vehicles cons i u e powe ul and
e ec i e ools o unde wa e da a ga he ing. These ehicles
ope a e wi h no ex e nal di ec con ol, ca ying a se o
ele an senso s o cha ac e ize he unde wa e en i onmen
and o loca e hemsel es.
Isu us (Fig. 1) is a REMUS (Remo e En i onmen Measu ing
Uni S) class AUV, buil by he Woods Hole Oceanog aphic
Ins i u ion, MA, USA, in 1997. These ehicles a e low cos ,
ligh weigh AUVs specially designed o coas al wa e s
moni o ing [1]. The educed weigh and dimensions makes
hem ex emely easy o handle, equi ing no special
equipmen o launching and eco e y. Fo he pas 6 yea s,
he LSTS has been cus omizing Isu us, and pe o ming
oceanog aphic and en i onmen al moni o ing missions wi h
1234
i in di e en ope a ional scena ios, including es ua ies, i e
dams and coas al ocean wa e s [2,3].
Fig. 1 – The Isu us AUV a he su ace
Isu us has a diame e o 20 cm and is abou 1.5 me e s long,
weigh ing abou 35 kg in ai . Inside he hull, se e al
subsys ems ha e been imp o ed o speci ically de eloped a
LSTS, con ibu ing o he con inuous enhancemen o he
ehicle pe o mance and eliabili y. The maximum o wa d
speed o he ehicle is 4 kno s, howe e he bes ene gy
e iciency is achie ed a abou 2 kno s. A his eloci y, he
ene gy p o ided by a se o echa geable Li hium-Ion
ba e ies may las o o e 20 hou s (i.e., o e 40 nau ical
miles). Al hough small in size, his ehicle can accommoda e
a wide ange o oceanog aphic senso s, such as CTD,
al ime e , sidescan sona , and op ical backsca e . Each o
hese senso s can be independen ly ins alled on boa d,
enabling he use o he ehicle in di e en con igu a ions
acco ding o he equi emen s o each mission.
The na iga ion sys em o he Isu us AUV es ima es he
posi ion o he ehicle based on da a p o ided om se e al
de ices and sys ems. The e ical coo dina e is ob ained om
a dep h cell ins alled on he ehicle. To es ima e he ehicle
ho izon al posi ion, he na iga ion sys em uses oge he dead
eckoning da a wi h absolu e posi ioning da a. The dead
eckoning da a is composed by he ehicle a i ude, ob ained
om a digi al compass and a se o il senso s, and also by
he ehicle eloci y wi h espec o he wa e , ob ained om
an encode ha measu es he p opelle o a ion speed.
The absolu e posi ioning da a consis s in ange measu emen s
o acous ic beacons deployed in he ope a ion a ea. These
beacons (Fig. 2) a e mul i- equency ansponde s and we e
de eloped a he LSTS [4]. Each beacon can be con igu ed o
eply wi h a signal o gi en equency when in e oga ed by
ano he signal o a possibly di e en equency.
Fig. 2 – Mul i- equency ansponde
To ob ain a ange measu emen o a pa icula beacon, he
ehicle has o send an in e oga ion signal and hen wai o
and de ec he beacon eply signal. The ange is compu ed
om he o e all ime o ligh o he acous ic signals
exchanged [5].
III. NAVIGATION ALGORITHM
Since he ehicle dep h is ob ained di ec ly om he dep h
cell, he majo ask o he na iga ion algo i hm is he usion
o he dead eckoning da a wi h he ange measu emen s o
es ima e he ho izon al posi ion.
The ins an aneous eloci y wi h espec o he wa e is
ob ained by measu ing he p opelle o a ion speed and he
ehicle heading, pi ch and oll. Veloci y measu emen s a e
used oge he wi h ange measu emen s by a Kalman il e
based algo i hm [6], aking ad an age o he cha ac e is ics o
each ype o da a. On one hand, he ehicle eloci y is
a ailable a a high a e, bu i s in eg a ion leads o a d i in
he es ima ed posi ion. On he o he , ange measu emen s,
a ailable a a lowe a e, can be noisy bu do no d i o e
ime. The algo i hm upda es he es ima e o he ehicle
posi ion a he same a e he eloci y is measu ed, and
co ec s i whene e a new ange measu emen is a ailable,
gi ing he bes es ima ed posi ion in eal- ime.
The ho izon al posi ion o he ehicle is de ined by he no h
(x) and eas (y) de ia ions om a base poin de ined o each
ope a ion. The il e ing algo i hm is based on a simple model
ha ela es he p opelle speed (ω), he ehicle pi ch (θ) and
heading (ψ) angles o he ime de i a i es o he x and y
coo dina es, de ined by he ollowing equa ions
cos( )cos( )
x
x
kw
=
ωθ ψ+
 (1)
cos( )sin( ) y
yk w
=
ωθ
ψ
+
 (2)
This model oughly cha ac e izes he ehicle mo ion in a
s aigh line a a cons an speed. In he abo e equa ions, wx
and wy ep esen he no h and eas componen s o he wa e
cu en eloci y, espec i ely, and k is a cons an o
p opo ionali y ob ained om a linea iza ion o he
ela ionship be ween p opelle o a ion speed and ehicle
longi udinal eloci y in s eady s a e.
The na iga ion algo i hm is based on a con inuous disc e e
Kalman il e , wi h he ehicle mo ion cha ac e ized by he
di e en ial equa ions (1) and (2). The il e has a s a e o
dimension 4, co esponding o he es ima es o he no h and
eas coo dina es o he ehicle posi ion and also o he no h
and eas componen s o he wa e speed. To keep he no a ion
simple he il e s a e will be ep esen ed by
[]
x
y
Exyww
=
. The il e also keeps a ma ix wi h he
co a iance o he es ima ion e o (P) ha is used o assess
he quali y o he posi ion es ima e.
Be ween he ecep ions o wo consecu i e ange
measu emen s, he e olu ion o E and P is uled by he
di e en ial equa ions
1235
cos( )cos( )
cos( )sin( )
(,,,) 0
0
x
x
kw
k
EFE
ωθ ψ+


ωθ ψ+

=ωθψ=




w


(3)
T
FF
EE
PPP
∂∂
∂∂
=⋅+⋅ +
Q (4)
whe e Q is a, possibly ime a ying, symme ic semi-posi i e
de ini e ma ix ha ep esen s he a e o inc ease o he
es ima ion e o , due o he impe ec modeling o he ehicle
mo ion.
Whene e a new ange measu emen is ecei ed and is
alida ed, he s a e E and he co a iance ma ix P a e
co ec ed acco ding o he exp essions
)( *
KEE −⋅+= −+ (5)
E P
−−+ ⋅⋅−=
P
H
K
P
P
(6)
whe e E
−
and P
−
a e he alues o he E and P be o e he
co ec ion and E+ and P+ a e hei alues a e he co ec ion;
is he measu ed ange and * is he expec ed ange o he
ansponde . The ma ices H and K a e, espec i ely, he
jacobian o he obse a ion wi h espec o he s a e, and he
Kalman gain [5,6].
IV. TRAJECTORY SMOOTHING
The na iga ion algo i hm p esen ed abo e p o ides a each
momen an es ima e o he ehicle posi ion. This es ima e is
upda ed by wo di e en mechanisms wi h qui e di e en
ea u es. The i s depends on he dead eckoning da a and is
desc ibed by he di e en ial equa ions (3) and (4). I is a
sampled con inuous p ocess, due o he sampling o he dead
eckoning da a (pe o med a 10 Hz). The o he upda ing
mechanism ac s whene e a new ange measu emen is
a ailable and upda es he posi ion es ima e acco ding o he
co ec ion equa ions (5) and (6). I is, he e o e, a disc e e
e en p ocess.
Due o he disc e e ime na u e o he co ec ion mechanism,
he posi ion es ima e gi en by he na iga ion algo i hm does
no e ol e con inuously (e en i he dead eckoning we e
pe o med a an in ini e sampling a e). I can ha e
discon inui ies a he momen s o co ec ions mo i a ed by
ecep ion o ange measu emen s. Such discon inui ies a e
inhe en o he na iga ion algo i hm employed as i p o ides
a each ins an he bes es ima e o he ehicle posi ion a ha
ins an , based on all he in o ma ion collec ed up o ha
momen .
While hese discon inui ies in he es ima e o he ehicle
posi ion a e accep able as a as na iga ion is conce ned, hey
a e a majo d awback whene e he ehicle is collec ing da a
ha ha e o be spa ially loca ed, as in he case o ba hyme y
missions. To o e come such di icul y we implemen ed an
algo i hm o pos p ocess he da a collec ed by he na iga ion
sys em du ing he execu ion o he mission. The algo i hm is
applied upon he execu ion o he mission and compu es he
spa ial e olu ion o he ehicle. The new posi ion es ima es
a e based on he in o ma ion collec ed du ing all he mission,
as opposed o he eal- ime es ima es ha can only ake in o
accoun pas da a.
The smoo hing algo i hm, desc ibed below, is based on he
Rauch-Tung-S iebel nonlinea smoo he [6]. The smoo he
has he same s a e o he eal ime il e .
Le { wi h i}
i
0,...N
=
, be he ins an s o ime a which he
dead eckoning da a we e collec ed du ing he mission. To
simpli y he p esen a ion, we a e assuming he e ha he ange
measu emen s can only occu a he ime ins an s i. The goal
o he smoo hing algo i hm is o compu e he es ima e o he
s a e E and o he e o co a iance ma ix P, a he ime
ins an s i, based on all he da a collec ed. Such es ima es will
be deno ed by and , espec i ely. These alues a e
compu ed in wo s eps. The i s s ep is essen ially he same
as he eal ime Kalman il e . This s ep compu es a sequence
o he es ima es o he s a e E and o he e o co a iance
ma ix P, a he ime ins an s
|iN
1]
|iN
i, each one based only on pas
da a. These es ima es, deno ed by Ei and Pi, espec i ely, a e
ob ained sequen ially acco ding o he ollowing p ocedu e.
The di e en ial equa ions (3) and (4) a e in eg a ed in he
in e al [,
ii
+
wi h ini ial da a Ei and Pi, espec i ely, o
ob ain 1i
E
−
+
and 1i
P
−
+
. I he e is no new ange measu emen a
he ins an 1i
+
, hen 1i
E1i
E
−
+
+
= and 1ii
PP
1
−
+
=
1i
+
+
. I he e is a
new ange measu emen a he ins an , hen 11ii
EE
+
+
+
=
and 1i1i
PP
+
+
+
=, whe e 1i
E
+
+
and a e compu ed acco ding
o (5) and (6).
1i
P+
+
In he second s ep o he smoo hing algo i hm, and
a e compu ed om and (and using he alues
om he i s s ep), acco ding o he equa ions
|iN
E|iN
P
1|iN
E+1|iN
P+
|1|
(
iN i i i N i i i
EEKE Eb
+)
=
+−
φ
− (7)
1
(
iiiiii i
KP P Q)
−
=φφφ+
TT (8)
|1|
(
iN i i i N i i i i i
PPKP PQK
+
=+ −φφ−
T
)
T
(9)
The ini ial condi ions a e and
|NN N
EE=|NN N
PP
=
. In he
abo e equa ions, is he alue a o he solu ion o he
di e en ial equa ion
i
b1i+
F
E
F
∂
∂
=+ F
E
Ebb wi h he ini ial
condi ion
∂

∂
−
() 0
i
b
=
,
1
(,
i
i
ii
Q
+
) ()(, )
i
Q d
=
φτ
∫τφτ τ
T, and
1
(,
ii
+)
i
φ
=φ , whe e (,)
φ
⋅⋅ is he ansi ion ma ix associa ed
o he linea ized ehicle dynamics, i.e., F
E
zz
∂
∂
=
.
V. EXPERIMENTAL RESULTS
Bo h he na iga ion and he pos mission smoo hing
algo i hms desc ibed he e ha e been ex ensi ely es ed
di e en scena ios o ope a ion. The es ing p og am enabled
1236
us o ine une he pa ame e s o bo h algo i hms o op imal
pe o mance. Cu en ly, hese algo i hms a e employed in
i ually all he missions pe o med wi h Isu us.
We p esen below some esul s illus a ing he beha io o
bo h he eal ime na iga ion and he pos mission smoo hing
algo i hms. The da a was collec ed du ing a 2 hou s mission
ha ook place 2 km o he po uguese coas nea A ei o. In
his mission, Isu us collec ed CTD and ba hyme ic da a in an
a ea abou 100 m by 200 m a se e al dep hs.
Fig. 3 shows he eal- ime es ima e o he ehicle posi ion
compu ed by he na iga ion algo i hm ( ed line) as well as
he desi ed ajec o y (black line). The discon inuous na u e
o he es ima ed ajec o y can be eadily obse ed in his
igu e, since he e we e la ge co ec ions o he es ima ed
posi ion. Typically, hese la ge co ec ions a ise jus a e he
ehicle being wi hou ecei ing ange measu emen s o a
long ime ( ypically mo e han 10 seconds).
-100 -50 050 100
-140
-120
-100
-80
-60
-40
-20
0
eas (m)
no h (m)
Fig. 3 – Real ime ajec o y es ima ion
I should be e e ed he e ha he la e al o se be ween he
wo ajec o ies in he eas -wes lines is no he esul o a
poo pe o mance o he na iga ion algo i hm, bu is a di ec
consequence o he s ong wa e cu en om he no h ha
was p esen du ing he mission. In ac , he heading e e ence
gene a ed by he ehicle guidance sys em does no in eg a e
he o ack e o , gi ing ise o la ge o ack e o s when
he e a e la ge wa e cu en s pe pendicula o he desi ed
ajec o y.
-100 -50 050 100
-140
-120
-100
-80
-60
-40
-20
0
eas (m)
no h (m)
Fig. 4 – Pos mission ajec o y es ima ion
Figu e 4 p esen s he es ima ed ajec o y p oduced by he
smoo hing algo i hm. As expec ed, his ajec o y does no
p esen discon inui ies, since each co ec ion due o a ange
measu emen is dis ibu ed along he mission in e al, and is
no accoun ed o a a single ins an o ime.
A de ail o he wo es ima es o he ehicle ajec o y is
shown in ig. 5, we e he ed line co esponds o he eal ime
es ima e and he blue one o he pos mission es ima e. The
wo es ima es a e e y simila when he ehicle is going in a
s aigh line o is u ning slowly. When he ehicle makes
igh u ns o when he e a e la ge co ec ions in he eal ime
es ima e, he wo es ima es can be a ew me e s away. Since
he dynamic model used he e does no desc ibe wi h
su icien accu acy he ehicle mo ion du ing u ns, la ge
co ec ions in he eal ime es ima e a e expec ed when he
ehicle is u ning.
55 60 65 70 75 80 85 90 95 100 105 110 115
-80
-75
-70
-65
-60
-55
-50
-45
-40
-35
eas (m)
no h (m)
Fig. 5 – Compa ison o he wo es ima es
One ad an age o using s ochas ic il e ing echniques, like
he Kalman il e used in he eal ime es ima ion o he
smoo hing il e used he pos mission es ima ion, is he
in o ma ion p o ided by he e o co a iance ma ix P. The
2×2 sub-ma ix o P, deno ed Pxy, and co esponding o he x
and y componen s o he il e s a e E, can be used o measu e
he quali y o he es ima e o he ehicle posi ion. The ma ix
Pxy de ines an unce ain y ellipse in he plane ha
cha ac e izes he e o o he posi ion es ima e. This ellipse in
de ined by 3 pa ame e s: he leng hs o i s wo p incipal axes
and he o ien a ion o i s majo axis.
Figu es 6 and 7 show he e olu ion o he leng hs o he axes
o he unce ain y ellipse along he mission o he eal ime
and he pos mission es ima es, espec i ely. The highe
alues o unce ain y occu ei he when he ehicle is u ning
o when ange measu emen s a e sca ce. In he eal ime case
he leng hs o he axes a e mos ly be ween 1 and 2 me e s,
while in he pos mission case hese leng hs a e almos
always below 0.8 me e s. This illus a es he supe io
accu acy o he pos mission ajec o y es ima e, as expec ed.
1237
01000 2000 3000 4000 5000 6000 7000
0
1
2
3
4
5
6
7
ime (s)
unce ain y (m)
Fig. 6 – Real ime unce ain y ellipse axes
01000 2000 3000 4000 5000 6000 7000
0
0.4
0.8
1.2
1.6
2
ime (s)
unce ain y (m)
Fig. 7 – Pos mission unce ain y ellipse axes
Finally, igu e 8 shows he e olu ion o he leng hs o he
majo axes o he unce ain y ellipses o bo h he eal ime
es ima e ( ed line) and he pos mission es ima e (blue line),
du ing a sho pe iod o he mission. The eal ime
unce ain y p esen s a ypical saw- oo h beha io : he
unce ain y g ows as he dead eckoning da a is in eg a ed
o e ime and is “ ese ” o a low alue whene e a new ange
measu emen is ecei ed by he na iga ion algo i hm. On he
con a y, he unce ain y o he pos mission es ima e a ies
smoo hly, e lec ing he con inui y p ope ies o his es ima e.
None heless, he wo unce ain ies e ol e in a co ela ed
way, since hei highe alues and hei lowe alues occu a
almos he same ins an s o ime.
6050 6100 6150 6200 6250 6300
0
0.5
1
1.5
2
2.5
3
3.5
ime (s)
unce ain y (m)
Fig. 8 – Compa ison o he wo unce ain ies
VI. CONCLUSIONS
Bo h he eal ime and he pos mission ajec o y es ima ion
algo i hms desc ibed in his pape ha e been success ully
implemen ed and es ed unde se e al condi ions. In a i s
s age, he pe o mances o bo h algo i hms ha e been
e alua ed wi h da a aken om p e ious missions. A e a
ew adjus men s in he pa ame e s o he algo i hms, hey
we e inally alida ed in eal applica ions.
The esul s ob ained so a also show ha he pos mission
ajec o y smoo hing algo i hm can g ea ly imp o e he
posi ion es ima e gi en by he on-boa d na iga ion sys em,
mainly when he ehicle mo es in u ns o when ange
measu emen s a e sca ce. This is pa icula ly ele an in he
cha ac e iza ion o unde wa e ea u es, as collec ed da a has
o be spa ially loca ed wi h he maximum possible accu acy.
Al hough Isu us is no equipped wi h sophis ica ed dead
eckoning na iga ion sys ems such as ine ial measu emen
uni s o Dopple based acous ic elocime e s, he
pe o mance o he pos mission algo i hm seems o be e y
good, e en when he ehicle is u ning.
REFERENCES
[1] C. on Al , B. Allen, T. Aus in, and R. S okey, “Remo e
En i onmen al Measu ing Uni s”, P oceedings o he
Au onomous Unde wa e Vehicle's 94 Con e ence,
Camb idge, USA, 1994.
[2] N. C uz, J. Sousa, F. L. Pe ei a, A. Ma os, E. Sil a, J.
Coimb a, and E. Dias, “PISCIS: Mul iple Au onomous
Unde wa e Vehicles o En i onmen al and
Oceanog aphic Field S udies”, P oceedings
En i onmen 2010 Con e ence, Po o, Po ugal, 2003.
[3] N. C uz, A. Ma os, A. Ma ins, J. Sil a, D. San os, D.
Bou o , D. Fe ei a and F. L. Pe ei a, “Es ua ine
En i onmen S udies wi h Isu us, a REMUS class
AUV”, P oceedings o he MTS/IEEE Oceans’99
Con e ence. Sea le, USA, 1999.
[4] N. C uz, L. Madu ei a, A. Ma os, and F. Lobo Pe ei a,
“A Ve sa ile Acous ic Beacon o Na iga ion and
Remo e T acking o Mul iple Unde wa e Vehicles”,
P oceedings o he MTS/IEEE Oceans’01 Con e ence,
Honolulu, USA, 2001.
[5] A. Ma os, N. C uz, A. Ma ins and F. Lobo Pe ei a,
“De elopmen and Implemen a ion o a Low-Cos LBL
Na iga ion Sys em o an AUV”, P oceedings o he
MTS/IEEE Oceans’99 Con e ence, Sea le, USA, 1999.
[6] A. Gelb, Applied Op imal Es ima ion, MIT P ess, 1989.
1238