# Low Thrust Orbit Maneuver
# J.T. Betts: Practical Methods for Optimal Control
# Using Nonlinear Programming, 
# Example 5.3, p.147ff
# Discretization by trapezoidal formulation
# Implemented by Dieter Kraft, 
# Munich University of Applied Sciences
# July, 2004

param pi := 4*atan(1);
param Isp := 450;
param mu := 1.407645794e16;
param G0 := 32.174;
param J2 := 1082.639e-6;
param J3 := -2.565e-6;
param J4 := -1.608e-6;
param T := 4.446618e-3;
param Re := 209.2566273e5;
param tauL := -50.0;
param p0 := 218.37080052835e5;
param f0 := 0.0;
param g0 := 0.0;
param h0 := -0.25396764647494;
param k0 := 0.0;
param L0 := pi;
param w0 := 1.0;
param wf := 0.2; # estimated
param pf := 400.07346015232e5;
param fgf := 0.73550320568829;
param hkf := 0.61761258786099;

param n; # discretization intervals

var tf >= 0;
var step = tf/n;

# state variables 
# modified equinoctial elements
# http://www.cdeagle.com/pdf/mee.pdf

var p{0..n};
var f{0..n};
var g{0..n};
var h{0..n};
var k{0..n};
var L{0..n};
var w{0..n};

# control variables (thrust components)

var ur{0..n};
var ut{0..n};
var uh{0..n};
var tau; # throttle parameter

# dependent variables

var q{i in 0..n} = 1.0 + f[i]*cos(L[i]) + g[i]*sin(L[i]);
var r{i in 0..n} = p[i]/q[i];
var a2{i in 0..n} = h[i]^2 - k[i]^2;
var s2{i in 0..n} = 1.0 + h[i]^2 + k[i]^2;
var r1{i in 0..n} = r[i]/s2[i]*((1.0+a2[i])*cos(L[i])+2*h[i]*k[i]*sin(L[i]));
var r2{i in 0..n} = r[i]/s2[i]*((1.0-a2[i])*sin(L[i])+2*h[i]*k[i]*cos(L[i]));
var r3{i in 0..n} = 2*r[i]/s2[i]*(h[i]*sin(L[i])-k[i]*cos(L[i]));
var v1{i in 0..n} = -1.0/s2[i]*sqrt(mu/p[i])
   *((1.0+a2[i])*sin(L[i])-2*h[i]*k[i]*cos(L[i])+(1.0+a2[i])*g[i]-2*f[i]*h[i]*k[i]);
var v2{i in 0..n} = -1.0/s2[i]*sqrt(mu/p[i])
   *((-1.0+a2[i])*cos(L[i])+2*h[i]*k[i]*sin(L[i])+(-1.0+a2[i])*f[i]+2*g[i]*h[i]*k[i]);
var v3{i in 0..n} = 2.0/s2[i]*sqrt(mu/p[i])
                  *(h[i]*cos(L[i])+k[i]*sin(L[i])+f[i]*h[i]+g[i]*k[i]);
