Intern
ati
o
n
a
l Jo
urn
a
l
o
f
R
o
botics
a
nd Au
tom
a
tion
(I
JR
A)
V
o
l.
3, N
o
. 3
,
Sep
t
em
b
e
r
2014
, pp
. 16
8
~
18
3
I
S
SN
: 208
9-4
8
5
6
1
68
Jo
urn
a
l
h
o
me
pa
ge
: h
ttp
://iaesjo
u
r
na
l.com/
o
n
lin
e/ind
e
x.ph
p
/
IJRA
Filterin
g
Meth
od for L
o
cation Es
timation of an Underwater
Robot
Na
k Y
o
n
g
Ko
*
1
,
T
a
e Gyun Kim
2
1
Dept.
Electron
i
cs Eng., Chosun
University
, Korea
2
Dept. Con
t
rol
& Instrumentation
Eng., Chosun
University
, Kor
e
a
Article Info
A
B
STRAC
T
Article histo
r
y:
Received
Mar 12, 2014
Rev
i
sed
May 12
, 20
14
Accepted
Jun 10, 2014
This pap
e
r describes an
application
of
extended Kalman filter
(EKF) fo
r
localization of an underwater ro
bot. For
the app
lication
,
lin
earized model of
robot motion
and sensor measurement
ar
e d
e
r
i
ved. Lik
e
usual EKF,
the
method is recursion of two
main steps: the time update (or prediction)
and
measurement update. The m
easurement update uses
exteroceptive sensors
such as four acoustic beacons
and a
pressure
sensor. The fou
r
beacons
provide four r
a
n
g
e data from th
ese beac
ons to th
e robot
and pres
sure sensor
does the depth d
a
ta of th
e robot.
One of the major contributions of the paper
is suggestion of
two m
easurement upd
ate appr
oaches. Th
e firs
t appro
a
ch
corrects the predicted states using th
e measurement data indiv
i
dually
.
Th
e
second one co
rrects th
e pred
icted st
ate using the m
easur
ement data
coll
ect
ivel
y.
Th
e s
i
m
u
lat
i
on an
al
y
s
is
s
hows
that
EKF
outperf
orm
s
leas
t
squares or odometr
y
b
a
sed dead-
r
eckoni
ng in the precision and robustness of
the estimation
.
Also, EKF with
colle
ctiv
e measurement
update brings out
better accur
a
cy
than th
e
EKF wi
th individu
al measurement update.
Keyword:
Collective m
e
a
s
urem
ent updat
e
Ex
tend
ed
Kalman
filter
Filterin
g
In
di
vi
dual
m
easurem
ent
u
pdat
e
Lo
calizatio
n
Un
de
rwat
er
r
o
bot
Copyright ©
201
4 Institut
e
o
f
Ad
vanced
Engin
eer
ing and S
c
i
e
nce.
All rights re
se
rve
d
.
Co
rresp
ond
i
ng
Autho
r
:
N
a
k Yon
g
Ko
Dept.
Electron
i
cs Eng., Chosun
University
, Korea
Em
a
il: n
y
k
o
@
ch
osun
.ac.kr
1.
INTRODUCTION
Kn
o
w
i
n
g t
h
e l
o
cat
i
on a
nd
or
i
e
nt
at
i
on i
s
vi
t
a
l
for na
vi
gat
i
on
of an
un
de
rwat
er m
obi
l
e
ro
b
o
t
[1
-2]
.
Local
i
zat
i
on i
s
al
so neede
d
f
o
r m
a
p bui
l
d
i
n
g
,
deci
si
o
n
m
a
ki
ng, e
xpl
o
r
at
i
o
n
,
en
vi
ro
n
m
ent
m
oni
t
o
ri
ng
, an
d
ob
ject
m
a
ni
pul
at
i
on i
n
u
n
d
er
wat
e
r e
n
vi
ro
n
m
ent
[3
-4]
.
There
ha
ve
be
en se
veral
t
e
c
h
nol
ogi
es
f
o
r
u
nde
r
w
at
er l
o
ca
l
i
zat
i
on. I
n
ert
i
al
navi
gat
i
o
n
a
i
ded
by
G
PS
(Gl
obal
Po
si
t
i
oni
ng Sy
st
em
) was one
of
t
h
e pract
i
cal
m
e
t
hods
. Thi
s
m
e
t
hod uses
i
n
ert
i
a
l
navi
gat
i
o
n
technology
when the robot
navigate
s
unde
rwater. On t
h
e
surface, it use
s
GPS t
o
fix t
h
e
bias acc
umulate
d
th
ro
ugh
th
e d
e
ad
-reckon
ing
.
It u
s
es IM
U
(In
e
rtial M
easure
m
ent Unit) and
DVL
(Dop
p
l
er Velo
city Log
)
for
dead-rec
k
oni
ng, and corrects
accum
u
late
d location e
r
ror using GPS whe
n
th
e robot surfaces
once in a while
[5]
.
Thi
s
m
e
t
hod re
q
u
i
r
es f
r
e
que
nt
su
rfaci
n
g
o
n
l
y
for l
o
ca
l
i
zat
i
on whi
c
h con
s
um
es t
i
m
e
and ene
r
gy
. A
l
so t
h
e
D
V
L
d
a
ta is
no
t av
ailab
l
e if
th
e ro
bo
t g
e
ts
o
u
t
o
f
bo
tto
m
tr
ack
ing
r
a
ng
e w
h
en
sur
f
acing
, thu
s
lets th
e r
obot
lo
se track
o
f
the lo
cation
.
Anothe
r m
e
thods
use
dist
ance
and/or bearing of
the
robot from
acous
tic
beacons.
T
h
e ac
oustic
beacon systems such as
USB
L
(Ultra
Short Base Line), SBL (Short Bas
e
Line), a
nd L
B
L (Long Base Line
)
provide locations inform
ation through
trilateration
or t
r
iangulation along
with least squares m
e
thod.
Unlike
th
e d
ead-reckon
ing
in
in
ertial n
a
v
i
g
a
ti
o
n
, they d
o
n
’
t accum
u
la
te error si
nce they rely only on the inform
ation
rel
a
t
i
v
e t
o
bea
c
on
s w
hose l
o
cat
i
on i
s
gi
ve
n
i
n
adva
nce.
Ho
we
ver
,
t
h
ey
req
u
i
r
e ex
pe
n
s
i
v
e aco
ust
i
c
beaco
n
sy
st
em
s and e
x
t
e
nsi
v
e cal
i
b
r
a
t
i
on e
f
f
o
rt
s.
Besid
e
s, they are availab
l
e
wh
en
t
h
e rob
o
t is with
in
so
me li
m
i
ted
range
from
the beacons
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
3, N
o
. 3,
Se
pt
em
ber 20
1
4
:
16
8 – 18
3
1
69
Othe
r a
p
proa
c
h
es a
p
propriate for
usi
n
g both t
h
e dea
d
-reckoning
a
n
d ranges from
beacons
are
suggeste
d. The
s
e approac
h
es
are base
d on Bayes filtering
m
e
thod. They
usua
lly use particle filter [6-10] or
Kalm
an filter [11] m
e
thodology. Ge
ne
rally,
the m
e
thod ca
n fuse
data from
several exte
roce
ptive se
ns
ors a
n
d
in
tern
al m
o
tio
n in
form
atio
n
.
Also
, t
h
ey can
b
e
u
s
ed
fo
r SL
AM
(Si
m
ul
t
a
neou
s Local
i
zat
i
on a
n
d M
a
p
p
i
n
g)
[1
2
-
13]
. T
h
ey
ha
v
e
been
use
d
w
i
del
y
for l
o
cal
i
zat
i
on an
d SL
AM
of
gr
o
u
n
d
ro
bot
o
r
i
n
do
ors
ro
b
o
t
,
an
d
i
t
was
ex
tend
ed
t
o
und
erwater lo
cali
zatio
n
[14
]
. It is g
e
n
e
rally k
n
o
w
n
th
at th
e
particle filter p
r
o
d
u
ces m
o
re precise
an
d rob
u
s
t
estimatio
n
th
an
t
h
e Kalm
an
filter
wh
ile it
requires m
o
re ex
ten
s
iv
e calcu
latio
n
s
.
In cases
wh
ere
co
m
p
u
t
atio
n
ti
me is critical, Kalm
an
filter ap
pro
ach
is m
o
re feasib
le t
h
an
p
a
rticle filter
[1
5
]
.
Thi
s
pa
per
de
vel
o
ps a
n
EK
F ba
sed
m
e
t
hod
f
o
r
l
o
cal
i
z
at
i
on
of
a
n
u
n
m
a
nned
u
n
d
e
r
wat
e
r
r
o
bot
.
Th
ou
g
h
t
h
e
pa
per a
d
opt
s E
K
F w
h
i
c
h i
s
pre
v
al
ent
a
p
p
r
oac
h
f
o
r e
s
t
i
m
a
t
i
o
n an
d
has
hu
n
d
re
ds
of
vari
a
n
t
s
[1
6]
,
t
h
e pape
r has
t
h
e fol
l
o
wi
n
g
cont
ri
b
u
t
i
ons
. It
deri
ves f
o
r
m
ul
at
i
ons fo
r appl
i
cat
i
o
n of
t
h
e EKF ap
pr
o
ach fo
r
localization of an underwater robot
and investigates the collective appli
cat
i
on an
d i
ndi
vi
d
u
al
appl
i
cat
i
on o
f
t
h
e
m
easurem
ent
up
dat
e
. A
p
p
l
i
cat
i
ons of E
K
F f
o
r
un
de
rwater localization are relativel
y few and deri
vation
of Jac
o
bi
an m
a
t
r
i
ces fo
r t
h
e i
m
pl
em
ent
a
t
i
on has n
o
t
bee
n
c
l
early revealed yet. Also, the
r
e has not bee
n
clear
d
i
stin
ctio
n b
e
t
w
een th
e co
llectiv
e app
licatio
n
an
d ind
i
v
i
dual ap
p
lication
o
f
th
e m
easu
r
emen
t u
p
d
a
te.
Th
is
p
a
p
e
r
d
e
riv
e
s and
app
lies Kalm
an
filter alg
o
rith
m
fo
r
u
n
d
e
rwater lo
calizatio
n
in
th
e sectio
n
2.
The dat
a
o
f
d
e
pt
h an
d ra
n
g
e
s fr
om
beacons are f
u
se
d t
oget
h
er
wi
t
h
t
h
e vel
o
ci
t
y
or
od
om
et
ry
i
n
form
at
i
o
n
wh
ich
is
ob
tain
ed in
tern
ally
fro
m
th
e robo
t m
o
tio
n
.
In
th
e sectio
n 3, t
h
e
p
r
op
o
s
ed
m
e
th
o
d
is sim
u
late
d
and
com
p
ared wi
t
h
l
east
sq
uares
m
e
t
hod
a
n
d de
ad-
r
eck
o
n
i
n
g. Sect
i
on 4
c
o
nc
l
udes
t
h
e pa
per
.
2.
LOCATION
ESTIMATION BY E
X
TE
NDED KAL
M
AN
FILTER (EKF)
The propose
d
m
e
thod follows conve
n
tional
approac
h
of
Kalm
an filtering
m
e
thod c
onsis
ting of two
recu
rsi
v
e st
ep
s
:
predi
c
t
i
o
n of
l
o
cat
i
on usi
n
g i
n
t
e
r
n
al
m
o
t
i
on i
n
fo
rm
ati
on an
d cor
r
ect
i
on
by
m
easur
em
ent
relativ
e to
ex
tern
al en
v
i
ronmen
t. Tab
l
e
1 d
e
p
i
cts
p
s
e
u
do code
of t
h
e
localiza
t
i
on m
e
t
h
o
d
. T
h
e
pr
oced
u
r
e
repeats at eve
r
y time step using th
e est
i
m
at
ion
resul
t
f
r
om
t
h
e pre
v
i
o
us t
i
m
e
st
ep. The pr
oce
d
u
r
e p
r
o
duce
s
two
esti
m
a
tio
n
s
: th
e lo
cati
o
n
X
t
and c
o
vari
ance
Σ
t
of the esti
m
a
ted
lo
catio
n
un
certain
ty. Al
o
ng wi
th
th
e
lo
catio
n
estim
a
tio
n
X
t-1
and covaria
n
ce estimation
Σ
t-1
at
tim
e t
-
1, t
h
e i
n
f
o
r
m
at
i
on on r
o
bo
t
m
o
t
i
on
u
t
wh
ich
is
f
e
d b
y
i
n
ter
n
al
sen
s
o
r
s su
ch as I
M
U or
od
ometer
sen
s
o
r
s
are
u
s
ed fo
r pred
ictio
n
of th
e ro
bo
t lo
cation
X
─
t
and
cova
riance
Σ
─
t
at
t
i
m
e
t
.
Thi
s
st
ep i
s
de
scri
b
e
d
on t
h
e l
i
n
e
1
of
t
h
e Ta
bl
e 1.
T
h
e p
r
e
d
i
c
t
e
d r
o
bot
l
o
cat
i
on
X
─
t
and
cova
riance
Σ
─
t
is
corrected at t
h
e line
2. T
h
e
c
o
rrection ste
p
uses
m
easurement
z
t
related
t
o
th
e land
m
a
rk
s, th
e
id
en
tificatio
n
o
f
t
h
e land
m
a
rk
c
t
, a
nd t
h
e d
a
t
a
on t
h
e l
a
n
d
m
a
rk
E
t
gi
ve
n be
f
o
re
han
d
.
The l
a
n
d
m
a
rk
dat
a
E
t
sp
ecifically refers to
t
h
e lo
catio
n
o
f
t
h
e land
m
a
rk
s. Detailed
deriv
a
tion
o
f
t
h
e two
step
s
o
f
p
r
ed
iction
and
co
rrectio
n
will b
e
d
e
scri
b
e
d
in
th
e fo
llowing
sectio
n
s
.
Tab
l
e 1
.
Pro
c
ed
ure for
EKF
lo
catio
n
estim
a
t
io
n
t
t
t
t
t
t
t
t
t
t-
t
t
t
t
t
t
t-
t
return
,
,
,
step
orrection
C
,
,
step
Prediction
)
,
,
,
,
,
EKF(
on
Localizati
,
.
3
)
,
(
,
2.
)
(
,
1.
1
1
1
1
X
E
c
z
X
X
X
X
E
c
z
X
u
u
The Figure
1 s
h
ows a sim
p
le
exam
ple of the esti
m
a
tio
n
resu
lt fo
r
robo
t lo
catio
n
and
covarian
ce. Th
e
ro
b
o
t
na
vi
gat
e
s t
h
r
o
u
g
h
pl
a
n
ar t
r
a
j
ect
ory
i
ndi
cat
ed
by
t
h
e b
o
l
d
l
i
n
e s
e
gm
ent
s
and
f
o
u
r
T
OA
(Ti
m
e of
arrival
)’s a
r
e used. Bi (i = 1,
2,
3, 4) re
pre
s
e
n
ts an
ac
oustic
beacon.
Arcs i
ndicate the ra
nge m
easurem
e
n
t data
of the
robot
from
the beac
o
n
s. Estim
ated
lo
catio
n
s
X
t
’s are m
a
rk
ed
t
o
g
e
t
h
er with ellip
se aro
und
th
e locatio
n
whic
h indicate
s
covaria
n
ce
Σ
t
of th
e estim
a
t
i
o
n error.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
RA I
S
SN
:
208
9-4
8
5
6
Filterin
g
Metho
d fo
r Lo
ca
tion
Estima
tion
o
f
an
Und
e
rw
a
t
er Robo
t (Na
k
Yo
ng
K
o
)
17
0
Fi
gu
re
1.
A
n
e
x
am
pl
e of l
o
ca
l
i
zat
i
on usi
n
g
EKF
2.
1.
Pr
ediction
Th
e
pred
iction step
up
d
a
tes t
h
e l
o
catio
n and
co
varian
ce
of th
e estim
a
t
ed
lo
cation
s
u
s
ing
the
v
e
lo
city
inform
ation of the robot. The ve
locity can be sensed using t
h
e
accelerom
eter, gyrosc
ope, and odometry
sens
ors
o
r
be
cal
cul
a
t
e
d f
r
o
m
t
h
e m
o
t
i
on com
m
a
nd t
o
t
h
e actu
a
t
o
r. Th
e
p
r
ed
ictio
n
o
f
t
h
e
robo
t locatio
n
is
d
e
scri
b
e
d as t
h
e state tran
sition
eq
u
a
tion
(1
).
t
rc
t
qs
t
rs
t
qc
t
t
rc
t
t
qs
t
p
t
c
wc
t
c
vs
t
us
t
c
ws
t
s
s
ws
t
c
vc
t
s
s
vs
t
s
uc
t
s
ws
t
c
s
wc
t
s
vc
t
c
s
vs
t
c
uc
ψ
z
y
x
g
t
t
t
t
t
t
t
t
t
sec
sec
,
1
1
1
1
1
1
1
X
x
u
(1
)
In (
1
),
u
t
= (
u
,
v
,
w
,
p
,
q
,
r
)
i
s
t
h
e vel
o
ci
t
y
of t
h
e r
o
bot
i
n
3
-
di
m
e
nsi
o
n
a
l
un
der
w
at
er
envi
ro
nm
ent
with res
p
ect to the body fixe
d fram
e.
X
t
= (
x, y, z,
ϕ
,
θ
,
ψ
) is th
e
p
o
s
ition
and
o
r
ien
t
atio
n
of th
e
robot w
ith
respect t
o
an
Earth
-fixe
d a
n
d
in
ertial co
ord
i
n
a
te fram
e.
u
t
and
X
t
are
represe
n
ted acc
ording to the c
o
mm
on
not
at
i
o
ns
fr
om
SN
AM
E(
Soci
et
y
of
Naval
Arc
h
i
t
ect
s an
d
M
a
ri
ne E
n
gi
neers
)
.
Δ
t is t
h
e tim
e d
i
fferen
c
e
bet
w
ee
n t
h
e t
w
o c
onsec
ut
i
v
e s
a
m
p
l
i
ng t
i
m
e t-1t
o t
.
T
h
e
pre
d
i
c
t
i
on
of
t
h
e c
ova
ri
ance i
s
su
bject
t
o
t
h
e e
q
uat
i
o
n
(2
).
T
t
t
t
T
t
t
t
t
V
M
V
G
G
1
(2
)
In (2
),
G
t
a
nd
V
t
are the Jac
o
bian
of t
h
e
g
(
u
t
,
X
t-1
) with
res
p
ect to the
state
X
t-1
and
u
t
re
s
p
ectively.
M
t
is the error covaria
n
ce of t
h
e velocity
u
t
. The f
o
l
l
o
wi
ng
equat
i
o
ns sh
o
w
h
o
w t
h
e Ja
cobi
a
n
G
t
and
V
t
are
d
e
ri
v
e
d. In
th
e d
e
ri
v
a
tio
n, for n
o
t
ation
a
l simp
licity,
th
e subscrip
ts t-1
represen
ting
th
e ti
me in
d
e
x
i
n
ϕ
t-1
,
θ
t-1
,
and
ψ
t-1
are
d
e
leted
.
Th
e
G
t
is
d
e
ri
v
e
d as th
e fo
llo
wi
n
g
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
3, N
o
. 3,
Se
pt
em
ber 20
1
4
:
16
8 – 18
3
1
71
1
sec
sec
sec
sec
0
0
0
0
1
0
0
0
0
sec
sec
1
0
0
0
0
1
0
0
(3,1)
(2,1)
(1,1)
0
1
0
(3,1)
(2,1)
(1,1)
0
0
1
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
,
2
2
t
2,
t
2,
t
2,
t
1,
t
1,
t
1,
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
,
1
1
1
t
t
rc
t
t
qs
t
rs
t
qc
t
rc
t
qs
t
rc
t
qs
t
t
rs
t
t
qc
t
s
wc
t
s
vs
t
uc
t
c
ws
t
c
vc
G
G
G
G
G
G
ψ
ψ
ψ
ψ
ψ
ψ
z
z
z
z
z
z
y
y
y
y
y
y
x
x
x
x
x
x
g
G
t
t
t
z
t
y
t
x
t
t
t
t
z
t
y
t
x
t
t
t
t
z
t
y
t
x
t
t
t
t
z
t
y
t
x
t
t
t
t
z
t
y
t
x
t
t
t
t
z
t
y
t
x
t
t
t
t
t
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
x
X
u
(3
)
The
G
1,t
an
d
G
2,t
are as t
h
e
fol
l
owings
.
t
s
ws
t
c
s
wc
t
s
vc
t
c
s
vs
t
c
uc
t
s
c
wc
t
s
c
vs
t
s
us
t
c
wc
t
s
s
ws
t
c
vs
t
s
s
vc
G
t
c
ws
t
s
s
wc
t
c
vc
t
s
s
vs
t
s
uc
t
c
c
wc
t
c
c
vs
t
c
us
t
wcs
t
c
s
ws
t
s
vs
t
c
s
vc
G
t
2,
t
1,
(4
)
The Jac
o
bian
V
t
wh
ich
asso
ciates th
e lo
catio
n at ti
m
e
t to
th
e
v
e
lo
city
u
t
i
s
de
ri
ve
d as
t
h
e f
o
l
l
o
wi
ng
.
t
c
t
s
t
s
t
c
t
t
c
t
t
s
t
t
c
c
t
c
s
t
s
t
c
s
t
s
s
c
t
c
c
t
s
s
s
t
s
c
t
s
s
t
c
s
c
t
s
c
t
c
s
s
t
c
c
ψ
ψ
ψ
ψ
ψ
ψ
z
z
z
z
z
z
y
y
y
y
y
y
x
x
x
x
x
x
g
V
r
t,
q
t,
p
t,
w
t,
v
t,
u
t,
r
t,
q
t,
p
t,
w
t,
v
t,
u
t,
r
t,
q
t,
p
t,
w
t,
v
t,
u
t,
r
t,
q
t,
p
t,
w
t,
v
t,
u
t,
r
t,
q
t,
p
t,
w
t,
v
t,
u
t,
r
t,
q
t,
p
t,
w
t,
v
t,
u
t,
t
t
t
t
sec
sec
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
'
,
1
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
u
X
(5
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
RA I
S
SN
:
208
9-4
8
5
6
Filterin
g
Metho
d fo
r Lo
ca
tion
Estima
tion
o
f
an
Und
e
rw
a
t
er Robo
t (Na
k
Yo
ng
K
o
)
17
2
The e
r
ror cova
riance
M
t
o
f
th
e v
e
lo
city
u
t
i
s
assum
e
d t
o
be
di
ag
onal
f
o
r t
h
e c
o
m
put
at
i
onal
c
o
n
v
e
n
i
e
nce.
It
im
plies that the linear and a
n
gula
r
vel
o
city in each dir
ecti
o
n ha
s no c
o
rrelation wit
h
the
othe
r com
p
one
n
ts of
th
e v
e
l
o
city.
)
1
,
6
(
P
0
0
0
0
0
0
)
1
,
5
(
P
0
0
0
0
0
0
)
1
,
4
(
P
0
0
0
0
0
0
)
1
,
3
(
P
0
0
0
0
0
0
)
1
,
2
(
P
0
0
0
0
0
0
M
t
t
t
t
t
t
(1,1)
P
t
(6
)
whe
r
e,
2
rs
rr
rq
rp
rw
rv
ru
2
qs
qr
qq
qp
qw
qv
qu
2
ps
pr
pq
pp
pw
pv
pu
2
ws
wr
vq
wp
ww
wv
wu
2
vs
vr
vq
vp
vw
vv
vu
2
us
ur
uq
up
uw
uv
uu
t
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
P
In
t
h
e equ
a
tion
(6
), th
e
p
a
ra
m
e
ter
α
v
1
v
2
relates th
e v
e
lo
city
v
2
to
th
e u
n
certain
ty of t
h
e v
e
l
o
city
v
1
. The
param
e
ter
α
v
1s
ad
dresses
th
e un
certain
ty o
f
velo
city
v
1
wh
en
th
e rob
o
t
stays still.
Tab
l
e 2
sh
ows th
e alg
o
r
ith
m
for pred
iction
o
f
th
e
robo
t
location and error c
ova
riance
. I
t
co
rr
espon
ds to
th
e
line 1 of the T
a
ble 1. Lines 3 to 5 calculate
the Jacobia
n
G
t
wh
ich
proj
ect
s th
e esti
m
a
ted
ro
bo
t lo
catio
n at
t-1
to
th
e a p
r
iori lo
catio
n
at ti
me t. Lin
e
6
cal
cu
lates th
e Jaco
b
i
an
V
t
wh
ich
m
a
p
s
th
e v
e
lo
city
u
t
to
th
e a
p
r
iori
l
o
cat
i
on at
t
i
m
e t
.
Li
nes
7
an
d
8
pr
ovi
des t
h
e e
r
r
o
r
co
vari
ance
M
t
o
f
th
e v
e
lo
city
u
t
.
Li
ne
9 t
r
a
n
s
f
o
r
m
s
t
h
e
lin
ear v
e
l
o
city an
d
ang
u
l
ar
velo
city rep
r
esen
ted
with
res
p
ect to the body fixed
fram
e
to those
re
pres
ente
d
with
resp
ect t
o
th
e
Earth-fi
x
e
d and
in
ertial co
ord
i
n
a
te
fram
e
.
T
E
1
and
T
E
2
a
r
e th
e E
u
le
r
tr
a
n
sfor
ma
t
i
o
n
matrices relati
n
g
th
e
bo
d
y
fix
e
d fram
e v
e
lo
city to
Eart
h
-
fix
e
d
fram
e v
e
lo
city.
T
E
1
is fo
r t
r
an
sfo
r
m
a
ti
o
n
of
lin
ear v
e
lo
city
an
d
T
E
2
is
for an
gu
lar v
e
l
o
city.
c
c
c
s
s
c
s
s
s
c
c
c
s
s
s
s
c
s
s
c
s
c
s
c
c
s
s
c
c
E
1
T
(7
)
sec
cos
sec
sin
0
sin
cos
0
tan
cos
tan
sin
1
T
2
E
(8
)
Fi
nal
l
y
, l
i
n
es
1
0
a
n
d
1
1
y
i
el
d
a p
r
i
o
ri
e
s
t
i
m
a
ti
on
o
f
r
o
bot
l
o
cat
i
on a
n
d
er
r
o
r c
ova
ri
ance
at
t
i
m
e
t
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
3, N
o
. 3,
Se
pt
em
ber 20
1
4
:
16
8 – 18
3
1
73
Tab
l
e 2
.
Pred
ictio
n
o
f
a p
r
iori robo
t
lo
cation
an
d
erro
r c
o
variance from
pre
v
ious estim
ates
at tim
e
t-1.
t
t
T
t
t
t
T
t
t
t
t
T
t
t
T
E
T
T
E
T
t
t
t
t
t
t
t
rs
rr
rq
rp
rw
rv
ru
qs
qr
qq
qp
qw
qv
qu
ps
pr
pq
pp
pw
pv
pu
ws
wr
vq
wp
ww
wv
wu
vs
vr
vq
vp
vw
vv
vu
us
ur
uq
up
uw
uv
uu
t
t
t
t
t
t
t
t
t
t
t
r
t,
q
t,
p
t,
w
t,
v
t,
u
t,
,
t
,
t
,
t
t
t
t
,
return
:
V
M
V
G
G
:
Δ
t
z
y
x
r
q
p
T
w
v
u
T
z
y
x
:
P
P
P
P
P
P
M
:
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
r
q
p
w
v
u
P
t
c
t
s
t
s
t
c
t
t
c
t
t
s
t
t
c
c
t
t
s
t
c
s
t
s
s
c
t
c
c
t
s
s
s
t
s
c
t
s
s
t
c
s
c
t
s
c
t
c
s
s
t
c
c
V
:
t
t
rc
t
t
qs
t
rs
t
qc
t
rc
t
qs
t
rc
t
qs
t
t
rs
t
t
qc
t
s
wc
t
s
vs
t
uc
t
c
ws
t
c
vc
G
G
G
G
G
G
G
:
t
s
ws
t
c
s
wc
t
s
vc
t
c
s
vs
t
c
uc
t
s
c
wc
t
s
c
vs
t
s
us
t
c
wc
t
s
s
ws
t
c
vs
t
s
s
vc
G
t
c
ws
t
s
s
wc
t
c
vc
t
s
s
vs
t
s
uc
t
c
c
wc
t
c
c
vs
t
c
us
t
wcs
t
c
s
ws
t
s
vs
t
c
s
vc
G
r
,
q
,
p
,
w
,
v
,
u
:
step
Prediction
X
X
X
X
X
X
X
12
11
:
10
)
,
,
(
,
,
,
)
,
,
(
,
,
9
)
1
,
6
(
0
0
0
0
0
0
)
1
,
5
(
0
0
0
0
0
0
)
1
,
4
(
0
0
0
0
0
0
)
1
,
3
(
0
0
0
0
0
0
)
1
,
2
(
0
0
0
0
0
0
)
1
,
1
(
8
.
7
sec
sec
0
0
0
0
0
0
0
0
0
0
0
0
0
0
cos
sin
0
0
0
0
0
0
6
1
sec
sec
sec
sec
0
0
0
0
1
0
0
0
0
sec
sec
1
0
0
0
0
1
0
0
)
1
,
3
(
2
)
1
,
2
(
2
)
1
,
1
(
2
0
1
0
)
1
,
3
(
1
)
1
,
2
(
1
)
1
,
1
(
1
0
0
1
5
2
:
4
1
:
3
:
2
,
,
1
)
,
,
(
1
1
2
1
2
2
2
2
2
2
2
2
1
1
1
1
1
u
u
u
u
u
u
u
2.
2.
Corr
ec
tion of the a
Pri
o
ri
E
s
timates
The c
o
rrection stage
whic
h is
also called the m
easur
e
m
en
t up
d
a
te correct
s th
e a
prio
ri esti
m
a
tes o
f
the robot locati
o
n and e
r
ror covaria
n
ce.
Wh
il
e th
e pred
ictio
n
stag
e u
s
es only th
e in
tern
al i
n
fo
rm
atio
n
of
ro
bo
t
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
RA I
S
SN
:
208
9-4
8
5
6
Filterin
g
Metho
d fo
r Lo
ca
tion
Estima
tion
o
f
an
Und
e
rw
a
t
er Robo
t (Na
k
Yo
ng
K
o
)
17
4
velocity and
previ
ous estim
a
t
es, the correct
ion stage
us
es
th
e m
easu
r
e
m
e
n
t in
fo
rm
atio
n
relativ
e to
ex
t
e
rn
al
envi
ronm
ent to adjust t
h
e a
priori estim
a
t
es. In
our a
p
p
licatio
n th
e ex
tern
al env
i
ron
m
en
t refers t
o
th
e acou
s
tic
beacons
. After
the beac
ons e
m
it acoustic signal, t
h
e
hydr
ophone[17] at the
robot receive
s the acoustic s
i
gnals
and cal
cul
a
t
e
s
t
h
e di
st
ance bet
w
ee
n t
h
e h
y
d
r
o
p
h
ones a
n
d t
h
e r
o
b
o
t
us
i
ng t
h
e T
O
A
(
t
i
m
e
of arri
val
)
of t
h
e
acoustic signals.
Also, the
me
thod
uses
de
pth of t
h
e
robot from
the
surface which is
detected by a
pressure
sens
or.
T
h
e
fol
l
owi
n
g e
q
uat
i
o
ns a
r
e
use
d
f
o
r
cor
r
ect
i
o
n
st
ag
e.
t
t
t
t
t
t
t
t
t
t
T
t
t
t
t
T
t
t
t
t
)
H
K
(I
)
z
(z
K
S
H
K
Q
H
H
S
ˆ
1
X
X
(9
)
Th
e m
a
trix
H
t
is th
e Jacob
i
an
wh
ich relates th
e rob
o
t
lo
catio
n
to
t
h
e measu
r
em
en
t.
Q
t
is th
e error
cova
riance of the
m
easure
m
ent
process
.
T
h
e proce
ss calculates the Ka
lm
an gain
K
t
and
uses i
t
fo
r t
h
e
correction
of the a
priori estimate
X
─
t
to
X
t
, a
n
d
Σ
─
t
to
Σ
t
.
We
appl
y
t
h
e c
o
r
r
e
c
t
i
on st
e
p
i
n
t
w
o
way
s
:
a
p
pl
y
i
ng
the proce
d
ure
for each m
eas
urem
ent indivi
dually in se
quence and appl
ying it for all the
m
easurements at
o
n
ce co
llectiv
ely. Th
e two
app
licatio
n
approaches are e
x
plained in t
h
e follo
wing section and they are
tested
in
th
e
sim
u
lat
i
o
n
s
.
2.2.1. De
aling with
Range Data
I
ndividual
ly:
Correcting the
Predicti
on
using
Only One Data at a
Time
The
pre
d
icted l
o
cation can be
correct
ed e
v
e
r
y tim
e
a
m
easurem
ent data is available.
A
da
ta of ra
nge
fr
om
a beaco
n
o
r
t
h
e
dept
h
dat
a
by
t
h
e
p
r
essur
e
se
ns
or
can
be
use
d
f
o
r
co
rrect
i
o
n.
M
easurem
ent
m
odel
h
TOA
(·
) fo
r cas
e of ra
nge
fr
o
m
a beacon and the m
odel
h
Depth
(·) f
o
r the
case of de
pth
are desc
ribe
d by
the
follo
win
g
fo
rm
ulas.
s
,
j
2
z
,
t
z
,
j
2
y
,
t
y
,
j
2
x
,
t
x
,
j
i
TOA
,
t
i
TOA
,
t
t
i
TOA
,
t
)
(
)
(
)
(
s
r
,
j
,
h
z
E
X
E
X
E
X
E
E
X
(1
0)
z
,
t
t
Depth
,
t
h
z
X
X
(1
1
)
z
i
t,
TOA
is th
e
d
a
ta related
t
o
t
h
e
i
-th beac
on
whe
r
e
r
i
t,
TOA
is the
distance
from
the
i
-t
h
be
acon
t
o
t
h
e
r
o
bot
an
d
s
i
t,
TOA
is the signature fo
r the
measurem
ent. (
E
i
,
x
,
E
i
,
y
,
E
i
,
z
) i
s
t
h
e c
o
o
r
di
nat
e
of
t
h
e
i
-t
h be
acon
an
d (
X
─
t,
x
,
X
─
t,
y
,
X
─
t,
z
) is th
e lo
cati
o
n
o
f
th
e ro
bot at
ti
me t.
Z
t,
D
e
pth
i
s
t
h
e dept
h dat
a
. The Ja
cobi
a
n
s f
o
r t
h
e
m
easurem
ent
m
odel
neede
d
fo
r ap
pl
i
cat
i
on o
f
E
K
F are
de
ri
ve
d f
r
om
t
h
e eq
uat
i
o
n
s
(
1
0
)
a
nd
(1
1
)
.
H
i
t,
TOA
and
H
t,
Depth
in
th
e
eq
u
a
tion
s
(1
2)
an
d (13
)
corresp
ond
to th
e li
nearizatio
n
of
h
TOA
(·) a
n
d
h
Depth
(·
), res
p
ectively
.
2
z
,
t
z
,
j
2
y
,
t
y
,
j
2
x
,
t
x
,
j
y
,
t
z
,
j
y
,
t
y
,
j
x
,
t
x
,
j
,
t
i
t
,
t
i
t
,
t
i
t
z
,
t
i
t
y
,
t
i
t
x
,
t
i
t
,
t
i
t
,
t
i
t
,
t
i
t
z
,
t
i
t
y
,
t
i
t
x
,
t
i
t
t
t
i
t
q
0
0
0
0
0
0
0
0
0
q
q
q
s
s
s
s
s
s
r
r
r
r
r
r
,
j
,
h
H
)
(
)
(
)
(
X
E
X
E
X
E
X
E
X
E
X
E
X
X
X
X
X
X
X
X
X
X
X
X
x
E
X
(1
2)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
3, N
o
. 3,
Se
pt
em
ber 20
1
4
:
16
8 – 18
3
1
75
0
0
0
1
0
0
h
H
,
t
z
,
t
,
t
z
,
t
,
t
z
,
t
z
,
t
z
,
t
y
,
t
z
,
t
x
,
t
z
,
t
t
t
Depth
,
t
X
X
X
X
X
X
X
X
X
X
X
X
x
X
(1
3)
Table 3 and 4 show the correction proce
dure for the
m
e
a
s
urem
ent of di
stance from
a
beacon and for the
m
easurem
ent
of
dept
h res
p
e
c
t
i
v
el
y
.
Li
ne 6
of t
h
e Ta
bl
e
3 use
s
t
h
e eq
u
a
t
i
on (
1
2), a
n
d
l
i
n
e 3 o
f
t
h
e
Tabl
e 4
doe
s t
h
e
eq
uat
i
o
n
(
1
3).
T
h
ey
f
o
l
l
o
w
t
h
e
us
ual
EKF
p
r
oced
u
r
e desc
ri
be
d i
n
t
h
e e
quat
i
o
n
(
9
)
.
In t
h
e Ta
ble
3,
z
i
t
= (
r
i
t
,
s
i
t
) re
fers
to t
h
e
distance
r
i
t
fro
m
th
e
i
-th
beacon and t
h
e
signat
u
re
s
i
t
of
t
h
e
measurem
ent. In the
Table
4,
z
t
rep
r
ese
n
t
s
de
pt
h
dat
a
. It
i
s
n
o
t
a
bl
e t
h
at
i
n
c
a
se of c
o
r
r
ect
i
on
by
ra
nge
da
t
a
, t
h
e
lo
catio
n
E
i
of
th
e
i
-th
beacon
is neede
d
as well as the range data
z
i
t
fr
om
t
h
e beac
on
E
i
.
These Ta
bles return
th
e fi
n
a
l estimatio
n
o
f
th
e
ro
bo
t lo
cation
and th
e erro
r cov
a
rian
ce
Σ
t
o
f
th
e lo
catio
n
esti
m
a
tio
n
.
Tabl
e 3. Pr
oce
d
u
r
e fo
r
t
h
e
c
o
rrect
i
o
n usi
n
g
t
h
e ran
g
e dat
a
z
i
t
from
a beacon
E
i
TOA
,
t
TOA
,
t
t
TOA
,
t
t
TOA
,
t
t
i
t
i
t
t
i
TOA
,
t
i
TOA
,
t
i
t
t
t
1
i
t
T
i
t
t
i
t
t
T
i
t
t
i
t
i
t
z
,
t
z
,
j
y
,
t
y
,
j
x
,
t
x
,
j
i
t
s
,
j
i
TOA
,
t
2
z
,
t
z
,
j
2
y
,
t
y
,
j
2
x
,
t
x
,
j
i
t
T
i
t
i
t
i
TOA
,
t
2
s
2
r
t
t
t
t
t
,
return
:
13
,
:
12
endfor
:
11
)
H
K
(I
:
10
)
z
ˆ
(z
K
:
9
S
H
K
:
8
Q
H
H
S
:
7
0
0
0
0
0
0
0
0
0
q
q
q
H
:
6
q
z
ˆ
:
5
)
(
)
(
)
(
q
:
4
c
j
:
3
do
s
r
z
TOA
tures of
served fea
for all ob
:
2
σ
0
0
σ
Q
:
1
)
,
,c
,
,
(
TOA
on
step
orrection
C
X
X
X
X
X
X
E
X
E
X
E
E
X
E
X
E
X
E
E
X
z
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
RA I
S
SN
:
208
9-4
8
5
6
Filterin
g
Metho
d fo
r Lo
ca
tion
Estima
tion
o
f
an
Und
e
rw
a
t
er Robo
t (Na
k
Yo
ng
K
o
)
17
6
Tabl
e 4. Pr
oce
d
u
r
e fo
r
t
h
e
c
o
rrect
i
o
n usi
n
g
t
h
e dept
h
m
easurem
ent
.
t
t
t
t
t
t
t
t
t
t
t
1
t
T
t
t
t
t
T
t
t
t
t
t
z
,
t
t
2
d
t
t
t
t
,
return
:
8
)
H
K
(I
:
7
)
z
ˆ
(z
K
:
6
S
H
K
:
5
Q
H
H
S
:
4
0
0
0
1
0
0
H
:
3
z
ˆ
:
2
σ
Q
:
1
)
,
,
(
depth
on
step
orrection
C
X
X
X
X
X
z
2.2.2. De
aling
with Range
Data Collecti
v
ely: Correc
ti
ng the
Predic
tion using all
the
Range Data from
Every
Beac
ons
and Depth Data Collecti
v
el
y
All th
e
m
easu
r
e
m
en
t d
a
ta can
b
e
u
s
ed
co
llectiv
ely fo
r th
e co
rrectio
n
o
f
the p
r
ed
icted
esti
m
a
t
i
o
n
of
the location and error c
ova
ria
n
ce at a tim
e. I
t
is assu
m
e
d that there are n range
data
r
i
t,
TO
A
(
i=
1,…
,
n
) fr
om
n
beacons a
nd
one data of de
pt
h
d
t,
Depth
. Each
range data
r
t,
TOA
com
e
s
t
oget
h
er wi
t
h
one m
o
re
dat
a
of si
g
n
at
u
r
e
s
i
t,
TOA
. So the obs
erved m
easurem
ent data is
z
t
=(
r
1
t,
TOA
,
s
1
t,
TOA
, …,
r
n
t,
TOA
,
s
n
t,
TOA
,
d
t,
Depth
). The m
easure
m
ent
m
odel
i
s
descri
bed
as t
h
e e
qua
t
i
on
(1
4)
.
z
,
t
s
,
i
2
z
,
t
z
,
i
2
y
,
t
y
,
i
2
x
,
t
x
,
i
s
,
1
2
z
,
t
z
,
1
2
y
,
t
y
,
1
2
x
,
t
x
,
1
T
Depth
,
t
i
TOA
,
t
i
TOA
,
t
1
TOA
,
t
1
TOA
,
t
t
t
)
(
)
(
)
(
)
(
)
(
)
(
d
s
r
s
r
,
h
z
X
E
X
E
X
E
X
E
E
X
E
X
E
X
E
E
X
(1
4)
From
the m
eas
urem
ent equation
(14
)
, th
e
Jaco
b
i
an
m
a
trix
H
t
is d
e
riv
e
d
as th
e
fo
llowing.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
3, N
o
. 3,
Se
pt
em
ber 20
1
4
:
16
8 – 18
3
1
77
2
z
,
t
z
,
i
2
y
,
t
y
,
i
2
x
,
t
x
,
i
i
i
y
,
t
z
,
n
i
y
,
t
y
,
n
i
x
,
t
x
,
n
1
y
,
t
z
,
1
1
y
,
t
y
,
1
1
x
,
t
x
,
1
,
t
z
,
t
,
t
z
,
t
,
t
z
,
t
z
,
t
z
,
t
y
,
t
z
,
t
x
,
t
z
,
t
,
t
i
t
,
t
i
t
,
t
i
t
z
,
t
i
t
y
,
t
i
t
x
,
t
i
t
,
t
i
t
,
t
i
t
,
t
i
t
z
,
t
i
t
y
,
t
i
t
x
,
t
i
t
,
t
1
t
,
t
1
t
,
t
1
t
z
,
t
1
t
y
,
t
1
t
x
,
t
1
t
,
t
1
t
,
t
1
t
,
t
1
t
z
,
t
1
t
y
,
t
1
t
x
,
t
1
t
t
t
t
q
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
q
q
q
0
0
0
0
0
0
0
0
0
q
q
q
s
s
s
s
s
s
r
r
r
r
r
r
s
s
s
s
s
s
r
r
r
r
r
r
,
h
H
)
(
)
(
)
(
X
E
X
E
X
E
X
E
X
E
X
E
X
E
X
E
X
E
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
X
x
E
X
(1
5)
Tabl
e
5
descri
bes t
h
e c
o
r
r
ect
i
o
n
st
ep
o
f
E
K
F
w
h
i
c
h
uses
all the m
easurements c
o
llectiv
ely. It re
quires al
l the
measurem
ent data
z
t
= (
r
1
t,
TOA
,
s
1
t,
TOA
, …,
r
n
t,
TOA
,
s
n
t,
TOA
,
d
t,
Depth
) and all the
beacon locations
E
= (
E
1,
x
,
E
1,
y
,
E
1,
z
,
…,
E
n
,
x
,
E
n
,
y
,
E
n
,
z
) co
rres
p
on
di
ng
to t
h
e
ran
g
e
s
r
1
t,
TOA
,
r
n
t,
TOA
. Line
5
uses
the linearization
deri
ved at the
equat
i
o
n (
1
5
)
.
Evaluation Warning : The document was created with Spire.PDF for Python.