N=100;
T=0.05;
Xout=zeros（N3）;
Tout=zeros（N1）;
for?k=1:1:N
????Xout（k1）=k*T;
????Xout（k2）=2;
????Xout（k3）=0;
????Tout（k1）=（k-1）*T;
end

Nx=3;
Nu=2;
[NrNc]=size（Xout）;
Tsim=30;
X0=[0?0?pi/3];
L=1;
vd1=1;
vd2=0;


x_real=zeros（NrNc）;
x_piao=zeros（NrNc）;
u_real=zeros（NrNu）;
u_piao=zeros（NrNu）;
x_real（1:）=X0;
x_piao（1:）=x_real（1:）-Xout（1:）;
X_PIAO=zeros（NrNx*Tsim）;
XXX=zeros（NrNx*Tsim）;
q=[1?0?0;0?1?0;0?0?0.5];
Q_cell=cell（TsimTsim）;
for?i=1:1:Tsim
????for?j=1:1:Tsim
????????if?i==j
????????????Q_cell{ij}=q;
?????????else
????????????Q_cell{ij}=zeros（NxNx）;
????????end
????end
end
Q=cell2mat（Q_cell）;
R=0.1*eye（Nu*TsimNu*Tsim）;


for?i=1:1:Nr
????t_d=Xout（i3）;
????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;
???????vd2*T/L??vd1*T/（cos（vd2）^2）;];
????A_cell=cell（Tsim1）;
????B_cell=cell（TsimTsim）;
??for?j=1:1:Tsim
??????A_cell{j1}=a^j;
??????for?k=1:1:Tsim
??????????if?k<=j
??????????????B_cell{jk}=（a^（j-k））*b;
??????????else????????
??????????????
????????????B_cell{jk}=zeros（NxNu）;
??????????end?
??????end
??end
??A=cell2mat（A_cell）;
??B=cell2mat（B_cell）;
??
??H=2*（B‘*Q*B+R）;
??f=2*B‘*Q*A*x_piao（i:）‘;
??A_cons=[];
??b_cons=[];
??lb=[-2.2;-0.64];
??ub=[0.2;0.64];
??[Xfval（i1）exitflag（i1）output（i1）]=quadprog（HfA_consb_cons[][]lbub）;
??X_PIAO（i:）=（A*x_piao（i:）‘+B*X）‘;
??if?i+j??????for?j=1:1:Tsim
???XXX（i1+3*（j-1））=X_PIAO（i1+3*（j-1））+Xout（i+j1）;
???XXX（i2+3*（j-1））=X_PIAO（i2+3*（j-1））+Xout（i+j2）;
???XXX（i3+3*（j-1））=X_PIAO（i3+3*（j-1））+Xout（i+j3）;
??????end
???else
??????for?j=1:1:Tsim
????XXX（i1+3*（j-1））=X_PIAO（i1+3*（j-1））+Xout（Nr1）;
????XXX（i2+3*（j-1））=X_PIAO（i2+3*（j-1））+Xout（Nr2）;
????XXX（i3+3*（j-1））=X_PIAO（i3+3*（j-1））+Xout（Nr3）;
??????end
??end