%      PlatoonDemo.M
%
%   Properties of the vehicle platoons
%   for communication structure design
%    from PT2Systems.m of 8.9.2015
%
% J. Lunze
% 9.3.2018
%
% Version of 6.1.2019
% for 2nd edition: 26.3.2021
% 
close all
clear all

%%   Series connection of 2nd-order systems with sinusoidal input
%      PT2Systems.m   figure(5)
%
%  System2: damping factor d2=1/sqrt(2)
%
%  sinusoidal input -> decreasing magnitude along the series connection
%  because |G(jw)| \le 1
%
figure(1)
subplot(2,1,1)
%
T=1;
d1=1;
d2=1/sqrt(2);
d3=0.5;
ks=1;
%      2nd-order systems 1/(T^2 s^2 + 2*d*T s + 1)
System1=tf([1],[T*T 2*d1*T 1]);
System2=tf([1],[T*T 2*d2*T 1]);
System3=tf([1],[T*T 2*d3*T 1]);
%
omega=0.7;
Tend=80;
Time=[0:0.01:Tend];
U=sin(omega*Time);
Y1=lsim(System2, U, Time);
Y2=lsim(System2*System2, U, Time);
Y3=lsim(System2*System2*System2, U, Time);
Y4=lsim(System2*System2*System2*System2, U, Time);
plot(Time, Y1, 'b', Time, Y2, 'b', Time, Y3, 'b',Time, Y4, 'b');
latexylabel('$$y_i$$');
rotateY;
%latexxlabel('$$t$$');
latextitle('$${\tt PlatoonDemo:}$$ Series of second-order systems');
yticks([-1 0 1]);
xticks([0 20 40 60 80 100]);
%
%  analysis with step input
subplot(2,1,2)
Tend=10;
Times=[0:0.01:Tend];
Ys1=step(System2, Times);
Ys2=step(System2*System2, Times);
Ys3=step(System2*System2*System2, Times);
Ys4=step(System2*System2*System2*System2, Times);
Ys5=step(System2*System2*System2*System2*System2, Times);
plot(Times, Ys1, 'b', Times, Ys2, 'b', Times, Ys3, 'b', Times, Ys4, 'b', Times, Ys5, 'b');
hold on
plot([0 Tend],[1 1],'k:');
latexylabel('$$h_i$$');
rotateY;
latexxlabel('$$t$$');
axis([0 10 0 1.2]);
yticks([0 1]);
xticks([0 2 4 6 8 10]);
hold off
% 
%   max(Ys1)/max(Ys2) decreases from d2=1/sqrt(2) ... d2=0.867;

%%   Detailed analysis
%
figure(2)
subplot(3,1,[1,2]);
Tend=80;
dt=0.0001;
Times=[0:dt:Tend];
Ys1=step(System2, Times);
plot(Times, Ys1, 'b');
hold on
Ymax=max(Ys1);
k2=1;
while Ys1(k2) < Ymax
    k2=k2+1;
