Improvements to script for Finite Burn Trajectory Optimization with MATLAB

Question

I've written a MATLAB script to use the bvp4c two point boundary equation solver to implement a Calculus of Variations trajectory optimization of the problem of a single finite burn with a free coast to pick the optimum ignition point. I'd like any input on mistakes that I've made or ways to improve the code. What it does is solve for the optimum finite burn from a 185x185 km orbit to a 185x1000 km orbit, with an optional inclination change. It sets up a multipoint boundary value problem to solve a burn-coast problem using the primer vector equation.

The code is based off of MATLAB code in the appendices of Optimal Control with Aerospace Applications by Longuski, Guzman and Prussing. The coast time and burn times have been implemented as free parameters and the boundary value problem uses normalized time (tau) so that [0,1] is the coast period and [1,2] is the burn period.

The ODE implements the equations of motion and the primer vector equation for central force motion in 3D cartesian coordinates. I think I've gotten those correct, but could use someone double checking my work. The multiplication by the coast or burn times in the return value of the ODE is necessary to convert the derivative from $\frac{d}{dt}$ to $\frac{d}{d \tau}$.

The Boundary Conditions are something that I've got a question about if I've done correctly. The initial position, velocity and mass are trivial conditions. There are also 14 conditions which impose continuity across all the variables on the lefthand side and righthand side in between the coast and the burn (tau = 1).

The remaining conditions are the 5 constraints on the terminal orbital conditions, and then the 3 transversality conditions to account for the true anomaly of the attachment point being free, and the times of the coast and the burn being free. For the terminal orbital conditions I'm using the angular momentum vector and the first two components of the eccentricity vector being equal to the target orbit. What I'm using for transverality conditions are that if the hamiltonian is broken up into $H = H_0 + T S$ where T is the thrust and S the switching function, then I set $H_0(t_2) = 0$, $H_0(t_0) = 0$ and then with $S = ( \lvert p_v \rvert - 1)$ set $\lvert p_v(t_1) \rvert = 1$. I lack some confidence that I've gotten that entirely correct.

Results seem to to be pretty decent, but I'm a little surprised that I don't see $\lvert p_v(t_2) \rvert = 1$. I also see convergence issues for any problems involving an inclination above about 25 degrees (but that gets into 300 sec / 3000 dV burns).

I have not yet normalized the units for position and velocity so they're still in $m$ and $m/s$.

For more moderate burns it seems to perform pretty well and gives burntimes within about a second of the impulsive burn model and looks like it leads the burn correctly with a negative coast of roughly half the burntime.

I'd appreciate any help though in finding mistakes that I've made or ways to improve this code to make it more robust.

close all; clear all; clc;

global r0 v0 m0 rT vT g0
global MU Thrust Mdot

MU = 3.9860044189e+14;  % earth
Thrust = 232.7 * 1000;  % N; LR-91 232.7 kN
m0 = 32.74 * 1000;      % kg; 32.74t - 1g start accel
isp = 316;              % sec; LR-91
g0 = 9.80665;           % m/s; standard gravity
ve = isp * g0;          % m/s
a0 = Thrust / m0        % m/s^2; initial accel
tau = ve / a0           % s; time to burn rocket to zero
Mdot = Thrust / ve;     % kg/sec

rearth = 6.371e+6;      % m; earth radius
r185 = rearth + 0.185e+6;
r1000 = rearth + 1.000e+6;

% initial position (185x185)
r0 = [ r185, 0, 0 ];
v185 = sqrt(MU/r185);
v0 = [ 0, v185, 0 ];

% target orbital parameters (185x1000)
rT = [ r185, 0, 0 ];
smaT = ( r185 + r1000 ) / 2;
inc = 10;  % degrees
vTm = sqrt(MU * (2/r185 - 1/smaT) );
vT = [ 0, vTm * cosd(inc), vTm * sind(inc)];

