function?pursuit
clc;clear;close?all
%預瞄
global?dt;dt?=?0.1;??%?[s]
global?L;L?=?2.9;??%?[m]?wheel?base?of?vehicle
%main
%??target?course
%?cx?=?0:0.1:50;%
%?cy?=?sin（cx/5.0）.*?cx/2.0?;

%?t=0:pi/200:2*pi;
%?cx?=?10*cos（t）;
%?cy?=?10*sin（t）?;
%?plot（cxcy）
cx=0:0.005:600;
for?i=1:length（cx）
if??（cx（i）>=200?&&?cx（i）<=321）
????cy（i）=0;
elseif?（cx（i）>=363?&&?cx（i）<=398）
?????cy（i）=3.5;
elseif?（cx（i）>=433）?
????cy（i）=0;
elseif?（cx（i）>321?&&?cx（i）<363）
????cy（i）=3.5/（363-321）*cx（i）-3.5/（363-321）*321;
elseif?（cx（i）>398?&&?cx（i）<433）
????cy（i）=-3.5/（433-398）*cx（i）+43.3;
end
end
?plot（cx?cy?‘.r‘）;hold?on
target_speed?=?10.0?/?3.6;?%?[m/s]
T?=?1000.0;???%?max?simulation?time
%initial?state
state?=?[11.00.00.00.0];???%[xyyawv]為什么是-3
lastIndex?=?length（cx）;
time?=?0.0;
x?=?state（1）;
y?=?state（2）;
yaw?=?state（3）;
v?=?state（4）;
t?=?0.0;
target_ind?=?calc_target_index（state?cx?cy）;
%%?main?loop
while?（T?>=?time）?&&?（lastIndex?>?target_ind）
????ai?=?PIDControl（target_speed?state（4））;%獲得ai需要（）中的兩個內容
????[di?target_ind]?=?pure_pursuit_control（state?cx?cy?target_ind）;
????state?=?update（state?ai?di）;
????time?=?time?+?dt;
????x=[xstate（1）];
????y=[ystate（2）];
????yaw=[yawstate（3）];
????v=[vstate（4）];
????t=[ttime];
end
%?Test
%?if?show_animation
%?????plot（cx?cy?“.r“?label=“course“）
%?????plot（x?y?“-b“?label=“trajectory“）
%?????plot（cx[target_ind]?cy[target_ind]?“xg“?label=“target“）
%?????axis（“equal“）
%?????grid（True）
%?????plt.title（“Speed[km/h]:“?+?str（state.v?*?3.6）[:4]）
%?????plt.pause（0.001）
%?end
plot（x?y?‘b‘）
legend（‘courser‘?‘trajectory‘）
title（‘預瞄跟蹤算法‘）
hold?on;
end
%%?sub?function
%預瞄尋找距離參考軌跡中最近一點
function?target_ind?=?calc_target_index（state?cx?cy）
%?search?nearest?point?index
%%[xy