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 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)
