Matlab's nonlinear mpc results in u0 control input for all time

Question

I'm working with the nonlinear MPC toolbox of MATLAB and try to implement some straightforward controller. However, given an initial control signal uk, any control signal generated by nlmpcmove is the same as uk (saveStates is filled with whatever uk at zero time is).

Relevant portion of code;

%% Create MPC object
numStates = 12;
numOutputs = 12;
numControl = 2;

nlobj = nlmpc(numStates,numOutputs,numControl);
nlobj.Ts = constants.Ts;
Tswing = 0.1;
nlobj.PredictionHorizon = 5;
nlobj.ControlHorizon = 4;

nlobj.Model.StateFcn = @legStateFcn;
nlobj.Model.IsContinuousTime = false;

% Output function is exact state
nlobj.Model.OutputFcn = @(x,u) x;
nlobj.Jacobian.OutputFcn = @(x,u) eye(numStates);

% Weights
nlobj.Optimization.CustomCostFcn = @costFcn;
nlobj.Optimization.ReplaceStandardCost = true;

nlobj.Optimization.CustomIneqConFcn = @ineqFcn;

% Kalman filter
EKF = extendedKalmanFilter(@legStateFcn,@legMeasurementFcn);
EKF.State = x0;

%% Run MPC
u0 = zeros(numControl,1);
uk = u0;

yinit = x0;
yfinal = x0 + 1;
yref = [yinit yfinal]';

validateFcns(nlobj,yinit,u0) % THIS SAYS IT IS ALL OK

saveState = zeros(length(yinit),length(t));
saveControl = zeros(length(uk),length(t));
y = yinit;
for i = 1:length(t)
    disp(i/length(t)*100)
    
    xk = correct(EKF,y);
    saveState(:,i) = xk;
    
    uk = nlmpcmove(nlobj,xk,uk,yref(2,:));
    saveControl(:,i) = uk;

    % Predict state for next iteration
    predict(EKF,uk);
    % Implement optimal control move
    x = legStateFcn(xk,uk);
    y = x;    
end

Since validateFcns(nlobj,yinit,u0) returns all OK so I don't see a reason to show you all the function handles but just for good measure;

LegStateFcn.m; function xk1 = legStateFcn(xk,uk)

costFcn.m; function J = costFcn(X,U,e,data)

ineqFcn.m; function ineq = ineqFcn(X,U,e,data)

legMeasurementFcn.m; function yk = legMeasurementFcn(xk)

MATLAB does not whine about anything that I am doing here however, given any u0 given before the for-loop, any resulting control signal from nlmpcmove is that same u0. I've debugged by renaming the uk before and after nlmpcmove, removing the nlobj.Jacobian.OutputFcn, simulating the resulting state (which do not adhere to the inequality constraints) and seeing that it indeed follows the dynamics of the system as I would expect.

I can't figure out what I am doing wrong and I'm afraid that it is a case of such an obvious little mistake that I shouldn't have even made it in the first place.

Matlab's nonlinear mpc results in u0 control input for all time

0 Answers0