Global Positioning System Reference
In-Depth Information
l
b
l
ie
Using
R
from Equation 1, calculate the skew-symmetric matrix for earth rotation rate
ω
since the last velocity calculation:
⎡
⎣
⎤
⎦
0
−
ω
e
sin
φω
e
cos
φ
ω
e
sin
φ
0
0
l
ie
ω
=
(10)
−
ω
e
cos
φ
0
0
l
el
In addition, calculate the skew-symmetric matrix for the L-frame change of orientation
ω
since last calculation:
⎡
⎣
⎤
⎦
V
e
tan
φ
V
e
N
0
−
N
+
h
+
h
φ
V
e
tan
V
n
M
0
el
ω
=
(11)
+
+
N
h
h
V
e
N
V
n
M
−
−
0
+
h
+
h
Use the following equation to provide velocity increments of the mobile robot in the body
frame:
⎡
⎣
⎤
⎦
f
x
Δ
t
f
y
Δ
t
Δ
V
b
=
(12)
f
z
Δ
t
l
b
,
ie
el
, the effect of gravity in the local frame in addition to
With the three matrices
R
ω
and
ω
Δ
V
b
the new velocities are calculated by determining the
rate-of-change for velocity increments
the body-frame velocity increments
Δ
V
l
in the local frame as follows:
2
el
Δ
Δ
V
l
l
b
V
b
ie
V
l
g
l
=
R
−
ω
+
ω
+
Δ
t
Δ
t
(13)
T
. Integration is performed using the previous values for velocities
g
l
Where
= [
00
−
g
]
V
l
1toget
V
l
Δ
V
l
at time
k
−
at time
k
using
as follows:
0.5
V
l
) =
V
l
Δ
V
l
) +
Δ
V
l
(
(
−
) +
(
(
−
)
k
k
1
k
k
1
(14)
2.4.3 Position equations
The equations for altitude
h
, latitude
φ
and longitude
λ
are as follows:
)
=
−
)
+
)
+
−
h
(
k
h
(
k
1
0.5
[
V
u
(
k
V
u
(
k
1
)]
Δ
t
(15)
t
Δ
φ
(
k
) =
φ
(
k
−
1
) +
0.5
[
V
n
(
k
) +
V
n
(
k
−
1
)]
(16)
+
R
h
Δ
t
λ
(
) =
λ
(
−
) +
[
(
) +
(
−
)]
k
k
1
0.5
V
e
k
V
e
k
1
(17)
(
R
+
h
)
cos
φ
It should be noted that any uncompensated bias or drift error in the accelerometer data
will lead to growing errors when integrating acceleration to get velocity and again when
integrating to get position. Furthermore, any uncompensated bias or drift error in the vertical
gyroscope reading will lead to error growth when integrating to get yaw and again (together
with velocity) to get position.
Search WWH ::
Custom Search