def Fz(z,dT,mu):
S = Stumpff.S(z)
C = Stumpff.C(z)
y = absr1 + absr2 + A * ((z*S-1)/n.sqrt(C))
F = (y/C)**1.5 * S + A*n.sqrt(y) - n.sqrt(mu)*dT
return F
def Fzdash(z):
S = Stumpff.S(z)
C = Stumpff.C(z)
y = absr1 + absr2 + A * ((z*S-1)/n.sqrt(C))
if z == 0:
F = n.sqrt(2)/40 * y**1.5 + A/8 * (n.sqrt(y) + A*n.sqrt(1/(2*y)))
else:
F = (y/C)**1.5 * (1/(2*z) * (C - (1.5*S/C)) + 0.75 * S**2/C) +\
A/8 * (3*(S/C) * n.sqrt(y) + A*n.sqrt(C/z))
return F
def Lambert(r1, r2, delta_t):
if np.cross(r1, r2)[2] >= 0: #z-component of np.cross(r1, r2)
delta_theta = np.degrees(acos((np.dot(r1, r2) / (r1_mag * r2_mag))))
else:
delta_theta = 360 - np.degrees(
cos((np.dot(r1, r2) / (r1_mag * r2_mag))))
A = sin(radians(delta_theta)) * sqrt(r1_mag * r2_mag /
(1 - cos(radians(delta_theta))))
z = 1.5
def F1(z):
y = r1_mag + r2_mag + A * (z * Stumpff.stumpS(z) - 1) / sqrt(
Stumpff.stumpC(z))
F = (y / Stumpff.stumpC(z))**(1.5) * Stumpff.stumpS(z) + (
A * sqrt(y)) - (delta_t * sqrt(mu))
x1 = (1 / (2 * z)) * (Stumpff.stumpC(z) - (3 * Stumpff.stumpS(z) /
(2 * Stumpff.stumpC(z))))
x2 = (3 * (Stumpff.stumpS(z))**2) / (4 * Stumpff.stumpC(z))
x3 = 3 * (Stumpff.stumpS(z) / Stumpff.stumpC(z)) * sqrt(y)
x4 = A * sqrt(Stumpff.stumpC(z) / y)
F_prime = (y / Stumpff.stumpC(z))**(1.5) * (x1 + x2) + ((A / 8) *
(x3 + x4))
return F / F_prime
ratio = 1
while abs(ratio) > tol: #Newton's Method
ratio = F1(z)
z = z - ratio
y = r1_mag + r2_mag + A * (z * Stumpff.stumpS(z) - 1) / sqrt(
Stumpff.stumpC(z))
f = 1 - (y / r1_mag)
g = A * sqrt(y / mu)
g_dot = 1 - (y / r2_mag)
v1 = (1 / g) * (r2 - f * np.asarray(r1))
v2 = (1 / g) * (g_dot * np.asarray(r2) - r1)
angMomentum, SemiMajorAxis, Period, inclination, RAAN, eccentricity, omega, trueAnomaly = OrbitalElements.computeOrbitalElements(
r1, v1)
return angMomentum, SemiMajorAxis, Period, inclination, RAAN, eccentricity, omega, trueAnomaly
def lagrange_g(delta_t, chi, z):
g = delta_t - (chi**3 * Stumpff.stumpS(z)/sqrt(mu))
return g
def lagrange_f(r0_mag, chi, z):
f = 1 - (chi**2 * Stumpff.stumpC(z)/r0_mag)
return f
def lagrange_gdot(r_mag, chi, z):
gdot = 1 - (chi**2 * Stumpff.stumpC(z)/r_mag)
return gdot
def lagrange_fdot(r_mag, r0_mag, alpha, chi, z):
fdot = (sqrt(mu)/(r_mag * r0_mag)) * (alpha * chi**3 * Stumpff.stumpS(z) - chi)
return fdot
def F1(z):
y = r1_mag + r2_mag + A * (z * Stumpff.stumpS(z) - 1) / sqrt(
Stumpff.stumpC(z))
F = (y / Stumpff.stumpC(z))**(1.5) * Stumpff.stumpS(z) + (
A * sqrt(y)) - (delta_t * sqrt(mu))
x1 = (1 / (2 * z)) * (Stumpff.stumpC(z) - (3 * Stumpff.stumpS(z) /
(2 * Stumpff.stumpC(z))))
x2 = (3 * (Stumpff.stumpS(z))**2) / (4 * Stumpff.stumpC(z))
x3 = 3 * (Stumpff.stumpS(z) / Stumpff.stumpC(z)) * sqrt(y)
x4 = A * sqrt(Stumpff.stumpC(z) / y)
F_prime = (y / Stumpff.stumpC(z))**(1.5) * (x1 + x2) + ((A / 8) *
(x3 + x4))
return F / F_prime
def compute_fchi_derivative(r0, vr0):
D = (r0 * vr0 / sqrt(mu)) * (chi) * (1 -
(alpha *
(chi)**2 * Stumpff.stumpS(z)))
E = (1 - alpha * r0) * (chi)**2 * Stumpff.stumpC(z) + r0
return D + E
def compute_fchi(r0, vr0, delta_t):
A = (r0 * vr0 / sqrt(mu)) * (chi)**2 * Stumpff.stumpC(z)
B = (1 - alpha * r0) * (chi)**3 * Stumpff.stumpS(z)
C = r0 * chi - (sqrt(mu) * delta_t)
return A + B + C
def lambert(r1, r2, dT, mu):
global absr1, absr2, A
nmax = 1000
count = 0
ratio = 1
z = 0
ratio = 1
absr1 = n.linalg.norm(r1)
absr2 = n.linalg.norm(r2)
if n.cross(r1, r2)[2] >= 0:
dtheta = n.arccos((n.dot(r1, r2))/(absr1*absr2))
elif n.cross(r1, r2)[2] < 0:
dtheta = 2*n.pi - n.arccos(n.dot(r1, r2)/(absr1*absr2))
A = n.sin(dtheta) * n.sqrt((absr1*absr2)/(1-n.cos(dtheta)))
#Solve for the z value using Newton's method:
while abs(ratio) > 1e-8 and count <= nmax:
ratio = Fz(z, dT,mu)/Fzdash(z)
z = z - ratio
count = count+1
#Update Stumpff functions given the computed value of z:
S = Stumpff.S(z)
C = Stumpff.C(z)
y = absr1 + absr2 + A * ((z*S - 1)/n.sqrt(C))
#Determine the lagrange coefficients:
f = 1 - y/absr1
g = A * n.sqrt(y/mu)
fdot = n.sqrt(mu/(absr1*absr2)**2) * n.sqrt(y/C) * (z*S - 1)
gdot = 1 - y/absr2
#Now, calculate the new velocity vectors at positions r1 and r2
v1 = 1/g * (r2 - f*r1)
v2 = 1/g * (gdot*r2 - r1)
#Finally, given the position and velocity at one of these points, orbital
#elements can be calculated:
State = Transf.State2Orb(r1, v1, mu)
State2 = Transf.State2Orb(r2, v2, mu)
print("Lambert's Problem, computed elements: \n")
print("Momentum: ", State[0], " km3/s2")
print("Semi-major axis: ", State[1], " km")
print("Eccentricity: ", State[2])
print("RAAN: ", State[3], " deg")
print("Inclination: ", State[4], " deg")
print("Argument of Perigee: ", State[5], " deg")
print("True Anomaly: ", State[6], " deg")
return State, State2