Improvements to script for Finite Burn Trajectory Optimization with MATLAB

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

• I admit not being very familiar with Longuski's work (although I should since I work on finite burn optimization and suboptimal solutions), so please excuse the uninformed questions. Could you explain to us why the first three elements of co-state is the norm of the velocity vector (if I read correctly)? Moreover, is there any specific reason you've coded everything up in meters instead of kilometers? Furthermore, are you sure that this TPBVP algorithm works with large inclination changes? Many optimization algos break down for large OE changes or once some perturbations are enabled. – ChrisR Jan 27 '18 at 6:15
• If you're referring to yinit that is just the initial guess at the primer vector costate which is a unit vector in the prograde direction. the primer vector equation for the costate is down nearly at the bottom of the script. – lamont Jan 27 '18 at 7:16
• I don't know enough about calculus of variations to know whether they break down: I will ask some colleagues who will hopefully have an answer for you. What I can assert however is that a Lambert solver is one way to solve the TPBVP and it definitely does work for small inclination changes. And I also don't see why it would fail for even very large inclination changes... This is puzzling! Great question! :-D – ChrisR Jan 27 '18 at 7:32
• I may be able to refactor this considerably to use the ode45() function in matlab along with a couple of popular and robust-looking jacobianest() and newtonraphson() routines on mathworks file exchange (and the latter seems to be a quasi-newton approach that degrades to gradient ascent so may be appropriate for this kind of application). The result should be broadly equivalent to all the CoV approaches in the literature (analytical treatment of the coast period and normalization of the units would finish it up). – lamont Jan 27 '18 at 22:58
• I'm voting to close this question as off-topic because it's about solving equations with matlab. The equations themselves are relevant, but the question isn't about them or how they've been applied. – user20636 May 9 '18 at 4:53

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);

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

• tagging @ChrisR – lamont Mar 10 '18 at 1:14
• I converted the above script to do multipoint root finding, which turned out to be conceptually simpler than I expected. Even though I took out the fixed variables (r0, v0 and m) it still expands the number of equations from 8 to 20 and the time to solve goes from 0.4s to 1.4s. – lamont Mar 11 '18 at 22:44
• Removing the variables seems to be a bit of an unnecessary micro-optimization. Expanding it to a 28-dimensional root solving problem doesn't seem to cost much (since its the initial conditions the jacobian is trival and the initial guess is exact). That starts to simplify the code. The Levenburg-Marquardt fsolve() implementation also seems to suitable to solving this kind of problem. – lamont Mar 18 '18 at 5:02