检测对生成出的期望轨迹的跟踪效果，期望轨迹为连续变道的蛇形轨迹：

clc
clear all

 load('D:\ccs\DaischTest\CurveCurve.mat')
% load('D:\ccs\DaischTest\TraceC10.mat')
 spacelimit = 1;    
 start =10;
 finish =1;
x= PosLat(start : spacelimit:length(PosLat)-finish);
y = PosLon(start: spacelimit:length(PosLat)-finish);
ha = AngleHeading(start: spacelimit:length(PosLat)-finish);
time = Time(start: spacelimit:length(PosLat)-finish);

% sw = SAS_SteerWheelAngle(start: spacelimit:length(PosLat)-finish);

x = roundn(x, -8);
y =  roundn(y,-8);
B0 = x(1);
L0 = y(1);
HA= roundn(ha, -2);
% SW = vpa(sw,3);
% plot(x,y, 'LineWidth',10)

%%

for  i = 1: length(x)
     [X(i),Y(i)]=transformation1(x(i),y(i),B0,L0);
     i
end

X = X';
Y = Y';

%%
figure 
for i = 1: 1:length(X)
% plot(X(3:i),Y(3:i),'-o')
% i
plot(X(i), Y(i), 'bo')
hold on

%  drawnow
end

%%
 load('D:\ccs\DaischTest\CurveCurveRecord.mat')
 spacelimit = 1;    
 start =1;
 finish =1;
x= PosLat(start : spacelimit:length(PosLat)-finish);
y = PosLon(start: spacelimit:length(PosLat)-finish);

 for  i = 1: length(x)
     [XT(i),YT(i)]=transformation1(x(i),y(i),B0,L0);
     i
end
 plot(XT,YT,'r*')

在车辆20kph的时速下，运行效果如图：

细节部分：

其中蓝色部分为期望轨迹，红色部分为真实的运动轨迹。