def vallado(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 dFdz(z, r1, r2, A): """ Derivative of Function F for Netwon's Method :param z: :param r1: :param r2: :param A: :return: df: derivative for Netwon's method. """ mu = mu_Earth if z == 0: C_z_i = c2(0) S_z_i = c3(0) y_0 = r1 + r2 + A * (0 * S_z_i - 1.0) / np.sqrt(C_z_i) dF = np.sqrt(2) / 40.0 * y_0**1.5 + A / 8.0 * ( np.sqrt(y_0) + A * np.sqrt(1 / 2 / y_0)) else: C_z_i = c2(z) S_z_i = c3(z) y_z = r1 + r2 + A * (z * S_z_i - 1.0) / np.sqrt(C_z_i) dF = (y_z / C_z_i)**1.5 * \ (1.0 / 2.0 / z * (C_z_i - 3.0 * S_z_i/ 2.0 / C_z_i) + 3.0 * S_z_i + 2.0 / 4.0 / C_z_i) +\ A / 8.0 * (3.0 * S_z_i / C_z_i * np.sqrt(y_z) + A * np.sqrt(C_z_i / y_z)) return dF
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_allclose(c2(psi), expected_c2) assert_allclose(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): # 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(xi_new - xi) < 1e-7: 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 kepler(k, r0, v0, tof, numiter): # 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(xi_new - xi) < 1e-7: 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 F_z_i(z, t, r1, r2, A): """ Function F for Newton's method :param z: :param t: :param r1: :param r2: :param A: :return: F: function """ mu = mu_Earth C_z_i = c2(z) S_z_i = c3(z) y_z = r1 + r2 + A * (z * S_z_i - 1.0) / np.sqrt(C_z_i) F = (y_z / C_z_i)**1.5 * S_z_i + A * np.sqrt(np.abs(y_z)) - np.sqrt(mu) * t return F
def vallado(k, r0, r, tof, short, numiter, rtol): r"""Solves the Lambert's problem. The algorithm returns the initial velocity vector and the final one, these are computed by the following expresions: .. math:: \vec{v_{o}} &= \frac{1}{g}(\vec{r} - f\vec{r_{0}}) \\ \vec{v} &= \frac{1}{g}(\dot{g}\vec{r} - \vec{r_{0}}) Therefore, the lagrange coefficients need to be computed. For the case of Lamber's problem, they can be expressed by terms of the initial and final vector: .. math:: \begin{align} f = 1 -\frac{y}{r_{o}} \\ g = A\sqrt{\frac{y}{\mu}} \\ \dot{g} = 1 - \frac{y}{r} \\ \end{align} Where y(z) is a function that depends on the :py:mod:`poliastro.core.stumpff` coefficients: .. math:: y = r_{o} + r + A\frac{zS(z)-1}{\sqrt{C(z)}} \\ A = \sin{(\Delta \nu)}\sqrt{\frac{rr_{o}}{1 - \cos{(\Delta \nu)}}} The value of z to evaluate the stump functions is solved by applying a Numerical method to the following equation: .. math:: z_{i+1} = z_{i} - \frac{F(z_{i})}{F{}'(z_{i})} Function F(z) to the expression: .. math:: F(z) = \left [\frac{y(z)}{C(z)} \right ]^{\frac{3}{2}}S(z) + A\sqrt{y(z)} - \sqrt{\mu}\Delta t Parameters ---------- k: float Gravitational Parameter r0: ~np.array Initial position vector r: ~np.array Final position vector tof: ~float Time of flight numiter: int Number of iterations to rtol: int Number of revolutions Returns ------- v0: ~np.array Initial velocity vector v: ~np.array Final velocity vector Examples -------- >>> from poliastro.core.iod import vallado >>> from astropy import units as u >>> import numpy as np >>> from poliastro.bodies import Earth >>> k = Earth.k.to(u.km ** 3 / u.s ** 2) >>> r1 = np.array([5000, 10000, 2100]) * u.km # Initial position vector >>> r2 = np.array([-14600, 2500, 7000]) * u.km # Final position vector >>> tof = 3600 * u.s # Time of flight >>> v1, v2 = vallado(k.value, r1.value, r2.value, tof.value, short=True, numiter=35, rtol=1e-8) >>> v1 = v1 * u.km / u.s >>> v2 = v2 * u.km / u.s >>> print(v1, v2) [-5.99249503 1.92536671 3.24563805] km / s [-3.31245851 -4.19661901 -0.38528906] km / s Note ---- This procedure can be found in section 5.3 of Curtis, with all the theoretical description of the problem. Analytical example can be found in the same book under name Example 5.2. """ t_m = 1 if short else -1 norm_r0 = np.dot(r0, r0)**0.5 norm_r = np.dot(r, r)**0.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))**0.5 if A == 0.0: raise RuntimeError("Cannot compute orbit, phase angle is 180 degrees") psi = 0.0 psi_low = -4 * np.pi**2 psi_up = 4 * np.pi**2 count = 0 while count < numiter: y = norm_r0_plus_norm_r + A * (psi * c3(psi) - 1) / c2(psi)**0.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)**0.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 vallado(k, r0, v0, tof, numiter): r"""Solves Kepler's Equation by applying a Newton-Raphson method. If the position of a body along its orbit wants to be computed for a specific time, it can be solved by terms of the Kepler's Equation: .. math:: E = M + e\sin{E} In this case, the equation is written in terms of the Universal Anomaly: .. math:: \sqrt{\mu}\Delta t = \frac{r_{o}v_{o}}{\sqrt{\mu}}\chi^{2}C(\alpha \chi^{2}) + (1 - \alpha r_{o})\chi^{3}S(\alpha \chi^{2}) + r_{0}\chi This equation is solved for the universal anomaly by applying a Newton-Raphson numerical method. Once it is solved, the Lagrange coefficients are returned: .. math:: \begin{align} f &= 1 \frac{\chi^{2}}{r_{o}}C(\alpha \chi^{2}) \\ g &= \Delta t - \frac{1}{\sqrt{\mu}}\chi^{3}S(\alpha \chi^{2}) \\ \dot{f} &= \frac{\sqrt{\mu}}{rr_{o}}(\alpha \chi^{3}S(\alpha \chi^{2}) - \chi) \\ \dot{g} &= 1 - \frac{\chi^{2}}{r}C(\alpha \chi^{2}) \\ \end{align} Lagrange coefficients can be related then with the position and velocity vectors: .. math:: \begin{align} \vec{r} &= f\vec{r_{o}} + g\vec{v_{o}} \\ \vec{v} &= \dot{f}\vec{r_{o}} + \dot{g}\vec{v_{o}} \\ \end{align} Parameters ---------- k: float Standard gravitational parameter. r0: ~np.array Initial position vector. v0: ~np.array Initial velocity vector. tof: float Time of flight. numiter: int Number of iterations. Returns ------- f: float First Lagrange coefficient g: float Second Lagrange coefficient fdot: float Derivative of the first coefficient gdot: float Derivative of the second coefficient Note ---- The theoretical procedure is explained in section 3.7 of Curtis in really deep detail. For analytical example, check in the same book for example 3.6. """ # Cache some results dot_r0v0 = np.dot(r0, v0) norm_r0 = np.dot(r0, r0)**0.5 sqrt_mu = k**0.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)**0.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(xi_new - xi) < 1e-7: 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 vallado(k, r0, r, tof, short, numiter, rtol): r"""Solves the Lambert's problem. The algorithm returns the initial velocity vector and the final one, these are computed by the following expresions: .. math:: \vec{v_{o}} &= \frac{1}{g}(\vec{r} - f\vec{r_{0}}) \\ \vec{v} &= \frac{1}{g}(\dot{g}\vec{r} - \vec{r_{0}}) Therefore, the lagrange coefficients need to be computed. For the case of Lamber's problem, they can be expressed by terms of the initial and final vector: .. math:: \begin{align} f = 1 -\frac{y}{r_{o}} \\ g = A\sqrt{\frac{y}{\mu}} \\ \dot{g} = 1 - \frac{y}{r} \\ \end{align} Where y(z) is a function that depends on the :py:mod:`poliastro.core.stumpff` coefficients: .. math:: y = r_{o} + r + A\frac{zS(z)-1}{\sqrt{C(z)}} \\ A = \sin{(\Delta \nu)}\sqrt{\frac{rr_{o}}{1 - \cos{(\Delta \nu)}}} The value of z to evaluate the stump functions is solved by applying a Numerical method to the following equation: .. math:: z_{i+1} = z_{i} - \frac{F(z_{i})}{F{}'(z_{i})} Function F(z) to the expression: .. math:: F(z) = \left [\frac{y(z)}{C(z)} \right ]^{\frac{3}{2}}S(z) + A\sqrt{y(z)} - \sqrt{\mu}\Delta t Parameters ---------- k: float Gravitational Parameter r0: ~np.array Initial position vector r: ~np.array Final position vector tof: ~float Time of flight numiter: int Number of iterations to rtol: int Number of revolutions Returns ------- v0: ~np.array Initial velocity vector v: ~np.array Final velocity vector Examples -------- >>> from poliastro.core.iod import vallado >>> from astropy import units as u >>> import numpy as np >>> from poliastro.bodies import Earth >>> k = Earth.k.to(u.km**3 / u.s**2) >>> r1 = np.array([5000, 10000, 2100])*u.km #Initial position vector >>> r2 = np.array([-14600, 2500, 7000])*u.km #Final position vector >>> tof = 3600*u.s #Time of fligh >>> v1, v2 = vallado(k.value, r1.value, r2.value, tof.value, short=True, numiter=35, rtol=1e-8) >>> v1 = v1*u.km / u.s >>> v2 = v2*u.km / u.s >>> print(v1, v2) [-5.99249499 1.92536673 3.24563805] km / s [-3.31245847 -4.196619 -0.38528907] km / s Note ---- This procedure can be found in section 5.3 of Curtis, with all the theoretical description of the problem. Analytical example can be found in the same book under name Example 5.2. """ if short: t_m = +1 else: t_m = -1 norm_r0 = np.dot(r0, r0) ** 0.5 norm_r = np.dot(r, r) ** 0.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)) ** 0.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) ** 0.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) ** 0.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 lamberts_method(R1, R2, delta_time): """ lamberts method that generates velocity vectors for each radius vectors that are put in here. :param R1: radius vector of measuements #1 :param R2: radius vector of measuements #2 :param delta_time: delta time between R1 and R2 measurements, in seconds. Only works for same pass :return: V1: velocity vector of R1 V2: velocity vector of R2 """ r1 = np.linalg.norm(R1) r2 = np.linalg.norm(R2) c12 = np.cross(R1, R2) theta = np.arccos(np.dot(R1, R2) / r1 / r2) # todo: automatic check for pro or retrograde orbits is needed. currently set to prograde trajectory = "prograde" inclination = 0.0 if inclination >= 0.0 and inclination < 90.0: trajectory = "prograde" if c12[2] >= 0: theta = theta else: theta = np.pi * 2 - theta if inclination >= 90.0 and inclination < 180.0: trajectory = "retrograde" if c12[2] < 0: theta = theta else: theta = np.pi * 2 - theta A = np.sin(theta) * np.sqrt(r1 * r2 / (1.0 - np.cos(theta))) z = 0.0 while F_z_i(z, delta_time, r1, r2, A) < 0.0: z = z + 0.1 tol = 1.e-8 nmax = 5000 ratio = 1 n = 0 while (np.abs(ratio) > tol) and (n <= nmax): n = n + 1 ratio = F_z_i(z, delta_time, r1, r2, A) #print(n, z, ratio) if np.isnan(ratio) == True: break ratio = ratio / dFdz(z, r1, r2, A) z = np.abs(z - ratio) C_z_i = c2(z) S_z_i = c3(z) y_z = r1 + r2 + A * (z * S_z_i - 1.0) / np.sqrt(C_z_i) f = 1.0 - y_z / r1 mu = mu_Earth g = A * np.sqrt(y_z / mu) gdot = 1.0 - y_z / r2 V1 = 1.0 / g * (np.add(R2, -np.multiply(f, R1))) V2 = 1.0 / g * (np.multiply(gdot, R2) - R1) return V1, V2
def kepler(k, r0, v0, tof, numiter): r"""Solves Kepler's Equation by applying a Newton-Raphson method. If the position of a body along its orbit wants to be computed for an specific time, it can be solved by terms of the Kepler's Equation: .. math:: E = M + e\sin{E} In this case, the equation is written in terms of the Universal Anomaly: .. math:: \sqrt{\mu}\Delta t = \frac{r_{o}v_{o}}{\sqrt{\mu}}\chi^{2}C(\alpha \chi^{2}) + (1 - \alpha r_{o})\chi^{3}S(\alpha \chi^{2}) + r_{0}\chi This equation is solved for the universal anomaly by applying a Newton-Raphson numerical method. Once it is solved, the Lagrange coefficients are returned: .. math:: \begin{align} f &= 1 \frac{\chi^{2}}{r_{o}}C(\alpha \chi^{2}) \\ g &= \Delta t - \frac{1}{\sqrt{\mu}}\chi^{3}S(\alpha \chi^{2}) \\ \dot{f} &= \frac{\sqrt{\mu}}{rr_{o}}(\alpha \chi^{3}S(\alpha \chi^{2}) - \chi) \\ \dot{g} &= 1 - \frac{\chi^{2}}{r}C(\alpha \chi^{2}) \\ \end{align} Lagrange coefficients can be related then with the position and velocity vectors: .. math:: \begin{align} \vec{r} &= f\vec{r_{o}} + g\vec{v_{o}} \\ \vec{v} &= \dot{f}\vec{r_{o}} + \dot{g}\vec{v_{o}} \\ \end{align} Parameters ---------- k: float Standard gravitational parameter r0: ~numpy.array Initial position vector v0: ~numpy.array Initial velocity vector numiter: int Number of iterations Returns ------- f: float First Lagrange coefficient g: float Second Lagrange coefficient fdot: float Derivative of the first coefficient gdot: float Derivative of the second coefficient Note ---- The theoretical procedure is explained in section 3.7 of Curtis in really deep detail. For analytical example, check in the same book for example 3.6. """ # Cache some results dot_r0v0 = np.dot(r0, v0) norm_r0 = np.dot(r0, r0) ** 0.5 sqrt_mu = k ** 0.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) ** 0.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(xi_new - xi) < 1e-7: 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 lamberts_method(R1, R2, delta_time, trajectory_cw=False): """ lamberts method that generates velocity vectors for each radius vectors that are put in here. Similar to https://esa.github.io/pykep/documentation/core.html#pykep.lambert_problem :param R1: radius vector of measuements #1 :param R2: radius vector of measuements #2 :param delta_time: delta time between R1 and R2 measurements, in seconds. Only works for same pass :param trajectory_cw: bool. True for retrograde motion (clockwise), False if counter-clock wise :return: V1: velocity vector of R1 V2: velocity vector of R2 ratio: absolute tolerance result """ r1 = np.linalg.norm(R1) r2 = np.linalg.norm(R2) c12 = np.cross(R1, R2) theta = np.arccos(np.dot(R1, R2) / r1 / r2) # todo: automatic check for pro or retrograde orbits is needed. currently set to prograde if trajectory_cw == False: trajectory = "prograde" if trajectory_cw == True: trajectory = "retrograde" #inclination = 0.0 #if inclination >= 0.0 and inclination < 90.0: if trajectory == "prograde": if c12[2] >= 0: theta = theta else: theta = np.pi * 2 - theta #if inclination >= 90.0 and inclination < 180.0: if trajectory == "retrograde": if c12[2] < 0: theta = theta else: theta = np.pi * 2 - theta A = np.sin(theta) * np.sqrt(r1 * r2 / (1.0 - np.cos(theta))) z = 0.0 while F_z_i(z, delta_time, r1, r2, A) < 0.0: z = z + 0.1 tol = 1.e-10 # tolerance nmax = 6000 # iterations ratio = 1 n = 0 while (np.abs(ratio) > tol) and (n <= nmax): n = n + 1 ratio = F_z_i(z, delta_time, r1, r2, A) if np.isnan(ratio) == True: break ratio = ratio / dFdz(z, r1, r2, A) z = np.abs(z - ratio) C_z_i = c2(z) S_z_i = c3(z) y_z = r1 + r2 + A * (z * S_z_i - 1.0) / np.sqrt(C_z_i) f = 1.0 - y_z / r1 mu = mu_Earth g = A * np.sqrt(y_z / mu) gdot = 1.0 - y_z / r2 V1 = 1.0 / g * (np.add(R2, -np.multiply(f, R1))) V2 = 1.0 / g * (np.multiply(gdot, R2) - R1) return V1, V2, np.abs(ratio)