''' '''
'''
ISC License
Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
Permission to use, copy, modify, and/or distribute this software for any
purpose with or without fee is hereby granted, provided that the above
copyright notice and this permission notice appear in all copies.
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
'''
# import required modules:
import sys
import math
import numpy as np
from numpy import linalg as la
class ClassicElements(object):
a = None
e = None
i = None
Omega = None
omega = None
f = None
rmag = None
alpha = None
rPeriap = None
rApoap = None
class EquinoctialElements(object):
a = None
P1 = None
P2 = None
Q1 = None
Q2 = None
l = None
L = None
N_DEBYE_PARAMETERS = 37 # orbitalMotion.h #
DB0_EPS = 1e-30
eps = 1e-13
maxIteration = 200
tolerance = 1e-15
AU = 149597870.693 # astronomical unit in units of kilometers #
D2R = (np.pi / 180.)
"""
Gravitational Constants mu = G*m, where m is the planet of the attracting planet. All units are km^3/s^2.
Values are obtained from SPICE kernels in http://naif.jpl.nasa.gov/pub/naif/generic_kernels/
"""
MU_SUN = 132712440023.310
MU_MERCURY = 22032.080
MU_VENUS = 324858.599
MU_EARTH = 398600.436
MU_MOON = 4902.799
MU_MARS = 42828.314
MU_JUPITER = 126712767.881
MU_SATURN = 37940626.068
MU_URANUS = 5794559.128
MU_NEPTUNE = 6836534.065
MU_PLUTO = 983.055
"""
planet information for major solar system bodies. Units are in km.
data taken from http://nssdc.gsfc.nasa.gov/planetary/planets.html
"""
# Sun #
REQ_SUN = 695000. # km #
# Mercury #
REQ_MERCURY = 2439.7 # km #
J2_MERCURY = 60.0e-6
SMA_MERCURY = 0.38709893 * AU
I_MERCURY = 7.00487 * D2R
E_MERCURY = 0.20563069
# Venus #
REQ_VENUS = 6051.8 # km #
J2_VENUS = 4.458e-6
SMA_VENUS = 0.72333199 * AU
I_VENUS = 3.39471 * D2R
E_VENUS = 0.00677323
# Earth #
REQ_EARTH = 6378.1366 # km, from SPICE #
RP_EARTH = 6356.7519 # km, from SPICE #
J2_EARTH = 1082.616e-6
J3_EARTH = -2.53881e-6
J4_EARTH = -1.65597e-6
J5_EARTH = -0.15e-6
J6_EARTH = 0.57e-6
SMA_EARTH = 1.00000011 * AU
I_EARTH = 0.00005 * D2R
E_EARTH = 0.01671022
OMEGA_EARTH = 0.00007292115 # Earth's planetary rotation rate, rad/sec #
# Moon #
REQ_MOON = 1737.4
J2_MOON = 202.7e-6
SMA_MOON = 0.3844e6
E_MOON = 0.0549
# Mars #
REQ_MARS = 3396.19 # km #
RP_MARS = 3376.2 # km #
J2_MARS = 1960.45e-6
SMA_MARS = 1.52366231 * AU
I_MARS = 1.85061 * D2R
E_MARS = 0.09341233
OMEGA_MARS = 0.00007262203 # Mars' polar rotation rate, rad/sec #
# Phobos #
REQ_PHOBOS = 11.2 # km #
# Deimos #
REQ_DEIMOS = 6.1 # km #
# Jupiter #
REQ_JUPITER = 71492.
J2_JUPITER = 14736.e-6
SMA_JUPITER = 5.20336301 * AU
I_JUPITER = 1.30530 * D2R
E_JUPITER = 0.04839266
# Saturn #
REQ_SATURN = 60268.
J2_SATURN = 16298.e-6
SMA_SATURN = 9.53707032 * AU
I_SATURN = 2.48446 * D2R
E_SATURN = 0.05415060
# Uranus #
REQ_URANUS = 25559.
J2_URANUS = 3343.43e-6
SMA_URANUS = 19.19126393 * AU
I_URANUS = 0.76986 * D2R
E_URANUS = 0.04716771
# Neptune #
REQ_NEPTUNE = 24746.
J2_NEPTUNE = 3411.e-6
SMA_NEPTUNE = 30.06896348 * AU
I_NEPTUNE = 1.76917 * D2R
E_NEPTUNE = 0.00858587
# Pluto #
REQ_PLUTO = 1137.
SMA_PLUTO = 39.48168677 * AU
I_PLUTO = 17.14175 * D2R
E_PLUTO = 0.24880766
[docs]def E2f(Ecc, e):
"""
Function: E2f
Purpose: Maps eccentric anomaly angles into true anomaly angles
This function requires the orbit to be either circular or
non-rectilinar elliptic orbit
Inputs:
Ecc = eccentric anomaly (rad)
e = eccentric (0 <= e < 1)
Outputs:
f = true anomaly (rad)
"""
if e >= 0.0 and e < 1.0:
f = 2.0 * math.atan2(math.sqrt(1.0 + e) * math.sin(Ecc / 2.0), math.sqrt(1.0 - e) * math.cos(Ecc / 2.0))
return f
raise ValueError('Error: E2f() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def E2M(Ecc, e):
"""
Function: E2M
Purpose: Maps the eccentric anomaly angle into the corresponding
mean elliptic anomaly angle. Both 2D and 1D elliptic
orbit are allowed.
Inputs:
Ecc = eccentric anomaly (rad)
e = eccentricity (0 <= e < 1)
Outputs:
M = mean elliptic anomaly (rad)
"""
if e >= 0.0 and e < 1.0:
M = Ecc - e * math.sin(Ecc)
return M
raise ValueError('Error: E2M() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def f2E(f, e):
"""
Function: f2E
Purpose: Maps true anomaly angles into eccentric anomaly angles.
This function requires the orbit to be either circular or
non-rectilinar elliptic orbit.
Inputs:
f = true anomaly angle (rad)
e = eccentricity (0 <= e < 1)
Outputs:
Ecc = eccentric anomaly (rad)
"""
if e >= 0.0 and e < 1.0:
Ecc = 2.0 * math.atan2(math.sqrt(1.0 - e) * math.sin(f / 2.0), math.sqrt(1.0 + e) * math.cos(f / 2.0))
return Ecc
raise ValueError('Error: f2E() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def f2H(f, e):
"""
Function: f2H
Purpose: Maps true anomaly angles into hyperbolic anomaly angles.
This function requires the orbit to be hyperbolic
Inputs:
f = true anomaly angle (rad)
e = eccentricity (e > 1)
Outputs:
H = hyperbolic anomaly (rad)
"""
if e > 1.0:
H = 2.0 * math.atanh(math.sqrt((e - 1.0) / (e + 1.0)) * math.tan(f / 2.0))
return H
raise ValueError('Error: f2H() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def H2f(H, e):
"""
Function: H2f
Purpose: Maps hyperbolic anomaly angles into true anomaly angles.
This function requires the orbit to be hyperbolic
Inputs:
H = hyperbolic anomaly (rad)
e = eccentricity (e > 1)
Outputs:
f = true anomaly angle (rad)
"""
if e > 1.0:
f = 2.0 * math.atan(math.sqrt((e + 1.0) / (e - 1.0)) * math.tanh(H / 2.0))
return f
raise ValueError('Error: H2f() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def H2N(H, e):
"""
Function: H2N
Purpose: Maps the hyperbolic anomaly angle H into the corresponding
mean hyperbolic anomaly angle N.
Inputs:
H = hyperbolic anomaly (rad)
e = eccentricity (e > 1)
Outputs:
N = mean hyperbolic anomaly (rad)
"""
if e > 1.0:
N = e * math.sinh(H) - H
return N
raise ValueError('Error: H2N() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def M2E(M, e):
"""
Purpose: Maps the mean elliptic anomaly angle into the corresponding
eccentric anomaly angle. Both 2D and 1D elliptic
orbit are allowed.
Inputs:
M = mean elliptic anomaly (rad)
e = eccentricity (0 <= e < 1)
Outputs:
Ecc = eccentric anomaly (rad)
"""
dE = 10.0 * eps
E1 = M
count = 0
if e >= 0.0 and e < 1.0:
while math.fabs(dE) > eps:
dE = (E1 - e * math.sin(E1) - M) / (1 - e * math.cos(E1))
E1 -= dE
count += 1
if count > maxIteration:
print('Iteration error in M2E({},{})'.format(str(M), str(e)))
dE = 0.0
return E1
raise ValueError('Error: M2E() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def N2H(N, e):
"""
Function: N2H
Purpose: Maps the mean hyperbolic anomaly angle N into the corresponding
hyperbolic anomaly angle H.
Inputs:
N = mean hyperbolic anomaly (rad)
e = eccentricity (e > 1)
Outputs:
H = hyperbolic anomaly (rad)
"""
dH = 10.0 * eps
H1 = N
count = 0
if e > 1.0:
while math.fabs(dH) > eps:
dH = (e * math.sinh(H1) - H1 - N) / (e * math.cosh(H1) - 1.0)
H1 -= dH
count += 1
if count > maxIteration:
print('Iteration error in M2E({},{})'.format(str(N), str(e)))
dH = 0.
return H1
raise ValueError('Error: N2H() received e = {}, the value of e should be 0 <= e < 1'.format(str(e)))
[docs]def elem2rv_parab(mu, elements):
"""
Function: elem2rv
Purpose: Translates the orbit elements
a - semi-major axis (km)
e - eccentricity
i - inclination (rad)
AN - ascending node (rad)
AP - argument of periapses (rad)
f - true anomaly angle (rad)
to the inertial Cartesian position and velocity vectors.
The attracting body is specified through the supplied
gravitational constant mu (units of km^3/s^2).
The code can handle the following cases:
circular: e = 0 a > 0
elliptical-2D: 0 < e < 1 a > 0
elliptical-1D: e = 1 a > 0 f = Ecc. Anom. here
parabolic: e = 1 rp = -a
hyperbolic: e > 1 a < 0
Note: to handle the parabolic case and distinguish it form the
rectilinear elliptical case, instead of passing along the
semi-major axis a in the "a" input slot, the negative radius
at periapses is supplied. Having "a" be negative and e = 1
is a then a unique identified for the code for the parabolic
case.
Inputs:
mu = gravitational parameter
elements = orbital elements
Outputs:
rVec = position vector
vVec = velocity vector
"""
# map classical elements structure into local variables #
a = elements.a
e = elements.e
i = elements.i
AN = elements.Omega
AP = elements.omega
f = elements.f
ir = np.zeros(3)
rVec = np.zeros(3)
vVec = np.zeros(3)
# TODO: Might want to have an error band on this equality #
if e == 1.0 and a > 0.0: # rectilinear elliptic orbit case #
Ecc = f # f is treated as ecc. anomaly #
r = a * (1.0 - e * math.cos(Ecc)) # orbit radius #
v = math.sqrt(2.0 * mu / r - mu / a)
ir[0] = math.cos(AN) * math.cos(AP) - math.sin(AN) * math.sin(AP) * math.cos(i)
ir[1] = math.sin(AN) * math.cos(AP) + math.cos(AN) * math.sin(AP) * math.cos(i)
ir[2] = math.sin(AP) * math.sin(i)
rVec = r * ir
if math.sin(Ecc) > 0.0:
vVec = -v * ir
else:
vVec = v * ir
else:
if e == 1.0 and a < 0.0: # parabolic case #
rp = -a # radius at periapses #
p = 2.0 * rp # semi-latus rectum #
else: # elliptic and hyperbolic cases #
p = a * (1.0 - e * e) # semi-latus rectum #
r = p / (1.0 + e * math.cos(f)) # orbit radius #
theta = AP + f # true latitude angle #
h = math.sqrt(mu * p) # orbit ang. momentum mag.
rVec[0] = r * (math.cos(AN) * math.cos(theta) - math.sin(AN) * math.sin(theta) * math.cos(i))
rVec[1] = r * (math.sin(AN) * math.cos(theta) + math.cos(AN) * math.sin(theta) * math.cos(i))
rVec[2] = r * (math.sin(theta) * math.sin(i))
vVec[0] = -mu / h * (math.cos(AN) * (math.sin(theta) + e * math.sin(AP)) + math.sin(AN) * (math.cos(
theta) + e * math.cos(AP)) * math.cos(i))
vVec[1] = -mu / h * (math.sin(AN) * (math.sin(theta) + e * math.sin(AP)) - math.cos(AN) * (math.cos(
theta) + e * math.cos(AP)) * math.cos(i))
vVec[2] = -mu / h * (-(math.cos(theta) + e * math.cos(AP)) * math.sin(i))
return rVec, vVec
[docs]def elem2rv(mu, elements):
"""
Function: elem2rv
Purpose: Translates the orbit elements
a - semi-major axis (km)
e - eccentricity
i - inclination (rad)
AN - ascending node (rad)
AP - argument of periapses (rad)
f - true anomaly angle (rad)
to the inertial Cartesian position and velocity vectors.
The attracting body is specified through the supplied
gravitational constant mu (units of km^3/s^2).
Inputs:
mu = gravitational parameter
elements = orbital elements
Outputs:
rVec = position vector
vVec = velocity vector
"""
rVec = np.zeros(3)
vVec = np.zeros(3)
if 1.0 + elements.e * math.cos(elements.f) < tolerance:
print('WARNING: Radius is near infinite in elem2rv conversion.')
# Calculate the semilatus rectum and the radius #
p = elements.a * (1.0 - elements.e * elements.e)
r = p / (1.0 + elements.e * math.cos(elements.f))
theta = elements.omega + elements.f
rVec[0] = r * (
math.cos(theta) * math.cos(elements.Omega) - math.cos(elements.i) * math.sin(theta) * math.sin(
elements.Omega))
rVec[1] = r * (
math.cos(theta) * math.sin(elements.Omega) + math.cos(elements.i) * math.sin(theta) * math.cos(
elements.Omega))
rVec[2] = r * (math.sin(theta) * math.sin(elements.i))
if math.fabs(p) < tolerance:
if math.fabs(1.0 - elements.e) < tolerance:
# Rectilinear orbit #
raise ValueError('elem2rv does not support rectilinear orbits')
# Parabola #
rp = -elements.a
p = 2.0 * rp
h = math.sqrt(mu * p)
vVec[0] = -mu / h * (math.cos(elements.Omega) * (elements.e * math.sin(elements.omega) + math.sin(theta)) +
math.cos(elements.i) * (
elements.e * math.cos(elements.omega) + math.cos(theta)) * math.sin(elements.Omega))
vVec[1] = -mu / h * (math.sin(elements.Omega) * (elements.e * math.sin(elements.omega) + math.sin(theta)) -
math.cos(elements.i) * (elements.e * math.cos(elements.omega) + math.cos(theta)) *
math.cos(elements.Omega))
vVec[2] = mu / h * (elements.e * math.cos(elements.omega) + math.cos(theta)) * math.sin(elements.i)
return rVec, vVec
[docs]def rv2elem_parab(mu, rVec, vVec):
"""
Function: rv2elem
Purpose: Translates the orbit elements inertial Cartesian position
vector rVec and velocity vector vVec into the corresponding
classical orbit elements where
a - semi-major axis (km)
e - eccentricity
i - inclination (rad)
AN - ascending node (rad)
AP - argument of periapses (rad)
f - true anomaly angle (rad)
if the orbit is rectilinear, then this will be the
eccentric or hyperbolic anomaly
The attracting body is specified through the supplied
gravitational constant mu (units of km^3/s^2).
The code can handle the following cases:
circular: e = 0 a > 0
elliptical-2D: 0 < e < 1 a > 0
elliptical-1D: e = 1 a > 0
parabolic: e = 1 a = -rp
hyperbolic: e > 1 a < 0
For the parabolic case the semi-major axis is not defined.
In this case -rp (radius at periapses) is returned instead
of a. For the circular case, the AN and AP are ill-defined,
along with the associated ie and ip unit direction vectors
of the perifocal frame. In this circular orbit case, the
unit vector ie is set equal to the normalized inertial
position vector ir.
Inputs:
mu = gravitational parameter
rVec = position vector
vVec = velocity vector
Outputs:
elements = orbital elements
TODO: (SAO) Modify this code to return true longitude of periapsis
(non-circular, equatorial), argument of latitude (circular, inclined),
and true longitude (circular, equatorial) when appropriate instead of
simply zeroing out omega and Omega
"""
dum = np.zeros(3)
dum2 = np.zeros(3)
ie = np.zeros(3)
ip = np.zeros(3)
ih = np.zeros(3)
cVec = np.zeros(3)
hVec = np.zeros(3)
ir = np.zeros(3)
elements = ClassicElements()
# compute orbit radius #
r = la.norm(rVec)
elements.rmag = r
ir = v3Normalize(rVec)
# compute the angular momentum vector #
hVec = np.cross(rVec, vVec)
h = la.norm(hVec)
# compute the eccentricity vector #
cVec = np.cross(vVec, hVec)
dum = (-mu / r) * rVec
cVec = cVec + dum
elements.e = la.norm(cVec) / mu
# compute semi-major axis #
elements.alpha = 2.0 / r - np.dot(vVec, vVec) / mu
if math.fabs(elements.alpha) > eps:
# elliptic or hyperbolic case #
elements.a = 1.0 / elements.alpha
elements.rPeriap = elements.a * (1.0 - elements.e)
elements.rApoap = elements.a * (1.0 + elements.e)
else:
# parabolic case #
elements.alpha = 0.
p = h * h / mu
rp = p / 2.0
elements.a = -rp # a is not defined for parabola, so - rp is returned instead #
elements.e = 1.0
elements.rPeriap = rp
elements.rApoap = -rp # periapses radius doesn't exist, returning -rp instead #
if h < eps: # rectilinear motion case #
dum = np.array([0.0, 0.0, 1.0])
dum2 = np.array([0.0, 1.0, 0.0])
ih = np.cross(ie, dum)
ip = np.cross(ie, dum2)
if la.norm(ih) > la.norm(ip):
ih = v3Normalize(ih)
else:
ih = v3Normalize(ip)
ip = np.cross(ih, ie)
else:
ih = v3Normalize(hVec)
if math.fabs(elements.e) > eps:
ie = (1.0 / mu / elements.e) * cVec
else:
ie = ir
# circular orbit case. Here ie, ip are arbitrary, as long as they #
# are perpenticular to the ih vector. #
ip = np.cross(ih, ie)
# compute the 3-1-3 orbit plane orientation angles #
elements.i = math.acos(ih[2])
if elements.i > eps and elements.i < np.pi - eps:
elements.Omega = math.atan2(ih[0], -ih[1])
elements.omega = math.atan2(ie[2], ip[2])
else:
elements.Omega = 0.
elements.omega = math.atan2(ie[1], ie[0])
if h < eps: # rectilinear motion case #
if elements.alpha > 0: # elliptic case #
Ecc = math.acos(1 - r * elements.alpha)
if np.dot(rVec, vVec) > 0:
Ecc = 2.0 * np.pi - Ecc
elements.f = Ecc # for this mode the eccentric anomaly is returned #
else: # hyperbolic case #
H = math.acosh(r * elements.alpha + 1)
if np.dot(rVec, vVec) < 0:
H = 2.0 * np.pi - H
elements.f = H # for this mode the hyperbolic anomaly is returned #
else:
# compute true anomaly #
dum = np.cross(ie, ir)
elements.f = math.atan2(np.dot(dum, ih), np.dot(ie, ir))
return elements
[docs]def rv2elem(mu, rVec, vVec):
"""
Function: rv2elem
Purpose: Translates the orbit elements inertial Cartesian position
vector rVec and velocity vector vVec into the corresponding
classical orbit elements where
a - semi-major axis (km)
e - eccentricity
i - inclination (rad)
AN - ascending node (rad)
AP - argument of periapses (rad)
f - true anomaly angle (rad)
if the orbit is rectilinear, then this will be the
eccentric or hyperbolic anomaly
The attracting body is specified through the supplied
gravitational constant mu (units of km^3/s^2).
Inputs:
mu = gravitational parameter
rVec = position vector
vVec = velocity vector
Outputs:
elements = orbital elements
"""
hVec = [0.0] * 3
v3 = [0.0] * 3
nVec = [0.0] * 3
eVec = [0.0] * 3
eVec2 = [0.0] * 3
elements = ClassicElements()
if (np.isnan(np.sum(rVec)) or np.isnan(np.sum(vVec))):
print("ERROR: received NAN rVec or vVec values.")
elements.a = np.NaN
elements.alpha = np.NaN
elements.e = np.NaN
elements.i = np.NaN
elements.AN = np.NaN
elements.AP = np.NaN
elements.f = np.NaN
elements.rmag = np.NaN
return
# Calculate the specific angular momentum and its magnitude #
hVec = np.cross(rVec, vVec)
h = la.norm(hVec)
p = h * h / mu
# Calculate the line of nodes #
v3 = np.array([0.0, 0.0, 1.0])
nVec = np.cross(v3, hVec)
n = la.norm(nVec)
# Orbit eccentricity and energy #
r = la.norm(rVec)
v = la.norm(vVec)
eVec = (v * v / mu - 1.0 / r) * rVec
v3 = (np.dot(rVec, vVec) / mu) * vVec
eVec = eVec - v3
elements.e = la.norm(eVec)
elements.rmag = r
elements.rPeriap = p / (1.0 + elements.e)
# compute semi-major axis #
elements.alpha = 2.0 / r - v * v / mu
if math.fabs(elements.alpha) > eps:
# elliptic or hyperbolic case #
elements.a = 1.0 / elements.alpha
elements.rApoap = p / (1.0 - elements.e)
else:
rp = p / 2.
elements.a = -rp
elements.rApoap = -1.0
# Calculate the inclination #
elements.i = math.acos(hVec[2] / h)
if elements.e >= 1e-11 and elements.i >= 1e-11:
# Case 1: Non-circular, inclined orbit #
elements.Omega = math.acos(nVec[0] / n)
if nVec[1] < 0.0:
elements.Omega = 2.0 * np.pi - elements.Omega
elements.omega = math.acos(np.dot(nVec, eVec) / n / elements.e)
if eVec[2] < 0.0:
elements.omega = 2.0 * np.pi - elements.omega
elements.f = math.acos(np.dot(eVec, rVec) / elements.e / r)
if np.dot(rVec, vVec) < 0.0:
elements.f = 2.0 * np.pi - elements.f
elif elements.e >= 1e-11 and elements.i < 1e-11:
# Case 2: Non-circular, equatorial orbit #
# Equatorial orbit has no ascending node #
elements.Omega = 0.0
# True longitude of periapsis, omegatilde_true #
elements.omega = math.acos(eVec[0] / elements.e)
if eVec[1] < 0.0:
elements.omega = 2.0 * np.pi - elements.omega
elements.f = math.acos(np.dot(eVec, rVec) / elements.e / r)
if np.dot(rVec, vVec) < 0.0:
elements.f = 2.0 * np.pi - elements.f
elif elements.e < 1e-11 and elements.i >= 1e-11:
# Case 3: Circular, inclined orbit #
elements.Omega = math.acos(nVec[0] / n)
if nVec[1] < 0.0:
elements.Omega = 2.0 * np.pi - elements.Omega
elements.omega = 0.0
# Argument of latitude, u = omega + f #
elements.f = math.acos(np.dot(nVec, rVec) / n / r)
if rVec[2] < 0.0:
elements.f = 2.0 * np.pi - elements.f
elif elements.e < 1e-11 and elements.i < 1e-11:
# Case 4: Circular, equatorial orbit #
elements.Omega = 0.0
elements.omega = 0.0
# True longitude, lambda_true #
elements.f = math.acos(rVec[0] / r)
if rVec[1] < 0:
elements.f = 2.0 * np.pi - elements.f
else:
print("Error: rv2elem couldn't identify orbit type.")
if elements.e > 1.0 and math.fabs(elements.f) > np.pi:
twopiSigned = math.copysign(2.0 * np.pi, elements.f)
elements.f -= twopiSigned
return elements
[docs]def atmosphericDensity(alt):
"""
* Function: atmosphericDensity
* Purpose: This program computes the atmospheric density based on altitude
* supplied by user. This function uses a curve fit based on
* atmospheric data from the Standard Atmosphere 1976 Data. This
* function is valid for altitudes ranging from 100km to 1000km.
*
* Note: This code can only be applied to spacecraft orbiting the Earth
* Inputs:
* alt = altitude in km
* Outputs:
* density = density at the given altitude in kg/m^3
"""
# Smooth exponential drop-off after 1000 km #
if alt > 1000.:
logdensity = (-7E-05) * alt - 14.464
density = math.pow(10., logdensity)
return density
# Calculating the density based on a scaled 6th order polynomial fit to the log of density #
val = (alt - 526.8000) / 292.8563
logdensity = 0.34047 * math.pow(val, 6) - 0.5889 * math.pow(val, 5) - 0.5269 * math.pow(val, 4) \
+ 1.0036 * math.pow(val, 3) + 0.60713 * math.pow(val, 2) - 2.3024 * val - 12.575
# Calculating density by raising 10 to the log of density #
density = math.pow(10., logdensity)
return density
[docs]def debyeLength(alt):
"""
* Function: debyeLength
* Purpose: This program computes the debyeLength length for a given
* altitude and is valid for altitudes ranging
* from 200 km to GEO (35000km). However, all values above
* 1000 km are HIGHLY speculative at this point.
* Inputs:
* alt = altitude in km
* Outputs:
* debye = debye length given in m
"""
X = [200.0, 250.0, 300.0, 350.0, 400., 450., 500., 550., 600., 650., 700., 750., 800., 850.,
900., 950., 1000., 1050., 1100., 1150., 1200., 1250., 1300., 1350., 1400., 1450.,
1500., 1550., 1600., 1650., 1700., 1750., 1800., 1850., 1900., 1950., 2000.]
Y = [5.64E-03, 3.92E-03, 3.24E-03, 3.59E-03, 4.04E-03, 4.28E-03, 4.54E-03, 5.30E-03, 6.55E-03,
7.30E-03, 8.31E-03, 8.38E-03, 8.45E-03, 9.84E-03, 1.22E-02, 1.37E-02, 1.59E-02, 1.75E-02,
1.95E-02, 2.09E-02, 2.25E-02, 2.25E-02, 2.25E-02, 2.47E-02, 2.76E-02, 2.76E-02, 2.76E-02,
2.76E-02, 2.76E-02, 2.76E-02, 2.76E-02, 3.21E-02, 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02, 3.96E-02]
# Flat debyeLength length for altitudes above 2000 km #
if alt > 2000.0 and alt <= 30000.0:
alt = 2000.0
elif alt > 30000.0 and alt <= 35000.0:
debyedist = 0.1 * alt - 2999.7
return debyedist
elif alt < 200.0 or alt > 35000.0:
raise ValueError("ERROR: debyeLength() received alt = {}, the value of alt should be in the range "
"of [200 35000].".format(str(alt)))
# Interpolation of data #
i = 0
for i in range(0, N_DEBYE_PARAMETERS - 1):
if X[i + 1] > alt:
break
a = (alt - X[i]) / (X[i + 1] - X[i])
debyedist = Y[i] + a * (Y[i + 1] - Y[i])
return debyedist
[docs]def atmosphericDrag(Cd, A, m, rvec, vvec):
"""
* Function: atmosphericDrag
* Purpose: This program computes the atmospheric drag acceleration
* vector acting on a spacecraft.
* Note the acceleration vector output is inertial, and is
* only valid for altitudes up to 1000 km.
* Afterwards the drag force is zero. Only valid for Earth.
* Inputs:
* Cd = drag coefficient of the spacecraft
* A = cross-sectional area of the spacecraft in m^2
* m = mass of the spacecraft in kg
* rvec = Inertial position vector of the spacecraft in km [x;y;z]
* vvec = Inertial velocity vector of the spacecraft in km/s [vx;vy;vz]
* Outputs:
* advec = The inertial acceleration vector due to atmospheric
* drag in km/sec^2
"""
# find the altitude and velocity #
r = la.norm(rvec)
v = la.norm(vvec)
alt = r - REQ_EARTH
advec = np.zeros(3)
# Checking if user supplied a orbital position is insede the earth #
if alt <= 0.:
print("ERROR: atmosphericDrag() received rvec = [{} {} {}].". \
format(str(rvec[1]), str(rvec[2]), str(rvec[3])))
print('The value of rvec should produce a positive altitude for the Earth.')
advec.fill(np.NaN)
return
# get the Atmospheric density at the given altitude in kg/m^3 #
density = atmosphericDensity(alt)
# compute the magnitude of the drag acceleration #
ad = ((-0.5) * density * (Cd * A / m) * (math.pow(v * 1000., 2))) / 1000.
# computing the vector for drag acceleration #
advec = (ad / v) * vvec
return advec
[docs]def jPerturb(rvec, num, planet):
"""
* Function: jPerturb
* Purpose: Computes the J2_EARTH-J6_EARTH zonal graviational perturbation
* accelerations.
* Inputs:
* rvec = Cartesian Position vector in kilometers [x;y;z].
* num = Corresponds to which J components to use,
* must be an integer between 2 and 6.
* (note: Additive- 2 corresponds to J2_EARTH while 3 will
* correspond to J2_EARTH + J3_EARTH)
* planet:
CELESTIAL_MERCURY
CELESTIAL_VENUS
CELESTIAL_EARTH
CELESTIAL_MOON
CELESTIAL_MARS
*CELESTIAL_PHOBOS
*CELESTIAL_DEIMOS
CELESTIAL_JUPITER
*CELESTIAL_SATURN
CELESTIAL_URANUS
CELESTIAL_NEPTUNE
*CELESTIAL_PLUTO
*CELESTIAL_SUN
* Outputs:
* ajtot = The total acceleration vector due to the J
* perturbations in km/sec^2 [accelx;accely;accelz]
"""
ajtot = np.zeros(3)
# default #
mu = MU_EARTH
req = REQ_EARTH
J2 = J2_EARTH
J3 = J3_EARTH
J4 = J4_EARTH
J5 = J5_EARTH
J6 = J6_EARTH
if planet == 'CELESTIAL_MERCURY':
mu = MU_MERCURY
req = REQ_MERCURY
J2 = J2_MERCURY
J3 = 0.0
J4 = 0.0
J5 = 0.0
J6 = 0.0
elif planet == 'CELESTIAL_VENUS':
mu = MU_VENUS
req = REQ_VENUS
J2 = J2_VENUS
J3 = 0.0
J4 = 0.0
J5 = 0.0
J6 = 0.0
elif planet == 'CELESTIAL_MOON':
mu = MU_MOON
req = REQ_MOON
J2 = J2_MOON
J3 = 0.0
J4 = 0.0
J5 = 0.0
J6 = 0.0
elif planet == 'CELESTIAL_MARS':
mu = MU_MARS
req = REQ_MARS
J2 = J2_MARS
J3 = 0.0
J4 = 0.0
J5 = 0.0
J6 = 0.0
elif planet == 'CELESTIAL_JUPITER':
mu = MU_JUPITER
req = REQ_JUPITER
J2 = J2_JUPITER
J3 = 0.0
J4 = 0.0
J5 = 0.0
J6 = 0.0
elif planet == 'CELESTIAL_URANUS':
mu = MU_URANUS
req = REQ_URANUS
J2 = J2_URANUS
J3 = 0.0
J4 = 0.0
J5 = 0.0
J6 = 0.0
elif planet == 'CELESTIAL_NEPTUNE':
mu = MU_NEPTUNE
req = REQ_NEPTUNE
J2 = J2_NEPTUNE
J3 = 0.0
J4 = 0.0
J5 = 0.0
J6 = 0.0
elif planet == 'CELESTIAL_PLUTO':
return
elif planet == 'CELESTIAL_SUN':
return
# Calculate the J perturbations #
x = rvec[0]
y = rvec[1]
z = rvec[2]
r = la.norm(rvec)
# Error Checking #
if num < 2 or num > 6:
raise ValueError("ERROR: jPerturb() received num = {}.The value of num should be 2 <= num <= 6.".format(str(num)))
# Calculating the total acceleration based on user input #
if num >= 2:
ajtot = np.array([(1.0 - 5.0 * math.pow(z / r, 2.0)) * (x / r),
(1.0 - 5.0 * math.pow(z / r, 2.0)) * (y / r),
(3.0 - 5.0 * math.pow(z / r, 2.0)) * (z / r)])
ajtot = (-3.0 / 2.0 * J2 * (mu / math.pow(r, 2.0)) * math.pow(req / r, 2.0)) * ajtot
if num >= 3:
temp = np.array([5.0 * (7.0 * math.pow(z / r, 3.0) - 3.0 * (z / r)) * (x / r),
5.0 * (7.0 * math.pow(z / r, 3.0) - 3.0 * (z / r)) * (y / r),
-3.0 * (10.0 * math.pow(z / r, 2.0) - (35.0 / 3.0) * math.pow(z / r, 4.0) - 1.0)])
temp2 = (1.0 / 2.0 * J3 * (mu / math.pow(r, 2.0)) * math.pow(req / r, 3.0)) * temp
ajtot = ajtot + temp2
if num >= 4:
temp = np.array([(3.0 - 42.0 * math.pow(z / r, 2.0) + 63.0 * math.pow(z / r, 4.0)) * (x / r),
(3.0 - 42.0 * math.pow(z / r, 2.0) + 63.0 * math.pow(z / r, 4.0)) * (y / r),
(15.0 - 70.0 * math.pow(z / r, 2) + 63.0 * math.pow(z / r, 4.0)) * (z / r)])
temp2 = (5.0 / 8.0 * J4 * (mu / math.pow(r, 2.0)) * math.pow(req / r, 4.0)) * temp
ajtot = ajtot + temp2
if num >= 5:
temp = np.array([3.0 * (35.0 * (z / r) - 210.0 * math.pow(z / r, 3.0) + 231.0 * math.pow(z / r, 5.0)) * (x / r),
3.0 * (35.0 * (z / r) - 210.0 * math.pow(z / r, 3.0) + 231.0 * math.pow(z / r, 5.0)) * (y / r),
-(15.0 - 315.0 * math.pow(z / r, 2.0) + 945.0 * math.pow(z / r, 4.0) - 693.0 * math.pow(z / r,
6.0))])
temp2 = (1.0 / 8.0 * J5 * (mu / math.pow(r, 2.0)) * math.pow(req / r, 5.0)) * temp
ajtot = ajtot + temp2
if num >= 6:
temp = np.array([(35.0 - 945.0 * math.pow(z / r, 2) + 3465.0 * math.pow(z / r, 4.0) - 3003.0 * math.pow(z / r,
6.0)) * (
x / r),
(35.0 - 945.0 * math.pow(z / r, 2.0) + 3465.0 * math.pow(z / r, 4.0) - 3003.0 * math.pow(z / r,
6.0)) * (
y / r),
-(3003.0 * math.pow(z / r, 6.0) - 4851.0 * math.pow(z / r, 4.0) + 2205.0 * math.pow(z / r,
2.0) - 245.0) * (
z / r)])
temp2 = (-1.0 / 16.0 * J6 * (mu / math.pow(r, 2.0)) * math.pow(req / r, 6.0)) * temp
ajtot = ajtot + temp2
return ajtot
[docs]def solarRad(A, m, sunvec):
"""
* Function: solarRad
* Purpose: Computes the inertial solar radiation force vectors
* based on cross-sectional Area and mass of the spacecraft
* and the position vector of the planet to the sun.
* Note: It is assumed that the solar radiation pressure decreases
* quadratically with distance from sun (in AU)
*
* Solar Radiation Equations obtained from
* Earth Space and Planets Journal Vol. 51, 1999 pp. 979-986
* Inputs:
* A = Cross-sectional area of the spacecraft that is facing
* the sun in m^2.
* m = The mass of the spacecraft in kg.
* sunvec = Position vector to the Sun in units of AU.
* Earth has a distance of 1 AU.
* Outputs:
* arvec = The inertial acceleration vector due to the effects
* of Solar Radiation pressure in km/sec^2. The vector
* components of the output are the same as the vector
* components of the sunvec input vector.
"""
# Solar Radiation Flux #
flux = 1372.5398
# Speed of light #
c = 299792458.
# Radiation pressure coefficient #
Cr = 1.3
# Magnitude of position vector #
sundist = la.norm(sunvec)
# Computing the acceleration vector #
arvec = ((-Cr * A * flux) / (m * c * math.pow(sundist, 3)) / 1000.) * sunvec
return arvec
def v3Normalize(v):
result = np.zeros(3)
norm = la.norm(v)
if norm > DB0_EPS:
result = (1. / norm) * v
return result
def clMeanOscMap(req, J2, oe, oep, sign):
# Classical orbital elements = (a,e,i,Omega,omega,f)
# First-order J2 Mapping Between Mean and Osculating Orbital Elements
# sgn=1:mean to osc, sgn=-1:osc to mean
# Analytical Mechanics of Space Systems
# Hanspeter Schaub, John L. Junkins, 4th edition.
# [m] or [km] should be the same both for req and elements.a
a = oe.a
e = oe.e
i = oe.i
Omega = oe.Omega
omega = oe.omega
f = oe.f
E = f2E(f, e)
M = E2M(E, e)
gamma2 = sign*J2/2*((req/oe.a)**2)
eta = math.sqrt(1-oe.e*oe.e)
gamma2p = gamma2/(eta**4)
a_r = (1+oe.e*math.cos(oe.f))/(eta**2)
# calculate oep.a
ap = oe.a + oe.a*gamma2*((3*(math.cos(oe.i))**2-1)*(a_r**3-1/(eta**3)) \
+3*(1-(math.cos(oe.i))**2)*(a_r**3)*math.cos(2*oe.omega+2*oe.f)) # (F.7)
de1 = gamma2p/8*e*(eta**2)*(1-11*((math.cos(i))**2)-40*((math.cos(i)) **4) \
/(1-5*((math.cos(i))**2)))*math.cos(2*omega) # (F.8)
de = de1 + (eta ** 2) / 2 * (gamma2 *((3 * ((math.cos(i)) ** 2) - 1) / (eta ** 6) \
*(e * eta + e / (1 + eta) + 3 * math.cos(f) + 3 * e * ((math.cos(f)) ** 2) + (e ** 2) \
*((math.cos(f)) ** 3)) + 3 * (1 - ((math.cos(i)) ** 2)) / (eta ** 6) \
*(e + 3 * math.cos(f) + 3 * e * ((math.cos(f)) ** 2) + (e ** 2) * ((math.cos(f)) ** 3)) * math.cos(2 * omega + 2 * f)) \
- gamma2p * (1 - ((math.cos(i)) ** 2)) *(3 * math.cos(2 * omega + f) + math.cos(2 * omega + 3 * f))) # (F.9)
di = -e*de1/(eta**2)/math.tan(i) + gamma2p/2*math.cos(i)*math.sqrt(1-((math.cos(i))**2)) \
*(3*math.cos(2*omega+2*f) + 3*e*math.cos(2*omega+f)+e*math.cos(2*omega+3*f)) # (F.10)
MpopOp = M + omega + Omega + gamma2p / 8 * (eta ** 3) * (1 - 11 * ((math.cos(i)) ** 2) \
- 40 *((math.cos(i)) ** 4) / (1 - 5 * ((math.cos(i)) ** 2))) * math.sin(2 * omega) \
- gamma2p / 16 * (2 + (e ** 2) - 11 * (2 + 3 * (e ** 2)) * ((math.cos(i)) ** 2) - 40 * (2 + 5 * (e ** 2)) \
*((math.cos(i)) ** 4) / (1 - 5 * ((math.cos(i)) ** 2)) - 400 * (e ** 2) * ((math.cos(i)) ** 6) \
/((1 - 5 * ((math.cos(i)) ** 2)) ** 2)) * math.sin(2 * omega) \
+ gamma2p / 4 * (-6 *(1 - 5 * ((math.cos(i)) ** 2)) * (f - M + e * math.sin(f)) + (3 - 5 * ((math.cos(i)) ** 2)) \
*(3 * math.sin(2 * omega + 2 * f) + 3 * e * math.sin(2 * omega + f) + e * math.sin(2 * omega + 3 * f))) \
- gamma2p / 8 * (e ** 2) * math.cos(i) * (11 + 80 *((math.cos(i)) ** 2) / (1 - 5 * ((math.cos(i)) ** 2)) \
+ 200 * ((math.cos(i)) ** 4) / ((1 - 5 * ((math.cos(i)) ** 2)) ** 2)) * math.sin(2 * omega) \
- gamma2p / 2 * math.cos(i) * (6 *(f - M + e * math.sin(f)) - 3 * math.sin(2 * omega + 2 * f) \
- 3 * e * math.sin(2 * omega + f) - e * math.sin(2 * omega + 3 * f)) # (F.11)
edM = gamma2p / 8 * e * (eta ** 3) * (1 - 11 * ((math.cos(i)) ** 2) - 40 * ((math.cos(i)) ** 4) \
/(1 - 5 * ((math.cos(i)) ** 2))) * math.sin(2 * omega) - gamma2p / 4 * (eta ** 3) * (2 * (3 * ((math.cos(i)) ** 2) - 1) \
* ((a_r * eta) ** 2 + a_r + 1) * math.sin(f) + 3 *(1 - ((math.cos(i)) ** 2)) *((-(a_r * eta) ** 2 - a_r + 1) \
* math.sin(2 * omega + f) + ((a_r * eta) ** 2 + a_r + 1 / 3) * math.sin(2 * omega + 3 * f))) # (F.12)
dOmega = -gamma2p / 8 * (e ** 2) * math.cos(i) * (11 + 80 * ((math.cos(i)) ** 2) /(1 - 5 * ((math.cos(i)) ** 2)) \
+ 200 * ((math.cos(i)) ** 4) /((1 - 5 * ((math.cos(i)) ** 2)) ** 2)) * math.sin(2 * omega) \
- gamma2p / 2 * math.cos(i) * (6 *(f - M + e * math.sin(f)) - 3 * math.sin(2 * omega + 2 * f) \
- 3 * e * math.sin(2 * omega + f) - e* math.sin(2 * omega + 3 * f)) # (F.13)
d1 = (e+de)*math.sin(M) + edM*math.cos(M) # (F.14)
d2 = (e+de)*math.cos(M) - edM*math.sin(M) # (F.15)
Mp = math.atan2(d1, d2) # (F.16)
ep = math.sqrt(d1**2+d2**2) # (F.17)
d3 = (math.sin(i/2)+math.cos(i/2)*di/2)*math.sin(Omega) + math.sin(i/2)*dOmega*math.cos(Omega) # (F.18)
d4 = (math.sin(i/2)+math.cos(i/2)*di/2)*math.cos(Omega) - math.sin(i/2)*dOmega*math.sin(Omega) # (F.19)
Omegap = math.atan2(d3, d4) # (F.20)
ip = 2*math.asin(math.sqrt(d3**2+d4**2)) # (F.21)
omegap = MpopOp - Mp - Omegap # (F.22)
Ep = M2E(Mp, ep)
fp = E2f(Ep, ep)
oep.a = ap
oep.e = ep
oep.i = ip
oep.Omega = Omegap
oep.omega = omegap
oep.f = fp
return
def clElem2eqElem(elements_cl, elements_eq):
# conversion
# from classical orbital elements (a,e,i,Omega,omega,f)
# to equinoctial orbital elements (a,P1,P2,Q1,Q2,l,L)
elements_eq.a = elements_cl.a
elements_eq.P1 = elements_cl.e * math.sin(elements_cl.Omega + elements_cl.omega)
elements_eq.P2 = elements_cl.e * math.cos(elements_cl.Omega + elements_cl.omega)
elements_eq.Q1 = math.tan(elements_cl.i / 2) * math.sin(elements_cl.Omega)
elements_eq.Q2 = math.tan(elements_cl.i / 2) * math.cos(elements_cl.Omega)
E = f2E(elements_cl.f, elements_cl.e)
M = E2M(E, elements_cl.e)
elements_eq.l = elements_cl.Omega + elements_cl.omega + M
elements_eq.L = elements_cl.Omega + elements_cl.omega + elements_cl.f
return