Including State of Optimal Control Problem

Hello, this may be the wrong place to ask this. I have a CVX program that I’ve successfully used to solve an optimal control problem. The problem is a linearized glide-vehicle model and the solution is relatively feasible. However, within CVX the continuous optimal control problem is discretized (using trapezoidal discretization) into 50 distinct discretization “nodes”. The convex program then iterates over the 50 nodes and gives an optimal control history in a 50x2 matrix as required.

My issue arises in that within the convex program the state is propagated across discretization nodes as follows (for a simplified Euler discretization).

for i = 1:N  (i.e. For each node.)
x(:,i+1) = x(i) + dN*xd(i); (Euler integration in this case for simplicity.)
end

Where dN is the time-step gathered by dividing the final time (tf) by the number of discretization nodes (50), that is tf/50 = dN. For my program, trapezoidal discretization is used, which has a local truncation error of h^3. h in this case is dN, typically, this number is much larger than 1. Therefore, dN^3 can be very large.

Therefore, I have attempted to involve a sub-iteration scheme within the cvx program, there the following happens instead:

for i = 1:N (for each node)
    tmpx = x(:,i); (Set temporary x to be the state at node i.)
    for j = 1:dN/dt (start at 1 and go until the next node)
         tmpx(:,j+1) = tmpx(:,j) + dt*tmpxd(:,j); (Update temporary x using accurate (dt) integration.)
    end
x(:,i+1) = tmpx(:,end); (Update the state at the terminal node with the more accurate integration.)
end

Now, originally, I would simply increase the number of discretization nodes to a high number such that the integration is more accurate. However, this dramatically slows down the performance. I would like to optimize ONLY for 50 discretization nodes, and simply integrate between these points. For example, if there is 10 seconds between each discretization node, then my algorithm would give me a control that I was going to apply for the next 10 seconds.

I feel like this should work, based on my preliminary analysis. However, actually getting the program to do what I want is proving difficult. As it stands, this works for the first iteration (I’m solving this successively), but on the second pass it throws out the error. The second iteration throws out a “Conversion from CVX to DOUBLE” is not possible.

The full CVX program is posted here:

%% CVX Program
% Input nothing, output optimal solution.
function [output] = CVXProgram()
%% Preamble
% Constants
Re = 6378e3;
g  = 9.81;
rho0 = 1.225570827014494;
kQ = 1.65e-4*sqrt(g*Re)^(3.15);
hs = 7254.24;
Ar = 249.9091776;
m = 92079.2525560557;
% Size information
szu = 2;
szx = 8;
% Initial and terminal states
xi = [79248+Re;0;0;7802.88;1;-90;0;0]; % Initial conditions.
xf = [24384+Re;0;0;762;-5;0;0;0]; % Terminal conditions.
xi = xi.*[1;pi/180;pi/180;1;pi/180;pi/180;pi/180;pi/180]; % Scaled initial condition.
xf = xf.*[1;pi/180;pi/180;1;pi/180;pi/180;pi/180;pi/180]; % Scaled terminal condition.
% tf = 2000; % Final time.
% Convex parameters
N = 25; % Discretization nodes.
dt = 1e-1; % Time-step.
tf = 2000; % Estimate Final time.
if floor(tf/N) == tf/N
else
    tf = floor(tf/N)*N;
end
dN = tf/N;
TimeS = 0:dt:tf; % Time in seconds.
TimeN = linspace(0,tf,N); % Time in nodes.

%% Reference generation.
% Find the reference trajectory with zero control via the original
% (nonlinear) dynamics propagated using high-order runge-kutta, and tiny
% time-step.
x = zeros(szx,numel(TimeS));
x(:,1) = xi;
for i = 1:numel(TimeS)
    k1 = StateFunction(x(:,i));
    k2 = StateFunction(x(:,i)+dt*k1/2);
    k3 = StateFunction(x(:,i)+dt*k2/2);
    k4 = StateFunction(x(:,i)+dt*k3);
    x(:,i+1) = x(:,i) + (dt/6)*(k1+2*k2+2*k3+k4);
