The MPC controller image range is incorrect

I built an MPC controller, the height of the trace path of the output is correct, but the range is not quite right, can you help me see if the MPC controller is wrong？Stop time is 250 seconds.The following is the code.

function [sys,x0,str,ts] = MPC_car1(t,x,u,flag)
switch flag
  case 0
    [sys,x0,str,ts]=mdlInitializeSizes;
  case 2
    sys=mdlUpdate(t,x,u);
  case 3
    sys=mdlOutputs(t,x,u);
  case {1,4,9}
    sys=[];
  otherwise
    error.(['Unhandled flag=',num2str(flag)]);

end

function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 3;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 6;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);
x0  = [0.0001,0.0001,0.0001]';
str = [];
ts  = [0.01 0];
global U
U=[0;0];

function sys=mdlUpdate(t,x,u)

sys = x;

function sys=mdlOutputs(t,x,u)
global A1 B1 u_piao U kesi
Nx=3;
Nu=2;
Np=100;
Nc=80;
Row=10;
T=0.01;
xo=u(1);
yo=u(2);
yaw=u(3);
x_ref=u(4);
y_ref=u(5);
yaw_ref=u(6);
vd1=15;
vd2=0;
L=2.6;
kesi=zeros(Nx+Nu,1);
kesi(1)=xo-x_ref;
kesi(2)=yo-y_ref;
kesi(3)=yaw-yaw_ref;
kesi(4)=U(1);
kesi(5)=U(2);
Q=eye(Nx*Np);
R=eye(Nc*Nu);
A1=[1 0 -vd1*T*sin(yaw);
    0 1 vd1*T*cos(yaw);
    0 0 1];
B1=[cos(yaw)*T 0;
    sin(yaw)*T 0;
    tan(vd2)*T/L vd1*T/L/(cos(vd2))^2];
A_cell=cell(2,2);
A_cell{1,1}=A1;
A_cell{1,2}=B1;
A_cell{2,1}=zeros(Nu,Nx);
A_cell{2,2}=eye(Nu);
A=cell2mat(A_cell);
B_cell=cell(2,1);
B_cell{1,1}=B1;
B_cell{2,1}=eye(Nu);
B=cell2mat(B_cell);
C_cell=cell(1,2);
C_cell{1,1}=eye(Nx);
C_cell{1,2}=zeros(Nx,Nu);
C=cell2mat(C_cell);
PHI_cell=cell(Np,1);
for i=1:1:Np
    PHI_cell{i,1}=C*A^i;
end
PHI=cell2mat(PHI_cell);
THETA_cell=cell(Np,Nc);
for i=1:1:Np
    for j=1:1:Nc
        if  i>=j
            THETA_cell{i,j}=C*A^(i-j)*B;
        else
            THETA_cell{i,j}=zeros(3,2);
        end
    end
end
THETA=cell2mat(THETA_cell);
H_cell=cell(2,2);
H_cell{1,1}=THETA'*Q*THETA+R;
H_cell{1,2}=zeros(Nc*Nu,1);
H_cell{2,1}=zeros(1,Nc*Nu);
H_cell{2,2}=Row;
H=cell2mat(H_cell);
error=PHI*kesi;
f_cell=cell(2,1);
f_cell{1,1}=2*error'*Q*THETA;
f_cell{1,2}=0;
f=cell2mat(f_cell);
A_t=zeros(Nc,Nc);
for i=1:1:Nc
    for j=1:1:Nc
        if i>=j
            A_t(i,j)=1;
        else
            A_t(i,j)=0;
        end
    end
end
A_I=kron(A_t,eye(Nu));
Ut=kron(ones(Nc,1),U);
umax=[0.2;0.44];
umin=[-0.2;-0.44];
umax_dt=[0.05;0.005];
umin_dt=[-0.05;-0.005];
Umax=kron(ones(Nc,1),umax);
Umin=kron(ones(Nc,1),umin);
Umax_dt=kron(ones(Nc,1),umax_dt);
Umin_dt=kron(ones(Nc,1),umin_dt);
A_cons_cell=cell(2,2);
A_cons_cell{1,1}=A_I;
A_cons_cell{1,2}=zeros(Nu*Nc,1);
A_cons_cell{2,1}=-A_I;
A_cons_cell{2,2}=zeros(Nu*Nc,1);
A_cons=cell2mat(A_cons_cell);
B_cons_cell=cell(2,1);
B_cons_cell{1,1}=Umax-Ut;
B_cons_cell{2,1}=-Umin+Ut;
B_cons=cell2mat(B_cons_cell);
lb=[Umin_dt;0];
ub=[Umax_dt;Row];
options=optimset('Algorithm','interior-point-convex');
[X,fval,exitflag] =quadprog(H,f,A_cons,B_cons,[],[],lb,ub,[],options);
u_piao(1)=X(1);
u_piao(2)=X(2);
U(1)=kesi(4)+u_piao(1);
U(2)=kesi(5)+u_piao(2);
u_real(1)=U(1)+vd1;
u_real(2)=U(2)+vd2;

sys = u_real;

[Sam Chak]

Yours is a manually-built MPC using quadprog()?

Not sure how to check the custom MPC algortihm because it is written in S-function.