How to express this constraint in CVX

1

Hi everyone! And thank you in advance for taking the time to read my post.

I am new to CVX and I am trying to reproduce the results of a couple of scientific papers regarding the powered descend guidance problem [A Powered Descent Guidance Algorithm for Mars Pinpoint Landing and Lossless Convexification of Nonconvex Control Bound and Pointing Constraints of the Soft Landing Optimal Control Problem]. Right now my implementation is as follows, and produces a feasible solution, altthough not all the constraints have been added yet.

clc
clear
close all

clear cvx_problem

n_steps = 350;
disp(['Number of steps: ', num2str(n_steps)]);

% Constants
g = 3.7114; % [m/s2]
g_E = 9.81; % [m/s2]
x_0 = 1000; % [m]
z_0 = 3000; % [m]
vx_0 = 10;  % [m/s]
vz_0 = -50; % [m/s]
m_0 = 1905; % [kg]

thrust_engine = 3100; % [N]
thrust_max = 0.8*thrust_engine*6; % [N]
thrust_min = 0.2*thrust_engine*6; % [N]
isp = 225;
alpha = 1 / (isp * g_E);

dt = 0.1;

cvx_begin

variables st(5,n_steps+1) u(3,n_steps)

st(:,1) == [x_0; z_0; vx_0; vz_0; log(m_0)]; % [m, m, m/s, m/s, log(kg)]

for k = 1:n_steps,

    % State vector dynamics
    st(1,k+1) == 0.5 * u(1,k) * dt^2 + st(3,k) * dt + st(1,k);
    st(2,k+1) == 0.5 * u(2,k) * dt^2 + st(4,k) * dt + st(2,k);
    st(3,k+1) == u(1,k) * dt + st(3,k);
    st(4,k+1) == u(2,k) * dt + st(4,k);
    st(5,k+1) == -alpha * u(3,k) * dt + st(5,k);

    % Control constraints
    z_0 = log(m_0 - alpha * thrust_max * (k-1) * dt);
    thrust_min * exp(-z_0) * (1 - (st(5,k) - z_0) + 0.5*(st(5,k) - z_0)^2) <= u(3,k) <= thrust_max * exp(-z_0) * (1-(st(5,k) - z_0));
    norm([u(1,k),u(2,k)]) <= u(3,k);
    
    % State vector constraints
    0 <= st(2,k);

end

st(1,n_steps+1) == 0.0001;
st(2,n_steps+1) == 0.0001;
st(3,n_steps+1) == 0.0001;
st(4,n_steps+1) == 0.0001;
st(5,n_steps+1) >= 0.0001;

minimize(sum(u(3,:)));

cvx_end

%% Plots
if (strcmp(cvx_status,'Solved'))
   disp('Problem solved');

   time = 0:dt:n_steps*dt;
   mass = exp(st(5,:));
   thrust_x = u(1,:) .* mass(1:n_steps);
   thrust_z = u(2,:) .* mass(1:n_steps);
   thrust_mag = u(3,:) .* mass(1:n_steps);
   thrust_mag2 = sqrt(thrust_x.^2 + thrust_z.^2);

   figure(); 
   subplot(4,2,1);
   plot(st(1,:), st(2,:),'b-'); 
   title('X vs Z');
   subplot(4,2,2);       
   plot(st(3,:), st(4,:),'b-');
   title('VX vs VZ');
   subplot(4,2,3);
   plot(time, mass, 'b-');
   title('Mass vs Time');
   subplot(4,2,4);
   plot(time, st(1,:), 'b-');
   title('X vs Time');
   subplot(4,2,5);
   plot(time, st(2,:), 'b-');
   title('Z vs Time');
   subplot(4,2,6);
   plot(time, st(3,:), 'b-');
   title('VX vs Time');
   subplot(4,2,7);
   plot(time, st(4,:), 'b-');
   title('VZ vs Time');

   figure();
   plot(time(1:n_steps), thrust_x, 'b-');
   hold on;
   plot(time(1:n_steps), thrust_z, 'r-');
   plot(time(1:n_steps), thrust_mag, 'k-');
   title('Thrust components and magnitude');
   hold off;

   figure();
   subplot(2,1,1);
   plot(time(1:n_steps), thrust_mag, 'b-');
   title('Thrust mag');
   subplot(2,1,2);
   plot(time(1:n_steps), atan2d(thrust_z, thrust_x), 'b-');
   title('Thrust angle');

   figure();
   plot(time(1:n_steps), thrust_mag, 'b-');
   hold on;
   plot(time(1:n_steps), thrust_mag2, 'r-');
   title('Thrust magnitudes by Sigma and sqrt of components');

   figure();
   plot(st(1,:), st(2,:),'LineWidth',2);
   hold on;
   q = quiver(st(1,1:n_steps), st(2,1:n_steps),thrust_x,thrust_z);
   title('Position and thrust');

