I
AE
S
I
nte
rna
t
io
na
l J
o
urna
l o
f
Ro
bo
t
ics a
nd
Aut
o
m
a
t
io
n
(
I
J
RA)
Vo
l.
1
0
,
No
.
4
,
Dec
em
b
er
2
0
2
1
,
p
p
.
2
9
6
~
3
0
7
I
SS
N:
2722
-
2
5
8
6
,
DOI
: 1
0
.
1
1
5
9
1
/i
jr
a
.
v
10
i
4
.
pp
2
9
6
-
307
296
J
o
ur
na
l ho
m
ep
a
g
e
:
h
ttp
:
//ij
r
a
.
ia
esco
r
e.
co
m
Rea
ltime auto
no
mo
us na
v
ig
a
tion in V
-
R
ep bas
ed
st
a
tic
a
nd
dy
na
mic env
iro
nment using
E
K
F
-
SLAM
Um
m
e
H
a
ni,
L
ub
na
M
o
in
De
p
a
rtme
n
t
o
f
El
e
c
tri
c
a
l
E
n
g
i
n
e
e
rin
g
,
Na
ti
o
n
a
l
Un
i
v
e
rsity
o
f
S
c
ien
c
e
s &
Tec
h
n
o
lo
g
y
,
Kar
ac
h
i
,
P
a
k
i
sta
n
Art
icle
I
nfo
AB
S
T
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
Ap
r
1
8
,
2
0
2
1
R
ev
is
ed
Ma
y
2
6
,
2
0
2
1
Acc
ep
ted
J
u
l 2
3
,
2
0
2
1
Lo
c
a
li
z
a
ti
o
n
i
n
a
n
a
u
to
n
o
m
o
u
s
m
o
b
il
e
ro
b
o
t
a
ll
o
ws
it
to
o
p
e
ra
te
a
u
to
n
o
m
o
u
sly
i
n
a
n
u
n
k
n
o
w
n
a
n
d
u
n
p
re
d
icta
b
le
e
n
v
iro
n
m
e
n
t
wit
h
th
e
a
b
il
it
y
to
d
e
term
in
e
i
ts
p
o
siti
o
n
a
n
d
h
e
a
d
in
g
.
S
imu
lt
a
n
e
o
u
s
l
o
c
a
li
z
a
ti
o
n
a
n
d
m
a
p
p
in
g
(S
LA
M
)
a
re
in
tro
d
u
c
e
d
to
so
l
v
e
th
e
p
ro
b
lem
wh
e
r
e
n
o
p
ri
o
r
in
fo
rm
a
ti
o
n
a
b
o
u
t
t
h
e
e
n
v
ir
o
n
m
e
n
t
is
a
v
a
il
a
b
le
e
it
h
e
r
sta
ti
c
o
r
d
y
n
a
m
ic
to
a
c
h
iev
e
sta
n
d
a
rd
m
a
p
-
b
a
se
d
lo
c
a
li
z
a
ti
o
n
.
T
h
e
p
rima
ry
fo
c
u
s
o
f
t
h
i
s
re
se
a
r
c
h
is
a
u
to
n
o
m
o
u
s
m
o
b
il
e
r
o
b
o
t
n
a
v
ig
a
ti
o
n
u
sin
g
th
e
e
x
te
n
d
e
d
Ka
l
m
a
n
fil
ter
(
EKF
)
-
S
LAM
e
n
v
iro
n
m
e
n
t
m
o
d
e
li
n
g
tec
h
n
iq
u
e
w
h
ich
p
r
o
v
id
e
s
h
ig
h
e
r
a
c
c
u
ra
c
y
a
n
d
re
li
a
b
il
i
ty
in
m
o
b
il
e
ro
b
o
t
lo
c
a
li
z
a
ti
o
n
a
n
d
m
a
p
p
in
g
re
su
lt
s
.
In
th
is
p
a
p
e
r
,
EKF
-
S
LAM
p
e
rfo
rm
a
n
c
e
is
v
e
rifi
e
d
b
y
sim
u
latio
n
s
p
e
r
fo
rm
e
d
i
n
a
sta
ti
c
a
n
d
d
y
n
a
m
ic
e
n
v
iro
n
m
e
n
t
d
e
sig
n
e
d
i
n
V
-
REP
i.
e
.
,
3
D
Ro
b
o
t
sim
u
latio
n
e
n
v
ir
o
n
m
e
n
t.
I
n
t
h
i
s
wo
rk
S
LAM
p
ro
b
lem
o
f
tw
o
-
wh
e
e
led
d
iffere
n
ti
a
l
d
riv
e
r
o
b
o
t
i
.
e
.
,
P
io
n
e
e
r
3
-
DX
in
in
d
o
o
r
sta
ti
c
a
n
d
d
y
n
a
m
ic
e
n
v
iro
n
m
e
n
t
i
n
teg
ra
ted
wit
h
Las
e
r
ra
n
g
e
fin
d
e
r
i
.
e
.
,
H
o
k
u
y
o
U
RG
-
0
4
LX
-
UG
0
1
,
LIDAR
,
a
n
d
Ultras
o
n
ic
se
n
so
rs
is
so
lv
e
d
.
EKF
-
S
LAM
sc
rip
ts
a
re
d
e
v
e
lo
p
e
d
u
sin
g
M
ATLAB
th
a
t
i
s
li
n
k
e
d
to
V
-
REP
v
ia
r
e
m
o
te
AP
I
f
e
a
tu
re
t
o
e
v
a
lu
a
te
EKF
-
S
LA
M
p
e
rfo
rm
a
n
c
e
.
Th
e
re
a
c
h
e
d
re
su
lt
s
c
o
n
firm
th
e
EKF
-
S
LAM
is
a
re
li
a
b
le
a
p
p
ro
a
c
h
fo
r
r
eal
-
ti
m
e
a
u
to
n
o
m
o
u
s
n
a
v
ig
a
ti
o
n
fo
r
m
o
b
il
e
ro
b
o
ts i
n
c
o
m
p
a
riso
n
t
o
o
th
e
r
tec
h
n
i
q
u
e
s.
K
ey
w
o
r
d
s
:
E
KF
-
SLAM
Ob
s
tacle
av
o
id
an
ce
Path
esti
m
atio
n
Static a
n
d
d
y
n
am
ic
en
v
ir
o
n
m
en
t c
o
m
p
ar
is
o
n
V
-
R
E
P
T
h
is i
s
a
n
o
p
e
n
a
c
c
e
ss
a
rticle
u
n
d
e
r th
e
CC BY
-
SA
li
c
e
n
se
.
C
o
r
r
e
s
p
o
nd
ing
A
uth
o
r
:
Um
m
e
Han
i
Dep
ar
tm
en
t o
f
E
lectr
ical
E
n
g
i
n
ee
r
in
g
Natio
n
al
Un
iv
er
s
ity
o
f
Scien
c
es a
n
d
T
ec
h
n
o
lo
g
y
PNEC
R
o
ad
,
PNS
J
au
h
ar
Kar
s
az
Fais
al
C
an
to
n
m
en
t,
Kar
ac
h
i
,
Pak
is
tan
E
m
ail: u
m
m
e.
h
a
n
i2
0
1
7
@
p
n
ec
.
n
u
s
t.e
d
u
.
p
k
,
u
m
m
eh
a
n
i0
9
8
@
g
m
ail.
co
m
1.
I
NT
RO
D
UCT
I
O
N
Au
to
n
o
m
o
u
s
m
o
b
ile
r
o
b
o
ts
’
n
av
ig
atio
n
is
co
n
s
id
er
a
b
ly
a
m
atter
o
f
co
n
te
n
tio
n
f
or
th
e
last
two
d
ec
ad
es.
r
esear
ch
er
s
an
d
s
cien
tis
ts
ar
e
p
u
ttin
g
p
r
o
d
ig
io
u
s
ef
f
o
r
ts
to
f
in
d
r
eliab
le
s
o
lu
tio
n
with
th
e
h
elp
o
f
ar
tific
ial
in
tellig
en
ce
an
d
c
o
m
p
u
ter
v
is
io
n
.
T
h
e
ap
p
licatio
n
s
o
f
m
o
b
ile
r
o
b
o
ts
ar
e
p
r
o
f
icie
n
t
with
a
d
eg
r
ee
o
f
s
elf
-
s
u
f
f
icien
cy
an
d
th
e
cr
iter
ia
o
f
its
au
to
n
o
m
y
ar
e
s
e
n
s
in
g
th
e
e
n
v
ir
o
n
m
en
t,
s
elf
-
m
ain
ten
an
ce
,
tas
k
p
er
f
o
r
m
in
g
,
a
u
to
n
o
m
o
u
s
n
a
v
ig
atio
n
th
at
in
cl
u
d
es
in
d
o
o
r
an
d
o
u
td
o
o
r
n
av
ig
atio
n
wh
ile
in
clu
tter
ed
en
v
ir
o
n
m
en
ts
r
eliab
le
d
ata
ass
o
ciatio
n
is
im
p
o
r
tan
t
[
1
]
b
ec
a
u
s
e
wr
o
n
g
d
ata
ass
o
ciatio
n
ca
n
h
av
e
ca
tast
r
o
p
h
ic
co
n
s
eq
u
en
ce
s
.
C
u
r
r
en
t
s
im
u
ltan
eo
u
s
lo
ca
liz
atio
n
an
d
m
a
p
p
in
g
(
SLAM
)
ap
p
r
o
ac
h
es
in
clu
d
e
th
e
g
r
a
p
h
-
SLAM
m
eth
o
d
,
p
ar
ticle
f
ilter
in
g
m
et
h
o
d
(
PF
)
,
s
ca
n
m
atch
in
g
m
eth
o
d
(
SM)
,
an
d
th
e
ex
ten
d
ed
K
alm
an
f
ilter
m
eth
o
d
(
E
KF)
.
T
h
e
g
r
ap
h
-
SLAM
m
eth
o
d
tak
es
th
e
SLAM
p
r
o
b
le
m
as
an
o
p
tim
izatio
n
p
r
o
b
le
m
[
2
]
.
T
h
e
o
b
jectiv
e
f
u
n
ctio
n
an
d
co
n
s
tr
ain
ts
ar
e
d
ef
in
ed
,
a
n
d
it
is
s
o
lv
ed
u
s
in
g
th
e
m
ath
em
atica
l
p
r
o
g
r
am
m
in
g
m
eth
o
d
.
T
h
e
PF
m
eth
o
d
is
a
r
ec
u
r
s
iv
e
B
ay
esian
esti
m
ato
r
f
o
r
d
y
n
am
ic
B
ay
esian
n
etwo
r
k
s
th
at
a
p
p
r
o
x
i
m
ates
th
e
PDF
i.e
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
ea
ltime
a
u
to
n
o
m
o
u
s
n
a
vi
g
a
ti
o
n
in
V
-
R
ep
b
a
s
ed
s
ta
tic
a
n
d
d
yn
a
mic
en
viro
n
men
t
… (
Umm
e
Ha
n
i
)
297
p
r
o
b
a
b
ilit
y
d
e
n
s
ity
f
u
n
ctio
n
b
y
s
ea
r
ch
in
g
f
o
r
th
e
clu
s
ter
o
f
r
an
d
o
m
s
am
p
les,
u
s
es
th
e
s
am
p
le
m
ea
n
to
r
ep
lace
th
e
in
teg
r
al
o
p
er
atio
n
s
,
an
d
a
cq
u
ir
e
s
th
e
v
ar
ian
ce
d
is
tr
ib
u
t
i
o
n
o
f
th
e
s
tate
[
3
]
.
I
t
h
as
a
s
p
ec
ial
ad
v
an
tag
e
in
d
ea
lin
g
with
s
ta
tu
s
f
ilter
in
g
o
f
p
a
r
am
eter
esti
m
atio
n
o
f
n
o
n
-
Gau
s
s
ian
an
d
n
o
n
lin
ea
r
tim
e
-
v
ar
y
i
n
g
s
y
s
tem
s
wh
ile
th
e
m
ajo
r
d
r
awb
ac
k
is
in
h
ig
h
-
d
im
en
s
io
n
al
s
p
ac
es
s
am
p
lin
g
th
at
ca
n
b
e
in
e
f
f
icie
n
t.
T
h
e
SM
m
eth
o
d
m
in
im
izes
th
e
m
etr
ic
d
is
tan
ce
s
b
etwe
en
th
e
s
ca
n
ch
a
r
ac
ter
is
tics
o
r
o
r
i
g
in
al
d
ata,
an
d
alig
n
s
th
e
o
v
e
r
lap
o
f
t
h
e
s
ca
n
n
in
g
d
atasets
[
4
]
.
Th
e
SL
AM
p
r
o
ce
s
s
lead
s
to
b
etter
n
a
v
ig
atio
n
in
d
y
n
a
m
ic
en
v
ir
o
n
m
en
t
s
au
to
n
o
m
o
u
s
ly
.
Ho
wev
er
,
a
lack
o
f
p
r
o
p
r
io
c
ep
tiv
e
f
ee
d
b
ac
k
f
r
o
m
a
n
au
t
o
n
o
m
o
u
s
m
o
b
ile
r
o
b
o
t
m
ay
ca
u
s
e
tr
o
u
b
le
in
th
e
au
to
n
o
m
o
u
s
o
p
er
atio
n
o
f
t
h
e
r
o
b
o
t
in
u
n
p
r
ed
ictab
le
an
d
u
n
k
n
o
wn
en
v
ir
o
n
m
en
ts
.
Ab
o
v
e
m
en
tio
n
ed
is
s
u
es
ar
e
s
o
m
e
b
asic
p
r
o
b
lem
s
o
f
SLAM
.
No
w
a
d
ay
s
,
m
an
y
r
esea
r
ch
er
s
an
aly
ze
d
if
f
e
r
en
t
ap
p
r
o
ac
h
es
to
s
o
lv
i
n
g
p
r
o
b
lem
s
in
SLAM
an
d
it
is
an
aly
ze
d
th
at
th
e
ap
p
r
o
ac
h
b
a
s
ed
o
n
E
KF
am
o
n
g
all
ap
p
r
o
ac
h
es
ar
e
th
e
m
o
s
t
co
m
m
o
n
l
y
u
s
ed
to
s
o
lv
e
SLAM
p
r
o
b
lem
s
.
T
esli
ć
et
al
.
[
5
]
u
s
es
L
ab
Ma
te
TM
.
to
s
h
o
w
r
o
b
o
t
n
av
ig
atio
n
in
in
d
o
o
r
en
v
ir
o
n
m
e
n
t w
ith
in
teg
r
ated
u
s
e
o
f
laser
r
an
g
ef
i
n
d
er
s
en
s
o
r
.
E
KF
-
SLAM
is
o
n
e
o
f
th
e
p
r
o
b
ab
ilis
tic
ap
p
r
o
ac
h
es
to
co
n
tr
o
l
u
n
ce
r
tain
ties
o
f
an
y
m
o
b
ile
r
o
b
o
t
[
6
]
–
[
8
]
is
m
o
s
t
wid
ely
u
s
ed
i
n
m
o
b
ile
r
o
b
o
tics
,
esp
ec
ially
in
SLAM
.
Mo
r
e
o
v
er
,
th
e
SLAM
f
ilter
in
g
s
o
lu
tio
n
,
wh
ich
is
b
ased
o
n
th
e
E
KF
a
p
p
licatio
n
is
t
h
e
f
ir
s
t
s
u
cc
ess
f
u
lly
im
p
lem
e
n
ted
[
9
]
,
[
1
0
]
,
a
n
d
m
o
s
t
o
f
te
n
u
s
ed
o
n
lin
e
SLAM
alg
o
r
ith
m
.
T
h
er
e
ar
e
m
an
y
th
eo
r
etica
l
a
n
d
p
r
ac
tical
wo
r
k
s
d
ed
icate
d
t
o
E
KF
u
s
ag
e
with
d
if
f
er
en
t
a
p
p
r
o
ac
h
es
[
1
1
]
,
[
1
2
]
,
an
d
a
p
p
licatio
n
f
ield
s
[
1
3
]
,
[
1
4
]
.
I
n
t
h
e
au
t
o
n
o
m
o
u
s
r
o
b
o
t
n
a
v
ig
atio
n
,
th
e
E
KF
b
ased
SLAM
is
ca
teg
o
r
i
ze
d
as
n
o
n
-
lin
ea
r
SLAM
,
w
h
e
r
e
lin
ea
r
izatio
n
o
f
n
o
n
-
lin
ea
r
m
o
d
els
alo
n
g
with
a
s
u
m
m
atio
n
o
f
s
y
s
tem
n
o
is
e
with
Gau
s
s
ian
f
ilter
tak
es
p
lace
s
o
th
at
th
e
Kalm
an
f
ilter
alg
o
r
ith
m
ca
n
b
e
ap
p
lied
.
I
n
co
m
p
ar
is
o
n
with
t
h
e
Kalm
an
f
ilter
,
E
KF
-
SLA
M
r
ep
r
esen
ts
th
e
n
o
n
-
lin
ea
r
m
o
d
els
wh
ich
ar
e
a
n
ess
en
tial
p
ar
t
o
f
all
n
av
ig
atio
n
p
r
o
b
lem
s
alm
o
s
t.
E
KF
-
SLAM
esti
m
ates
m
o
b
ile
r
o
b
o
t
lo
c
atio
n
as
it
is
m
o
v
in
g
u
s
in
g
in
cr
em
en
tal
m
a
x
im
u
m
lik
elih
o
o
d
esti
m
ato
r
,
it
g
e
n
e
r
ates
th
e
m
ap
b
y
lo
ca
lizatio
n
an
d
g
en
e
r
ated
m
ap
in
f
o
r
m
atio
n
u
s
es to
u
p
d
ate
its
cu
r
r
en
t states
an
d
m
a
p
s
im
u
ltan
eo
u
s
ly
[
1
5
]
.
I
n
th
is
wo
r
k
,
th
e
E
KF
-
SLAM
alg
o
r
ith
m
h
as
b
ee
n
s
u
cc
ess
f
u
lly
im
p
lem
e
n
ted
u
s
in
g
a
p
i
o
n
ee
r
3
-
DX
i.e
.
,
a
s
m
all
lig
h
tweig
h
t
two
-
wh
ee
l
d
if
f
e
r
en
tial
d
r
iv
e
s
m
o
b
ile
r
o
b
o
t.
T
h
e
alg
o
r
ith
m
is
i
m
p
lem
en
ted
a
n
d
its
p
er
f
o
r
m
an
ce
is
an
al
y
ze
d
in
a
clu
s
ter
ed
en
v
ir
o
n
m
e
n
t
cr
ea
te
d
o
n
V
-
R
E
P.
L
id
ar
s
en
s
o
r
in
t
er
f
ac
ed
with
r
o
b
o
t
m
o
d
el
an
d
th
e
o
u
t
p
u
t
o
f
r
o
b
o
t
m
o
to
r
s
,
s
en
s
o
r
s
ar
e
th
e
n
in
te
g
r
ated
with
MA
T
L
AB
to
ap
p
l
y
th
e
al
g
o
r
ith
m
an
d
o
b
s
er
v
e
its
p
er
f
o
r
m
an
ce
in
a
s
tatic
an
d
d
y
n
am
ic
en
v
ir
o
n
m
en
t
.
2.
RE
S
E
ARCH
M
E
T
H
O
D
T
h
e
m
o
b
ile
r
o
b
o
t
m
o
d
el
p
r
ed
i
cts
th
e
cu
r
r
en
t
s
tates
b
ased
o
n
co
n
tr
o
l
in
p
u
t
an
d
p
r
e
v
io
u
s
s
tates
o
f
th
e
r
o
b
o
t
is
s
h
o
wn
in
Fig
u
r
e
1
.
Ma
th
em
atica
l
ly
,
th
e
p
r
o
ce
s
s
m
o
d
el
in
th
e
d
is
cr
ete
f
o
r
m
ca
n
b
e
wr
itten
as
(
1
)
.
T
h
e
r
o
b
o
t
p
o
s
itio
n
(
x
,
y
)
an
d
o
r
ie
n
tatio
n
an
g
le
i.e
.
,
Ө
esti
m
ated
th
e
r
o
b
o
t
s
tates
in
t
h
is
r
ese
ar
ch
wo
r
k
.
I
t
ca
n
b
e
s
h
o
wn
in
th
e
f
o
r
m
o
f
a
s
tate
v
ec
to
r
as
(
2
)
.
I
n
th
is
r
esear
c
h
wo
r
k
s
p
ee
d
an
d
a
n
g
u
lar
v
e
lo
city
is
ap
p
lied
a
s
co
n
tr
o
l in
p
u
t c
a
n
b
e
ex
h
ib
it a
s
in
(
3
)
.
Fig
u
r
e
1
.
R
o
b
o
t
m
o
d
el
(
+
1
)
=
(
(
)
,
(
)
)
(
1
)
=
[
x
y
θ
]
(
2
)
(
)
=
[
v
(
n
)
ω
(
n
)
]
(
3
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
ob
&
A
u
to
m
,
Vo
l
.
1
0
,
No
.
4
,
Dec
em
b
er
2
0
2
1
:
2
9
6
–
307
298
I
n
d
is
cr
ete
-
tim
e,
iter
ativ
ely
f
o
r
ev
er
y
n
th
s
am
p
le
,
th
e
p
r
e
d
ictiv
e
s
tate
o
f
th
e
r
o
b
o
t
is
p
r
ed
icted
in
s
am
p
le
tim
e
T
,
wh
ich
ca
p
t
u
r
e
in
tim
e
(
n
.
T
)
.
Sin
ce
th
e
c
o
n
tr
o
l
in
p
u
t
to
th
e
r
o
b
o
t
m
o
d
els
a
r
e
s
p
ee
d
an
d
a
n
g
u
lar
v
elo
city
t
h
at
is
m
ea
s
u
r
e
d
an
d
u
s
ed
to
p
r
ed
ict
r
o
b
o
t
s
tates
in
th
i
s
p
r
o
ce
s
s
.
T
h
e
d
is
cr
ete
-
tim
e
r
o
b
o
t
k
in
e
m
atics
m
o
d
el
f
o
r
th
e
ab
o
v
e
ca
s
e
is
ex
p
r
ess
ed
in
(
4
)
.
R
(
n
+
1
)
=
[
x
(
n
+
1
)
y
(
n
+
1
)
θ
(
n
+
1
)
]
[
x
(
n
)
y
(
n
)
θ
(
n
)
]
+
T
[
v
(
n
)
.
c
os
(
θ
(
n
)
)
v
(
n
)
.
s
in
(
θ
(
n
)
)
ω
(
n
)
]
(
4
)
R
o
b
o
t
m
o
d
el
J
ac
o
b
ian
ca
n
b
e
r
ep
r
esen
ted
in
(
5
)
.
R
(
n
+
1
)
|
n
=
R
(
n
)
|
n
+
T
[
v
(
n
)
.
c
os
(
R
(
3
)
(
n
)
|
n
)
v
(
n
)
.
s
in
(
R
(
3
)
(
n
)
|
n
)
ω
(
n
)
]
(
5
)
I
d
ea
lly
,
th
e
c
u
r
r
en
t
s
tates
o
f
th
e
r
o
b
o
t
ca
n
b
e
esti
m
ated
ac
cu
r
ately
b
y
u
s
in
g
th
e
ab
o
v
e
r
o
b
o
t
k
in
em
atics
m
o
d
el
b
u
t
i
n
p
r
ac
t
ic
e,
it
is
in
f
lu
en
ce
d
b
y
e
r
r
o
r
s
th
at
ca
n
b
e
n
o
is
e
in
th
e
m
ea
s
u
r
em
en
t
o
f
s
en
s
o
r
s
an
d
f
r
ictio
n
.
E
q
u
atio
n
(
1
)
with
th
e
ad
d
itio
n
o
f
th
e
n
o
is
e
m
o
d
el
ca
n
b
e
wr
itten
as
(
6
)
.
(
+
1
)
=
(
(
)
,
(
)
+
δ
u
(
n
)
)
=
(
(
)
,
(
)
)
+
δ
(
6
)
I
n
th
e
ab
o
v
e
ca
s
e,
th
e
n
o
is
e
m
o
d
el
is
esti
m
ated
as
Gau
s
s
ian
n
o
is
e
with
ze
r
o
m
ea
n
r
e
p
r
esen
ted
b
y
.
T
h
e
co
v
ar
ian
ce
o
f
th
is
s
y
s
tem
n
o
is
e
o
r
ig
in
ated
d
u
r
in
g
th
e
m
ea
s
u
r
em
en
t
p
r
o
ce
s
s
ca
n
b
e
ca
l
cu
lated
as
it
ca
n
b
e
s
ee
n
in
(
7
)
.
=
.
.
(
7
)
I
n
wh
ich
:
=
C
o
v
ar
ian
ce
o
f
Pro
ce
s
s
n
o
is
e
=
C
o
v
ar
ian
ce
o
f
C
o
n
tr
o
l i
n
p
u
t m
ea
s
u
r
e
m
e
n
t
=
(
,
)
T
h
e
p
r
o
ce
s
s
m
o
d
el
J
ac
o
b
ian
m
atr
ices
co
n
ce
r
n
in
g
th
e
c
o
n
tr
o
l
in
p
u
t
m
ea
s
u
r
em
e
n
t
an
d
r
o
b
o
t
s
tates
R
ca
n
b
e
ex
p
r
ess
ed
in
(
8
)
an
d
(
9
)
,
r
esp
ec
tiv
ely
.
F
u
=
δ
f
δ
(
x
,
y
,
θ
)
=
[
1
0
−
T
.
v
(
n
)
.
s
in
(
θ
(
n
)
)
0
1
T
.
v
(
n
)
.
c
os
(
θ
(
n
)
)
0
0
1
]
(
8
)
F
u
=
δ
f
δ
(
v
,
ω
)
=
[
T
.
c
os
(
R
(
3
)
(
n
)
)
0
T
.
s
in
(
R
(
3
)
(
n
)
)
0
0
T
]
(
9
)
2
.
1
.
SL
A
M
o
pera
t
io
n b
a
s
ed
o
n 2
D
-
E
K
F
T
h
e
SLAM
p
r
o
ce
s
s
f
lo
w
i
n
t
h
is
r
esear
ch
wo
r
k
ca
n
b
e
o
b
s
er
v
ed
i
n
Fig
u
r
e
2
.
I
n
c
o
n
tr
as
t
to
E
KF
lo
ca
lizatio
n
SLAM
o
p
er
atio
n
i
n
v
o
lv
es
th
e
in
itializatio
n
o
f
la
n
d
m
ar
k
s
to
u
p
d
ate
r
o
b
o
t p
o
s
iti
o
n
.
T
h
e
s
tates
d
u
r
in
g
th
e
p
r
o
ce
s
s
th
at
ar
e
esti
m
ated
co
n
s
is
t
o
f
esti
m
ated
lan
d
m
ar
k
s
an
d
r
o
b
o
t
s
tates.
Po
in
t
lan
d
m
ar
k
s
ar
e
u
s
ed
in
th
is
r
esear
ch
,
wh
ich
h
a
s
two
-
d
i
m
en
s
io
n
al
(
x
an
d
y
)
s
tates.
E
q
u
atio
n
(
1
0
)
ex
h
ib
its
th
e
SLAM
o
p
er
atio
n
b
a
s
ed
w
h
o
le
esti
m
ated
s
tates
v
ec
to
r
.
T
h
e
m
o
b
ile
r
o
b
o
t
s
tates
(
x
,
y
,
θ)
ar
e
co
r
r
elate
d
b
y
R
V
an
d
s
et
o
f
la
n
d
m
ar
k
s
tates
(
L
x
1
,
L
y
1
,
L
x
2
,
L
y
2
,
…,
L
x
n
,
L
y
n
)
ar
e
co
r
r
elate
d
b
y
R
1
,
wi
th
n
is
th
e
r
eg
is
ter
ed
lan
d
m
ar
k
n
u
m
b
er
.
Similar
to
th
e
lo
ca
lizatio
n
p
r
o
ce
s
s
,
in
wh
ich
E
KF
is
u
s
ed
t
o
esti
m
ate
th
e
s
tates
an
d
Gau
s
s
ian
v
ar
iab
les
in
clu
d
in
g
co
v
ar
ian
ce
m
atr
ix
P
an
d
m
ea
n
i.e
.
,
th
e
ex
p
ec
ted
v
alu
e
o
f
s
tate
v
ec
to
r
is
u
s
ed
to
m
o
d
el
all
th
e
p
r
o
ces
se
s
.
T
h
e
c
o
r
r
esp
o
n
d
en
t o
f
th
e
ex
p
ec
ted
v
alu
e
o
f
th
e
s
tate
v
ec
to
r
an
d
its
co
v
ar
ian
ce
m
at
r
ix
P
ca
n
b
e
s
h
o
wn
in
(
1
1
)
an
d
(
1
2
)
.
T
h
ese
two
m
atr
ices
ex
p
a
n
d
ev
er
y
tim
e
in
s
ize
with
th
e
in
itializatio
n
o
f
th
e
lan
d
m
ar
k
p
r
o
ce
s
s
,
wh
en
e
v
er
t
h
e
m
o
b
ile
r
o
b
o
t
d
etec
ts
a
n
ew
lan
d
m
ar
k
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
ea
ltime
a
u
to
n
o
m
o
u
s
n
a
vi
g
a
ti
o
n
in
V
-
R
ep
b
a
s
ed
s
ta
tic
a
n
d
d
yn
a
mic
en
viro
n
men
t
… (
Umm
e
Ha
n
i
)
299
Fig
u
r
e
2
.
2
D
E
KF
-
SLAM
o
p
e
r
atio
n
R
=
[
R
V
R
1
]
=
[
x
y
θ
L
x
1
L
y
1
⋮
L
x
n
L
y
n
]
(
1
0
)
R
=
[
R
V
̅
̅
̅
̅
R
1
̅
̅
̅
]
=
[
R
V
̅
̅
̅
̅
L
1
̅
̅
̅
.
.
L
1
̅
̅
̅
]
(
1
1
)
P
=
[
P
X
v
,
X
v
P
X
v
,
X
1
P
X
1
,
X
v
P
M
,
X
1
]
(
1
2
)
P
=
[
P
X
v
,
X
v
P
L
1
,
X
v
.
P
X
v
,
L
1
P
L
1
,
L
1
.
.
.
P
X
v
,
L
n
.
.
P
L
1
,
L
n
.
.
.
.
P
L
1
,
X
v
P
L
1
,
X
v
.
.
P
L
n
,
L
n
]
(
1
3
)
2
.
2
.
2
D
E
K
F
-
ba
s
ed
SL
AM
pro
ce
s
s
g
ener
a
l
o
pera
t
io
n
2
.
2
.
1
.
Ste
p 0
.
r
o
bo
t
i
nitia
liza
t
io
n
At
th
e
in
itial
tim
e
(
t=0
)
,
o
n
ly
th
e
r
o
b
o
t
s
tates
ar
e
in
itial
s
tat
es,
an
d
n
o
lan
d
m
a
r
k
is
r
e
g
is
ter
ed
to
th
e
m
ap
.
Alth
o
u
g
h
,
P
=
P0
ca
n
b
e
co
n
s
id
er
ed
as
th
e
co
v
a
r
ian
c
e
m
atr
ix
as
th
e
r
o
b
o
t
i
n
itial
s
tate
is
ass
u
m
ed
to
b
e
k
n
o
wn
s
o
t
h
at
th
er
e
is
n
o
u
n
ce
r
tain
ty
.
I
n
i
t
i
a
l
S
t
a
t
e
s &
C
o
v
a
r
i
a
n
c
e
No
U
p
d
a
t
e
d
st
a
t
e
s
a
n
d
c
o
v
a
r
i
a
n
c
e
P
re
dict
io
n
P
r
e
d
i
c
t
i
o
n
o
f
st
a
t
e
s
b
a
s
e
d
o
n
p
r
o
c
e
ss
mo
d
e
l
R
o
b
o
t
M
o
v
i
n
g
A
n
y
K
n
o
w
n
l
a
n
d
m
a
r
k
s
?
Upda
te
of
s
tate
s
a
nd
c
ova
r
ianc
e
ba
s
e
d
on
c
ur
r
e
nt
map
a
nd
obs
e
r
va
ti
on
Y
e
s
A
n
y
n
e
w
l
a
n
d
m
a
r
k
s
?
I
n
i
t
i
a
l
i
z
a
t
i
o
n
o
f
l
a
n
d
m
a
r
k
s
Y
e
s
No
Upda
t
ing
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
ob
&
A
u
to
m
,
Vo
l
.
1
0
,
No
.
4
,
Dec
em
b
er
2
0
2
1
:
2
9
6
–
307
300
R
̅
=
R
V
̅
̅
̅
̅
=
[
x
y
θ
]
=
[
0
0
0
]
P
=
[
0
0
0
0
0
0
0
0
0
]
(
1
4
)
2
.
2
.
2
.
Ste
p 1
.
pr
edict
io
n st
ep
a.
Upda
t
ing
r
o
bo
t
s
t
a
t
es
T
h
e
m
o
b
ile
r
o
b
o
t
m
o
v
em
en
t
a
f
f
ec
ts
th
e
r
o
b
o
t
s
tates
o
n
ly
an
d
b
ased
o
n
th
e
r
o
b
o
t
p
r
o
ce
s
s
m
o
d
el
a
n
d
its
m
o
v
em
en
t,
t
h
e
n
ew
esti
m
ated
r
o
b
o
t
s
tate
(
R
V
)
as
in
(
1
5
)
an
d
th
e
lan
d
m
a
r
k
s
tate
(
R
1
)
as
i
n
(
1
6
)
ca
n
b
e
p
r
ed
icted
.
R
V
̅
̅
̅
̅
←
f
R
v
(
R
V
̅
̅
̅
̅
,
u
,
N
̅
)
(
1
5
)
R
1
̅
̅
̅
←
R
1
̅
̅
̅
(
1
6
)
E
q
u
atio
n
(
1
5
)
c
o
r
r
elate
s
to
th
e
r
o
b
o
t
p
r
o
ce
s
s
m
o
d
el
(
4
)
b
as
ed
o
n
its
co
n
tr
o
l
i
n
p
u
t
(
u
)
,
p
r
e
v
io
u
s
s
tate
(
R
V
)
an
d
n
o
is
e
m
o
d
el
(
N)
wh
ile
N
̅
is
eq
u
al
to
ze
r
o
as th
e
n
o
is
e
in
th
is
p
r
o
ce
s
s
is
m
o
d
eled
in
a
s
wh
ite
n
o
is
e.
b.
Ro
bo
t
co
v
a
ria
nce
up
da
t
ing
T
h
e
u
p
d
ate
d
co
v
a
r
ian
ce
P
in
th
is
p
r
o
ce
s
s
b
ased
o
n
m
o
d
el
p
r
ed
ictio
n
is
ca
lcu
lated
as
in
(
1
7
)
,
wh
er
e
co
r
r
esp
o
n
d
in
g
to
th
e
p
r
o
ce
s
s
m
o
d
el
eq
u
atio
n
,
F
R
is
th
e
J
ac
o
b
i
an
o
f
th
e
s
tate
an
d
Pn
is
th
e
m
ea
s
u
r
em
en
t
in
p
u
t
co
n
tr
o
l n
o
is
e
co
v
ar
ian
ce
.
P
←
F
R
P
F
R
T
+
Pn
(
1
7
)
As
m
en
tio
n
ed
p
r
e
v
io
u
s
ly
,
s
ta
tes
o
f
th
e
r
o
b
o
t
(
R
̅
)
ar
e
o
n
ly
af
f
ec
ted
b
y
th
e
r
o
b
o
t
m
o
v
em
en
t
s
o
th
e
co
v
ar
ian
ce
m
atr
i
x
P
r
elate
d
to
th
e
r
o
b
o
t
s
tates
af
f
ec
ts
th
e
J
ac
o
b
ian
m
atr
ix
to
u
p
d
ate.
T
h
er
ef
o
r
e,
in
th
is
p
r
o
ce
s
s
,
th
e
J
ac
o
b
ia
n
m
atr
ix
is
c
alcu
lated
as
in
(
1
8
)
,
in
wh
ich
0
co
r
r
esp
o
n
d
s
to
ze
r
o
m
atr
ices
an
d
I
co
r
r
esp
o
n
d
to
th
e
id
en
tity
m
at
r
ix
.
F
x
=
[
d
f
R
dR
0
0
I
]
Pn
=
[
d
f
R
dN
0
]
(
1
8
)
2
.
2
.
3
.
Ste
p 2
.
la
nd
m
a
rk
-
ba
s
ed
o
bs
er
v
a
t
io
n upd
a
t
ing
pr
o
ce
s
s
T
h
e
m
o
b
ile
r
o
b
o
t
o
b
s
er
v
es
a
r
o
u
n
d
th
e
lan
d
m
ar
k
wh
ile
it
is
m
o
v
in
g
u
s
in
g
th
e
laser
s
en
s
o
r
th
at
m
ea
s
u
r
es
o
b
s
er
v
ab
le
lan
d
m
a
r
k
s
r
an
g
e
an
d
b
ea
r
in
g
r
elate
d
to
th
e
r
o
b
o
t
o
r
ie
n
tatio
n
a
n
d
p
o
s
itio
n
.
I
f
th
e
o
b
s
er
v
ed
lan
d
m
a
r
k
is
alr
ea
d
y
r
eg
is
ter
ed
to
th
e
m
ap
,
its
r
an
g
e
an
d
b
ea
r
i
n
g
m
ea
s
u
r
em
en
t
ar
e
u
s
ed
t
o
u
p
d
ate
th
e
s
tate
'
s
esti
m
at
io
n
(
R
̅
)
an
d
also
i
ts
co
v
ar
ian
ce
(
P).
T
h
e
m
ea
s
u
r
em
en
t
p
r
o
ce
s
s
an
d
its
co
r
r
esp
o
n
d
ed
co
v
a
r
ian
ce
m
o
d
eled
as
in
(
1
9
)
a
n
d
(
2
0
)
ar
e
in
d
ep
e
n
d
en
t
f
o
r
ea
ch
la
n
d
m
ar
k
(
i)
.
T
h
e
s
tate
u
p
d
atin
g
p
r
o
ce
s
s
b
ased
o
n
o
b
s
er
v
ed
la
n
d
m
ar
k
s
is
p
r
o
ce
s
s
ed
o
n
e
b
y
o
n
e
o
f
ea
ch
lan
d
m
ar
k
.
z
̅
=
y
i
−
h
i
(
R
V
,
L
i
)
(
1
9
)
H
R
=
[
H
R
v
0
…
0
H
L
i
0
…
0
]
(
2
0
)
H
R
v
=
δ
h
i
(
R
V
̅
̅
̅
̅
,
L
i
̅
)
δ
R
V
;
H
L
i
=
δ
h
i
(
R
V
̅
̅
̅
̅
,
L
i
̅
)
δ
L
i
(
2
1
)
B
ased
o
n
th
e
J
ac
o
b
ia
n
,
s
ee
(
20
)
an
d
ab
o
v
e
m
ea
s
u
r
em
en
t
m
o
d
el,
th
e
u
p
d
ate
d
s
tate
p
r
o
c
ess
an
d
its
u
p
d
ated
co
v
a
r
ian
ce
b
ased
o
n
a
s
et
o
f
(
2
2
)
,
(
2
3
)
,
(
2
4
)
,
(
2
5
)
,
a
n
d
(
2
6
)
ar
e
ca
lcu
lated
,
i
n
wh
ich
K
in
th
is
u
p
d
atin
g
p
r
o
ce
s
s
is
Kalm
an
g
a
in
.
z
̅
=
y
i
−
h
i
(
R
V
,
L
i
)
(
2
2
)
Z
=
H
X
P
H
X
T
+
R
(
2
3
)
K
=
P
H
X
T
Z
−
1
(
2
4
)
R
̅
←
R
̅
−
K
z
̅
(
2
5
)
P
←
P
−
KZ
K
T
(
2
6
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
ea
ltime
a
u
to
n
o
m
o
u
s
n
a
vi
g
a
ti
o
n
in
V
-
R
ep
b
a
s
ed
s
ta
tic
a
n
d
d
yn
a
mic
en
viro
n
men
t
… (
Umm
e
Ha
n
i
)
301
2
.
2
.
4
.
Ste
p 3
.
I
nitia
liza
t
io
n o
f
la
nd
m
a
r
k
s
W
h
en
th
e
lan
d
m
ar
k
s
f
o
r
th
e
f
ir
s
t
tim
e
ar
e
o
b
s
er
v
ed
b
y
th
e
r
o
b
o
ts
h
av
e
n
o
t
r
eg
is
ter
ed
o
n
th
e
m
ap
.
W
h
er
ea
s
,
b
ased
o
n
its
r
a
n
g
e
a
n
d
b
ea
r
in
g
m
ea
s
u
r
em
en
t
t
h
e
s
tate
o
f
th
is
n
ew
la
n
d
m
ar
k
is
esti
m
ated
in
clu
d
in
g
x
an
d
y
g
l
o
b
al
co
o
r
d
in
ate
c
o
r
r
es
p
o
n
d
in
g
to
th
e
r
o
b
o
t
s
tate
R
.
T
h
e
n
ew
lan
d
m
a
r
k
esti
m
ated
s
tate’
s
f
u
n
ctio
n
ca
n
b
e
ex
h
i
b
it
as
(
2
7
)
ar
e
ca
lcu
la
ted
as
th
e
in
v
er
t
o
f
o
b
s
er
v
ati
o
n
f
u
n
ctio
n
(
h
i
(
R
,
L
i
)
)
wh
ile
th
e
n
ew
lan
d
m
ar
k
s
tates c
o
r
r
esp
o
n
d
in
g
J
ac
o
b
ian
,
th
e
in
v
er
s
e
o
b
s
er
v
atio
n
f
u
n
cti
o
n
an
d
th
e
r
o
b
o
t states
ar
e
wr
i
tten
as
(
2
8
)
.
L
̅
n
+
1
=
g
(
X
̅
V
,
y
̅
n
+
1
)
(
2
7
)
G
X
v
=
∂
g
(
X
̅
V
,
y
̅
n
+
1
)
∂
X
V
;
G
L
i
+
1
=
∂
g
(
X
̅
V
,
y
̅
n
+
1
)
∂
L
i
+
1
(
2
8
)
T
h
e
n
ew
lan
d
m
a
r
k
co
v
ar
ian
ce
an
d
cr
o
s
s
-
co
v
a
r
ian
ce
ar
e
ca
lc
u
lated
b
ased
o
n
(
2
9
)
an
d
(
3
0
)
r
elate
d
to
th
e
p
r
io
r
s
tates.
P
LL
=
G
X
v
P
LL
G
X
v
T
+
G
y
n
+
1
X
v
G
y
n
+
1
T
(
2
9
)
P
LX
=
G
X
v
P
LL
P
X
v
X
1
(
3
0
)
B
ased
o
n
th
e
esti
m
atio
n
r
esu
lt
,
th
ese
n
ew
lan
d
m
ar
k
s
tates a
n
d
th
eir
co
v
ar
ian
ce
ar
e
th
e
n
s
u
m
m
ed
in
to
th
e
r
o
b
o
t f
u
ll st
ate,
m
ap
,
a
n
d
c
o
v
ar
ian
ce
as in
(
3
1
)
.
X
̅
←
[
X
̅
L
n
+
1
]
P
←
[
P
P
LX
T
P
LX
P
LL
]
(
3
1
)
3.
RE
SU
L
T
S
A
ND
D
I
SCU
SS
I
O
N
T
o
an
aly
ze
th
e
p
e
r
f
o
r
m
an
ce
o
f
E
KF
-
SLAM
alg
o
r
ith
m
f
o
r
au
to
n
o
m
o
u
s
m
o
b
ile
r
o
b
o
t
n
av
ig
atio
n
r
ea
lizatio
n
,
in
d
o
o
r
en
v
ir
o
n
m
e
n
t
with
s
tatic
an
d
d
y
n
am
ic
o
b
jects
was
d
es
ig
n
ed
o
n
V
-
R
E
P
s
im
u
lato
r
th
at
was
co
n
n
ec
ted
with
MA
T
L
AB
v
ia
r
em
o
te
API
f
ea
t
u
r
es
wh
ile
two
-
wh
ee
led
d
if
f
er
e
n
tial
d
r
iv
e
r
o
b
o
t
i.e
.
,
Pio
n
ee
r
3
-
DX
in
teg
r
ated
with
laser
r
a
n
g
e
f
in
d
er
i.e
.
,
Ho
k
u
y
o
UR
G
-
0
4
L
X
-
UG0
1
to
d
eter
m
in
e
its
p
er
f
o
r
m
an
ce
ef
f
ec
tiv
en
ess
with
th
e
in
teg
r
a
tio
n
o
f
L
I
DAR
an
d
u
ltra
s
o
n
i
c
s
en
s
o
r
s
.
E
KF
-
SLAM
alg
o
r
ith
m
is
d
ev
elo
p
ed
u
s
in
g
MA
T
L
AB
th
at
i
s
lin
k
ed
to
V
-
R
E
P
v
ia
r
em
o
te
API
f
ea
tu
r
e.
T
o
ev
alu
ate
E
KF
-
SLAM
p
er
f
o
r
m
an
ce
.
Fig
u
r
e
3
s
h
o
ws
an
e
n
v
ir
o
n
m
e
n
t
with
s
tatic
o
b
jects
d
esig
n
e
d
o
n
V
-
R
E
P
an
d
Fig
u
r
e
4
s
h
o
ws
an
e
n
v
ir
o
n
m
en
t
with
d
y
n
am
ic
o
b
jects d
esig
n
e
d
o
n
V
-
R
E
P
.
Fig
u
r
e
3
.
T
h
e
e
n
v
ir
o
n
m
e
n
t w
ith
s
tatic
o
b
jects
d
esig
n
ed
o
n
V
-
R
E
P
Fig
u
r
e
4
.
T
h
e
e
n
v
ir
o
n
m
e
n
t w
ith
d
y
n
a
m
ic
o
b
jects
d
esig
n
ed
o
n
V
-
R
E
P
T
h
e
s
im
u
latio
n
was
p
e
r
f
o
r
m
e
d
o
n
V
-
R
E
P
s
im
u
lato
r
s
ettin
g
s
i.e
.
,
s
im
u
latio
n
tim
e
s
tep
d
t
=
5
0
m
s
,
d
y
n
am
ics
en
g
in
e
=
b
u
llet
2
.
7
8
,
d
y
n
am
ic
s
ettin
g
s
=
ac
cu
r
ate
.
W
h
en
th
e
s
im
u
latio
n
b
eg
in
s
m
o
b
ile
r
o
b
o
t
s
tar
ts
s
ca
n
n
in
g
with
th
e
2
D
laser
r
an
g
e
f
in
d
er
“Ho
k
u
y
o
UR
G
-
0
4
L
X
-
UG0
1
”
th
at
allo
ws
a
wid
e
s
ca
n
n
in
g
r
an
g
e
o
f
5
6
0
0
m
m
×2
4
0
°
f
o
r
m
ea
s
u
r
em
en
t
o
f
t
h
e
lan
d
m
ar
k
s
in
th
e
i
n
d
o
o
r
en
v
ir
o
n
m
en
t
th
at
i
n
teg
r
ated
in
to
t
h
e
r
o
b
o
t
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
ob
&
A
u
to
m
,
Vo
l
.
1
0
,
No
.
4
,
Dec
em
b
er
2
0
2
1
:
2
9
6
–
307
302
an
d
av
o
i
d
o
b
s
tacle
s
with
th
e
h
elp
o
f
E
KF
-
SLAM
alg
o
r
ith
m
.
I
n
an
ex
p
er
im
en
t,
th
e
e
x
p
lo
r
a
tio
n
is
s
im
u
lated
in
a
r
o
o
m
b
o
u
n
d
ed
b
y
2
4
0
c
m
h
ig
h
walls,
8
0
cm
walls
in
t
h
e
ce
n
ter
,
ch
air
,
tab
le,
cu
p
b
o
ar
d
,
a
n
d
r
ac
k
b
y
a
Pio
n
ee
r
3
-
DX
r
o
b
o
t
eq
u
i
p
p
ed
with
a
m
id
-
n
o
is
e
an
d
o
d
o
m
e
tr
y
2
D
r
an
g
e
s
ca
n
n
er
i.e
.
,
Ho
k
u
y
o
UR
G
-
0
4
L
X
-
UG0
1
.
Gau
s
s
ian
n
o
is
e
d
is
tr
ib
u
ted
in
p
o
lar
co
o
r
d
i
n
ates.
I
ter
ativ
e
clo
s
et
p
o
in
t
(I
C
P)
alg
o
r
ith
m
is
u
s
ed
f
o
r
d
ata
ass
o
ciatio
n
[
1
6
]
i.e
.
,
p
er
f
o
r
m
ed
at
ea
ch
s
tep
an
d
ex
p
lo
r
atio
n
p
er
f
o
r
m
e
d
with
B
r
ain
ten
b
er
g
,
L
y
ap
u
n
o
v
,
Z
ig
Z
ag
,
an
d
c
o
r
n
er
in
g
alg
o
r
it
h
m
s
[
1
7
]
wh
ich
m
ax
im
ize
p
e
r
f
o
r
m
an
ce
.
I
t
h
as
b
ee
n
o
b
s
er
v
e
d
th
at
u
n
ce
r
tain
ty
is
th
e
cr
itical
m
ea
s
u
r
em
en
t
p
ar
am
et
er
o
f
p
er
f
o
r
m
an
ce
d
eg
r
a
d
atio
n
wh
ich
af
f
ec
ts
m
ea
s
u
r
em
en
t
m
o
d
els
an
d
m
o
tio
n
d
u
e
to
its
d
ir
ec
t
in
f
lu
en
ce
co
n
s
is
ten
cy
.
Fig
u
r
e
5
s
h
o
ws
s
ca
n
n
in
g
with
s
tatic
o
b
j
ec
ts
o
n
th
e
V
-
R
E
P
s
im
u
lato
r
an
d
Fig
u
r
e
6
s
h
o
ws
s
ca
n
n
in
g
with
d
y
n
am
ic
o
b
jects o
n
th
e
V
-
R
E
P si
m
u
lato
r
.
Fig
u
r
e
5
.
Scan
n
in
g
with
s
tatic
o
b
jects o
n
V
-
R
E
P
s
im
u
lato
r
Fig
u
r
e
6
.
Scan
n
in
g
with
d
y
n
a
m
ic
o
b
jects o
n
V
-
R
E
P
s
im
u
lato
r
Gr
ap
h
ical
u
s
er
in
ter
f
ac
e
(
G
UI
)
d
esig
n
ed
o
n
MA
T
L
AB
wh
ich
was
in
ter
co
n
n
ec
ted
with
V
-
R
E
P
s
im
u
lato
r
.
As
th
e
s
im
u
latio
n
s
tar
ts
,
r
o
b
o
t
m
o
tio
n
n
av
ig
atio
n
is
tr
ac
k
ed
o
n
th
e
V
-
R
E
P
s
im
u
lato
r
an
d
MA
T
L
AB
GUI
s
im
u
ltan
eo
u
s
ly
.
T
h
e
lan
d
m
ar
k
s
ar
e
p
o
in
ted
with
a
r
e
d
cr
o
s
s
wh
ile
th
e
r
o
b
o
t
was
r
e
p
r
esen
ted
with
a
s
m
all
tr
ian
g
le.
R
u
n
tim
e
s
et
at
3
0
0
0
s
.
Fig
u
r
e
7
s
h
o
ws
MA
T
L
AB
GUI
f
o
r
r
o
b
o
t
n
av
ig
atio
n
.
Fig
u
r
e
7
.
MA
T
L
AB
GUI
f
o
r
r
o
b
o
t n
a
v
ig
atio
n
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
ea
ltime
a
u
to
n
o
m
o
u
s
n
a
vi
g
a
ti
o
n
in
V
-
R
ep
b
a
s
ed
s
ta
tic
a
n
d
d
yn
a
mic
en
viro
n
men
t
… (
Umm
e
Ha
n
i
)
303
As
th
e
r
o
b
o
t
m
o
v
es
it
f
in
d
o
b
s
er
v
ed
la
n
d
m
ar
k
s
,
av
o
id
s
o
b
s
tacle
s
,
u
p
d
ate
s
s
tates
an
d
c
o
v
ar
ian
ce
m
atr
ices
b
ased
o
n
th
e
cu
r
r
en
t
m
ap
an
d
o
b
s
er
v
atio
n
.
I
f
a
n
y
n
ew
lan
d
m
ar
k
s
ar
e
f
o
u
n
d
,
it
in
itializes
u
p
d
ated
s
tates a
n
d
co
v
ar
ian
ce
.
W
h
ile
t
h
e
r
o
b
o
t m
o
tio
n
-
tr
ac
k
e
d
o
n
a
g
r
id
m
ap
,
as sh
o
wn
in
Fig
u
r
e
8
an
d
Fig
u
r
e
9
.
Gr
id
m
ap
s
ar
e
in
tr
o
d
u
ce
d
in
1
9
8
5
b
y
M
o
r
av
ec
a
n
d
E
lf
,
th
at
r
ep
r
esen
ts
en
v
ir
o
n
m
e
n
t
b
y
a
g
r
id
,
ass
u
m
in
g
r
o
b
o
t
p
o
s
itio
n
is
k
n
o
wn
a
n
d
o
cc
u
p
a
n
cy
o
f
an
in
d
iv
id
u
al
ce
ll
is
in
d
e
p
en
d
e
n
t
s
to
r
es
th
e
p
o
s
ter
io
r
p
r
o
b
a
b
ilit
y
th
at
a
lo
ca
tio
n
o
r
co
r
r
esp
o
n
d
in
g
ar
ea
is
o
cc
u
p
ied
b
y
an
o
b
s
tacle
,
th
e
lar
g
er
v
alu
e
r
ep
r
esen
ts
o
b
s
tacle
m
ar
k
ed
b
y
b
lack
co
lo
r
an
d
th
e
s
m
aller
v
alu
e
r
e
p
r
ese
n
ts
f
r
ee
s
p
ac
e
m
ar
k
e
d
b
y
wh
it
e
co
lo
r
.
Acc
u
r
ate
m
ap
p
in
g
ca
n
b
e
ac
h
iev
ed
b
y
co
m
b
in
in
g
lo
ts
o
f
d
a
ta
wh
ile
ea
ch
ce
ll
in
th
e
g
r
id
r
ep
r
esen
ts
a
b
it
o
f
th
e
r
o
b
o
t’
s
en
v
ir
o
n
m
en
t
in
d
icat
e
s
o
m
e
m
ea
s
u
r
e
o
f
“
o
b
s
tacle
Ness
”
in
ea
ch
g
r
id
ce
ll b
ased
o
n
laser
s
en
s
o
r
r
ea
d
in
g
s
wh
ile
th
e
alg
o
r
ith
m
o
p
er
ates
if
th
e
s
en
s
o
r
d
ata
h
as
b
ee
n
o
b
tain
ed
d
ir
ec
tly
f
r
o
m
a
laser
s
ca
n
n
er
an
d
u
s
in
g
o
n
ly
th
e
o
d
o
m
etr
y
[
1
8
]
-
[
2
0
]
.
Fig
u
r
e
8
.
Gr
id
m
ap
with
s
tatic
o
b
jects
Fig
u
r
e
9
.
Gr
id
m
ap
with
d
y
n
a
m
ic
o
b
jects
I
n
th
e
r
o
b
o
t
n
av
ig
atio
n
v
iew,
g
r
ee
n
lin
es
r
ep
r
esen
t
l
id
ar
s
en
s
o
r
ex
p
lo
r
atio
n
,
r
ed
an
d
b
lu
e
tr
ian
g
les
in
d
icate
a
m
o
b
ile
r
o
b
o
t,
b
lu
e
c
ir
cles
r
ep
r
esen
t
la
n
d
m
ar
k
s
an
d
th
e
R
ed
C
r
o
s
s
in
d
icate
s
esti
m
ated
lan
d
m
ar
k
s
,
as
s
h
o
wn
in
Fig
u
r
e
1
0
a
n
d
Fig
u
r
e
1
1
.
Fig
u
r
e
10
.
Nav
ig
atio
n
v
iew
with
s
tatic
o
b
jects
Fig
u
r
e
11
.
Nav
ig
atio
n
v
iew
with
d
y
n
a
m
ic
o
b
jects
T
h
e
p
e
r
f
o
r
m
an
ce
o
f
th
e
p
r
o
p
o
s
ed
E
KF
-
SLAM
alg
o
r
ith
m
in
th
e
in
d
o
o
r
en
v
ir
o
n
m
e
n
t
with
s
tatic
an
d
d
y
n
am
ic
o
b
jects
was
o
b
s
er
v
e
d
an
d
co
m
p
a
r
ed
in
ter
m
s
o
f
th
e
s
tan
d
ar
d
d
ev
iatio
n
o
f
th
e
v
e
h
icle
h
ea
d
in
g
an
d
it
was
o
b
s
er
v
ed
th
at
th
e
u
n
ce
r
tai
n
ty
in
p
o
s
itio
n
o
v
er
tim
e
is
b
et
ter
th
an
u
s
u
al
SLAM
r
esu
lts
ex
h
ib
it
in
Fig
u
r
e
1
2
an
d
Fig
u
r
e
1
3
.
I
t
was
o
b
s
er
v
ed
th
at
th
e
u
n
ce
r
tain
ty
in
p
o
s
itio
n
o
v
er
tim
e
was
o
b
s
er
v
ed
b
y
p
lo
ttin
g
th
e
s
tan
d
ar
d
d
e
v
iatio
n
o
f
p
o
s
itio
n
(
m
)
o
n
th
e
y
-
ax
is
an
d
tim
e
(
s
)
o
n
th
e
x
-
ax
is
,
as sh
o
wn
in
T
ab
le
1
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
ob
&
A
u
to
m
,
Vo
l
.
1
0
,
No
.
4
,
Dec
em
b
er
2
0
2
1
:
2
9
6
–
307
304
Fig
u
r
e
12
.
Un
ce
r
tain
ty
i
n
p
o
s
i
tio
n
o
v
er
tim
e
with
s
tatic
o
b
jects
Fig
u
r
e
13
.
Un
ce
r
tain
ty
i
n
p
o
s
i
tio
n
o
v
er
tim
e
with
d
y
n
am
ic
o
b
jects
T
ab
le
1
.
Simu
lated
u
n
ce
r
tain
t
y
in
p
o
s
itio
n
o
v
er
tim
e
with
s
tatic
an
d
d
y
n
am
ic
o
b
jects
U
n
c
e
r
t
a
i
n
t
y
i
n
p
o
s
i
t
i
o
n
o
v
e
r
t
i
me
W
i
t
h
s
t
a
t
i
c
o
b
j
e
c
t
s
W
i
t
h
d
y
n
a
mi
c
o
b
j
e
c
t
s
P
o
si
t
i
o
n
(
m)
Ti
me
(
s)
P
o
si
t
i
o
n
(
m)
Ti
me
(
s)
M
a
x
i
m
u
m
st
a
n
d
a
r
d
d
e
v
i
a
t
i
o
n
i
n
x
0
.
0
2
7
8
6
1
2
0
.
0
2
4
5
5
6
1
5
M
i
n
i
m
u
m
st
a
n
d
a
r
d
d
e
v
i
a
t
i
o
n
i
n
x
0
.
0
1
7
5
2
5
5
0
.
0
1
6
9
3
2
4
1
0
M
a
x
i
m
u
m
st
a
n
d
a
r
d
d
e
v
i
a
t
i
o
n
i
n
y
0
.
0
2
5
8
4
2
5
0
.
0
2
1
4
5
77
M
i
n
i
m
u
m
st
a
n
d
a
r
d
d
e
v
i
a
t
i
o
n
i
n
y
0
.
0
1
6
3
2
9
1
7
0
.
0
0
9
8
7
2
4
1
2
E
KF
-
SLAM
p
er
f
o
r
m
an
ce
a
n
d
ac
cu
r
ac
y
ca
n
also
b
e
esti
m
ated
b
y
p
lo
ttin
g
er
r
o
r
s
in
p
o
s
itio
n
o
v
er
tim
e.
Un
ce
r
tain
ty
in
r
o
b
o
t
n
a
v
ig
atio
n
ca
n
b
e
co
m
p
u
ted
b
y
co
n
s
id
er
in
g
n
o
is
e
an
d
J
ac
o
b
i
an
s
.
Fig
u
r
e
1
4
an
d
Fig
u
r
e
1
5
r
ep
r
esen
t
th
e
e
r
r
o
r
in
p
o
s
itio
n
o
v
e
r
tim
e
in
th
e
d
esig
n
ed
in
d
o
o
r
en
v
ir
o
n
m
en
t
with
s
tatic
an
d
d
y
n
am
ic
o
b
jects
r
esp
ec
tiv
el
y
.
I
t
was
o
b
s
er
v
ed
at
t
u
r
n
in
g
p
o
s
itio
n
s
in
s
id
e
th
e
r
o
o
m
th
e
r
e
ar
e
s
p
i
k
es
af
ter
a
ce
r
tain
in
s
tan
t
o
f
tim
e
an
d
m
o
r
e
s
p
ik
es
wer
e
o
b
s
er
v
ed
in
s
im
u
latio
n
with
d
y
n
am
i
c
o
b
jects
at
a
ce
r
tain
in
s
tan
t
o
f
tim
e.
I
n
o
u
r
c
o
n
tr
o
lled
s
im
u
latio
n
en
v
i
r
o
n
m
e
n
t,
we
h
a
v
e
co
m
p
ar
ed
th
e
p
er
f
o
r
m
an
ce
o
f
th
e
m
ap
jo
i
n
in
g
a
n
d
th
e
co
n
s
is
ten
cy
o
f
th
e
m
ap
p
in
g
alg
o
r
ith
m
wh
ile
f
o
r
s
o
m
e
tr
a
jecto
r
y
p
o
in
ts
d
is
cr
ep
an
cies
ar
e
o
b
s
er
v
ed
f
u
r
th
est
f
r
o
m
th
e
in
itial lo
ca
tio
n
o
f
th
e
v
eh
icle.
W
h
en
m
o
b
ile
r
o
b
o
t
h
ea
d
in
g
p
o
s
itio
n
is
esti
m
ated
b
y
o
d
o
m
e
tr
y
,
Fig
u
r
e
1
6
an
d
Fig
u
r
e
1
7
r
ep
r
esen
ts
s
ca
n
er
r
o
r
o
v
er
tim
e
b
y
p
lo
tti
n
g
p
o
s
itio
n
er
r
o
r
(
m
)
an
d
an
g
u
lar
er
r
o
r
(
r
a
d
)
o
n
th
e
y
-
a
x
is
v
er
s
u
s
tim
e
o
n
th
e
x
-
ax
is
,
wh
ile
o
d
o
m
etr
y
er
r
o
r
o
v
er
tim
e
r
ep
r
esen
ts
in
Fig
u
r
e
1
8
an
d
Fig
u
r
e
1
9
.
T
h
e
m
o
b
ile
r
o
b
o
t
co
r
r
ec
ts
its
p
o
s
itio
n
b
y
u
p
d
atin
g
lan
d
m
ar
k
s
.
Simu
latio
n
with
s
tatic
o
b
ject
s
an
d
d
y
n
am
ic
o
b
jects
r
esu
lt
s
ar
e
s
u
m
m
ar
ized
in
T
a
b
le
2
.
I
t
ca
n
b
e
an
aly
ze
d
t
h
at
f
ewe
r
s
ca
n
an
d
o
d
o
m
etr
y
er
r
o
r
s
i
n
3
0
0
0
s
ec
o
n
d
s
s
im
u
latio
n
tim
e
o
b
s
e
r
v
ed
with
d
y
n
am
ic
o
b
jects
as
co
m
p
ar
ed
to
s
tat
ic
o
b
jects,
alth
o
u
g
h
it
an
aly
ze
d
th
at
b
o
th
s
im
u
latio
n
s
p
er
f
o
r
m
ed
ac
cu
r
at
e
n
av
ig
atio
n
u
s
in
g
E
KF
-
SLAM
alg
o
r
ith
m
.
Fig
u
r
e
14
.
E
r
r
o
r
in
p
o
s
itio
n
o
v
er
tim
e
with
s
tatic
Fig
u
r
e
15
.
E
KF
-
SLAM
-
er
r
o
r
i
n
p
o
s
itio
n
o
v
er
tim
e
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
ea
ltime
a
u
to
n
o
m
o
u
s
n
a
vi
g
a
ti
o
n
in
V
-
R
ep
b
a
s
ed
s
ta
tic
a
n
d
d
yn
a
mic
en
viro
n
men
t
… (
Umm
e
Ha
n
i
)
305
o
b
jects
with
d
y
n
am
ic
o
b
jects
Fig
u
r
e
16
.
Scan
er
r
o
r
s
o
v
er
tim
e
with
s
tatic
o
b
jects
Fig
u
r
e
17
.
Scan
er
r
o
r
s
o
v
er
tim
e
with
d
y
n
a
m
ic
o
b
jects
Fig
u
r
e
18
.
Od
o
m
etr
y
er
r
o
r
o
v
er
tim
e
with
s
tatic
o
b
jects
Fig
u
r
e
19
.
Od
o
m
etr
y
er
r
o
r
o
v
er
tim
e
with
d
y
n
a
m
ic
o
b
jects
T
ab
le
2
.
Simu
latio
n
r
esu
lts
o
f
E
KF
-
SLAM
alg
o
r
ith
m
W
i
t
h
s
t
a
t
i
c
o
b
j
e
c
t
s
W
i
t
h
d
y
n
a
mi
c
o
b
j
e
c
t
s
P
o
si
t
i
o
n
e
r
r
o
r
A
n
g
u
l
a
r
e
r
r
o
r
P
o
si
t
i
o
n
e
r
r
o
r
A
n
g
u
l
a
r
e
r
r
o
r
M
a
x
i
m
u
m
sca
n
e
r
r
o
r
s
o
v
e
r
t
i
me
0
.
1
3
4
m
a
t
8
5
7
se
c
0
.
0
0
0
2
2
8
1
r
a
d
a
t
1
0
1
7
s
e
c
0
.
0
8
0
3
4
m
a
t
2
9
4
4
s
e
c
0
.
0
0
1
6
2
1
4
r
a
d
a
t
2
7
5
7
s
e
c
M
i
n
i
m
u
m
sca
n
e
r
r
o
r
s
o
v
e
r
t
i
me
0
.
0
5
6
m
a
t
1
9
0
7
se
c
0
.
0
9
3
9
r
a
d
a
t
1
7
6
9
s
e
c
0
.
0
0
4
4
4
7
8
m
a
t
2
9
7
2
se
c
0
.
0
0
4
4
7
8
r
a
d
a
t
2
9
7
2
se
c
M
a
x
i
m
u
m
o
d
o
m
e
t
r
y
e
r
r
o
r
s
o
v
e
r
t
i
m
e
0
.
1
3
3
1
7
m
a
t
1
0
8
4
s
e
c
0
.
0
9
0
0
2
8
r
a
d
a
t
8
4
3
s
e
c
0
.
1
3
4
1
2
m
a
t
8
5
7
se
c
0
.
0
9
3
9
1
7
r
a
d
a
t
1
7
6
9
se
c
M
i
n
i
m
u
m
o
d
o
me
t
r
y
e
r
r
o
r
s
o
v
e
r
t
i
m
e
0
.
0
5
5
9
1
m
a
t
8
2
8
se
c
0
.
0
0
0
1
5
8
r
a
d
a
t
2
6
3
3
se
c
0
.
0
5
6
0
9
m
a
t
1
9
0
7
s
e
c
0
.
0
0
0
4
7
7
r
a
d
a
t
1
1
9
7
se
c
4.
CO
NCLU
SI
O
N
T
h
is
r
esear
ch
r
ep
r
esen
ts
E
KF
-
SLAM
alg
o
r
ith
m
p
er
f
o
r
m
a
n
ce
in
an
i
n
d
o
o
r
en
v
ir
o
n
m
en
t
d
esig
n
ed
with
s
tatic
an
d
d
y
n
a
m
ic
o
b
j
ec
ts
o
n
a
V
-
R
E
P
s
im
u
lato
r
.
I
t
was
o
b
s
er
v
ed
an
d
v
er
if
ie
d
with
r
esu
lts
th
at
au
to
n
o
m
o
u
s
m
o
b
ile
r
o
b
o
t
n
av
ig
ates
u
s
in
g
E
KF
-
SLAM
alg
o
r
ith
m
im
p
lem
en
ted
o
n
MA
T
L
AB
wh
ile
r
u
n
tim
e
r
o
b
o
t
n
av
ig
atio
n
v
iewe
d
o
n
V
-
R
E
P
wh
ich
was
later
lin
k
ed
with
MA
T
L
AB
GUI
v
ia
r
e
m
o
te
API
co
m
m
a
n
d
s
.
T
h
e
Pio
n
ee
r
3
-
DX
Mo
b
ile
r
o
b
o
t
i.e
.
,
eq
u
ip
p
ed
with
th
e
L
i
d
ar
an
d
Ultr
aso
n
ic
s
en
s
o
r
s
p
e
r
f
o
r
m
e
d
ex
p
lo
r
atio
n
with
s
tatic
an
d
d
y
n
am
ic
o
b
jec
ts
,
it
was
o
b
s
er
v
e
d
t
h
r
o
u
g
h
L
a
n
d
m
ar
k
s
m
ea
s
u
r
em
en
t
th
at
E
KF
-
SLAM
co
r
r
ec
ts
o
d
o
m
etr
y
er
r
o
r
.
T
h
e
s
im
u
latio
n
v
er
if
ies
th
at
th
e
d
e
g
r
ad
atio
n
in
o
d
o
m
et
r
y
er
r
o
r
a
n
d
er
r
o
r
in
p
o
s
itio
n
o
v
e
r
tim
e
wh
ich
in
r
esu
lts
d
ec
r
ea
s
e
u
n
c
er
tain
ty
in
p
o
s
itio
n
o
v
er
tim
e.
T
h
e
r
o
b
o
t
d
ef
in
es
its
p
ath
b
y
av
o
id
in
g
o
b
s
tacle
s
an
d
u
p
d
ate
lan
d
m
ar
k
s
if
an
y
n
ew
lan
d
m
ar
k
s
ar
e
f
o
u
n
d
d
u
r
in
g
th
e
s
ca
n
.
E
KF
-
SLAM
p
er
f
o
r
m
an
ce
o
b
s
er
v
e
d
th
at
th
e
r
o
b
o
t
p
er
f
o
r
m
s
n
a
v
i
g
atio
n
with
less
p
o
s
itio
n
er
r
o
r
,
s
ca
n
er
r
o
r
,
an
d
o
d
o
m
etr
y
er
r
o
r
i
n
th
e
in
d
o
o
r
en
v
ir
o
n
m
en
t
with
s
tatic
o
b
je
cts
as
co
m
p
ar
ed
to
d
y
n
am
ic
o
b
jects.
T
h
e
s
u
g
g
ested
E
KF
-
SLAM
s
o
lv
es
th
e
o
n
lin
e
SLAM
p
r
o
b
lem
b
y
u
s
in
g
a
lin
e
ar
ized
Ga
u
s
s
ian
p
r
o
b
ab
ilit
y
d
is
tr
ib
u
tio
n
m
eth
o
d
.
I
t
was
s
u
p
p
o
s
ed
a
s
f
ir
s
t
p
r
o
b
ab
ilis
ti
c
SLAM
alg
o
r
ith
m
.
Data
a
s
s
o
ciatio
n
is
o
n
e
o
f
th
e
ch
allen
g
in
g
p
r
o
b
lem
s
in
n
av
ig
atio
n
in
wh
ich
ass
o
ciatio
n
b
etwe
en
m
e
asu
r
em
en
ts
an
d
f
ea
t
u
r
es is
u
n
k
n
o
wn
th
at
is
s
o
lv
ed
u
s
in
g
E
K
F
-
SLAM
.
Fu
tu
r
e
wo
r
k
is
to
esti
m
ate
al
g
o
r
ith
m
p
e
r
f
o
r
m
an
ce
f
o
r
a
lo
n
g
er
p
er
io
d
with
r
ed
u
c
in
g
co
m
p
u
tatio
n
al
co
m
p
lex
ity
,
th
r
o
u
g
h
ex
p
er
im
en
t
s
an
d
co
m
p
a
r
e
its
r
esu
lt
with
cu
r
r
e
n
t
r
esear
ch
f
in
d
in
g
s
.
Ho
wev
er
,
E
KF
-
Evaluation Warning : The document was created with Spire.PDF for Python.