%      PlatoonNonlin.M
%
%   Vehicle platoon with nonlinear vehicles
%
% J. Lunze
% 9.10.2018
%
% Version of 14.1.2019
% for 2nd edition: 2.8.2021
% 
close all
clear all

%   Parameters
%
Ts=0.04;
T1=0.3;   % time constant of power train
alpha=3;
beta=2;

%%      Vehicle with proportional distance controller
%     from PlatoonDemo.m, figure(7)
%    
figure(1)
%    Vehicle model
TendV=10;
TimeV=0:Ts:TendV;
kendV=length(TimeV)-1;
AVeh=[  0       1      0;      % x_1=v_i
        0     -1/T1    0;      % x_2=a_i
        -1      0      0 ];    % x_3=d_i
BVeh=[0; 1/T1; 0];
EVeh=[0; 0; 1];         % input matrix w.r.t. coupling signal v_{i-1}
Cz1=[1 0 0];                     % output= velocity
C1=[0 0 1];                     % output= distance dtilde
n=3;
Veh=ss(AVeh, BVeh, Cz1, 0);
Vehv=ss(AVeh, EVeh, Cz1, 0);
%    Discrete-time models
Vehd=c2d(Veh, Ts, 'zoh');
AVehd=Vehd.A;
BVehd=Vehd.B;
Vehvd=c2d(Vehv, Ts, 'zoh');
EVehd=Vehvd.B;
%     Distance controller:  u_i = kd(beta*v_i - dtilde_i + alpha)
%                               = kd(beta*x_1 - x_3 + alpha)
kd=-1;    % controller parameter
%     Impulse response = initial response for the initial state x0
x0=[0; 0; 1+alpha];
Y=[];
Y(1,:)=[Cz1*x0 C1*x0];  %  velocity; distance
xold=x0;
for k1=1:kendV
    u=kd*(beta*xold(1,1) - xold(3,1) + alpha);
    xnew=AVehd*xold + BVehd*u;
    Y(k1+1,:)=[Cz1*xnew C1*xnew-alpha];
    xold=xnew;
end
%
subplot(4,1,[1 2]);
plot(TimeV, Y(:,1), 'b', TimeV, Y(:,2), 'b--', [0 TendV], [0 0], 'k:');
latexylabel('$$\bar{g}, \bar{f}$$');
rotateY;
axis([0 TendV -0.2 1.05]);
yticks([0 0.5 1]);
xticks([0 5 10]);
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonNonlin:}$$ Vehicle with PD distance controller');
latextext(8, 0.7, ['$$T=$$', num2str(T1)]);
latextext(8, 0.45, ['$$\beta=$$', num2str(sum(Y(:,2))*Ts,'%.3g')]);

%%  Vehicle string with ACC without braking limitations
%
figure(2)
%  Velocity of the leader
Tend=40;
Time=0:Ts:Tend;
kend=length(Time);
brakemax=1.1*9.81;   % m/s^2    max. braking for the leader
time1=10;
kstart=ceil(time1/Ts);
v0=120/3.6;   % initial velocity  80 km/h
V0=v0*ones(kend,1);
U0=zeros(kend,1);
Limit0=-ones(kend,1);
k1=kstart;
while V0(k1,1)>brakemax*Ts
    k1=k1+1;
    V0(k1,1)=v0-(k1-kstart)*Ts*brakemax;
    U0(k1,1)=-brakemax;
    Limit0(k1,1)=0;
end
for k2=k1:kend
    V0(k2,1)=0;
end
subplot(5,1,[1,2]);
plot(Time, V0, 'b', [0 Tend], [0 0], 'k:');
hold on
subplot(5,1,[3,4])
plot(Time, U0, 'b', [0 Tend], [0 0], 'k:');
hold on
%   Follower 1
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];     % initial distance: alpha+beta*v0
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    U(k1+1,1)=u;
    xnew=AVehd*xold + BVehd*u + EVehd*V0(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V1=Y(:,1);
D10=Y(:,2);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,[3,4])
plot(Time, U(:,1), 'b');
hold on
%   Follower 2
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    U(k1+1,1)=u;
    xnew=AVehd*xold + BVehd*u + EVehd*V1(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V2=Y(:,1);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,[3,4])
