%      DelayGMSC.M
%
%   Properties of the delay measure
% 
%
% J. Lunze
% 18.9.2023
%
% Version of 24.9.2023
% 
close all
clear all


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  More information makes the performance worse
%
figure(1)
%   yref(t): sprungförmige Führungsgröße
%
%   a) 0 -> 4, 6, 7;    6 -> 9;   4 -> 7;   
%   b) 0 -> 4, 6, 7;    6 -> 9;   4 -> 7;   7 -> 9
%  wobei Nummerierung der Agenten 0...5 zu den Indizes 1...6 gehört
%      Agent:  0  4  6  7  9
%          i=  1  2  3  4  5
T1=1;
GPT1=tf([1],[T1 1]);
Tend=8;
%  couping structure a)
[Y1,Time]=step(GPT1*GPT1,Tend);
subplot(2,1,1)
plot(Time, Y1, 'b');
hold on
axis([-0.3 Tend -0.1 1.1]);
plot([-3 0 0 Tend],[0 0 1 1],'b');
latextext(0.3, 0.8, '$$y_{\rm ref}$$');
latexxlabel('$$t$$');    
latexylabel('$$y_9\;$$');    
rotateY;
latextitle('$${\tt DelayGMSC:}$$ Unidirectional neighbouring couplings');
yticks([0 5 10]);
xticks([0 2 4 6 8]);
%  Coupling structure b)
Y2=step(GPT1*GPT1*GPT1,Time);
plot(Time, 0.5*(Y1+Y2), 'b--');
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Robot formation problem
%   from  DelayDemo.m in NCSAufl2
%   
%  new robot parameters
%  Vehicle model with velocity and position control loops
m1=10;           % m= 10 kg  mass
m2=15;
c=0.2;          %  c=1kg/s friction constant
                % forces in N,  hence input  u=acceleration in N
                % time in seconds
                % distance in m
                % velocity in m/s,   
                %         hence output  v=velocity in m/s
%               PI velocity controller
kI=5;          % kI=5
kP=14;          % kP=10
%               Controlled vehicle
%               input = set-point for velocity controller
%               output = velocity
Ac1=[-c/m1-kP/m1 -kI/m1; 1 0];
Bc1=[kP/m1; -1];
Ac2=[-c/m2-kP/m2 -kI/m2; 1 0];
Bc2=[kP/m2; -1];
Cc=[1 0];
Dc=0;
na=2;            % dynamical order of vehicle with velocity controller
n=na;
%
%
%  Vehicle with output=position
A1=[0 Cc; zeros(na,1) Ac1];
B1=[0; Bc1];
A2=[0 Cc; zeros(na,1) Ac2];
B2=[0; Bc2];
C=[1 zeros(1,na)];
D=0;
Robot1=ss(A1, B1, C, D);
Robot2=ss(A2, B2, C, D);



%% Design of the feedback gain k
%
figure(2)
subplot(2,2,1);
rlocprintblue(Robot1);
hold on
axis([-1 0.2 -0.6 0.6]);
axis('square');
latextitle('$${\tt DelayDemo:}$$ Root locus of controlled vehicle');
% kP=rlocfind(vehicle);
kP1=0.3;          % feedback gain 0.03
Poles=rlocus(Robot1, kP1);
plot(real(Poles(1)), imag(Poles(1)), 'kd');
plot(real(Poles(2)), imag(Poles(2)), 'kd');
plot(real(Poles(3)), imag(Poles(3)), 'kd');
yticks([-0.5 0 0.5]);
xticks([-1 -0.5 0]);
hold off
%
subplot(2,2,2)
rlocprintblue(Robot2);
hold on
axis([-1 0.2 -0.6 0.6]);
axis('square');
% kP=rlocfind(vehicle);
kP2=0.25;          % feedback gain 0.03
Poles=rlocus(Robot2, kP2);
plot(real(Poles(1)), imag(Poles(1)), 'kd');
plot(real(Poles(2)), imag(Poles(2)), 'kd');
plot(real(Poles(3)), imag(Poles(3)), 'kd');
yticks([-0.5 0 0.5]);
xticks([-1 -0.5 0]);
hold off

