I
nte
rna
t
io
na
l J
o
urna
l o
f
E
lect
rica
l a
nd
Co
m
p
ute
r
E
ng
in
ee
ring
(
I
J
E
CE
)
Vo
l.
10
,
No
.
5
,
Octo
b
er
2
0
2
0
,
p
p
.
5
2
4
3
~
5
2
5
0
I
SS
N:
2
0
8
8
-
8708
,
DOI
: 1
0
.
1
1
5
9
1
/
i
j
ec
e
.
v
1
0
i
5
.
pp
5
2
4
3
-
5
2
5
0
5243
J
o
ur
na
l ho
m
ep
a
g
e
:
h
ttp
:
//ij
ec
e.
ia
esco
r
e.
co
m/in
d
ex
.
p
h
p
/I
JE
C
E
The cubic
roo
t
un
scented
k
a
l
m
a
n
f
i
lter to
esti
m
a
te
the
po
sitio
n and
orienta
tion o
f
m
o
b
ile robo
t
t
ra
ject
o
r
y
O
m
a
r
B
a
y
a
s
li,
H
ess
en
Sa
lhi
De
p
a
rtme
n
t
o
f
El
e
c
tro
n
ic,
U
n
iv
e
rsity
o
f
Bli
d
a
1
,
A
lg
e
ria
Art
icle
I
nfo
AB
ST
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
No
v
3
0
,
2
0
1
9
R
ev
i
s
ed
A
p
r
2
6
,
2
0
2
0
A
cc
ep
ted
Ma
y
6
,
2
0
2
0
In
t
h
is
p
a
p
e
r
w
e
in
tr
o
d
u
c
e
a
c
u
b
i
c
r
o
o
t
u
n
sc
e
n
te
d
k
a
lm
a
n
f
i
lt
e
r
(CRUK
F
)
c
o
m
p
a
re
d
t
o
t
h
e
u
n
s
c
e
n
te
d
k
a
lm
a
n
f
il
ter
(U
KF
)
f
o
r
c
a
lc
u
la
t
in
g
t
h
e
c
o
v
a
ria
n
c
e
c
u
b
ic
m
a
tr
ix
a
n
d
c
o
v
a
r
ia
n
c
e
m
a
tr
ix
w
it
h
i
n
a
se
n
s
o
r
f
u
si
o
n
a
l
g
o
r
it
h
m
t
o
e
st
im
a
te
t
h
e
m
e
a
su
re
m
e
n
ts
o
f
a
n
o
m
n
i
d
irec
ti
o
n
a
l
m
o
b
i
le
r
o
b
o
t
tra
jec
to
ry
.
W
e
stu
d
y
t
h
e
f
u
s
i
o
n
o
f
t
h
e
d
a
ta
o
b
t
a
i
n
e
d
b
y
th
e
p
o
s
it
i
o
n
a
n
d
o
r
ie
n
t
a
ti
o
n
w
i
t
h
a
g
o
o
d
p
re
c
i
si
o
n
t
o
l
o
c
a
l
ize
t
h
e
r
o
b
o
t
i
n
a
n
e
x
te
r
n
a
l
m
e
d
i
u
m
;
w
e
a
p
p
ly
th
e
t
e
c
h
n
i
q
u
e
s
o
f
k
a
lm
a
n
f
i
lt
e
r
(K
F
)
t
o
t
h
e
e
s
t
im
a
ti
o
n
o
f
t
h
e
tra
jec
t
o
ry
.
W
e
su
p
p
o
s
e
a
m
o
v
e
m
e
n
t
o
f
m
o
b
i
le
r
o
b
o
t
o
n
a
p
la
n
i
n
tw
o
d
im
e
n
si
o
n
s.
T
h
e
se
n
s
o
r
a
p
p
ro
a
c
h
is
b
a
s
e
d
o
n
t
h
e
CRU
KF
a
n
d
t
o
o
o
n
t
h
e
s
ta
n
d
a
r
d
UK
F
w
h
ic
h
a
re
m
o
d
if
ie
d
t
o
h
a
n
d
le
m
e
a
su
re
m
e
n
t
s
f
r
o
m
t
h
e
p
o
si
ti
o
n
a
n
d
o
rie
n
tat
i
o
n
.
A
re
a
l
-
ti
m
e
im
p
lem
e
n
ta
ti
o
n
is
d
o
n
e
o
n
a
t
h
re
e
-
w
h
e
e
le
d
o
m
n
i
d
i
re
c
ti
o
n
a
l
m
o
b
i
le
r
o
b
o
t
,
u
s
i
n
g
a
d
y
n
a
m
ic
m
o
d
e
l
w
it
h
t
ra
je
c
t
o
r
ies
.
T
h
e
a
l
g
o
r
it
h
m
i
s
a
n
a
l
y
z
e
d
a
n
d
v
a
li
d
a
te
d
w
it
h
sim
u
l
a
ti
o
n
s
.
K
ey
w
o
r
d
s
:
C
R
U
KF
Mo
b
ile
r
o
b
o
t
No
n
li
n
ea
r
s
y
s
te
m
Sen
s
o
r
UKF
Co
p
y
rig
h
t
©
2
0
2
0
In
stit
u
te o
f
A
d
v
a
n
c
e
d
E
n
g
i
n
e
e
rin
g
a
n
d
S
c
ien
c
e
.
Al
l
rig
h
ts re
se
rv
e
d
.
C
o
r
r
e
s
p
o
nd
ing
A
uth
o
r
:
O
m
ar
B
a
y
asl
i,
Dep
ar
t
m
en
t o
f
E
lectr
o
n
ic,
Un
i
v
er
s
it
y
B
lid
a
1
,
B
.
P
.
2
7
0
,
R
o
ad
o
f
So
u
m
aa
,
B
lid
a,
A
lg
er
ia
.
E
m
ail:
b
a
y
ao
m
ar
9
7
@
g
m
ail.
co
m
1.
I
NT
RO
D
UCT
I
O
N
T
h
e
m
o
b
ile
r
o
b
o
ts
ar
e
eq
u
ip
p
ed
w
it
h
s
e
n
s
o
r
s
to
m
ea
s
u
r
e
t
h
e
d
is
tan
ce
o
f
th
e
r
o
b
o
t
f
r
o
m
t
h
e
s
p
ac
e
o
f
th
e
o
b
j
ec
ts
w
h
er
e
t
h
e
y
m
o
v
e.
Me
asu
r
e
m
e
n
ts
w
h
ich
ar
e
al
w
a
y
s
li
n
k
ed
b
y
n
o
i
s
es
ar
e
th
en
f
u
s
ed
to
g
et
h
er
i
n
a
f
ilter
to
o
b
tain
an
esti
m
ate
o
f
th
e
p
o
s
itio
n
an
d
o
r
ien
tatio
n
o
f
th
e
m
o
b
ile
r
o
b
o
t
tr
a
j
ec
to
r
y
i
n
th
e
s
p
ac
e
an
d
p
lan
[
1
,
2
]
.
T
h
e
m
o
s
t
i
m
p
o
r
tan
t
p
r
o
b
le
m
s
r
elate
d
to
r
o
b
o
ts
ar
e
s
it
u
ated
in
s
e
n
s
o
r
s
p
o
wer
e
b
y
t
h
eir
b
atter
y
w
h
ic
h
r
ed
u
ce
s
th
e
a
u
to
n
o
m
y
.
A
l
s
o
th
er
e
ar
e
s
itu
a
tio
n
s
w
h
er
e
s
en
s
o
r
s
ca
n
n
o
t
f
u
n
ctio
n
s
i
m
u
lta
n
eo
u
s
l
y
,
w
h
en
th
e
y
u
s
e
t
h
e
s
a
m
e
f
r
eq
u
en
c
y
b
an
d
[
3
,
4
]
.
T
h
e
Kalm
a
n
f
ilte
r
is
an
o
p
ti
m
al
li
n
ea
r
esti
m
ato
r
w
h
e
n
t
h
e
p
r
o
ce
s
s
n
o
is
e
a
n
d
th
e
m
ea
s
u
r
e
m
en
t
n
o
is
e
ca
n
b
e
m
o
d
eled
b
y
w
h
ite
Gau
s
s
ia
n
n
o
is
e.
No
n
-
li
n
e
ar
p
r
o
b
lem
s
ca
n
b
e
s
o
lv
ed
w
it
h
t
h
e
u
n
s
ce
n
ted
Kal
m
an
f
ilter
(
UKF)
a
n
d
w
e
p
r
o
p
o
s
e
a
n
e
w
alg
o
r
it
h
m
t
h
at
i
s
ca
lled
th
e
c
u
b
ic
r
o
o
t
u
n
s
ce
n
ted
Kal
m
a
n
f
il
ter
(
C
R
UKF)
to
s
o
lv
e
also
n
o
n
li
n
ea
r
p
r
o
b
lem
s
.
I
n
o
u
r
w
o
r
k
,
w
e
u
s
e
th
i
s
tech
n
iq
u
e
to
esti
m
ate
th
e
p
o
s
itio
n
a
n
d
th
e
an
g
le
o
f
th
e
r
o
b
o
t
in
its
d
is
p
lace
m
e
n
t
th
e
n
a
co
m
p
ar
i
s
o
n
b
et
w
ee
n
th
e
p
er
f
o
r
m
a
n
ce
s
o
f
t
h
e
t
w
o
f
i
lter
s
is
p
r
ese
n
ted
to
g
i
v
e
an
ev
alu
atio
n
o
f
m
o
r
e
p
r
ec
is
ed
m
ea
s
u
r
e
m
en
t
a
n
d
w
i
l
l
ap
p
r
ec
iate
b
etter
s
tatis
tical
p
r
o
p
er
ties
[
2
,
5
]
.
T
h
is
p
ap
er
is
o
r
g
a
n
ized
as
f
o
ll
o
w
;
w
e
p
r
esen
t
t
h
e
m
o
d
eli
n
g
o
f
t
h
e
m
o
b
ile
r
o
b
o
t
w
it
h
th
e
e
q
u
atio
n
s
o
f
tr
aj
ec
to
r
y
an
d
an
g
les
o
f
t
h
e
r
o
b
o
t.
Nex
t,
w
e
p
ass
to
th
e
p
r
esen
tat
io
n
o
f
t
h
e
UKF
al
g
o
r
it
h
m
a
n
d
w
e
p
r
o
p
o
s
e
a
C
R
UKF
alg
o
r
it
h
m
to
i
m
p
r
o
v
e
th
e
ac
c
u
r
ac
y
o
f
th
e
s
tate
es
ti
m
ate.
Fi
n
all
y
w
e
p
r
esen
t
a
n
d
d
is
c
u
s
s
th
e
s
i
m
u
latio
n
r
esu
l
ts
an
d
o
u
t
l
in
e
s
o
m
e
p
o
s
s
ib
le
ex
te
n
s
io
n
s
f
o
r
f
u
t
u
r
e
in
v
esti
g
atio
n
s
.
2.
M
O
DE
L
I
N
G
O
F
M
O
B
I
L
E
RO
B
O
T
T
h
e
r
o
b
o
t
tak
es
th
e
p
o
s
itio
n
tr
aj
ec
to
r
y
T
b
etw
ee
n
t
w
o
m
o
m
en
ts
f
r
o
m
s
a
m
p
li
n
g
w
it
h
th
e
v
ar
iatio
n
in
th
e
p
o
s
itio
n
an
d
t
h
e
d
ir
ec
tio
n
th
en
w
e
s
u
p
p
o
s
es
i
n
o
u
r
p
ap
er
th
e
o
m
n
id
ir
ec
tio
n
al
m
o
b
ile
r
o
b
o
t
lik
e
s
h
o
w
n
i
n
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
n
t J
E
lec
&
C
o
m
p
E
n
g
,
Vo
l.
10
,
No
.
5
,
Octo
b
e
r
2
0
2
0
:
5
2
4
3
-
5250
5244
F
ig
u
r
e
1.
Ou
r
r
o
b
o
t h
av
e
th
r
ee
w
h
ee
l
s
w
h
o
o
s
e
f
f
ec
t
o
n
th
e
r
o
b
o
t sp
ee
d
b
ec
au
s
e
o
f
t
h
eir
w
ei
g
h
t
an
d
f
r
ictio
n
o
n
th
e
g
r
o
u
n
d
,
w
e
s
u
p
p
o
s
e
1
,
2
an
d
3
th
e
r
o
b
o
t
m
as
s
ap
p
lied
o
n
th
e
w
h
ee
ls
.
T
h
e
o
m
n
id
ir
ec
tio
n
al
m
o
b
ile
R
o
b
o
t
h
as
t
w
o
i
n
d
ep
en
d
en
tl
y
s
p
ee
d
-
co
n
tr
o
lled
w
h
ee
ls
eq
u
i
p
p
ed
w
ith
s
e
n
s
o
r
s
,
a
n
d
a
ca
s
t
o
r
w
h
e
el
[
2
,
6
-
1
2
]
.
Fo
r
s
u
ch
a
r
o
b
o
t,
an
ap
p
r
o
x
i
m
ated
,
d
is
cr
ete
-
ti
m
e
m
o
d
el
is
:
+
1
=
+
.
.
.
1
.
(
+
1
)
+
1
+
1
=
+
.
.
.
2
.
(
+
1
)
+
2
+
1
=
+
∆
+
3
(
1
)
w
h
er
e
(
,
)
is
th
e
p
o
s
itio
n
o
f
th
e
r
o
b
o
t
at
tim
e
,
w
it
h
=
(
1
2
3
)
,
=
1
,
=
2
,
=
3
,
r
ep
r
esen
te
th
e
a
n
g
le
b
et
w
ee
n
t
h
e
r
o
b
o
t
ax
le
an
d
t
h
e
x
-
a
x
is
,
=
(
1
2
3
)
ar
e
ze
r
o
-
m
ea
n
u
n
co
r
r
elate
d
Gau
s
s
ia
n
n
o
is
e
s
an
d
=
(
1
2
)
ar
e
t
h
e
an
g
u
lar
v
elo
citie
s
o
f
w
h
ee
ls
,
=
.
(
1
+
2
+
3
)
/
3
i
s
th
e
m
a
s
s
o
f
th
e
r
o
b
o
t
in
w
h
e
els
.
=
.
(
1
+
2
)
/
2
is
th
e
lin
ea
r
v
elo
cit
y
o
f
th
e
r
o
b
o
t,
=
+
1
−
is
th
e
s
a
m
p
l
in
g
p
er
io
d
an
d
∆
=
.
(
2
−
1
)
.
/
2
is
th
e
r
o
tatio
n
w
it
h
in
[
,
+
1
]
.
is
th
e
r
ad
iu
s
o
f
th
e
w
h
ee
ls
a
n
d
L
is
t
h
e
len
g
t
h
o
f
th
e
a
x
es.
T
h
e
o
m
n
id
ir
ec
tio
n
a
l
m
o
b
ile
r
o
b
o
t
is
eq
u
ip
p
ed
w
ith
s
e
n
s
o
r
s
to
m
ea
s
u
r
e
i
ts
p
o
s
it
i
o
n
an
d
i
ts
o
r
ien
tatio
n
l
ik
e
s
h
o
w
n
i
n
Fi
g
u
r
e
2
.
T
h
e
f
i
g
u
r
e
a
ls
o
d
ef
i
n
e
s
th
e
e
n
v
ir
o
n
m
en
t
o
f
th
e
r
o
b
o
t,
ass
u
m
ed
p
er
f
ec
tl
y
k
n
o
w
n
[
7
,
10
,
1
3
]
.
I
n
o
u
r
w
o
r
k
w
e
p
r
o
p
o
s
e
th
e
tr
aj
ec
to
r
y
T
o
n
w
h
ich
w
e
ap
p
l
y
t
h
e
tec
h
n
ic
al
o
f
Kal
m
a
n
f
ilter
s
C
R
U
KF a
n
d
UK
F to
esti
m
ate
th
is
tr
aj
ec
to
r
y
.
Fig
u
r
e
1
.
O
m
n
id
ir
ec
tio
n
al
m
o
b
ile
r
o
b
o
t
Fig
u
r
e
2
.
O
m
n
id
ir
ec
tio
n
al
m
o
b
ile
r
o
b
o
t t
r
aj
ec
t
o
r
y
T
2
.
1
.
T
he
equa
t
io
ns
o
f
t
ra
j
ec
t
o
ry
T
T
h
e
tr
aj
ec
to
r
y
o
f
t
h
e
r
o
b
o
t
is
d
e
f
in
ed
b
y
t
h
e
eq
u
at
io
n
+
1
=
(
+
1
)
w
h
er
e
=
1
=
1
;
=
2
=
1
;
=
3
=
1
;
=
1
;
1
=
2
=
0
,
02
m/
s
;
1
=
0
,
2
;
2
=
0
,
1
;
∆
=
/
2
;
1
=
0
,
3
g
;
2
=
0
,
3
g
;
3
=
0
,
3
g
;
=
0
,
5
.
1
0
−
2
m
=
0
,
18
m
an
d
=
.
(
1
+
2
+
3
)
/
3
L
;
th
en
=
0
,
0083
g
.
W
e
h
av
e
o
u
r
m
o
d
el
:
{
A
k
+
1
=
A
k
+
v
1
m
k
t
u
1
c
os
(
θ
k
+
1
)
+
w
1
B
k
+
1
=
B
k
+
v
2
m
k
t
u
2
s
in
(
θ
k
+
1
)
+
w
2
θ
k
+
1
=
θ
k
+
∆
θ
+
w
3
t
h
en
{
A
k
+
1
−
A
k
−
w
1
=
v
1
m
k
t
u
1
c
os
(
θ
k
+
1
)
B
k
+
1
−
B
k
−
w
2
=
v
2
m
k
t
u
2
s
in
(
θ
k
+
1
)
t
h
en
(
+
1
−
−
1
)
1
1
=
(
+
1
)
(
2
)
X
Y
T
R
o
b
o
t
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J
E
lec
&
C
o
m
p
E
n
g
I
SS
N:
2
0
8
8
-
8708
Th
e
cu
b
ic
r
o
o
t u
n
s
ce
n
ted
K
a
l
ma
n
filt
er to
esti
ma
te
th
e
p
o
s
itio
n
a
n
d
o
r
ien
ta
tio
n
... (
Oma
r
B
a
ya
s
li
)
5245
(
+
1
−
−
2
)
2
2
=
(
+
1
)
(
3
)
a
n
d
w
e
m
a
k
e
s
q
u
ar
e:
(
+
1
−
−
1
)
2
(
1
1
)
2
=
2
(
+
1
)
(
4
)
(
+
1
−
−
2
)
2
(
2
2
)
2
=
2
(
+
1
)
(
5
)
(
4
)
+
(
5
)
⟹
(
A
k
+
1
−
A
k
−
w
1
)
2
(
v
1
m
k
t
u
1
)
2
+
(
B
k
+
1
−
B
k
−
w
2
)
2
(
v
2
m
k
t
u
2
)
2
=
c
os
2
(
θ
k
+
1
)
+
s
in
2
(
θ
k
+
1
)
t
h
en
(
A
k
+
1
−
A
k
−
w
1
)
2
(
v
1
m
k
t
u
1
)
2
+
(
B
k
+
1
−
B
k
−
w
2
)
2
(
v
2
m
k
t
u
2
)
2
=
1
⟹
(
B
k
+
1
−
B
k
−
w
2
)
2
(
v
2
m
k
t
u
2
)
2
=
1
−
(
A
k
+
1
−
A
k
−
w
1
)
2
(
v
1
m
k
t
u
1
)
2
⟹
(
B
k
+
1
−
B
k
−
w
2
)
2
=
(
v
2
m
k
t
u
2
)
2
(
1
−
(
B
k
+
1
−
B
k
−
w
1
)
2
(
v
1
m
k
t
u
1
)
2
)
⟹
B
k
+
1
−
B
k
−
w
2
=
√
(
v
2
m
k
t
u
2
)
2
(
1
−
(
A
k
+
1
−
A
k
−
w
1
)
2
(
v
1
m
k
t
u
1
)
2
)
t
h
en
B
k
+
1
=
B
k
+
w
2
+
v
2
m
k
t
u
2
√
(
1
−
(
A
k
+
1
−
A
k
−
w
1
)
2
(
v
1
m
k
t
u
1
)
2
)
Fo
r
th
e
tr
aj
ec
to
r
y
T
w
e
ta
k
e:
1
=
0
,
2
=
0
an
d
3
=
0
B
k
+
1
=
1
+
0
+
0
,
02
.
1
.
0
,
0083
.
0
,
1
.
√
(
1
−
(
A
k
+
1
−
1
−
0
)
2
(
0
,
02
.
10
,
0083
.
.
0
,
2
)
2
)
T
h
en
th
e
f
in
al
eq
u
atio
n
o
f
T
is
:
B
k
+
1
=
1
+
0
,
016
.
10
−
3
√
(
1
−
(
A
k
+
1
−
1
)
2
0
,
256
.
10
−
6
)
2
.
2
.
T
he
a
ng
le
o
f
ro
bo
t
+
W
e
h
av
e
{
(
A
k
+
1
−
A
k
−
w
1
)
v
1
m
k
t
u
1
=
c
os
(
θ
k
+
1
)
(
B
k
+
1
−
B
k
−
w
2
)
v
2
m
k
t
u
2
=
s
in
(
θ
k
+
1
)
(
2
)
⟹
+
1
=
(
+
1
−
−
1
1
1
)
i
s
th
e
a
n
g
le
o
f
r
o
b
o
t w
ith
a
x
is
,
o
r
ca
n
w
e
f
i
n
d
th
i
s
an
g
le
b
y
(
3
)
:
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
n
t J
E
lec
&
C
o
m
p
E
n
g
,
Vo
l.
10
,
No
.
5
,
Octo
b
e
r
2
0
2
0
:
5
2
4
3
-
5250
5246
(
3
)
⟹
(
B
k
+
1
−
B
k
−
w
2
)
v
2
m
k
t
u
2
=
s
in
(
θ
k
+
1
)
⟹
θ
k
+
1
=
a
s
in
(
B
k
+
1
−
B
k
−
w
2
v
2
m
k
t
u
2
)
=
(
(
+
1
)
(
+
1
)
)
is
th
e
a
n
g
le
o
f
r
o
b
o
t w
ith
w
h
e
els ax
i
s
3.
UNSCE
NT
E
D
K
AL
M
AN
F
I
L
T
E
R
AND
H
E
R
AL
G
O
R
I
T
H
M
T
h
e
UKF
h
as
b
ee
n
d
e
v
elo
p
ed
in
r
ec
e
n
t
y
ea
r
s
to
o
v
er
co
m
e
t
w
o
m
ai
n
p
r
o
b
le
m
s
o
f
th
e
E
KF
(
ex
te
n
d
ed
Kal
m
a
n
f
ilter
)
,
t
h
e
p
o
o
r
ap
p
r
o
x
i
m
at
io
n
p
r
o
p
er
ties
o
f
th
e
f
i
r
s
t
o
r
d
er
ap
p
r
o
x
im
at
io
n
a
n
d
t
h
e
r
eq
u
ir
e
m
en
t
f
o
r
th
e
n
o
is
e
s
to
b
e
Gau
s
s
ian
.
T
h
e
b
asic
id
ea
b
eh
in
d
th
e
UK
F
is
th
at
o
f
f
i
n
d
i
n
g
a
tr
an
s
f
o
r
m
atio
n
t
h
at
allo
w
s
ap
p
r
o
x
im
a
tin
g
t
h
e
m
ea
n
a
n
d
th
e
co
v
ar
ia
n
ce
o
f
a
r
a
n
d
o
m
v
ec
to
r
o
f
len
g
t
h
w
h
en
it
i
s
tr
an
s
f
o
r
m
ed
b
y
a
n
o
n
l
in
ea
r
m
ap
.
T
h
is
i
s
d
o
n
e
b
y
co
m
p
u
ti
n
g
a
s
et
o
f
2
+
1
p
o
in
ts
,
ca
l
led
s
i
g
m
a
p
o
in
t
s
,
o
n
t
h
e
b
asi
s
o
f
th
e
m
ea
n
a
n
d
v
ar
ian
ce
o
f
t
h
e
o
r
ig
in
al
v
ec
to
r
,
tr
an
s
f
o
r
m
i
n
g
t
h
ese
p
o
in
ts
b
y
t
h
e
n
o
n
li
n
ea
r
m
ap
an
d
th
e
n
ap
p
r
o
x
im
a
tin
g
t
h
e
m
ea
n
an
d
v
ar
ian
ce
o
f
t
h
e
tr
a
n
s
f
o
r
m
ed
v
ec
to
r
f
r
o
m
t
h
e
tr
an
s
f
o
r
m
ed
s
i
g
m
a
p
o
in
ts
.
W
e
r
ef
er
to
[
2
-
5
,
1
4
-
1
7
]
f
o
r
th
e
th
eo
r
etica
l a
s
p
ec
ts
.
As
f
o
r
ap
p
r
o
x
i
m
at
in
g
p
r
o
p
er
ties
o
f
t
h
e
f
ilter
,
it
h
as
b
ee
n
s
h
o
w
n
t
h
at,
w
h
ile
t
h
e
E
KF
e
s
ti
m
ate
o
f
th
e
s
tate
is
ac
c
u
r
ate
to
th
e
f
ir
s
t
o
r
d
er
,
th
e
UK
F
es
ti
m
ate
i
s
a
cc
u
r
ate
to
t
h
e
th
ir
d
o
r
d
er
in
th
e
ca
s
e
o
f
Ga
u
s
s
ia
n
n
o
is
es.
T
h
e
co
v
ar
ia
n
ce
esti
m
ate
also
is
ac
c
u
r
ate
to
th
e
f
ir
s
t
o
r
d
er
f
o
r
th
e
E
KF,
an
d
to
t
h
e
s
ec
o
n
d
o
r
d
er
f
o
r
th
e
UKF
a
n
d
h
er
alg
o
r
it
h
m
is
:
UKF Al
gorithm
•
Initialization:
̂
0
=
[
0
]
,
0
=
[
(
0
−
̂
0
)
(
0
−
̂
0
)
]
̂
0
=
[
̂
0
]
=
[
̂
0
̅
0
̅
0
]
0
=
[
(
0
−
̂
0
)
(
0
−
̂
0
)
]
=
[
0
0
0
0
0
0
0
]
For
k
=
1
,
.
.
.
,
∞
:
1. Set
=
−
1
(
for unscented Kalman filter)
2. Calculate sigma
-
points:
=
[
̂
̂
+
√
̂
−
√
]
3. Time
-
update equations:
|
=
(
,
,
)
̂
=
∑
2
=
0
,
|
=
∑
(
,
|
−
̂
)
2
=
0
(
,
|
−
̂
)
4. Measurement
-
update equations:
|
=
ℎ
(
|
,
)
̂
=
∑
2
=
0
,
|
̅
=
∑
(
,
|
−
̂
)
2
=
0
(
,
|
−
̂
)
=
∑
(
,
|
−
̂
)
2
=
0
(
,
|
−
̂
)
=
−
1
̂
=
̂
+
(
−
̂
)
=
−
4.
T
H
E
CUB
I
C
RO
O
T
UK
F
(
CRUK
F
)
P
RO
P
O
SI
T
I
O
N
I
n
o
u
r
p
ap
er
w
e
p
r
o
p
o
s
e
th
e
cu
b
ic
r
o
o
t
C
R
UK
F
al
g
o
r
ith
m
w
h
ic
h
i
s
b
ased
o
n
s
q
u
ar
e
r
o
o
t
UKF
alg
o
r
ith
m
[1
8
,
1
9
]
.
So
w
e
u
s
e
in
o
u
r
al
g
o
r
ith
m
a
p
r
o
p
o
s
itio
n
o
f
n
e
w
f
u
n
ctio
n
(
)
an
d
UK
F
a
lg
o
r
ith
m
[
4
]
.
W
e
s
tu
d
y
t
h
e
co
ef
f
icien
ts
o
f
co
v
ar
ian
ce
m
a
tr
ix
to
in
tr
o
d
u
c
e
th
e
cu
b
ic
r
o
o
t.
I
n
th
is
tech
n
iq
u
e,
w
e
b
ased
o
n
th
e
s
a
m
e
p
r
in
cip
als o
f
th
e
UK
F a
lg
o
r
ith
m
.
4
.
1
.
T
he
f
un
ct
io
n q
q(x
)
pro
po
s
it
i
o
n
T
h
e
n
e
w
f
u
n
ctio
n
(
)
is
th
e
q
u
ad
r
atu
r
e
q
u
ad
r
atu
r
e
o
r
o
r
th
o
g
o
n
al
o
r
th
o
g
o
n
al
f
u
n
ctio
n
f
o
r
f
i
n
d
an
d
ca
lcu
late
s
t
w
o
m
atr
i
x
es q
u
ad
r
atu
r
e
o
r
o
r
th
o
g
o
n
al
an
d
it
s
eq
u
atio
n
i
s
:
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J
E
lec
&
C
o
m
p
E
n
g
I
SS
N:
2
0
8
8
-
8708
Th
e
cu
b
ic
r
o
o
t u
n
s
ce
n
ted
K
a
l
ma
n
filt
er to
esti
ma
te
th
e
p
o
s
itio
n
a
n
d
o
r
ien
ta
tio
n
... (
Oma
r
B
a
ya
s
li
)
5247
=
1
−
2
1
1
‖
1
‖
.
2
−
2
2
2
‖
2
‖
1
an
d
2
ar
e
id
en
tit
y
m
atr
i
x
1
an
d
2
ar
e
v
ec
to
r
s
;
1
an
d
2
ar
e
v
ec
to
r
s
tr
an
s
p
o
s
es
‖
1
‖
an
d
‖
2
‖
v
ec
to
r
s
m
o
d
u
le
s
4
.
2
.
T
he
CRUK
F
a
lg
o
rit
h
m
CRUKF
Algorithm
•
Initialization:
̂
0
=
[
0
]
,
0
=
√
[
(
0
−
̂
0
)
(
0
−
̂
0
)
]
3
,
=
√
3
=
√
3
̂
0
=
[
]
=
[
̂
0
]
0
=
√
[
(
0
−
̂
0
)
(
0
−
̂
0
)
]
3
,
ℎ
0
=
[
0
0
0
0
0
0
0
]
For
=
1
,
.
.
.
,
∞
:
1. Set
=
−
1
; (for the cubic root unscented Kalman filter)
2. Calculate sigma
-
points for time
-
update:
=
[
̂
̂
+
̂
−
]
3. Time
-
update equations:
|
=
(
,
,
)
̂
=
∑
2
=
0
,
|
=
{
[
√
1
3
(
1
:
2
,
|
−
̂
)
]
}
; (quadrature quadrature function)
=
{
,
0
,
|
−
̂
,
0
(
)
}
; time update
|
=
ℎ
(
,
|
,
)
̂
=
∑
2
=
0
,
|
4. Measurement
-
update equations:
=
{
[
√
1
3
(
1
:
2
,
|
−
̂
)
]
}
=
{
,
0
,
|
−
̂
,
0
(
)
}
=
∑
(
,
|
−
̂
)
(
,
|
−
̂
)
2
=
0
=
(
/
)
/
̂
=
̂
+
(
−
̂
)
=
=
{
,
,
−
1
}
W
e
h
av
e
t
w
o
f
ilter
s
UKF
an
d
C
R
UK
F
w
h
er
e:
=
√
+
,
=
√
+
3
,
0
=
(
+
)
⁄
,
0
=
0
+
(
1
+
2
+
)
,
=
=
1
2
(
+
)
⁄
,
f
o
r
=
1
.
.
.
2
,
an
d
=
2
(
+
)
is
a
co
m
p
o
u
n
d
s
ca
lin
g
p
ar
a
m
e
ter
.
is
t
h
e
d
i
m
en
s
io
n
o
f
t
h
e
a
u
g
m
e
n
ted
s
tate
-
v
ec
to
r
0
<
<
1
is
t
h
e
p
r
i
m
ar
y
s
ca
lin
g
f
ac
to
r
d
eter
m
i
n
i
n
g
t
h
e
e
x
te
n
t
o
f
th
e
s
p
r
ea
d
o
f
th
e
s
i
g
m
a
-
p
o
in
ts
ar
o
u
n
d
t
h
e
p
r
io
r
m
ea
n
.
T
y
p
ical
r
a
n
g
e
f
o
r
is
1
−
3
<
<
1
.
is
a
s
ec
o
n
d
ar
y
s
ca
li
n
g
f
ac
to
r
u
s
ed
to
e
m
p
h
a
s
ize
th
e
w
ei
g
h
tin
g
o
n
t
h
e
ze
r
o
s
s
ig
m
a
-
p
o
in
t
f
o
r
th
e
p
o
s
ter
io
r
co
v
ar
ian
ce
ca
lc
u
latio
n
.
ca
n
b
e
u
s
ed
to
m
in
i
m
ize
ce
r
tain
h
ig
h
er
-
o
r
d
er
er
r
o
r
ter
m
s
b
ased
o
n
k
n
o
w
n
m
o
m
e
n
ts
o
f
th
e
p
r
io
r
.
Fo
r
Gau
s
s
ia
n
p
r
io
r
s
,
=
2
is
o
p
ti
m
al.
is
a
ter
tiar
y
s
ca
li
n
g
f
ac
to
r
an
d
is
u
s
u
all
y
s
et
eq
u
al
to
0
.
I
n
g
en
er
al,
th
e
o
p
tim
al
v
alu
es
o
f
th
e
s
e
s
ca
li
n
g
p
ar
a
m
eter
s
w
ill
b
e
a
s
p
ec
if
ic
p
r
o
b
le
m
[
2
,
1
8
,
1
9
,
2
0
-
25]
.
-
Gen
er
al
n
o
tes
:
T
h
e
au
g
m
e
n
ted
s
tate
v
ec
t
o
r
an
d
s
ig
m
a
-
p
o
in
t
v
ec
to
r
is
g
iv
e
n
b
y
:
x
a
=
[
x
T
v
T
n
T
]
T
,
=
[
(
)
(
)
(
)
]
w
h
er
e
an
d
ar
e
th
e
p
r
o
ce
s
s
-
n
o
is
e
an
d
o
b
s
er
v
atio
n
-
n
o
is
e
co
v
ar
ian
ce
m
atr
ices.
-
L
i
n
ea
r
-
al
g
eb
r
a
o
p
er
ato
r
s
:
√
.
3
:
m
atr
ix
c
u
b
ic
-
r
o
o
t u
s
in
g
lo
wer
tr
ian
g
u
lar
.
(
)
:
lo
w
er
-
o
r
th
o
g
o
n
al
p
ar
t
o
f
m
atr
i
x
r
es
u
lti
n
g
f
r
o
m
f
u
n
ct
io
n
ℎ
(
)
d
ec
o
m
p
o
s
i
tio
n
o
f
d
ata
-
m
a
t
r
ix
.
{
,
,
−
+
}
:
co
n
s
ec
u
tiv
e
r
an
k
-
1
u
p
d
ates
o
f
t
h
e
lo
w
er
-
o
r
t
h
o
g
o
n
al
f
ac
t
o
r
b
y
t
h
e
co
lu
m
n
s
o
f
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
n
t J
E
lec
&
C
o
m
p
E
n
g
,
Vo
l.
10
,
No
.
5
,
Octo
b
e
r
2
0
2
0
:
5
2
4
3
-
5250
5248
√
:
.
/
:
E
f
f
icie
n
t
lea
s
t
-
s
q
u
ar
es
p
s
eu
d
o
in
v
er
s
e
i
m
p
le
m
e
n
ted
u
s
i
n
g
o
r
th
o
g
o
n
al
d
ec
o
m
p
o
s
itio
n
w
i
t
h
p
iv
o
tin
g
f
in
al
l
y
t
h
is
p
ar
a
m
e
ter
s
f
o
r
t
w
o
f
ilter
s
C
R
UKF
a
n
d
UKF.
5.
SI
M
UL
AT
I
O
N
R
E
S
UL
T
S
W
e
co
m
p
ar
e
th
e
p
er
f
o
r
m
a
n
c
e
o
f
C
R
UKF
a
n
d
UKF
i
n
o
u
r
p
ap
er
,
w
e
s
tar
te
f
r
o
m
th
e
p
o
in
t
o
f
tr
aj
ec
to
r
y
T
:
(
0
,
0
)
=
(
1
,
1
)
w
ith
0
=
0
.
I
n
th
e
ch
o
s
en
s
a
m
p
le
ti
m
e
=
1
s
an
d
ea
ch
tr
ajec
to
r
y
h
a
s
b
ee
n
co
m
p
leted
i
n
=
400
,
i
m
p
o
s
i
n
g
a
p
r
o
f
ile
to
t
h
e
a
n
g
u
lar
v
elo
cities
o
f
t
h
e
w
h
ee
ls
.
T
h
e
p
ar
a
m
e
t
er
s
an
d
w
ei
g
h
ts
u
s
ed
in
t
h
e
C
R
UK
F
an
d
UKF
ar
e:
=
1
0
−
4
;
=
5
;
=
(
8
–
)
/
2
;
=
2
(
+
)
–
.
=
3
m
in
i
m
izes t
h
e
er
r
o
r
o
f
th
e
a
p
o
s
ter
io
r
i c
o
v
ar
ian
ce
w
h
e
n
th
e
r
an
d
o
m
v
ec
to
r
is
Ga
u
s
s
ian
.
-
=
{
0
.
123
0
.
124
0
.
121
}
,
w
h
ic
h
co
r
r
esp
o
n
d
s
to
a
s
ta
n
d
ar
d
d
ev
iatio
n
les
s
t
h
an
o
n
e
c
en
ti
m
eter
o
n
an
d
an
d
less
t
h
a
n
o
n
e
d
eg
r
ee
o
n
;
-
=
{
0
.
110
0
.
100
0
.
111
}
,
a
s
tan
d
ar
d
d
ev
iatio
n
les
s
th
at
0
.
1
ce
n
ti
m
eter
f
o
r
ea
ch
s
e
n
s
o
r
;
-
̂
0
/
0
=
[
1
.
5000
1
.
9821
2
.
4123
]
′
,
th
e
in
it
ial
esti
m
ate
s
tate
o
f
r
o
b
o
t
-
0
=
[
1
1
0
]
′
,
th
e
in
it
ial
s
tate
o
f
r
o
b
o
t
-
0
/
0
=
[
0
.
5010
0
.
5021
0
.
5023
]
′
,
a
s
tan
d
ar
d
d
ev
iatio
n
o
f
w
id
th
an
d
len
g
t
h
o
n
th
e
p
o
s
itio
n
an
d
s
tan
d
ar
d
o
r
ien
tatio
n
.
W
e
co
m
p
ar
e
th
e
p
er
f
o
r
m
a
n
ce
o
f
th
e
t
w
o
f
ilter
s
w
e
h
av
e
co
m
p
u
ted
-
T
h
e
av
er
ag
e
er
r
o
r
p
o
s
itio
n
in
d
ex
:
=
1
+
1
∑
‖
[
−
̂
/
,
−
̂
/
]
′
‖
=
0
-
T
h
e
av
er
ag
e
o
r
ien
tatio
n
p
o
s
iti
o
n
in
d
ex
:
=
1
+
1
∑
|
+
1
−
̂
+
1
/
|
=
0
-
T
h
e
esti
m
atio
n
er
r
o
r
co
v
ar
ian
ce
av
er
a
g
ed
alo
n
g
t
h
e
w
h
o
le
tr
aj
ec
to
r
y
h
a
s
b
ee
n
co
m
p
u
ted
:
=
1
+
1
∑
(
∑
/
)
=
0
T
h
e
co
m
p
ar
is
o
n
b
et
w
ee
n
C
R
UKF
a
n
d
UKF
at
o
u
r
ca
s
e
a
s
f
o
llo
w
s
:
w
e
s
tar
t
w
i
th
ti
m
e,
we
tak
e
t
h
r
ee
v
a
lu
e
s
f
o
r
k
to
ea
ch
al
g
o
r
ith
m
.
T
ab
l
e
1
s
h
o
w
t
h
e
co
m
p
ar
is
o
n
a
n
d
th
e
s
i
m
u
latio
n
r
es
u
lts
s
h
o
w
i
n
d
if
f
er
en
t
f
ig
u
r
es:
F
ig
u
r
e
s
3
-
6.
5
.
1
.
E
s
t
i
m
a
t
io
n o
f
po
s
it
io
n by
CRUK
F
a
nd
UK
F
Fig
u
r
es
3
a
n
d
4
s
h
o
w
th
e
s
i
m
u
latio
n
r
e
s
u
l
ts
u
s
i
n
g
t
h
e
U
KF
an
d
C
R
UKF
ap
p
lied
to
t
h
e
tr
aj
ec
to
r
y
esti
m
atio
n
a
n
d
er
r
o
r
s
.
Fig
u
r
e
3
s
h
o
w
s
t
h
e
r
o
b
o
t
tr
a
j
e
cto
r
y
w
it
h
its
es
ti
m
ate
u
s
in
g
UKF
(
r
ed
)
co
m
p
ar
ed
to
th
e
C
R
UK
F
(
b
leu
)
,
t
h
e
p
o
u
r
s
u
it
w
a
s
w
ell
d
o
n
e.
Fig
u
r
e
4
p
r
esen
ts
th
e
e
s
ti
m
atio
n
er
r
o
r
b
et
w
ee
n
t
h
e
o
r
ig
i
n
a
l
tr
aj
ec
to
r
y
an
d
b
o
th
o
f
th
e
t
wo
f
ilter
s
also
t
h
e
MSE
(
m
ea
n
s
q
u
ar
e
er
r
o
r
)
o
f
th
e
al
g
o
r
ith
e
m
s
,
it
ca
n
b
e
s
ee
n
th
e
C
R
UK
F
w
as b
etter
th
e
n
t
h
e
UKF.
T
ab
le
1
.
C
o
m
p
ar
is
o
n
b
et
w
ee
n
C
R
U
KF a
n
d
UK
F
A
l
g
o
r
i
t
h
m
x
(
m)
θ
(
r
a
d
)
C
R
U
K
F
0
.
0
1
5
6
0
.
0
2
5
8
0
.
0
0
7
7
0
.
3
5
5
0
0
.
4
4
0
5
0
.
1
8
1
5
0
.
0
2
6
3
0
.
0
1
1
6
0
.
0
1
0
4
UKF
0
.
0
0
8
2
0
.
0
0
5
0
0
.
0
0
7
4
0
.
1
5
6
3
0
.
0
7
1
6
0
.
0
4
9
3
0
.
0
0
1
5
0
.
0
0
1
0
0
.
0
0
0
3
Fig
u
r
e
3
.
E
s
ti
m
ate
r
e
f
er
en
ce
tr
aj
ec
to
r
y
T
b
y
C
R
UKF
a
n
d
UKF
Fig
u
r
e
4
.
E
s
ti
m
ate
er
r
o
r
er
1
,
e
r
2
an
d
h
er
esti
m
at
io
n
b
y
C
R
UKF
a
n
d
UKF
an
d
MS
E
1
2
3
4
5
6
7
8
0
.
9
6
0
.
9
8
1
1
.
0
2
1
.
0
4
1
.
0
6
1
.
0
8
1
.
1
1
.
1
2
1
.
1
4
t
(
s
)
p
o
s
i
t
i
o
n
(
c
m
)
r
e
f
e
r
e
n
c
e
(
T
)
C
R
U
K
F
U
K
F
R
o
b
o
t
0
5
10
15
20
25
30
35
40
45
50
-
0
.
2
0
0
.
2
t
(
s
)
e
r
1
e
r
1
C
R
U
K
F
U
K
F
0
5
10
15
20
25
30
35
40
45
50
-
0
.
2
0
0
.
2
t
(
s
)
e
r
2
e
r
2
C
R
U
K
F
U
K
F
0
5
10
15
20
25
30
35
40
45
50
0
0
.
0
1
0
.
0
2
t
(
s
)
M
S
E
(
%
)
C
R
U
K
F
U
K
F
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J
E
lec
&
C
o
m
p
E
n
g
I
SS
N:
2
0
8
8
-
8708
Th
e
cu
b
ic
r
o
o
t u
n
s
ce
n
ted
K
a
l
ma
n
filt
er to
esti
ma
te
th
e
p
o
s
itio
n
a
n
d
o
r
ien
ta
tio
n
... (
Oma
r
B
a
ya
s
li
)
5249
5
.
2
.
E
s
t
i
m
a
t
io
n o
f
a
ng
le
by
CRUK
F
a
nd
UK
F
F
ig
u
r
e
5
s
h
o
w
s
t
h
e
a
n
g
le
e
s
ti
m
ate
o
f
t
h
e
r
o
b
o
t
u
s
i
n
g
t
h
e
t
w
o
f
ilter
s
.
Fi
g
u
r
e
6
ex
h
ib
it
s
esti
m
atio
n
er
r
o
r
s
o
f
th
ese
f
i
lter
s
w
h
er
e
th
e
UKF
w
as h
er
e
b
etter
th
a
n
t
h
e
C
R
UK
F.
Fig
u
r
e
5
.
T
h
e
esti
m
ate
an
g
le
o
f
tr
aj
ec
to
r
y
T
w
it
h
ax
i
s
b
y
C
R
UKF
a
n
d
UKF
Fig
u
r
e
6
.
E
s
ti
m
ate
er
r
o
r
er
1
,
er
2
o
f
an
g
le
a
n
d
h
er
esti
m
atio
n
b
y
C
R
UKF
a
n
d
UKF
an
d
MSE
6.
CO
NCLU
SI
O
N
T
h
e
an
al
y
s
i
s
o
f
t
h
e
r
es
u
lts
i
n
T
a
b
le
1
is
th
at
th
e
t
w
o
f
ilter
s
p
er
f
o
r
m
co
m
p
ar
ab
l
y
t
h
e
p
o
s
itio
n
a
n
d
o
r
ien
tatio
n
o
f
t
h
e
r
o
b
o
t,
w
e
s
ee
th
at
th
e
est
i
m
a
tio
n
er
r
o
r
o
f
UKF
is
w
ea
k
t
h
an
t
h
o
s
e
t
h
e
esti
m
atio
n
er
r
o
r
o
f
C
R
U
KF
in
a
n
g
le
e
s
ti
m
atio
n
c
o
n
tr
ar
y
in
t
h
e
p
o
s
itio
n
w
h
er
e
t
h
e
C
R
UKF
w
as
b
etter
b
ec
au
s
e
th
e
ap
p
r
o
x
im
a
tin
g
p
r
o
p
er
ties
o
f
ea
ch
f
ilter
.
T
h
es
e
t
w
o
f
ilter
s
,
al
w
a
y
s
g
i
v
i
n
g
a
v
alu
e
o
f
th
e
er
r
o
r
co
v
ar
ian
ce
π
o
f
in
d
ex
w
h
ic
h
is
b
etter
th
an
t
h
o
s
e
g
i
v
en
b
y
a
n
y
s
e
n
s
o
r
,
an
d
co
m
p
lete
l
y
clo
s
e
to
th
at
o
b
tain
ed
w
h
e
n
all
t
h
e
s
en
s
o
r
s
ar
e
u
s
ed
.
R
es
u
lts
s
h
o
w
s
th
e
es
ti
m
ate
r
ef
er
en
ce
tr
aj
ec
to
r
y
T
an
d
esti
m
ate
er
r
o
r
b
y
C
R
UK
F
an
d
U
KF
to
o
MSE
o
f
th
is
ca
s
e
an
d
th
e
esti
m
ate
o
f
t
h
e
r
o
b
o
t
an
g
le
an
d
its
esti
m
ate
er
r
o
r
b
y
C
R
UK
F
an
d
UKF
to
o
MSE
o
f
th
is
ca
s
e,
w
e
s
ee
th
a
t
th
e
e
s
ti
m
atio
n
s
wer
e
o
p
tim
al.
I
n
t
h
e
n
ea
r
f
u
tu
r
e
th
is
tech
n
iq
u
e
o
f
w
o
r
k
ca
n
b
e
d
o
n
e
an
d
d
ev
elo
p
o
n
o
th
er
t
y
p
es
o
f
r
o
b
o
ts
an
d
o
th
er
ap
p
licatio
n
s
w
it
h
le
s
s
s
e
n
s
o
r
b
u
t
w
it
h
a
g
r
ea
t
m
ea
s
u
r
i
n
g
ac
c
u
r
ac
y
i
n
r
ea
l
-
ti
m
e
b
u
t i
ts
b
etter
to
m
a
k
e
o
n
e
f
ilter
co
m
p
lete
t
h
e
o
th
er
.
RE
F
E
R
E
NC
E
S
[1
]
L
.
D’
A
l
f
o
n
so
,
e
t
a
l
.,
“
M
o
b
il
e
ro
b
o
t
lo
c
a
li
z
a
ti
o
n
v
ia E
KF
a
n
d
UK
F
:
A
c
o
m
p
a
riso
n
b
a
se
d
o
n
re
a
l
d
a
ta,
”
Ro
b
o
ti
c
s a
n
d
Au
to
n
o
m
o
u
s
S
y
ste
ms
,
v
o
l.
7
4
,
p
a
r
t
A
,
p
p
.
1
2
2
-
2
7
,
2
0
1
5
.
[2
]
L
.
D'
A
l
f
o
n
so
,
e
t
a
l
.,
“
F
il
ters
f
o
r
m
o
b
il
e
ro
b
o
ts:
EKF
,
UK
F
a
n
d
se
n
so
r
sw
it
c
h
in
g
-
e
x
p
e
ri
m
e
n
tal
re
s
u
lt
s,
”
2
0
1
1
9
t
h
IEE
E
In
ter
n
a
ti
o
n
a
l
C
o
n
fer
e
n
c
e
o
n
Co
n
tro
l
a
n
d
Au
t
o
ma
ti
o
n
(
ICCA)
,
S
a
n
ti
a
g
o
,
p
p
.
9
2
5
-
9
3
0
,
2
0
1
1
.
[3
]
E
.
A
.
Wan
a
n
d
R.
V
a
n
De
r
M
e
rwe
,
“
T
h
e
u
n
sc
e
n
ted
Ka
lm
a
n
fil
ter
f
o
r
n
o
n
l
in
e
a
r
e
stim
a
ti
o
n
,
”
Pro
c
e
e
d
in
g
s
o
f
t
h
e
IEE
E
2
0
0
0
Ad
a
p
ti
v
e
S
y
ste
ms
fo
r
S
ig
n
a
l
Pro
c
e
ss
in
g
,
Co
mm
u
n
ica
ti
o
n
s,
a
n
d
Co
n
tro
l
S
y
mp
o
siu
m
(
Ca
t
.
No
.
0
0
EX
3
7
3
)
,
L
a
k
e
L
o
u
ise
,
A
lb
e
rta,
Ca
n
a
d
a
,
p
p
.
1
5
3
-
1
5
8
,
2
0
0
0
.
[4
]
S
.
J.
Ju
li
e
r
a
n
d
J.
K.
Uh
lm
a
n
n
,
“
Un
sc
e
n
ted
f
il
terin
g
a
n
d
n
o
n
li
n
e
a
r
e
sti
m
a
ti
o
n
,
”
in
Pro
c
e
e
d
i
n
g
s
o
f
th
e
IEE
E
,
v
o
l.
9
2
,
n
o
.
3
,
p
p
.
4
0
1
-
4
2
2
,
M
a
r.
2
0
0
4
.
[5
]
M
.
S
t
-
P
ierre
a
n
d
D.
G
in
g
ra
s,
“
Co
m
p
a
riso
n
b
e
tw
e
e
n
th
e
u
n
sc
e
n
ted
Ka
l
m
a
n
f
il
ter
a
n
d
th
e
e
x
ten
d
e
d
K
a
lma
n
f
il
ter
f
o
r
th
e
p
o
si
ti
o
n
e
stim
a
ti
o
n
m
o
d
u
le
o
f
a
n
in
teg
ra
ted
n
a
v
ig
a
ti
o
n
in
f
o
rm
a
ti
o
n
sy
ste
m
,
”
IEE
E
In
tel
l
ig
e
n
t
Ve
h
icle
s
S
y
mp
o
si
u
m,
2
0
0
4
,
P
a
rm
a
,
Italy
,
p
p
.
8
3
1
-
8
3
5
,
2
0
0
4
.
[6
]
H
.
Ha
rian
to
,
M
.
Riv
a
i,
D.
P
u
rw
a
n
to
,
“
Im
p
le
m
e
n
tatio
n
o
f
El
e
c
tro
n
ic No
se
in
Om
n
i
-
d
irec
ti
o
n
a
l
R
o
b
o
t
,
”
In
ter
n
a
ti
o
n
a
l
J
o
u
rn
a
l
o
f
El
e
c
trica
l
a
n
d
C
o
mp
u
t
e
r E
n
g
i
n
e
e
rin
g
(
IJ
ECE
)
,
v
o
l.
3
,
n
o
.
3
,
p
p
.
3
9
9
-
4
0
6
,
2
0
1
3
.
[7
]
M.
Ha
m
issi
a
n
d
K.
F
a
e
z
,
“
Re
a
l
-
T
i
m
e
Ha
n
d
G
e
stu
re
Re
c
o
g
n
it
io
n
Ba
se
d
o
n
th
e
De
p
th
M
a
p
f
o
r
Hu
m
a
n
Ro
b
o
t
In
tera
c
ti
o
n
,
”
I
n
ter
n
a
ti
o
n
a
l
J
o
u
r
n
a
l
o
f
El
e
c
trica
l
a
n
d
Co
m
p
u
ter
E
n
g
i
n
e
e
rin
g
(
IJ
ECE
)
,
v
o
l.
3
,
n
o
.
6
,
p
p
.
7
7
0
-
7
7
8
,
2
0
1
3
.
[8
]
O.
Ba
c
h
ir
a
n
d
A.
-
F.
Zo
u
b
ir
,
“
A
d
a
p
ti
v
e
N
e
u
ro
-
f
u
z
z
y
In
f
e
r
e
n
c
e
S
y
ste
m
Ba
se
d
Co
n
tro
l
o
f
p
u
m
a
6
0
0
Ro
b
o
t
M
a
n
ip
u
lato
r
,
”
In
ter
n
a
ti
o
n
a
l
J
o
u
r
n
a
l
o
f
El
e
c
trica
l
a
n
d
Co
m
p
u
ter
En
g
i
n
e
e
rin
g
(
IJ
ECE
)
,
v
o
l.
2
,
n
o
.
1
,
p
p
.
90
-
97
,
2
0
1
1
.
[9
]
B.
J.
S
tep
h
e
n
s
a
n
d
C.
G
.
A
tk
e
so
n
,
“
P
u
sh
Re
c
o
v
e
r
y
b
y
ste
p
p
in
g
f
o
r
h
u
m
a
n
o
id
ro
b
o
ts
w
it
h
fo
rc
e
c
o
n
tro
ll
e
d
jo
in
ts
,
”
2
0
1
0
1
0
th
IEE
E
-
R
AS
I
n
te
rn
a
ti
o
n
a
l
Co
n
fer
e
n
c
e
o
n
Hu
ma
n
o
id
R
o
b
o
ts
,
Na
sh
v
il
le,
T
N,
p
p
.
5
2
-
59
,
2
0
1
0
.
0
5
10
15
20
25
30
35
40
45
50
-
0
.
5
0
0
.
5
1
1
.
5
2
t
(
s
)
A
n
g
l
e
(
r
a
d
)
A
n
g
l
e
o
f
r
e
f
r
e
n
c
e
T
C
R
U
K
F
U
K
F
0
5
10
15
20
25
30
35
40
45
50
-
0
.
5
0
0
.
5
t
(
s
)
e
r
1
e
r
1
C
R
U
K
F
U
K
F
0
5
10
15
20
25
30
35
40
45
50
-
0
.
5
0
0
.
5
t
(
s
)
e
r
2
e
r
2
C
R
U
K
F
U
K
F
0
5
10
15
20
25
30
35
40
45
50
0
0
.
1
0
.
2
t
(
s
)
M
S
E
(
%
)
C
R
U
K
F
U
K
F
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
n
t J
E
lec
&
C
o
m
p
E
n
g
,
Vo
l.
10
,
No
.
5
,
Octo
b
e
r
2
0
2
0
:
5
2
4
3
-
5250
5250
[1
0
]
A
.
Re
z
a
e
e
a
n
d
R.
Af
sh
a
r
,
“
Dig
it
a
l
En
c
o
d
e
r
De
sig
n
in
g
f
o
r
M
o
b
il
e
Ro
b
o
t
Co
n
tro
l
,
”
In
ter
n
a
ti
o
n
a
l
J
o
u
rn
a
l
o
f
El
e
c
trica
l
a
n
d
C
o
mp
u
ter
En
g
in
e
e
rin
g
(
IJ
ECE
)
,
v
o
l.
4
,
n
o
.
5
,
p
p
.
6
4
8
-
6
5
7
,
2
0
1
4
.
[1
1
]
J.
S
a
h
u
,
B.
B.
C
h
o
u
d
h
u
ry
,
a
n
d
M.
R.
P
a
tra,
“
A
n
Eff
e
c
ti
v
e
D
e
v
e
lo
p
m
e
n
t
a
n
d
A
n
a
l
y
sis
o
f
a
M
o
b
i
le
Ro
b
o
t
,
”
In
ter
n
a
t
io
n
a
l
J
o
u
rn
a
l
o
f
E
lec
trica
l
a
n
d
C
o
mp
u
ter
En
g
in
e
e
rin
g
(
IJ
ECE
)
,
v
o
l.
6
,
n
o
.
2
,
p
p
.
6
5
4
-
6
6
5
,
2
0
1
6
.
[1
2
]
S.
Nu
rm
a
in
i
a
n
d
B.
T
u
tu
k
o
,
“
I
n
telli
g
e
n
t
Ro
b
o
ti
c
s
Na
v
ig
a
ti
o
n
S
y
st
e
m
:
P
ro
b
lem
s,
M
e
th
o
d
s,
a
n
d
A
lg
o
rit
h
m
,
”
In
ter
n
a
t
io
n
a
l
J
o
u
rn
a
l
o
f
E
lec
trica
l
a
n
d
C
o
mp
u
ter
En
g
in
e
e
rin
g
(
IJ
ECE
)
,
v
o
l
.
7
,
n
o
.
6
,
pp
.
3
7
1
1
-
3
7
2
6
,
2
0
1
7
.
[1
3
]
S.
M
.
M
a
lag
o
n
-
S
o
ld
a
ra
,
M
.
T
o
led
a
n
o
-
A
y
a
la,
G
.
S
o
to
-
Zara
z
u
a
,
R.
V
.
Ca
rril
l
o
-
S
e
rra
n
o
,
E
.
A
.
Riv
a
s
-
A
ra
i
z
a
,
“
M
o
b
il
e
Ro
b
o
t
L
o
c
a
li
z
a
ti
o
n
:
A
Re
v
ie
w
o
f
P
ro
b
a
b
il
isti
c
M
a
p
-
Ba
se
d
T
e
c
h
n
iq
u
e
s
,”
IAE
S
I
n
ter
n
a
t
io
n
a
l
J
o
u
r
n
a
l
o
f
R
o
b
o
ti
c
s
a
n
d
Au
to
ma
ti
o
n
(
IJ
RA
)
,
v
o
l.
4
,
n
o
.
1
,
p
p.
73
-
81
,
2
0
1
5
.
[1
4
]
S
.
J.
Ju
li
e
r
a
n
d
J.
K.
U
h
lm
a
n
n
,
“
Re
d
u
c
e
d
sig
m
a
p
o
in
t
f
il
ters
f
o
r
th
e
p
ro
p
a
g
a
ti
o
n
o
f
m
e
a
n
s
a
n
d
c
o
v
a
rian
c
e
s
th
ro
u
g
h
n
o
n
li
n
e
a
r
tran
sf
o
rm
a
ti
o
n
s,
”
Pro
c
e
e
d
in
g
s
o
f
t
h
e
2
0
0
2
Ame
ric
a
n
Co
n
tro
l
Co
n
fer
e
n
c
e
(
IEE
E
Ca
t.
No
.
CH3
7
3
0
1
)
,
A
n
c
h
o
ra
g
e
,
A
K,
USA
,
v
o
l.
2
,
p
p
.
8
8
7
-
8
9
2
,
2
0
0
2
.
[1
5
]
A
.
Ch
a
h
m
i,
M
.
Be
n
d
je
b
b
a
r,
B
.
Ra
iso
n
,
a
n
d
M
.
Be
n
b
o
u
z
id
,
“
A
n
Ex
ten
d
e
r
Ka
l
m
a
n
F
il
ter
-
b
a
se
d
In
d
u
c
ti
o
n
M
a
c
h
in
e
s
F
a
u
lt
s
De
tec
ti
o
n
,”
I
n
ter
n
a
ti
o
n
a
l
J
o
u
r
n
a
l
o
f
El
e
c
trica
l
a
n
d
Co
m
p
u
ter
En
g
i
n
e
e
rin
g
(
IJ
ECE
)
,
v
o
l.
6
,
n
o
.
2
,
p
p
.
5
3
5
-
548
,
2
0
1
6
.
[1
6
]
L
.
F
.
De
M
e
lo
a
n
d
J.
F
.
M
.
Ju
n
io
r,
“
T
ra
jec
to
r
y
P
lan
n
i
n
g
f
o
r
No
n
h
o
lo
n
o
m
ic
M
o
b
il
e
R
o
b
o
t
Us
in
g
E
x
ten
d
e
d
Ka
lm
a
n
F
il
ter,
”
M
a
th
e
ma
ti
c
a
l
Pro
b
lem
s in
En
g
i
n
e
e
rin
g
,
v
o
l.
2
0
1
0
,
p
p
.
1
-
2
2
,
2
0
1
0
.
[1
7
]
J.
Ch
a
n
d
ra
se
k
a
r,
A
.
J.
Rid
le
y
a
n
d
D.
S
.
Be
rn
ste
in
,
“
A
Co
m
p
a
riso
n
o
f
th
e
Ex
ten
d
e
d
a
n
d
U
n
sc
e
n
ted
Ka
l
m
a
n
F
il
ters
f
o
r
Disc
re
te
-
T
i
m
e
S
y
ste
m
s
w
it
h
No
n
d
if
fe
re
n
ti
a
b
le
Dy
n
a
m
i
c
s,
”
2
0
0
7
Ame
ric
a
n
C
o
n
tr
o
l
Co
n
fer
e
n
c
e
,
Ne
w
Yo
rk
,
NY
,
p
p
.
4
4
3
1
-
4
4
3
6
,
2
0
0
7
.
[1
8
]
R.
V
a
n
d
e
r
M
e
rw
e
a
n
d
E.
A.
W
a
n
,
“
T
h
e
sq
u
a
re
-
ro
o
t
u
n
sc
e
n
ted
Ka
l
m
a
n
f
il
ter
f
o
r
sta
te
a
n
d
p
a
ra
m
e
t
e
r
-
e
sti
m
a
ti
o
n
,
”
2
0
0
1
IEE
E
In
ter
n
a
ti
o
n
a
l
Co
n
fer
e
n
c
e
o
n
Ac
o
u
stics
,
S
p
e
e
c
h
,
a
n
d
S
ig
n
a
l
Pro
c
e
ss
in
g
.
Pr
o
c
e
e
d
in
g
s
(
Ca
t.
No
.
0
1
CH3
7
2
2
1
)
,
S
a
lt
L
a
k
e
Cit
y
,
UT
,
US
A
,
v
o
l.
6
,
p
p
.
3
4
6
1
-
3
4
6
4
,
2
0
0
1
.
[1
9
]
Z
.
L
iu
,
e
t
a
l
.
,
“
A
n
im
p
ro
v
e
d
sq
u
a
re
ro
o
t
u
n
sc
e
n
ted
Ka
lm
a
n
f
il
ter
f
o
r
p
ro
jec
ti
le'
s
a
tt
it
u
d
e
d
e
term
in
a
ti
o
n
,
”
2
0
1
0
5
t
h
IEE
E
Co
n
fer
e
n
c
e
o
n
In
d
u
stri
a
l
El
e
c
tro
n
ics
a
n
d
A
p
p
l
ica
ti
o
n
s
,
T
a
ich
u
n
g
,
p
p
.
1
7
4
7
-
1
7
5
1
,
2
0
1
0
.
[2
0
]
E
.
A
.
W
a
n
,
R
.
v
a
n
d
e
r
M
e
rwe
,
a
n
d
A
.
T
.
Ne
lso
n
,
“
Du
a
l
Esti
m
a
t
io
n
a
n
d
t
h
e
Un
sc
e
n
ted
T
ra
n
sf
o
rm
a
ti
o
n
,
”
Ne
u
ra
l
In
fo
rm
a
t
io
n
Pro
c
e
ss
in
g
S
y
ste
ms
,
p
p
.
6
6
6
-
672
,
2
0
0
0
.
[2
1
]
J.
A
n
d
ra
d
e
-
Ce
tt
o
,
T
.
V
i
d
a
l
-
Ca
ll
e
ja
a
n
d
A
.
S
a
n
f
e
li
u
,
“
Un
s
c
e
n
ted
T
ra
n
sf
o
r
m
a
ti
o
n
o
f
V
e
h
icle
S
tate
s
i
n
S
LA
M
,
”
Pro
c
e
e
d
in
g
s
o
f
th
e
2
0
0
5
IEE
E
In
ter
n
a
ti
o
n
a
l
C
o
n
fer
e
n
c
e
o
n
Ro
b
o
ti
c
s
a
n
d
Au
t
o
ma
t
io
n
,
B
a
rc
e
lo
n
a
,
S
p
a
i
n
,
p
p
.
3
2
3
-
328
,
2
0
0
5
.
[2
2
]
S
.
S
a
d
h
u
,
e
t
a
l
.,
“
Be
a
rin
g
o
n
ly
t
ra
c
k
in
g
u
sin
g
sq
u
a
re
ro
o
t
sig
m
a
p
o
in
t
Ka
lma
n
f
il
ter,
”
Pro
c
e
e
d
in
g
s
o
f
th
e
IEE
E
INDICO
N
2
0
0
4
.
Fi
rs
t
In
d
ia
An
n
u
a
l
Co
n
fer
e
n
c
e
,
Kh
a
ra
g
p
u
r,
I
n
d
ia,
p
p
.
6
6
-
69
,
2
0
0
4
.
[2
3
]
R.
Ka
n
d
e
p
u
,
B.
F
o
ss
,
a
n
d
L
.
I
m
sla
n
d
,
“
A
p
p
l
y
in
g
th
e
u
n
sc
e
n
ted
Ka
lm
a
n
f
il
ter
f
o
r
n
o
n
li
n
e
a
r
sta
te
e
sti
m
a
ti
o
n
,
”
J
o
u
r
n
a
l
o
f
Pro
c
e
ss
Co
n
tro
l
,
v
o
l
.
1
8
,
iss
u
e
7
-
8
,
p
p
.
7
5
3
-
7
6
8
,
2
0
0
8
.
[2
4
]
M
.
Rh
u
d
y
,
e
t
a
l
.,
“
Ev
a
lu
a
ti
o
n
o
f
M
a
tri
x
S
q
u
a
re
Ro
o
t
Op
e
ra
ti
o
n
s
fo
r
UK
F
w
it
h
in
a
U
A
V
G
P
S
/INS
S
e
n
so
r
F
u
si
o
n
A
p
p
li
c
a
ti
o
n
,
”
I
n
ter
n
a
t
io
n
a
l
J
o
u
r
n
a
l
o
f
Na
v
i
g
a
ti
o
n
a
n
d
O
b
se
rv
a
ti
o
n
,
v
o
l.
2
0
1
1
,
p
p
.
1
-
1
1
,
2
0
1
1
.
[2
5
]
M
.
Rh
u
d
y
,
e
t
a
l
.,
“
S
e
n
siti
v
it
y
a
n
d
Ro
b
u
stn
e
ss
A
n
a
l
y
sis
o
f
EKF
a
n
d
UK
F
De
sig
n
P
a
ra
m
e
ter
s
f
o
r
G
P
S
/INS
S
e
n
so
r
F
u
sio
n
,
”
AIA
A
J
o
u
r
n
a
l
o
f
Aer
o
sp
a
c
e
In
fo
rm
a
ti
o
n
S
y
ste
ms
,
v
o
l.
1
0
,
n
o
.
3
,
p
p
.
1
3
1
-
1
4
3
,
2
0
1
3
.
Evaluation Warning : The document was created with Spire.PDF for Python.