clc;
clear;

%%
%參考軌跡生成
N?=?100;???????%參考軌跡點數量
T?=?0.05;??????%采樣周期
Xout?=?zeros（N3）;
Tout?=?zeros（N1）;
for?k?=?1:1:N
????Xout（k1）?=?k*T;????????%參考x坐標
????Xout（k2）?=?2;??????????%參考y坐標
????Xout（k3）?=?0;??????????%參考航向角
????Tout（k1）?=?（k-1）*T;????%第k個參考點時刻
end
%%
%控制系統基本情況介紹
Nx?=?3;????????%狀態量個數
Nu?=?2;????????%控制量個數
[NrNc]?=?size（Xout）;????%Nr=100Nc=3
Tsim?=?20;?????%仿真時間
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）;???????????%目標函數中Q矩陣
R?=?0.1?*?eye（Nu*TsimNu*Tsim）;?%目標函數中R矩陣
%%
%模型預測控制的主體，在每一周期開始，獲取系統當前狀態量，更新狀態空間方程。
%求解一個標準二次規劃問題。將所得第一個解施加到系統上。
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;];???????????????%狀態空間中的矩陣A
????b?=?[cos（t_d）*T?0;
?????????sin（t_d）*T?0;
?????????vd2*T/L?vd1*T/（cos（vd2）^2）;];?%狀態空間中的矩陣B
????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）;???%預測模型中A矩陣
????B?=?cell2mat（B_cell）;???%預測模型中B矩陣
????
????H?=?2?*?（B‘?*?Q?*?B?+?R）;
????f?=?2?*?B‘?*?Q?*?A?*?x_piao（i:）‘;
????A_cons?=?[];
????b_cons?=?[];
????lb?=?[-2.3;?-0.64];???????%控制量下界
????ub?=?[0.3?;?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（