plot(Time, U(:,1), 'b');
%   Follower 3
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    U(k1+1,1)=u;
    xnew=AVehd*xold + BVehd*u + EVehd*V2(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V3=Y(:,1);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,[3,4])
plot(Time, U(:,1), 'b');
%   Follower 4
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    U(k1+1,1)=u;
    xnew=AVehd*xold + BVehd*u + EVehd*V3(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V4=Y(:,1);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,[3,4])
plot(Time, U(:,1), 'b');
%   Follower 5
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    U(k1+1,1)=u;
    xnew=AVehd*xold + BVehd*u + EVehd*V1(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
axis([0 Tend -2 40]);
yticks([0 20 40]);
xleer;
xticksempty([0 20 40 60 80 100]);
latextitle('$${\tt PlatoonNonlin:}$$ Platoon without braking limitation');
hold off
%
subplot(5,1,[3,4])
plot(Time, U(:,1), 'b');
plot([0 Tend], [-brakemax -brakemax], 'b--');
latexylabel('$$u_i$$ in $$\frac{\rm m}{\rm s^2}\;$$');
rotateY;
axis([0 Tend -12 0.5]);
yticks([-10 -5 0]);
xticks([0 20 40 60 80 100]);
latexxlabel('$$t$$ in s');
latextext(30,-9,'$$a_{0 {\rm max}}$$');
hold off

%%  Vehicle string with ACC, braking acceleration limitation
%
figure(3)
brakemax1=0.75*brakemax;
%  Leader behaviour
subplot(5,1,[1,2]);
plot(Time, V0, 'b', [0 Tend], [0 0], 'k:');
hold on
subplot(5,1,3);
plot(Time, Limit0, 'bo');
axis([0 Tend 0 5]);
hold on
subplot(5,1,[4,5]);
plot(Time, U0, 'b', [0 Tend], [0 0], 'k:');
hold on
%   Follower 1
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];     % initial distance: alpha+beta*v0
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    if  u<=-brakemax1    % reduced brake limitation
        u=-brakemax1;
        Limit(k1,1)=1;   
    end
    U(k1+1,1)=u;
    xnew=AVehd*xold + BVehd*u + EVehd*V0(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V1=Y(:,1);
D1=Y(:,2);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,3);
plot(Time, Limit, 'bo');
subplot(5,1,[4,5])
plot(Time, U(:,1), 'b');
%   Follower 2
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    if abs(u)>brakemax
        u=-brakemax;
        Limit(k1,1)=2;     % brake limited
    end
    U(k1+1,1)=u;
    xnew=AVehd*xold + BVehd*u + EVehd*V1(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V2=Y(:,1);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,3);
plot(Time, Limit, 'bo');
subplot(5,1,[4, 5])
%plot(Time, Y(:,2), 'b');
%   Follower 3
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    if abs(u)>brakemax
        u=-brakemax;
        Limit(k1,1)=3;     % brake limited
    end
    xnew=AVehd*xold + BVehd*u + EVehd*V2(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V3=Y(:,1);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,3);
plot(Time, Limit, 'bo');
subplot(5,1,[4, 5])
%plot(Time, Y(:,2), 'b');
%   Follower 4
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    if abs(u)>brakemax
        u=-brakemax;
        Limit(k1,1)=4;     % brake limited
    end
    xnew=AVehd*xold + BVehd*u + EVehd*V3(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
V4=Y(:,1);
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
subplot(5,1,3);
plot(Time, Limit, 'bo');
subplot(5,1,[4, 5])
%plot(Time, Y(:,2), 'b');
%   Follower 5
Y=[];
U=[];
Limit=-ones(kend,1);
Y(1,:)=[v0, alpha+beta*v0];   %  v_1  d_1
x0=[v0; 0; alpha+beta*v0];
xold=x0;
for k1=1:kend-1
    u=kd*(beta*xold(1,1) - xold(3,1)+alpha);
    if abs(u)>brakemax
        u=-brakemax;
        Limit(k1,1)=5;     % brake limited
    end
    xnew=AVehd*xold + BVehd*u + EVehd*V1(k1,1);
    Y(k1+1,:)=[Cz1*xnew C1*xnew];
    xold=xnew;
end
subplot(5,1,[1,2]);
plot(Time, Y(:,1), 'b');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}\;$$');
rotateY;
axis([0 Tend -2 40]);
yticks([0 20 40]);
xleer;
xticksempty([0 20 40 60 80 100]);
latextitle('$${\tt PlatoonNonlin:}$$ Platoon with identical vehicles');
hold off
%
subplot(5,1,3);
plot(Time, Limit, 'bo');
latexylabel('$$i\;$$');
rotateY;
yticks([0 5]);
xleer;
xticksempty([0 20 40 60 80 100]);
hold off
%
subplot(5,1,[4, 5])
%plot(Time, U(:,2), 'b');
plot([0 Tend], [-brakemax1 -brakemax1], 'b--');
plot([0 Tend], [-brakemax -brakemax], 'b--');
latexylabel('$$\tilde{u}_0, \tilde{u}_1$$ in $$\frac{\rm m}{\rm s^2}\;$$');
rotateY;
axis([0 Tend -12 0.5]);
yticks([-10 -5 0]);
xticks([0 20 40 60 80 100]);
latexxlabel('$$t$$ in s');
latextext(30,-6,'$$a_{1 {\rm max}}$$');
latextext(30,-9.5,'$$a_{0 {\rm max}}$$');
hold off


