%參數定義
dt=0.1;?%循環的時間，每次加0.1
Ld=1.9;?%車輛的前后輪軸距
k=0.7;??%純追蹤公式里的參數K
L=4;????%目標點的預瞄距離L
T=0.0;??%起始時間0
%目標軌跡??就是那個跟蹤的軌跡，用公式表達出來
a=10;
x1=a+linspace（-aa）;
y1=3./（1.0+exp（-0.5*（x1-a）））+1.5;
%plot（x1y1‘.‘）;

%初始化車輛狀態
idx=0;
x?=?0.0;????%車輛起始位置坐標
y?=?1.5;????%車輛起始位置坐標
yaw?=?0.0;??%車輛航向角0度，即x軸正方向
v?=?20/3.6;?%車輛的速度
X=x;
Y=y;
Yaw=yaw;
V=v;
%&&x<=40&&y<=4.5
while（T<=4）??%這個循環是用于設置整個程序跑的時間的，用來跟蹤目標路徑
????
?T=T+dt;
%更新目標點（預瞄距離）
i=100;
%D=norm（[x-x1（1i）?y-y1（1i）]）;
while（1）??%這個循環是用來更新目標點的，不斷計算當前點和目標路徑間的距離，知道為L時為目標點
????D=norm（[x-x1（1i）?y-y1（1i）]）;??%計算兩個點之間的直線距離函數（內部的）
????i=i-1;
????if（i<1）
????????i=1;
????????break;
????end
????
????if（D<=L）
????????idx=i;
????????break;
????end
end
??
%Pure?Pursuit純追蹤模型
alpha?=?atan2（y1（1i）?-?y?x1（1i）?-?x）?-?yaw;??%atan2函數計算點（xy）與x軸正向夾角即atan2（yx）.
%Lf=?k?*?v?+?L;
Lf=?k?*?v;
delta?=?atan2（2.0?*?Ld?*?sin（alpha）?/?Lf?1.0）;

%更新車輛狀態
x?=?x?+?v?*?cos（yaw）?*?dt;
y?=?y?+?v?*?sin（yaw）?*?dt;
yaw?=?yaw?+?