function[sys,x0,str,ts]=MY_MPCController(t,x,u,flag)

switch flag,

case 0

[sys,x0,str,ts]=mdlInitializeSizes;

case 2

sys=mdlUpdates(t,x,u);

case 3

sys=mdlOutputs(t,x,u);

case {1,4,9}

sys=[];

otherwise

error(['unhandled flag=',num2str(flag)]);

end

%s函数主程序结束

function[sys,x0,str,ts]=mdlInitializeSizes

sizes=simsizes;

sizes.NumContStates=0;

sizes.NumDiscStates=3;

sizes.NumOutputs=2;

sizes.NumInputs=3;

sizes.DirFeedthrough=1;

sizes.NumSampleTimes=1;

sys=simsizes(sizes);

x0=[0;0;0];

global U;

U=[0;0];

str=[];

ts=[0.05 0];

%初始化子函数结束

function sys=mdlUpdates(t,x,u)

sys=x;

%end of mdlUpdate.

function sys=mdlOutputs(t,x,u)

global a b u_piao;

global U;

global kesi;

Nx=3;%状态量的个数

Nu=2;%控制量的个数

Np=60;%预测步长

Nc=30;%控制步长

Row=10;

fprintf('Update start,t=%6.3f\n',t)

t_d=u(3)*3.14159265/180;%角度和弧度的转化

r(1)=25*sin(0.2*t);

r(2)=25+10-25*cos(0.2*t);

r(3)=0.2*t;

vd1=5;

vd2=0.104;

%半径为25m的圆形轨迹，速度为5m

%参数的设计

kesi=zeros(Nx+Nu,1);

kesi(1)=u(1)-r(1);

kesi(2)=u(2)-r(2);

kesi(3)=t_d-r(3);

kesi(4)=U(1);

kesi(5)=U(2);

fprintf('Update start,u(1)=%4.2f\n',U(1))

fprintf('Update start,u(2)=%4.2f\n',U(2))

T=0.05;

T_all=40;

L=2.6;

%Mobile Robor/Vehicle variable

u_piao=zeros(Nx,Nu);

Q=eye(Nx*Np,Nx*Np);

R=5*eye(Nu*Nc);

a=[1 0 -vd1*sin(t_d)*T;0 1 vd1*cos(t_d)*T;0 0 1;];

b=[cos(t_d)*T 0;sin(t_d)*T 0;tan(vd2)*T/L vdl*T/((cos(vd2)^2)*L);];

%对应4.6

A_cell=cell(2,2);

B_cell=cell(2,1);

A_cell{1,1}=a;

A_cell{1,2}=b;

A_cell{2,1}=zeros(Nu,Nx);

A_cell{2,2}=eye(Nu);

B_cell{1,1}=b;

B_cell{2,1}=eye(Nu);

A_cell2mat(A_cell);

B_cell2mat(B_cell);

C=[1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;];

%对应4.10

PHI_cell=cell(Np,1);

THETA_cell=cell(Np,Nc);

for j=1:1:Np

PHI_cell{j,1}=C*A^j;

for k=1:1:Nc

if k<=j

THETA_cell{j,k}=C*A^(j-k)*B;

else

THETA_cell{j,k}=zeros(Nx,Nu);

end

end

end

PHI=cell2mat(PHI_cell);

THETA=cell2mat(THETA_cell);

%对应4.12

H_cell=cell(2,2);

H_cell{1,1}=THETA'*Q*THETA+R;

H_cell{1,2}=zeros(Nu*Nc,1);

H_cell{2,1}=zeros(1,Nu*Nc);

H_cell{2,2}=0;

H=cell2mat(H_cell);

error=PHI*kesi;

f_cell=cell(1,2);

f_cell{1,1}=2*error'*Q*THETA;

f_cell{1,2}=Row;

f=cell2mat(f_cell);

%对应4.19

A_t=zeros(Nc,Nc);

for p=1:1:Nc

for q=1:1:Nc

if q<=p

A_t(p,q)=1;

else

A_t(p,q)=0;

end

end

end

A_I=kron(A_t,eye(Nu));

%对应4.17

Ut=kron(one(Nc,1),U);

umin=[-0.2;-0.54;];

umax=[0.2;0;0.332];

delta_umin=[-0.05;-0.0082;];

delta_umax=[0.05;0.0082];

Umin=kron(one(Nc,1),umin);

Umax=kron(one(Nc,1),umax);

A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1)};

b_cons_cell={Umax-Ut;-Umin+Ut};

A_cons=cell2mat(A_cons_cell);

%状态量不等式约束增益

b_cons=cell2mat(b_cons_cell);

M=10;

delta_Umin=kron(ones(Nc,1),delta_umin);

delta_Umax=kron(ones(Nc,1),delta_umax);

lb=[delta_Umin;0];

ub=[delta_Umax;M];

%开始求解

options=optimset('Algorithm','active-set');

%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;

toc

%End of mdlOutputs.

这是我的程序