vBurn = vT - v0;
dV = norm(vBurn)

% list initial conds
yinit = [r0 v0 vBurn/norm(vBurn) 0 0 0 m0 ];  % initial state and costate
tb_guess = tau * (1 - exp(-dV/ve) )
tc_guess = - tb_guess / 2

% sanity checks on the impulsive burn model
mf_impulsive = m0 - tb_guess * Mdot
af_impulsive = Thrust / mf_impulsive / g0

% parameters
parameters = [ tc_guess, tb_guess ];

% number of timeslices
Nt = 41;
tau = [
  linspace(0,1,Nt)'
  linspace(1,2,Nt)'
];

% initial guess
solinit = bvpinit(tau, yinit, parameters);

% bump up the mesh by a bit
option=bvpset('Nmax',50000);

% solve
sol = bvp4c(@Burn_odes, @Burn_bcs, solinit, option);

% extract times
tc = sol.parameters(1)
tb = sol.parameters(2)

% pull out values for times
Z = deval(sol, tau);

% convert taus to times
for i=1:length(tau)
  if tau(i) <= 1
    time(i) = tau(i) * tc;
  else
    time(i) = tc + ( tau(i) - 1 ) * tb;
  end
end

% extract solution vars
x_sol = Z(1,:);
y_sol = Z(2,:);
z_sol = Z(3,:);
r_sol = sqrt( x_sol.^2 + y_sol.^2 + z_sol.^2 );
vx_sol = Z(4,:);
vy_sol = Z(5,:);
vz_sol = Z(6,:);
v_sol = sqrt( vx_sol.^2 + vy_sol.^2 + vz_sol.^2 );
pvx_sol = Z(7,:);
pvy_sol = Z(8,:);
pvz_sol = Z(9,:);
pv_sol = sqrt( pvx_sol.^2 + pvy_sol.^2 + pvz_sol.^2 );
m_sol = Z(13,:);

for i = 1:length(tau)
  r = Z(1:3, i);
  v = Z(4:6, i);
  h(i) = norm(cross(r,v));
end

% plots

