"""
Orbital functions
-----------------
Functions used within multiple orbital classes in Stone Soup
"""
import numpy as np
from . import dotproduct
from ..types.array import StateVector, StateVectors
[docs]
def stumpff_s(z):
r"""The Stumpff S function
.. math::
S(z) = \begin{cases}\frac{\sqrt(z) - \sin{\sqrt(z)}}{(\sqrt(z))^{3}}, & (z > 0)\\
\frac{\sinh(\sqrt(-z)) - \sqrt(-z)}{(\sqrt(-z))^{3}}, & (z < 0) \\
\frac{1}{6}, & (z = 0)\end{cases}
Parameters
----------
z : float, array-like
input parameter, :math:`z` or :math:`[z]`
Returns
-------
: float, array-like
Output value, :math:`S(z)` in the same format and same size as input.
"""
gti = z > 0
lti = z < 0
eqi = z == 0
if not np.shape(z):
if gti:
sqz = np.sqrt(z)
out = (sqz - np.sin(sqz)) / sqz ** 3
elif lti:
sqz = np.sqrt(-z)
out = (np.sinh(sqz) - sqz) / sqz ** 3
else:
out = 1 / 6
else:
out = np.zeros(np.shape(z)).view(type(z))
out[gti] = (np.sqrt(z[gti]) - np.sin(np.sqrt(z[gti]))) / np.sqrt(z[gti]) ** 3
out[lti] = (np.sinh(np.sqrt(-z[lti])) - np.sqrt(-z[lti])) / np.sqrt(-z[lti]) ** 3
out[eqi] = 1 / 6
return out
[docs]
def stumpff_c(z):
r"""The Stumpff C function
.. math::
C(z) = \begin{cases}\frac{1 - \cos{\sqrt(z)}}{z}, & (z > 0)\\
\frac{\cosh{\sqrt(-z)} - 1}{-z}, & (z < 0) \\
\frac{1}{2}, & (z = 0)\end{cases}
Parameters
----------
z : float, array-like
input parameter, :math:`z`
Returns
-------
: float, array-like
Output value, :math:`C(z)` in same format and size as input
"""
gti = z > 0
lti = z < 0
eqi = z == 0
if not np.shape(z):
if gti:
out = (1 - np.cos(np.sqrt(z))) / np.sqrt(z) ** 2
elif lti:
out = (np.cosh(np.sqrt(-z)) - 1) / np.sqrt(-z) ** 2
else:
out = 1 / 2
else:
out = np.zeros(np.shape(z)).view(type(z))
out[gti] = (1 - np.cos(np.sqrt(z[gti]))) / np.sqrt(z[gti]) ** 2
out[lti] = (np.cosh(np.sqrt(-z[lti])) - 1) / np.sqrt(-z[lti]) ** 2
out[eqi] = 1 / 2
return out
[docs]
def universal_anomaly_newton(o_state_vector, delta_t,
grav_parameter=3.986004418e14, precision=1e-8, max_iterations=1e5):
r"""Calculate the universal anomaly via Newton's method. Algorithm 3.3 in [1]_.
Parameters
----------
o_state_vector : :class:`~.StateVector`, :class:`~.StateVectors`
The orbital state vector formed as
:math:`[r_x, r_y, r_z, \dot{r}_x, \dot{r}_y, \dot{r}_z]^T`
delta_t : timedelta
The time interval over which to estimate the universal anomaly
grav_parameter : float, optional
The universal gravitational parameter. Defaults to that of the
Earth, :math:`3.986004418 \times 10^{14} \ \mathrm{m}^{3} \
\mathrm{s}^{-2}`
precision : float, optional
For Newton's method, the difference between new and old estimates of the universal anomaly
below which the iteration stops and the answer is returned, (default = 1e-8)
max_iterations : float, optional
Maximum number of iterations allowed in while loop (default = 1e5)
Returns
-------
: numpy.ndarray
The universal anomaly, :math:`\chi`
References
----------
.. [1] Curtis H.D. 2010, Orbital Mechanics for Engineering Students, 3rd Ed., Elsevier
"""
# This should really have the calculation abstracted out and then do
# if statevector do code, else do iteration over code
# if type(o_state_vector) != StateVectors:
# o_state_vector = StateVectors([o_state_vector])
mag_r_0 = np.sqrt(dotproduct(o_state_vector[0:3, :], o_state_vector[0:3, :]))
mag_v_0 = np.sqrt(dotproduct(o_state_vector[3:6, :], o_state_vector[3:6, :]))
v_rad_0 = dotproduct(o_state_vector[3:6, :], o_state_vector[0:3, :]) / mag_r_0
root_mu = np.sqrt(grav_parameter)
inv_sma = 2 / mag_r_0 - (mag_v_0 ** 2) / grav_parameter
chi_i = root_mu * np.abs(inv_sma) * delta_t.total_seconds()
out = []
for iinv_sma, cchi_i, mmag_r_0, mmag_v_0, vv_rad_0 in \
zip(inv_sma.ravel(), chi_i.ravel(), mag_r_0.ravel(), mag_v_0.ravel(), v_rad_0.ravel()):
ratio = 1
count = 0
# Do Newton's method
while np.abs(ratio) > precision and count <= max_iterations:
z_i = iinv_sma * cchi_i ** 2
f_chi_i = mmag_r_0 * vv_rad_0 / root_mu * cchi_i ** 2 * stumpff_c(z_i) + \
(1 - iinv_sma * mmag_r_0) * cchi_i ** 3 * stumpff_s(z_i) + mmag_r_0 * cchi_i \
- root_mu * delta_t.total_seconds()
fp_chi_i = mmag_r_0 * vv_rad_0 / root_mu * cchi_i * \
(1 - iinv_sma * cchi_i ** 2 * stumpff_s(z_i)) + (1 - iinv_sma * mmag_r_0) \
* cchi_i ** 2 * stumpff_c(z_i) + mmag_r_0
ratio = f_chi_i / fp_chi_i
cchi_i = cchi_i - ratio
count += 1
out.append(cchi_i)
return np.reshape(out, np.shape(np.atleast_2d(o_state_vector[0, :])))
[docs]
def lagrange_coefficients_from_universal_anomaly(o_state_vector, delta_t,
grav_parameter=3.986004418e14,
precision=1e-8, max_iterations=1e5):
r""" Calculate the Lagrangian coefficients, f and g, and their time derivatives, by way of the
universal anomaly and the Stumpff functions [2]_.
Parameters
----------
o_state_vector : :class:`~.StateVector`, :class:`~.StateVectors`
The (Cartesian) orbital state vector,
:math:`[r_x, r_y, r_z, \dot{r}_x, \dot{r}_y, \dot{r}_z]^T`
delta_t : timedelta
The time interval over which to calculate
grav_parameter : float, optional
The universal gravitational parameter. Defaults to that of the
Earth, :math:`3.986004418 \times 10^{14} \ \mathrm{m}^{3} \
\mathrm{s}^{-2}`. Note that the units of time must be seconds.
precision : float, optional
Precision to which to calculate the :meth:`universal anomaly` (default = 1e-8). See the doc
section for that function
max_iterations : float, optional
Maximum number of iterations in determining universal anomaly (default = 1e5)
Returns
-------
: tuple(numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray)
The Lagrange coefficients, :math:`f, g, \dot{f}, \dot{g}`, in that order.
References
----------
.. [2] Bond V.R., Allman M.C. 1996, Modern Astrodynamics: Fundamentals and Perturbation
Methods, Princeton University Press
"""
# First get the universal anomaly using Newton's method
chii = universal_anomaly_newton(o_state_vector, delta_t,
grav_parameter=grav_parameter,
precision=precision, max_iterations=max_iterations)
# Get the position and velocity vectors
bold_r_0 = o_state_vector[0:3, :]
bold_v_0 = o_state_vector[3:6, :]
# Calculate the magnitude of the position and velocity vectors
r_0 = np.sqrt(dotproduct(bold_r_0, bold_r_0))
v_0 = np.sqrt(dotproduct(bold_v_0, bold_v_0))
# For convenience
root_mu = np.sqrt(grav_parameter)
inv_sma = 2 / r_0 - (v_0 ** 2) / grav_parameter
z = inv_sma * chii ** 2
# Get the Lagrange coefficients using Stumpf
f = 1 - chii ** 2 / r_0 * stumpff_c(z)
g = delta_t.total_seconds() - 1 / root_mu * chii ** 3 * \
stumpff_s(z)
# Get the position vector and magnitude of that vector
bold_r = f * bold_r_0 + g * bold_v_0
r = np.sqrt(dotproduct(bold_r, bold_r))
# and the Lagrange (time) derivatives also using Stumpf
fdot = root_mu / (r * r_0) * (inv_sma * chii ** 3 * stumpff_s(z) - chii)
gdot = 1 - (chii ** 2 / r) * stumpff_c(z)
return f, g, fdot, gdot
[docs]
def eccentric_anomaly_from_mean_anomaly(mean_anomaly, eccentricity,
precision=1e-8, max_iterations=1e5):
r"""Approximately solve the transcendental equation :math:`E - e sin E = M_e` for E. This is
an iterative process using Newton's method.
Parameters
----------
mean_anomaly : float
Current mean anomaly
eccentricity : float
Orbital eccentricity
precision : float, optional
Precision used for the stopping point in determining eccentric anomaly from mean anomaly,
(default = 1e-8)
max_iterations : float, optional
Maximum number of iterations for the while loop, (default = 1e5)
Returns
-------
: float
Eccentric anomaly of the orbit
"""
if mean_anomaly < np.pi:
ecc_anomaly = mean_anomaly + eccentricity / 2
else:
ecc_anomaly = mean_anomaly - eccentricity / 2
ratio = 1
count = 0
while np.abs(ratio) > precision and count <= max_iterations:
f = ecc_anomaly - eccentricity * np.sin(ecc_anomaly) - mean_anomaly
fp = 1 - eccentricity * np.cos(ecc_anomaly)
ratio = f / fp # Need to check conditioning
ecc_anomaly = ecc_anomaly - ratio
count += 1
return ecc_anomaly # Check whether this ever goes outside 0 < 2pi
[docs]
def tru_anom_from_mean_anom(mean_anomaly, eccentricity, precision=1e-8, max_iterations=1e5):
r"""Get the true anomaly from the mean anomaly via the eccentric anomaly
Parameters
----------
mean_anomaly : float
The mean anomaly
eccentricity : float
Eccentricity
precision : float, optional
Precision used for the stopping point in determining eccentric anomaly from mean anomaly,
(default = 1e-8)
max_iterations : float, optional
Maximum number of iterations in determining eccentric anomaly, (default = 1e5)
Returns
-------
: float
True anomaly
"""
cos_ecc_anom = np.cos(eccentric_anomaly_from_mean_anomaly(
mean_anomaly, eccentricity, precision=precision, max_iterations=max_iterations))
sin_ecc_anom = np.sin(eccentric_anomaly_from_mean_anomaly(
mean_anomaly, eccentricity, precision=precision, max_iterations=max_iterations))
# This only works for M_e < \pi
# return np.arccos(np.clip((eccentricity - cos_ecc_anom) /
# (eccentricity*cos_ecc_anom - 1), -1, 1))
return np.remainder(np.arctan2(np.sqrt(1 - eccentricity ** 2) *
sin_ecc_anom,
cos_ecc_anom - eccentricity), 2 * np.pi)
[docs]
def perifocal_position(eccentricity, semimajor_axis, true_anomaly):
r"""The position vector in perifocal coordinates calculated from the Keplerian elements
Parameters
----------
eccentricity : float
Orbit eccentricity
semimajor_axis : float
Orbit semi-major axis
true_anomaly
Orbit true anomaly
Returns
-------
: numpy.array
:math:`[r_x, r_y, r_z]` position in perifocal coordinates
"""
# Cache some trigonometric functions
c_tran = np.cos(true_anomaly)
s_tran = np.sin(true_anomaly)
# Copes with floats and (row) arrays
rot_v = np.reshape(np.array([c_tran, s_tran, np.zeros(np.shape(c_tran))]),
(3, np.shape(np.atleast_2d(true_anomaly))[1]))
return semimajor_axis * (1 - eccentricity ** 2) / (1 + eccentricity * c_tran) * rot_v
[docs]
def perifocal_velocity(eccentricity, semimajor_axis, true_anomaly, grav_parameter=3.986004418e14):
r"""The velocity vector in perifocal coordinates calculated from the Keplerian elements
Parameters
----------
eccentricity : float
Orbit eccentricity
semimajor_axis : float
Orbit semi-major axis
true_anomaly : float
Orbit true anomaly
grav_parameter : float, optional
Standard gravitational parameter :math:`\mu = G M`. Default is
:math:`3.986004418 \times 10^{14} \mathrm{m}^3 \mathrm{s}^{-2}`
Returns
-------
: numpy.narray
:math:`[\dot{r}_x, \dot{r}_y, \dot{r}_z]` velocity in perifocal coordinates
"""
# Cache some trigonometric functions
c_tran = np.cos(true_anomaly)
s_tran = np.sin(true_anomaly)
# Copes with floats and (row) arrays
rot_v = np.reshape(np.array([-s_tran, eccentricity + c_tran, np.zeros(np.shape(c_tran))]),
(3, np.shape(np.atleast_2d(true_anomaly))[1]))
a_1_e_2 = np.array(semimajor_axis).astype(float) * \
(1 - np.array(eccentricity).astype(float) ** 2)
return np.sqrt(grav_parameter / a_1_e_2) * rot_v
[docs]
def perifocal_to_geocentric_matrix(inclination, raan, argp):
r"""Return the matrix which transforms from perifocal to geocentric coordinates
Parameters
----------
inclination : float
Orbital inclination
raan : float
Orbit Right Ascension of the ascending node
argp : float
The orbit's argument of periapsis
Returns
-------
: numpy.array
The :math:`3 \times 3` array that transforms from perifocal coordinates to geocentric
coordinates
"""
# Cache some trig functions
s_incl = np.sin(inclination)
c_incl = np.cos(inclination)
s_raan = np.sin(raan)
c_raan = np.cos(raan)
s_aper = np.sin(argp)
c_aper = np.cos(argp)
# Build the matrix
return np.array([[-s_raan * c_incl * s_aper + c_raan * c_aper,
-s_raan * c_incl * c_aper - c_raan * s_aper,
s_raan * s_incl],
[c_raan * c_incl * s_aper + s_raan * c_aper,
c_raan * c_incl * c_aper - s_raan * s_aper,
-c_raan * s_incl],
[s_incl * s_aper, s_incl * c_aper, c_incl]])
[docs]
def keplerian_to_rv(state_vector, grav_parameter=3.986004418e14):
r"""Convert the Keplerian orbital elements to position, velocity state vector
Parameters
----------
state_vector : :class:`~.StateVector`, :class:`~.StateVectors`
The Keplerian orbital state vector is defined as
.. math::
X = [e, a, i, \Omega, \omega, \theta]^{T} \\
where:
:math:`e` is the orbital eccentricity (unitless),
:math:`a` the semi-major axis (m),
:math:`i` the inclination (rad),
:math:`\Omega` is the longitude of the ascending node (rad),
:math:`\omega` the argument of periapsis (rad), and
:math:`\theta` the true anomaly (rad)
grav_parameter : float, optional
Standard gravitational parameter :math:`\mu = G M`. The default is :math:`3.986004418
\times 10^{14} \mathrm{m}^3 \mathrm{s}^{-2}`
Returns
-------
: :class:`~.StateVector`, :class:`~.StateVectors`
Orbital state vector as :math:`[r_x, r_y, r_z, \dot{r}_x, \dot{r}_y, \dot{r}_z]`
Warning
-------
No checking undertaken. Assumes Keplerian elements rendered correctly as above
"""
# The (hidden) function which does this on a single StateVector
def _kep_to_rv_statevector(statevector):
# Calculate the position vector in perifocal coordinates
rx = perifocal_position(statevector[0], statevector[1], statevector[5])
# Calculate the velocity vector in perifocal coordinates
vx = perifocal_velocity(statevector[0], statevector[1], statevector[5],
grav_parameter=grav_parameter)
# Transform position (perifocal) and velocity (perifocal) into geocentric
r = perifocal_to_geocentric_matrix(statevector[2], statevector[3], statevector[4]) @ rx
v = perifocal_to_geocentric_matrix(statevector[2], statevector[3], statevector[4]) @ vx
return StateVector(np.concatenate((r, v), axis=0))
# Do this a statevector at a time to avoid having to do tensor multiplication
if type(state_vector) is StateVector:
return _kep_to_rv_statevector(state_vector)
elif type(state_vector) is StateVectors:
outrv = np.zeros(np.shape(state_vector))
for i, sv in enumerate(state_vector):
outrv[:, slice(i, i+1)] = _kep_to_rv_statevector(sv)
return StateVectors(outrv)
else:
raise TypeError(r"Input must be :class:`~.StateVector` or :class:`~.StateVectors`")
[docs]
def mod_inclination(x):
r"""Calculates the modulus of an inclination. Inclination angles are within the range :math:`0`
to :math:`\pi`.
Parameters
----------
x: float
inclination angle in radians
Returns
-------
float
Angle in radians in the range :math:`0` to :math:`+\pi`
"""
x = x % np.pi
return x
[docs]
def mod_elongitude(x):
r"""Calculates the modulus of an ecliptic longitude in which angles are within the range
:math:`0` to :math:`2 \pi`.
Parameters
----------
x: float
longitudinal angle in radians
Returns
-------
float
Angle in radians in the range :math:`0` to :math:`+2 \pi`
"""
x = x % (2 * np.pi)
return x