%%   Delay of controlled vehicle
% 
%   D = int_0^\infty (ks - h(t)) dt
figure(3)
subplot(4,1,[1 2]);
%  Step response of the controlled vehicle
Abar1=A1-kP1*B1*C;
Bbar1=kP1*B1;
Cbar=C;
Dbar=D;
n=na+1;
contrVehicle1=ss(Abar1, Bbar1, Cbar, Dbar);
Tsim=25;      % simulation time long enough to determine the delay with 
              % sufficient accuracy
dt=0.005;
Time=0:dt:Tsim;
Y1=step(contrVehicle1,Time);
fill([Time 0 0],[Y1' 1 0],[0.92 0.92 0.92]);
hold on
plot(Time, Y1,'b');
latexylabel('$$y_i$$ in m');
rotateY;
xleer;
latextitle('$${\tt DelayDemo:}$$ Step response of the controlled vehicle');
axis([0 15 0 1.1]);
hold on
ks=dcgain(contrVehicle1);
plot([0 Tsim],[ks ks],'k:');
% 
%  Determination of the delay
DVeh1=trapz(1-Y1)*dt;
latextext(0.5, 0.8, ['$$\Delta= $$', num2str(DVeh1,'%.3g')]);
yticks([0 0.5 1]);
xticksempty([0 5 10 15 20]);
hold off;
%
subplot(4,1,[3 4]);
%  Step response of the controlled vehicle
Abar2=A2-kP2*B2*C;
Bbar2=kP2*B2;
Cbar=C;
Dbar=D;
n=na+1;
contrVehicle2=ss(Abar2, Bbar2, Cbar, Dbar);
Tsim=25;      % simulation time long enough to determine the delay with 
              % sufficient accuracy
dt=0.005;
Time=0:dt:Tsim;
Y2=step(contrVehicle2,Time);
fill([Time 0 0],[Y2' 1 0],[0.92 0.92 0.92]);
hold on
plot(Time, Y2,'b');
latexylabel('$$y_i$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 15 0 1.1]);
hold on
ks=dcgain(contrVehicle2);
plot([0 Tsim],[ks ks],'k:');
% 
%  Determination of the delay
DVeh2=trapz(1-Y2)*dt;
latextext(0.5, 0.8, ['$$\Delta= $$', num2str(DVeh2,'%.3g')]);
yticks([0 0.5 1]);
xticks([0 5 10 15 20]);
hold off;


%%  Vehicles with all possible couplings
%
%   yref=1;
%   all vehicles initially at s_i(0)=0;  v_i(0)=0;
figure(4);
N1=5;
subplot(6,1,[1,2])
MatNull=zeros(n,n);
AF=[Abar1 MatNull MatNull MatNull MatNull;
    0.5*Bbar1*Cbar Abar1 MatNull MatNull MatNull;
    0.5*Bbar2*Cbar 0.5*Bbar2*Cbar Abar2 MatNull MatNull;
    MatNull   0.5*Bbar1*Cbar  0.5*Bbar1*Cbar Abar1 MatNull
    MatNull MatNull 0.5*Bbar1*Cbar 0.5*Bbar1*Cbar Abar1];
BF=[Bbar1
    0.5*Bbar1
    zeros(3*n,1)];
CF=kron(diag(ones(N1,1)), C);
DF=zeros(N1,1);
VehicleRow=ss(AF, BF, CF, DF);
Tend=30;
[Y, Time]=step(VehicleRow, Tend);
plot(Time, Y, 'b');
hold on
axis([0 Tend 0 1.1]);
%latexxlabel('$$t$$ in s');
xleer;
latexylabel('$$y_i$$ in m');
rotateY;
latextitle('$${\tt DelayDemo:}$$  Unidirectional neighbouring couplings');
%   Marking of the delay:  D_{i, ref} = i*DVeh
plot(DVeh1, 0.63, 'kx'); 
plot(1.5*DVeh1, 0.63, 'kx'); 
plot(1.25*DVeh1+DVeh2, 0.63, 'kx'); 
plot(2.36*DVeh1+0.5*DVeh2, 0.63, 'kx'); 
plot(2.8*DVeh1+0.75*DVeh2, 0.63, 'kx'); 
yticks([0 0.5 1]);
xticksempty([0 5 10 15 20 25 30]);
hold off
 


%%  Vehicles with communication structure b)
%
%  adjacency matrix:
figure(4)
subplot(6,1,[3 4])
AF=[Abar1 MatNull MatNull MatNull MatNull;
    Bbar1*Cbar Abar1 MatNull MatNull MatNull;
    MatNull Bbar2*Cbar Abar2 MatNull MatNull;
    MatNull Bbar1*Cbar MatNull Abar1 MatNull
    MatNull MatNull Bbar1*Cbar MatNull Abar1];
BF=[Bbar1;
    zeros(n,1);
    zeros(3*n,1)];
CF=kron(diag(ones(N1,1)), C);
DF=zeros(N1,1);
VehicleRow=ss(AF, BF, CF, DF);
Tend=30;
[Y, Time]=step(VehicleRow, Tend);
plot(Time, Y, 'b');
hold on
axis([0 Tend 0 1.1]);
%latexxlabel('$$t$$ in s');
xleer;
latexylabel('$$y_i$$ in m');
rotateY;
%   Marking of the delay:  D_{i, ref} = i*DVeh
plot(DVeh1, 0.63, 'kx'); 
plot(2*DVeh1, 0.63, 'kx'); 
plot(2*DVeh1+DVeh2, 0.63, 'kx'); 
plot(3*DVeh1, 0.63, 'kx'); 
plot(3*DVeh1+DVeh2, 0.63, 'kx'); 
yticks([0 0.5 1]);
xticksempty([0 5 10 15 20 25 30]);
hold off
 

%%  Vehicles with communication structure c)
%
%  adjacency matrix:
figure(4)
subplot(6,1,[5 6])
AF=[Abar1 MatNull MatNull MatNull MatNull;
    MatNull Abar1 MatNull MatNull MatNull;
    Bbar2*Cbar MatNull Abar2 MatNull MatNull;
    MatNull Bbar1*Cbar MatNull Abar1 MatNull
    MatNull MatNull Bbar1*Cbar MatNull Abar1];
BF=[Bbar1;
    Bbar1;
    zeros(3*n,1)];
CF=kron(diag(ones(N1,1)), C);
DF=zeros(N1,1);
VehicleRow=ss(AF, BF, CF, DF);
Tend=30;
[Y, Time]=step(VehicleRow, Tend);
plot(Time, Y, 'b');
hold on
axis([0 Tend 0 1.1]);
latexxlabel('$$t$$ in s');
latexylabel('$$y_i$$ in m');
rotateY;
%   Marking of the delay:  D_{i, ref} = i*DVeh
plot(DVeh1, 0.63, 'kx'); 
plot(DVeh1, 0.63, 'kx'); 
plot(DVeh1+DVeh2, 0.63, 'kx'); 
plot(2*DVeh1, 0.63, 'kx'); 
plot(2*DVeh1+DVeh2, 0.63, 'kx'); 
yticks([0 0.5 1]);
xticks([0 5 10 15 20 25 30]);
hold off
 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  PT_n systems with identical time constants T1
%   What is the system output at the time t=Delta
%   Delta = T1 for all systems considered
%   DelayDemo1.eps  in NCSAufl2
%
figure(5)
subplot(2,1,1)
T1=1;
Delta=T1;
N=10;   % highest dynamical order
dt=0.01;  % simulation time step
Time=0:dt:5*T1;
kT=Delta/dt;      % Number of time point at which t=Delta
yD=[];         % y(kT) - output value at time t=D
nD=[];         % n - dynamical order (number of time constants)
for k1=1:N
    PT1Syst=tf(1,[T1/k1,1]);
    System=1;
    for k2=1:k1
        System=System*PT1Syst;     % series connection expands the system by one PT1-system
    end
    [Y, Time]=step(System, Time);
    nD=[nD; k1];               % dynamical order of system
    yD=[yD; Y(kT)];            % output at time t=n*T1
    plot(Time, Y, 'b');
    hold on
end
plot([1 1],[-1 2],'k--', [0 5], [1 1], 'k:');
latextitle('$${\tt DelayGMSC:}$$ PT$$_n$$ system with $$T_1=1$$');
latextext(0.9, -0.2, '$$\Delta$$');
axis([0 5 0 1.1]);
latexylabel('$$y\;$$');
rotateY;
latexxlabel('$$t$$');
yticks([0 0.5 1]);
xticks([0 2 4 6]);
hold off
%
subplot(2,1,2)
plot(nD, yD, 'b', nD, yD, 'bo');
axis([0 10 0 0.8]);
latexylabel('$$y(\Delta)$$');
rotateY;
latexxlabel('$$n$$');
latextext(0.5, 0.45, '0.63');
latextext(8.5, 0.7, '0.54');
yticks([0 0.5 1]);
xticks([0 2 4 6 8 10]);

%%  PT_2 systems with two different time constants T1, T2
%   What is the output value y(T1+T2) of the system at time t=D=T1+T2?
%   D = T1+T2?
figure(6)
T1=1;
T2min=0.01;
T2max=5;
dT2=(T2max-T2min)/50;
dt=0.001;  % simulation time step
Time=0:dt:(T1+T2max);
System1=tf(1,[T1 1]);
yD=[];
T2set=[];           % T2 for all scenarios
for T2=T2min:dT2:T2max
    System2=tf(1, [T2, 1]);
    System=System1*System2;     % series connection 
    [Y, Time]=step(System, Time);
    kT=ceil((T1+T2)/dt);       % Number of time point at which t=T1+T2
    T2set=[T2set; T2];         % time constant T2
    yD=[yD; Y(kT)];            % output at time t=T1+T2
end
subplot(2,1,1)
plot(T2set, yD, 'b');
latexylabel('$$y(\Delta)$$');
rotateY;
latexxlabel('$$T_2$$');
latextitle('$${\tt DelayGMSC:}$$ PT$$_2$$ system with $$T_1=1$$ and variable $$T_2$$');
yticks([0.58 0.60 0.62 0.64]);
xticks([0 1 2 3 4 5]);
hold on
%
%   T1=T2
System=System1*System1;
[Y, Time]=step(System, Time);
kT=ceil((2*T1)/dt);
plot(1, Y(kT), 'bo');
axis([0 5 0.58 0.635]);
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Vehicle swarm
%
figure(7)
subplot(2,1,1)
T1=0.8;   % T1 + T2 = beta = 2
T2=1.2;
AVeh=[-1/T1 0 0; 1/T2 -1/T2 0; 0 -1 0];
BACC=[1/T1; 0; 1];      % input v_si=v_{i-1}
BCACC=[1/T1; 0; 0];     % input v_si
Bd=[0; 0; 1];           % input v_{i-1}
CVeh=[0 1 0];
CDist=[0 0 1];
nVeh=3;
Follower=ss(AVeh,BACC,CVeh,0);
FollowerDist=ss(AVeh,BACC,CDist,0);
%   leader
ALead=[-1/T1 0; 1/T2 -1/T2];
BLead=[1/T1; 0];
CLead=[0 1];
nLead=2;
Leader=ss(ALead,BLead,CLead,0);
%  ACC vehicle platoon
Abar=[ALead              zeros(nLead,3*nVeh)
      BACC*CLead         AVeh             zeros(nVeh,2*nVeh)
      zeros(nVeh,nLead)  BACC*CVeh        AVeh            zeros(nVeh,nVeh)
      zeros(nVeh,nLead)  zeros(nVeh,nVeh) BACC*CVeh       AVeh];
Bbar=[BLead;
       zeros(3*nVeh,1)];
Cbar=[CLead zeros(1,3*nVeh);
      zeros(1,nLead) CVeh  zeros(1,2*nVeh)
      zeros(1,nLead+nVeh) CVeh zeros(1,nVeh)
      zeros(1,nLead+2*nVeh) CVeh];
ACCPlatoon=ss(Abar, Bbar, Cbar, 0);
[Y,Time,X]=step(ACCPlatoon);
plot(Time, Y, 'b', [0 50],[1 1],'k:');
latexylabel('$$v_i\;$$');
rotateY;
latexxlabel('$$t$$ in s');
latextitle('$${\tt DelayGMSC:}$$ Vehicle platoon with ACC');
yticks([0 0.5 1]);
xticks([0 10 20 30 40 50]);
axis([0 20 0 1.1]);
%
%   Model of the vehicle distances
CbarDist=[
      zeros(1,nLead) CDist  zeros(1,2*nVeh)
      zeros(1,nLead+nVeh) CDist zeros(1,nVeh)
      zeros(1,nLead+2*nVeh) CDist];
ACCPlatoonDist=ss(Abar, Bbar, CbarDist, 0);
%  Determination of the delay
Y1=Y(:,1);
dt=Time(2)-Time(1);
DVeh1=trapz(1-Y1)*dt;
%latextext(0.5, 0.8, ['$$\Delta= $$', num2str(DVeh2,'%.3g')]);

%%  Vehicle platoon with ACC
% Reference trajectory for the leader vehicle
%  from  Fehlermodelle.m   LAURIN
figure(8)
subplot(4,1,[1,2]);
%  Sollgeschwindigkeit in m/s
        v0=0;   % Stillstand
t1=2;           % Beginn des Beschleunigungsvorgangs
        v1=10;  % v0 ... v1
t2=10;           % Geschwindigkeit v1 erreicht
t3=30;           % weiter mit konstanter Geschwindigkeit v1
        v3=25;  % Beschleunigung v1...v3
t4=50;          % Geschwindigkeit v3 erreicht
                % konstante Geschwindigkeit
t5=75;          % Beginn des Abbremsvorgangs auf v6
        v6=18;
t6=95;          % v6 erreicht
                % konstante Geschwindigkeit
t7=118;          % Beginn des Abbremsvorgangs auf v7
        v7=9;
t8=127;          % v7 erreicht, weiteres Abbremsen
        v9=0;
t9=152;          % Stillstand erreicht
Tend=170;
dt=0.1;
%  Umrechnung in Zeitschritte der Simulation
k1=ceil(t1/dt);
k2=ceil(t2/dt);
k3=ceil(t3/dt);
k4=ceil(t4/dt);
k5=ceil(t5/dt);
k6=ceil(t6/dt);
k7=ceil(t7/dt);
k8=ceil(t8/dt);
k9=ceil(t9/dt);
kend=ceil(Tend/dt);
W=[v0*ones(k1,1);
   v0+(v1-v0)*[1:k2-k1]'/(k2-k1);
   v1*ones(k3-k2,1);
   v1+(v3-v1)*[1:k4-k3]'/(k4-k3);
   v3*ones(k5-k4,1);
   v3-(v3-v6)*[1:k6-k5]'/(k6-k5);
   v6*ones(k7-k6,1);
   v6-(v6-v7)*[1:k8-k7]'/(k8-k7);
   v7-v7*[1:k9-k8]'/(k9-k8);
   0*ones(kend-k9,1);
   ];%
wbar=max(W);
Time=0:dt:(length(W)-1)*dt;
plot(Time,3.6*W,'b--');
hold on
%
[V,Time,X]=lsim(ACCPlatoon,W,Time);
plot(Time, 3.6*V, 'b');
latexylabel('$$v_i\;$$ in km/h');
rotateY;
xleer;
latextitle('$${\tt DelayGMSC:}$$ Vehicle platoon with ACC');
yticks([0 30 60 90]);
xticksempty([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 0 100]);
hold off
%
subplot(4,1,[3,4]);
x0=[0;0; 0;0;2; 0;0;2; 0;0;2];
[DACC,Time,X]=lsim(ACCPlatoonDist,W,Time,x0);
plot(Time, DACC, 'b');
latexylabel('$$d_i\;$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 -10 60]);


%%  Vehicle platton with CACC (first version)
%
figure(9)
%  CACC vehicle platoon
Abar=[ALead              zeros(nLead,7*nVeh)
      BCACC*CLead         AVeh               zeros(nVeh,6*nVeh)
      BCACC*CLead         zeros(nVeh,nVeh)   AVeh             zeros(nVeh,5*nVeh)
      BCACC*CLead         zeros(nVeh,2*nVeh) AVeh             zeros(nVeh,4*nVeh)
      zeros(nVeh,nLead)  BCACC*CVeh          zeros(nVeh,2*nVeh)  AVeh   zeros(nVeh,3*nVeh)
      zeros(nVeh,nLead)  1/3*BCACC*CVeh 1/3*BACC*CVeh 1/3*BCACC*CVeh zeros(nVeh,nVeh) AVeh zeros(nVeh,2*nVeh)
      zeros(nVeh,nLead)  zeros(nVeh,3*nVeh) 0.5*BCACC*CVeh 0.5*BCACC*CVeh AVeh zeros(nVeh,nVeh)
      zeros(nVeh,nLead)  zeros(nVeh,5*nVeh) BCACC*CVeh AVeh ];
Bbar=[BLead;
      zeros(7*nVeh,1)];
Cbar=[CLead zeros(1,7*nVeh);
      zeros(1,nLead)        CVeh   zeros(1,6*nVeh)
      zeros(1,nLead+nVeh)   CVeh   zeros(1,5*nVeh)
      zeros(1,nLead+2*nVeh) CVeh   zeros(1,4*nVeh)
      zeros(1,nLead+3*nVeh) CVeh   zeros(1,3*nVeh)
      zeros(1,nLead+4*nVeh) CVeh   zeros(1,2*nVeh)
      zeros(1,nLead+5*nVeh) CVeh   zeros(1,nVeh)
      zeros(1,nLead+6*nVeh) CVeh   ];
CACCPlatoon=ss(Abar, Bbar, Cbar, 0);
%   Model of the vehicle distances: integrators with input V
AbarDist=zeros(7,7);
BbarDist=[1 -1 0 0 0 0 0 0   % d_2
          1 0 -1 0 0 0 0 0   % d_3
          1 0 0 -1 0 0 0 0
          0 1 0 0 -1 0 0 0
          0 0 1 0 0 -1 0 0
          0 0 0 0 1 0 -1 0
          0 0 0 0 0 0 1 -1];
CbarDist=eye(7,7);
CACCPlatoonDist=ss(AbarDist, BbarDist, CbarDist, 0);
%
subplot(4,1,[1 2]);
[V,Time,X]=lsim(CACCPlatoon,W,Time);
plot(Time,3.6*W,'b--');
hold on
plot(Time, 3.6*V, 'b');
latexylabel('$$v_i\;$$ in km/h');
rotateY;
xleer;
latextitle('$${\tt DelayGMSC:}$$ Vehicle platoon with CACC');
yticks([0 30 60 90]);
xticksempty([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 0 100]);
hold off
%
subplot(4,1,[3,4]);
x0=2*ones(7,1);
[DCACC1,Time,X]=lsim(CACCPlatoonDist,V,Time,x0);
plot(Time, DCACC1, 'b');
latexylabel('$$d_i\;$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 -10 60]);

%%  Vehicle platton with CACC (second version)
% 
figure(10)
%  CACC vehicle platoon
Abar=[ALead              zeros(nLead,7*nVeh)
      BACC*CLead         AVeh               zeros(nVeh,6*nVeh)
      BACC*CLead         zeros(nVeh,nVeh)   AVeh             zeros(nVeh,5*nVeh)
      BACC*CLead         zeros(nVeh,2*nVeh) AVeh             zeros(nVeh,4*nVeh)
      0.5*BACC*CLead     0.5*BACC*CVeh          zeros(nVeh,2*nVeh)  AVeh   zeros(nVeh,3*nVeh)
      0.5*BACC*CLead     zeros(nVeh,nVeh)   0.5*BACC*CVeh zeros(nVeh,2*nVeh) AVeh zeros(nVeh,2*nVeh)
      zeros(nVeh,nLead)  0.5*BACC*CVeh      zeros(nVeh,2*nVeh)   0.5*BACC*CVeh zeros(nVeh,nVeh) AVeh zeros(nVeh,nVeh)
      zeros(nVeh,nLead)  zeros(nVeh,5*nVeh) BACC*CVeh AVeh ];
Bbar=[BLead;
      zeros(7*nVeh,1)];
Cbar=[CLead zeros(1,7*nVeh);
      zeros(1,nLead) CVeh         zeros(1,6*nVeh)
      zeros(1,nLead+nVeh)  CVeh   zeros(1,5*nVeh)
      zeros(1,nLead+2*nVeh) CVeh  zeros(1,4*nVeh)
      zeros(1,nLead+3*nVeh) CVeh  zeros(1,3*nVeh)
      zeros(1,nLead+4*nVeh) CVeh  zeros(1,2*nVeh)
      zeros(1,nLead+5*nVeh) CVeh  zeros(1,nVeh)
      zeros(1,nLead+6*nVeh) CVeh   ];
CACCPlatoon=ss(Abar, Bbar, Cbar, 0);
%
subplot(4,1,[1 2]);
[V,Time,X]=lsim(CACCPlatoon,W,Time);
plot(Time,3.6*W,'b--');
hold on
plot(Time, 3.6*V, 'b');
latexylabel('$$v_i\;$$ in km/h');
rotateY;
xleer;
latextitle('$${\tt DelayGMSC:}$$ Vehicle platoon with CACC');
yticks([0 30 60 90]);
xticksempty([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 0 100]);
hold off
%
subplot(4,1,[3,4]);
[DCACC2,Time,X]=lsim(CACCPlatoonDist,V,Time,x0);
plot(Time, DCACC2, 'b');
latexylabel('$$d_i\;$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 -10 60]);


%%  Comparison of the vehicle distances
%
figure(11)
subplot(6,1,[1,2]);
plot(Time, DCACC1(:,4), 'b--');    % Vehicle 5:  d_5
hold on
plot(Time, DCACC2(:,4), 'b');
latexylabel('$$d_5\;$$ in m');
rotateY;
xleer;
latextitle('$${\tt DelayGMSC:}$$ Vehicle distances with ACC and CACC');
yticks([0 20 40 60]);
xticksempty([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 0 60]);
hold off
%
subplot(6,1,[3 4]);
plot(Time, DCACC1(:,5), 'b--');
hold on
plot(Time, DCACC2(:,5), 'b');
latexylabel('$$d_6\;$$ in m');
rotateY;
xleer;
yticks([0 20 40 60]);
xticksempty([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 0 60]);
hold off
%
subplot(6,1,[5 6]);
plot(Time, DCACC1(:,6), 'b--');
hold on
plot(Time, DCACC2(:,6), 'b');
latexylabel('$$d_7\;$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 20 40 60]);
xticks([0 20 40 60 80 100 120 140 160 180]);
axis([0 170 -10 60]);
hold off




%%
%
epsfigc15('DelayGMSC');