%%
figure(4)
plot(Time, D1, 'b', Time, D10, 'b--')

%%  Determination of the time-headway coefficient for vehicles with weak brakes
%
figure(5)
subplot(4,1,[1,2]);
%    Vehicle model
beta1=2.0;
%     step response for different velocity steps
x0=[0; 0; alpha];
for vref=[1, 50/3.6, 70/3.6, 100/3.6, 120/3.6]
    Y=[];
    Y(1,:)=0;  %  velocity
    xold=x0;
    for k1=1:kendV
        u=kd*(beta1*xold(1,1) - xold(3,1) + alpha);
        if abs(u)>brakemax1
            u=brakemax1;       % positive sign for acceleration
            Limit(k1,1)=1;     % brake limited
        end
        xnew=AVehd*xold + BVehd*u + EVehd*vref;
        Y(k1+1,1)=Cz1*xnew;
        xold=xnew;
    end
    %
    plot(TimeV, Y(:,1)/vref, 'b');
    hold on
end
plot([0 TendV], [1 1], 'k:');
latexylabel('$$\bar{h}\;$$');
rotateY;
axis([0 TendV 0 1.4]);
yticks([0 0.5 1]);
xticksempty([0 5 10 15 20]);
xleer;
latextitle('$${\tt PlatoonNonlin:}$$ Vehicle with limited brake');
latextext(8, 0.7, ['$$\beta=$$', num2str(beta1)]);
hold off
%
subplot(4,1,[3,4])
beta1=2.9;
%     step response for different velocity steps
for vref=[1, 50/3.6, 70/3.6, 100/3.6, 120/3.6]
    Y=[];
    Y(1,:)=0;  %  velocity
    xold=x0;
    for k1=1:kendV
        U=kd*(beta1*xold(1,1) - xold(3,1) + alpha);
        if abs(U)>brakemax1
            U=brakemax1;       % positive sign for acceleration
            Limit(k1,1)=1;     % brake limited
        end
        xnew=AVehd*xold + BVehd*U + EVehd*vref;
        Y(k1+1,1)=Cz1*xnew;
        xold=xnew;
    end
    %
    plot(TimeV, Y(:,1)/vref, 'b');
    hold on
end
plot([0 TendV], [1 1], 'k:');
latexylabel('$$\bar{h}\;$$');
rotateY;
axis([0 TendV 0 1.4]);
yticks([0 0.5 1]);
xticks([0 5 10 15 20]);
latexxlabel('$$t$$ in s');
latextext(8, 0.7, ['$$\beta=$$', num2str(beta1)]);
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   CACC with time delay
%    Model of the platoon from PlatoonDemo.m, figure(13)
%    Behaviour of the platoon without time delay
%
figure(6)
%
N1=6;     % Vehicles 0,1,..., 5
beta=2;
D0=2;   % individual delays
D1=3;
D2=2.5;
D3=3;
D4=2.5;
D5=2;      % for D5>2 the information has to come from vehicles 2 and 3
%   Leader modelled as PT1, but with order n=3 as all vehicles
CVeh=[0 0 1;    % output=distance d_i; for Vehicle0: output=position
      1 0 0];   % output = velocity v_i