var rnorm{i in 0..n} = sqrt(r1[i]^2+r2[i]^2+r3[i]^2);
var rcrossv1{i in 0..n} = r2[i]*v3[i]-v2[i]*r3[i]; 
var rcrossv2{i in 0..n} = r3[i]*v1[i]-v3[i]*r1[i]; 
var rcrossv3{i in 0..n} = r1[i]*v2[i]-v1[i]*r2[i]; 
var rcrossvnorm{i in 0..n} = sqrt(rcrossv1[i]^2+rcrossv2[i]^2+rcrossv3[i]^2);
var rcrossvcrossr1{i in 0..n} = rcrossv2[i]*r3[i]-rcrossv3[i]*r2[i];
var rcrossvcrossr2{i in 0..n} = rcrossv3[i]*r1[i]-rcrossv1[i]*r3[i];
var rcrossvcrossr3{i in 0..n} = rcrossv1[i]*r2[i]-rcrossv2[i]*r1[i];
var a12{i in 0..n} = 2*p[i]/q[i]*sqrt(p[i]/mu);
var a21{i in 0..n} = sqrt(p[i]/mu)*sin(L[i]);
var a22{i in 0..n} = 1/q[i]*sqrt(p[i]/mu)*((1+q[i])*cos(L[i])+f[i]);
var a23{i in 0..n} = -g[i]/q[i]*sqrt(p[i]/mu)*(h[i]*sin(L[i])-k[i]*cos(L[i]));
var a31{i in 0..n} = -sqrt(p[i]/mu)*cos(L[i]);
var a32{i in 0..n} = 1/q[i]*sqrt(p[i]/mu)*((1+q[i])*sin(L[i])+g[i]);
var a33{i in 0..n} = f[i]/q[i]*sqrt(p[i]/mu)*(h[i]*sin(L[i])-k[i]*cos(L[i]));
var a43{i in 0..n} = sqrt(p[i]/mu)*s2[i]/q[i]/2*cos(L[i]);
var a53{i in 0..n} = sqrt(p[i]/mu)*s2[i]/q[i]/2*sin(L[i]);
var a63{i in 0..n} = sqrt(p[i]/mu)/q[i]*(h[i]*sin(L[i])-k[i]*cos(L[i]));
var sphi{i in 0..n} = 2*(h[i]*sin(L[i])-k[i]*cos(L[i]))/s2[i];
var P2{i in 0..n} = (3*sphi[i]^2-1)/2;
var P3{i in 0..n} = (5*sphi[i]^3-3*sphi[i])/2;
var P4{i in 0..n} = (35*sphi[i]^4-30*sphi[i]^2+3)/8;
var dP2{i in 0..n} = 3*sphi[i];
var dP3{i in 0..n} = (15*sphi[i]^2-3)/2;
var dP4{i in 0..n} = (70*sphi[i]^3-30*sphi[i])/4;
var dgn{i in 0..n} = -mu*sqrt(1-sphi[i]^2)/r[i]^2*
                    ((Re/r[i])^2*dP2[i]*J2+(Re/r[i])^3*dP3[i]*J3+(Re/r[i])^4*dP4[i]*J4);
var dgr{i in 0..n} = -mu/r[i]^2*
                    (3*(Re/r[i])^2*P2[i]*J2+4*(Re/r[i])^3*P3[i]*J3+5*(Re/r[i])^4*P4[i]*J4);
var ir1{i in 0..n} = r1[i]/rnorm[i];
var ir2{i in 0..n} = r2[i]/rnorm[i];
var ir3{i in 0..n} = r3[i]/rnorm[i];
var it1{i in 0..n} = rcrossvcrossr1[i]/(rcrossvnorm[i]*rnorm[i]);
var it2{i in 0..n} = rcrossvcrossr2[i]/(rcrossvnorm[i]*rnorm[i]);
var it3{i in 0..n} = rcrossvcrossr3[i]/(rcrossvnorm[i]*rnorm[i]);
var ih1{i in 0..n} = rcrossv1[i]/rcrossvnorm[i];
var ih2{i in 0..n} = rcrossv2[i]/rcrossvnorm[i];
var ih3{i in 0..n} = rcrossv3[i]/rcrossvnorm[i];
var hin1{i in 0..n} = -ir3[i]*ir1[i];
var hin2{i in 0..n} = -ir3[i]*ir2[i];
var hin3{i in 0..n} = 1-ir3[i]*ir3[i];
var innorm{i in 0..n} = sqrt(hin1[i]^2+hin2[i]^2+hin3[i]^2);
var in1{i in 0..n} = hin1[i]/innorm[i];
var in2{i in 0..n} = hin2[i]/innorm[i];
var in3{i in 0..n} = hin3[i]/innorm[i];
var dg1{i in 0..n} = dgn[i]*in1[i]-dgr[i]*ir1[i];
var dg2{i in 0..n} = dgn[i]*in2[i]-dgr[i]*ir2[i];
var dg3{i in 0..n} = dgn[i]*in3[i]-dgr[i]*ir3[i];
var g1{i in 0..n} = ir1[i]*dg1[i]+ir2[i]*dg2[i]+ir3[i]*dg3[i];
var g2{i in 0..n} = it1[i]*dg1[i]+it2[i]*dg2[i]+it3[i]*dg3[i];
var g3{i in 0..n} = ih1[i]*dg1[i]+ih2[i]*dg2[i]+ih3[i]*dg3[i];
var T1{i in 0..n} = G0*T*(1+tau/100)/w[i]*ur[i];
var T2{i in 0..n} = G0*T*(1+tau/100)/w[i]*ut[i];
var T3{i in 0..n} = G0*T*(1+tau/100)/w[i]*uh[i];
var Delta1{i in 0..n} = g1[i]+T1[i];
var Delta2{i in 0..n} = g2[i]+T2[i];
var Delta3{i in 0..n} = g3[i]+T3[i];

