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