DVeh=[0; 0];
beta0=2;
A0=[-1/D0 0 0; 0 0 1; 1 0 0];
B0=[1/D0; 0; 0];
Veh0=ss(A0, B0, CVeh, DVeh);
beta1=3;
T1=0.7;           % 0.4  time constant of vehicle
kv1=1/D1;
A1=[0 1 0; -kv1/T1 -1/T1 0; 1 0 0];
B1=[0; kv1/T1; 0];
Veh1=ss(A1, B1, CVeh, DVeh);
beta2=2;
kv2=1/D2;
A2=[0 1 0; -kv2/T1 -1/T1 0; 1 0 0];
B2=[0; kv2/T1; 0];
Veh2=ss(A2, B2, CVeh, DVeh);
beta3=2;
kv3=1/D3;
A3=[0 1 0; -kv3/T1 -1/T1 0; 1 0 0];
B3=[0; kv3/T1; 0];
Veh3=ss(A3, B3, CVeh, DVeh);
beta4=2;
kv4=1/D4;
A4=[0 1 0; -kv4/T1 -1/T1 0; 1 0 0];
B4=[0; kv4/T1; 0];
Veh4=ss(A4, B4, CVeh, DVeh);
beta5=2;
kv5=1/D5;
A5=[0 1 0; -kv5/T1 -1/T1 0; 1 0 0];
B5=[0; kv5/T1; 0];
Veh5=ss(A5, B5, CVeh, DVeh);
n=3;
%
v0=0;
si=alpha+beta*v0;   % initial spacing
s1=alpha+beta1*v0;
x00=[v0; 0; 0]; 
x10=[v0; 0; -s1]; 
x20=[v0; 0; -s1-si]; 
x30=[v0; 0; -s1-2*si];
x40=[v0; 0; -s1-3*si];
x50=[v0; 0; -s1-4*si];
%   Determination of the weighting factors
a21=(beta1+beta2-D2)/beta1;
a20=1-a21;
a32=(beta2+beta3-D3)/beta2;
a31=1-a32;
a43=(beta3+beta4-D4)/beta3;
a42=1-a43;
a54=(beta4+beta5-D5)/beta4;
a53=1-a54;
%  Velocity command for the leading vehicle
dt=0.04;
Tv0=0;
Tv1=3;
Tv2=12;
Tv3=20;
Tend=45;
k0e=1; 
k1e=ceil(Tv1-Tv0)/dt;
k2e=k1e+ceil(Tv2-Tv1)/dt;
k3e=k2e+ceil(Tv3-Tv2)/dt;
k4e=k3e+ceil(Tend-Tv3)/dt;
v1=20;    % velocity in m/s
W=v0+v1*[0*ones(k1e-k0e,1);
         0.9*ones(k2e-k1e,1); 
         0.3*ones(k3e-k2e,1); 
         0.6*ones(k4e-k3e+1,1)];
TimeW=0:dt:(k4e-1)*dt;
%   Determination of the vehicle outputs
Y0=lsim(Veh0, W, TimeW, x00);
W1=Y0(:,2);
Y1=lsim(Veh1, W1, TimeW, x10);
Dist1=Y0(:,1)-Y1(:,1);
W2=a20*Y0(:,2)+a21*Y1(:,2);
Y2=lsim(Veh2, W2, TimeW, x20);
Dist2=Y1(:,1)-Y2(:,1);
W3=a31*Y1(:,2)+a32*Y2(:,2);
Y3=lsim(Veh3, W3, TimeW, x30);
Dist3=Y2(:,1)-Y3(:,1);
W4=a42*Y2(:,2)+a43*Y3(:,2);
Y4=lsim(Veh4, W4, TimeW, x40);
Dist4=Y3(:,1)-Y4(:,1);
W5=a53*Y3(:,2)+a54*Y4(:,2);
Y5=lsim(Veh5, W5, TimeW, x50);
Dist5=Y4(:,1)-Y5(:,1);
%
subplot(4,1,[1 2])
plot(TimeW, [Y0(:,2) Y1(:,2) Y2(:,2) Y3(:,2) Y4(:,2) Y5(:,2)],'b');
hold on
plot([-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
plot(TimeW, W,'b--'); % command to leading vehicle
axis([-1 Tend -2 v1*1.1]);
latexylabel('$$v_i\;$$');  %   in m/s
rotateY;
xleer;
yticks([0 10 20]);
xticksempty([0 10 20 30 40]);
%latexxlabel('$$t$$');
latextitle('$${\tt PlatoonNonlin:}$$ CACC without time delay');
hold off;
%
subplot(4,1,[3 4])
plot(TimeW, [Dist2 Dist3 Dist4 Dist5],'b');
hold on
plot(TimeW, Dist1,'b--');
plot([0 0],[-100 100],'k:');
axis([-1 Tend 0 50]);
latexylabel('$$d_i\;$$');   %  in m
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 10 20 30 40]);
hold off;

