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.
7
,
No
.
5
,
Octo
b
e
r
2
0
1
7
,
p
p
.
2
5
9
6
~
2
6
0
4
I
SS
N:
2
0
8
8
-
8708
,
DOI
: 1
0
.
1
1
5
9
1
/
i
j
ec
e
.
v7
i
5
.
p
p
2
5
9
6
-
2604
2596
J
o
ur
na
l ho
m
ep
a
g
e
:
h
ttp
:
//ia
e
s
jo
u
r
n
a
l.c
o
m/o
n
lin
e/in
d
ex
.
p
h
p
/I
JE
C
E
Inertia
l
Na
v
ig
a
tion for
Q
ua
dro
tor
Using
K
a
l
m
a
n
Fil
ter
w
ith
Drift Co
m
pensa
ti
o
n
L
a
s
m
a
di
,
Ad
ha
Ca
hy
a
di
,
Sa
m
ia
dji
H
er
djuna
nto
,
Ris
a
nu
ri
H
ida
y
a
t
De
p
a
rt
m
e
n
t
o
f
El
e
c
tri
c
a
l
En
g
in
e
e
rin
g
a
n
d
I
n
f
o
rm
a
ti
o
n
T
e
c
h
n
o
lo
g
y
,
F
a
c
u
lt
y
o
f
En
g
in
e
e
rin
g
Un
iv
e
rsitas
G
a
d
jah
M
a
d
a
,
Yo
g
y
a
k
a
rta,
In
d
o
n
e
sia
Art
icle
I
nfo
AB
ST
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
Oct
25
,
2
0
1
6
R
ev
i
s
ed
A
p
r
1
0
,
2
0
1
7
A
cc
ep
ted
J
u
n
2
6
,
2
0
1
7
T
h
e
m
a
in
d
isa
d
v
a
n
tag
e
o
f
a
n
In
e
rti
a
l
Na
v
ig
a
ti
o
n
S
y
ste
m
is
a
lo
w
a
c
c
u
ra
c
y
d
u
e
to
n
o
ise
,
b
ias
,
a
n
d
d
r
if
t
e
rro
r
in
th
e
in
e
rt
ial
se
n
so
r.
T
h
is
re
se
a
r
c
h
a
ims
to
d
e
v
e
lo
p
th
e
a
c
c
e
lero
m
e
ter
a
n
d
g
y
ro
sc
o
p
e
se
n
so
r
f
o
r
q
u
a
d
r
o
t
o
r
n
a
v
ig
a
ti
o
n
s
y
ste
m
,
b
ias
c
o
m
p
e
n
sa
t
io
n
,
a
n
d
Z
e
ro
V
e
l
o
c
it
y
Co
m
p
e
n
sa
t
io
n
(Z
V
C
).
Ka
lma
n
F
il
ter
is
d
e
sig
n
e
d
t
o
re
d
u
c
e
t
h
e
n
o
ise
o
n
t
h
e
se
n
so
r
w
h
il
e
b
ias
c
o
m
p
e
n
sa
t
io
n
a
n
d
Z
V
C
a
re
d
e
sig
n
e
d
to
e
li
m
in
a
t
e
th
e
b
ias
a
n
d
d
rif
t
e
rro
r
in
th
e
se
n
so
r
d
a
ta
.
T
e
st
re
su
lt
s
sh
o
w
e
d
th
e
Ka
lma
n
F
il
ter
d
e
sig
n
is
a
c
c
e
p
tab
le
to
r
e
d
u
c
e
th
e
n
o
ise
i
n
th
e
se
n
s
o
r
d
a
ta
.
M
o
re
o
v
e
r
,
th
e
b
ias
c
o
m
p
e
n
sa
t
io
n
a
n
d
ZV
C
c
a
n
re
d
u
c
e
th
e
d
r
if
t
e
rro
r
d
u
e
to
in
teg
ra
ti
o
n
p
r
o
c
e
ss
a
s
w
e
ll
a
s
i
m
p
ro
v
e
th
e
p
o
siti
o
n
e
stim
a
ti
o
n
a
c
c
u
ra
c
y
o
f
th
e
q
u
a
d
ro
t
o
r.
A
t
th
e
ti
m
e
o
f
t
e
stin
g
,
th
e
s
y
ste
m
p
ro
v
id
e
d
th
e
a
c
c
u
ra
c
y
a
b
o
v
e
9
0
%
w
h
e
n
it
tes
ted
i
n
d
o
o
r
.
K
ey
w
o
r
d
:
Dr
if
t c
o
m
p
e
n
s
atio
n
I
m
u
I
n
er
tial n
a
v
i
g
atio
n
Kal
m
a
n
f
ilter
Qu
ad
r
o
to
r
Co
p
y
rig
h
t
©
2
0
1
x
In
sti
tu
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
:
A
d
h
a
C
a
h
y
ad
i
,
Dep
ar
t
m
en
t o
f
E
lectr
ical
E
n
g
i
n
ee
r
in
g
an
d
I
n
f
o
r
m
at
io
n
T
ec
h
n
o
lo
g
y
,
Facu
lt
y
o
f
E
n
g
i
n
ee
r
i
n
g
,
J
ln
Gr
af
i
k
a
2
,
Un
iv
er
s
itas
Gad
j
ah
Ma
d
a
,
Yo
g
y
a
k
ar
ta,
I
n
d
o
n
e
s
ia
.
E
m
ail:
ad
h
a.
i
m
a
m
@
u
g
m
.
ac
.
id
1.
I
NT
RO
D
UCT
I
O
N
T
h
e
q
u
ad
r
o
to
r
is
o
n
e
o
f
U
n
m
an
n
ed
Air
cr
af
t
S
y
s
te
m
(
U
A
S)
th
at
ca
n
f
l
y
a
u
to
n
o
m
o
u
s
l
y
o
r
r
e
m
o
te
l
y
co
n
tr
o
lled
[
1
]
.
R
ec
en
tl
y
,
q
u
ad
r
o
to
r
is
a
f
r
u
it
f
u
l
t
o
p
ic
o
f
i
n
te
r
est
i
n
m
an
y
r
e
s
ea
r
ch
ar
ea
s
,
i
n
clu
d
i
n
g
i
n
ci
v
ilia
n
an
d
m
ili
tar
y
.
Q
u
ad
r
o
to
r
d
o
es
n
o
t
n
ee
d
a
lar
g
e
p
lace
f
o
r
lan
d
i
n
g
,
it
ca
n
tak
e
o
f
f
v
er
ticall
y
an
d
ab
ilit
y
to
h
i
g
h
m
an
e
u
v
er
t
h
an
t
h
e
o
th
er
v
e
h
i
cle
[
2
]
,
[
3
]
.
T
h
is
m
a
k
es
U
A
S
o
r
UA
V
s
(
U
n
m
a
n
n
ed
A
er
ial
Veh
icle
s
)
h
a
s
b
ee
n
w
id
el
y
u
s
ed
i
n
v
ar
io
u
s
f
ield
s
o
f
b
o
th
m
il
itar
y
a
n
d
ci
v
ilia
n
.
I
n
t
h
e
m
ilit
ar
y
f
ield
,
U
AV
s
a
r
e
u
s
ed
to
co
m
b
at
o
p
er
atio
n
s
an
d
d
ec
is
io
n
-
m
a
k
i
n
g
,
i
n
tel
lig
e
n
ce
,
I
S
R
(
I
n
telli
g
en
t
S
u
r
v
ei
llan
ce
a
n
d
R
ec
o
n
n
a
is
s
a
n
ce
)
,
as
w
ell
a
s
at
m
o
s
p
h
er
ic
r
esear
c
h
[
4
]
.
I
n
ad
d
itio
n
,
UA
V
s
ar
e
u
s
ed
to
S
A
R
(
Sear
ch
a
n
d
R
esc
u
e)
,
s
u
p
er
v
is
io
n
r
ea
l
-
t
i
m
e,
r
ec
o
n
n
ais
s
an
ce
,
i
n
s
p
ec
tio
n
o
f
th
e
d
an
g
er
o
u
s
p
lace
[
1
]
,
tr
af
f
ic
m
o
n
ito
r
[
5
]
,
m
o
n
ito
r
o
f
n
atu
r
al
d
is
aster
s
[
6
]
,
p
est
an
d
d
is
ea
s
e
co
n
tr
o
l
[
7
]
,
i
n
s
p
ec
tio
n
o
f
elec
tr
i
ca
l
n
et
w
o
r
k
s
[
8
]
as
w
el
l
as
in
t
h
e
f
ield
o
f
m
eteo
r
o
lo
g
y
[
9
]
.
Qu
ad
r
o
to
r
ca
n
b
e
u
s
ed
ef
f
icie
n
tl
y
to
p
er
f
o
r
m
tas
k
s
th
a
t
ca
n
b
e
r
is
k
y
i
f
t
h
e
h
u
m
an
p
ilo
t
[
1
0
]
.
T
h
e
r
esear
ch
to
p
ic
s
of
UAV
s
ar
e
b
r
o
ad
f
r
o
m
m
o
d
eli
n
g
a
n
d
co
n
tr
o
l
[
1
1
]
–
[
1
3
]
,
n
av
ig
at
io
n
,
p
ath
p
lan
n
i
n
g
to
s
i
m
u
lta
n
eo
u
s
lo
ca
lizatio
n
a
n
d
m
ap
p
in
g
(
S
L
A
M)
[
1
4
]
,
an
d
m
u
c
h
m
o
r
e.
I
t
h
as
b
ee
n
s
ep
ar
atel
y
a
d
d
r
ess
e
d
b
y
r
e
s
ea
r
ch
er
s
i
n
ar
ea
s
o
f
r
o
b
o
tics
,
m
ec
h
atr
o
n
ic
s
,
co
m
p
u
ter
s
cie
n
ce
s,
etc
[
1
5
]
.
Nav
i
g
atio
n
s
y
s
te
m
s
tak
e
an
i
m
p
o
r
ta
n
t
r
o
le
of
th
e
U
A
V
s
y
s
t
e
m
.
T
o
b
e
ab
le
to
m
o
v
e
a
u
to
n
o
m
o
u
s
l
y
i
n
th
e
s
p
ec
i
f
ic
ar
ea
,
a
r
o
b
o
t
m
u
s
t
h
a
v
e
th
e
ab
ilit
y
to
d
eter
m
i
n
e
its
o
w
n
p
o
s
itio
n
an
d
r
ea
ch
th
e
d
esti
n
atio
n
b
y
u
s
i
n
g
t
h
e
b
es
t
p
o
s
s
ib
le
p
ath
[
1
0
]
.
B
ef
o
r
e
r
ea
ch
in
g
t
h
e
d
e
s
ti
n
atio
n
,
a
q
u
ad
r
o
to
r
m
u
s
t
d
eter
m
i
n
e
it
s
p
o
s
itio
n
a
n
d
an
g
u
lar
o
r
ien
ta
tio
n
.
T
h
is
tech
n
iq
u
e
i
s
ca
lled
lo
ca
lizat
io
n
.
U
s
u
all
y
,
t
h
e
u
s
e
GP
S in
th
e
U
A
V
n
a
v
i
g
atio
n
s
y
s
te
m
ca
n
p
r
o
v
id
e
ac
c
u
r
ate
p
o
s
itio
n
m
ea
s
u
r
e
m
e
n
t
[
1
6
]
.
Ho
w
e
v
er
,
th
e
i
n
f
o
r
m
atio
n
f
r
o
m
GP
S
h
a
s
li
m
ita
tio
n
s
i
n
t
h
e
ce
r
tain
en
v
ir
o
n
m
e
n
t
th
a
t
led
to
th
e
s
ig
n
al
n
o
is
y
o
r
u
n
a
v
ail
ab
le,
s
u
ch
as
i
n
s
id
e
a
b
u
ild
in
g
[
1
7
]
–
[
1
9
]
,
in
th
e
u
r
b
an
en
v
ir
o
n
m
e
n
t
[
2
0
]
–
[
2
2
]
,
an
d
u
n
d
er
w
ater
[
2
3
]
.
I
n
th
e
en
v
ir
o
n
m
e
n
t
t
h
at
is
n
o
t
co
v
er
ed
b
y
GP
S,
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
E
C
E
I
SS
N:
2
0
8
8
-
8708
I
n
erti
a
l Na
vig
a
tio
n
fo
r
Qu
a
d
r
o
to
r
Usi
n
g
K
a
lma
n
F
ilter
w
ith
Dri
ft C
o
mp
en
s
a
tio
n
(
La
s
ma
d
i
)
2597
n
av
i
g
atio
n
o
r
lo
ca
lizatio
n
o
f
a
U
A
V
ca
n
o
n
l
y
d
ep
en
d
o
n
t
h
e
I
MU
(
I
n
er
tial
Me
a
s
u
r
e
m
en
t
U
n
it)
an
d
t
h
e
ex
ter
o
ce
p
tiv
e
s
e
n
s
o
r
s
u
ch
as t
h
e
laser
r
an
g
e
f
i
n
d
er
(
L
R
F)
an
d
th
e
ca
m
er
a
.
T
h
e
s
y
s
te
m
s
u
s
e
t
h
e
L
R
F
s
e
n
s
o
r
f
o
r
U
A
V
n
av
i
g
atio
n
s
y
s
te
m
h
as
b
ee
n
ca
r
r
ied
o
u
t
in
[
2
1
]
,
[
2
4
]
,
an
d
[
2
5
]
.
T
h
e
L
R
F
s
e
n
s
o
r
h
a
s
a
li
m
itatio
n
o
f
t
h
e
r
an
g
e
t
h
at
m
a
y
lead
to
less
ac
cu
r
at
e
p
o
s
itio
n
m
ea
s
u
r
e
m
e
n
t
[
1
9
]
.
T
h
is
ap
p
r
o
ac
h
o
n
l
y
w
o
r
k
s
in
t
h
e
s
tr
u
ct
u
r
ed
en
v
ir
o
n
m
e
n
t
[
2
6
]
.
T
h
en
,
n
a
v
i
g
atio
n
s
y
s
te
m
s
b
ased
o
n
th
e
ca
m
er
a
th
at
h
a
s
b
ee
n
r
ep
o
r
ted
in
[
2
]
,
[
1
9
]
,
[
2
2
]
,
an
d
[
2
7
]
,
h
av
e
g
o
o
d
ac
cu
r
ac
y
w
h
e
n
th
e
ca
m
er
a's
f
i
eld
o
f
v
ie
w
ch
a
n
g
e
s
r
elativ
el
y
s
lo
w
ly
[
2
0
]
.
T
h
e
ca
m
er
a
ca
n
b
e
u
s
ed
to
3
D
lo
ca
li
za
tio
n
,
b
u
t
th
e
y
m
o
r
e
s
lo
w
l
y
t
h
an
t
h
e
I
MU
[
2
8
]
.
I
n
ad
d
itio
n
,
t
h
e
s
y
s
te
m
b
a
s
ed
o
n
v
is
u
al
o
d
o
m
etr
y
ca
n
wo
r
k
ef
f
ec
tiv
e
l
y
w
h
e
n
it
i
n
an
en
v
ir
o
n
m
e
n
t
w
i
t
h
s
u
f
f
icie
n
t
li
g
h
ti
n
g
an
d
s
tatic
s
ce
n
er
y
w
ith
en
o
u
g
h
te
x
t
u
r
e
i
n
o
r
d
er
to
o
b
tain
clea
r
m
o
v
e
m
en
t
to
b
e
e
x
tr
ac
ted
[
2
3
]
.
I
n
g
en
er
al,
th
e
ca
m
er
a
-
b
ased
s
y
s
te
m
r
eq
u
ir
e
s
co
m
p
u
t
a
tio
n
ca
p
ab
ilit
y
h
ea
v
ier
b
ec
au
s
e
o
f
th
e
s
en
s
o
r
d
ata
to
b
e
p
r
o
ce
s
s
ed
[
2
]
,
[
2
9
]
,
s
o
it
w
il
l b
e
d
if
f
ic
u
lt to
be
i
m
p
le
m
e
n
t
ed
in
t
h
e
e
m
b
ed
d
ed
s
y
s
te
m
s
[
3
0
]
.
I
n
th
i
s
s
t
u
d
y
,
t
h
e
n
av
i
g
atio
n
s
y
s
te
m
b
ased
o
n
I
MU
s
e
n
s
o
r
w
it
h
Kal
m
a
n
Fi
lter
(
KF)
is
p
r
o
p
o
s
ed
.
A
n
I
MU
co
n
s
i
s
t
o
f
th
e
ac
ce
ler
o
m
eter
a
n
d
g
y
r
o
s
co
p
e
w
h
ic
h
tech
n
o
lo
g
y
b
ase
d
o
n
Mic
r
o
E
lectr
o
M
ec
h
an
ical
S
y
s
te
m
(
ME
MS)
.
T
h
e
ME
MS
tec
h
n
o
lo
g
y
h
a
s
t
h
e
ad
v
an
tag
e
o
f
s
m
all
s
ize,
li
g
h
t
w
ei
g
h
t,
lo
w
p
o
w
er
co
n
s
u
m
p
tio
n
,
an
d
h
i
g
h
r
esi
s
ta
n
ce
[
3
1
]
th
at
is
ab
le
to
p
r
o
v
id
e
r
ap
id
r
esp
o
n
s
e
[
2
8
]
.
T
h
e
I
MU
u
s
u
al
l
y
is
ap
p
lied
to
esti
m
at
e
t
h
e
o
r
ien
tatio
n
o
f
a
r
o
b
o
t
an
d
s
p
ac
ec
r
af
t.
Ho
w
ev
er
,
d
u
e
to
li
m
it
atio
n
p
er
f
o
r
m
an
ce
at
lo
w
co
s
t
-
MEMS
,
th
e
m
ea
s
u
r
e
m
e
n
t
ac
cu
r
a
c
y
o
f
p
o
s
itio
n
a
n
d
s
p
ee
d
w
ill
d
ec
r
ea
s
e
w
it
h
in
cr
ea
s
i
n
g
in
te
g
r
atio
n
ti
m
e
[
2
]
,
[
2
6
]
.
T
h
is
is
ca
u
s
ed
b
y
t
h
e
b
ias
an
d
th
e
d
r
if
t
er
r
o
r
in
th
e
I
MU
[
3
0
]
.
Kal
m
a
n
F
i
lter
h
a
s
b
ee
n
t
h
e
s
u
b
j
ec
t
of
ex
ten
s
i
v
e
ap
p
licatio
n
an
d
r
esear
c
h
,
esp
ec
ial
l
y
i
n
t
h
e
au
to
n
o
m
o
u
s
n
a
v
ig
a
tio
n
a
n
d
g
u
id
ed
n
a
v
i
g
atio
n
ar
ea
s
.
T
h
e
Kal
m
a
n
Fil
ter
p
er
f
o
r
m
s
well
in
p
r
ac
tice
an
d
attr
ac
tiv
e
i
n
t
h
eo
r
etica
l
b
ec
au
s
e
it
ca
n
m
i
n
i
m
ize
t
h
e
v
ar
ian
ce
o
f
t
h
e
e
s
ti
m
atio
n
er
r
o
r
[3
2]
.
T
h
e
s
m
a
ll
co
m
p
u
tatio
n
al
r
eq
u
ir
e
m
e
n
t,
r
ec
u
r
s
i
v
el
y
p
r
o
p
er
ties
,
an
d
s
tatu
s
as
o
p
tim
a
l
esti
m
ato
r
ar
e
th
e
g
r
ea
t
s
u
cc
e
s
s
o
f
th
e
Kal
m
a
n
Fil
ter
[
3
3
]
.
I
n
th
is
s
t
u
d
y
,
Kal
m
a
n
Fil
ter
i
s
d
esig
n
ed
to
r
ed
u
ce
th
e
n
o
is
e
o
n
t
h
e
s
e
n
s
o
r
.
Z
er
o
V
elo
cit
y
C
o
m
p
e
n
s
at
io
n
(
Z
VC
)
is
a
m
e
th
o
d
th
at
is
u
s
ed
to
elim
i
n
at
io
n
t
h
e
d
r
if
t
ef
f
ec
t
on
th
e
s
ig
n
al
d
ata
[
3
4
]
.
I
n
th
is
s
t
u
d
y
,
Z
V
C
i
s
d
esi
g
n
ed
to
r
ed
u
c
i
n
g
t
h
e
d
r
if
t
er
r
o
r
o
n
t
h
e
s
en
s
o
r
s
ig
n
al
d
ata
an
d
s
tatio
n
ar
y
d
etec
tio
n
o
f
t
h
e
q
u
ad
r
o
to
r
.
W
ith
Z
VC
,
t
h
e
s
en
s
o
r
s
ig
n
al
d
ata
w
o
u
ld
b
e
co
n
s
id
er
ed
to
ze
r
o
if
th
e
s
en
s
o
r
is
s
tatio
n
ar
y
e
v
e
n
t
h
o
u
g
h
n
o
t
eq
u
al
to
ze
r
o
.
I
d
ea
ll
y
,
if
t
h
e
ac
ce
ler
o
m
eter
a
n
d
g
y
r
o
s
co
p
e
ar
e
s
tatio
n
ar
y
,
th
e
v
elo
cit
y
o
u
tp
u
t
i
s
eq
u
al
to
ze
r
o
.
Ho
w
ev
er
,
t
h
e
f
ac
t
d
esp
i
te
th
e
s
e
n
s
o
r
d
o
es
n
o
t
m
o
v
e,
t
h
e
s
e
n
s
o
r
o
u
tp
u
t
is
n
o
t
eq
u
al
to
ze
r
o
.
T
h
is
ca
n
r
es
u
lt
i
n
a
cal
c
u
latio
n
er
r
o
r
in
d
e
ter
m
i
n
in
g
th
e
p
o
s
i
tio
n
a
n
d
d
ir
ec
tio
n
s
e
n
s
o
r
w
h
e
n
th
e
s
e
n
s
o
r
d
o
es n
o
t
m
o
v
e.
T
h
is
er
r
o
r
is
k
n
o
w
n
a
s
d
r
ift
.
T
h
e
co
n
tr
ib
u
tio
n
s
o
f
th
i
s
s
tu
d
y
ar
e
t
h
e
d
ev
elo
p
m
e
n
t
o
f
t
h
e
alg
o
r
ith
m
f
o
r
esti
m
atio
n
r
o
tat
io
n
al
an
d
tr
an
s
latio
n
al
d
is
p
lace
m
e
n
t
o
f
th
e
f
l
y
in
g
r
o
b
o
t
b
ased
o
n
I
MU
s
en
s
o
r
,
esp
ec
iall
y
f
o
r
t
h
e
q
u
ad
r
o
to
r
.
T
h
e
p
r
o
p
o
s
ed
alg
o
r
ith
m
also
ca
n
co
m
p
e
n
s
ate
th
e
d
r
if
t
er
r
o
r
in
th
e
g
y
r
o
s
co
p
e
s
i
g
n
al
d
ata
.
T
h
e
b
en
e
f
it
s
o
f
th
i
s
r
esear
ch
p
r
o
v
id
e
a
m
et
h
o
d
f
o
r
h
an
d
lin
g
th
e
n
o
is
e
o
f
th
e
I
MU
s
en
s
o
r
to
i
m
p
r
o
v
e
th
e
ac
cu
r
ac
y
o
f
n
a
v
i
g
atio
n
d
ata
an
d
co
n
tr
ib
u
t
e
o
f
d
ev
elo
p
m
e
n
t
Kal
m
an
Fi
lter
as
n
a
v
ig
a
tio
n
s
tate
es
ti
m
ato
r
an
d
n
o
is
e
f
i
lter
o
n
th
e
ac
ce
ler
o
m
eter
a
n
d
g
y
r
o
s
co
p
e
s
en
s
o
r
.
I
n
g
e
n
er
al
u
s
ag
e
,
t
h
is
s
y
s
te
m
ca
n
p
r
o
v
id
e
an
a
lter
n
ativ
e
s
o
l
u
tio
n
to
a
lo
w
-
co
s
t
n
av
i
g
atio
n
as
t
h
e
n
a
v
ig
atio
n
a
l u
n
d
er
th
e
lack
o
f
GP
S si
g
n
al
.
2.
RE
S
E
ARCH
M
E
T
H
O
D
I
n
th
i
s
s
t
u
d
y
,
w
e
u
s
e
th
e
R
OS
(
R
o
b
o
t
Op
er
atin
g
S
y
s
te
m
)
to
g
et
d
ata
f
r
o
m
t
h
e
I
MU
s
en
s
o
r
th
at
r
u
n
n
i
n
g
o
n
t
h
e
L
i
n
u
x
Ub
u
n
t
u
o
p
er
atin
g
s
y
s
te
m
.
T
h
e
d
ata
at
th
is
m
o
m
e
n
t i
s
p
r
o
ce
s
s
ed
o
f
f
-
li
n
e.
T
h
e
illu
s
tr
at
i
on
o
f
an
g
u
lar
an
d
tr
an
s
lat
io
n
m
o
v
e
m
e
n
t
tes
t
is
s
h
o
w
n
i
n
Fi
g
u
r
e
1
.
T
h
e
s
ch
em
a
tic
o
f
ex
p
er
i
m
en
tal
s
et
u
p
an
d
d
ata
p
r
o
ce
s
s
in
g
ar
e
s
h
o
w
n
i
n
Fi
g
u
r
e
2
.
T
h
e
alg
o
r
ith
m
d
e
s
ig
n
i
n
cl
u
d
e
s
th
e
p
h
ase
s
a
s
f
o
llo
w
s
.
(
a
)
A
n
g
u
lar
m
o
v
e
m
e
n
t
(
r
o
ll)
(
b
)
T
r
an
s
latio
n
m
o
v
e
m
e
n
t
Fig
u
r
e
2
.
T
h
e
s
ch
e
m
atic
o
f
ex
p
er
im
e
n
tal
s
etu
p
a
n
d
d
ata
p
r
o
ce
s
s
in
g
Fig
u
r
e
1
.
I
llu
s
tr
atio
n
o
f
r
o
ll a
n
d
tr
an
s
latio
n
m
o
v
e
m
e
n
t te
s
t
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
J
E
C
E
Vo
l.
7
,
No
.
5
,
Octo
b
e
r
2
0
1
7
:
2
5
9
6
–
2
6
0
4
2598
2
.
1
.
M
ea
s
ure
m
e
nt
M
o
del o
f
Sens
o
r
a
nd
Ro
t
a
t
io
n M
a
t
rix
T
h
e
A
cc
eler
o
m
eter
i
s
a
s
e
n
s
o
r
m
ea
s
u
r
es
t
h
e
li
n
ea
r
ac
ce
ler
atio
n
o
f
v
eh
icles.
I
n
r
ea
lit
y
,
t
h
e
ac
ce
ler
o
m
eter
s
e
n
s
o
r
n
o
t
o
n
l
y
m
ea
s
u
r
e
s
t
h
e
li
n
ea
r
ac
ce
ler
ati
o
n
b
u
t
also
t
h
e
g
r
a
v
it
y
ac
ce
le
r
atio
n
.
T
h
e
g
r
a
v
it
y
ac
ce
ler
atio
n
is
m
ea
s
u
r
ed
b
y
th
e
s
en
s
o
r
w
ill
i
n
ter
f
er
e
t
h
e
m
ea
s
u
r
e
m
en
t
r
es
u
lts
.
T
h
er
ef
o
r
e,
t
h
e
m
ea
s
u
r
e
m
e
n
t
o
f
th
e
ac
ce
ler
o
m
eter
ca
n
b
e
m
o
d
eled
as:
=
̃
−
+
+
,
(
1
)
w
h
er
e
is
t
h
e
ac
ce
ler
o
m
eter
r
ea
d
in
g
i
n
th
e
b
o
d
y
f
r
a
m
e,
̃
is
th
e
ac
tu
al
ac
ce
ler
atio
n
,
is
th
e
g
r
av
it
y
co
n
s
ta
n
t
,
is
th
e
ac
ce
ler
o
m
e
ter
b
ias
,
an
d
is
th
e
n
o
is
e
i
n
ac
ce
ler
o
m
eter
.
On
th
e
o
th
er
h
an
d
,
th
e
m
ea
s
u
r
e
m
e
n
t o
f
a
ME
MS
g
y
r
o
s
co
p
e
ca
n
b
e
m
o
d
eled
as:
Ω
=
Ω
̃
+
+
,
(
2
)
w
h
er
e
Ω
is
t
h
e
g
y
r
o
s
co
p
e
r
ea
d
in
g
in
t
h
e
b
o
d
y
f
r
a
m
e,
Ω
̃
is
t
h
e
a
ctu
al
a
n
g
u
lar
v
elo
cit
y
,
an
d
ar
e
th
e
g
y
r
o
s
co
p
e
b
ias an
d
th
e
n
o
is
e
i
n
g
y
r
o
s
co
p
e
r
esp
ec
tiv
el
y
.
T
h
e
s
ig
n
al
d
ata
m
ea
s
u
r
ed
b
y
t
h
e
s
en
s
o
r
is
th
e
d
ata
in
th
e
b
o
d
y
co
o
r
d
in
ate
(
b
o
d
y
f
r
a
m
e)
.
T
h
er
ef
o
r
e,
t
h
e
r
o
tatio
n
m
a
tr
ix
n
ee
d
s
to
b
e
tr
an
s
f
o
r
m
e
d
f
r
o
m
b
o
d
y
f
r
a
m
e
to
n
a
v
i
g
atio
n
f
r
a
m
e
(
g
lo
b
al
f
r
a
m
e)
.
T
o
tr
an
s
f
o
r
m
t
h
e
m
ea
s
u
r
e
m
e
n
t d
ata
f
r
o
m
b
o
d
y
f
r
a
m
e
to
n
av
ig
at
i
o
n
f
r
a
m
e
ca
n
b
e
ca
r
r
ied
o
u
t a
s
f
o
llo
w
s
.
=
[
]
,
(
3
)
W
h
er
e
=
[
]
,
=
[
−
+
+
−
−
]
,
an
d
=
[
]
.
T
h
e
tr
an
s
f
o
r
m
at
io
n
m
atr
ix
is
u
s
ed
to
co
n
v
er
t
f
r
o
m
t
h
e
s
e
n
s
o
r
f
r
a
m
e
to
g
lo
b
al
f
r
a
m
e,
is
th
e
ac
ce
ler
atio
n
in
t
h
e
b
o
d
y
f
r
a
m
e
,
is
th
e
ac
ce
ler
atio
n
i
n
t
h
e
g
lo
b
al
f
r
a
m
e
,
w
i
th
i
is
t
h
e
ax
is
i
n
d
ex
(
i
ϵ
{
x
,
y
,
z
}
)
,
c
r
ep
r
esen
ts
co
s
i
n
e,
s
r
ep
r
esen
ts
s
in
u
s
,
θ
is
t
h
e
r
o
ll
an
g
le
,
is
t
h
e
p
itch
a
n
g
le
,
an
d
is
th
e
y
a
w
an
g
le
.
2
.
2
.
K
a
l
m
a
n
F
ilte
r
Desig
n f
o
r
G
y
ro
s
co
pe
Da
t
a
I
n
th
is
s
t
u
d
y
,
t
h
e
Kal
m
a
n
F
il
ter
is
u
s
ed
to
r
ed
u
c
e
t
h
e
n
o
i
s
e
o
f
g
y
r
o
s
co
p
e
s
ig
n
al
.
T
h
e
g
y
r
o
s
co
p
e
s
ig
n
al
is
t
h
e
an
g
u
lar
v
elo
cit
y
d
ata
o
f
ea
ch
ax
is
.
Hen
ce
,
t
h
e
an
g
u
lar
v
elo
cit
y
d
ata
is
u
s
ed
t
o
in
p
u
t
th
e
Kal
m
a
n
Fil
ter
f
o
r
o
b
ta
in
in
g
th
e
o
r
ien
ta
tio
n
an
g
le
o
f
th
e
q
u
ad
r
o
to
r
.
T
h
e
p
r
o
ce
s
s
m
o
d
el
an
d
m
ea
s
u
r
e
m
en
t
m
o
d
el
o
f
th
e
Kal
m
a
n
Fil
ter
f
o
r
g
y
r
o
s
co
p
e
s
en
s
o
r
ca
n
b
e
ex
p
r
ess
ed
i
n
eq
u
atio
n
(
4
)
an
d
(
5
)
as
,
+
1
=
+
+
,
(
4
)
ɀ
=
+
,
(
5
)
w
it
h
=
[
1
−
0
1
]
,
=
[
0
]
,
an
d
=
[
1
0
]
,
I
n
th
e
ab
o
v
e
eq
u
at
io
n
,
is
th
e
p
r
o
ce
s
s
ed
s
tate
in
t
i
m
e
k
,
is
th
e
s
tate
tr
an
s
itio
n
m
atr
i
x
to
i
-
ax
i
s
,
is
t
h
e
co
n
tr
o
l
m
atr
ix
to
i
-
ax
is
,
is
t
h
e
i
n
p
u
t
v
ec
to
r
,
an
d
is
t
h
e
p
r
o
ce
s
s
n
o
is
e
.
T
h
en
,
ɀ
is
th
e
m
e
asu
r
e
m
e
n
t
m
o
d
el,
is
th
e
m
ea
s
u
r
e
m
e
n
t
m
atr
i
x
to
i
-
ax
is
,
an
d
is
th
e
m
ea
s
u
r
e
m
en
t
n
o
is
e
f
o
r
g
y
r
o
s
co
p
e
s
ig
n
a
l
.
T
is
a
s
a
m
p
li
n
g
ti
m
e
d
ata
an
d
i
is
th
e
ax
is
in
d
ex
(
i
ϵ
{
x,
y
,
z
}
).
Fo
r
t
h
is
s
t
u
d
y
,
th
e
i
n
p
u
t
v
ec
to
r
s
is
th
e
r
a
w
d
ata
o
f
th
e
g
y
r
o
s
co
p
e
s
en
s
o
r
of
ea
ch
a
x
is
af
ter
it
ca
lib
r
atio
n
.
Kal
m
a
n
Fi
lter
ass
u
m
ed
t
h
e
p
r
o
ce
s
s
n
o
i
s
e
an
d
m
ea
s
u
r
e
m
en
t
n
o
is
e
u
n
co
r
r
elate
d
ea
ch
o
t
h
er
,
w
i
th
th
ei
r
av
er
ag
e
v
al
u
es
ar
e
ze
r
o
.
T
h
e
p
r
o
ce
s
s
n
o
is
e
co
v
ar
ian
ce
an
d
m
ea
s
u
r
e
m
e
n
t
n
o
is
e
co
v
ar
ian
c
e
R
a
r
e
ex
p
r
ess
ed
as
,
=
(
.
)
=
,
(
6
)
=
(
.
)
=
(
)
.
(
7
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
E
C
E
I
SS
N:
2
0
8
8
-
8708
I
n
erti
a
l Na
vig
a
tio
n
fo
r
Qu
a
d
r
o
to
r
Usi
n
g
K
a
lma
n
F
ilter
w
ith
Dri
ft C
o
mp
en
s
a
tio
n
(
La
s
ma
d
i
)
2599
B
y
ca
lc
u
lati
n
g
all
ax
e
s
,
th
e
m
a
tr
ices
A
,
C
,
,
an
d
R
of
th
e
Ka
l
m
an
Fil
ter
f
o
r
g
y
r
o
s
co
p
e
s
en
s
o
r
d
ata
ar
e
as f
o
llo
w
s
:
=
[
0
2
0
2
0
2
0
2
0
2
0
2
]
,
=
[
0
1
×
2
0
1
×
2
0
1
×
2
0
1
×
2
0
1
×
2
0
1
×
2
]
,
=
[
1
0
2
0
2
0
2
1
0
2
0
2
0
2
1
]
,
=
[
0
0
0
0
0
0
]
,
w
h
er
e
0
2
is
2
×
2
ze
r
o
m
atr
ix
,
0
1
×
2
is
1
×
2
ze
r
o
m
atr
i
x
.
2
.
3
.
K
a
l
m
a
n F
ilte
r
Desig
n f
o
r
Acc
eler
o
m
et
er
Da
t
a
As
in
th
e
g
y
r
o
s
co
p
e
,
th
e
Kalm
an
F
ilter
is
also
u
s
ed
to
r
ed
u
c
e
th
e
n
o
is
e
o
f
ac
ce
ler
o
m
eter
s
ig
n
al
.
T
h
e
ac
ce
ler
o
m
eter
d
ata
is
t
h
e
li
n
ea
r
ac
ce
ler
atio
n
o
f
ea
ch
ax
is
.
T
h
e
p
r
o
ce
s
s
m
o
d
el
a
n
d
m
ea
s
u
r
e
m
en
t
m
o
d
el
o
f
th
e
Kal
m
a
n
Fil
ter
f
o
r
ac
ce
ler
o
m
e
t
er
s
en
s
o
r
ca
n
b
e
ex
p
r
ess
ed
i
n
eq
u
atio
n
(
7
)
an
d
(
8
)
as,
+
1
=
+
+
ϛ
,
(
8
)
ʑ
=
+
,
(
9
)
w
h
er
e
=
[
1
0
0
1
0
−
0
.
5
2
−
1
]
,
=
[
0
.
5
2
0
]
,
an
d
=
[
1
0
0
]
.
I
n
th
e
ab
o
v
e
eq
u
atio
n
s
,
is
t
h
e
p
r
o
ce
s
s
s
tate
i
n
ti
m
e
k
,
is
t
h
e
s
tate
tr
a
n
s
itio
n
m
atr
ix
to
i
-
ax
is
,
F
i
is
t
h
e
co
n
tr
o
l
m
atr
ix
to
i
-
ax
i
s
,
u
k
is
th
e
in
p
u
t
v
ec
to
r
,
an
d
ϛ
is
t
h
e
p
r
o
ce
s
s
n
o
is
e
.
T
h
en
,
ʑ
is
t
h
e
m
ea
s
u
r
e
m
en
t
m
o
d
el,
H
i
is
t
h
e
m
ea
s
u
r
e
m
e
n
t
m
atr
i
x
to
i
-
a
x
i
s
,
an
d
th
e
m
e
asu
r
e
m
en
t
n
o
is
e
f
o
r
ac
ce
ler
o
m
eter
s
i
g
n
al
.
T
is
ti
m
e
s
a
m
p
li
n
g
d
ata
an
d
i
is
a
n
in
d
ex
a
x
is
(
i
ϵ
{
x,
y,
z
})
.
A
s
in
th
e
g
y
r
o
s
en
s
o
r
,
th
e
in
p
u
t
v
ec
to
r
is
th
e
r
a
w
d
ata
of
th
e
ac
ce
ler
o
m
eter
s
e
n
s
o
r
f
o
r
ea
ch
ax
is
a
i
af
ter
it
ca
lib
r
at
ed
,
th
en
th
e
o
f
f
s
et
is
r
ed
u
ce
d
.
T
h
e
p
r
o
ce
s
s
n
o
is
e
co
v
ar
ia
n
ce
an
d
th
e
m
ea
s
u
r
e
m
e
n
t
n
o
is
e
co
v
ar
ian
ce
R
ar
e
ex
p
r
ess
ed
as
:
=
(
ϛ
.
ϛ
)
=
,
(
10
)
=
(
.
)
=
(
)
.
(
1
1
)
B
y
ca
lcu
lati
n
g
all
ax
es
,
th
e
m
atr
ices
D
,
H
,
,
an
d
R
o
n
Kalm
an
Fil
ter
f
o
r
ac
ce
ler
o
m
eter
s
e
n
s
o
r
d
ata
ar
e
s
et
as f
o
llo
w
s
:
=
[
0
2
0
2
0
2
0
2
0
2
0
2
]
,
=
[
0
1
×
2
0
1
×
2
0
1
×
2
0
1
×
2
0
1
×
2
0
1
×
2
]
,
=
[
1
0
2
0
2
0
2
1
0
2
0
2
0
2
1
]
,
=
[
0
0
0
0
0
0
]
,
w
h
er
e
0
2
is
2
×
2
ze
r
o
m
atr
ix
,
0
1
×
2
is
1
×
2
ze
r
o
m
atr
i
x
.
2
.
3
.
Z
er
o
Velo
cit
y
Co
m
pen
s
a
t
io
n
Alg
o
rit
h
m
s
I
n
o
r
d
er
to
en
ab
le
t
h
e
s
e
n
s
o
r
au
to
m
at
icall
y
d
eter
m
in
e
s
t
h
e
co
n
d
itio
n
w
h
ile
at
r
e
s
t
,
w
e
co
m
p
ar
e
th
e
s
tan
d
ar
d
d
ev
iat
io
n
o
f
t
h
e
s
e
n
s
o
r
d
ata
(
)
w
it
h
i
n
a
n
n
-
s
ized
s
a
m
p
le
at
ti
m
e
k
w
it
h
a
t
h
r
es
h
o
ld
v
al
u
e
(
ℎ
)
as
d
escr
ib
ed
in
A
l
g
o
r
ith
m
1
.
If
t
h
e
s
tan
d
ar
d
d
ev
iatio
n
o
f
d
ata
is
les
s
t
h
a
n
ℎ
,
t
h
e
s
e
n
s
o
r
is
c
o
n
s
id
er
ed
in
th
e
r
est
co
n
d
itio
n
s
o
t
h
at
t
h
e
s
p
ee
d
is
ass
u
m
ed
ze
r
o
(
v
el
o
cit
y
=0
)
an
d
th
e
p
o
s
itio
n
i
s
as
s
u
m
ed
ze
r
o
(
p
o
s
itio
n
≈
0
)
.
I
f
th
e
s
tan
d
ar
d
d
ev
iatio
n
o
f
d
ata
is
n
o
t
less
t
h
a
n
ℎ
,
t
h
e
s
e
n
s
o
r
i
s
m
o
v
i
n
g
.
T
h
e
s
tatio
n
ar
y
d
etec
t
i
o
n
alg
o
r
it
h
m
ca
n
b
e
s
h
o
w
n
a
s
i
n
A
lg
o
r
it
h
m
1
[
3
4
]
.
I
n
th
e
i
n
er
tial
s
en
s
o
r
,
d
r
if
t
p
r
o
b
lem
i
s
a
p
r
o
b
lem
i
n
a
n
i
n
er
tial
n
av
i
g
atio
n
s
y
s
te
m
o
f
w
h
ic
h
th
e
co
m
p
u
ted
p
o
s
itio
n
m
o
v
e
s
ev
en
i
f
th
e
v
e
h
icle
d
o
es
n
o
t
m
o
v
e
.
T
h
is
p
h
en
o
m
en
o
n
o
cc
u
r
s
b
ec
au
s
e
o
f
t
h
e
in
te
g
r
atio
n
p
r
o
ce
s
s
o
f
t
h
e
s
en
s
o
r
s
ig
n
al
th
at
i
s
v
er
y
s
en
s
iti
v
e
to
th
e
n
o
i
s
e.
T
h
e
n
o
is
e
w
i
ll
p
r
o
p
ag
ate
to
th
e
p
o
s
itio
n
r
ap
id
ly
v
ia
t
h
e
i
n
teg
r
a
tio
n
p
r
o
ce
s
s
.
T
o
r
ed
u
ce
th
is
p
h
en
o
m
e
n
o
n
,
w
e
u
s
ed
th
e
Alg
o
r
i
th
m
2
.
First
ly
,
t
h
e
alg
o
r
it
h
m
w
i
ll
co
m
p
u
te
t
h
e
m
o
v
in
g
m
ea
n
b
y
cr
ea
t
in
g
s
er
ie
s
o
f
av
er
ag
e
s
o
f
d
i
f
f
er
en
t
s
u
b
s
ets
o
f
t
h
e
f
u
l
l
d
ata
s
et
.
I
n
t
h
i
s
s
t
u
d
y
,
m
o
v
in
g
m
ea
n
is
u
s
ed
to
s
m
o
o
t
h
o
u
t
s
h
o
r
t
-
t
er
m
f
lu
ct
u
atio
n
s
a
n
d
h
ig
h
li
g
h
t
lo
n
g
er
-
ter
m
tr
en
d
s
o
f
th
e
r
a
w
d
ata
.
T
o
co
m
p
en
s
a
te
th
e
b
ia
s
er
r
o
r
,
we
u
s
ed
t
h
e
m
ea
n
o
f
s
tatic
d
ata
.
I
f
t
h
e
m
o
v
in
g
m
ea
n
is
le
s
s
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
J
E
C
E
Vo
l.
7
,
No
.
5
,
Octo
b
e
r
2
0
1
7
:
2
5
9
6
–
2
6
0
4
2600
th
an
ze
r
o
,
th
e
v
elo
cit
y
is
ad
d
ed
w
i
th
t
h
e
m
e
an
o
f
t
h
e
s
tatic
c
o
n
d
itio
n
d
ata.
T
h
e
n
,
i
f
t
h
e
m
o
v
in
g
m
ea
n
is
h
i
g
h
er
th
an
ze
r
o
,
th
e
v
e
lo
cit
y
i
s
s
u
b
tr
ac
ted
w
it
h
t
h
e
m
ea
n
o
f
t
h
e
s
ta
tic
co
n
d
itio
n
da
ta.
Mo
r
eo
v
er
,
t
o
id
en
tify
t
h
e
ze
r
o
v
elo
cit
y
,
w
e
co
m
p
ar
e
t
h
e
v
elo
cit
y
w
it
h
t
h
e
th
r
es
h
o
ld
.
I
f
th
e
v
elo
cit
y
i
s
les
s
th
a
n
t
h
e
th
r
es
h
o
ld
,
th
e
v
elo
cit
y
i
s
co
n
s
id
er
ed
ze
r
o
.
W
e
also
u
s
ed
th
e
li
n
ea
r
f
itti
n
g
to
co
m
p
en
s
a
te
th
e
li
n
ea
r
tr
en
d
o
f
t
h
e
s
i
g
n
a
l
d
ata
d
u
e
to
m
a
n
y
f
ac
to
r
s
s
u
ch
a
s
te
m
p
er
at
u
r
e
.
A
l
g
o
r
ith
m
1
.
Statio
n
ar
y
d
etec
t
io
n
1.
=
−
1
;
2.
=
−
1
+
ω
;
3.
if
(
k
)
<
ℎ
then
4.
=
0
;
5.
=
−
1
;
6.
e
lse
7.
=
−
1
;
8.
=
−
1
+
ω
;
9
.
end if
A
l
g
o
r
it
hm
2
.
Z
er
o
V
elo
cit
y
C
o
m
p
e
n
s
at
io
n
(
Z
VC
)
1.
=
1
∑
−
1
−
1
=
0
;
2.
if
<
0
then
3.
=
+
|
0
|
;
4.
elseif
>
0
then
5
.
=
−
|
0
|
;
6
.
e
nd
7
.
if
<
ℎ
then
8
.
=
0
;
9
.
end if
I
n
th
e
ab
o
v
e
A
l
g
o
r
ith
m
,
M
m
is
th
e
m
o
v
in
g
m
ea
n
,
n
is
d
ata
s
a
m
p
le
(
in
th
i
s
s
t
u
d
y
,
n
=3
0
)
,
is
th
e
v
elo
cit
y
d
ata
,
0
is
th
e
m
ea
n
o
f
th
e
s
tat
ic
co
n
d
itio
n
d
ata
,
th
is
th
e
t
h
r
esh
o
ld
,
k
is
t
h
e
ti
m
e
i
n
d
ex
,
an
d
T
is
t
h
e
s
a
m
p
li
n
g
ti
m
e
o
f
th
e
d
ata
s
e
n
s
o
r
.
Fu
r
th
er
m
o
r
e
, t
h
e
Kal
m
a
n
F
ilter
in
p
u
t
i
s
u
p
d
ated
as in
Alg
o
r
ith
m
2
.
3.
RE
SU
L
T
S
A
ND
D
I
SCU
SS
I
O
N
3
.
1
.
S
t
a
t
ic
T
est
T
h
e
test
s
ar
e
p
er
f
o
r
m
ed
at
t
h
e
s
e
n
s
o
r
o
n
t
h
e
q
u
ad
r
o
to
r
at
th
e
s
tatic
co
n
d
itio
n
as
(
x
,
y
,z
)
=
(
0
,
0
,0
)
m
w
it
h
r
o
tatio
n
a
n
g
le
i
s
f
i
x
ed
as
(
r
o
ll,p
itch
,
y
a
w
)
=
(
0
,
0
,
0
)
r
a
d
.
I
n
th
e
s
tatio
n
ar
y
o
r
a
s
tatic
co
n
d
itio
n
,
id
ea
ll
y
,
th
e
s
ig
n
al
d
ata
f
r
o
m
t
h
e
acc
eler
o
m
eter
an
d
g
y
r
o
s
co
p
e
w
il
l
s
h
o
w
ze
r
o
.
T
h
e
test
s
ar
e
co
n
d
u
ct
ed
b
y
co
m
p
ar
in
g
t
h
e
s
en
s
o
r
s
ig
n
al
b
ef
o
r
e
an
d
af
ter
f
ilter
i
n
g
a
n
d
co
m
p
en
s
at
io
n
i
s
ad
d
ed
.
Fig
u
r
e
3
(
a)
s
h
o
w
s
t
h
e
g
y
r
o
s
c
o
p
e
s
ig
n
al
(
r
a
w
d
ata
)
o
f
th
e
X
,
Y,
a
n
d
Z
a
xe
s
as
th
e
q
u
ad
r
o
to
r
at
th
e
s
tatic
co
n
d
itio
n
.
T
h
e
s
p
ik
es
s
h
o
w
t
h
at
th
e
s
i
g
n
al
is
n
o
is
y
d
u
e
to
f
o
r
ce
,
s
en
s
o
r
d
y
n
a
m
ic
s
,
an
d
o
th
er
s
.
T
h
e
m
i
n
i
m
u
m
a
n
d
m
a
x
i
m
u
m
a
m
p
l
itu
d
e
o
f
th
e
s
i
g
n
al
i
n
t
h
e
X
ax
is
is
-
0
.
0
0
2
0
an
d
0
.
0
0
2
6
;
in
th
e
Y
ax
is
i
s
-
0
.
0
0
6
7
an
d
0
.
0
0
6
1
;
an
d
in
th
e
Z
ax
i
s
i
s
-
0
.
0
0
6
0
an
d
0
.
0
0
6
9
,
in
u
n
its
o
f
r
ad
/s
f
o
r
all.
Fi
g
u
r
e
3
(
b
)
s
h
o
w
s
th
e
g
y
r
o
s
co
p
e
d
ata
af
ter
f
ilter
i
n
g
an
d
co
m
p
en
s
atio
n
.
I
d
ea
ll
y
,
at
t
h
e
s
tat
i
c
co
n
d
itio
n
,
t
h
e
s
ig
n
al
a
m
p
li
tu
d
e
is
ze
r
o
.
T
h
e
a
m
p
lit
u
d
e
o
f
th
e
s
i
g
n
a
l
s
i
n
th
e
X
,
Y,
a
n
d
Z
a
x
e
s
ar
e
ze
r
o
.
Mo
r
eo
v
er
,
th
e
s
p
i
k
e
n
o
is
e
h
a
s
b
ee
n
r
e
m
o
v
ed
f
r
o
m
th
e
s
i
g
n
al
s
.
T
h
is
s
h
o
w
s
t
h
at
t
h
e
alg
o
r
ith
m
is
ab
le
to
re
m
o
v
e
t
h
e
s
i
g
n
a
l
n
o
is
e
i
n
t
h
e
s
tatio
n
ar
y
co
n
d
itio
n
.
(
a)
T
h
e
g
y
r
o
s
co
p
e
s
ig
n
al
b
ef
o
r
e
f
ilter
i
n
g
(
b
)
T
h
e
g
y
r
o
s
co
p
e
s
ig
n
al
af
ter
f
ilter
i
n
g
Fig
u
r
e
3
.
T
h
e
g
y
r
o
s
co
p
e
s
ig
n
a
l
o
f
s
tatic
co
n
d
itio
n
Fig
u
r
e
4
(
a)
s
h
o
w
s
t
h
e
ac
ce
ler
o
m
eter
s
i
g
n
al
(
r
a
w
d
ata)
o
f
th
e
X
,
Y,
an
d
Z
ax
e
s
as
t
h
e
q
u
ad
r
o
to
r
at
s
tatic
co
n
d
itio
n
.
T
h
e
s
i
g
n
al
s
o
f
t
h
e
t
h
r
ee
ax
es
ar
e
n
o
t
eq
u
al
ze
r
o
.
Mo
r
e
o
v
er
,
th
er
e
is
D
C
v
alu
e
(
o
f
f
s
et)
.
T
h
e
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-5
0
5
x
1
0
-3
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
o
f
t
h
e
a
x
i
s
-
X
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-
0
.
0
1
0
0
.
0
1
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
o
f
t
h
e
a
x
i
s
-
Y
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-
0
.
0
1
0
0
.
0
1
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
o
f
t
h
e
a
x
i
s
-
Z
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
v
-
r
o
l
l
v
-
p
i
t
c
h
v
-
y
a
w
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-1
0
1
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
o
f
t
h
e
a
x
i
s
-
X
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-1
0
1
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
o
f
t
h
e
a
x
i
s
-
Y
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-1
0
1
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
o
f
t
h
e
a
x
i
s
-
Z
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
v
-
r
o
l
l
v
-
p
i
t
c
h
v
-
y
a
w
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
E
C
E
I
SS
N:
2
0
8
8
-
8708
I
n
erti
a
l Na
vig
a
tio
n
fo
r
Qu
a
d
r
o
to
r
Usi
n
g
K
a
lma
n
F
ilter
w
ith
Dri
ft C
o
mp
en
s
a
tio
n
(
La
s
ma
d
i
)
2601
a
m
p
lit
u
d
e
o
f
th
e
s
i
g
n
al
i
n
t
h
e
Z
ax
i
s
is
h
i
g
h
er
t
h
an
t
h
e
a
m
p
li
tu
d
e
o
f
th
e
s
ig
n
al
i
n
th
e
X
a
n
d
Y
ax
es
b
ec
au
s
e
o
f
th
e
Z
a
x
is
m
ea
s
u
r
e
th
e
ac
ce
le
r
atio
n
f
o
r
ce
t
h
at
i
s
a
f
f
ec
ted
b
y
g
r
av
i
t
y
.
I
d
ea
ll
y
,
t
h
e
a
m
p
lit
u
d
e
o
f
t
h
e
s
ig
n
al
d
ata
i
n
t
h
e
Z
ax
i
s
is
r
ea
d
9
.
8
1
2
m
/
s
2
.
T
h
e
ac
ce
ler
o
m
eter
r
ea
d
in
g
i
n
t
h
e
X
a
n
d
Y
ax
e
s
ar
e
n
o
t
z
er
o
co
u
ld
b
e
d
u
e
to
th
e
i
m
p
r
o
p
er
p
er
p
en
d
icu
lar
o
f
th
e
s
e
n
s
o
r
to
th
e
ea
r
t
h
f
r
a
m
e
.
A
t
t
h
e
s
ta
tio
n
ar
y
,
id
ea
ll
y
,
t
h
e
a
m
p
lit
u
d
e
o
f
th
e
s
ig
n
al
i
n
th
e
X
,
Y,
an
d
Z
a
x
e
s
ar
e
ze
r
o
.
Fig
u
r
e
4
(
b
)
s
h
o
w
s
th
e
ac
ce
ler
o
m
e
ter
d
ata
af
ter
f
ilter
i
n
g
a
n
d
co
m
p
e
n
s
at
io
n
.
W
e
ca
n
s
ee
th
e
s
ig
n
al
a
m
p
lit
u
d
e
o
f
t
h
e
X
an
d
Y
ax
e
s
ar
e
ze
r
o
an
d
th
e
Z
ax
is
is
9
.
8
1
2
m
/s
2
.
Mo
r
e
o
v
er
,
th
e
b
i
as
(
o
f
f
s
et)
c
an
b
e
co
m
p
e
n
s
ated
an
d
th
e
n
o
is
e
ca
n
b
e
r
em
o
v
ed
.
T
h
is
s
h
o
w
s
t
h
e
f
ilter
a
n
d
co
m
p
e
n
s
at
io
n
ca
n
w
o
r
k
q
u
ite
w
ell
s
o
th
e
n
o
is
e
an
d
b
ias er
r
o
r
d
u
e
to
g
r
av
it
y
ca
n
b
e
eli
m
in
a
ted
.
(
a)
T
h
e
ac
ce
ler
o
m
eter
s
i
g
n
al
b
ef
o
r
e
f
ilter
i
n
g
(
b
)
T
h
e
ac
ce
ler
o
m
eter
s
i
g
n
a
l
af
ter
f
i
lter
in
g
Fig
u
r
e
4
.
T
h
e
a
cc
eler
o
m
eter
s
i
g
n
al
o
f
s
tatic
co
n
d
itio
n
3
.
2
.
Ang
ula
r
M
o
v
em
e
nt
T
es
t
A
n
g
u
lar
m
o
v
e
m
e
n
t
te
s
t
ai
m
s
to
test
t
h
e
al
g
o
r
ith
m
o
n
t
h
e
g
y
r
o
s
co
p
e
s
en
s
o
r
.
T
h
e
alg
o
r
ith
m
esti
m
ate
s
th
e
an
g
u
lar
m
o
v
e
m
en
t
(
r
o
ll,
p
itch
,
an
d
y
a
w
)
in
th
e
g
lo
b
al
f
r
a
m
e.
T
h
e
an
g
u
lar
m
o
v
e
m
e
n
t
t
est
is
s
tar
ted
w
h
e
n
th
e
s
en
s
o
r
in
a
s
tat
i
o
n
ar
y
(
n
o
t
m
o
v
in
g
)
.
I
n
th
i
s
co
n
d
it
io
n
,
t
h
e
g
y
r
o
s
co
p
e
s
i
g
n
al
i
s
r
ec
o
r
d
ed
as
th
e
s
tat
io
n
ar
y
d
ata
.
W
h
ile
at
s
tatio
n
ar
y
,
t
h
e
q
u
ad
r
o
to
r
is
r
o
tated
alo
n
g
o
n
e
ce
r
tain
a
x
is
as
i
s
ill
u
s
tr
ated
in
Fig
u
r
e
1
(
a
)
.
T
h
en
,
th
e
g
y
r
o
s
co
p
e
s
ig
n
al
is
r
ec
o
r
d
ed
as d
ata
o
f
th
e
an
g
u
lar
m
o
v
e
m
en
t
.
Fig
u
r
e
5
(
a)
s
h
o
w
s
th
e
g
y
r
o
s
co
p
e
s
ig
n
al
w
h
e
n
i
t
is
r
o
tate
d
ab
o
u
t
th
e
X
a
x
is
.
T
h
e
s
ig
n
al
s
ar
e
t
h
e
an
g
u
lar
v
elo
cit
y
o
f
t
h
e
X,
Y,
an
d
Z
ax
e
s
.
T
h
e
s
ig
n
al
o
f
t
h
e
X
a
x
is
s
h
o
w
s
h
ig
h
er
t
h
an
o
th
er
s
b
ec
au
s
e
t
h
e
q
u
ad
r
o
to
r
is
r
o
tated
ab
o
u
t
th
e
X
ax
is
w
h
i
le
t
h
e
a
n
g
u
lar
v
el
o
cit
y
o
f
t
h
e
Y
a
n
d
Z
a
x
es
id
ea
ll
y
clo
s
e
to
ze
r
o
.
Fig
u
r
e
5
(
b
)
s
h
o
w
s
t
h
e
r
o
ll
an
g
le
esti
m
at
io
n
r
es
u
lts
.
T
h
e
d
o
tted
lin
e
cu
r
v
e
s
h
o
w
s
t
h
e
test
r
es
u
lt
s
b
y
K
F
(
w
it
h
o
u
t
co
m
p
en
s
atio
n
)
w
h
ile
th
e
s
o
lid
li
n
e
c
u
r
v
e
s
h
o
w
s
th
e
test
r
es
u
lts
b
y
KF
-
Z
V
C
(
w
it
h
co
m
p
e
n
s
at
io
n
)
.
I
n
Fig
u
r
e
5
(
b
)
,
th
e
r
o
ll
a
n
g
le
esti
m
at
io
n
r
esu
lts
b
y
KF
ca
n
b
e
s
ee
n
to
h
a
v
e
d
r
if
t
er
r
o
r
w
h
ile
b
y
KF
-
Z
V
C
r
elativ
el
y
h
a
s
n
o
d
r
if
t e
r
r
o
r
.
I
t
s
h
o
w
s
t
h
at
t
h
e
alg
o
r
it
h
m
is
ab
l
e
co
m
p
e
n
s
ate
d
r
if
t e
r
r
o
r
in
th
e
g
y
r
o
s
co
p
e
d
ata
.
(
a)
T
h
e
g
y
r
o
s
co
p
e
s
ig
n
al
o
f
r
o
ll
m
o
v
e
m
e
n
t
(
b
)
T
h
e
r
o
ll e
s
ti
m
atio
n
s
ig
n
al
0
200
400
600
800
1000
1200
1400
1600
1800
2000
0
0
.
2
0
.
4
L
i
n
e
a
r
a
c
c
e
l
e
r
a
t
i
o
n
o
n
t
h
e
a
x
i
s
-
X
S
a
m
p
l
i
n
g
A
c
c
e
l
.
(
m
/
s
2
)
m
e
a
s
u
r
e
m
e
n
t
-
a
x
0
200
400
600
800
1000
1200
1400
1600
1800
2000
0
0
.
5
1
L
i
n
e
a
r
a
c
c
e
l
e
r
a
t
i
o
n
o
n
t
h
e
a
x
i
s
-
Y
S
a
m
p
l
i
n
g
A
c
c
e
l
.
(
m
/
s
2
)
m
e
a
s
u
r
e
m
e
n
t
-
a
y
0
200
400
600
800
1000
1200
1400
1600
1800
2000
0
5
10
L
i
n
e
a
r
a
c
c
e
l
e
r
a
t
i
o
n
o
n
t
h
e
a
x
i
s
-
Z
S
a
m
p
l
i
n
g
A
c
c
e
l
.
(
m
/
s
2
)
m
e
a
s
u
r
e
m
e
n
t
-
a
z
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-1
0
1
L
i
n
e
a
r
a
c
c
e
l
e
r
a
t
i
o
n
o
n
t
h
e
a
x
i
s
-
X
S
a
m
p
l
i
n
g
A
c
c
e
l
.
(
m
/
s
2
)
0
200
400
600
800
1000
1200
1400
1600
1800
2000
-1
0
1
L
i
n
e
a
r
a
c
c
e
l
e
r
a
t
i
o
n
o
n
t
h
e
a
x
i
s
-
Y
S
a
m
p
l
i
n
g
A
c
c
e
l
.
(
m
/
s
2
)
0
200
400
600
800
1000
1200
1400
1600
1800
2000
8
10
12
L
i
n
e
a
r
a
c
c
e
l
e
r
a
t
i
o
n
o
n
t
h
e
a
x
i
s
-
Z
S
a
m
p
l
i
n
g
A
c
c
e
l
.
(
m
/
s
2
)
E
s
t
i
m
a
t
i
o
n
-
A
x
E
s
t
i
m
a
t
i
o
n
-
A
y
E
s
t
i
m
a
t
i
o
n
-
A
z
0
500
1000
1500
2000
2500
-5
0
5
10
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
0
500
1000
1500
2000
2500
-2
-1
0
1
2
E
s
t
i
m
a
t
i
o
n
r
o
l
l
a
n
g
l
e
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
)
v
-
r
o
l
l
v
-
p
i
t
c
h
v
-
y
a
w
w
i
t
h
c
o
m
p
e
n
s
a
t
i
o
n
w
i
t
h
o
u
t
c
o
m
p
e
n
s
a
t
i
o
n
0
500
1000
1500
2000
2500
-5
0
5
10
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
v
-
r
o
l
l
v
-
p
i
t
c
h
v
-
y
a
w
0
500
1000
1500
2000
2500
-2
-1
0
1
2
E
s
t
i
m
a
t
i
o
n
r
o
l
l
a
n
g
l
e
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
)
w
i
t
h
c
o
m
p
e
n
s
a
t
i
o
n
w
i
t
h
o
u
t
c
o
m
p
e
n
s
a
t
i
o
n
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
J
E
C
E
Vo
l.
7
,
No
.
5
,
Octo
b
e
r
2
0
1
7
:
2
5
9
6
–
2
6
0
4
2602
(
c)
T
h
e
g
y
r
o
s
co
p
e
s
ig
n
al
o
f
4
6
° r
o
llin
g
(
d
)
T
h
e
r
o
ll e
s
ti
m
atio
n
o
f
4
6
°
r
o
llin
g
Fig
u
r
e
5
.
T
h
e
g
y
r
o
s
co
p
e
s
ig
n
a
l
o
f
r
o
ll
m
o
v
e
m
en
t
test
W
h
en
t
h
e
s
e
n
s
o
r
is
r
o
tated
4
6
°
ab
o
u
t
th
e
X
ax
is
,
t
h
e
g
y
r
o
s
c
o
p
e
s
ig
n
al
is
s
h
o
w
n
i
n
Fi
g
u
r
e
5
(
c)
.
I
t
is
s
ee
n
t
h
at
t
h
e
s
i
g
n
al
o
f
th
e
X
a
x
is
s
h
o
w
s
h
i
g
h
er
t
h
an
o
t
h
er
s
b
ec
au
s
e
th
e
q
u
ad
r
o
to
r
is
r
o
tated
ab
o
u
t
th
e
X
ax
is
w
h
ile
t
h
e
s
i
g
n
al
alo
n
g
t
h
e
Y
an
d
Z
ax
e
s
ar
e
v
er
y
clo
s
e
to
ze
r
o
.
Fig
u
r
e
5
(
d
)
s
h
o
w
s
t
h
e
r
o
ll
an
g
le
esti
m
at
io
n
r
esu
lt
.
T
h
e
d
o
tted
lin
e
cu
r
v
e
s
h
o
w
s
t
h
e
esti
m
atio
n
r
es
u
lt
w
i
t
h
KF
(
w
it
h
o
u
t
co
m
p
e
n
s
atio
n
)
w
h
ile
t
h
e
s
o
lid
lin
e
cu
r
v
e
s
h
o
w
s
t
h
e
esti
m
atio
n
r
e
s
u
lt
w
i
th
KF
-
Z
V
C
(
w
ith
co
m
p
en
s
atio
n
)
.
T
h
e
esti
m
a
tio
n
r
es
u
lts
o
f
r
o
ll
an
g
le
b
y
KF
s
h
o
w
s
s
i
g
n
i
f
ica
n
t
d
r
if
t
er
r
o
r
w
h
ile
t
h
e
esti
m
atio
n
b
y
KF
-
Z
V
C
is
ab
l
e
to
r
ej
ec
t
d
r
if
t
er
r
o
r
in
th
e
s
ig
n
al.
P
lease
b
e
n
o
ted
th
at
t
h
e
d
r
if
t
er
r
o
r
is
s
tar
ted
at
iter
atio
n
1
6
0
.
I
n
th
is
iter
atio
n
,
t
h
e
r
o
ll
esti
m
atio
n
b
y
KF
-
Z
V
C
is
0
.
0
0
0
2
4
r
ad
(
0
.
0
0
1
4
°)
w
h
il
e
th
e
r
o
ll
esti
m
atio
n
b
y
KF
i
s
0
.
0
1
1
0
r
a
d
(
0
.
6
3
0
3
°).
A
t
iter
atio
n
s
6
4
5
,
th
e
r
o
ll
esti
m
atio
n
b
y
KF
-
Z
VC
is
0
.
0
1
0
0
r
ad
(
0
.
0
5
7
3
°)
w
h
ile
t
h
e
r
o
ll
esti
m
at
io
n
b
y
K
F
i
s
0
.
0
4
5
9
r
ad
(
2
.
6
2
9
9
°).
Fu
r
t
h
er
m
o
r
e
,
th
e
f
i
n
al
r
o
ll
e
s
ti
m
atio
n
b
y
KF
-
Z
V
C
i
s
0
.
8
3
6
1
r
ad
(
4
7
.
9
0
5
0
°)
w
h
i
le
th
e
f
i
n
al
r
o
ll
esti
m
a
tio
n
b
y
KF
is
0
.
8
8
7
7
r
ad
(
5
0
.
8
6
1
5
°)
o
f
a
n
g
le
r
ef
er
e
n
ce
4
6
°.
I
n
th
is
ca
s
e,
th
e
r
elat
iv
e
er
r
o
r
b
y
KF
-
Z
VC
(
w
it
h
co
m
p
e
n
s
at
io
n
)
i
s
1
.
9
0
5
0
°
w
h
i
le
b
y
K
F
(
w
it
h
o
u
t
co
m
p
e
n
s
ati
o
n
)
is
4
.
8
6
1
5
°.
I
t
s
h
o
w
s
t
h
e
a
lg
o
r
ith
m
is
ab
le
to
i
m
p
r
o
v
e
th
e
ac
c
u
r
ac
y
o
f
an
g
le
esti
m
a
tio
n
.
3.
3
.
T
ra
ns
la
t
io
n
M
o
v
e
m
ent
T
est
T
r
an
s
latio
n
m
o
v
e
m
en
t
tes
t
is
s
tar
ted
as
t
h
e
q
u
ad
r
o
to
r
’
s
at
s
t
atic
co
n
d
itio
n
(
n
o
t
m
o
v
i
n
g
)
.
T
h
en
,
f
r
o
m
th
e
s
tatic
co
n
d
it
io
n
(
as
in
it
ial
p
o
s
itio
n
(
x
,
y
)
=(
0
,
0
)
)
th
e
q
u
ad
r
o
to
r
is
m
o
v
ed
to
th
e
g
o
al
p
o
s
itio
n
i
n
co
o
r
d
in
ate
(
x
,
y
)
=(
2
,
1
)
m
as
i
s
illu
s
tr
ated
in
Fi
g
u
r
e
1
(
b
)
.
T
h
e
test
ai
m
s
to
test
o
f
th
e
al
g
o
r
ith
m
to
esti
m
ate
t
h
e
tr
an
s
latio
n
m
o
v
e
m
e
n
t
i
n
th
e
X
-
Y
d
ir
ec
tio
n
.
Fu
r
t
h
er
m
o
r
e
,
th
e
e
s
ti
m
ati
o
n
r
esu
lt
s
ar
e
v
al
id
ated
w
i
th
th
e
tr
u
e
d
i
s
tan
ce
to
d
eter
m
in
e
t
h
e
ac
cu
r
ac
y
o
f
t
h
e
alg
o
r
it
hm
.
Fig
u
r
e
6
(
a)
an
d
(
b
)
s
h
o
w
s
t
h
e
ac
ce
ler
o
m
eter
s
i
g
n
al
i
n
th
e
X
an
d
Y
ax
es
.
T
h
e
d
o
tted
lin
e
c
u
r
v
e
s
h
o
w
s
th
e
ac
ce
ler
o
m
eter
d
ata
b
ef
o
r
e
f
ilter
i
n
g
w
h
ile
t
h
e
s
o
l
id
lin
e
c
u
r
v
e
s
h
o
w
s
th
e
ac
ce
l
er
o
m
eter
d
ata
a
f
ter
f
ilter
i
n
g
a
n
d
co
m
p
en
s
atio
n
.
W
e
ca
n
s
ee
t
h
e
n
o
i
s
e
an
d
b
ias
er
r
o
r
in
th
e
s
i
g
n
al
b
ef
o
r
e
f
il
ter
in
g
.
A
f
ter
th
e
s
ig
n
al
is
d
o
n
e
t
h
e
b
ias co
m
p
e
n
s
a
tio
n
an
d
f
i
lter
in
g
,
t
h
e
b
ias er
r
o
r
an
d
n
o
is
e
ca
n
b
e
r
ed
u
ce
d
.
T
h
is
s
h
o
w
s
t
h
e
al
g
o
r
ith
m
ab
le
to
co
m
p
en
s
ate
th
e
b
ias
(
o
f
f
s
et
)
er
r
o
r
.
(
a)
A
cc
eler
atio
n
s
i
g
n
al
o
f
X
ax
is
(b
)
A
c
c
e
ler
a
ti
o
n
sig
n
a
l
o
f
Y
a
x
is
(
c)
Po
s
ito
n
esti
m
atio
n
o
f
q
u
ad
r
o
to
r
in
tr
an
s
latio
n
te
s
t
(
d
)
Qu
ad
r
o
to
r
tr
a
j
ec
to
r
y
o
n
2
D
p
lan
e
Fig
u
r
e
6
.
A
cc
eler
atio
n
s
i
g
n
al,
p
o
s
itio
n
esti
m
atio
n
,
an
d
q
u
ad
r
o
to
r
tr
a
j
ec
to
r
y
0
500
1000
1500
-
0
.
2
0
0
.
2
0
.
4
0
.
6
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
0
500
1000
1500
-
0
.
5
0
0
.
5
1
E
s
t
i
m
a
t
i
o
n
r
o
l
l
a
n
g
l
e
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
)
v
-
r
o
l
l
v
-
p
i
t
c
h
v
-
y
a
w
w
i
t
h
c
o
m
p
e
n
s
a
t
i
o
n
w
i
t
h
o
u
t
c
o
m
p
e
n
s
a
t
i
o
n
0
500
1000
1500
-
0
.
2
0
0
.
2
0
.
4
0
.
6
A
n
g
u
l
a
r
r
a
t
e
s
i
g
n
a
l
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
/
s
)
0
500
1000
1500
-
0
.
5
0
0
.
5
1
E
s
t
i
m
a
t
i
o
n
r
o
l
l
a
n
g
l
e
S
a
m
p
l
i
n
g
O
u
t
p
u
t
(
r
a
d
)
v
-
r
o
l
l
v
-
p
i
t
c
h
v
-
y
a
w
w
i
t
h
c
o
m
p
e
n
s
a
t
i
o
n
w
i
t
h
o
u
t
c
o
m
p
e
n
s
a
t
i
o
n
0
100
200
300
400
500
600
700
800
-2
-1
0
1
2
T
i
m
e
-
v
a
r
y
i
n
g
K
a
l
m
a
n
Fi
l
t
e
r
r
e
s
p
o
n
s
e
S
a
m
p
l
i
n
g
A
c
c
e
l
e
r
a
t
i
o
n
(
m
/
s
2
)
0
100
200
300
400
500
600
700
800
-
0
.
5
0
0
.
5
1
1
.
5
S
a
m
p
l
i
n
g
O
u
t
p
u
t
M
e
a
s
u
r
e
m
e
n
t
a
n
d
E
s
t
i
m
a
t
i
o
n
E
r
r
o
r
M
e
a
s
u
r
e
m
e
n
t
-
a
x
E
s
t
i
m
a
t
i
o
n
K
F-
A
x
M
e
a
s
.
e
r
r
o
r
-
X
E
s
t
.
E
r
r
o
r
-
X
0
100
200
300
400
500
600
700
800
-2
-1
0
1
2
T
i
m
e
-
v
a
r
y
i
n
g
K
a
l
m
a
n
Fi
l
t
e
r
r
e
s
p
o
n
s
e
S
a
m
p
l
i
n
g
A
c
c
e
l
e
r
a
t
i
o
n
(
m
/
s
2
)
0
100
200
300
400
500
600
700
800
-3
-2
-1
0
1
S
a
m
p
l
i
n
g
O
u
t
p
u
t
M
e
a
s
u
r
e
m
e
n
t
a
n
d
E
s
t
i
m
a
t
i
o
n
E
r
r
o
r
M
e
a
s
u
r
e
m
e
n
t
-
a
y
E
s
t
i
m
a
t
i
o
n
K
F-
A
y
M
e
a
s
.
e
r
r
o
r
-
Y
E
s
t
.
E
r
r
o
r
-
Y
0
100
200
300
400
500
600
700
800
-2
0
2
4
P
o
s
i
t
i
o
n
e
s
t
i
m
a
t
i
o
n
S
a
m
p
l
i
n
g
D
i
s
p
l
a
c
e
m
e
n
t
(
m
)
0
0
.
2
0
.
4
0
.
6
0
.
8
1
1
.
2
1
.
4
1
.
6
1
.
8
2
-
0
.
5
0
0
.
5
1
Q
u
a
d
r
o
t
o
r
t
r
a
j
e
c
t
o
r
y
X
-
p
l
a
n
e
(
m
)
Y
-
p
l
a
n
e
(
m
)
K
F-
X
(
x
1
)
K
F-
Y
(
x
1
)
L
P
F-
X
(
x
1
0
)
L
P
F-
Y
(
x
1
0
)
R
e
f
e
r
e
n
c
e
T
r
a
j
e
c
t
o
r
y
0
100
200
300
400
500
600
700
800
-2
0
2
4
P
o
s
i
t
i
o
n
e
s
t
i
m
a
t
i
o
n
S
a
m
p
l
i
n
g
D
i
s
p
l
a
c
e
m
e
n
t
(
m
)
0
0
.
2
0
.
4
0
.
6
0
.
8
1
1
.
2
1
.
4
1
.
6
1
.
8
2
-
0
.
5
0
0
.
5
1
Q
u
a
d
r
o
t
o
r
t
r
a
j
e
c
t
o
r
y
X
-
p
l
a
n
e
(
m
)
Y
-
p
l
a
n
e
(
m
)
K
F-
X
(
x
1
)
K
F-
Y
(
x
1
)
L
P
F-
X
(
x
1
0
)
L
P
F-
Y
(
x
1
0
)
R
e
f
e
r
e
n
c
e
T
r
a
j
e
c
t
o
r
y
x
=1
6
0
y
=0
.
0
1
1
0
x=
645
y
=0
.
0
459
x
=1
400
y
=0
.
8877
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
E
C
E
I
SS
N:
2
0
8
8
-
8708
I
n
erti
a
l Na
vig
a
tio
n
fo
r
Qu
a
d
r
o
to
r
Usi
n
g
K
a
lma
n
F
ilter
w
ith
Dri
ft C
o
mp
en
s
a
tio
n
(
La
s
ma
d
i
)
2603
Fig
u
r
e
6
(
c)
s
h
o
w
s
t
h
e
p
o
s
i
tio
n
es
ti
m
a
tio
n
r
e
s
u
l
t
s
o
f
t
h
e
q
u
ad
r
o
to
r
.
T
h
e
test
i
s
p
er
f
o
r
m
ed
b
y
co
m
p
ar
i
n
g
th
e
esti
m
atio
n
r
es
u
lt
s
b
y
L
P
F
(
lo
w
p
ass
f
i
lter
)
an
d
KF
(
Kal
m
a
n
Fi
lter
)
w
it
h
co
m
p
e
n
s
at
io
n
.
T
h
e
ex
p
er
i
m
e
n
ts
w
er
e
p
er
f
o
r
m
ed
in
d
o
o
r
w
it
h
7
13
d
ata
s
a
m
p
li
n
g
o
f
0
.
0
1
s
ec
o
n
d
s
r
ate
.
T
h
e
c
u
r
v
e
o
f
L
P
F
-
X
a
n
d
L
P
F
-
Y
s
h
o
w
t
h
e
test
r
esu
lt
s
b
y
L
P
F
i
n
t
h
e
X
a
x
is
d
ir
ec
tio
n
an
d
i
n
t
h
e
Y
ax
is
d
ir
ec
tio
n
r
es
p
ec
tiv
el
y
w
h
ile
th
e
cu
r
v
e
o
f
KF
-
X
an
d
KF
-
Y
s
h
o
w
th
e
test
r
es
u
lt
s
b
y
K
F
i
n
th
e
X
ax
is
d
ir
ec
tio
n
an
d
i
n
t
h
e
Y
ax
is
d
ir
ec
tio
n
r
esp
ec
tiv
el
y
.
T
h
e
f
i
n
al
p
o
s
itio
n
alo
n
g
t
h
e
X
d
ir
ec
tio
n
b
y
KF
is
1
.
9
6
2
5
m
w
h
ile
b
y
L
P
F
is
3
5
.
8
5
1
0
m
,
w
it
h
th
e
tr
u
e
p
o
s
itio
n
,
is
2
m
.
T
h
e
f
i
n
al
p
o
s
itio
n
alo
n
g
t
h
e
Y
d
ir
ec
tio
n
b
y
KF
i
s
0
.
9624
m
w
h
ile
b
y
L
P
F
is
1
7
.
9
7
7
4
m
,
w
it
h
th
e
tr
u
e
p
o
s
itio
n
,
i
s
1
m
.
B
ased
o
n
e
x
p
er
i
m
e
n
ts
i
n
d
o
o
r
in
t
h
i
s
ca
s
e
,
t
h
e
K
F
g
av
e
s
ig
n
i
f
ican
t
i
m
p
r
o
v
e
m
e
n
t
in
ac
cu
r
a
c
y
t
h
a
n
th
e
L
P
F
.
T
h
is
s
h
o
w
s
t
h
e
alg
o
r
it
h
m
d
esi
g
n
ab
le
to
i
m
p
r
o
v
e
th
e
ac
cu
r
ac
y
o
f
th
e
p
o
s
itio
n
esti
m
atio
n
r
es
u
lt
s
.
F
ig
u
r
e
6
(
d
)
s
h
o
w
s
t
h
e
tr
aj
ec
to
r
y
p
lo
t
o
f
th
e
q
u
ad
r
o
to
r
o
n
th
e
X
-
Y
p
la
n
e
b
y
KF
.
As
ca
n
b
e
s
ee
n
,
th
er
e
is
a
d
if
f
er
e
n
ce
b
et
w
ee
n
th
e
r
esu
lt
s
tr
aj
ec
to
r
y
an
d
th
e
r
ef
er
en
ce
.
T
h
is
is
ca
u
s
ed
b
y
t
he
s
e
n
s
o
r
is
n
o
t
ex
ac
tl
y
m
o
v
ed
fi
t
w
it
h
t
h
e
tr
u
e
test
i
n
g
b
ec
au
s
e
th
er
e
i
s
n
o
a
cc
u
r
ate
test
eq
u
ip
m
en
t
.
Fre
q
u
en
c
y
s
a
m
p
lin
g
m
a
y
b
e
less
f
ast,
s
o
th
e
d
ata
ca
n
n
o
t
r
ep
r
esen
t
t
h
e
ac
tu
al
ac
ce
ler
atio
n
ac
cu
r
atel
y
.
I
n
ad
d
itio
n
,
th
e
d
ata
ch
ar
ac
ter
is
tic
s
o
f
ac
ce
ler
o
m
et
er
s
en
s
o
r
m
a
y
b
e
n
o
t
li
n
ea
r
o
r
n
o
t
co
n
s
i
s
te
n
t
a
n
d
t
h
e
d
ata
o
f
ea
ch
s
e
n
s
o
r
a
x
is
i
s
n
o
t
r
ea
ll
y
i
n
d
ep
en
d
en
t
.
T
ab
le
1
s
h
o
w
s
t
h
e
p
o
s
itio
n
es
ti
m
atio
n
t
est
o
f
th
e
X
-
Y
d
ir
e
ctio
n
m
o
v
e
m
en
t
b
y
v
ar
iatio
n
s
R
an
d
Q
.
T
h
e
co
v
(
a
i
)
r
ep
r
esen
t
s
t
h
e
ac
ce
ler
atio
n
d
ata
co
v
ar
ian
ce
o
f
2
0
0
d
ata
s
am
p
le
(
a
x
f
o
r
th
e
X
ax
is
a
n
d
a
y
f
o
r
th
e
Y
ax
is
)
w
h
ile
th
e
Q
v
al
u
e
is
a
n
u
m
b
er
is
d
eter
m
i
n
ed
in
t
u
iti
v
e
ly
.
As
s
h
o
w
n
in
T
ab
le
1
,
b
y
th
e
R
v
al
u
e
i
s
1
0
ti
m
es
co
v
(
a
i
)
w
h
ile
th
e
Q
v
al
u
e
i
s
0
.
0
0
6
1
7
is
o
b
tai
n
ed
th
e
KF
-
X
ac
cu
r
ac
y
(
t
h
e
X
d
ir
ec
tio
n
)
an
d
KF
-
Y
ac
cu
r
ac
y
(
t
h
e
Y
d
ir
ec
tio
n
)
ar
e
98.
12
%
a
n
d
9
6
.2
4
%
r
e
s
p
ec
tiv
el
y
w
it
h
t
h
e
r
o
o
t
m
ea
n
s
s
q
u
ar
e
er
r
o
r
(
R
MSE
)
ar
e
0
.
0
0
1
4
m
an
d
0
.
0
0
1
4
m
r
esp
ec
tiv
el
y
.
T
ab
le
1
.
P
o
s
itio
n
esti
m
atio
n
with
KF
f
o
r
X
-
Y
m
o
v
e
m
e
n
t
tes
t
Q
=β
R
T
r
u
e
X
(
m)
T
r
u
e
Y
(
m)
Est
i
m
a
t
i
o
n
KF
-
X
(
m)
Est
i
m
a
t
i
o
n
KF
-
Y
(
m)
R
M
S
E
KF
-
X
(
m)
R
M
S
E
KF
-
Y
(
m)
A
c
c
u
r
.
KF
-
X
(
%)
A
c
c
u
r
.
KF
-
Y
(
%)
(
0
.
0
0
6
1
7
)
1/
4
c
o
v
(
a
i
)
2
1
3
.
5
4
3
6
2
.
0
5
0
8
0
.
0
6
2
6
0
.
0
3
9
4
2
2
.
8
2
-
5
.
0
8
(
0
.
0
0
6
1
7
)
1/
2
c
o
v
(
a
i
)
2
1
2
.
6
5
6
9
0
.
4
7
4
3
0
.
0
2
4
6
0
.
0
1
9
7
6
7
.
1
5
4
7
.
4
3
0
.
0
0
6
1
7
c
o
v
(
a
i
)
2
1
2
.
0
1
0
2
0
.
7
6
1
0
3
.
8
3
5
5
e
-
04
0
.
0
0
9
0
9
9
.
4
9
7
6
.
1
0
.
0
0
6
1
7
10
×
c
o
v
(
a
i
)
2
1
1
.
9
6
2
5
0
.
9
6
2
4
0
.
0
0
1
4
0
.
0
0
1
4
9
8
.
1
2
9
6
.
2
4
0
.
0
0
6
1
7
1
0
0
×
c
o
v
(
a
i
)
2
1
1
.
9
4
9
1
0
.
9
3
1
1
0
.
0
0
1
9
0
.
0
0
2
6
9
7
.
4
5
9
3
.
1
1
0
.
0
0
6
1
7
1
0
0
0
×
c
o
v
(
a
i
)
2
1
1
.
9
4
5
1
0
.
8
9
6
4
0
.
0
0
2
1
0
.
0
0
3
9
9
4
.
5
1
8
9
.
6
4
4.
CO
NCLU
SI
O
N
B
ased
o
n
th
e
test
r
esu
lts
,
t
h
e
Kal
m
a
n
Fil
ter
d
esig
n
is
ab
le
to
r
ed
u
c
e
n
o
is
e
o
n
th
e
I
MU
s
en
s
o
r
d
ata.
T
h
e
ze
r
o
v
elo
cit
y
co
m
p
e
n
s
a
t
io
n
an
d
b
ia
s
co
m
p
e
n
s
atio
n
ar
e
ab
le
to
r
ed
u
ce
th
e
e
f
f
ec
ts
o
f
d
r
i
f
t
d
u
e
to
in
te
g
r
atio
n
a
n
d
ab
le
to
im
p
r
o
v
e
th
e
p
o
s
itio
n
esti
m
atio
n
o
f
th
e
q
u
ad
r
o
to
r
.
T
h
e
a
lg
o
r
ith
m
s
ca
n
p
r
o
v
id
e
th
e
tr
an
s
latio
n
e
s
ti
m
atio
n
a
n
d
an
g
u
lar
r
o
tatio
n
est
i
m
at
io
n
o
f
th
e
q
u
ad
r
o
t
o
r
w
it
h
r
ea
s
o
n
ab
le
ac
c
u
r
ac
y
th
a
n
w
it
h
o
u
t
K
al
m
a
n
F
il
ter
an
d
co
m
p
en
s
ati
o
n
.
B
ased
o
n
th
e
ex
p
er
i
m
en
t
s
,
th
e
s
y
s
te
m
s
ca
n
p
r
o
v
id
e
th
e
a
n
g
u
lar
ac
cu
r
ac
y
o
f
ab
o
u
t
9
5
%
an
d
th
e
tr
an
s
latio
n
ac
cu
r
ac
y
o
f
ab
o
u
t
9
0
%.
M
o
r
eo
v
er
,
b
ased
o
n
th
e
test
r
es
u
lt
it
i
s
s
h
o
w
ed
th
at
t
h
e
ac
ce
ler
o
m
eter
a
n
d
t
h
e
g
y
r
o
s
co
p
e
w
it
h
Kal
m
a
n
Fi
lter
p
er
f
o
r
m
s
ig
n
i
f
ica
n
t
i
m
p
r
o
v
e
m
e
n
t
at
least
f
o
r
s
h
o
r
t
p
er
io
d
o
f
ti
m
e
.
T
h
e
alg
o
r
ith
m
h
a
s
b
ee
n
tes
t
ed
o
f
f
-
li
n
e
.
F
o
r
t
h
e
r
ea
l
-
ti
m
e
ap
p
licatio
n
,
t
h
is
a
lg
o
r
ith
m
m
a
y
h
a
v
e
s
o
m
e
li
m
itatio
n
s
.
RE
F
E
R
E
NC
E
S
[1
]
S
.
G
.
G
u
p
ta,
M
.
M
.
G
h
o
n
g
e
,
a
n
d
P
.
M
.
Ja
w
a
n
d
h
iy
a
,
“
Re
v
ie
w
o
f
U
n
m
a
n
n
e
d
A
ircra
f
t
S
y
ste
m
(
U
A
S
),
”
In
t.
J
.
A
d
v
.
Res
.
Co
mp
u
t.
E
n
g
.
T
e
c
h
n
o
l.
,
v
o
l.
2
,
n
o
.
4
,
p
p
.
1
6
4
6
–
1
6
5
8
,
2
0
1
3
.
[2
]
K.
S
c
h
a
u
w
e
c
k
e
r
a
n
d
A
.
Zell,
“
On
-
Bo
a
rd
Du
a
l
-
S
tere
o
-
V
isio
n
f
o
r
A
u
to
n
o
m
o
u
s
Qu
a
d
r
o
to
r
Na
v
ig
a
ti
o
n
,
”
i
n
2013
In
ter
n
a
t
io
n
a
l
C
o
n
fer
e
n
c
e
o
n
U
n
ma
n
n
e
d
Ai
rc
ra
ft
S
y
ste
ms
(
ICUAS
)
,
2
0
1
3
,
p
p
.
3
3
3
–
3
4
2
.
[3
]
K.
-
H.
Oh
a
n
d
H.
-
S
.
A
h
n
,
“
Ex
ten
d
e
d
Ka
lma
n
F
il
ter
w
it
h
M
u
lt
i
-
f
re
q
u
e
n
c
y
Re
f
e
r
e
n
c
e
D
a
ta
f
o
r
Qu
a
d
ro
to
r
Na
v
ig
a
ti
o
n
,
”
in
1
5
t
h
In
ter
n
a
ti
o
n
a
l
Co
n
fer
e
n
c
e
o
n
C
o
n
tr
o
l,
A
u
t
o
ma
ti
o
n
a
n
d
S
y
ste
ms
(
ICCAS
2
0
1
5
)
,
2
0
1
5
,
n
o
.
Ic
c
a
s,
p
p
.
2
0
1
–
2
0
6
.
[4
]
J.
D.
Ba
rto
n
,
“
F
u
n
d
a
m
e
n
tals o
f
S
m
a
ll
Un
m
a
n
n
e
d
A
ircra
f
t
F
li
g
h
t,
”
J
o
h
n
Ho
p
k
in
s A
PL
T
e
c
h
.
Dig
.
, v
o
l.
3
1
,
n
o
.
2
,
p
p
.
132
–
1
4
9
,
2
0
1
2
.
[5
]
A
.
P
u
ri,
K
.
P
.
V
a
lav
a
n
is,
a
n
d
M
.
Ko
n
ti
tsis,
“
S
tatisti
c
a
l
P
ro
f
il
e
G
e
n
e
ra
ti
o
n
f
o
r
T
ra
ff
ic
M
o
n
it
o
rin
g
Us
in
g
Re
a
l
-
ti
m
e
UA
V
b
a
se
d
V
id
e
o
Da
ta,”
in
2
0
0
7
IEE
E
M
e
d
it
e
rr
a
n
e
a
n
C
o
n
fer
e
n
c
e
o
n
Co
n
tro
l
a
n
d
Au
t
o
ma
t
io
n
,
At
h
e
n
s.
Gr
e
e
c
e
,
2
0
0
7
,
n
o
.
2
7
–
2
9
Ju
ly
.
[6
]
J.
M
.
M
.
Ne
to
t,
R.
A
.
P
a
ix
a
o
,
a
n
d
L
.
R.
L
.
Ro
d
rig
u
e
s,
“
A
S
u
rv
e
il
lan
c
e
Tas
k
f
o
r
a
U
A
V
in
a
N
a
tu
ra
l
Disa
ste
r
S
c
e
n
a
rio
,
”
in
2
0
1
2
IEE
E
In
ter
n
a
ti
o
n
a
l
S
y
mp
o
siu
m
o
n
In
d
u
stri
a
l
El
e
c
tro
n
ics
(
IS
IE)
,
2
0
1
2
,
p
p
.
1
5
1
6
–
1
5
2
2
.
[7
]
J.
Am
e
n
y
o
,
D.
P
h
e
lp
s,
O.
Ola
d
i
p
o
,
F
.
S
e
w
o
v
o
e
-
e
k
u
o
e
,
S
.
Ja
d
o
o
n
a
n
a
n
,
T
.
T
a
b
a
s
su
m
,
S
.
G
n
a
b
o
d
e
,
T.
D.
S
h
e
rp
a
,
M
.
F
a
lzo
n
e
,
A
.
Ho
ss
a
in
,
a
n
d
A
.
K
u
b
lal,
“
M
e
d
izD
ro
id
s
P
ro
jec
t :
Ultra
-
L
o
w
Co
st
,
L
o
w
-
A
lt
it
u
d
e
,
Aff
o
rd
a
b
le
a
n
d
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
8
-
8708
I
J
E
C
E
Vo
l.
7
,
No
.
5
,
Octo
b
e
r
2
0
1
7
:
2
5
9
6
–
2
6
0
4
2604
S
u
sta
in
a
b
le
UA
V
M
u
lt
ic
o
p
ter
D
ro
n
e
s
F
o
r
M
o
sq
u
it
o
V
e
c
to
r
C
o
n
tro
l
in
M
a
laria
Dise
a
se
M
a
n
a
g
e
m
e
n
t,
”
in
2
0
1
4
IEE
E
Glo
b
a
l
Hu
ma
n
it
a
ria
n
T
e
c
h
n
o
l
o
g
y
C
o
n
fer
e
n
c
e
,
2
0
1
4
.
[8
]
J.
Zh
a
n
g
,
L
.
L
iu
,
B.
W
a
n
g
,
X
.
C
h
e
n
,
a
n
d
Q.
W
a
n
g
,
“
Hig
h
sp
e
e
d
A
u
to
m
a
ti
c
P
o
w
e
r
L
in
e
De
te
c
ti
o
n
a
n
d
T
ra
c
k
in
g
f
o
r
a
U
A
V
-
Ba
se
d
In
sp
e
c
ti
o
n
,
”
in
2
0
1
2
IEE
E
In
te
rn
a
ti
o
n
a
l
Co
n
f
e
re
n
c
e
o
n
In
d
u
stria
l
C
o
n
tr
o
l
a
n
d
El
e
c
tro
n
ic
En
g
i
n
e
e
rin
g
,
2
0
1
2
,
p
p
.
2
6
6
–
2
6
9
.
[9
]
K.
J.
Ro
g
e
rs
a
n
d
A
.
F
in
n
,
“
F
re
q
u
e
n
c
y
Esti
m
a
ti
o
n
f
o
r
3
D
A
t
m
o
sp
h
e
ric
T
o
m
o
g
ra
p
h
y
u
sin
g
Un
m
a
n
n
e
d
A
e
rial
V
e
h
icle
s,” in
2
0
1
3
IEE
E
IS
S
NIP
,
2
0
1
3
,
p
p
.
3
9
0
–
3
9
5
.
[1
0
]
S
.
G
u
p
te
a
n
d
J
.
M
.
C
o
n
ra
d
,
“
A
su
rv
e
y
o
f
q
u
a
d
ro
to
r
U
n
m
a
n
n
e
d
A
e
ri
a
l
V
e
h
icle
s,”
in
2
0
1
2
Pro
c
e
e
d
in
g
s
o
f
IE
EE
S
o
u
th
e
a
stc
o
n
,
2
0
1
2
,
p
p
.
1
–
6.
[1
1
]
H.
T
n
u
n
a
y
,
M
.
Q.
A
b
d
u
rro
h
m
a
n
,
Y.
Nu
g
ro
h
o
,
R.
I
n
o
v
a
n
,
A
.
Ca
h
y
a
d
i,
a
n
d
Y.
Ya
m
a
m
o
to
,
“
A
u
to
-
T
u
n
in
g
Qu
a
d
c
o
p
ter
Us
i
n
g
L
o
o
p
S
h
a
p
in
g
,
”
in
2
0
1
3
In
ter
n
a
ti
o
n
a
l
Co
n
fer
e
n
c
e
o
n
Co
mp
u
ter
,
C
o
n
tr
o
l,
I
n
fo
r
ma
ti
c
s
a
n
d
It
s
Ap
p
li
c
a
ti
o
n
s (
IC3
INA)
,
2
0
1
3
,
p
p
.
1
1
1
–
1
1
5
.
[1
2
]
G
.
Ya
n
h
u
i,
X
.
Qia
n
g
u
i,
H.
S
h
o
u
s
o
n
g
,
a
n
d
J.
X
iao
,
“
F
li
g
h
t
Co
n
tro
l
S
y
ste
m
S
i
m
u
latio
n
P
latf
o
rm
f
o
r
UA
V
b
a
se
d
o
n
In
teg
ra
ti
n
g
S
im
u
li
n
k
w
it
h
S
tate
f
l
o
w
,
”
T
EL
KOM
NIKA
,
v
o
l.
1
0
,
n
o
.
5
,
p
p
.
9
8
5
–
9
9
1
,
2
0
1
2
.
[1
3
]
S
.
Am
in
i
a
n
d
A
.
A
.
Ak
b
a
ri,
“
A
d
a
p
ti
v
e
S
li
d
in
g
M
o
d
e
Co
n
tr
o
ll
e
r
De
sig
n
F
o
r
A
tt
it
u
d
e
S
m
a
ll
U
A
V
,
”
In
t.
J
.
Ro
b
o
t
.
Au
to
m.
,
v
o
l.
4
,
n
o
.
3
,
p
p
.
2
1
9
–
2
2
9
,
2
0
1
5
.
[1
4
]
A
.
Bu
d
iy
a
n
to
,
A
.
Ca
h
y
a
d
i,
T
.
B.
A
d
ji
,
a
n
d
O.
W
a
h
y
u
n
g
g
o
ro
,
“
UA
V
Ob
sta
c
le
Av
o
id
a
n
c
e
Us
in
g
P
o
ten
ti
a
l
F
iel
d
u
n
d
e
r
Dy
n
a
m
ic
En
v
iro
n
m
e
n
t,
”
in
2
0
1
5
In
ter
n
a
ti
o
n
a
l
Co
n
fer
e
n
c
e
o
n
Co
n
tro
l
,
El
e
c
tro
n
ics
,
Re
n
e
wa
b
le
En
e
rg
y
a
n
d
Co
mm
u
n
ica
ti
o
n
s (
ICCER
EC)
,
2
0
1
5
,
p
p
.
1
8
7
–
1
9
2
.
[1
5
]
W
.
F
e
i,
C.
Be
n
-
m
e
i,
a
n
d
L
.
E
.
E.
T
.
H,
“
A
Co
m
p
re
h
e
n
siv
e
U
AV
I
n
d
o
o
r
Na
v
ig
a
ti
o
n
S
y
ste
m
Ba
se
d
o
n
V
isio
n
Op
ti
c
a
l
F
lo
w
a
n
d
L
a
se
r
F
a
stS
LAM
,
”
Acta
A
u
t
o
m.
S
in
.
,
v
o
l.
3
9
,
n
o
.
1
1
,
p
p
.
1
8
8
9
–
1
8
9
9
,
2
0
1
3
.
[1
6
]
S
.
T
.
G
o
h
,
O.
A
b
d
e
lk
h
a
li
k
,
a
n
d
S
.
A
.
R.
Ze
k
a
v
a
t,
“
A
W
e
i
g
h
ted
M
e
a
su
re
m
e
n
t
F
u
sio
n
Ka
lm
a
n
F
il
ter i
m
p
le
m
e
n
tatio
n
f
o
r
U
A
V
n
a
v
ig
a
ti
o
n
,
”
Aer
o
s
p
.
S
c
i
.
T
e
c
h
n
o
l.
,
v
o
l.
2
8
,
n
o
.
1
,
p
p
.
3
1
5
–
3
2
3
,
2
0
1
3
.
[1
7
]
C.
T
ro
ian
i,
A
.
M
a
rti
n
e
ll
i,
C.
L
a
u
g
ier,
a
n
d
D.
S
c
a
ra
m
u
z
z
a
,
“
L
o
w
c
o
m
p
u
tatio
n
a
l
-
c
o
m
p
lex
it
y
a
lg
o
rit
h
m
s
f
o
r
v
isio
n
-
a
id
e
d
in
e
rt
ial
n
a
v
ig
a
ti
o
n
o
f
m
icro
a
e
ria
l
v
e
h
icle
s,”
Ro
b
.
A
u
to
n
.
S
y
st.
,
v
o
l.
6
9
,
p
p
.
8
0
–
9
7
,
2
0
1
5
.
[1
8
]
Y.
L
u
o
l,
C.
C.
T
sa
n
g
,
G
.
Zh
a
n
g
,
Z.
Do
n
g
,
G
.
S
h
il
,
S
.
Y.
Kw
o
k
,
W
.
J.
L
il
,
P
.
H.
W
.
L
e
o
n
g
,
a
n
d
M
.
Yiu
w
o
n
g
,
“
A
n
A
tt
it
u
d
e
Co
m
p
e
n
sa
ti
o
n
T
e
c
h
n
iq
u
e
f
o
r
a
M
EM
S
M
o
ti
o
n
S
e
n
so
r
Ba
se
d
Dig
it
a
l
W
rit
in
g
In
str
u
m
e
n
t,
”
in
Pro
c
e
e
d
in
g
s
o
f
t
h
e
1
st I
E
EE
C
o
n
fer
e
n
c
e
o
n
Na
n
o
/M
icr
o
E
n
g
i
n
e
e
re
d
a
n
d
M
o
lec
u
la
r S
y
ste
ms
,
2
0
0
6
,
p
p
.
9
0
9
–
9
1
4
.
[1
9
]
Y.
M
.
M
u
sta
f
a
h
,
A
.
W
.
A
z
m
a
n
,
a
n
d
F
.
A
k
b
a
r,
“
In
d
o
o
r
UA
V
P
o
siti
o
n
in
g
Us
in
g
S
tere
o
V
isi
o
n
S
e
n
so
r,
”
Pro
c
e
d
i
a
En
g
.
,
v
o
l.
4
1
,
n
o
.
Iris,
p
p
.
5
7
5
–
5
7
9
,
2
0
1
2
.
[2
0
]
L
.
Ro
d
o
lf
o
,
G
.
Ca
rril
lo
,
A
.
En
riq
u
e
,
D.
L
ó
p
e
z
,
R.
L
o
z
a
n
o
,
a
n
d
C.
P
é
g
a
rd
,
“
Co
m
b
in
i
n
g
S
tere
o
V
isi
o
n
a
n
d
In
e
rti
a
l
Na
v
ig
a
ti
o
n
S
y
ste
m
f
o
r
a
Qu
a
d
-
R
o
to
r
UA
V
,
”
S
p
ri
n
g
e
r,
J
.
I
n
tell.
Ro
b
o
t.
S
y
st.
,
p
p
.
3
7
3
–
3
8
7
,
2
0
1
2
.
[2
1
]
Y.
S
o
n
g
,
B.
X
ia
n
,
Y.
Zh
a
n
g
,
X.
Jia
n
g
,
a
n
d
X
.
Z
h
a
n
g
,
“
T
o
w
a
rd
s
a
u
to
n
o
m
o
u
s
c
o
n
tro
l
o
f
q
u
a
d
ro
t
o
r
u
n
m
a
n
n
e
d
a
e
rial
v
e
h
icle
s
in
a
G
P
S
-
d
e
n
ied
u
rb
a
n
a
re
a
v
ia
las
e
r
ra
n
g
e
r
f
in
d
e
r,
”
Op
t.
-
In
t.
J
.
L
i
g
h
t
El
e
c
tro
n
O
p
t.
,
v
o
l.
1
2
6
,
n
o
.
2
3
,
p
p
.
3
8
7
7
–
3
8
8
2
,
2
0
1
5
.
[2
2
]
W
.
Zh
e
n
g
,
F
.
Zh
o
u
,
a
n
d
Z.
W
a
n
g
,
“
Ro
b
u
st
a
n
d
A
c
c
u
ra
te
M
o
n
o
c
u
lar
V
isu
a
l
Na
v
ig
a
ti
o
n
C
o
m
b
in
in
g
IM
U
f
o
r
a
Qu
a
d
ro
t
o
r,
”
IEE
E/
CAA
J
.
Au
t
o
m.
S
in
.
,
v
o
l
.
2
,
n
o
.
1
,
p
p
.
3
3
–
4
4
,
2
0
1
5
.
[2
3
]
B.
D.
S
c
a
ra
m
u
z
z
a
a
n
d
F
.
F
ra
u
n
d
o
rf
e
r,
“
V
isu
a
l
Od
o
m
e
tr
y
,
”
IEE
E
Ro
b
o
ti
c
&
Au
to
ma
ti
o
n
M
a
g
a
zi
n
e
,
n
o
.
De
c
e
m
b
e
r,
p
p
.
8
0
–
9
2
,
De
c
-
2
0
1
1
.
[2
4
]
C.
Ko
w
n
a
c
k
i,
“
De
si
g
n
o
f
a
n
a
d
a
p
ti
v
e
Ka
l
m
a
n
f
il
ter
to
e
li
m
in
a
te
m
e
a
su
re
m
e
n
t
f
a
u
lt
s
o
f
a
las
e
r
r
a
n
g
e
f
in
d
e
r
u
se
d
in
th
e
UA
V
sy
ste
m
,
”
Aer
o
sp
.
S
c
i.
T
e
c
h
n
o
l.
,
v
o
l.
4
1
,
p
p
.
8
1
–
8
9
,
2
0
1
5
.
[2
5
]
Y.
Ho
u
a
n
d
C.
Yu
,
“
A
u
t
o
n
o
m
o
u
s
T
a
rg
e
t
L
o
c
a
li
z
a
ti
o
n
u
si
n
g
Qu
a
d
r
o
to
r,
”
in
T
h
e
2
6
th
Ch
in
e
se
Co
n
tr
o
l
a
n
d
De
c
isio
n
Co
n
fer
e
n
c
e
(
2
0
1
4
CCDC
)
,
2
0
1
4
,
p
p
.
8
6
4
–
8
6
9
.
[2
6
]
X
.
B
in
,
Y.
S
e
n
,
a
n
d
Z
.
X
u
,
“
Co
n
tro
l
o
f
a
q
u
a
d
ro
to
r
h
e
li
c
o
p
ter
u
sin
g
th
e
COM
P
A
S
S
(
Be
iDo
u
)
s
y
ste
m
a
n
d
o
n
-
b
o
a
rd
v
isi
o
n
sy
s
te
m
,
”
Op
t.
-
In
t.
J
.
L
ig
h
t
El
e
c
tro
n
O
p
t.
,
v
o
l.
1
2
7
,
n
o
.
1
7
,
p
p
.
6
8
2
9
–
6
8
3
8
,
2
0
1
6
.
[2
7
]
S
.
A
g
a
r
wa
l,
S
.
B.
L
a
z
a
ru
s,
a
n
d
A
.
S
a
v
v
a
ris,
“
M
o
n
o
c
u
lar
V
isi
o
n
Ba
se
d
Na
v
ig
a
ti
o
n
a
n
d
L
o
c
a
li
sa
ti
o
n
in
I
n
d
o
o
r
En
v
iro
n
m
e
n
ts,
”
in
I
FA
C
Pro
c
e
e
d
in
g
s V
o
l
u
me
s
,
2
0
1
2
,
v
o
l.
4
5
,
n
o
.
1
,
p
p
.
9
7
–
1
0
2
.
[2
8
]
D.
A
b
e
y
wa
rd
e
n
a
,
S
.
Ko
d
a
g
o
d
a
,
G
.
Diss
a
n
a
y
a
k
e
,
a
n
d
R.
M
u
n
a
si
n
g
h
e
,
“
Im
p
ro
v
e
d
S
tate
Esti
m
a
ti
o
n
in
Q
u
a
d
r
o
to
r
M
A
V
s : A
No
v
e
l
Dri
f
t
-
F
re
e
V
e
lo
c
it
y
Esti
m
a
to
r,
”
IEE
E
R
o
b
o
t.
Au
t
o
m.
M
a
g
.
,
v
o
l.
2
0
,
n
o
.
4
,
p
p
.
3
2
–
3
9
,
2
0
1
3
.
[2
9
]
A
.
R.
V
e
trella,
A
.
S
a
v
v
a
ris,
G
.
F
a
sa
n
o
,
a
n
d
D.
A
c
c
a
rd
o
,
“
RG
B
-
D
Ca
m
e
ra
-
Ba
se
d
Qu
a
d
ro
to
r
Na
v
ig
a
ti
o
n
I
n
G
P
S
-
De
n
ied
a
n
d
L
o
w
L
ig
h
t
En
v
iro
n
m
e
n
ts
Us
in
g
Kn
o
w
n
3
D
M
a
rk
e
rs,”
in
In
ter
n
a
t
io
n
a
l
C
o
n
fer
e
n
c
e
o
n
Un
ma
n
n
e
d
Ai
rc
ra
ft
S
y
ste
ms
(
ICUAS
)
,
2
0
1
5
,
p
p
.
1
8
5
–
1
9
2
.
[3
0
]
M
.
A
c
h
telik
,
A
.
Ba
c
h
ra
c
h
,
R.
He
,
S
.
P
re
n
ti
c
e
,
a
n
d
N.
Ro
y
,
“
A
u
to
n
o
m
o
u
s
Na
v
ig
a
ti
o
n
a
n
d
E
x
p
lo
ra
ti
o
n
o
f
a
Qu
a
d
ro
t
o
r
He
li
c
o
p
ter
in
G
P
S
-
d
e
n
ied
In
d
o
o
r
En
v
iro
n
m
e
n
ts,
”
in
Ro
b
o
ti
c
s:
S
c
ien
c
e
a
n
d
S
y
ste
ms
Co
n
fer
e
n
c
e
.
,
2
0
0
8
,
p
p
.
1
–
1
2
.
[3
1
]
H.
Du
,
“
De
sig
n
o
f
A
d
a
p
ti
v
e
Ka
lm
a
n
F
il
ter
A
lg
o
rit
h
m
in
In
teg
ra
ted
Na
v
ig
a
ti
o
n
S
y
ste
m
f
o
r
L
a
n
d
V
e
h
icle
s,”
i
n
P
r
o
c
e
e
d
in
g
o
f
2
0
1
3
IEE
E
In
ter
n
a
ti
o
n
a
l
Co
n
fer
e
n
c
e
o
n
M
e
c
h
a
tro
n
ic
a
n
d
Au
to
ma
ti
o
n
s
A
u
g
u
st
4
-
7
,
T
a
k
a
ma
ts
u
,
J
a
p
a
n
,
2
0
1
3
,
p
p
.
1
4
9
2
–
1
4
9
6
.
[3
2
]
D.
S
im
o
n
,
“
Ka
lm
a
n
F
il
terin
g
,
”
Emb
e
d
d
e
d
S
y
ste
ms
Pro
g
r
a
mm
in
g
,
v
o
l.
1
4
,
n
o
.
Ju
n
e
,
p
p
.
7
2
–
7
9
,
2
0
0
1
.
[3
3
]
R.
F
a
ra
g
h
e
r,
“
Un
d
e
rsta
n
d
in
g
t
h
e
Ba
sis
o
f
th
e
Ka
lm
a
n
F
il
ter
V
ia
a
S
im
p
le
a
n
d
I
n
t
u
it
iv
e
De
riv
a
ti
o
n
,
”
IEE
E
S
ig
n
a
l
Pro
c
e
ss
.
M
a
g
.
,
v
o
l
.
2
9
,
n
o
.
S
e
p
tem
b
e
r,
p
p
.
1
2
8
–
1
3
2
,
2
0
1
2
.
[3
4
]
C.
Ch
iu
,
“
Err
o
r
Re
d
u
c
ti
o
n
T
e
c
h
n
iq
u
e
s
f
o
r
a
M
EM
S
A
c
c
e
lero
m
e
ter
-
b
a
se
d
Dig
it
a
l
In
p
u
t
De
v
ice
,
”
T
h
e
Ch
in
e
se
Un
iv
e
rsit
y
o
f
Ho
n
g
Ko
n
g
,
2
0
0
8
.
Evaluation Warning : The document was created with Spire.PDF for Python.