Ignored/Incorrect MV constraint in nonlinear MPC block with parameter (2024)

%% cost function weights (best perf @ w=95000,w_dot=105,R=2,'E=1')

Ts = 0.01;

Q_n = 1000;

Q_ndot = 0; % fix this

R = 15;

E = 1;

%% physical constraints

% maximum forces (currently +/- 15) (units: N)

Fc_max = 15;

Fc_min = -15;

% maximum rate of change in forces (units: N/Ts) (at Ts = 0.01 this is 10)

delta_Fc_max = 1000*Ts;

delta_Fc_min =-1000*Ts;

% maximum displacement of the spacing (units: m)

n_max = 0.05;

n_min = -0.05;

%% misc params

alpha_o = 1/(ms) + 1/(mu);

eta_ddot = 0;

Fc_last = 0;

Hc = 1;

%% NMPC settings, weights, and constraints

nx = 2;

ny = nx;

nu = 1;

nulmpcobj = nlmpc(nx,ny,nu);

nulmpcobj.Ts = Ts;

nulmpcobj.PredictionHorizon = 20;

nulmpcobj.ControlHorizon = Hc;

% how plant states evolve over time. Model is DT.

nulmpcobj.Model.StateFcn = "stateFunction";

nulmpcobj.Model.IsContinuousTime = false;

nulmpcobj.Model.NumberOfParameters = 15;

nulmpcobj.Model.OutputFcn = "outputFunction";

% set up cost function and constraints

nulmpcobj.Optimization.CustomCostFcn = "costFunction";

Optimization.CustomEqConFcn = "eqConFunction";

Optimization.CustomIneqConFcn = "ineqConFunction";

nulmpcobj.Optimization.ReplaceStandardCost = true;

SolverOptions.Algorithm = 'sqp';

% initial conditions

x0 = [0;0];

u0 = 0;

%% WARNING: EVERY TIME THIS IS EDITED, YOU NEED TO EDIT ALL OTHER FUNCTIONS WITH PARAMS...

parameters = {alpha_o,eta_ddot,Q_n,Q_ndot,R,E,Fc_max,Fc_min,delta_Fc_max,delta_Fc_min,n_max,n_min,Ts,Fc_last,Hc};

% Set up parameter bus for NMPC object (if it isn't already initialized)

if exist('paramBus','var')==0

mdl = 'NULMPC_RAS';

open_system(mdl)

createParameterBus(nulmpcobj,[mdl '/NULMPC'],'paramBus',parameters);

end

%% Now the NLMPC and simulation parameters are ready to go!

%% Run the simulation or optionally validate the NMPC functions here in MATLAB.

% rng(0);

% validateFcns(nulmpcobj,rand(nx,1),rand(nu,1),[],parameters);

sim(mdl);

function cineq = ineqConFunction(X,U,data,alpha_o,eta_ddot,Q_n,Q_ndot,R,E,Fc_max,Fc_min,delta_Fc_max,delta_Fc_min,n_max,n_min,Ts,Fc_last,Hc)

cineq = zeros(6,1);

p = data.PredictionHorizon;

% delta constraint max/min

U1 = U(1:Hc,data.MVIndex(1));

cineq(1) = U1 - delta_Fc_max;

cineq(2) = -U1 + delta_Fc_min;

% magnitude

X1 = X(2:p+1,1);

cineq(3) = X1 - n_max;

cineq(4) = -X1 + n_min;

%% output constraint (THIS IS THE ONE I THINK IS INCORRECT)

cineq(5) = (U1 + Fc_last) - Fc_max;

cineq(6) = -(U1 + Fc_last) + Fc_min;

end

⋮

⋮