figure;
subplot(3,2,1);
plot(time,m_sol/1000);
ylabel('mass, tons', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

subplot(3,2,2);
plot(time,r_sol/1000);
ylabel('radius, km', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

subplot(3,2,3);
plot(time,v_sol/1000);
ylabel('velocity, km/s', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

subplot(3,2,4);
plot(time,h);
ylabel('h', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

subplot(3,2,5);
plot(time,pv_sol);
ylabel('pv magnitude', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

figure;
plot(x_sol/1000, y_sol/1000);
grid on;
axis equal
xlabel('x, km', 'fontsize', 14);
ylabel('y, km', 'fontsize', 14);


%
% Boundary conditions
%

function PSI = Burn_bcs(yleft, yright, parameters)

Y0 = yleft(:,1);
Y1 = yleft(:,2);  % == yright(:,1)
Y2 = yright(:,2);

global r0 v0 m0 rT vT MU g0 Thrust

ri = Y0(1:3);
vi = Y0(4:6);
pvi = Y0(7:9);
pri = Y0(10:12);
mi = Y0(13);
ri3 = norm(ri)^3;

rf = Y2(1:3);
vf = Y2(4:6);
pvf = Y2(7:9);
prf = Y2(10:12);
mf = Y2(13);
rf3 = norm(rf)^3;

r1 = Y1(1:3);
v1 = Y1(4:6);
pv1 = Y1(7:9);
pr1 = Y1(10:12);
r13 = norm(r1)^3;

hT = cross(rT, vT);
hf = cross(rf, vf);

eT = - (rT/norm(rT) + cross(hT,vT)/MU);
ef = - (rf/norm(rf) + cross(hf,vf)/MU);

emiss = ef - eT';

uf = pvf / norm(pvf);

H0ti = dot(pri, vi) - MU * dot(pvi, ri) / ri3;
H0t1 = dot(pr1, v1) - MU * dot(pv1, r1) / r13;
H0tf = dot(prf, vf) - MU * dot(pvf, rf) / rf3;
Htf = H0tf - Thrust * ( norm(pvf) - 1 );

PSI = [
    Y0(1:3) - r0'                       % 3 - initial position
    Y0(4:6) - v0'                       % 3 - initial velocity
    Y0(13) - m0                         % 1 - initial mass
    norm(Y1(7:9)) - 1                   % 1 - norm(pv1) = 1 (so H(t1) = 0 on both sides of t1)
    hf - hT'                            % 3 - terminal constraint on angular momentum
    emiss(1:2)                          % 2 - terminal constraint on ecc vector
    H0ti                                % 1 - H0(t0) = 0
    H0tf                                % 1 - H0(tf) = 0
    yleft(:,2) - yright(:,1)            % 14 - values at t1 are continuous
    ];

end

%
% Equations of motion
%

function dX_dtau = Burn_odes(tau, X, k, parameters)

global MU Thrust Mdot

if k == 1
  thr = 0;
  md = 0;
  tinterval = parameters(1);  % tc
else
  thr = Thrust;
  md = Mdot;
  tinterval = parameters(2);  % tb
end

r = X(1:3);
v = X(4:6);
pv = X(7:9);
pr = X(10:12);
m = X(13);

u = pv / norm(pv);

Fm = thr / m;
r3 = norm(r)^3;
r2 = dot(r,r);
r5 = norm(r)^5;

rdot  = v;
vdot  = - MU * r / r3 + Fm * u;
pvdot = pr;
prdot = - MU / r5 * ( 3 * r' * r - r2 * eye(3,3) ) * pv;

dX_dtau = tinterval*[rdot', vdot', pvdot', prdot', -md ];

end

Answer 1

I finally realized that by trying to use bvp4c that I was making the whole problem harder than I needed to. By using CoV I've already converted the problem from a TPBVP to a root-finding problem. The default root-finder fsolve in Matlab is quasi-Newtonian with a dogleg trust region and supports computing numerical jacobians. That ticks all the boxes.

So I've updated the script above to use fsolve. I've also applied the numerical conditioning that Ping Lu uses.

It seems to handle the case of doing a burn to go from a 185x185km equatorial orbit to a 185x1000 orbit with an 89 degree inclination change just fine and produces plausible looking results. Only takes 0.4 seconds on my desktop when fed the initial guess corresponding to the impulsive burn. That is a 11,089 dV burn, which is getting pretty silly in terms of reality but the solver handles it.

This script has a singularity in the boundary conditions when targeting polar orbits (the reference I got the boundary conditions from suggests swizzling the vectors through a change of coordinate system to address that). That is why I used an 89 degree change instead of 90 degrees.

I believe the biggest numerical deficiency here is that this is a single-shooting approach and for multiple burn-coast phases that the problem should use a multiple shooting approach for additional stability (or for long burn or coast arcs).

I'm also just throwing ODE45 at integrating the solution. It would be better to use Goodyear's analytical solution for coast arcs. Ping Lu has a series of papers on different approaches to analytical approximations to the burn arcs.

I'm also still not certain I have the 3 boundary conditions for the free attachment and optimization of the coast and burn times correct -- I simply need to circle back around to those now that I have a more stable optimization solution.

I also need to look a bit at cleaning up the variables in the code for normalization -- I'm pretty certain my mu_bar is always algebraically identical to 1.000.. and can be dropped entirely, etc.

To run this code Matlab R2017b is required (for vecnorm) and the rv2orb() function converts state vectors to keplerian elements and is here.

close all; clear all; clc;
format shortG;

mu = 3.9860044189e+14;  % m^3/s^2; earth
thrust = 232.7 * 1000;  % N; kg m / s^2; LR-91 232.7 kN
m0 = 32.74 * 1000;      % kg; 32.74t - 1g start accel
isp = 316;              % sec; LR-91
g0 = 9.80665;           % m/s; standard gravity
ve = isp * g0;          % m/s
a0 = thrust / m0;       % m/s^2; initial accel
tau = ve / a0;          % s; time to burn rocket to zero
mdot = thrust / ve;     % kg/sec

rearth = 6.371e+6;      % m; earth radius
r185   = rearth + 0.185e+6;
r1000  = rearth + 1.000e+6;
v185   = sqrt(mu/r185); % 185x185 velocity

% initial conditions
r0 = [ r185, 0, 0 ];
v0 = [ 0, v185, 0 ];

% target conditions
rT = [ r185, 0, 0 ];
smaT = ( r185 + r1000 ) / 2;
inc = 89; % degrees
vTm = sqrt(mu * (2/r185 - 1/smaT) );
vT = [ 0, vTm * cosd(inc), vTm * sind(inc)];

% impulsive burn approximation
dV = vT - v0;
dVm = norm(dV)
burnTime = tau * (1 - exp(-dVm/ve) )

% scaling factors
g_bar = mu / norm(r0)^2;
r_scale = norm(r0);
v_scale = sqrt( norm(r0) * g_bar );
t_scale = sqrt( norm(r0) / g_bar );

% applying scaling
tb_bar     = burnTime / t_scale;
tc_bar     = - burnTime / t_scale / 2;
r0_bar     = r0 / r_scale;
rT_bar     = rT / r_scale;
v0_bar     = v0 / v_scale;
vT_bar     = vT / v_scale;
mu_bar     = mu / r_scale^3 * t_scale^2;    % this is just always == 1.000 tho right?
thrust_bar = thrust / r_scale * t_scale^2;  % can these two...
mdot_bar   = mdot * t_scale;                % ...get simplified?

% solve the problem
fun = @(x) coastBurn5constraintFun(r0_bar, v0_bar, x(1:3), x(4:6), m0, x(7), x(8), mu_bar, thrust_bar, mdot_bar, rT_bar, vT_bar);

tic
[x, z, exitflag, output, jacobian] = fsolve(fun, [ dV/norm(dV) zeros(1,3) tc_bar tb_bar ]);
toc

disp(z');

disp(output);

% use the solution to pull data out of the IVP again
xinit = [ r0_bar v0_bar x(1:3) x(4:6) m0 ];
tc_bar = x(7);
tb_bar = x(8);
[x, tfull, xfull] = coastBurnIVP(xinit, tc_bar, tb_bar, mu_bar, thrust_bar, mdot_bar);

% reverse scaling
tfull = tfull * t_scale;
xfull(:,1:3) = xfull(:,1:3) * r_scale;
xfull(:,4:6) = xfull(:,4:6) * v_scale;

% print some output
rf = xfull(end, 1:3)
vf = xfull(end, 4:6)

tc = tc_bar * t_scale
tb = tb_bar * t_scale

[a,eMag,i,O,o,nu,truLon,argLat,lonPer,p] = rv2orb(rf', vf', mu);

inc = rad2deg(i)

PeR = (1 - eMag) * a
ApR = (1 + eMag) * a

PeA = PeR - rearth
ApA = ApR - rearth

% plots

figure;
subplot(3,2,1);
plot(tfull,xfull(:,13)/1000);
ylabel('mass, tons', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

subplot(3,2,2);
plot(tfull,vecnorm(xfull(:,1:3)')'/1000);
ylabel('radius, km', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

subplot(3,2,3);
plot(tfull,vecnorm(xfull(:,4:6)')'/1000);
ylabel('velocity, km/s', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

%subplot(3,2,4);
%plot(time,h);
%ylabel('h', 'fontsize', 14);
%xlabel('Time, sec', 'fontsize', 14);
%hold on;
%grid on;

subplot(3,2,5);
plot(tfull,vecnorm(xfull(:,7:9)')');
ylabel('pv magnitude', 'fontsize', 14);
xlabel('Time, sec', 'fontsize', 14);
hold on;
grid on;

%figure;
%plot(x_sol/1000, y_sol/1000);
%grid on;
%axis equal
%xlabel('x, km', 'fontsize', 14);
%ylabel('y, km', 'fontsize', 14);


% IVP function returning the 5-constraint BC miss
%
function z = coastBurn5constraintFun(r0_bar, v0_bar, pv, pr, m0, tc_bar, tb_bar, mu_bar, thrust_bar, mdot_bar, rT_bar, vT_bar)
  xinit = [ r0_bar v0_bar pv pr m0 ];
  [x, tfull, xfull] = coastBurnIVP(xinit, tc_bar, tb_bar, mu_bar, thrust_bar, mdot_bar);
  z = coastBurn5constraintBC(x, mu_bar, rT_bar, vT_bar);
end

% 5 orbital constraint Boundary Conditions for Coast-Burn with free times
%
function z = coastBurn5constraintBC(x, mu_bar, rT, vT)
  x0 = x(1,:);
  x1 = x(2,:);
  x2 = x(3,:);

  r0 = x0(1:3);
  v0 = x0(4:6);
  pv0 = x0(7:9);
  pr0 = x0(10:12);

  r1 = x1(1:3);
  v1 = x1(4:6);
  pv1 = x1(7:9);
  pr1 = x1(10:12);

  r2 = x2(1:3);
  v2 = x2(4:6);
  pv2 = x2(7:9);
  pr2 = x2(10:12);

  hT = cross(rT, vT);
  h2 = cross(r2, v2);

  hmiss = h2 - hT;

  eT = - (rT/norm(rT) + cross(hT, vT)/mu_bar);
  e2 = - (r2/norm(r2) + cross(h2, v2)/mu_bar);

  emiss = e2 - eT;

  H0t1 = dot(pr1, v1) - mu_bar * dot(pv1, r1) / norm(r1)^3;
  H0t2 = dot(pr2, v2) - mu_bar * dot(pv2, r2) / norm(r2)^3;

  z = [
    hmiss'          % 3 constraints
    emiss(1:2)'     % 2 constraints
    H0t1
    H0t2
    norm(pv0) - 1
  ];
end

% Multi step integration of the IVP
%
function [x, tfull, xfull] = coastBurnIVP(x0, tc, tb, mu, thrust, mdot)
  coastfun = @(t,x) centralForceThrustEOM(t, x, mu, 0, 0);  % FIXME: Use Goodyear instead of ODE45
  [t1, x1] = ode45(coastfun, [0 tc], x0);

  burnfun = @(t,x) centralForceThrustEOM(t, x, mu, thrust, mdot);
  [t2, x2] = ode45(burnfun, [tc tc+tb], x1(end,:));

  % x values at the boundaries
  x = [
    x1(1,:)
    x2(1,:)
    x2(end,:)
  ];

  % all the t's for graphing
  tfull = [
    t1
    t2
  ];

  % all the x's for graphing
  xfull = [
    x1
    x2
  ];
end

% Equations of Motion
%
function dX_dt = centralForceThrustEOM(t, X, mu, thrust, mdot)
  r  = X(1:3);
  v  = X(4:6);
  pv = X(7:9);
  pr = X(10:12);
  m  = X(13);

  u = pv/norm(pv);
  Fm = thrust / m;

  r3 = norm(r)^3;
  r2 = dot(r,r);
  r5 = r2 * r3;

  rdot  = v;
  vdot  = - mu * r / r3 + Fm * u;
  pvdot = pr;
  prdot = - mu / r5 * ( 3 * r * r' - r2 * eye(3,3) ) * pv;

  dX_dt = [ rdot' , vdot', pvdot', prdot', -mdot ]';
end