nonlinear-mpc fmincon error

I am trying to make a bicycle vehicle model follow a reference trajectory that makes a vehicle go straight and slows down before reaching its final destination. I am getting an error from this Error Report from the mpc and can't seem to understand what its asking for since its an error under the mask of the nlmpc...

------------------------------------------------------
Start of Error Report                
------------------------------------------------------
Error using sqpInterface
Nonlinear constraint function is undefined at initial point. Fmincon cannot continue.

Error in fmincon (line 904)
[X,FVAL,EXITFLAG,OUTPUT,LAMBDA,GRAD,HESSIAN] = sqpInterface(funfcn,X,full(A),full(B),full(Aeq),full(Beq), ...
    
Error in nlmpc/nlmpcmove (line 174)
[z, cost, ExitFlag, Out] = fmincon(CostFcn, z0, A, B, [], [], zLB, zUB, ConFcn, fminconOpt);

Error in nmpcblock_interface (line 163)
[mv, ~, Info] = nlmpcmove(nlobj, x, lastmv, ref, md, Options);
------------------------------------------------------
End of Error Report                
------------------------------------------------------
Error:Error occurred when calling NLP solver "fmincon". See the error report displayed above.
Error in nmpcblock_interface.m (line 165)
throw(ME)
Error in 'noMatlabFunctionscen/MPC controller/Nonlinear MPC Controller/MPC/NLMPC' (line 24)

Here is my nlmpc settup code, nonlinear model code, and nonlinear jacobian model...

%% nlmpc settup code
clear all, close all, clc

Rho = 0.2; 
Amax_accel = 4; %% Ego's max acceleration
Amax_brake = 5; %% Object's max brake
Amin_brake = 3; %% Object's min brake
Amin_brake_correct = 2; %% Ego's min brake

% During time [0,Rho] two cars will apply lateral acceleration towards each other then apply lateral deacceleration and the distance between both cars are at the minimal distance of u
Amax_lat_accel = 2; %% Maximum Lateral acceleration both cars are heading towards each other during [0,rho]... This is assuming the things coming by 
Amin_lat_brake = 5; %% Minimum lateral brake acceleration both cars will do until they reach to 0 velocity or no collision...

VP = Rho*Amax_accel;
V1LatP= Rho*Amax_lat_accel;

%%MPC controller
nx = 10; % # of state variables
ny = 4; % # of output variables
nu = 2; % # of input variables [acceleration steeringangle]
nlobj = nlmpc(nx,ny,nu);

Zero weights are applied to one or more OVs because there are fewer MVs than OVs.

Ts = 0.01;
nlobj.Ts = Ts;
nlobj.PredictionHorizon = 10;
nlobj.ControlHorizon = 3;

nlobj.MV(1).Min = Amin_brake_correct;
nlobj.MV(1).Max = Amax_accel;
nlobj.MV(2).Min = -1.0472; %% degrees:-60 (radians:-1.0472 )
nlobj.MV(2).Max = 1.0472; %% degrees:60 (radians:1.0472 )

nlobj.Model.StateFcn = @(x,u) NonlinModel(x,u);
nlobj.Jacobian.StateFcn = @(x,u) NonlinModelJacobian(x,u);

nlobj.Model.OutputFcn = @(x,u) [x(3);x(8);x(9);x(10)];
nlobj.Jacobian.OutputFcn = @(x,u) [0 0 1 0 0 0 0 0 0 0;...
                                   0 0 0 0 0 0 0 1 0 0;...
                                   0 0 0 0 0 0 0 0 1 0;...
                                   0 0 0 0 0 0 0 0 0 1];

nlobj.Weights.OutputVariables = [1 1 1 1];
nlobj.Weights.ManipulatedVariables = [0.3 0.1];

x0 = [-12.4781 7.3388 20 0 0 -0.0226 0 155.4781 0.0612 -0.0226]; %% [X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3] % e1=Xd-X e2=Yd-Y e2=Yawd-Yaw 
u0 = [0 0];
ref0 = [0; 0; 0; 0].';
validateFcns(nlobj,x0,u0,{},{},ref0)

Error using validateFcns
Expected x to be an array with number of elements equal to 10.

Error in nlmpc/validateFcns (line 43)
validateattributes(x,{'double'},{'vector','real','finite','numel',nlobj.nx},FcnName,'x');

This is the new error...

%% nonlinear model code
function dxdt = NonlinModel(x,u)
%%[X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3]

m = 1600;
Iz = 3500;
lf = 3;
lr = 1.5;
Cf = 35000;
Cr = 30000;
T = 0.2;

Xd = 143;
Yd = 7;
Yawd = 0;

a1 = -(2*Cf+2*Cr)/m/x(3); 
a2 = -(2*Cf*lf-2*Cr*lr)/m/x(3) - x(3);
a3 = -(2*Cf*lf-2*Cr*lr)/Iz/x(3);
a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/x(3);
b1 = 2*Cf/m;
b2 = 2*Cf*lf/Iz;

dxdt = x;
dxdt(1) = x(3);                        %% X  
dxdt(2) = x(5);                        %% Y
dxdt(3) = x(7)*x(5)+x(4);              %% Vx ?
dxdt(4) = (1/T)*(-x(4) + u(1));      %% Vxdot ?
dxdt(5) = a1*x(5) + a2*x(7) + b1*u(2); %% Vy
dxdt(6) = x(7);                        %% Yaw
dxdt(7) = a3*x(5) + a4*x(7) + b2*u(2); %% Yawdot
dxdt(8) = Xd-sin(x(6));                 %% Error 1 X difference  Vxd-cos(x(6))
dxdt(9) = Yd+cos(x(6));                 %% Error 2 Y difference  Vyd-sin(x(6))
dxdt(10) = Yawd-x(6);                  %% Error 3 Yaw difference Yawdotd - x(6)
end

%% nonlinear jacobian model
function [A,B] = NonlinModelJacobian(x,u)
%% [X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3]

m = 1600;
Iz = 3500;
lf = 3;
lr = 1.5;
Cf = 35000;
Cr = 30000;
T = 0.2;

Xd = 143;
Yd = 7;
Yawd = 0;

a1 = -(2*Cf+2*Cr)/m/x(3); 
a2 = -(2*Cf*lf-2*Cr*lr)/m/x(3) - x(3);
a3 = -(2*Cf*lf-2*Cr*lr)/Iz/x(3);
a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/x(3);
b1 = 2*Cf/m;
b2 = 2*Cf*lf/Iz;

A = zeros(10,10);
A(1,3) = 1;

A(2,5) = 1;

A(3,4) = 1;
A(3,5) = x(7);
A(3,7) = x(5);

A(4,4) = -1/T;

A(5,3) = ((2*Cf+2*Cr)/m/(x(3)^2))*x(1) + (((2*Cf*lf-2*Cr*lr)/m/(x(3)^2))-1)*x(2);
A(5,5) = a1;
A(5,7) = a2;

A(7,3) = ((2*Cf*lf-2*Cr*lr)/Iz/(x(3)^2))*x(1) + ((2*Cf*lf^2+2*Cr*lr^2)/Iz/(x(3)^2))*x(2);
A(7,5) = a3;
A(7,7) = a4;

A(8,6) = -cos(x(6));

A(9,6) = -sin(x(6));

A(10,10) = -1;

B = zeros(10,2);
B(4,1) = 1/T;
B(5,2) = b1;
B(7,2) = b2;
end

Here is the simulink model...

[Torsten]

I think you didn't include the nonlinear constraint function in which the error occurs, did you ? Or is it internally generated and passed to "fmincon" ?

Again a case for Emmanouil Tzorakoleftherakis :-)