end
sx = x; % Unscaled reference for accuracte integration during cvx.
rx = x(:,1:dN/dt:tf/dt);  % Scale the reference trajectory to coincide at discretization nodes.
%% Constraints
tc = [0;1;1;0;0;0;0;0]; % Enforced terminal constraints.
% Structural constraints
Qdmax = 1500e3;
qmax = 18e3;
nmax = 2.5*g;
%% Matrix Setup
% A is provided as a usable function with state input by AFunc
B = zeros(szx,szu);
B(szx+1-szu:end,:) = eye(szu);
C = eye(szx);
%% Convex Procedure.
rxs(:,:,1) = rx;
% for k = 1:50
k = 1;
cvx_begin
variables x(szx,N+1) u(szu,N) v(szx,N)
x(:,1) == xi;
% State Update Criteria
for i = 1:N % For each discretization node we output the state.
    xj = x(:,i);
    rj = sx(:,1+(i-1)*(dN/dt):i*(dN/dt));
    for j = 1:dN/dt-1 % For each discretization node we propagate more accurately within, by using dt time-steps.
        xd(:,j) = StateFunction(rj(:,j)) + AFunc(rj(:,j))*(xj(:,j)-rj(:,j)) + B*u(:,i) + C*v(:,i); % Linearized State Derivatve for time i+dt*j.
        xj(:,j+1) = xj(:,j) + dt*xd(:,j); % Euler integration for now.
    end
    x(:,i+1) == xj(:,end);
    tsx(:,1+(i-1)*(dN/dt):i*(dN/dt)) = xj;
end
cvx_end
rxs(:,:,k+1) = x(:,1:end-1);
sx = tsx;
% end

%% Outputs
% Vector length fix.
u(:,end+1) = u(:,end);
v(:,end+1) = v(:,end);
% Outputs.
output.ref = rxs;
output.status = cvx_status;
output.x = x;
output.v = v;
output.u = u;
output.timeS = TimeS;
output.timeN = TimeN;
output.constraints.heatingrate = Qdmax;
output.constraints.dynamicpressure = qmax;
output.constraints.normalforce = nmax;
output.convex.N = N;
output.convex.cputtime = cvx_cputime;
output.convex.optval = cvx_optval;
end

function [A] = AFunc(x)
x1 = x(1);
x2 = x(2);
x3 = x(3);
x4 = x(4);
x5 = x(5);
x6 = x(6);
x7 = x(7);
x8 = x(8);

t2 = cos(x3);
t3 = cos(x5);
t4 = cos(x6);
t5 = cos(x7);
t6 = sin(x5);
t7 = sin(x6);
t8 = sin(x7);
t9 = tan(x3);
t10 = x4.^2;
t11 = x8.^2;
t12 = 1.0./x1;
t14 = 1.0./x4;
t20 = x8.*1.6756;
t21 = x8.*3.529e-1;
t26 = x1.*1.378504157568539e-4;
t13 = t12.^2;
t15 = 1.0./t10;
t16 = 1.0./t2;
t17 = 1.0./t3;
t18 = t10.*t12;
t19 = t11.*(5.1e+1./2.5e+1);
t22 = -t21;
t24 = t20-1.07e+2./1.0e+3;
t27 = -t26;
t23 = t18-9.81e+2./1.0e+2;
t25 = t19+t22+7.85e-2;
t28 = t27+8.792099516972143e+2;
t29 = exp(t28);
mt1 = [0.0,-t3.*t7.*t13.*t16.*x4,-t3.*t4.*t13.*x4,t10.*t25.*t29.*2.292645563890263e-7,-t14.*(t3.*t10.*t13+t5.*t10.*t24.*t29.*2.292645563890263e-7)];
mt2 = [-t14.*(t3.*t7.*t9.*t10.*t13+t8.*t10.*t17.*t24.*t29.*2.292645563890263e-7),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3.*t7.*t12.*t16.^2.*x4.*sin(x3),0.0,0.0,0.0,t3.*t7.*t12.*x4.*(t9.^2+1.0),0.0,0.0,t6,t3.*t7.*t12.*t16,t3.*t4.*t12];
mt3 = [t25.*t29.*x4.*(-3.32628023107906e-3)];
mt4 = [t14.*(t3.*t12.*x4.*2.0+t5.*t24.*t29.*x4.*3.32628023107906e-3)-t15.*(t3.*t23+t5.*t10.*t24.*t29.*1.66314011553953e-3)];
mt5 = [-t15.*(t3.*t7.*t9.*t18+t8.*t10.*t17.*t24.*t29.*1.66314011553953e-3)+t14.*(t3.*t7.*t9.*t12.*x4.*2.0+t8.*t17.*t24.*t29.*x4.*3.32628023107906e-3),0.0,0.0,t3.*x4,-t6.*t7.*t12.*t16.*x4,-t4.*t6.*t12.*x4,t3.*(-9.81e+2./1.0e+2)];
mt6 = [-t6.*t14.*t23,-t14.*(t6.*t7.*t9.*t18-t6.*t8.*t10.*t17.^2.*t24.*t29.*1.66314011553953e-3),0.0,0.0,0.0,t3.*t4.*t12.*t16.*x4,-t3.*t7.*t12.*x4,0.0,0.0,t3.*t4.*t9.*t12.*x4,0.0,0.0,0.0,0.0,0.0,0.0,t8.*t24.*t29.*x4.*(-1.66314011553953e-3)];
mt7 = [t5.*t17.*t24.*t29.*x4.*1.66314011553953e-3,0.0,0.0,0.0,0.0,0.0,t10.*t29.*(x8.*(1.02e+2./2.5e+1)-3.529e-1).*(-1.66314011553953e-3)];
mt8 = [t5.*t29.*x4.*2.786757577598036e-3,t8.*t17.*t29.*x4.*2.786757577598036e-3,0.0,0.0];
A = reshape([mt1,mt2,mt3,mt4,mt5,mt6,mt7,mt8],8,8);
end