end

My doubt comes when I try to add a constraint in the rate of change of the angular velocity of the thrust angle, which is defined as follows in one of the mentioned papers (equation 34):

image
image1189×170 22.4 KB

image
image1135×146 12.1 KB

and I is the identity matrix with dimension 2x2, u is a vector of 2 elements that represents the components of the thrust vector (control variables) divided by mass, sigma is a slack variable that in the optimized solution is equal to the magnitude of the thrust divided by mass and omega is defined as the cosine of the maximum angular rate multiplied by delta t [omega = cosd(max_angular_rate * t)].

My code and its output with this constraint is as follows:

clc
clear
close all

clear cvx_problem

n_steps = 350;
disp(['Number of steps: ', num2str(n_steps)]);

% Constants
g = 3.7114; % [m/s2]
g_E = 9.81; % [m/s2]
x_0 = 1000; % [m]
z_0 = 3000; % [m]
vx_0 = 10;  % [m/s]
vz_0 = -50; % [m/s]
m_0 = 1905; % [kg]

thrust_engine = 3100; % [N]
thrust_max = 0.8*thrust_engine*6; % [N]
thrust_min = 0.2*thrust_engine*6; % [N]
isp = 225;
alpha = 1 / (isp * g_E);

dt = 0.1;

% Constraints variables
max_angular_rate = 2; % [deg]
omega = cosd(max_angular_rate * dt);

q_matrix = [1       0 -0.5    0;
            0       1    0 -0.5;
            -0.5    0    1    0;
            0    -0.5    0    1];

p_matrix = [1        -omega/2;
            -omega/2        1];   

sqrt_q_matrix = q_matrix^0.5;
sqrt_p_matrix = p_matrix^0.5;

cvx_begin

% u is composed by the u vector plus sigma
% st is composed by x, z, vx, vz and mass
variables st(5,n_steps+1) u(3,n_steps)

st(:,1) == [x_0; z_0; vx_0; vz_0; log(m_0)]; % [m, m, m/s, m/s, log(kg)]

lambda_p_matrix = lambda_min(sqrt_p_matrix);

for k = 1:n_steps,

    % State vector dynamics
    st(1,k+1) == 0.5 * u(1,k) * dt^2 + st(3,k) * dt + st(1,k);
    st(2,k+1) == 0.5 * u(2,k) * dt^2 + st(4,k) * dt + st(2,k);
    st(3,k+1) == u(1,k) * dt + st(3,k);
    st(4,k+1) == u(2,k) * dt + st(4,k);
    st(5,k+1) == -alpha * u(3,k) * dt + st(5,k);

    % Control constraints
    z_0 = log(m_0 - alpha * thrust_max * (k-1) * dt);
    thrust_min * exp(-z_0) * (1 - (st(5,k) - z_0) + 0.5*(st(5,k) - z_0)^2) <= u(3,k) <= thrust_max * exp(-z_0) * (1-(st(5,k) - z_0));
    norm([u(1,k),u(2,k)]) <= u(3,k);

    % New constraint for the angular rate of change of the thrust
    if (k >= 2)
       z_k = [u(1,k); u(2,k); u(1,k-1); u(2,k-1)];           
       norm(sqrt_q_matrix * z_k) <= lambda_p_matrix/sqrt(2) * (u(3,k) + u(3,k-1));
    end
    
    % State vector constraints
    0 <= st(2,k);

end

