Professional Documents
Culture Documents
Navigation Equations in The Earth Centered Earth Fixed Frame
Navigation Equations in The Earth Centered Earth Fixed Frame
Navigation Equations in The Earth Centered Earth Fixed Frame
Arland B. Thompson
Chief Scientist Advanced Technology Associates
(www.atacolorado.com)
I) Introduction
U (t ) =πθNP Eq. 1
Where:
P – Precession
N – Nutation
θ - Earth rotation
π - Polar motion
r i = g i + a i (Eq. 2)
Where:
a - Acceleration due to thrust and aerodynamics in the inertial
i
frame of reference.
g - Acceleration due to gravity in the inertial frame of reference.
i
Note that the first time derivative of the transformation DCM from
ECEF to inertial ( C ei ) is as follows:
The angular rate vector of the ECEF frame relative to the ECI
frame used to populate the elements of Ωiee in Eq. 3 is:
0
ω =e
0
ie (Eqs. 4)
ω
Where:
ω - Earth rotation rate (approximately 7.292e-5 radians/second)
Using the multiplication rule, the second time derivative of the
transformation DCM from ECEF to inertial is as follows:
i = C i Ω
C e + C i Ωe
e e ie e ie
= C Ω
C i i + C Ωe Ωe
e i
(Eqs. 5)
e e ie e ie ie
e
i e
C =C Ω +Ω Ω
i
e ( e
ie
e
ie ie )
r i = Cei r e (Eq. 7)
r i = Cei r e + C ei r e (Eq. 8)
d e
r = r e
dt
d e (Eqs. 11)
r = g e + a e − 2Ωiee r e − Ωiee Ωeie r e
dt
IV) Quaternions
Quaternions will be used to maintain the direction cosine matrix
(attitude) of the vehicle body frame relative to the Earth fixed frame
discussed in the next section. Therefore, it is briefly introduced here.
There exists a single judiciously chosen axis about which one frame of
reference (call it the “I” frame) can be rotated by some angle to bring it
into alignment with another frame of reference (call it the “F” frame).
This is illustrated by Figure 1.
Figure 1
π
0 * sin 8
0 * sin π
8
q=
π
(Eq. 14)
1 * sin
8
π
cos
8
0
0
q = (Eq. 15)
0.382683432
0.923879533
Figure 2
q00 = qUnit[0]*qUnit[0]
q11 = qUnit[1]*qUnit[1]
q22 = qUnit[2]*qUnit[2]
q33 = qUnit[3]*qUnit[3]
q01 = qUnit[0]*qUnit[1]
q02 = qUnit[0]*qUnit[2]
q03 = qUnit[0]*qUnit[3]
I Frame at t
q12 = qUnit[1]*qUnit[2]
q13 = qUnit[1]*qUnit[3]
q23 = qUnit[2]*qUnit[3] (Eqs. 16)
f = a −g (Eq. 17)
The angular rate of the vehicle body frame relative to the Earth fixed
frame can be computed using the following expression:
Where:
∆θebb (t ) = (∆θ (t )) ∆θ
b
eb
T
b
eb (t ) (Eq. 23)
(
Cib ( t ) Cie ( t ) ) T
= Ceb ( t ) (Eq. 24)
Cie ( t ) →qie ( t )
Cie ( t +δt ) →qei ( t +δt )
∆q( t ) = qie ( t ) qei ( t +δt )
*
(
θ = 2.0 * a tan 2 1 −∆q[3] * ∆q[3] , ∆q[3] )
∆q[0]
e[0] =
1 − ∆q[3] * ∆q[3] (Eqs. 26)
∆q[1]
e[1] =
1 − ∆q[3] * ∆q[3]
∆q[ 2]
e[ 2] =
1 − ∆q[3] * ∆q[3]
∆θiee (t ) =θ * e
attraction is nearly constant over the IMU measurement cycle leads to the
following position and velocity update formulas:
r e ( t + ∆t ) = r e ( t ) + r e ( t ) ∆t +
2
( 2
)
g + 2Ωiee r e − Ωiee Ωiee r e ∆t∆t + (Ceb ( t ) ) ∆V b ( t ) ∆t
1 e 1 t
Eqs. 27
e e
(
r ( t + ∆t ) = r ( t ) + g + 2Ω r − Ω Ω r ∆t + (C ( t ) ) ∆V ( t )
e e
ie
e e
ie
e
ie
e
) b
e
t b
µ
∞ n
R n
V ( r , φ, λ) = 1 + ∑∑ Pnm ( sin φ )[ Cnm cos ( mλ) + S nm sin ( mλ) ] Eq. 28
r
n=2 m=0 r
Where:
r - Distance from geo center to object
φ - Geocentric latitude of object
λ - Longitude of object
R - Equatorial radius magnitude of Earth
“n” is referred to as the degree of the model, and “m” is referred to as the
order. Pnm are the associated Legendre functions with argument sin (φ) , and will be
discussed later. Cnm and Snm are the spherical harmonic coefficients. In the above
series expansion, when the order equals 0 (m = 0), the coefficients are referred to as
zonal harmonics. These describe the variation in the Earth’s gravitational field due
to latitude. The largest variation is due to oblateness (J2).
It is orders of magnitude larger than the other coefficients (C20 = -1.0826266836e-
003; C21 = -2.414e-010), and for many applications, it is sufficient to only model J2.
When n = m, the coefficients are referred to as sectorial. Sectorial harmonics
account for the gravitational field variation in longitude. The coefficients are
referred to as tesseral harmonics when m ≠ n ≠ 0. They divide the Earth sphere into
a checkerboard array or tiles (Hence the name, tesseral). Degree and order of the
models used in practice depend upon the application. The shuttle uses degree and
order 8 for on-board navigation. SEASAT uses degree and order 36 for precise orbit
determination.
T T T
∂V ∂r ∂V ∂φ ∂V ∂λ
ag = + +
∂r ∂r ∂φ ∂r ∂λ ∂r
∂V µ ∞ n
R
n
=− 2 ∑∑
1 + ( n +1) Pnm ( sin φ )[Cnm cos ( mλ) + S nm sin ( mλ) ] Eqs. 29
∂r r n =2 m =0
r
R ∂Pnm ( sin φ )
n
∂V ∞ n
= ∑∑ [Cnm cos ( mλ) + S nm sin ( mλ) ]
∂φ n=2 m =0 r ∂φ
n
∂V µ ∞ n R
= ∑∑ Pnm ( sin φ ) m[ S nm cos ( mλ) − Cnm sin ( mλ) ]
∂λ r n =2 m =0 r
∂r x y z
= , ,
∂r r r r
∂φ 1 − xz − yz z2
=
2 , 2 , 1 − 2
∂r x2 + y2 r r r
∂λ 1
= 2 [ − y , x ,0 ]
∂r x + y2
As stated earlier, Pnm are the associated Legendre functions. The associated
Legendre functions are obtained from the Rodriguez formula.
Pn ( u ) =
1 dn
2 n
( u 2 − 1)
n
2 n! du
Eqs. 30
Pnm ( u ) = (1 − u 2 )
m
m/2 d
Pn ( u )
du m
For the second equation in Eqs. 31 the index ranges from m = 1 to m = desired
degree. For the third equation in Eqs. 31 the index ranges from m = 0 to m = desired
degree - 1. For the fourth equation in Eqs. 31 the index ranges from n = 2 to desired
degree and m = 0 to n – 2 for each n.
′
P00 = 0
′ ′
Pmm = ( 2m −1) cos( φ) Pm −1, m −1 − ( 2m −1) sin( φ) Pm −1, m −1
′ ′
Pm +1, m = ( 2m +1) sin( φ) Pmm + ( 2m +1) cos( φ) Pmm
Eqs. 32
′
Pnm =
1
( 2n −1) sin( φ) Pn −1,m ′ + ( 2n −1) cos( φ) Pn −1,m − ( n + m −1) Pn −2,m ′
n −m
For a model of small degree and order (say 2X2), the gravitational equations
can be written explicitly for computational speed. This may be needed in a real time
system.
π π
cos − + φ 0 − sin − + φ
π 2 2
Y − + φ = 0 1 0 Eqs. 33
2 π π
sin − 2 + φ 0 cos − 2 + φ
π
NED
C ECEF = Y − + φ * Z ( λ )
2
Where:
λ - Longitude of the vehicle (radians)
φ - Geodetic latitude of the vehicle (radians)
C ENCEEDF - Transformation direction cosine matrix from ECEF to NED
v NED = C ECEF
NED
* r e
Eqs. 34
ψ = a tan 2( vNED [1], vNED [0])
Where:
ψ - Vehicle heading (angle of velocity vector from true north)
Initial computations
λ = a tan 2( r e [1], r e [0])
p = r e [0] * r e [0] + r e [1] * r e [1]
Eqs. 35
r e [ 2] 1.0
ϕ = tan −1
p * 1 − e * e
Loop through the following until arbitrary small change in latitude
N =
( RE )
2
Also note that GPS raw data is supplied in the ECEF frame, which makes
implementing a blended INS/GPS navigation solution more straight forward in the
ECEF frame.
VIII) References