def mee_ocp_central_difference(t,x,rho):
delx = 1e-3*x/norm(x)
dxdt_m = eom_mee_twobody_minfuel(t,x_m,rho)
dxdt_p = eom_mee_twobody_minfuel(t,x_p,rho)
for (j in range size(x)):
jac[i,j] = (dxdt_p[i,j] - dxdt_m[i,j])/2/delx[j]
return
def residuals_mee_ocp(p0, tspan, states0, statesf, rho):
"""
Computes error between current final and desired final state for the low thrust,
min-fuel, optimal trajectory using modified equinoctial elements.
Parameters:
===========
p0 -- current initial costate
tspan -- vector containing initial and final time
states0 -- initial states
statesf -- final states
rho -- switch smoothing parameter
Returns:
========
res -- residual
External:
=========
numpy, scipy.integrate
"""
## Computes the error between the current and desired final state.
states_in = np.zeros(14)
states_in[0:7] = states0[0:7]
states_in[7:14] = p0[0:7]
# sol = integrator(eom_mee_twobody_minfuel,(tspan[0],tspan[-1]),states_in,method='LSODA',rtol=1e-12)
sol = integrator(lambda t, y: eom_mee_twobody_minfuel(t, y, rho),
tspan,
states_in,
method='LSODA',
rtol=1e-12)
# Free final mass and free final true longitude (angle)
res = np.linalg.norm(statesf[0:5] - sol.y[0:5, -1])
return res
def residuals(p0,tspan,states0,statesf,rho):
## Computes the error between the current and desired final state.
states_in = np.zeros(14)
states_in[0:7] = states0[0:7]
states_in[7:14] = p0[0:7]
# sol = integrator(eom_mee_twobody_minfuel,(tspan[0],tspan[-1]),states_in,method='LSODA',rtol=1e-12)
sol = integrator(lambda t,y: eom_mee_twobody_minfuel(t,y,rho),tspan,states_in,method='LSODA',rtol=1e-12)
# Free final mass and free final true longitude (angle)
res = np.linalg.norm(statesf[0:5] - sol.y[0:5,-1])
# print(res)
return res
def mps_mee_ocp(tspan, p0, states0, statesf, rho, mps_tol):
# Method of particular solutions to solve Lambert's problem
mps_err = 1
cnt = 0
IC = np.zeros(14)
Del = np.zeros((5, 7))
Del_plus = np.zeros((5, 7))
Del_minus = np.zeros((5, 7))
DEL = np.array([1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5])
# p0 = states0[7:14]
MatInv = np.zeros((7, 7))
res = np.zeros(7)
del_p0 = np.zeros(7)
while mps_err > mps_tol and cnt < 30:
for tcnt in range(0, 15):
if tcnt == 0:
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
elif tcnt > 0 and tcnt < 8:
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
IC[tcnt + 6] = IC[tcnt + 6] + DEL[tcnt - 1]
elif tcnt > 7 and tcnt < 15:
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
IC[tcnt + 6 - 7] = IC[tcnt + 6 - 7] - DEL[tcnt - 1 - 7]
# Integrate Dynamics
sol = integrator(lambda t, y: eom_mee_twobody_minfuel(t, y, rho),
tspan,
IC,
method='LSODA',
rtol=1e-13)
# sol = integrator(eom_mee_twobody_minfuel,(tspan[0],tspan[-1]),IC,method='LSODA',rtol=1e-12)
soln = sol.y[0:7, -1]
if tcnt == 0:
ref = soln
traj = sol
elif tcnt > 0 and tcnt < 8:
# Del_plus[0:5,tcnt-1] = (soln[0:5]-ref[0:5])/DEL # Free final mass & free final true longitude (angle)
Del_plus[0:5, tcnt - 1] = soln[
0:5] # Free final mass & free final true longitude (angle)
elif tcnt > 7 and tcnt < 15:
Del_minus[0:5, tcnt - 1 - 7] = soln[
0:5] # Free final mass & free final true longitude (angle)
# Compute Updated Variables
for i in range(5):
for j in range(7):
Del[i, j] = (Del_plus[i, j] - Del_minus[i, j]) / (2 * DEL[j])
MatInv = np.linalg.pinv(Del)
res = statesf[0:5] - ref[
0:5] # Free final mass & free final true longitude (angle)
del_p0 = np.matmul(MatInv, res)
p0 = p0 + del_p0
DEL = 1e-5 * (p0 / np.linalg.norm(p0))
# Compute Error
mps_err = np.linalg.norm(res)
print(mps_err)
# Counter
cnt = cnt + 1
return traj, p0, mps_err
eclipse = False # Commented out in eom.py - too slow, need use a C++ eom and wrap in python)
while rho > 1e-4:
if rho < 1e-3:
mps_tol = 1e-5
rho_rate = 0.95
# Method of Particular Solutions
[soln, p0, mps_err] = mps_mee_ocp(tspan, p0, mee0, meef, rho, eclipse,
mps_tol)
print("rho:", rho, "mps err:", mps_err)
rho = rho * rho_rate
# Update
iter = iter + 1
# Final Solution
rho = rho / rho_rate # rho for converged solution
mee0_new = np.zeros(14)
mee0_new[0:7] = mee0[0:7]
mee0_new[7:14] = p0[0:7]
# Check Solution
sol = integrator(lambda t, y: eom_mee_twobody_minfuel(t, y, rho, eclipse),
tspan,
mee0_new,
method='LSODA',
rtol=1e-13)
data = np.array(soln.y, dtype=float).T
time = np.array(soln.t, dtype=float).T
plot_mee_minfuel(time, data, rho, eclipse)