end
plot(k2*dt, Ys1(k2), 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
%  series connection of N systems
Ysi=Ys1;
N=40;
for k1=2:N
    Ysi=lsim(System2, Ysi, Times);
    plot(Times, Ysi, 'b');
    Ymax=max(Ysi);
    % search for the time instant of the max value
    k2=1;
    while Ysi(k2) < Ymax
        k2=k2+1;
    end
    plot(k2*dt, Ysi(k2), 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
end
plot([0 Tend],[1 1],'k:');
latexylabel('$$h_i$$');
rotateY;
latexxlabel('$$t$$');
latextitle('$${\tt PlatoonDemo:}$$ Series of second-order systems');
axis([0 Tend 0.9 1.2]);
yticks([0.9 1 1.1 1.2]);
xticks([0 20 40 60 80]);


%%   Braking-accelerating manoeuvres
%    from PlatoonITS.m,  figure(1)
%
figure(3)
T1=2;    % time constant, nominal: T1=2;
D1=T1;   % delay measure
Tend=5;
% Step response of a string of agents
%  v_i = coupling output
%  s_i = control output
%
N1=5;
A1=[-1/T1 0; 1 0];
B1=[1/T1; 0];
C1=[0 1];
Cz1=[1 0];
n=2;
%
v0=30/3.6;   % initial velocity  30 km/h
alpha=1;
beta=2;      % proportional spacing in seconds
si=alpha+beta*v0;   % initial spacing
x0=[v0; 0; v0; -si; v0; -2*si; v0; -3*si; v0; -4*si];
vref=0/3.6;   % new velocity set-point
Tend=25;
%   System with N1 agents in a row
Null=zeros(n,n);
Abar=[A1  Null  Null  Null  Null;
      B1*Cz1 A1  Null Null Null;
      Null B1*Cz1  A1  Null Null;
      Null Null B1*Cz1  A1  Null;
      Null Null Null B1*Cz1 A1    ];
Bbar=[B1; zeros((N1-1)*n, 1)];
Cv=kron(eye(N1, N1), Cz1);
Cs=kron(eye(N1, N1), C1);
Stringv=ss(Abar, Bbar, Cv, 0);
Strings=ss(Abar, Bbar, Cs, 0);
%
subplot(4,1,[1 2])
[V, Time]=step(Stringv, Tend);
V0=initial(Stringv, x0, Time);
V=vref*V+V0;
plot(Time, V,'b', [-3 0],[v0 v0],'k', [0 0],[-1 9],'k:',[-1 30],[0 0],'k:');
axis([-1 Tend vref-0.3 v0+0.3]);
latexylabel('$$v_i$$ in m/s');
rotateY;
xleer;
yticks([0 4 8]);
xticksempty([0 10 20]);
%latexxlabel('$$t$$');
latextitle('$${\tt PlatoonDemo:}$$ Braking manoeuvre');
hold off;
latextext(18, 6, ['$$\beta_i=$$', num2str(T1)]);
%
subplot(4,1,[3 4])
S=step(Strings, Time);
S0=initial(Strings, x0, Time);
S=vref*S+S0;
for k1=1:N1-1
    plot(Time, S(:,k1)-S(:,k1+1), 'b');
    hold on
    dmin(k1)=min(S(:,k1)-S(:,k1+1));
end
dmin=min(dmin);
plot([-3 0],[si si],'k');
dsoll=alpha+beta*vref;
dmin=min([dmin, dsoll]);
plot([-3 Tend], [dsoll, dsoll],'k:');
axis([-1 Tend dmin-3 S(1,1)-S(1,2)+3]);
latexylabel('$$d_i$$ in m');
%hold off;
rotateY;
latexxlabel('$$t$$ in s');
%   draw alpha+beta*v_i
Dref=alpha+beta*V;
plot(Time, Dref(:,2:N1), 'b:', [0 0],[-1 30],'k:');
yticks([1 10 20]);
xticks([0 10 20]);
hold off;


%%   Draw the positions for CACC
%    similar to figure(2) in StringCACC.m
%
figure(15)
plot(Time, S(:,1:N1),'b', [0 0],[50 -100],'k:');
hold on
for k1=1:N1
    plot([-3 0],[S(1,k1)-3*v0, S(1,k1)], 'b');
end
axis([-1 Tend -80 20]);
latexylabel('$$s_i$$ in m');
rotateY;
xleer;
yticks([-100 -50 0 50]);
xticks([0 10 20 30 40]);
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre with CACC');
hold off;


%%    Braking-accelerating manoeuvres
%      from PlatoonITS.m, figure(3)
%
figure(4)
%  Velocity v_{i-1}
dt=0.1;
Tv0=0;
Tv1=4;
Tv2=10;
Tv3=16;
TendS=40;
k0e=1; 
k1e=ceil(Tv1-Tv0)/dt;
k2e=k1e+ceil(Tv2-Tv1)/dt;
k3e=k2e+ceil(Tv3-Tv2)/dt;
k4e=k3e+ceil(TendS-Tv3)/dt;
VPred=v0+v0*[0*ones(k1e-k0e,1);
             0.7*ones(k2e-k1e,1); 
             0.3*ones(k3e-k2e,1); 
             0.6*ones(k4e-k3e+1,1)];
TimeS=0:dt:(k4e-1)*dt;
%
subplot(4,1,[1 2])
V=lsim(Stringv, VPred, TimeS, x0);
plot(TimeS, VPred, 'b--');
hold on
plot(TimeS, V,'b', [-3 0],[v0 v0],'k');
axis([-1 TendS min(min(V))-0.5 max(max(V))+0.5]);
latexylabel('$$v_i$$ in m/s');
rotateY;
yticks([8 10 12 14]);
xleer;
xticksempty([0 10 20 30 40]);
%latexxlabel('$$t$$');
latextitle('$${\tt PlatoonDemo:}$$ Braking-accelerating manoeuvre');
%latextext(18, 6, ['$$T=$$', num2str(T1)]);
hold off;
%
subplot(4,1,[3 4])
S=lsim(Strings, VPred, TimeS, x0);
for k1=1:N1-1
    plot(TimeS, S(:,k1)-S(:,k1+1), 'b');
    hold on
end
plot([-3 0],[si si],'b');
latexylabel('$$d_i$$ in m');
%hold off;
rotateY;
latexxlabel('$$t$$ in s');
%   draw alpha+beta*v_i
Dref=alpha+beta*V;
plot(TimeS, Dref(:,2:N1), 'b:');
%latextext(15, si+2, ['$$T=$$', num2str(T1)]);
dmin=min(min(Dref));
dmax=max(max(Dref));
axis([-1 TendS dmin-2 dmax+3]);
yticks([0 20 30 40]);
xticks([0 10 20 30 40]);
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%    Vehicle with velocity controller = PT1 for power train + integrator
%     from PlatoonITS.m, figure(3)
%         which is from   StringVehicle.m  of 18.5.2017
%
%  input: velocity reference signal v_si
%  output: velocity v_i
%  velocity controller: K(s) = kV
%
figure(5)
subplot(2,1,1)
T1=0.5;    % time constant of the power train (for larger T1, gbar is not positive)
beta=2;   % time-headway constant
kv=1/beta;   % feedback gain for D=beta
Tend=10;
A=[0 1; -kv/T1 -1/T1];
B=[0; kv/T1];
C=[1 0];   % output = velocity
D=0;
Vbar=ss(A, B, C, D);
[Y, Time]=impulse(Vbar, Tend);
plot(Time, Y, 'b', [0 Tend],[0 0],'k:');
hold on;
%   required first-order behaviour
Ahat=[-1/beta];
Bhat=[1/beta];
Chat=1;
Dhat=0;
Vhat=ss(Ahat, Bhat, Chat, Dhat);
Yhat=impulse(Vhat, Time);
plot(Time, Yhat,'b--');
%
latexylabel('$$\bar{g}$$');
rotateY;
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonDemo:}$$ Comparison of vehicle model and requirement');
axis([0 Tend -0.1 1.1]);
yticks([0 0.5 1]);
xticks([0 5 10 15]);
latextext(8, 0.4, ['$$T=$$', num2str(T1)]);
latextext(0.5, 0.5, '$$\bar{g}^*$$');
hold off;


%%   Distance between neighbouring vehicles and required distance
%    
figure(5)
subplot(2,1,2)
%   Model of first-order vehicles  from  StringPTone.m of 3.5.2017
N1=5;
A1=[-1/beta 0; 1 0];
B1=[1/beta; 0];
C1=[0 1];
Cz1=[1 0];
n=2;
%
v0=30/3.6;   % initial velocity  30 km/h
alpha=1;
beta=2;      % proportional spacing in seconds
si=alpha+beta*v0;   % initial spacing
x0=[v0; 0; v0; -si; v0; -2*si; v0; -3*si; v0; -4*si];
vref=0/3.6;   % new velocity set-point
Tend=20;
%   System with N1 agents in a row
Null=zeros(n,n);
Abar=[A1  Null  Null  Null  Null;
      B1*Cz1 A1  Null Null Null;
      Null B1*Cz1  A1  Null Null;
      Null Null B1*Cz1  A1  Null;
      Null Null Null B1*Cz1 A1    ];
Bbar=[B1; zeros((N1-1)*n, 1)];
Cv=kron(eye(N1, N1), Cz1);
Cs=kron(eye(N1, N1), C1);
Stringv=ss(Abar, Bbar, Cv, 0);
Strings=ss(Abar, Bbar, Cs, 0);
%
[S, Time]=step(Strings, Tend);
S0=initial(Strings, x0, Time);
S=vref*S+S0;
for k1=1:N1-1
    plot(Time, S(:,k1)-S(:,k1+1), 'b--');
    hold on
    dmin(k1)=min(S(:,k1)-S(:,k1+1));
end
dmin=min(dmin);
plot([-3 0],[si si],'b');
dsoll=alpha+beta*vref;
dmin=min([dmin, dsoll]);
plot([-3 Tend], [dsoll, dsoll],'k:');
axis([-1 Tend dmin-3 S(1,1)-S(1,2)+3]);
latexylabel('$$d_i$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([1 10 20]);
xticks([0 10 20]);
%  
%   Comparison with second-order vehicle model
%   Extend vehicle model to get the position s
As=[A zeros(2,1); C 0];
Bs=[B; 0];
Cv=[C 0];             % coupling output: velocity
Cs=[zeros(1,2) 1];    % output: position s_i
Ds=0;
n=3;
%   System with N1 agents in a row
Null=zeros(n,n);
Asbar=[As  Null  Null  Null  Null;
      Bs*Cv As  Null Null Null;
      Null Bs*Cv  As  Null Null;
      Null Null Bs*Cv  As  Null;
      Null Null Null Bs*Cv As    ];
Bsbar=[Bs; zeros((N1-1)*n, 1)];
Csbar=kron(eye(N1, N1), Cs);     %  output: positions
Stringss=ss(Asbar, Bsbar, Csbar, 0);
%  initial state
xi0=-inv(A)*B*v0;
xs0=[xi0; 0; xi0; -si; xi0; -2*si; xi0; -3*si; xi0; -4*si];
%
[Ss, Time]=step(Stringss, Tend);
Ss0=initial(Stringss, xs0, Time);
Ss=vref*Ss+Ss0;
for k1=1:N1-1
    plot(Time, Ss(:,k1)-Ss(:,k1+1), 'b');
    hold on
end
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%      Vehicle with distance controller
%
figure(6)
subplot(2,1,1);
Tend=10;
beta=2;
kd=-1.5;
kv=-5;     % -5
A1=[(kv+kd*beta)/(1-kv*beta) -kd/(1-kv*beta);    % x_1=v_i
                    -1                 0      ]; % x_2=d_itilde
B1=[-kv/(1-kv*beta); 1];      % input = velocity of predecessor
Cz1=[1 0];                     % output= velocity
C1=[0 1];                     % output= distance dtilde
n=2;
Vbarv=ss(A1, B1, Cz1, 0);     % output= velocity
Vbard=ss(A1, B1, C1, 0);      % output= distance dtilde
[Vi, TimeG]=impulse(Vbarv, Tend);
Di=impulse(Vbard, TimeG);
plot(TimeG, Vi, 'b', TimeG, Di, 'b--', [0 Tend], [0 0], 'k:');
latexylabel('$$\bar{g}, \bar{f}$$');
rotateY;
xleer;
axis([0 Tend -0.05 1.05]);
yticks([0 0.5 1]);
xticks([0 5 10]);
latextitle('$${\tt PlatoonDemo:}$$ Vehicle with PD distance controller');
%

%%   Compensation of wrong initial positions by a distance controller
%
figure(24)
subplot(3,1,[1,2]);
%  initial state
delta=8;   % additional distance error in m
v0=20/3.6;      % initial velocity 20 km/h
alpha=1;
beta=2;      % proportional spacing in seconds
di=alpha+beta*v0;   % initial spacing according to time-headway policy
x0=[v0; di+0.7*delta; v0; di+delta; v0; di+0.5*delta; v0; di+2*delta; v0; di+1.3*delta];
vref=30/3.6;   % new velocity set-point
Tend=25;
%   System with N1 agents in a row
Null=zeros(n,n);
Abar=[A1  Null  Null  Null  Null;
      B1*Cz1 A1  Null Null Null;
      Null B1*Cz1  A1  Null Null;
      Null Null B1*Cz1  A1  Null;
      Null Null Null B1*Cz1 A1    ];
Bbar=[B1; zeros((N1-1)*n, 1)];   % input: reference velocity of platoon
Cv=kron(eye(N1, N1), Cz1);       % output= velocity
Cd=kron(eye(N1, N1), C1);        % output= distance
Stringv=ss(Abar, Bbar, Cv, 0);
Stringd=ss(Abar, Bbar, Cd, 0);
%
[Dist, Time]=step(Strings, Tend);
Dist0=initial(Strings, x0, Time);
Dist=vref*Dist+Dist0;
plot(Time, Dist, 'b');
hold on
%  Reference distance of first follower according to time-headway rule
x00=[v0; 0];
[V0, Time]=step(Vbarv, Tend);
V00=initial(Vbarv, x00, Time);
V0=vref*V0+V00;
plot(Time, beta*V0, 'b--');
axis([0 Tend 0 30]);
latexylabel('$$\tilde{d}_i$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 10 20 30]);
xticks([0 10 20]);
latextitle('$${\tt PlatoonDemo:}$$ Vehicle with PD distance controller');
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%      Vehicle with proportional distance controller
%
%   in comparison to Fig.4: kv=0
%   but with PT1 power train
figure(7)
subplot(4,1,[1 2]);
Tend=10;
T1=0.3;
kd=-1;
A1=[  0           1      0;        % x_1=v_i
    kd*beta/T1 -1/T1   -kd/T1;     % x_2=a_i
      -1      0            0 ];    % x_3=d_itilde
B1=[0; 0; 1];
Cz1=[1 0 0];                     % output= velocity
C1=[0 0 1];                     % output= distance dtilde
n=3;
Vbarv=ss(A1, B1, Cz1, 0);
Vbard=ss(A1, B1, C1, 0);
[Vi, TimeG]=impulse(Vbarv, Tend);
Di=impulse(Vbard, TimeG);
plot(TimeG, Vi, 'b', TimeG, Di, 'b--', [0 Tend], [0 0], 'k:');
latexylabel('$$\bar{g}, \bar{f}$$');
rotateY;
axis([0 Tend -0.2 1.05]);
yticks([0 0.5 1]);
xticks([0 5 10]);
xleer;
latextitle('$${\tt PlatoonDemo:}$$ Vehicle with PD distance controller');
latextext(8, 0.7, ['$$T=$$', num2str(T1)]);
%
%     large time constant for power train
subplot(4,1,[3 4]);
Tend=10;
T1=0.7;
kd=-0.8;
A1=[  0           1      0;        % x_1=v_i
    kd*beta/T1 -1/T1   -kd/T1;     % x_2=a_i
      -1      0            0 ];    % x_3=d_itilde
Vbarv=ss(A1, B1, Cz1, 0);
Vbard=ss(A1, B1, C1, 0);
[Vi, TimeG]=impulse(Vbarv, Tend);
Di=impulse(Vbard, TimeG);
plot(TimeG, Vi, 'b', TimeG, Di, 'b--', [0 Tend], [0 0], 'k:');
latexylabel('$$\bar{g}, \bar{f}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend -0.2 1.05]);
yticks([0 0.5 1]);
xticks([0 5 10]);
latextext(8, 0.7, ['$$T=$$', num2str(T1)]);


%%  Distance and velocity controller
%   with power train as PT1 element
%
figure(8)
subplot(2,1,1);
%  Tuning of the P velocity controller
Tend=10;
T1=0.7;
kv=1/beta;
A1=[  0       1      0;        % x_1=v_i
    -kv/T1  -1/T1    0;        % x_2=a_i
      -1      0      0 ];      % x_3=d_itilde
B1=[0; kv/T1; 1];
Cz1=[1 0 0];                     % output= velocity
C1=[0 0 1];                     % output= distance dtilde
n=3;
Vbarv=ss(A1, B1, Cz1, 0);
Vbard=ss(A1, B1, C1, 0);
[Vi, TimeG]=impulse(Vbarv, Tend);
Di=impulse(Vbard, TimeG);
plot(TimeG, Vi, 'b', TimeG, Di, 'b--', [0 Tend], [0 0], 'k:');
latexylabel('$$\bar{g}, \bar{f}$$');
rotateY;
xleer;
axis([0 Tend -0.05 1.05]);
yticks([0 0.5 1]);
%xticks([0 5 10]);
latextitle('$${\tt PlatoonDemo:}$$ Vehicle with P velocity controller');
latextext(8, 0.7, ['$$T=$$', num2str(T1)]);
hold off
%

%%  Extension by the distance controller
%
figure(9)
subplot(4,1,[1 2]);
kd=-0.01;
T1=0.7;
Tend=10;
A1=[  0                      1      0;               % x_1=v_i
    -kv/T1+kd*kv*beta/T1  -1/T1    -kv*kd/T1;        % x_2=a_i
      -1                     0      0 ];             % x_3=d_itilde
B1=[0; kv/T1; 1];
Cz1=[1 0 0];                     % output= velocity
C1=[0 0 1];                     % output= distance dtilde
n=3;
Vbarv=ss(A1, B1, Cz1, 0);
Vbard=ss(A1, B1, C1, 0);
[Vi, TimeG]=impulse(Vbarv, Tend);
Di=impulse(Vbard, TimeG);
plot(TimeG, Vi, 'b', TimeG, Di, 'b--', [0 Tend], [0 0], 'k:');
latexylabel('$$\bar{g}, \bar{f}$$');
rotateY;
xleer;
axis([0 Tend -0.05 1.05]);
yticks([0 0.5 1]);
xticksempty([0 10 20]);
latextitle('$${\tt PlatoonDemo:}$$ Vehicle with P velocity controller');
latextext(8, 0.7, ['$$T=$$', num2str(T1)]);
hold off
%
%   Distance between neighbouring vehicles and required distance
%    
subplot(4,1,[3 4])
%
v0=30/3.6;   % initial velocity  30 km/h
alpha=1;
beta=2;      % proportional spacing in seconds
di0=beta*v0;   % initial spacing
x0=[v0; 0; 0; v0; 0; di0; v0; 0; di0; v0; 0; di0; v0; 0; di0];
vref=0/3.6;   % new velocity set-point
Tend=25;
%    Leading vehicle with ideal dynamics; for simplicity with n=2
A0=[-0.5 0 0;
    1 0 0;
    0 0 -1];    % state variables: v_0, s_0
B0=[0.5; 0; 0];
%   System with N1 agents in a row
Null=zeros(n,n);
Abar=[A0  Null  Null  Null  Null;
      B1*Cz1 A1  Null Null Null;
      Null B1*Cz1  A1  Null Null;
      Null Null B1*Cz1  A1  Null;
      Null Null Null B1*Cz1 A1    ];
Bbar=[B0; zeros((N1-1)*n, 1)];
Cv=kron(eye(N1, N1), Cz1);
Cs=kron(eye(N1, N1), C1);    % outputs: s_0, d_1,...,d_4
Stringv=ss(Abar, Bbar, Cv, 0);
Strings=ss(Abar, Bbar, Cs, 0);
%
[D0,Time]=initial(Strings, x0, Tend);
Dist=D0+alpha;
for k1=2:N1
    plot(Time, Dist(:,k1), 'b');
    hold on
end
plot([-3 0],[si si],'b');
plot([-3 Tend], [alpha alpha],'k:');
latexylabel('$$d_i$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([1 10 20]);
xticks([0 10 20]);
axis([-1 Tend dmin-3 S(1,1)-S(1,2)+3]);
hold off

%%   Acceleration/braking manoeuvre
%     from StringCACC.m,  figure 3
%  controlled vehicles are represented by PT1-elements
%  v_i = coupling output
%  s_i = control output
figure(10)
%  Velocity command for the leading vehicle
dt=0.02;
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;
v0=0;
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;
%
beta=2;    % time-headway coefficient
alpha=1;
N1=6;     % Vehicles 0,1,..., 5
T0=1.5;
T1=2;
T2=2.5;
T3=3;
T4=2;
T5=3.5;
A0=[-1/T0 0; 1 0];
B0=[1/T0; 0];
A1=[-1/T1 0; 1 0];
B1=[1/T1; 0];
A2=[-1/T2 0; 1 0];
B2=[1/T2; 0];
A3=[-1/T3 0; 1 0];
B3=[1/T3; 0];
A4=[-1/T4 0; 1 0];
B4=[1/T4; 0];
A5=[-1/T5 0; 1 0];
B5=[1/T5; 0];
C=[0 1];
Cz=[1 0];
n=2;
%
si=alpha+beta*v0;   % initial spacing
x0=[v0; 0; v0; -si; v0; -2*si; v0; -3*si; v0; -4*si; v0; -5*si];
%   Determination of the weighting factors
a21=0.75;
a20=1-a21;
a32=0.5;
a31=1-a32;
a44=0;
a43=1-a44;
a54=0.25;
a53=1-a54;
%   System with N1 agents in a row
Null=zeros(n,n);
Abar=[A0 Null Null Null Null Null;
      B1*Cz  A1  Null  Null  Null  Null;
      a20*B2*Cz a21*B2*Cz A2  Null Null Null;
      Null a31*B3*Cz a32*B3*Cz  A3  Null Null;
      Null Null Null a43*B4*Cz  A4  Null;
      Null Null Null a53*B5*Cz a54*B5*Cz A5    ];
Bbar=[B0; zeros((N1-1)*n, 1)];
Cv=kron(eye(N1, N1), Cz);
Cd=kron(eye(N1, N1), C)-kron(diag(ones(N1-1,1),1), C); % output: d_i
Cs=kron(eye(N1, N1), C); % output: s_i
Stringv=ss(Abar, Bbar, Cv, 0);
Stringd=ss(Abar, Bbar, Cd, 0);
Strings=ss(Abar, Bbar, Cs, 0);
%
subplot(4,1,[1 2])
V=lsim(Stringv, W, TimeW, x0);
plot(TimeW, V(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
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]);
%xticks([0 10 20]);
%latexxlabel('$$t$$');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre with CACC');
hold off;
%
subplot(4,1,[3 4])
Dist=lsim(Stringd, W, TimeW, x0);   % output: d_i
plot(TimeW, Dist(:,2:N1-1)+alpha,'b');
hold on
plot(TimeW, Dist(:,1)+alpha,'b--');
plot([0 0],[-100 100],'k:');
axis([-1 Tend 0 50]);
latexylabel('$$d_i$$ in m');    %  true vehicle distance
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 10 20 30 40]);
hold off;


%% For comparison: platoon with ACC with the same vehicles
%  from StringCACC.m, figure 1
%
figure(11)
%  delays
D1=T1;
D2=T2;
D3=T3;
D4=T4;
D5=T5;
%   System with N1 agents in a row
Null=zeros(n,n);
AbarACC=[A0 Null Null Null Null Null;
      B1*Cz  A1  Null  Null  Null  Null;
      Null B2*Cz A2  Null Null Null;
      Null Null B3*Cz  A3  Null Null;
      Null Null Null B4*Cz  A4  Null;
      Null Null Null Null B5*Cz A5    ];
Bbar=[B0; zeros((N1-1)*n, 1)];
Cv=kron(eye(N1, N1), Cz);
Cd=kron(eye(N1, N1), C)-kron(diag(ones(N1-1,1),1), C); % output: d_i
Cs=kron(eye(N1, N1), C); % output: s_i
StringvACC=ss(AbarACC, Bbar, Cv, 0);
StringdACC=ss(AbarACC, Bbar, Cd, 0);
%
subplot(4,1,[1 2])
V=lsim(StringvACC, W, TimeW, x0);
plot(TimeW, V(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
plot(TimeW, W,'b--'); % command to leading vehicle
axis([-1 Tend -2 v1*1.1]);
latexylabel('$$v_i$$ in m/s');
rotateY;
xleer;
xticksempty([0 10 20 30 40]);
yticks([0 10 20]);
%xticks([0 10 20]);
%latexxlabel('$$t$$');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre with ACC');
hold off;
%
subplot(4,1,[3 4])
Dist=lsim(StringdACC, W, TimeW, x0);  % output: d_i
plot(TimeW, Dist(:,2:N1-1),'b');
hold on
plot(TimeW, Dist(:,1),'b--'); %  plot distance of first vehicle from leader 
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]);
xticks([0 10 20 30 40]);
hold off;

%%   Draw the positions for CACC
%    from StringCACC.m, figure 2, but here for CACC
%
figure(12)
S=lsim(Strings, W, TimeW, x0);
plot(TimeW, S(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
axis([0 Tend -20 400]);
latexylabel('$$s_i$$ in m');
rotateY;
xleer;
yticks([0 100 200 300 400 500]);
xticks([0 10 20 30 40]);
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre with CACC');
hold off;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CACC of vehicles with different individual delay
%   from StringCACC.m, figure 11 and 12
%
%  controlled vehicles are represented by PT2-elements
%  (IT1-element for vehicle, P-controller)
%  v_i = coupling output
%  s_i = control output
%
figure(13)
N1=6;     % Vehicles 0,1,..., 5
D0=1.5;   % individual delays
D1=2;
D2=2.5;
D3=3;
D4=2;
D5=3.5;
%   Leader modelled as PT1, but with order n=3 as all vehicles
A0=[-1/D0 0 0; 0 0 1; 1 0 0];
B0=[1/D0; 0; 0];
T1=0.7;           % 0.4  time constant of vehicle
kP1=1/D1;
A1=[0 1 0; -kP1/T1 -1/T1 0; 1 0 0];
B1=[0; kP1/T1; 0];
kP2=1/D2;
A2=[0 1 0; -kP2/T1 -1/T1 0; 1 0 0];
B2=[0; kP2/T1; 0];
kP3=1/D3;
A3=[0 1 0; -kP3/T1 -1/T1 0; 1 0 0];
B3=[0; kP3/T1; 0];
kP4=1/D4;
A4=[0 1 0; -kP4/T1 -1/T1 0; 1 0 0];
B4=[0; kP4/T1; 0];
kP5=1/D5;
A5=[0 1 0; -kP5/T1 -1/T1 0; 1 0 0];
B5=[0; kP5/T1; 0];
C=[0 0 1];     % output= distance
Cz=[1 0 0];    % output= velocity v_i
n=3;
%
si=alpha+beta*v0;   % initial spacing
x0=[v0; 0; 0; v0; 0; -si; v0; 0; -2*si; v0; 0; -3*si; v0; 0; -4*si; v0; 0; -5*si];
%   Determination of the weighting factors
a21=0.75;
a20=1-a21;
a32=0.5;
a31=1-a32;
a43=1;
a54=0.25;
a53=1-a54;
%   System with N1 agents in a row
Null=zeros(n,n);
Abar=[A0        Null      Null       Null       Null      Null;
      B1*Cz     A1        Null       Null       Null      Null;
      a20*B2*Cz a21*B2*Cz A2         Null       Null      Null;
      Null      a31*B3*Cz a32*B3*Cz  A3         Null      Null;
      Null      Null      Null       a43*B4*Cz  A4        Null;
      Null      Null      Null       a53*B5*Cz  a54*B5*Cz A5    ];
Bbar=[B0; zeros((N1-1)*n, 1)];
Cv=kron(eye(N1, N1), Cz);
Cd=kron(eye(N1, N1), C)-kron(diag(ones(N1-1,1),1), C); % output: d_i
Cs=kron(eye(N1, N1), C); % output: s_i
Stringv=ss(Abar, Bbar, Cv, 0);
Stringd=ss(Abar, Bbar, Cd, 0);
%
%  Test of time-headway
Sys=ss(Abar, Bbar, Cv, 0);
[Y, Time]=step(Sys);
trapz(Time,1-Y)
%
subplot(4,1,[1 2])
V=lsim(Stringv, W, TimeW, x0);
plot(TimeW, V(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
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]);
%xticks([0 10 20]);
%latexxlabel('$$t$$');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre');
hold off;
%
subplot(4,1,[3 4])
Dist=lsim(Stringd, W, TimeW, x0);
plot(TimeW, Dist(:,2:N1-1),'b');
hold on
plot(TimeW, Dist(:,1),'b--');
plot([0 0],[-100 100],'k:');
axis([-1 Tend 0 50]);
latexylabel('$$\tilde{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
%
figure(14)
%
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*G30;
G50=G5*(a53*G30+a54*G40);
Tend=30;
%
subplot(2,1,1)
%[Y, Time]=impulse(Integrator*(G00-G10));
%plot(Time, Y);
%hold on
[Y, Time]=impulse(Integrator*(G10-G20), Tend);
plot(Time, Y, 'b');
hold on
Y=impulse(Integrator*(G20-G30),Time);
plot(Time, Y, 'b');
Y=impulse(Integrator*(G30-G40),Time);
plot(Time, Y, 'b');
Y=impulse(Integrator*(G40-G50),Time);
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 PlatoonDemo:}$$ Test of collision avoidance');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   CACC of identical vehicles with collision avoidance
%    show that the vehicle is externally positive
%
%  Vehicle model structure is the same as in figure(5)
figure(16)
subplot(2,1,1)
%
D1=3;        % time-headway constant
T1=0.75;    % time constant of the power train (for larger T1, gbar is not positive)
            %  cf. condition T1 <= beta/4
kv=1/D1;     % feedback gain to get the delay D
Tend1=20;
A=[0 1; -kv/T1 -1/T1];
B=[0; kv/T1];
C=[1 0];   % output = velocity
D=0;
Vbar=ss(A, B, C, D);
[Y, Time]=impulse(Vbar, Tend1);
plot(Time,Y,'b', [0 Tend1], [0 0], 'k:')
axis([0 Tend1 -0.02 0.25]);
latexylabel('$$\bar{g}_i$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 0.1 0.2]);
xticks([0 10 20]);
hold off;
latextitle('$${\tt PlatoonDemo:}$$ Test the positivity of the controlled vehicle');
%  Test of time-headway
[Y, Time]=step(Vbar);
D1=trapz(Time,1-Y)


%%  Analysis of the platoon with CACC
%
figure(17)
beta=2;
D1=3;
%  overall system model as for figure(13)
A0=[-1/D1 0 0; 0 0 1; 1 0 0];
B0=[1/D1; 0; 0];
A1=[0 1 0; -kv/T1 -1/T1 0; 1 0 0];
B1=[0; kv/T1; 0];
C=[0 0 1];     % output= distance
Cz=[1 0 0];    % output= velocity v_i
n=3;
%
d1=alpha+D1*v0;
si=alpha+beta*v0;   % initial spacing
x0=[v0; 0; 0; v0; 0; -d1; v0; 0; -d1-si; v0; 0; -d1-2*si; v0; 0; -d1-3*si; v0; 0; -d1-4*si];
%   Determination of the weighting factors
a1=2-D1/beta;
a2=1-a1;
N1=6;
%   System with N1 agents in a row
Null=zeros(n,n);
AbarCACC=[A0        Null      Null       Null       Null      Null;
      B1*Cz     A1        Null       Null       Null      Null;
      a2*B1*Cz  a1*B1*Cz  A1         Null       Null      Null;
      Null      a2*B1*Cz  a1*B1*Cz   A1         Null      Null;
      Null      Null      a2*B1*Cz   a1*B1*Cz   A1        Null;
      Null      Null      Null       a2*B1*Cz   a1*B1*Cz   A1    ];
Bbar=[B0; zeros((N1-1)*n, 1)];
Cv=kron(eye(N1, N1), Cz);
Cd=kron(eye(N1, N1), C)-kron(diag(ones(N1-1,1),1), C); % output: d_i
Cs=kron(eye(N1, N1), C); % output: s_i
StringvCACC=ss(AbarCACC, Bbar, Cv, 0);
StringdCACC=ss(AbarCACC, Bbar, Cd, 0);
%
subplot(4,1,[1 2])
V=lsim(StringvCACC, W, TimeW, x0);
plot(TimeW, V(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
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 PlatoonDemo:}$$ Acceleration/braking manoeuvre');
hold off;
%
subplot(4,1,[3 4])
Dist=lsim(StringdCACC, W, TimeW, x0);
plot(TimeW, Dist(:,2:N1-1),'b');
hold on
plot(TimeW, Dist(:,1),'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;


%%   Draw the positions for CACC
%    similar to figure(12)
%
figure(18)
Cs=kron(eye(N1, N1), C); % output: s_i
StringsCACC=ss(AbarCACC, Bbar, Cs, 0);
S=lsim(StringsCACC, W, TimeW, x0);
plot(TimeW, S(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
plot([40 40],[S(2000,1) S(2000,N1)],'b', [40 40],[S(2000,1) S(2000,N1)],'bx');
axis([0 Tend -20 450]);
latexylabel('$$s_i$$ in m');
rotateY;
xleer;
yticks([0 100 200 300 400 500]);
xticks([0 10 20 30 40]);
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre with CACC');
hold off;


%%  Compare with the same behaviour with ACC
%
figure(19)
AbarACC=[A0        Null      Null       Null       Null      Null;
      B1*Cz     A1        Null       Null       Null      Null;
      Null      B1*Cz     A1         Null       Null      Null;
      Null      Null      B1*Cz      A1          Null     Null;
      Null      Null      Null       B1*Cz      A1        Null;
      Null      Null      Null       Null       B1*Cz     A1    ];
StringsACC=ss(AbarACC, Bbar, Cs, 0);
StringvACC=ss(AbarACC, Bbar, Cv, 0);
S=lsim(StringsACC, W, TimeW, x0);
plot(TimeW, S(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
plot([40 40],[S(2000,1) S(2000,N1)],'b', [40 40],[S(2000,1) S(2000,N1)],'bx');
axis([0 Tend -20 450]);
latexylabel('$$s_i$$ in m');
rotateY;
xleer;
yticks([0 100 200 300 400 500]);
xticks([0 10 20 30 40]);
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre with ACC');
hold off;

%%  Step response of the platoon
%
figure(20)
subplot(4,1,[1 2])
%   generation of the reference input to the leader
Num=conv([7 1], [5 1]);
Gd=tf([1], Num);
W1=5*step(Gd, TimeW);
V=lsim(StringvCACC, W1, TimeW, x0);
plot(TimeW, V(:,1:3),'b', [-3 0],[v0 v0],'b');
hold on
VDraw=[(V(:,2)+V(:,1))/2; flip(V(:,3))];
TDraw=[TimeW flip(TimeW)];
fill(TDraw, VDraw, [0.9 0.9 0.9]);
axis([-1 Tend -0.5 5]);
latexylabel('$$v_i$$ in m/s');
rotateY;
xleer;
yticks([0 2 4 6]);
xticksempty([0 10 20 30 40]);
%latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration manoeuvre with CACC and ACC');
%  Comparison with ACC
subplot(4,1,[3 4])
V=lsim(StringvACC, W1, TimeW, x0);
plot(TimeW, V(:,1),'b', [-3 0],[v0 v0],'b');
hold on
VDraw=[V(:,2); flip(V(:,3))];
TDraw=[TimeW flip(TimeW)];
fill(TDraw, VDraw, [0.9 0.9 0.9]);
plot(TimeW, V(:,3),'w');      % overwrite the lower curve
plot(TimeW, V(:,3),'b--');    % draw the lower curve as in the upper subplot
axis([-1 Tend -0.5 5]);
latexylabel('$$v_i$$ in m/s');
rotateY;
yticks([0 2 4 6]);
xticks([0 10 20 30 40]);
latexxlabel('$$t$$ in s');
hold off;
%
subplot(4,1,[1 2])
plot(TimeW, V(:,3),'b--');
hold off


%%   Corrected CACC for uniform vehicle distance
%    figure(17) shows different vehicle distances
%    the CACC coefficients are corrected so that the distances are beta
%    then the collision avoidance test has to be repeated
figure(21)
%
%   System with N1 agents in a row
a21=beta/D1;
a20=1-a21;
AbarCACC=[A0        Null      Null       Null       Null      Null;
      B1*Cz     A1        Null       Null       Null      Null;
      a20*B1*Cz a21*B1*Cz  A1         Null       Null      Null;
      Null      a2*B1*Cz  a1*B1*Cz   A1         Null      Null;
      Null      Null      a2*B1*Cz   a1*B1*Cz   A1        Null;
      Null      Null      Null       a2*B1*Cz   a1*B1*Cz   A1    ];
StringvCACC=ss(AbarCACC, Bbar, Cv, 0);
StringdCACC=ss(AbarCACC, Bbar, Cd, 0);
%
% subplot(2,1,1)
% V=lsim(StringvCACC, W, TimeW, x0);
% plot(TimeW, V(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
% hold on
% 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]);
% %xticks([0 10 20]);
% %latexxlabel('$$t$$');
% latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre');
% hold off;
%
subplot(4,1,[1 2])
Dist=lsim(StringdCACC, W, TimeW, x0);
plot(TimeW, Dist(:,2:N1-1),'b');
hold on
plot(TimeW, Dist(:,1),'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;


%%   Draw the positions for CACC
%    similar to figure(18)
%
figure(22)
StringsCACC=ss(AbarCACC, Bbar, Cs, 0);
S=lsim(StringsCACC, W, TimeW, x0);
plot(TimeW, S(:,1:N1),'b', [-3 0],[v0 v0],'b', [0 0],[25 0],'k:');
hold on
plot([40 40],[S(2000,1) S(2000,N1)],'b', [40 40],[S(2000,1) S(2000,N1)],'bx');
axis([0 Tend -20 450]);
latexylabel('$$s_i$$ in m');
rotateY;
xleer;
yticks([0 100 200 300 400 500]);
xticks([0 10 20 30 40]);
latexxlabel('$$t$$ in s');
latextitle('$${\tt PlatoonDemo:}$$ Acceleration/braking manoeuvre with CACC');
hold off;
S(2000,1)-S(2000,N1)


%%  Test of collision avoidance condition
%   as in figure(14)
figure(23)
%
G0=tf(1,1);
G1=tf(Vbar);
G2=G1;
G3=G1;
G4=G1;
G5=G1;
Integrator=tf(1,[1 0]);
G00=1;
G10=G1;
G20=G2*(a20+a21*G10);
G30=G3*(a1*G20+a2*G10);
G40=G4*(a1*G30+a2*G20);
G50=G5*(a1*G40+a2*G30);
%
subplot(4,1,[1 2])
%[Y, Time]=impulse(Integrator*(G00-G10));
%plot(Time, Y);
%hold on
[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 PlatoonDemo:}$$ Test of collision avoidance');

%%  figure(15) after figure(3)
%   figure(24) after figure(6)

%%
%
epsfigc15('PlatoonDemo');