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!

[Sam Chak]

I don't see the settings on the Force:

nulmpcobj.ManipulatedVariables.Min = Fc_min;
nulmpcobj.ManipulatedVariables.Max = Fc_max;

[Aidan]

Thank you for your response! However, my NMPC problem formulation would not allow me to use those commands. My manipulated variable is an increment in force, but I need to express the constraint in terms of a magnitude . This means I cannot use the built in settings you sent to express a constraint on force magnitude, and must use a custom constraint function. In the custom function I need to express something like in my custom function.

I do constrain the min/max rate allowed by the controller as well (where the code you sent could help), but my custom constraint function appears to work in that case.

The inequality constraint function returns a vector of scalars, where if a row's value is <= 0, the constraint is satisfied for that row. I referenced this documentation when writing my function.

[Sam Chak]

Hi @Aidan

I'm also wondering what went wrong. Could it be the reason that the last two constraints are in the lowest order of priority?

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

cineq(5) = (U1 + Fc_last) - Fc_max; % Previous_F + ΔF ≤ Fmax

cineq(6) = - (U1 + Fc_last) + Fc_min; % Fmin ≤ Previous_F + ΔF

[Aidan]

I put the constraints at the first and second entries, but got the same result. Here's the code I used:

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;
    % the first row of x is current state
    % the last row of u is duplicated

    % magnitude constraint max/min
    U1 = U(1:Hc,data.MVIndex(1));
    cineq(1) = (U1 + Fc_last) - Fc_max;
    cineq(2) = -(U1 + Fc_last) + Fc_min;
    % output constraint max/min
    X1 = X(2:p+1,1);
    cineq(3) = X1 - n_max;
    cineq(4) = -X1 + n_min;
    % delta magnitude max/min
    cineq(5) = U1 - delta_Fc_max;
    cineq(6) = -U1 + delta_Fc_min;
end

[Sam Chak]

Can you possibly check when these two constraints are violated throughout the simulation?

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

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

[Aidan]

I don't know how to do that. I don't think it is as simple as setting up a breakpoint inside the function. Here's what I will try. I will reformulate my MPC problem to have my manipulated variable be the magnitude of force, and try writing a function that creates the same constraints. If that works, I can at least know if the error is due to my syntax or not. Sadly I cannot make use of this reformulated MPC (I need the MV to be for a variety of reasons), but it will help me debug.

Answer 1

[Aidan]

I figured out the issue (and a lot of other ones), but the solution to my main issue was that I had typed:

Optimization.CustomEqConFcn = "eqConFunction";
Optimization.CustomIneqConFcn = "ineqConFunction";

Rather than:

nulmpcobj.Optimization.CustomEqConFcn = "eqConFunction";
nulmpcobj.Optimization.CustomIneqConFcn = "ineqConFunction";

MATLAB just let this happen since I believe both lines edit valid fields (either that, or MATLAB just created a struct and field for me with the same names). My custom functions were therefore not being used. After fixing this, they were mostly written correctly. If anyone comes across this thread looking for MPC Toolbox help with custom functions, feel free to reach out and I can send my files.