st(1,n_steps+1) == 0.0001;
st(2,n_steps+1) == 0.0001;
st(3,n_steps+1) == 0.0001;
st(4,n_steps+1) == 0.0001;
st(5,n_steps+1) >= 0.0001;

minimize(sum(u(3,:)));

cvx_end

%% Plots
if (strcmp(cvx_status,'Solved'))
   disp('Problem solved');

   time = 0:dt:n_steps*dt;
   mass = exp(st(5,:));
   thrust_x = u(1,:) .* mass(1:n_steps);
   thrust_z = u(2,:) .* mass(1:n_steps);
   thrust_mag = u(3,:) .* mass(1:n_steps);
   thrust_mag2 = sqrt(thrust_x.^2 + thrust_z.^2);

   figure(); 
   subplot(4,2,1);
   plot(st(1,:), st(2,:),'b-'); 
   title('X vs Z');
   subplot(4,2,2);       
   plot(st(3,:), st(4,:),'b-');
   title('VX vs VZ');
   subplot(4,2,3);
   plot(time, mass, 'b-');
   title('Mass vs Time');
   subplot(4,2,4);
   plot(time, st(1,:), 'b-');
   title('X vs Time');
   subplot(4,2,5);
   plot(time, st(2,:), 'b-');
   title('Z vs Time');
   subplot(4,2,6);
   plot(time, st(3,:), 'b-');
   title('VX vs Time');
   subplot(4,2,7);
   plot(time, st(4,:), 'b-');
   title('VZ vs Time');

   figure();
   plot(time(1:n_steps), thrust_x, 'b-');
   hold on;
   plot(time(1:n_steps), thrust_z, 'r-');
   plot(time(1:n_steps), thrust_mag, 'k-');
   title('Thrust components and magnitude');
   hold off;

   figure();
   subplot(2,1,1);
   plot(time(1:n_steps), thrust_mag, 'b-');
   title('Thrust mag');
   subplot(2,1,2);
   plot(time(1:n_steps), atan2d(thrust_z, thrust_x), 'b-');
   title('Thrust angle');

   figure();
   plot(time(1:n_steps), thrust_mag, 'b-');
   hold on;
   plot(time(1:n_steps), thrust_mag2, 'r-');
   title('Thrust magnitudes by Sigma and sqrt of components');

   figure();
   plot(st(1,:), st(2,:),'LineWidth',2);
   hold on;
   q = quiver(st(1,1:n_steps), st(2,1:n_steps),thrust_x,thrust_z);
   title('Position and thrust');

end

Output for the optimization with the new constraint:
Number of steps: 350

Calling SeDuMi 1.3.4: 6377 variables, 2877 equality constraints
For improved efficiency, SeDuMi is solving the dual problem.