[Marshall Botta]

No, I dont think i have a nonlinear constraint function included except for the input constraint values, but the error is internally generated under the mask of the mpc block. I am kind of following the format of https://www.mathworks.com/help/mpc/ug/lane-following-using-nonlinear-model-predictive-control.html and it seems like this example doesnt have a constraint function too and when they validate the constraint function is verified for Matlabs example.

[Torsten]

So it's not possible to see or at least call the internally generated constraint function with your initial x0 and u0 ? Because according to the error message, it somehow returns Inf or NaN values.

But one error message pops up in your code from above: your x0 vector must have 10 instead of 9 elements.

[Marshall Botta]

I'll try right now, I am fairly new to internal code and how to use it.

[Marshall Botta]

I have also fixed the x0 vector, I forgot to add yaw error, thank you. Still same error report, going to be working on the internal code now.

[Torsten]

Using

x0 = [-12.4781 7.3388 20 0 0 -0.0226 0 155.4781 0.0612 -0.0226]; %% [X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3] % e1=Xd-X e2=Yd-Y e2=Yawd-Yaw 
u0 = [0 0];
ref0 = [0; 0; 0; 0].';
validateFcns(nlobj,x0,u0,{},{},ref0)

you will get a new error message about your Jacobian function.

[Marshall Botta]