let n := 400;

# Cost function

maximize weight: 10000*w[n];

# differential equations (trapezoidal rule of discretization)

de1 {i in 0..n-1}: p[i+1] = p[i]+step*(a12[i]*Delta2[i]+a12[i+1]*Delta2[i+1])/2;
de2 {i in 0..n-1}: f[i+1] = f[i]+step*(a21[i]*Delta1[i]+a22[i]*Delta2[i]+a23[i]*Delta3[i]
                   +a21[i+1]*Delta1[i+1]+a22[i+1]*Delta2[i+1]+a23[i+1]*Delta3[i+1])/2;
de3 {i in 0..n-1}: g[i+1] = g[i]+step*(a31[i]*Delta1[i]+a32[i]*Delta2[i]+a33[i]*Delta3[i]
                   +a31[i+1]*Delta1[i+1]+a32[i+1]*Delta2[i+1]+a33[i+1]*Delta3[i+1])/2;
de4 {i in 0..n-1}: h[i+1] = h[i]+step*(a43[i]*Delta3[i]+a43[i+1]*Delta3[i+1])/2;
de5 {i in 0..n-1}: k[i+1] = k[i]+step*(a53[i]*Delta3[i]+a53[i+1]*Delta3[i+1])/2;  
de6 {i in 0..n-1}: L[i+1] = L[i]+step*(a63[i]*Delta3[i]+a63[i+1]*Delta3[i+1]
                          + sqrt(mu*p[i])*(q[i]/p[i])^2 
                 	  + sqrt(mu*p[i+1])*(q[i+1]/p[i+1])^2)/2;
de7 {i in 0..n-1}: w[i+1] = w[i]-step*T/Isp*(1+tau/100);  

# control constraints

cc {i in 0..n}: sqrt(ur[i]^2+ut[i]^2+uh[i]^2) = 1.0;
tc: tauL <= tau <= 0.0;

u1 {i in 0..n}: -1 <= ur[i] <= 1;
u2 {i in 0..n}: -1 <= ut[i] <= 1;
u3 {i in 0..n}: -1 <= uh[i] <= 1;

# state constraints


# boundary values

ic1: p[0] = p0;
ic2: f[0] = f0; 
ic3: g[0] = g0;
ic4: h[0] = h0;
ic5: k[0] = k0;
ic6: L[0] = L0;
ic7: w[0] = w0;
if1: p[n] = pf;
if2: sqrt(f[n]^2+g[n]^2) = fgf;
if3: sqrt(h[n]^2+k[n]^2) = hkf;
if4: f[n]*h[n]+g[n]*k[n] = 0.0;
if5: g[n]*h[n]-f[n]*k[n] <= 0.0;

# initial estimates

let {i in 0..n} p[i] := p0+(pf-p0)*i/n;
let {i in 0..n} f[i] := -0.12*i/n;
let {i in 0..n} g[i] := 0.7*(i/n)^4;
let {i in 0..n} h[i] := -0.6*(i/n)^3;
let {i in 0..n} k[i] := 0.02*i/n;
let {i in 0..n} L[i] := (16*pi)*i/n;
let {i in 0..n} w[i] := w0+(wf-w0)*i/n;
let {i in 0..n} ur[i] := 0.1;
let {i in 0..n} ut[i] := 0.7-i/n;
let {i in 0..n} uh[i] := -0.1;
let tau := -9.0;
let tf := 90000.0;

option solver ipopt; # (Andreas Waechter, CMU/IBM)
# www-124.ibm.com/developerworks/opensource/coin/Ipopt

solve;

display _total_solve_time, tf, tau, 
if2, if2.slack, if3, if3.slack, if4, if4.slack, if5, if5.slack;

printf {i in 0..n}: 
"%10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f \n",
tf*i/n, p[i], f[i], g[i], h[i], k[i], L[i], w[i], ur[i], ut[i], uh[i] 
> orbit.out;

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