SeDuMi 1.3.4 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, Adaptive Step-Differentiation, theta = 0.250, beta = 0.500
eqs m = 2877, order n = 3851, dim = 6379, blocks = 1051
nnz(A) = 13259 + 436, nnz(ADA) = 27269, nnz(L) = 30705
Handling 2 + 1 dense columns.
it : by gap delta rate t/tP t/tD* feas cg cg prec
0 : 5.26E+01 0.000
1 : 2.84E+04 3.96E+01 0.000 0.7541 0.9000 0.9000 11.24 1 1 4.5E+00
2 : 7.70E+03 3.15E+01 0.000 0.7942 0.9000 0.9000 3.77 1 1 2.8E+00
3 : 1.20E+03 2.26E+01 0.000 0.7173 0.9000 0.9000 2.81 1 1 1.7E+00
4 : -3.70E+03 1.59E+01 0.000 0.7061 0.9000 0.9000 2.03 1 1 1.0E+00
5 : -4.95E+03 1.05E+01 0.000 0.6595 0.9000 0.9000 1.63 1 1 6.0E-01
6 : -4.56E+03 6.53E+00 0.000 0.6204 0.9000 0.9000 1.36 1 1 3.6E-01
7 : -3.86E+03 3.92E+00 0.000 0.6006 0.9000 0.9000 1.18 1 1 2.1E-01
8 : -2.97E+03 2.27E+00 0.000 0.5800 0.9000 0.9000 1.07 1 1 1.2E-01
9 : -2.04E+03 1.26E+00 0.000 0.5522 0.9000 0.9000 1.00 1 1 7.3E-02
10 : -1.48E+03 2.89E-07 0.000 0.0000 0.0000 0.9000 0.96 1 1 5.8E-02
11 : -1.32E+03 5.02E-08 0.000 0.1739 0.9260 0.9000 0.96 1 1 1.5E-02
12 : -1.32E+03 2.20E-08 0.000 0.4374 0.9000 0.0000 1.00 1 1 9.4E-03
13 : -1.36E+03 8.94E-09 0.000 0.4070 0.9000 0.5129 1.02 1 1 4.7E-03
14 : -1.51E+03 2.88E-09 0.000 0.3219 0.9113 0.9000 1.02 1 1 1.6E-03
15 : -1.58E+03 1.39E-09 0.000 0.4847 0.9356 0.9000 0.99 1 1 8.9E-04
16 : -1.63E+03 8.92E-10 0.000 0.6395 0.9357 0.9000 0.88 1 1 6.2E-04
17 : -1.68E+03 5.92E-10 0.000 0.6641 0.9325 0.9000 0.77 1 1 4.5E-04
18 : -1.73E+03 4.06E-10 0.000 0.6860 0.9261 0.9000 0.65 1 1 3.4E-04
19 : -1.77E+03 2.87E-10 0.000 0.7072 0.9122 0.9000 0.51 1 1 2.7E-04
20 : -1.81E+03 2.09E-10 0.000 0.7261 0.9077 0.9000 0.36 1 1 2.2E-04
21 : -1.85E+03 1.53E-10 0.000 0.7341 0.9051 0.9000 0.23 1 1 1.9E-04
22 : -1.89E+03 1.13E-10 0.013 0.7388 0.9067 0.9000 0.18 1 1 1.6E-04
23 : -1.93E+03 8.26E-11 0.000 0.7298 0.9013 0.9000 0.21 1 1 1.3E-04
24 : -1.96E+03 5.93E-11 0.044 0.7183 0.9000 0.9015 0.24 1 1 1.1E-04
25 : -2.00E+03 4.33E-11 0.000 0.7303 0.9000 0.9020 0.29 1 1 8.8E-05
26 : -2.03E+03 3.15E-11 0.073 0.7283 0.9000 0.9065 0.32 1 1 7.1E-05
27 : -2.05E+03 2.31E-11 0.000 0.7333 0.9000 0.9063 0.34 2 2 5.9E-05
28 : -2.07E+03 1.74E-11 0.000 0.7514 0.9000 0.9070 0.33 2 2 4.9E-05
29 : -2.09E+03 1.33E-11 0.000 0.7662 0.9000 0.9093 0.28 1 1 4.2E-05
30 : -2.11E+03 1.03E-11 0.000 0.7753 0.9000 0.9121 0.21 2 2 3.7E-05
31 : -2.12E+03 8.13E-12 0.127 0.7870 0.9000 0.9057 0.13 1 2 3.2E-05
32 : -2.13E+03 6.65E-12 0.000 0.8188 0.9000 0.8126 0.05 2 1 3.0E-05
33 : -2.15E+03 5.52E-12 0.000 0.8301 0.9000 0.9000 -0.01 2 2 2.7E-05
34 : -2.16E+03 4.60E-12 0.000 0.8325 0.9000 0.9000 -0.06 2 2 2.5E-05
35 : -2.17E+03 3.83E-12 0.000 0.8322 0.9000 0.9000 -0.09 2 2 2.4E-05
36 : -2.18E+03 3.19E-12 0.000 0.8331 0.9000 0.9000 -0.11 2 2 2.2E-05
37 : -2.19E+03 2.66E-12 0.000 0.8336 0.9000 0.9000 -0.12 2 2 2.0E-05
38 : -2.20E+03 2.21E-12 0.000 0.8332 0.9000 0.9000 -0.12 2 2 1.9E-05
39 : -2.21E+03 1.84E-12 0.000 0.8305 0.9000 0.9000 -0.11 2 2 1.8E-05
40 : -2.23E+03 1.53E-12 0.000 0.8344 0.9000 0.9000 -0.09 2 2 1.6E-05
41 : -2.24E+03 1.28E-12 0.000 0.8338 0.9000 0.9000 -0.07 2 2 1.5E-05
42 : -2.25E+03 1.06E-12 0.000 0.8268 0.9000 0.9000 -0.05 2 2 1.4E-05
43 : -2.27E+03 8.80E-13 0.000 0.8317 0.9000 0.9000 -0.03 2 2 1.3E-05
44 : -2.28E+03 7.28E-13 0.000 0.8278 0.9000 0.9000 0.00 2 2 1.2E-05
45 : -2.29E+03 6.03E-13 0.000 0.8280 0.9000 0.9000 0.03 2 2 1.1E-05
46 : -2.31E+03 5.00E-13 0.000 0.8283 0.9000 0.9000 0.06 2 2 9.6E-06
47 : -2.32E+03 4.12E-13 0.000 0.8239 0.9000 0.9000 0.09 2 2 8.7E-06
48 : -2.33E+03 3.40E-13 0.000 0.8266 0.9000 0.9000 0.13 2 2 7.8E-06
49 : -2.35E+03 2.82E-13 0.076 0.8293 0.9000 0.9000 0.16 2 2 7.0E-06
50 : -2.36E+03 2.34E-13 0.000 0.8291 0.9000 0.9000 0.20 2 2 6.3E-06
51 : -2.37E+03 1.94E-13 0.045 0.8276 0.9000 0.9000 0.23 2 2 5.6E-06
52 : -2.38E+03 1.61E-13 0.000 0.8303 0.9000 0.9000 0.27 2 2 5.0E-06
53 : -2.39E+03 1.33E-13 0.000 0.8290 0.9000 0.9000 0.31 2 2 4.4E-06
54 : -2.40E+03 1.11E-13 0.000 0.8298 0.9000 0.9000 0.35 2 2 3.9E-06
55 : -2.42E+03 9.01E-14 0.000 0.8148 0.9000 0.9000 0.38 2 2 3.4E-06
56 : -2.43E+03 7.38E-14 0.000 0.8185 0.9020 0.9000 0.42 2 2 3.0E-06
57 : -2.44E+03 6.03E-14 0.000 0.8182 0.9059 0.9000 0.47 2 2 2.6E-06
58 : -2.44E+03 4.90E-14 0.000 0.8114 0.9000 0.9048 0.52 2 2 2.2E-06
59 : -2.45E+03 3.93E-14 0.000 0.8028 0.9000 0.8108 0.56 2 2 1.8E-06
60 : -2.46E+03 3.17E-14 0.000 0.8052 0.9113 0.9000 0.60 2 2 1.5E-06
61 : -2.47E+03 2.27E-14 0.000 0.7167 0.9000 0.8667 0.65 2 2 1.2E-06
62 : -2.48E+03 1.85E-14 0.000 0.8148 0.9000 0.9000 0.68 2 2 9.9E-07
63 : -2.48E+03 1.42E-14 0.000 0.7700 0.9000 0.9000 0.74 2 2 7.9E-07
64 : -2.49E+03 1.07E-14 0.000 0.7498 0.9000 0.9000 0.79 2 2 6.1E-07
65 : -2.49E+03 7.03E-15 0.000 0.6584 0.9108 0.9000 0.83 2 2 4.1E-07
66 : -2.50E+03 4.94E-15 0.000 0.7026 0.9411 0.9000 0.88 2 2 2.7E-07
67 : -2.50E+03 2.91E-15 0.000 0.5892 0.7406 0.9000 0.90 21 21 1.6E-07
68 : -2.50E+03 1.56E-15 0.000 0.5351 0.1943 0.9000 0.93 31 35 9.6E-08
69 : -2.50E+03 8.99E-16 0.000 0.5776 0.7962 0.9000 0.94 29 42 5.7E-08
70 : -2.50E+03 3.24E-16 0.000 0.3604 0.9000 0.9000 0.97 43 39 2.1E-08
71 : -2.51E+03 6.94E-17 0.000 0.2142 0.9000 0.9000 0.99 44 55 4.4E-09