function [xd] = StateFunction(x)
% Constants
g = 9.81;
Re = 6378e3;
rho0 = 1.225570827014494;
hs = 7254.24;
m = 92079.2525560557;
Ar = 249.9091776;
% Atmosphere
h = x(1) - Re; % Altitude.
rho = rho0*exp(-h/hs); % Density.
% Aerodynamics
CL = -0.1070 + 1.6756*x(8); 
CD = 0.0785 - 0.3529*x(8) + 2.0400*x(8)^2;
q = 0.5*rho*x(4)^2; % Dynamic pressure.
D = q*Ar*CD/m;
L = q*Ar*CL/m;

% State derivative
rd = x(4)*sin(x(5));
ld = (x(4)*cos(x(5))*sin(x(6)))/(x(1)*cos(x(3)));
ad = (x(4)*cos(x(5))*cos(x(6)))/(x(1));
vd = -D -g*sin(x(5));
gd = (1/x(4))*(L*cos(x(7)) - cos(x(5)).*(g-x(4).^2./x(1)));
pd = (1/x(4))*(L*sin(x(7))./cos(x(5))+x(4).^2*cos(x(5))*sin(x(6))*tan(x(3))./x(1));
sd = 0;
od = 0;
xd  = [rd;ld;ad;vd;gd;pd;sd;od];
end
1 Like

I think you need to declare expression (holders), for xj, rj, tsx. Perhaps you are “getting away” with not doing so on the first iteration.

Assignment and expression holders

1 Like

That fixed it, you’re absolutely right. Thank you very much!

1 Like

Hi @AClayyy47/Shrivenham,
I have been reviewing your 36posts from Feb 9 to till to-date and have to say your thought through process is simply remarkable, clear, engaging & deeply enlightening. In this respect can you share your paper in reference for better understaning & appreciation of your problem.

Supra

Hi @supra,

I greatly appreciate your post and am more than happy to provide some references for the work that I’ve undertaken, though I have not yet published any results myself.

Pei et al. Online Re-entry Trajectory Optimization Using Modified Sequential Convex Programming for Hypersonic Vehicle DOI: 10.1109/ACCESS.2021.3056517.

Acikmese et al. Lossless Convexification of Optimal Control Problems with Semi-continuous Inputs http://arxiv.org/abs/1911.09013

Zhenbo and Grant Autonomous Entry Guidance for Hypersonic Vehicles by Convex Optimization DOI: 10.2514/1.A34102

1 Like

Hi @AClayyy47… Thanks for sharing your 3 reference papers

Supra

1 Like

@Mark_L_Stone, I thought the issue was resolved, but in fact I’d just kicked the can down the road.

The specific line causing the issue is

xd(:,j) = StateFunction(rj(:,j)) + AFunc(rj(:,j))*(xj(:,j)-rj(:,j)) + B*u(:,i) + C*v(:,i);

here rj(:,j) is an 8x1 real-numbered vector (not cvx),
xj is an 8x1 cvx real affine expression,
u is a 2x1 cvx real affine expression
v is an 8x1 real affine expression,
B is an 8x2 matrix where the last 2x2 is the second order identity matrix,
C is the 8x8 identity matrix.

StateFunction is a function which simply takes the real-valued vector rj and outputs the state derivative.
AFunc is the Jacobian of the nonlinear state.

Here, then, the only convex objects being used are xj in the AFunc(rj)(xj-rj), u in the Bu, and v in the Cv.

Bearing in mind that xj and rj are now defined expressions within the cvx syntax, I have no idea where Matlab is attempting to convert between convex and double, or why it would be necessary in my code to do that.

edit: I have just noticed that xd(:,j) is an 8x1 real-valued vector, whereas xd(:,j+1) is an 8x1 convex real-valued vector, so there is the conversion. Now I need to figure out why it’s valued for one iteration but not for the next.

1 Like

I neglected to list it, but xd also needs to be declared an expression, for the same reason as those other arrays into which you insert CVX expressions.

2 Likes

You’re absolutely correct. Thanks again Mark.

1 Like

After your first post saying it was solved, i looked at my answer again and noticed I was missing xd from my list. But since you said that it now worked, I figured you also declared that as an expression; so I didn’t bother editing/adding another post to that effect.

1 Like