Ignored/Incorrect MV constraint in nonlinear MPC block with parameter

Hello! I am implementing a nonlinear model predictive controller using MPC Toolbox in Simulink. A constraint of my NMPC appears to be ignored and I believe it is due to an error in my custon inequality constraint function. Here’s some context:
I am making a mass follow a reference displacement in a vehicle active suspension system. Currently, my NMPC’s manipulated variable is the increment in force for an actuator. My system under control takes a force magnitude as an input rather than an increment in force. Therefore, to constrain the maximum force magnitude allowed in my custom constraint function I need to pass in the previous magnitude as a parameter to add to the manipulated variable. This allows me to compare the sum of the increment and the previous magnitude to the constraint parameter. Currently, when looking at the produced force in a scope after a simulation, the amount is outside of the bounds of Fc_max and Fc_min (+/- 15N).
I have ensured the scope determines the value of the force magnitude correctly, and I have also ensured my cost function, state function, and equality constraints are configured properly. The bus that passes in params is also configured correctly. The controller exhibits acceptable tracking performance, it just ignores this constraint. Furthermore, the other constraints appear to be observed (but this could just be due to my reference not requiring the controller to apply those constraints). This leads me to believe my cineq function is incorrect somehow. The first block of code is my nlmpc setup, and the second is my cineq function. The picture is the force command produced (that violates the constraint).
%% 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
Since my control horizon is 1, the nmpc controller will hold my constrained input constant after the end of Hc. This should mean I do not need to have these constraints run across the whole prediction horizon p for the constraints to apply. Thank you to anyone who responds!Hello! I am implementing a nonlinear model predictive controller using MPC Toolbox in Simulink. A constraint of my NMPC appears to be ignored and I believe it is due to an error in my custon inequality constraint function. Here’s some context:
I am making a mass follow a reference displacement in a vehicle active suspension system. Currently, my NMPC’s manipulated variable is the increment in force for an actuator. My system under control takes a force magnitude as an input rather than an increment in force. Therefore, to constrain the maximum force magnitude allowed in my custom constraint function I need to pass in the previous magnitude as a parameter to add to the manipulated variable. This allows me to compare the sum of the increment and the previous magnitude to the constraint parameter. Currently, when looking at the produced force in a scope after a simulation, the amount is outside of the bounds of Fc_max and Fc_min (+/- 15N).
I have ensured the scope determines the value of the force magnitude correctly, and I have also ensured my cost function, state function, and equality constraints are configured properly. The bus that passes in params is also configured correctly. The controller exhibits acceptable tracking performance, it just ignores this constraint. Furthermore, the other constraints appear to be observed (but this could just be due to my reference not requiring the controller to apply those constraints). This leads me to believe my cineq function is incorrect somehow. The first block of code is my nlmpc setup, and the second is my cineq function. The picture is the force command produced (that violates the constraint).
%% 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;

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
Since my control horizon is 1, the nmpc controller will hold my constrained input constant after the end of Hc. This should mean I do not need to have these constraints run across the whole prediction horizon p for the constraints to apply. Thank you to anyone who responds! Hello! I am implementing a nonlinear model predictive controller using MPC Toolbox in Simulink. A constraint of my NMPC appears to be ignored and I believe it is due to an error in my custon inequality constraint function. Here’s some context:
I am making a mass follow a reference displacement in a vehicle active suspension system. Currently, my NMPC’s manipulated variable is the increment in force for an actuator. My system under control takes a force magnitude as an input rather than an increment in force. Therefore, to constrain the maximum force magnitude allowed in my custom constraint function I need to pass in the previous magnitude as a parameter to add to the manipulated variable. This allows me to compare the sum of the increment and the previous magnitude to the constraint parameter. Currently, when looking at the produced force in a scope after a simulation, the amount is outside of the bounds of Fc_max and Fc_min (+/- 15N).
I have ensured the scope determines the value of the force magnitude correctly, and I have also ensured my cost function, state function, and equality constraints are configured properly. The bus that passes in params is also configured correctly. The controller exhibits acceptable tracking performance, it just ignores this constraint. Furthermore, the other constraints appear to be observed (but this could just be due to my reference not requiring the controller to apply those constraints). This leads me to believe my cineq function is incorrect somehow. The first block of code is my nlmpc setup, and the second is my cineq function. The picture is the force command produced (that violates the constraint).
%% 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;

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;

Cart

Cart