iter seconds digits cx by
71 9.5 Inf -2.5054529403e+03 -2.5054528715e+03
|Ax-b| = 2.2e-08, [Ay-c]_+ = 4.4E-06, |x|= 7.8e+04, |y|= 2.2e+04

Detailed timing (sec)
Pre IPM Post
9.699E-02 2.558E+00 1.500E-02
Max-norms: ||b||=1, ||c|| = 3000,
Cholesky |add|=36, |skip| = 0, ||L.L|| = 4393.16.

Status: Solved
Optimal value (cvx_optval): +2513.26

Problem solved

However, this output does not follow the new constraint if I look into the output values, as it can be seen in the following image (max angular rate was 2 deg per second and the angle changes around 80 degrees in less than 10 seconds):

image
image1567×855 12.7 KB

What could be my mistake in the second implementation? Do you have any advice in how to solve this issue?

Thank you in advance for your help.

2

Did you numerically evaluate the constraints, as you entered them, using the optimal values of variables returned by CVX? Are all the constraints satisfied to within solver tolerance? If so, and you don’t “like” the solution, there is something wrong or inadequate with your model, as implemented in CVX.

3

Hi Mark,

Thank you for your comments and advice. I have been evaluating numerically all the constraints and for this case it is supposed to be followed, which surprises me to be fair. I compared both constraint 34 in the paper and my implementation (both below) and I cannot see the difference. The only change I have made to the paper is that my problem is in 2D and in the paper is 3D (which I believe it means that my problem should be easier to solve).

