clc;
clear;
close?all;

%?參數定義
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）;???????????????%?產生0~20?均布的線性矢量
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;

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

????%?純追蹤模型
????alpha?=?atan2（y1（1i）?-?y?x1（1i）?-?x）?-?yaw;??%?atan2函數計算點（xy）與x軸正向夾角即atan2（yx）.
????Lf=?k?*?v;
????delt