I am trying to simulate optimal rocket landing using the method proposed in this paper. The main idea is that you can linearize the dynamics of the rocket around some reference trajectory, generate an optimal trajectory around that linearized model (an LTV system), linearize the dynamics again around the new optimal trajectory, generate a new optimal trajectory, and repeat. When I implement this method and run it, I get the following error:
Calling SDPT3 4.0: 34503 variables, 15495 equality constraints
For improved efficiency, SDPT3 is solving the dual problem.
------------------------------------------------------------
num. of constraints = 15495
dim. of socp var = 15002, num. of socp blk = 7001
dim. of linear var = 15003
dim. of free var = 4498
1996 linear variables from unrestricted variable.
*** convert ublk to lblk
*******************************************************************
SDPT3: Infeasible path-following algorithms
*******************************************************************
version predcorr gam expon scale_data
NT 1 0.000 1 0
it pstep dstep pinfeas dinfeas gap prim-obj dual-obj cputime
-------------------------------------------------------------------
0|0.000|0.000|4.2e+06|1.2e+02|5.8e+10| 8.041790e+07 0.000000e+00| 0:0:00| splu 1 1
1|0.084|0.002|3.9e+06|1.2e+02|9.4e+10| 7.812963e+07 -1.697212e-01| 0:0:00| splu 1 1
2|0.512|0.623|1.9e+06|4.4e+01|4.2e+10| 4.529316e+07 -1.316740e+01| 0:0:00| splu 1 1
3|0.575|0.679|8.1e+05|1.4e+01|1.6e+10| 2.786513e+07 -1.725887e+01| 0:0:00| splu 1 1
4|0.423|0.764|4.7e+05|3.4e+00|4.6e+09| 2.189511e+07 -3.807985e+01| 0:0:00| splu 1 1
5|1.000|0.428|1.2e-05|1.9e+00|3.3e+09|-8.756032e+06 -4.082701e+01| 0:0:00| splu 1 1
6|1.000|0.931|2.1e-04|1.3e-01|2.2e+08|-9.583722e+06 -3.152219e-01| 0:0:01| splu 1 1
7|1.000|0.323|2.7e-06|9.0e-02|2.2e+08|-6.001427e+07 4.411018e-01| 0:0:01| splu 1 1
8|0.092|0.034|1.9e-06|8.7e-02|2.6e+08|-2.872995e+08 3.497041e-01| 0:0:01| splu 1 1
9|0.139|0.035|3.6e-06|8.4e-02|5.1e+08|-2.697022e+09 3.771366e-01| 0:0:01| splu 2 1
10|1.000|0.045|2.1e-04|8.0e-02|1.7e+10|-3.225645e+11 4.351742e-01| 0:0:01| splu 2 1
11|1.000|0.025|1.0e-01|7.8e-02|3.2e+12|-1.570147e+14 4.686199e-01| 0:0:01| splu 2 1
stop: primal infeas has deteriorated too much, 2.0e+02
12|1.000|0.006|1.0e-01|7.8e-02|3.2e+12|-1.570147e+14 4.686199e-01| 0:0:01|
prim_inf,dual_inf,relgap = 1.04e-01, 7.81e-02, 2.04e-02
sqlp stop: dual problem is suspected of being infeasible
-------------------------------------------------------------------
number of iterations = 12
residual of dual infeasibility
certificate X = 3.18e-12
reldist to infeas. <= 3.64e-18
Total CPU time (secs) = 0.88
CPU time per iteration = 0.07
termination code = 2
DIMACS: 2.1e-04 0.0e+00 7.3e-01 0.0e+00 -1.0e+00 5.2e-02
-------------------------------------------------------------------
------------------------------------------------------------
Status: Infeasible
Optimal value (cvx_optval): +Inf
Could someone help me figure out why the optimization problem is infeasible? I have been trying to figure it out for days but can’t seem figure it out.
The MATLAB implementation is below:
main.m: This script defines system parameters (rocket mass, bounds on thrust it can generate, etc.) and iteratively solves the optimization problem.
clear; close all; clc;
% Dimensional system parameters
m0 = 55000; % initial mass, kg
S_ref = 12.54; % surface area, m^2
I_sp = 443; % specific impulse, s
g0 = 9.81; % Earth grav. accel. at r=R0, m/s^2
R0 = 6378e3; % radius of Earth, m
rho = 1.293; % density of air at STP, kg/m^3
a = 343; % speed of sound at STP, m/s
param = [m0, S_ref, I_sp, g0, R0, rho, a]';
% Time discretization (dimensionless)
N = 1000; % number of discretization steps
tf = 50; % final time, NOT FREE!
dt = tf/N;
% Dimensionless initial and final conditions
r_init = (R0+3000)/R0; % initial radius
r_final = R0/R0; % final radius at t_f
s_init = 0/R0; % initial downrange
s_final = 950/R0; % final downrange
V_init = 285 / sqrt(g0*R0); % initial velocity
V_final = 2 / sqrt(g0*R0); % final velocity
gamma_init = -65*(pi/180); % initial flight path angle
gamma_final = -90*(pi/180); % final flight path angle
z_init = log(55000/m0); % initial log mass
x0 = [r_init; s_init; V_init; gamma_init; z_init];
xf = [r_final; s_final; V_final; gamma_final; NaN]; % final conditions (some may be free, denoted NaN)
% Dimensionless lower and upper bounds on control
T_max = 1375.6 / (m0*g0); % maximum thrust
T_min = 0.3*T_max; % mininmum thrust
% alpha_min = 165*(pi/180); % minimum angle-of-attack
% alpha_max = 195*(pi/180); % maximum angle-of-attack
alpha_min = -15*pi/180;
alpha_max = 15*pi/180;
e_min = -10 * pi/180; % minimum thrust direction
e_max = 10 * pi/180; % maximum thrust direction
control_bounds = [T_min, T_max, alpha_min, alpha_max, e_min, e_max];
% Set initial guess for nominal state and control trajectories (all of
% these should be dimensionless!)
% We will store states as X(:,j) for j=1:N+1 and controls as U(:,j) for j=1:N
gamma_init = deg2rad(-65); % initial angle in radians
gamma_final = -90*pi/180; % final angle in radians
r_guess = linspace(r_init, r_final, N+1); % linearly decreasing from r_init to r_final
V_guess = linspace(V_init, V_final, N+1); % linearly decreasing from V_init to V_final
gamma_guess = linspace(gamma_init, gamma_final, N+1); % linearly decreasing from gamma_init to gamma_final
z_guess = linspace(z_init, -3, N+1); % linearly decreasing from z_init
X_nominal = [r_guess; zeros(1,N+1); V_guess; gamma_guess; z_guess];
U_nominal = zeros(5,N); % guess zero control
% Successive iteration parameters
max_iter = 10;
tol_x = 1e-1;
tol_u = 1e-3;
for k = 1:max_iter
% Compute linearization around curent nominal trajectory
[A_cell, B_cell, c_cell] = compute_linearized_dynamics(X_nominal, U_nominal, dt, param);
% Solve the SOCP approximation with CVX
[X_new, U_new] = solve_socp_problem(X_nominal, U_nominal, A_cell, B_cell, c_cell, dt, x0, xf, param, control_bounds);
% Check convergence
dx = max(max(abs(X_new - X_nominal)));
du = max(max(abs(U_new - U_nominal)));
fprintf('Iteration %d: dx=%g, du=%g\n', k, dx, du);
X_nominal = X_new;
U_nominal = U_new;
if dx < tol_x && du < tol_u
fprintf('Converged at iteration %d\n', k);
break;
end
end
% X_current, U_current now hold the converged solution
% Plot results as needed
figure(1);
plot((0:dt:tf), X_nominal(1,:)); xlabel('time (s)'); ylabel('r');
title('Altitude trajectory');
figure(2);
plot((0:dt:tf), X_nominal(3,:)); xlabel('time (s)'); ylabel('V');
title('Velocity trajectory');
solve_socp_problem.m: This is the script that actually formulates the optimization problem (SOCP).
function [X_opt, U_opt] = solve_socp_problem(X_nominal, U_nominal, A_cell, B_cell, c_cell, dt, x0, xf, param, control_bounds)
N = size(U_nominal,2);
nx = size(X_nominal,1);
nu = size(U_nominal,1);
cvx_begin
variables X(nx, N+1) U(nu, N)
% Objective: minimize final mass (or equivalently -final z for max mass)
% Here, assume we want to maximize mass: final z is log(m), so maximizing mass = maximizing z(tf)
% Minimizing negative of final z:
minimize(-X(5, end))
% Extract problem paramters
m0 = param(1);
S_ref = param(2);
I_sp = param(3);
g0 = param(4);
R0 = param(5);
rho = param(6);
a = param(7);
% Aerodynamic models
C_L = @(alpha) 5.6676*alpha - 0.0049; % relation between CL and alpha
C_D = @(alpha) 135.9042*alpha^4 - 4.9921*alpha^3 - 0.5485*alpha^2 + 0.0420*alpha + 0.0058; % relation between CD and alpha
C_hat_L = C_L(5*pi/180); % in theory, this should be a function of time but we keep it constant
% Constraints:
% Initial condition
X(:,1) == x0;
% Final angle of attack
alpha_final = 180*pi/180;
% alpha_final_min = -2*pi/180;
% alpha_final_max = 2*pi/180;
% Control bounds
T_min = control_bounds(1);
T_max = control_bounds(2);
alpha_min = control_bounds(3);
alpha_max = control_bounds(4);
e_min = control_bounds(5);
e_max = control_bounds(6);
% Terminal conditions
X(1,end) == xf(1);
X(2,end) == xf(2);
0 <= X(3,end) <= xf(3);
X(4,end) == xf(4);
for j = 1:N
% DISCRETE DYNAMICS: X(:,j+1) = X(:,j) + dt*(A_j*X(:,j) + B_j*U(:,j) + c_j)
A_j = A_cell{j};
B_j = B_cell{j};
c_j = c_cell{j};
X(:,j+1) == X(:,j) + dt*(A_j*X(:,j) + B_j*U(:,j) + c_j);
% CONSTRAINTS
u_1 = U(1,j); u_2 = U(2,j); u_3 = U(3,j); u_4 = U(4,j); u_5 = U(5,j); % extract control inputs (decision vars.)
z_j = X(5,j); % extract z (decision var.)
z_nominal_current = X_nominal(5,j); % z^(k)(t)
z_nominal_final = X_nominal(5,end); % z^(k) (tf)
eta_min = C_L(alpha_min)/C_hat_L;
eta_max = C_L(alpha_max)/C_hat_L;
eta_bar = max(abs(eta_min), abs(eta_max));
eta_f = C_L(alpha_final)/C_hat_L;
delta = [10e3/R0, 10e3/R0, 200/sqrt(g0*R0), 50*pi/180, 3000/m0]'; % linearization trust region for state trajectory
Delta_u1 = 0.5*max(eta_bar); % linearization trust region for u1
% Compute k_1, d_1
k_1 = -eta_min * exp(-z_nominal_current);
d_1 = -k_1 * (1 + z_nominal_current);
% Compute k_bar_1, d_bar_1
k_bar_1 = -eta_max * exp(-z_nominal_current);
d_bar_1 = -k_bar_1 * (1 + z_nominal_current);
% Compute k_bar_2, d_bar_2
k_bar_2 = -2 * eta_bar^2 * exp(-2*z_nominal_current);
d_bar_2 = -k_bar_2 * (1 + 2*z_nominal_current) / 2;
% Compute k_5, d_5
k_5 = -T_min * exp(-z_nominal_current);
d_5 = -k_5 * (1 + z_nominal_current);
% Compute k_bar_5, d_bar_5
k_bar_5 = -T_max * exp(-z_nominal_current);
d_bar_5 = -k_bar_5 * (1 + z_nominal_current);
% Compute k_1f, d_1f for final time
k_1f = -eta_f * exp(-z_nominal_final);
d_1f = -k_1f * (1 + z_nominal_final);
% Compute k_u, d_u
k_u = 2*U_nominal(1,j);
d_u = U_nominal(1,j)^2 - k_u*U_nominal(1,j);
% ===== STATE DEVIATION CONSTRAINT (65) =====
abs(X(:,j) - X_nominal(:,j)) <= delta; % This applies element-wise
% ===== RELATION BETWEEN u_1 AND u_2 (66) =====
k_u*u_1 + d_u == u_2;
% ===== CONTROL DEVIATION CONSTRAINT (67) =====
abs(u_1 - U_nominal(1,j)) <= Delta_u1;
% ===== BOUNDS ON u_1 (68) =====
k_1*z_j + d_1 <= u_1 <= (k_bar_1)*z_j + d_bar_1;
% ===== BOUNDS ON u_2 (69) =====
0 <= u_2 <= k_bar_2*z_j + d_bar_2;
% ===== THRUST DIRECTION CONSTRAINT (70) =====
norm([u_3; u_4],2) <= u_5;
% ===== BOUNDS ON u_5 (71) =====
k_5*z_j + d_5 <= u_5 <= k_bar_5*z_j + d_bar_5;
% ===== ANGLE CONSTRAINTS (72)-(73) =====
u_4 >= u_3*tan(e_min);
u_4 <= u_3*tan(e_max);
end
% ===== Final constraints on u_1 and u_4 (75) =====
abs(U(1,end)) <= k_1f*X(5,end) + d_1f;
U(4,end) == 0;
cvx_end
if strcmp(cvx_status,'Infeasible') || strcmp(cvx_status,'Failed') || strcmp(cvx_status,'Unbounded')
% Handle failure:
% For example, set X_opt = X_nominal; U_opt = U_nominal; or raise an error
X_opt = X_nominal;
U_opt = U_nominal;
return;
end
partial_linearization.m: This script linearizes the rocket dynamics (nonlinear) around a single point (x,u).
function [A_cont, B_cont, c_cont] = partial_linearization(x, u, param)
% Computes A, B, c (cont. time) of partially linearized system at the
% current state x.
%
% x = [r; s; V; gamma; z=log(m)]
% u = [u_1; u_2; u_3; u_4; u_5]
% param = [m0; S_ref; I_sp; g0; R0; rho; a]
% Extract problem parameters
m0 = param(1);
S_ref = param(2);
I_sp = param(3);
g0 = param(4);
R0 = param(5);
rho = param(6);
a = param(7);
% Extract state vector
r = x(1);
s = x(2);
V = x(3);
gamma = x(4);
z = x(5);
% Aerodynamic model
C_L = @(alpha) 5.6676*alpha - 0.0049; % relation between CL and alpha
C_D = @(alpha) 135.9042*alpha^4 - 4.9921*alpha^3 - 0.5485*alpha^2 + 0.0420*alpha + 0.0058; % relation between CD and alpha
C_hat_L = C_L(5*pi/180); % CL at alpha that maximizes lift-to-drag
C_hat_D = C_D(5*pi/180); % CD at alpha that maximizes lift-to-drag
D_hat = @(V) 0.5*rho*(V*sqrt(g0*R0))^2*S_ref*C_hat_D/(m0*g0); % drag for max lift-drag
L_hat = @(V) 0.5*rho*(V*sqrt(g0*R0))*S_ref*C_hat_L/(m0*g0); % lift for max lift-drag
f = [V*sin(gamma);
V*cos(gamma);
-sin(gamma)/(r^2) - 0.5*D_hat(V)*exp(-z);
-cos(gamma)/(r^2*V);
0];
A_cont = [0 0 sin(gamma) V*cos(gamma) 0;
0 0 cos(gamma) -V*sin(gamma) 0;
2*sin(gamma)/(r^3) 0 0 -cos(gamma)/(r^2) 1/2*D_hat(V)*exp(-z);
2*cos(gamma)/(V*r^3) 0 cos(gamma)/(V^2*r^2) sin(gamma)/(V*r^2) 0;
0 0 0 0 0];
B_cont = [0 0 0 0 0;
0 0 0 0 0;
0 -0.5*D_hat(V)*exp(z) -1 0 0;
L_hat(V)/V 0 0 -1/V 0;
0 0 0 0 -1/I_sp];
c_cont = f - A_cont*x;
end
compute_linearized_dynamics.m: This script takes in a trajectory (x(t),u(t)) and linearizes the rocket dynamics around this entire trajectory.
function [A_cell, B_cell, c_cell] = compute_linearized_dynamics(X, U, dt, param)
% Linearizes the dynamics around a trajectory X and U and returns the
% time-varying A, B, c matrices for the resulting LTV system.
%
% X: state trajectory to linearize about
% U: input trajectory to linearize about
N = size(U,2); % number of time steps
nx = size(X,1);
nu = size(U,1);
A_cell = cell(N,1);
B_cell = cell(N,1);
c_cell = cell(N,1);
for j = 1:N
x_j = X(:,j);
u_j = U(:,j);
% Compute A(x_j), B(x_j), c(x_j) for continuous-time system
[A_cont, B_cont, c_cont] = partial_linearization(x_j, u_j, param);
% Here you might discretize the system if needed:
% For a simple Euler discretization:
A_cell{j} = A_cont;
B_cell{j} = B_cont;
c_cell{j} = c_cont;
end
end