def _kepler(k, r0, v0, tof, numiter, rtol):
# Cache some results
dot_r0v0 = np.dot(r0, v0)
norm_r0 = np.dot(r0, r0)**.5
sqrt_mu = k**.5
alpha = -np.dot(v0, v0) / k + 2 / norm_r0
# First guess
if alpha > 0:
# Elliptic orbit
xi_new = sqrt_mu * tof * alpha
elif alpha < 0:
# Hyperbolic orbit
xi_new = (np.sign(tof) * (-1 / alpha)**.5 * np.log(
(-2 * k * alpha * tof) /
(dot_r0v0 + np.sign(tof) * np.sqrt(-k / alpha) *
(1 - norm_r0 * alpha))))
else:
# Parabolic orbit
# (Conservative initial guess)
xi_new = sqrt_mu * tof / norm_r0
# Newton-Raphson iteration on the Kepler equation
count = 0
while count < numiter:
xi = xi_new
psi = xi * xi * alpha
c2_psi = c2(psi)
c3_psi = c3(psi)
norm_r = (xi * xi * c2_psi + dot_r0v0 / sqrt_mu * xi *
(1 - psi * c3_psi) + norm_r0 * (1 - psi * c2_psi))
xi_new = xi + (sqrt_mu * tof - xi * xi * xi * c3_psi -
dot_r0v0 / sqrt_mu * xi * xi * c2_psi - norm_r0 * xi *
(1 - psi * c3_psi)) / norm_r
if abs(np.divide(xi_new - xi,
xi_new)) < rtol or abs(xi_new - xi) < rtol:
break
else:
count += 1
else:
raise RuntimeError("Maximum number of iterations reached")
# Compute Lagrange coefficients
f = 1 - xi**2 / norm_r0 * c2_psi
g = tof - xi**3 / sqrt_mu * c3_psi
gdot = 1 - xi**2 / norm_r * c2_psi
fdot = sqrt_mu / (norm_r * norm_r0) * xi * (psi * c3_psi - 1)
return f, g, fdot, gdot
def _lambert(k, r0, r, tof, short, numiter, rtol):
if short:
t_m = +1
else:
t_m = -1
norm_r0 = np.dot(r0, r0)**.5
norm_r = np.dot(r, r)**.5
norm_r0_times_norm_r = norm_r0 * norm_r
norm_r0_plus_norm_r = norm_r0 + norm_r
cos_dnu = np.dot(r0, r) / norm_r0_times_norm_r
A = t_m * (norm_r * norm_r0 * (1 + cos_dnu))**.5
if A == 0.0:
raise RuntimeError("Cannot compute orbit, phase angle is 180 degrees")
psi = 0.0
psi_low = -4 * np.pi
psi_up = 4 * np.pi
count = 0
while count < numiter:
y = norm_r0_plus_norm_r + A * (psi * c3(psi) - 1) / c2(psi)**.5
if A > 0.0:
# Readjust xi_low until y > 0.0
# Translated directly from Vallado
while y < 0.0:
psi_low = psi
psi = (0.8 * (1.0 / c3(psi)) *
(1.0 - norm_r0_times_norm_r * np.sqrt(c2(psi)) / A))
y = norm_r0_plus_norm_r + A * (psi * c3(psi) - 1) / c2(psi)**.5
xi = np.sqrt(y / c2(psi))
tof_new = (xi**3 * c3(psi) + A * np.sqrt(y)) / np.sqrt(k)
# Convergence check
if np.abs((tof_new - tof) / tof) < rtol:
break
count += 1
# Bisection check
condition = (tof_new <= tof)
psi_low = psi_low + (psi - psi_low) * condition
psi_up = psi_up + (psi - psi_up) * (not condition)
psi = (psi_up + psi_low) / 2
else:
raise RuntimeError("Maximum number of iterations reached")
f = 1 - y / norm_r0
g = A * np.sqrt(y / k)
gdot = 1 - y / norm_r
v0 = (r - f * r0) / g
v = (gdot * r - r0) / g
return v0, v
def _lambert(k, r0, r, tof, short, numiter, rtol):
if short:
t_m = +1
else:
t_m = -1
norm_r0 = np.dot(r0, r0)**.5
norm_r = np.dot(r, r)**.5
cos_dnu = np.dot(r0, r) / (norm_r0 * norm_r)
A = t_m * (norm_r * norm_r0 * (1 + cos_dnu))**.5
if A == 0.0:
raise RuntimeError("Cannot compute orbit, phase angle is 180 degrees")
psi = 0.0
psi_low = -4 * np.pi
psi_up = 4 * np.pi
count = 0
while count < numiter:
y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / c2(psi)**.5
if A > 0.0 and y < 0.0:
# Readjust xi_low until y > 0.0
# Translated directly from Vallado
while y < 0.0:
psi_low = psi
psi = (0.8 * (1.0 / c3(psi)) *
(1.0 - (norm_r0 + norm_r) * np.sqrt(c2(psi)) / A))
y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / c2(psi)**.5
xi = np.sqrt(y / c2(psi))
tof_new = (xi**3 * c3(psi) + A * np.sqrt(y)) / np.sqrt(k)
# Convergence check
if np.abs((tof_new - tof) / tof) < rtol:
break
else:
count += 1
# Bisection check
if tof_new <= tof:
psi_low = psi
else:
psi_up = psi
psi = (psi_up + psi_low) / 2
else:
raise RuntimeError("Maximum number of iterations reached")
f = 1 - y / norm_r0
g = A * np.sqrt(y / k)
gdot = 1 - y / norm_r
v0 = (r - f * r0) / g
v = (gdot * r - r0) / g
return v0, v
def test_stumpff_functions_near_zero():
psi = 0.5
expected_c2 = (1 - cos(psi**.5)) / psi
expected_c3 = (psi**.5 - sin(psi**.5)) / psi**1.5
assert_almost_equal(c2(psi), expected_c2)
assert_almost_equal(c3(psi), expected_c3)
def test_stumpff_functions_under_zero():
psi = -3.0
expected_c2 = (cosh((-psi)**.5) - 1) / (-psi)
expected_c3 = (sinh((-psi)**.5) - (-psi)**.5) / (-psi)**1.5
assert_equal(c2(psi), expected_c2)
assert_equal(c3(psi), expected_c3)
def test_stumpff_functions_above_zero():
psi = 3.0
expected_c2 = (1 - cos(psi**.5)) / psi
expected_c3 = (psi**.5 - sin(psi**.5)) / psi**1.5
assert_equal(c2(psi), expected_c2)
assert_equal(c3(psi), expected_c3)
def _kepler(k, r0, v0, tof, numiter, rtol):
# Cache some results
dot_r0v0 = dot(r0, v0)
norm_r0 = dot(r0, r0) ** .5
sqrt_mu = k**.5
alpha = -dot(v0, v0) / k + 2 / norm_r0
# First guess
if alpha > 0:
# Elliptic orbit
xi_new = sqrt_mu * tof * alpha
elif alpha < 0:
# Hyperbolic orbit
xi_new = (np.sign(tof) * (-1 / alpha)**.5 *
np.log((-2 * k * alpha * tof) /
(dot_r0v0 + np.sign(tof) *
np.sqrt(-k / alpha) * (1 - norm_r0 * alpha))))
else:
# Parabolic orbit
# (Conservative initial guess)
xi_new = sqrt_mu * tof / norm_r0
# Newton-Raphson iteration on the Kepler equation
count = 0
while count < numiter:
xi = xi_new
psi = xi * xi * alpha
c2_psi = c2(psi)
c3_psi = c3(psi)
norm_r = (xi * xi * c2_psi +
dot_r0v0 / sqrt_mu * xi * (1 - psi * c3_psi) +
norm_r0 * (1 - psi * c2_psi))
xi_new = xi + (sqrt_mu * tof - xi * xi * xi * c3_psi -
dot_r0v0 / sqrt_mu * xi * xi * c2_psi -
norm_r0 * xi * (1 - psi * c3_psi)) / norm_r
if abs(np.divide(xi_new - xi, xi_new)) < rtol or abs(xi_new - xi) < rtol:
break
else:
count += 1
else:
raise RuntimeError("Maximum number of iterations reached")
# Compute Lagrange coefficients
f = 1 - xi**2 / norm_r0 * c2_psi
g = tof - xi**3 / sqrt_mu * c3_psi
gdot = 1 - xi**2 / norm_r * c2_psi
fdot = sqrt_mu / (norm_r * norm_r0) * xi * (psi * c3_psi - 1)
return f, g, fdot, gdot