Paper:

image
image693×297 37.7 KB

Code (the full code is in the first comment):

% Constraints variables
max_angular_rate = 5; % [deg/s]
omega = cosd(max_angular_rate * dt);

q_matrix = [1       0 -0.5    0;
            0       1    0 -0.5;
            -0.5    0    1    0;
            0    -0.5    0    1];

p_matrix = [1        -omega/2;
            -omega/2        1];   

sqrt_q_matrix = q_matrix^0.5;
sqrt_p_matrix = p_matrix^0.5;

cvx_begin

% ....

lambda_p_matrix = lambda_min(sqrt_p_matrix)

for k = 1:n_steps,
   % ....
    % New constraint for the angular rate of change of the thrust
    if (k >= 2)
       z_k = [u(1,k); u(2,k); u(1,k-1); u(2,k-1)];           
       norm(sqrt_q_matrix * z_k) <= lambda_p_matrix/sqrt(2) * (u(3,k) + u(3,k-1));
    end
    % ....

end

Moreover, I have seen that this constraint is valid only when the max_angular_rate is between 2 and 6, for other values it runs into numerical problems. I believe it could be caused, as I saw in another post in this forum, by the fact that the order of magnitudes of the different variables are significant.

Output:
Calling SeDuMi 1.3.4: 6377 variables, 2877 equality constraints
For improved efficiency, SeDuMi is solving the dual problem.

SeDuMi 1.3.4 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, Adaptive Step-Differentiation, theta = 0.250, beta = 0.500
eqs m = 2877, order n = 3851, dim = 6379, blocks = 1051
nnz(A) = 13259 + 436, nnz(ADA) = 27269, nnz(L) = 30705
Handling 2 + 1 dense columns.
it : by gap delta rate t/tP t/tD* feas cg cg prec
0 : 5.26E+01 0.000
1 : 2.84E+04 3.96E+01 0.000 0.7541 0.9000 0.9000 11.24 1 1 4.5E+00
Run into numerical problems.

iter seconds |Ax| [Ay]_+ |x| |y|
1 0.3 2.0e+02 2.8e+03 1.2e+02 1.3e+04
Failed: no sensible solution/direction found.

Detailed timing (sec)
Pre IPM Post
6.400E-02 1.230E-01 9.998E-03
Max-norms: ||b||=1, ||c|| = 3000,
Cholesky |add|=0, |skip| = 0, ||L.L|| = 3.5975.

Status: Failed
Optimal value (cvx_optval): NaN

Thank you for your time and help, and if you have any advice it will be more than welcome.

4

Bad numerical scaling can cause numerical difficulties and unreliability for optimization solvers. Improve the scaling before you do anything else.

5

Thank you Mark, I will improve it and come back with the results.