%%  Test of collision avoidance condition
%   as in PlatoonDemo.m, figure(14)
%
figure(7)
%
G0=tf(1,1);
G1=tf(1,[T1*D1 D1 1]);
G2=tf(1,[T1*D2 D2 1]);
G3=tf(1,[T1*D3 D3 1]);
G4=tf(1,[T1*D4 D4 1]);
G5=tf(1,[T1*D5 D5 1]);
Integrator=tf(1,[1 0]);
G00=1;
G10=G1;
G20=G2*(a20+a21*G10);
G30=G3*(a31*G10+a32*G20);
G40=G4*(a42*G20+a43*G30);
G50=G5*(a53*G30+a54*G40);
%
subplot(4,1,[1 2])
[Y, Time]=impulse(Integrator*(G10-G20));
plot(Time, Y, 'b');
hold on
[Y, Time]=impulse(Integrator*(G20-G30));
plot(Time, Y, 'b');
[Y, Time]=impulse(Integrator*(G30-G40));
plot(Time, Y, 'b');
[Y, Time]=impulse(Integrator*(G40-G50));
plot(Time, Y, 'b');
plot([0 25],[0 0],'k:');
axis([0 25 -0.05 0.45]);
latexylabel('$$g_{{\rm d}i}$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 0.2 0.4]);
xticks([0 10 20 30 40]);
hold off;
latextitle('$${\tt PlatoonNonlin:}$$ Test of collision avoidance for CACC');


%%  Add a small time delay
%
figure(8)
%
tau=0.8;      %  tau < beta!, 
              %  for tau>1.5 the collision avoidance condition is not satisfied
ktau=ceil(tau/dt);
kendtau=length(TimeW)-ktau;
%   Determination of the weighting factors
a21=(beta1+beta2-D2-tau)/(beta1-tau);
a20=1-a21;
a32=(beta2+beta3-D3-tau)/(beta2-tau);
a31=1-a32;
a43=(beta3+beta4-D4-tau)/(beta3-tau);
a42=1-a43;
a54=(beta4+beta5-D5-tau)/(beta4-tau);
a53=1-a54;
%   Determination of the vehicle outputs
V00=v0*ones(ktau,1);           % initial values of the dead time elements
Y0=lsim(Veh0, W, TimeW, x00);
W1=Y0(:,2);
V0tau=[V00; Y0(1:kendtau,2)];
Y1=lsim(Veh1, W1, TimeW, x10);
Dist1=Y0(:,1)-Y1(:,1);
W2=a20*V0tau+a21*Y1(:,2);
V1tau=[V00; Y1(1:kendtau,2)];
Y2=lsim(Veh2, W2, TimeW, x20);
Dist2=Y1(:,1)-Y2(:,1);
W3=a31*V1tau+a32*Y2(:,2);
V2tau=[V00; Y2(1:kendtau,2)];
Y3=lsim(Veh3, W3, TimeW, x30);
Dist3=Y2(:,1)-Y3(:,1);
W4=a42*V2tau+a43*Y3(:,2);
V3tau=[V00; Y3(1:kendtau,2)];
Y4=lsim(Veh4, W4, TimeW, x40);
Dist4=Y3(:,1)-Y4(:,1);
W5=a53*V3tau+a54*Y4(:,2);
Y5=lsim(Veh5, W5, TimeW, x50);
Dist5=Y4(:,1)-Y5(:,1);
%
subplot(4, 1,[1 2])
plot(TimeW, [Y0(:,2) Y1(:,2) Y2(:,2) Y3(:,2) Y4(:,2) Y5(:,2)],'b');
hold on
plot([-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
plot(TimeW, W,'b--'); % command to leading vehicle
axis([-1 Tend -2 v1*1.1]);
latexylabel('$$v_i$$ in m/s');
rotateY;
xleer;
yticks([0 10 20]);
xticksempty([0 10 20 30 40]);
latextitle('$${\tt PlatoonNonlin:}$$ CACC with small time delay');
hold off;
%
subplot(4,1,[3 4])
plot(TimeW, [Dist2 Dist3 Dist4 Dist5],'b');
hold on
plot(TimeW, Dist1,'b--');
plot([0 0],[-100 100],'k:');
axis([-1 Tend 0 50]);
latexylabel('$$d_i$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 10 20 30 40]);
hold off;


%%  Test of collision avoidance
%   for the time-delay transmission, the impulse responses to be checked
%   are recursively determined
%
figure(9)
%   Determination of the vehicle outputs for impulse input at vehicle 0
TendI=20;
dtI=0.03;
TimeI=0:dtI:TendI;
ktau=ceil(tau/dtI);
kendtau=length(TimeI)-ktau;
V00=zeros(ktau,1);           % initial values of the dead time elements
S00=zeros(ktau,1);
%
Y1=impulse(Veh1, TimeI);
V1tau=[V00; Y1(1:kendtau,2)];
%    G_d1 is not tested, because it is externally positive due to the
%    design
W1=Y1(:,2);
Y21=lsim(Veh2, W1, TimeI);     % reaction on the predecessor velocity v_1
Y22=impulse(Veh2,TimeI);       % reaction on the impulse function v_0
V22tau=[V00; Y22(1:kendtau,2)]; % delayed reaction on v_0
V2=a21*Y21(:,2)+a20*V22tau;  % v_2
V2tau=[V00; V2(1:kendtau)];
S2tau=[S00; Y22(1:kendtau,1)];  % delayed position
S2=a21*Y21(:,1)+a20*S2tau;
Dist2=Y1(:,1)-S2;   % g20(t)
%
W3=a31*V1tau+a32*V2;
Y3=lsim(Veh3, W3, TimeI);
Dist3=S2-Y3(:,1);
%
W4=a42*V2tau+a43*Y3(:,2);
V3tau=[V00; Y3(1:kendtau,2)];
Y4=lsim(Veh4, W4, TimeI);
Dist4=Y3(:,1)-Y4(:,1);
%
W5=a53*V3tau+a54*Y4(:,2);
Y5=lsim(Veh5, W5, TimeI);
Dist5=Y4(:,1)-Y5(:,1);
%
subplot(4,1,[1 2])
plot(TimeI, [Dist2 Dist3 Dist4 Dist5], 'b');
hold on
plot([0 25],[0 0],'k:');
axis([0 TendI -0.05 0.45]);
latexylabel('$$g_{{\rm d}i}$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 0.2 0.4]);
xticks([0 10 20 30 40]);
hold off;
latextitle('$${\tt PlatoonNonlin:}$$ Test of collision avoidance for delayed CACC');


%%  Add a large time delay
%
figure(10)
%
tau=1.5;      %  the communication structure has to be changed
ktau=ceil(tau/dt);
kendtau=length(TimeW)-ktau;
%   Determination of the weighting factors
a21=(beta1+beta2-D2-tau)/(beta1-tau);
a20=1-a21;
a31=(beta1+beta2+beta3-D3-tau)/(beta1);
a30=1-a31;
a42=(beta2+beta3+beta4-D4-tau)/(beta2);
a41=1-a42;
a54=1;
a53=1-a54;
%   Determination of the vehicle outputs
V00=v0*ones(ktau,1);           % initial values of the dead time elements
Y0=lsim(Veh0, W, TimeW, x00);
W1=Y0(:,2);
V0tau=[V00; Y0(1:kendtau,2)];
Y1=lsim(Veh1, W1, TimeW, x10);
Dist1=Y0(:,1)-Y1(:,1);
W2=a20*V0tau+a21*Y1(:,2);
V1tau=[V00; Y1(1:kendtau,2)];
Y2=lsim(Veh2, W2, TimeW, x20);
Dist2=Y1(:,1)-Y2(:,1);
W3=a30*V0tau+a31*V1tau;
V2tau=[V00; Y2(1:kendtau,2)];
Y3=lsim(Veh3, W3, TimeW, x30);
Dist3=Y2(:,1)-Y3(:,1);
W4=a41*V1tau+a42*V2tau;
V3tau=[V00; Y3(1:kendtau,2)];
Y4=lsim(Veh4, W4, TimeW, x40);
Dist4=Y3(:,1)-Y4(:,1);
W5=a53*V3tau+a54*Y4(:,2);
Y5=lsim(Veh5, W5, TimeW, x50);
Dist5=Y4(:,1)-Y5(:,1);
%
subplot(4,1,[1 2])
plot(TimeW, [Y0(:,2) Y1(:,2) Y2(:,2) Y3(:,2) Y4(:,2) Y5(:,2)],'b');
hold on
plot([-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
plot(TimeW, W,'b--'); % command to leading vehicle
axis([-1 Tend -2 v1*1.1]);
latexylabel('$$v_i\;$$');  %  
rotateY;
xleer;
yticks([0 10 20]);
xticksempty([0 10 20 30 40]);
latextitle('$${\tt PlatoonNonlin:}$$ CACC with large time delay');
hold off;
%
subplot(4,1,[3 4])
plot(TimeW, [Dist2 Dist3 Dist4 Dist5],'b');
hold on
plot(TimeW, Dist1,'b--');
plot([0 0],[-100 100],'k:');
axis([-1 Tend 0 50]);
latexylabel('$$d_i\;$$');  %   in m
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 10 20 30 40]);
hold off;

%%  Test of collision avoidance
%   as in figure(9), but with changed communication structure
%
figure(11)
%   Determination of the vehicle outputs for impulse input at vehicle 0
TendI=20;
dtI=0.03;
TimeI=0:dtI:TendI;
ktau=ceil(tau/dtI);
kendtau=length(TimeI)-ktau;
V00=zeros(ktau,1);           % initial values of the dead time elements
S00=zeros(ktau,1);
%
Y1=impulse(Veh1, TimeI);
V1tau=[V00; Y1(1:kendtau,2)];
%    G_d1 is not tested, because it is externally positive due to the
%    design
W1=Y1(:,2);
Y21=lsim(Veh2, W1, TimeI);     % reaction on the predecessor velocity v_1
Y22=impulse(Veh2,TimeI);       % reaction on the impulse function v_0
V22tau=[V00; Y22(1:kendtau,2)]; % delayed reaction on v_0
V2=a21*Y21(:,2)+a20*V22tau;  % v_2
V2tau=[V00; V2(1:kendtau)];
S2tau=[S00; Y22(1:kendtau,1)];  % delayed position
S2=a21*Y21(:,1)+a20*S2tau;
Dist2=Y1(:,1)-S2;   % g20(t)
%
Y31=lsim(Veh3, V2, TimeI);     % reaction on the predecessor velocity v_2
V31tau=[V00; Y31(1:kendtau,2)];
Y30=impulse(Veh3,TimeI);       % reaction on the impulse function v_0
V30tau=[V00; Y30(1:kendtau,2)]; % delayed reaction on v_0
V3=a31*V31tau+a30*V30tau;  % v_2
V3tau=[V00; V2(1:kendtau)];
S3tau=[S00; Y30(1:kendtau,1)];  % delayed position
S3=a31*Y31(:,1)+a30*S3tau;
Dist3=S2-S3;   % g30(t)
%
W4=a41*V1tau+a42*V2tau;
Y4=lsim(Veh4, W4, TimeI);
Dist4=S3-Y4(:,1);
%
W5=a53*V3tau+a54*Y4(:,2);
Y5=lsim(Veh5, W5, TimeI);
Dist5=Y4(:,1)-Y5(:,1);
%
subplot(4,1,[1 2])
plot(TimeI, [Dist2 Dist3 Dist4 Dist5], 'b');
hold on
plot([0 25],[0 0],'k:');
axis([0 TendI -0.05 0.45]);
latexylabel('$$g_{{\rm d}i}$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 0.2 0.4]);
xticks([0 10 20 30 40]);
hold off;
latextitle('$${\tt PlatoonNonlin:}$$ Test of collision avoidance for delayed CACC');

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
epsfigc15('PlatoonNonlin');