# Maximum cross-range shuttle reentry problem 
# J.T. Betts: Practical Methods for Optimal Control
# Using Nonlinear Programming, p.133ff
# Discretization by trapezoidal formulation
# Implemented by Dieter Kraft, 
# Munich University of Applied Sciences
# July, 2004

param pi := 4*atan(1);
param d2r := pi/180;
param r2d := 180/pi;
param h_0;
param v_0;
param phi_0;
param psi_0;
param theta_0;
param gamma_0;
param h_f;
param v_f;
param phi_f;
param psi_f;
param theta_f;
param gamma_f;
param h_max;
param v_max;
param phi_max;
param psi_max;
param theta_max;
param gamma_max;
param R_e;
param S;
param a_0;
param a_1;
param b_0;
param b_1;
param b_2;
param c_0;
param c_1;
param c_2;
param c_3;
param mu;
param q_max;
param rho_0;
param h_r;
param alpha_max;
param beta_max;
param beta_min;
param fac;
param m;
param n;

set N  := {0..n};
set N1 := {0..n-1};

var t_f >= 0;
var h {N};
var v {N};
#var phi {N}; 
# This variable does not occur
# in the other equations
var psi {N};
var theta {N};
var gamma {N};
var alpha {N};
var beta {N};
var step          = t_f/n;
var alfa {i in N} = alpha[i]*r2d;
var rho {i in N}  = rho_0*exp(-h[i]/h_r);
var r {i in N}    = R_e+h[i];
var g {i in N}    = mu/r[i]^2;
var c_L {i in N}  = a_0+a_1*alfa[i];
var c_D {i in N}  = b_0+b_1*alfa[i]+b_2*alfa[i]^2;
var q_a {i in N}  = c_0+c_1*alfa[i]+c_2*alfa[i]^2+c_3*alfa[i]^3;
var q_r {i in N}  = 17700*sqrt(rho[i])*(0.0001*v[i])^3.07;
var D {i in N}    = 0.5*c_D[i]*S*rho[i]*v[i]^2;
var L {i in N}    = 0.5*c_L[i]*S*rho[i]*v[i]^2;
var q {i in N}    = q_a[i]*q_r[i];

# Cost function

maximize final_cross_range : theta[n] 
+ fac*(sum {i in N1} (alpha[i+1]-alpha[i])^2/step 
+ sum {i in N1} (beta[i+1]-beta[i])^2/step);

# differential equations

H {i in N1}: h[i+1] = h[i] 
+ 0.5*step*(v[i]*sin(gamma[i]) + v[i+1]*sin(gamma[i+1]));

V {i in N1}: v[i+1] = v[i] 
- 0.5*step*(D[i]/m+g[i]*sin(gamma[i]) + D[i+1]/m+g[i+1]*sin(gamma[i+1]));    

Theta {i in N1}: theta[i+1] = theta[i] 
+ 0.5*step*(v[i]/r[i]*cos(gamma[i])*cos(psi[i])
+ v[i+1]/r[i+1]*cos(gamma[i+1])*cos(psi[i+1]));

Gamma {i in N1}: gamma[i+1] = gamma[i] 
+ 0.5*step*(L[i]/m/v[i]*cos(beta[i])+cos(gamma[i])*(v[i]/r[i]-g[i]/v[i])
+ L[i+1]/m/v[i+1]*cos(beta[i+1])+cos(gamma[i+1])*(v[i+1]/r[i+1]-g[i+1]/v[i+1]));

Psi {i in N1}: psi[i+1] = psi[i] 
+ 0.5*step*(L[i]*sin(beta[i])/m/v[i]/cos(gamma[i])
+ v[i]/r[i]*cos(gamma[i])*sin(psi[i])*sin(theta[i])/cos(theta[i])      
+ L[i+1]*sin(beta[i+1])/m/v[i+1]/cos(gamma[i+1])
+ v[i+1]/r[i+1]*cos(gamma[i+1])*sin(psi[i+1])*sin(theta[i+1])/cos(theta[i+1]));  

#Phi {i in N1}: phi[i+1] = phi[i] 
# + 0.5*step*(v[i]/r[i]*cos(gamma[i])*sin(psi[i])/cos(theta[i])
# + v[i+1]/r[i+1]*cos(gamma[i+1])*sin(psi[i+1])/cos(theta[i+1])); 

# state and control constraints (algebraic equations)

#Q  {i in N}:           q[i] <= q_max;
HS {i in 1..n-1}:          0 <= h[i];
VS {i in 1..n-1}:          1 <= v[i];
TH {i in 1..n-1}: -theta_max <= theta[i] <= theta_max;
GA {i in 1..n-1}:   -5.5*d2r <= gamma[i] <= 1*d2r;
AL {i in N}     :          0 <= alpha[i] <= 30*d2r;
BE {i in N}     :    -80*d2r <= beta[i]  <= 0;

# boundary values

h0:         h[0] = h_0;
v0:         v[0] = v_0;
theta0: theta[0] = theta_0;
gamma0: gamma[0] = gamma_0;
psi0:     psi[0] = psi_0;
#phi0:    phi[0] = phi_0;

hf:         h[n] = h_f;
vf:         v[n] = v_f;
gammaf: gamma[n] = gamma_f;

data shuttle.dat;

option ipopt_options 'imaxiter=3000 iprint=1 dtol=1e-8';
option solver ipopt; 
# (Andreas Waechter, CMU/IBM)
# more on options: 
# www-124.ibm.com/developerworks/opensource/coin/Ipopt/IPOPT_options.html

option loqo_options 'outlev=1';
#option solver loqo; # (Robert Vanderbei, Princeton University)

option snopt_options 'outlev=1';
#option solver snopt; # (Gill, Murray, Saunders, Wright)

option minos_options 'outlev=3';
#option solver minos; # (Murtaugh, Saunders)

solve;

display t_f, _total_solve_time;

printf {i in N}: "%10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f \n",
 i/n*t_f, h[i], v[i], theta[i], gamma[i], psi[i], q[i], alpha[i], beta[i] > 
 shuttle.out;

# gnuplot; plot 'shuttle.out' using 1:2 w lines