# 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 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):
"""
Maps eccentric anomaly angles into true anomaly angles
This function requires the orbit to be either circular or
non-rectilinear elliptic orbit
:param Ecc: eccentric anomaly (rad)
:param e: eccentric (0 <= e < 1)
:return: 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):
"""
Maps the eccentric anomaly angle into the corresponding
mean elliptic anomaly angle. Both 2D and 1D elliptic
orbit are allowed.
:param Ecc: eccentric anomaly (rad)
:param e: eccentricity (0 <= e < 1)
:return: 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):
"""
Maps true anomaly angles into eccentric anomaly angles.
This function requires the orbit to be either circular or
non-rectilinear elliptic orbit.
:param f: true anomaly angle (rad)
:param e: eccentricity (0 <= e < 1)
:return: 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):
"""
Maps true anomaly angles into hyperbolic anomaly angles.
This function requires the orbit to be hyperbolic
:param f: true anomaly angle (rad)
:param e: eccentricity (e > 1)
:return: 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):
"""
Maps hyperbolic anomaly angles into true anomaly angles.
This function requires the orbit to be hyperbolic
:param H: hyperbolic anomaly (rad)
:param e: eccentricity (e > 1)
:return: 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):
"""
Maps the hyperbolic anomaly angle H into the corresponding
mean hyperbolic anomaly angle N.
:param H: hyperbolic anomaly (rad)
:param e: eccentricity (e > 1)
:return: 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):
"""
Maps the mean elliptic anomaly angle into the corresponding
eccentric anomaly angle. Both 2D and 1D elliptic
orbit are allowed.
:param M: mean elliptic anomaly (rad)
:param e: eccentricity (0 <= e < 1)
:return: 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):
"""
Maps the mean hyperbolic anomaly angle N into the corresponding
hyperbolic anomaly angle H.
:param N: mean hyperbolic anomaly (rad)
:param e: eccentricity (e > 1)
:return: 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):
"""
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.
:param mu: gravitational parameter
:param elements: orbital elements
:return: 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):
"""
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).
:param mu: gravitational parameter
:param elements: orbital elements
:return: rVec, position vector
:return: 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):
"""
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 f 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.
:param mu: gravitational parameter
:param rVec: position vector
:param vVec: velocity vector
:return: orbital elements
Todo: 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):
"""
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).
:param mu: gravitational parameter
:param rVec: position vector
:param vVec: velocity vector
:return: 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.clip(np.dot(nVec, eVec) / n / elements.e, a_min=-1.0, a_max=1.0))
if eVec[2] < 0.0:
elements.omega = 2.0 * np.pi - elements.omega
elements.f = math.acos(np.clip(np.dot(eVec, rVec) / elements.e / r, a_min=-1.0, a_max=1.0))
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.clip(np.dot(eVec, rVec) / elements.e / r, a_min=-1.0, a_max=1.0))
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.clip(np.dot(nVec, rVec) / n / r, a_min=-1.0, a_max=1.0))
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):
"""
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
:param alt: altitude in km
:return: 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):
"""
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.
:param alt: altitude in km
:return: 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):
"""
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.
:param Cd: drag coefficient of the spacecraft
:param A: cross-sectional area of the spacecraft in m^2
:param m: mass of the spacecraft in kg
:param rvec: Inertial position vector of the spacecraft in km [x;y;z]
:param vvec: Inertial velocity vector of the spacecraft in km/s [vx;vy;vz]
:return: 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):
"""
Computes the J2_EARTH-J6_EARTH zonal gravitational perturbation
accelerations.
:param rvec: Cartesian Position vector in kilometers [x;y;z].
:param 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)
:param planet: planet variable, can be
CELESTIAL_MERCURY
CELESTIAL_VENUS
CELESTIAL_EARTH
CELESTIAL_MOON
CELESTIAL_MARS
CELESTIAL_JUPITER
CELESTIAL_URANUS
CELESTIAL_NEPTUNE
:return: 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):
"""
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
:param A: Cross-sectional area of the spacecraft that is facing the sun in m^2.
:param m: The mass of the spacecraft in kg.
:param sunvec: Position vector to the Sun in units of AU. Earth has a distance of 1 AU.
:return: 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
[docs]
def clMeanOscMap(req, J2, oe, oep, sign):
"""
First-order J2 Mapping Between Mean and Osculating Orbital Elements
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
:param req: equatorial radius
:param J2:
:param oe: classical orbit element set
:param oep:
:param sign: sgn=1:mean to osc, sgn=-1:osc to mean
"""
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(np.clip(math.sqrt(d3**2+d4**2), a_min=-1.0, a_max=1.0)) # (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
[docs]
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)
:param elements_cl: classical elements
:return: elements_eq, equinoctial elements
"""
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
[docs]
def hillFrame(rc_N, vc_N):
"""
Compute the Hill frame DCM HN
:param rc_N: inertial position vector
:param vc_N: inertial velocity vector
:return: HN: DCM that maps from the inertial frame N to the Hill (i.e. orbit) frame H
"""
ir = rc_N/la.norm(rc_N)
h = np.cross(rc_N, vc_N)
ih = h / la.norm(h)
itheta = np.cross(ih, ir)
return np.array([ir, itheta, ih])
[docs]
def rv2hill(rc_N, vc_N, rd_N, vd_N):
"""
Express the deputy position and velocity vector as chief by the chief Hill frame.
:param rc_N: chief inertial position vector
:param vc_N: chief inertial velocity vector
:param rd_N: chief deputy position vector
:param vd_N: chief deputy velocity vector
:return: rho_H, rhoPrime_H: Hill frame relative position and velocity vectors
"""
HN = hillFrame(rc_N, vc_N)
fDot = la.norm(np.cross(rc_N, vc_N))/(la.norm(rc_N)**2)
omega_HN_H = np.array([0, 0, fDot])
rho_H = np.matmul(HN, rd_N - rc_N)
rhoPrime_H = np.matmul(HN, vd_N - vc_N) - np.cross(omega_HN_H, rho_H)
return rho_H, rhoPrime_H
[docs]
def hill2rv(rc_N, vc_N, rho_H, rhoPrime_H):
"""
Map the deputy position and velocity vector relative to the chief Hill frame to inertial frame.
:param rc_N: chief inertial position vector
:param vc_N: chief inertial velocity vector
:param rho_H: deputy Hill relative position vector
:param rhoPrime_H: deputy Hill relative velocity vector
:return: rd_N, vd_N: Deputy inertial position and velocity vectors
"""
NH = hillFrame(rc_N, vc_N).transpose()
fDot = la.norm(np.cross(rc_N, vc_N)) / (la.norm(rc_N) ** 2)
omega_HN_H = np.array([0, 0, fDot])
rd_N = rc_N + np.matmul(NH, rho_H)
vd_N = vc_N + np.matmul(NH, rhoPrime_H + np.cross(omega_HN_H, rho_H))
return rd_N, vd_N