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