"""
Classes to solve for Keplerian orbital parameters from different initial inputs
# TODO: Provide overview of different solvers
TwoPosOrbitSolver:
GaussTwoPosOrbitSolver:
DanchickTwoPosOrbitSolver:
SheferTwoPosOrbitSolver:
ThreeAngleOrbitSolver
References:
- Shefer, V. A. (2010). New method of orbit determination from two position vectors based on solving Gauss’s equations. Solar System Research, 44, 252-266.
- Montenbruck, O., Gill, E., & Lutze, F. H. (2002). Satellite orbits: models, methods, and applications. Appl. Mech. Rev., 55(2), B27-B28.
"""
import abc
import numpy as np
from astropy.time import Time
from .orbit import Orbit
from .compute import rv
from .constants import EARTH_MU
from .utils import norm, lazy_property, newton_raphson, find_all_zeros
[docs]
class TwoPosOrbitSolver(metaclass=abc.ABCMeta):
"""
Parameters
----------
r1, r2 : (3,) array_like
Positions at t1 and t2 in meters.
t1, t2 : float or astropy.time.Time
Times of observations.
If float, then should correspond to GPS seconds; i.e., seconds since
1980-01-06 00:00:00 UTC
mu : float, optional
Gravitational constant of central body in m^3/s^2. (Default: Earth's
gravitational constant in WGS84).
kappaSign : int, optional
+1 for prograde, -1 for retrograde
lam : int, optional
Number of complete orbits between observations. (Default: 0)
eps : float, optional
Iteration tolerance.
maxiter : int, optional
Maximum number of iterations.
"""
def __init__(self, r1, r2, t1, t2, mu=EARTH_MU,
kappaSign=1, lam=0, eps=3e-16, maxiter=100):
if isinstance(t1, Time):
t1 = t1.gps
if isinstance(t2, Time):
t2 = t2.gps
self.r1 = r1
self.r2 = r2
self.t1 = t1
self.t2 = t2
self.mu = mu
self.kappaSign = kappaSign
self.lam = lam
self.eps = eps
self.maxiter = maxiter
# Preliminary calculations
self.tau = self.t2 - self.t1
self.r1mag = norm(self.r1)
self.r2mag = norm(self.r2)
self.U1 = self.r1 / self.r1mag
self.U2 = self.r2 / self.r2mag
self.r0 = self.r2 - np.dot(self.r2, self.U1) * self.U1 # M&G (2.110)
self.r0mag = norm(self.r0)
self.U0 = self.r0 / self.r0mag
self.cos2f = np.dot(self.U1, self.U2)
self.rr = np.sqrt(self.r1mag * self.r2mag)
# Shefer (8)
self.kappa = self.rr * norm(self.U1 + self.U2) * self.kappaSign
self.sigma = self.rr * norm(self.U2 - self.U1) # Shefer (8)
self.rbar = 0.5 * (self.r1mag + self.r2mag)
self.m = self.mu * self.tau**2 / self.kappa**3 # Shefer (5.5)
self.ell = self.rbar / self.kappa - 0.5 # Shefer (5.5)
# Can solve for the orbital plane immediately
self.W = np.cross(self.U1, self.U0) * self.kappaSign
# M&G (2.58)
self.raan = np.arctan2(self.W[0], -self.W[1]) % (2 * np.pi)
# M&G (2.58)
self.i = np.arctan2(self.W[0] / np.sin(self.raan), self.W[2])
# M&G (2.66)
self.u1 = np.arctan2(
self.U1[2],
-self.U1[0] * self.W[1] + self.U1[1] * self.W[0]
)
def _finishOrbit(self, p):
# M&G (2.114), good for both elliptical and hyperbolic orbits
ecosnu1 = p / self.r1mag - 1.0
ecosnu2 = p / self.r2mag - 1.0 # M&G (2.114)
# M&G (2.116)
esinnu1 = ((ecosnu1 * self.cos2f - ecosnu2)
/ (self.r0mag / self.r2mag)
* self.kappaSign)
e = np.hypot(ecosnu1, esinnu1)
nu1 = np.arctan2(esinnu1, ecosnu1) % (2 * np.pi) # trueAnomaly at t1
pa = (self.u1 - nu1) % (2 * np.pi) # M&G (2.117)
a = (p / (1 - e**2)) # M&G (2.118)
return Orbit.fromKeplerianElements(
a, e, self.i, pa, self.raan, nu1, self.t1, self.mu
)
# The main part that varies between methods (i.e., subclasses)
@abc.abstractmethod
def _getP(self):
pass
def solve(self):
p = self._getP()
return self._finishOrbit(p)
[docs]
class GaussTwoPosOrbitSolver(TwoPosOrbitSolver):
def __init__(self, *args, **kwargs):
TwoPosOrbitSolver.__init__(self, *args, **kwargs)
def _getP(self):
"""Compute p from Shefer (2)."""
# Solve for eta
eta = 1
d_eta = 1
niter = 0
while np.abs(d_eta) > self.eps and niter < self.maxiter:
x = (self.m / eta**2 - self.ell) # Shefer (5)
if 1e-15 < x < 1: # Shefer (6.5ish)
g = 2 * np.arcsin(np.sqrt(x))
X = (2 * g - np.sin(2 * g)) / np.sin(g)**3
elif x < -1e-15:
h = 2 * np.arcsinh(np.sqrt(-x))
X = (np.sinh(2 * h) - 2 * h) / np.sinh(h)**3
else: # -1e-15<x<1e-15
X = 4. / 3 + 8. / 5 * x + 64. / 35 * x
d_eta = 1. + (self.ell + x) * X - eta
eta += d_eta
niter += 1
# Plug eta into (2) to obtain p
p = (0.5 * eta * self.kappa * self.sigma / self.tau)**2 / self.mu
return p
[docs]
class DanchickTwoPosOrbitSolver(TwoPosOrbitSolver):
def __init__(self, *args, **kwargs):
TwoPosOrbitSolver.__init__(self, *args, **kwargs)
[docs]
@staticmethod
def X(g):
"""Compute X(g) from Shefer (11)."""
return (2 * g - np.sin(2 * g)) / (np.sin(g)**3)
[docs]
@staticmethod
def dXdg(g):
"""Compute dX(g)/dg from Shefer (12)."""
return (2 * (1 - np.cos(2 * g)) - 3 * (2 * g - np.sin(2 * g)) / np.tan(g)) / (np.sin(g)**3)
def _getP(self):
if self.cos2f >= 0:
# TODO: rewrite to use newton_raphson function.
eta = 0.5 * (np.sqrt(self.m / (self.ell + 1)) + np.sqrt(self.m / self.ell))
Geta = 1
niter = 0
while np.abs(Geta) > self.eps and niter < self.maxiter:
x = self.m / eta**2 - self.ell # Shefer (14)
if (1 - 2 * x) > 1 or (1 - 2 * x) < -1:
raise RuntimeError("Invalid x")
g = np.arccos(1 - 2 * x) # Shefer (9)
dgdx = 2 / np.sin(g) # Shefer (10)
dxdeta = -2 * self.m / eta**3 # next few lines are Shefer (14ish)
dXdeta = self.dXdg(g) * dgdx * dxdeta
Geta = eta - 1 - (self.ell + x) * self.X(g)
dGdeta = 1 - self.X(g) * dxdeta - (self.ell + x) * dXdeta
eta -= Geta / dGdeta
niter += 1
x = self.m / eta**2 - self.ell # Shefer (9)
elif self.cos2f < 0:
x = 0.5
Fx = 1
niter = 0
while np.abs(Fx) > self.eps and niter < self.maxiter:
if 1 - 2 * x > 1 or 1 - 2 * x < -1:
raise RuntimeError("Invalid x")
g = np.arccos(1 - 2 * x) # Shefer (9)
dgdx = 2 / np.sin(g) # Shefer (10)
eta = 1 + (self.ell + x) * self.X(g) # Shefer (15)
detadx = (self.ell + x) * self.dXdg(g) * dgdx + self.X(g)
Fx = x - self.m / eta**2 + self.ell # Shefer (15ish)
dFdx = 1 + (2 * self.m / eta**3) * detadx
x -= Fx / dFdx
niter += 1
eta = 1 + (self.ell + x) * self.X(np.arccos(1 - 2 * x))
else:
raise "Invalid value of cos2f"
# Shefer (2)
p = (0.5 * eta * self.kappa * self.sigma / self.tau)**2 / self.mu
return p
[docs]
class SheferTwoPosOrbitSolver(TwoPosOrbitSolver):
def __init__(self, *args, **kwargs):
self.robust = kwargs.pop('robust', False)
self.nExam = kwargs.pop('nExam', 100)
TwoPosOrbitSolver.__init__(self, *args, **kwargs)
[docs]
def alpha(self, x):
"""Evaluate alpha(x) from Shefer (18)."""
return (self.rbar + self.kappa * (x - 0.5)), self.kappa
[docs]
def beta(self, xi):
"""Evaluate beta(xi) and its derivative from Shefer (A.4) and (A.9)."""
val = self.rbar - 0.5 * self.kappa + xi * (self.rbar + 0.5 * self.kappa)
grad = self.rbar + 0.5 * self.kappa
return val, grad
[docs]
def Y(self, x):
"""Evaluate Y(x) and dY(x)/dx from Shefer (17)."""
XVal, XGrad = self.X(x)
alphaVal, alphaGrad = self.alpha(x)
val = self.kappa + alphaVal * XVal
# Evaluate dY(x)/dx using the chain rule
grad = alphaVal * XGrad + alphaGrad * XVal
return val, grad
def Yxi(self, xi):
betaVal, _ = self.beta(xi)
return self.kappa + betaVal * self.Z(xi)[0]
[docs]
@staticmethod
def X(x):
"""Evaluate X(x) function from Shefer (19) for elliptical orbits
and (20) for hyperbolic orbits. The derivative of X(x) is given in
(23).
"""
import astropy.units as u
if isinstance(x, u.Quantity):
x = x.value
if x < -1e-15:
om2x = 1 - 2 * x
xxm1 = x * (x - 1)
val = om2x * np.sqrt(xxm1) - np.arcsinh(np.sqrt(-x))
val /= 2 * (xxm1)**1.5
grad = 4 - 3 * (om2x) * val
grad /= -2 * xxm1
return val, grad
elif -1e-15 < x < 1e-15:
grad = 8 / 5 + 64 / 35 * x
val = grad * x + 4 / 3
return val, grad
elif x < 1.:
om2x = 1 - 2 * x
x1mx = x * (1 - x)
val = np.arcsin(np.sqrt(x)) - om2x * np.sqrt(x1mx)
val /= 2 * x1mx**1.5
grad = 4 - 3 * om2x * val
grad /= 2 * x1mx
return val, grad
else: # x > 1.
return np.inf # X(x) actually asymptotes to Infinity as x -> 1
[docs]
@staticmethod
def Z(xi):
"""Compute Z(xi) function from Shefer (A.5)."""
num = (1 + xi)**2 * np.arctanh(np.sqrt(-xi)) - (1 - xi) * np.sqrt(-xi)
denom = 2 * xi * np.sqrt(-xi)
val = num / denom
grad = (4 - (3 - xi) * val) / (2 * xi * (1 + xi))
return val, grad
[docs]
def D(self, x):
"""Compute D(x) and its derivative, dD(x)/dx, from Shefer (43)."""
aval, agrad = self.semiMajorAxis(x)
val = self.tau * np.sqrt(self.mu) - 2 * np.pi * self.lam * aval**1.5
grad = -3 * np.pi * self.lam * np.sqrt(aval) * agrad
# print("----- D: ", x, aval, agrad, self.tau, self.mu, self.lam)
return val, grad
[docs]
def semiMajorAxis(self, x):
"""Compute semi-major axis a(x) and its derivative, da(x)/dx, from
Shefer (42).
"""
assert x >= 0. and x <= 1., \
"ERROR: invalid x in Shefer semi_major_axis: {}".format(x)
alphaVal, alphaGrad = self.alpha(x)
x1mx = x * (1 - x)
val = alphaVal / (4 * x1mx)
grad = (self.kappa * x1mx - alphaVal * (1 - 2 * x)) / (4 * x1mx**2)
return val, grad
def Flam0(self, x): # Shefer (21), (22)
Y, _ = self.Y(x)
Ysq = Y**2
alphaVal, _ = self.alpha(x)
XVal, XGrad = self.X(x)
val = alphaVal * Ysq - self.mu * self.tau**2
grad = self.kappa * Ysq + 2 * alphaVal * Y * (self.kappa * XVal + alphaVal * XGrad)
return val, grad
[docs]
def F(self, x):
"""Compute F(x) and dF(x)/dx from Shefer (40) and (41)."""
if x >= 1.:
# Should not get here
val = 1.e16
grad = 1.e16
else:
alphaVal, alphaGrad = self.alpha(x)
YVal, YGrad = self.Y(x)
DVal, DGrad = self.D(x)
val = alphaVal * YVal**2 - DVal**2
grad = alphaGrad * YVal**2 + alphaVal * 2 * YVal * YGrad - 2 * DVal * DGrad
return val, grad
[docs]
def G(self, xi):
"""Compute G(xi) and its derivative from Shefer (A.7) and (A.8)."""
betaVal, betaGrad = self.beta(xi)
Y = self.Yxi(xi)
Ysq = Y**2
val = betaVal * Ysq - (1 + xi) * self.mu * self.tau**2
ZVal, ZGrad = self.Z(xi)
grad = (betaGrad * Ysq
+ 2 * betaVal * Y * (betaGrad * ZVal + betaVal * ZGrad)
- self.mu * self.tau**2)
return val, grad
def yPoly(self, y): # Shefer (44) and derivative
coef = 0.6 * (self.rbar + 1.5 * self.kappa)
ysqr = y * y
den = ysqr + self.kappa
x = (ysqr - self.rbar + 0.5 * self.kappa) / den # Shefer (36)
if x > 0 and x < 1 and self.lam > 0:
dxdy = (self.kappa + 2 * self.rbar) * y / den**2
Dval, Dgrad = self.D(x)
val = (ysqr + coef) * y - 1.2 * Dval
grad = 3 * ysqr + coef - 1.2 * Dgrad * dxdy
else:
val = (ysqr + coef) * y - 1.2 * self.tau * np.sqrt(self.mu) # Shefer (38)
grad = 3 * ysqr + coef
return val, grad
def _getInitialXGuess(self):
# Shefer step B)
if self.lam == 0:
# Shefer (38)
poly = [1.0, 0.0, 0.6 * (self.rbar + 1.5 * self.kappa),
-1.2 * (self.tau * np.sqrt(self.mu))]
roots = np.roots(poly)
# There should be exactly one positive real root.
w = np.where((np.abs(roots.imag) < 3e-16) & (roots.real > 0))
if len(w) > 1:
raise RuntimeError(
"Found more than one positive, real root! {}".format(roots)
)
if len(w) == 0:
raise RuntimeError(
"Found no positive real roots! {}".format(roots))
y = roots[w][0].real
else:
y = np.sqrt(2 * self.rbar - self.kappa)
y = newton_raphson(
y, self.yPoly, eps=self.eps, maxiter=self.maxiter
)
ysqr = y * y
# Shefer (36)
x = (ysqr - self.rbar + 0.5 * self.kappa) / (ysqr + self.kappa)
return x
def _getInitialXiGuess(self):
# Shefer (A.16)
poly = [1.0, 0.0, (3. / 13.) * (self.rbar + 3.5 * self.kappa),
-(12. / 13.) * (self.tau * np.sqrt(self.mu))]
roots = np.roots(poly)
# There should be exactly one positive real root.
w = np.where((np.abs(roots.imag) < 3e-16) & (roots.real > 0))
if len(w) > 1:
raise RuntimeError(
"Found more than one positive, real root! {}".format(roots))
if len(w) == 0:
raise RuntimeError(
"Found no positive real roots! {}".format(roots))
y = roots[w][0].real
ysqr = y * y
# Shefer (A.13)
xi = (ysqr - self.rbar + 0.5 * self.kappa) / (self.rbar + 0.5 * self.kappa)
return xi
def _getP(self):
"""Get the semi-latus rectum. This is the final result of the Shefer algorithm."""
x = self._getInitialXGuess()
if x < -1. or x > 1.:
xi = self._getInitialXiGuess()
# Shefer step C)
xi = newton_raphson(xi, self.G, eps=self.eps, maxiter=self.maxiter)
alpha = self.beta(xi)[0] / (1 + xi)
else:
if x < 0.:
self.lam = 0
# Shefer step C)
if self.lam == 0:
x = newton_raphson(
x, self.Flam0, eps=self.eps, maxiter=self.maxiter
)
else:
x = newton_raphson(
x, self.F, eps=self.eps, maxiter=self.maxiter
)
alpha = self.alpha(x)[0]
p = self.sigma**2 / (4 * alpha)
return p
def _getAllP(self):
xmin = 1e-15
xmax = 1.0
xs = find_all_zeros(lambda x: self.F(x)[0], xmin, xmax, n=self.nExam)
return [self.sigma**2 / (4 * self.alpha(x)[0]) for x in xs]
def _getEta(self, p):
"""Compute auxiliary value defined in Shefer (2)."""
return 2 * np.sqrt(p * self.mu) * self.tau / (self.kappa * self.sigma)
def solve(self):
# Try normal solution first.
orbit = TwoPosOrbitSolver.solve(self)
if not self.robust:
return orbit
r1_test, _ = rv(orbit, self.t1)
r2_test, _ = rv(orbit, self.t2)
try:
# 1 m tolerance...?
# I'd really like better than this, but probably good enough for a
# preliminary orbit determination.
np.testing.assert_allclose(self.r1, r1_test, atol=1, rtol=0)
np.testing.assert_allclose(self.r2, r2_test, atol=1, rtol=0)
except AssertionError:
pass
else:
# No AssertionError, so return early
return orbit
# Get here when there was an AssertionError above
# Didn't find a solution with Shefer initial guess, so brute-force find
# all the zeros of F and see if any of them yield a solution.
for p in self._getAllP():
orbit = self._finishOrbit(p)
r1_test, _ = rv(orbit, self.t1)
r2_test, _ = rv(orbit, self.t2)
try:
np.testing.assert_allclose(self.r1, r1_test, atol=1, rtol=0)
np.testing.assert_allclose(self.r2, r2_test, atol=1, rtol=0)
except AssertionError:
pass
else:
return orbit
# Get here when None of the p's work
raise RuntimeError("Cannot find orbit")
[docs]
class ThreeAngleOrbitSolver:
"""Determine orbit of satellite for set of three angle-only observations.
Might only work well for smallish changes in sector position of
observations.
Parameters
----------
e1, e2, e3 : (3,) array_like
Unit vectors indicating observed directions (dimensionless).
R1, R2, R3 : (3,) array_like
Vectors indicating the positions of the observers (in meters).
t1, t2, t3 : float or astropy.time.Time
Times of observations.
If float, then should correspond to GPS seconds; i.e., seconds since
1980-01-06 00:00:00 UTC
mu : float, optional
Gravitational constant of central body in m^3/s^2. (Default: Earth's
gravitational constant in WGS84).
tol : float, optional
Tolerance to use to stop iteratively improving solution.
(default 1e-8).
maxiter : int, optional
Maximum number of iterations to use. (default: 100)
"""
def __init__(self, e1, e2, e3, R1, R2, R3, t1, t2, t3, mu=EARTH_MU,
tol=1e-8, maxiter=100):
# Follows Montenbruck and Gill section 2.4.2
if isinstance(t1, Time):
t1 = t1.gps
if isinstance(t2, Time):
t2 = t2.gps
if isinstance(t3, Time):
t3 = t3.gps
self.e1 = e1
self.e2 = e2
self.e3 = e3
self.t1 = t1
self.t2 = t2
self.t3 = t3
self.R1 = R1
self.R2 = R2
self.R3 = R3
self.mu = mu
self.tol = tol
self.maxiter = maxiter
@lazy_property
def d1(self):
return np.cross(self.e2, self.e3)
@lazy_property
def d2(self):
return np.cross(self.e3, self.e1)
@lazy_property
def d3(self):
return np.cross(self.e1, self.e2)
@lazy_property
def D11(self):
return np.dot(self.d1, self.R1)
@lazy_property
def D12(self):
return np.dot(self.d1, self.R2)
@lazy_property
def D13(self):
return np.dot(self.d1, self.R3)
@lazy_property
def D21(self):
return np.dot(self.d2, self.R1)
@lazy_property
def D22(self):
return np.dot(self.d2, self.R2)
@lazy_property
def D23(self):
return np.dot(self.d2, self.R3)
@lazy_property
def D31(self):
return np.dot(self.d3, self.R1)
@lazy_property
def D32(self):
return np.dot(self.d3, self.R2)
@lazy_property
def D33(self):
return np.dot(self.d3, self.R3)
@lazy_property
def D(self):
return np.dot(self.e1, self.d1)
@lazy_property
def t21(self):
return self.t2 - self.t1
@lazy_property
def t31(self):
return self.t3 - self.t1
@lazy_property
def t32(self):
return self.t3 - self.t2
@lazy_property
def t3231(self):
return self.t32 / self.t31
@lazy_property
def t2131(self):
return self.t21 / self.t31
def _getRho(self, n1, n3):
"""Compute three unknown station-satellite distances from Montenbruck and Gill (2.129).
"""
rho1 = -1 / (n1 * self.D) * (n1 * self.D11 - self.D12 + n3 * self.D13)
rho2 = 1 / self.D * (n1 * self.D21 - self.D22 + n3 * self.D23)
rho3 = -1 / (n3 * self.D) * (n1 * self.D31 - self.D32 + n3 * self.D33)
return rho1, rho2, rho3
def _getR(self, rho1, rho2, rho3):
"""Compute three Earth-satellite position vectors from Montenbruck and Gill (2.122).
"""
r1 = self.R1 + rho1 * self.e1
r2 = self.R2 + rho2 * self.e2
r3 = self.R3 + rho3 * self.e3
return r1, r2, r3
def _getN(self, eta21, eta23):
"""Compute n1 and n3 terms from Montenbruck and Gill (2.132)."""
n1 = eta21 * self.t3231
n3 = eta23 * self.t2131
return n1, n3
def _getEta(self, r1, r2, t1, t2):
"""Use Shefer algorithm to improve eta estimates."""
solver = SheferTwoPosOrbitSolver(r1, r2, t1, t2)
p = solver._getP()
eta = solver._getEta(p)
return eta
def solve(self):
# initial estimate
eta21 = eta23 = 1.0
n1, n3 = self._getN(eta21, eta23)
# iterate to improve eta21, eta23
dn1 = np.inf
dn3 = np.inf
niter = 0
while ((np.abs(dn1) > self.tol or np.abs(dn3) > self.tol)
and niter < self.maxiter):
rho1, rho2, rho3 = self._getRho(n1, n3)
r1, r2, r3 = self._getR(rho1, rho2, rho3)
eta1 = self._getEta(r2, r3, self.t2, self.t3)
eta2 = self._getEta(r1, r3, self.t1, self.t3)
eta3 = self._getEta(r1, r2, self.t1, self.t2)
eta21 = eta2 / eta1
eta23 = eta2 / eta3
newn1, newn3 = self._getN(eta21, eta23)
dn1, n1 = newn1 - n1, newn1
dn3, n3 = newn3 - n3, newn3
niter += 1
solver = SheferTwoPosOrbitSolver(r1, r3, self.t1, self.t3)
return solver.solve()