How to use fmincon for minimizing multiple variables

Hello, I'm a student of the Master Degree in Control Systems Engineering, and I have to study a scientific paper and create the matlab code for solving it and obtain the right results.

This scientific paper speaks about a problem of Optimal Control.

I send you the paper, in which there are all the informations.

I also send you the Matlab codes that I’m trying to do for solving this problem, but I think that my code it’s not good at the moment.

Thank you

main.m

% PARAMETERS CONFIGURATION

beta_0 = 0.292; %friction coefficient
beta_1 = 1.005;
beta_2 = 2.652*(10^(-4)); %ohmic losses coefficient

t_0 = 0; % initial time
t_f = 1080; % final time

tau = 5; % step time (discrete)
N = 216; % size of discrete interval

v_t0 = 70; % Initial velocity
v_tf = 70; % Final velocity

s_t0 = 0; % Initial position
s_tf = 21; % Final position

m = 15950; % mass
g = 9.81; % gravity force
c_r = 0.1; % rolling force coefficient
sigma_d = 3.1246; % drag coefficient

v_min = 60; % minimal velocity
v_max = 80; % maximal velocity

% initial values

S0 = s_t0;
V0 = v_t0;
A0 = 0;

% initial conditions

init = [ S0; V0; A0 ]*ones(1,N);

% boundaries

lb = [ -inf; v_min; -inf ]*ones(1,N);
ub = [ inf; v_max; inf ]*ones(1,N);

options = optimoptions('fmincon','MaxFunEvals',5000000,'MaxIterations', 1000000);

[opt_sol, fval, exitflag, output, lambda, grad, hessian] = fmincon(@opt, init, [], [], [], [], lb, ub, @constr, options) ;

% for i=1:1:N
      
%     S(i+1) = S(i) + tau*V(i);
%     V(i+1) = V(i) + tau*A(i); 

constr.m

function [c, ceq] = constr(opt_sol)

N = 216; % size of discrete interval


s_t0 = 0; % Initial position
v_t0 = 70; % Initial velocity

s_tf = 21; % Final position
v_tf = 70; % Final velocity

S = opt_sol(1,1:N);
V = opt_sol(2,1:N);


c = [];

%initial values
ceq(1) = S(1) - s_t0;
ceq(2) = V(1) - v_t0;

% INSERIRE VINCOLI
%     for i=1:1:N
%         
%         S(i+1) = S(i) + tau*V(i);
%         V(i+1) = V(i) + tau*A(i); 
%     end

% final values 
ceq(3) = S(N) - s_tf;
ceq(4) = V(N) - v_tf;

[Riccardo Draghi]

function J = opt(opt_sol)

tau = 5; % step time (discrete)
N = 216; % size of discrete interval

beta_0 = 0.292; %friction coefficient
beta_1 = 1.005;
beta_2 = 2.652*(10^(-4)); %ohmic losses coefficient

m = 15950; % mass
g = 9.81; % gravity force
c_r = 0.1; % rolling force coefficient
sigma_d = 3.1246; % drag coefficient


% initializing unknown variables
%H = zeros(1,N+1);
PR = zeros(1, N+1);
gamma_g = zeros(1,N+1);
gamma_r = zeros(1,N+1);
%fi = zeros(1,N+1);

% initializig controlled variables

S = zeros(1,N+1);
V = zeros(1,N+1);
A = zeros(1,N+1);

S(1) = 0 ;
V(1) = 70 ;
A(1) = 0 ;

S = opt_sol(1,1:N);
V = opt_sol(2,1:N);
A = opt_sol(3,1:N);

% computation of gamma_g
% syms t
% eq = 225*cos(((3*pi)/21000)*t + (pi/4)) + 225;
% ris = diff(eq,t);

for i=1:1:N
          
      S(i+1) = S(i) + tau*V(i);
%     H(i) = 225*cos(((3*pi)/21000)*S(i) + (pi/4)) + 225;
      gamma_g(i) = -(9*pi*sin(pi/4 + (pi*S(i))/7000))/280;
      alpha = asin(gamma_g(i));
      gamma_r(i) = cos(alpha);
%     fi(i) = gamma_g(i) + (c_r*gamma_r(i));
      V(i+1) = V(i) + tau*A(i);
    
      PR(i) = tau*(beta_0*(V(i)^2) + beta_1*sigma_d*(V(i)^3) + beta_2*((m*A(i))^2) + (beta_2*(m*g*gamma_g(i) + sigma_d*(V(i)^2) + c_r*m*g*gamma_r(i))^2) + 2*beta_2*(m^2)*g*A(i)*(gamma_g(i)+c_r*gamma_r(i)));
    
end

J = sum(PR);
end

 

    

[Riccardo Draghi]

And here's my solution.

As you can see, the acceleration changes its value every time, but the velocity and the position remain constant, so at the moment doens't work. What' the problem in your opinion? Thank you

[Walter Roberson]

S = zeros(1,N+1);
V = zeros(1,N+1);
A = zeros(1,N+1);

S(1) = 0 ;
V(1) = 70 ;
A(1) = 0 ;

S = opt_sol(1,1:N);
V = opt_sol(2,1:N);
A = opt_sol(3,1:N);

Why do you create S and V and A as all zeros and initialize one entry, and then overwrite all of S, V, and A?

for i=1:1:N
          
      S(i+1) = S(i) + tau*V(i);

That overwrite S(2) before you have used the stored S(2) value, so you effectively ignore all of the passed-in values for S other than the very first. That would not seem to be wise. Likewise you overwrite V as well.

[Riccardo Draghi]

First of all, thanks for you answer, I'm fixing these parts of code.

For what concerns the result, do you know why I'm not able to get the correct values of velocities and positions?

In my results I have all constant values for position (0 0 0 0 0 0 ... ) and velocity (70 70 70 70 70 70 ..)

[Matt J]

Riccardo Draghi commented

Here's the paper.