This works, thank you! I have fixed my input jacobian matrix to...

B = zeros(10,2);
B(4,1) = 1/T;
B(5,2) = b1;
B(7,2) = b2;
end

Now just getting this warning from my Vydot with respect to Vx in the nonlinear model. As well as the same error from simulink with fmincon...

Zero weights are applied to one or more OVs because there are fewer MVs than OVs.
Model.StateFcn is OK.
Jacobian.StateFcn is OK.
Warning: Jacobian matrix with respect to "x" has maximum error = 8.4974 occurring at "(5,3)". 
> In ctrlMsgUtils.warning (line 25)
In nlmpc/validateFcns>compareJac (line 378)
In nlmpc/validateFcns (line 175)
In Initialize_constants (line 47) 
Model.OutputFcn is OK.
Jacobian.OutputFcn is OK.
Analysis of user-provided model, cost, and constraint functions complete.

[Torsten]

x=sym('x',[10 1]);
u = sym('u',[2 1]);
dxdt = NonlinModel(x,u);
jacobian(dxdt,x)

ans = 

jacobian(dxdt,u)

ans = 

%% nonlinear model code
function dxdt = NonlinModel(x,u)
%%[X Y Vx Vxdot Vy Yaw Yawdot e1 e2 e3]
m = 1600;
Iz = 3500;
lf = 3;
lr = 1.5;
Cf = 35000;
Cr = 30000;
T = 0.2;
Xd = 143;
Yd = 7;
Yawd = 0;
a1 = -(2*Cf+2*Cr)/m/x(3); 
a2 = -(2*Cf*lf-2*Cr*lr)/m/x(3) - x(3);
a3 = -(2*Cf*lf-2*Cr*lr)/Iz/x(3);
a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/x(3);
b1 = 2*Cf/m;
b2 = 2*Cf*lf/Iz;
dxdt = x;
dxdt(1) = x(3);                        %% X  
dxdt(2) = x(5);                        %% Y
dxdt(3) = x(7)*x(5)+x(4);              %% Vx ?
dxdt(4) = (1/T)*(-x(4) + u(1));      %% Vxdot ?
dxdt(5) = a1*x(5) + a2*x(7) + b1*u(2); %% Vy
dxdt(6) = x(7);                        %% Yaw
dxdt(7) = a3*x(5) + a4*x(7) + b2*u(2); %% Yawdot
dxdt(8) = Xd-sin(x(6));                 %% Error 1 X difference  Vxd-cos(x(6))
dxdt(9) = Yd+cos(x(6));                 %% Error 2 Y difference  Vyd-sin(x(6))
dxdt(10) = Yawd-x(6);                  %% Error 3 Yaw difference Yawdotd - x(6)
end

[Marshall Botta]

Ok I didn't even know you could solve jacobians like that. This works and its validated completely with no errors! Also in my simulink model the error was still occurring and then I directly fed vx, vy, yaw and yawdot data into the mux block rather than using gotos and froms and no more errors! The simulation is running im so happy!

Thank you so much for your time!

[Emmanouil Tzorakoleftherakis]

@Torsten looks like you have it covered :)