%     SyncIMPDemo.m
%
%   Demonstration of Internal Reference Principle
%   
% J. Lunze  
% 11.02.2018
% version of 1.10.2018
% for 2nd edition: 19.2.2021
%
echo off
clear
close all
%%  3 Agents with different dynamics
%   from \Vernetzte Systeme\MATLAB\IntRefPrinc.m
%
omega=1;
delta=0;
Tend=40;
%  System 1
A1=[delta omega  0;
   -omega delta  1;
    0       0    0];
B1=[10; 10; 5];
C1=[1 0 0];
D1=0;
n1=3;
Sys1=ss(A1, B1, C1, D1);
subplot(3,1,1);
[Y1, T1]=impulse(Sys1);
plot(T1, Y1);
axis([0 Tend -15 20]);
xleer;
latexylabel('$$y_1$$');
rotateY;
%  System 2
A2=[0];
B2=[10];
C2=[1];
D2=0;
n2=1;
Sys2=ss(A2, B2, C2, D2);
subplot(3,1,2);
[Y2, T2]=impulse(Sys2, 70);
plot(T2, Y2);
axis([0 Tend -15 20]);
latexylabel('$$y_2$$');
xleer;
rotateY;
%
%  System 3
A3=[delta omega;
   -omega delta];
B3=[10; 10];
C3=[1 0];
D3=0;
n3=2;
Sys3=ss(A3, B3, C3, D3);
subplot(3,1,3);
[Y3, T3]=impulse(Sys3);
plot(T3, Y3);
axis([0 Tend -15 20]);
latexxlabel('$$t$$');
latexylabel('$$y_3$$');
rotateY;
%


%%   Synchronisability analysis  
%   
figure(2)
%  System 2
subplot(1,3,1)
rlocprintblue(Sys1);
axis('square');
axis([-2.5 0.5 -1.5 1.5]);
%  System 3
subplot(1,3,2)
rlocprintblue(Sys2);
axis('square');
axis([-2.5 0.5 -1.5 1.5]);
%  System 4
subplot(1,3,3)
rlocprintblue(Sys3);
axis('square');
axis([-2.5 0.5 -1.5 1.5]);


%%   Controller selection for System 1
%   
figure(3)
rlocprintblue(Sys1);
hold on
axis([-2.5 0.5 -1.5 1.5]);
axis('square');
%rlocfind(Sys2);
k1=0.4;   % best feedback
Poles=rlocus(Sys1, k1);
plot(real(Poles(1)), imag(Poles(1)), 'd');
plot(real(Poles(2)), imag(Poles(2)), 'd');
plot(real(Poles(3)), imag(Poles(3)), 'd');
k1=0.37;  %  feedback to demonstrate (slow) synchronisation
Poles=rlocus(Sys1, k1);
plot(real(Poles(1)), imag(Poles(1)), 'x');
plot(real(Poles(2)), imag(Poles(2)), 'x');
plot(real(Poles(3)), imag(Poles(3)), 'x');
hold off



%%   Controller selection for System 3 
%   
figure(4)
rlocprintblue(Sys3);
hold on
axis([-2.5 0.5 -1.5 1.5]);
axis('square');
%rlocfind(Sys3);
k3=0.4;    % best feedback
Poles=rlocus(Sys3, k3);
plot(real(Poles(1)), imag(Poles(1)), 'd');
plot(real(Poles(2)), imag(Poles(2)), 'd');
k3=0.10;   %  feedback to demonstrate (slow) synchronisation
Poles=rlocus(Sys3, k3);
plot(real(Poles(1)), imag(Poles(1)), 'x');
plot(real(Poles(2)), imag(Poles(2)), 'x');
hold off


%%   Controller selection for System 2 
%   
figure(5)
rlocprintblue(Sys2);
hold on
axis([-5 0.5 -2.75 2.75]);
axis('square');
%rlocfind(Sys2);
k2=0.4; 
Poles=rlocus(Sys2, k2);
plot(real(Poles(1)), imag(Poles(1)), 'd');
hold off

%%  Synchronisation of System 1 and System 3
figure(6)
Tend=15;
A13=[A1-B1*k1*C1  B1*k1*C3;
     B3*k3*C1    A3-B3*k3*C3];
B13=[B1; B3];
C13=[C1 zeros(1,n3);
     zeros(1,n1) C3;
     C1 -C3];
D13=zeros(3,1);
Sys13=ss(A13, B13, C13, D13);
x10=10*[1; 1; 1];
x30=10*[-1; -1];
x130=[x10; x30];
[Y13, T13]=initial(Sys13, x130, Tend);
subplot(3,2,1);
plot(T13, Y13(:,1), 'b', T13, Y13(:,2),'b--');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend, -15 15]);
xleer;
latexylabel('$$y_1, y_3$$');
rotateY;
yticks([-10 0 10]);
%
subplot(3,2,3);
plot(T13, Y13(:,3),'b');
axis([0 Tend -1 15]);
latexxlabel('$$t$$');
latexylabel('$$y_1-y_3$$');
rotateY;
yticks([0 10]);
yticks([0 10 20 30])


%%  Synchronisation of System 1 and System 2
figure(7)
Tend=15;
A12=[A1-B1*k1*C1  B1*k1*C2;
     B2*k2*C1    A2-B2*k2*C2];
B12=[B1; B2];
C12=[C1 zeros(1,n2);
     zeros(1,n1) C2;
     C1 -C2];
D12=zeros(3,1);
Sys12=ss(A12, B12, C12, D12);
x20=10*[-1];
x120=[x10; x20];
[Y12, T12]=initial(Sys12, x120, Tend);
subplot(3,2,1);
plot(T12, Y12(:,1), 'b', T12, Y12(:,2),'b--');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend, -10 10]);
xleer;
latexylabel('$$y_1, y_2$$');
rotateY;
yticks([-10 0 10]);
%
subplot(3,2,3);
plot(T12, Y12(:,3),'b');
axis([0 Tend -1 15]);
latexxlabel('$$t$$');
latexylabel('$$y_1-y_2$$');
rotateY;
yticks([0 10]);
yticks([0 10 20 30])

%%  Synchronisation of System 2 and System 3
figure(8)
Tend=30;
A32=[A3-B3*k3*C3  B3*k3*C2;
     B2*k2*C3    A2-B2*k2*C2];
B32=[B3; B2];
C32=[C3 zeros(1,n2);
     zeros(1,n3) C2;
     -C3 C2];
D32=zeros(3,1);
Sys32=ss(A32, B32, C32, D32);
x320=[x30; -x20];
[Y32, T32]=initial(Sys32, x320, Tend);
subplot(3,2,1);
plot(T32, Y32(:,1),'b', T32, Y32(:,2),'b--');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend, -10 10]);
xleer;
latexylabel('$$y_2, y_3$$');
rotateY;
yticks([-10 0 10]);
%
subplot(3,2,3);
plot(T32, Y32(:,3),'b');
axis([0 Tend -3 15]);
latexxlabel('$$t$$');
latexylabel('$$y_2-y_3$$');
rotateY;
yticks([0 10]);
yticks([0 10 20 30])


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Three oscillators with different dynamics
%
figure(9)
T1=1;
T2=2;
T3=3;
omega=3;
As=[0 omega; -omega 0];
Bs=[1; 1];
Cs=[1 10];
ns=2;
A1=[As Bs;
    zeros(1,ns) -1/T1];
B1=[zeros(ns,1); 1/T1];
C=[1 10 0];
A2=[As Bs;
    zeros(1,ns) -1/T2];
B2=[zeros(ns,1); 1/T2];
A3=[As Bs;
    zeros(1,ns) -1/T3];
B3=[zeros(ns,1); 1/T3];
nTS=3;
%   networked system
Lhat=[1 -0.5 -0.5;
      -0.5 1 -0.5;
      -0.5 -0.5 1];
k1=0.1;
k2=0.1;
k3=0.1;
Abar=blkdiag(A1, A2, A3)-blkdiag(B1, B2, B3)*diag([k1,k2,k3])*Lhat*blkdiag(C, C, C);
Bbar=zeros(3*nTS);
Cbar=blkdiag(C, C, C);
Dbar=0;
Oscillators=ss(Abar, Bbar, Cbar, Dbar);
x0=[-1; 1; 0;
    1; 1; 0;
    0; 0; 1];
%x0=rand(3*nTS,1);
Tend=27;
[Y,Time]=initial(Oscillators, x0, Tend);
subplot(4,1,[1 2])
plot(Time,Y(:,1),'b', Time,Y(:,2),'b', Time,Y(:,3),'b');
axis([0 Tend -15 15]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$y_i$$');
rotateY;
yticks([-15 0 15]);
%xticks([0 10 20 30])

%  Synchronisation test
Abreve=[-1/T1 zeros(1,2*nTS);
        -[Bs;0] A2 zeros(nTS,nTS);
        -[Bs;0] zeros(nTS,nTS), A3]...
        -blkdiag(1/T1, B2, B3)*diag([k1,k2,k3])*Lhat*blkdiag(0, C, C);
hold off
%
%  smaller time constants
T1=0.1;
T2=0.2;
T3=3;
A1=[As Bs;
    zeros(1,ns) -1/T1];
B1=[zeros(ns,1); 1/T1];
A2=[As Bs;
    zeros(1,ns) -1/T2];
B2=[zeros(ns,1); 1/T2];
A3=[As Bs;
    zeros(1,ns) -1/T3];
B3=[zeros(ns,1); 1/T3];
%   networked system
Abar=blkdiag(A1, A2, A3)-blkdiag(B1, B2, B3)*diag([k1,k2,k3])*Lhat*blkdiag(C, C, C);
Oscillators=ss(Abar, Bbar, Cbar, Dbar);
[Y,Time]=initial(Oscillators, x0, Tend);
subplot(4,1,[3 4])
plot(Time,Y(:,1),'b', Time,Y(:,2),'b', Time,Y(:,3),'b');
axis([0 Tend -15 15]);
latexxlabel('$$t$$');
%xleer;
latexylabel('$$y_i$$');
rotateY;
yticks([-15 0 15]);
xticks([0 10 20 30])

%  Synchronisation test
Abreve=[-1/T1 zeros(1,2*nTS);
        -[Bs;0] A2 zeros(nTS,nTS);
        -[Bs;0] zeros(nTS,nTS), A3]...
        -blkdiag(1/T1, B2, B3)*diag([k1,k2,k3])*Lhat*blkdiag(0, C, C);
hold off


%%   Robustness with respect to changes of the couplings
%
figure(17)
%   networked system
Lhat=1.8*[1 -0.5 -0.5;
      -0.5 1 -0.5;
      -0.5 -0.5 1];
k1=0.1;
k2=0.1;
k3=0.1;
Abar=blkdiag(A1, A2, A3)-blkdiag(B1, B2, B3)*diag([k1,k2,k3])*Lhat*blkdiag(C, C, C);
Bbar=zeros(3*nTS);
Cbar=blkdiag(C, C, C);
Dbar=0;
Oscillators=ss(Abar, Bbar, Cbar, Dbar);
x0=[-1; 1; 0;
    1; 1; 0;
    0; 0; 1];
%x0=rand(3*nTS,1);
Tend=27;
[Y,Time]=initial(Oscillators, x0, Tend);
subplot(4,1,[1 2])
plot(Time,Y(:,1),'b', Time,Y(:,2),'b', Time,Y(:,3),'b');
axis([0 Tend -15 15]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$y_i$$');
rotateY;
latexxlabel('$$t$$');
yticks([-15 0 15]);
xticks([0 10 20 30])
%  Synchronisation test
Abreve=[-1/T1 zeros(1,2*nTS);
        -[Bs;0] A2 zeros(nTS,nTS);
        -[Bs;0] zeros(nTS,nTS), A3]...
        -blkdiag(1/T1, B2, B3)*diag([k1,k2,k3])*Lhat*blkdiag(0, C, C);
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Synchronisability of double-integrator systems with PD controller
%
figure(10)
A=[0 1; 0 0];
B=[0; 1];
C=[1 0];
D=0;
DoubleIntegrator=ss(A, B, C, D);
subplot(4,4,[1, 2, 5,6]);
rlocprintblue(DoubleIntegrator);
axis([-2.5 0.5 -1.5 1.5]);
yticks([-1 0 1]);
xticks([-2 -1 0 1]);
%
%  Double integrator system with PD controller
kD=1;
kP=1;
B0=[kD; kP];
ExtDoubleIntegrator=ss(A, B0, C, D);
subplot(4,4,[3,4,7,8]);
rlocprintblue(ExtDoubleIntegrator);
axis([-2.5 0.5 -1.5 1.5]);
yticks([-1 0 1]);
xticks([-2 -1 0 1]);

%%  Synchronisability of double-integrator systems with lead-lag controller
%
figure(11)
A=[0 1; 0 0];
B=[0; 1];
C=[1 0];
D=0;
TD=1;
kP=1;
T1=0.1;
A0=[0 1 0;
    0 0 kP-kP*TD/T1;
    0 0 -1/T1];
B0=[0; kP*TD/T1; 1/T1];
C0=[1 0 0];
D0=0;
ExtDoubleIntegrator=ss(A0, B0, C0, D0);
subplot(4,4,[1, 2, 5,6]);
rlocprintblue(ExtDoubleIntegrator);
axis([-12 2 -7 7]);
yticks([-10 -5 0 5]);
xticks([-10 -5 0 5]);
hold off
%   Details
subplot(4,4,[3,4,7,8]);
K=linspace(0.0113, 10, 2500);  % figure is sensitive to selection of points in K!
%   parts of rlocprintblue.m:
R=rlocus(ExtDoubleIntegrator,K);
N=size(R);
n=N(1,1);
for i=1:n
    plot(real(R(i,:)), imag(R(i,:)), 'b');
    hold on
end
plot([-4 4],[0 0],'k:', [0 0], [-4 4],'k:');
axis('square');
[P, Z]=pzmap(ExtDoubleIntegrator);
for i=1:length(P)
    plot(real(P(:)), imag(P(:)), 'bx');
end
for i=1:length(Z)
    plot(real(Z(:)), imag(Z(:)), 'bo');
end
latexxlabel('Re');
%latexylabel('Im');
%rotateY;
yleer;
axis([-3 0.6 -1.8 1.8]);
yticks([-1 0 1]);
xticks([-3 -2 -1 0 1]);
hold off


%%   Distance control of vehicles, which have a velocity controller
%    1. Properties of the velocity control loop   
%
figure(12)
%               Vehicle models differ in the mass m
n=2;            % dynamical order of a vehicle
c=100;          %  c=200 kg/s friction constant
                % forces in N,  hence input  u=acceleration in N
                % time in seconds
                % distance in m
                % velocity in m/s,   
                %         hence output  y=velocity in m/s
m(1)=1000;      % m= 1000 kg  mass
m(2)=300;
m(3)=500;
m(4)=1500;
m(5)=1200;
m(6)=100;
m(7)=2000;
m(8)=300;
m(9)=500;
m(10)=1000;
%
N1=10;           % N vehicles
%
%               PI controller
kIm=30/500;     % kI=40 for nominal mass m=500;
kPm=450/500;          % kP=200 for nominal mass
%               Controlled vehicle
%               input = set-point for velocity controller
%               output = velocity
for k1=1:N1
    kI(k1)=kIm*m(k1); 
    kP(k1)=kPm*m(k1);
end
Ca=[1 0];
Da=0;
na=2;            % dynamical order of the controlled vehicle
r=1;
%
Tend=20;
%      Draw the command step response of the velocity control loop 
%      for all vehicles
for k1=1:N1
    Aa=[-c/m(k1)-kP(k1)/m(k1) -kI(k1)/m(k1); 1 0];
    Ba=[kP(k1)/m(k1); -1];    
    Car=ss(Aa, Ba, Ca, Da);
    [Y, T]=step(Car,Tend);
    subplot(2,1,1)
    plot(T, Y, 'b');
    hold on
end
latexxlabel('$$t$$ in s');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$'); % in m/s
rotateY;
axis([0 Tend 0 1.2]);
yticks([0 1]);
xticks([0 5 10 15 20 30 40]);
latextitle('$${\tt SyncIMPDemo:}$$ Command step response of the vehicles');
hold off


%%   Distance control of vehicles, which have a velocity controller
%    2. Properties of the controlled vehicles   
%
figure(13)
%   Local controller is a PI controller with the following parameters
kri=0.001;     % 0.009
kei=0.3;      % 0.2
Ca=[1 0];
Da=0;
na=2;
k0=1;     % loop gain for the controlled agent
for k1=1:N1
    Aa=[-c/m(k1)-kP(k1)/m(k1) -kI(k1)/m(k1); 1 0];
    Ba=[kP(k1)/m(k1); -1];
    Ai=[0          Ca;
        zeros(na,1) Aa];
    Bi=[0; Ba];
    Ci=[1 zeros(1,na)];
    ni=na+1;
    A0=[Ai Bi*kri;
        zeros(1,ni) 0];
    B0=[Bi*kei; 1];
    C0=[Ci 0];
    D0=0;
    Abar=A0-B0*k0*C0;
    Bbar=B0*k0;
    Cbar=C0;
    Dbar=0;
    ClosedLoopCar=ss(Abar, Bbar, Cbar, Dbar);
    [Y, T]=step(ClosedLoopCar);
    subplot(2,1,1)
    plot(T, Y, 'b');
    hold on
end
latexxlabel('$$t$$ in s');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$'); % in m/s
rotateY;
axis([0 50 0 1.2]);
yticks([0 1]);
xticks([0 10 20 30 40]);
latextitle('$${\tt SyncIMPDemo:}$$ Command step response of the vehicles');
hold off


%%   Distance control of vehicles, which have a velocity controller
%    3. Vehicle platoon with neighbouring couplings
%        from PlatoonIntModDiff.m
%
figure(14)
N1=10;   % 10   number of followers
Lcoup=diag(ones(1,N1-1),-1);
Lhat=diag(sum(Lcoup'))-Lcoup;
%           Model of the followers
Abar=[];
for k1=1:N1
    Aa=[-c/m(k1)-kP(k1)/m(k1) -kI(k1)/m(k1); 1 0];
    Ba=[kP(k1)/m(k1); -1];
    Ai=[0          Ca;
        zeros(na,1) Aa];
    Bi=[0; Ba];
    Ci=[1 zeros(1,na)];
    ni=na+1;
    A0=[Ai Bi*kri;
        zeros(1,ni) 0];
    B0=[Bi*kei; 1];
    C0=[Ci 0];
    D0=0;
    Aibar=A0-B0*k0*C0;
    Bibar=B0*k0;
    Cibar=C0;
    Dibar=0;
    nTS=ni+1;
    Abar((k1-1)*nTS+1:k1*nTS, (k1-1)*nTS+1:k1*nTS)=Aibar;
    for j1=1:N1
        if j1~=k1
           Abar((k1-1)*nTS+1:k1*nTS, (j1-1)*nTS+1:j1*nTS)=-Lhat(k1,j1)*B0*C0;
                %  C0i is independent of vehicle number i
        end
    end
end
%  Extension of the model by the leading vehicle
As=[0 1; 0 0];
Cs=[1 0];
ns=2;
s0=1500;
vbar=20;
sbar=40;
%
k1=1;
Ba=[kP(k1)/m(k1); -1];  
Bi=[0 ; Ba];
B0=[Bi*kei; 1];   % Coupling of the leader to vehicle 1
ALF=[  As zeros(ns, N1*nTS); 
     [B0*Cs; zeros((N1-1)*nTS, ns)] Abar];
Cbar=kron(diag(ones(1,N1)), C0);
CLF=[Cs zeros(1, N1*nTS); 
     zeros(N1,ns) Cbar];
BLF=zeros(N1*nTS+ns, 1);  % not used in simulation
DLF=zeros(N1+1,1);
PlatoonLF=ss(ALF, BLF, CLF, DLF);
%   Initial states of vehicles   x_i=[s_i; 0; 0; 0];
y0=1000*rand([N1, 1]);     % starting positions y0i of the vehicles
y0=sort(y0, 'descend');
x0=zeros(nTS*N1, 1);
for k1=1:N1
    x0((k1-1)*nTS+1,1)=y0(k1,1);   % position is first state variable
    sbari(k1)=k1*sbar;             % required distance sbar from leader
end
%   initial velocity of all vehicles: vbar
%   the corresponding initial states xai(0), xri(0) are determined 
%   as follows:
for k1=1:N1
    Aa=[-c/m(k1)-kP(k1)/m(k1) -kI(k1)/m(k1); 1 0];
    Ba=[kP(k1)/m(k1); -1]; 
    xa0=-inv(Aa)*Ba*vbar;
    x0((k1-1)*nTS+2:(k1-1)*nTS+3, 1)=xa0;
    x0((k1-1)*nTS+4, 1)=vbar/kri;
end
%    add initial state of the leader
xLF0=[s0; vbar; x0];
Time=0:0.2:800;
Y=initial(PlatoonLF,xLF0,Time);
plot(Time, Y(:,2:N1+1), Time, Y(:,1),'--');
axis([0 100 0 3000]);
latexxlabel('$$t$$  in s');
xticks([0 50 100 200 300]);
latexylabel('$$y_i$$ in m');
yticks([0 1000 2000 3000 4000 5000]);
rotateY;
latextitle('$${\tt SyncIMPDemo:}$$ Platoon with neighbouring communication');
hold off
%
%   Transformation to vehicle position
S=[];
S(:,1)=Y(:,1);
for k1=1:N1    %  considere the followers
    S(:,k1+1)=Y(:,k1+1)-sbari(k1)*ones(size(Time))'; 
end
figure(15)
subplot(4,1,[1 2])
plot(Time, S(:,2:N1+1), 'b', Time, S(:,1),'r--');
axis([0 75 -100 3000]);
%xlabel('{\it t}  in s');
xticksempty([0 25 50 75 100 125 150]);
latexxlabel('$$t$$ in s');
latexylabel('$$y_i$$ in m');   % in m
rotateY;
yticks([-500 0 1000 2000 3000]);
latextitle('$${\tt SyncIMPDemo:}$$ Platoon with neighbouring communication');
hold off
%
% subplot(4,1,[3 4])
% plot(Time, Y(:,2:N1+1), 'b', Time, Y(:,1),'r--');
% axis([0 75 -100 3000]);
% %xlabel('{\it t}  in s');
% xticks([0 25 50 75 100 125 150]);
% latexxlabel('$$t$$ in s');
% latexylabel('$$s_i$$ in m');   % in m
% rotateY;
% yticks([-500 0 1000 2000 3000]);
% hold off


%%   Distance control of vehicles, which have a velocity controller
%    4. Vehicle platoon with couplings to two successors
%        from PlatoonIntModDiff.m
%
figure(16)
Lcoup=diag(0.5*ones(1,N1-1),-1)+diag(0.5*ones(1,N1-2),-2);
Lhat=diag(sum(Lcoup'))-Lcoup;
Lhat(2,1)=-1;
Lhat(2,2)=1;
%           Model of the followers
Abar=[];
for k1=1:N1
    Aa=[-c/m(k1)-kP(k1)/m(k1) -kI(k1)/m(k1); 1 0];
    Ba=[kP(k1)/m(k1); -1];
    Ai=[0          Ca;
        zeros(na,1) Aa];
    Bi=[0; Ba];
    Ci=[1 zeros(1,na)];
    ni=na+1;
    A0=[Ai Bi*kri;
        zeros(1,ni) 0];
    B0=[Bi*kei; 1];
    C0=[Ci 0];
    D0=0;
    Aibar=A0-B0*k0*C0;
    Bibar=B0*k0;
    Cibar=C0;
    Dibar=0;
    nTS=ni+1;
    Abar((k1-1)*nTS+1:k1*nTS, (k1-1)*nTS+1:k1*nTS)=Aibar;
    for j1=1:N1
        if j1~=k1
           Abar((k1-1)*nTS+1:k1*nTS, (j1-1)*nTS+1:j1*nTS)=-Lhat(k1,j1)*B0*C0;
                %  C0i is independent of vehicle number i
        end
    end
end
%  Extension of the model by the leading vehicle
k1=1;
Ba=[kP(k1)/m(k1); -1];  
Bi=[0 ; Ba];
B0=[Bi*kei; 1];   % Coupling of the leader to vehicle 1
ALF=[  As zeros(ns, N1*nTS); 
     [B0*Cs; zeros((N1-1)*nTS, ns)] Abar];
PlatoonLF=ss(ALF, BLF, CLF, DLF);
Y=initial(PlatoonLF,xLF0,Time);
plot(Time, Y(:,2:N1+1), Time, Y(:,1),'--');
axis([0 100 0 3000]);
latexxlabel('$$t$$  in s');
xticks([0 50 100 200 300]);
latexylabel('$$y_i$$ in m');
yticks([0 1000 2000 3000 4000 5000]);
rotateY;
latextitle('$${\tt SyncIMPDemo:}$$ Platoon with neighbouring communication');
hold off
%
%   Transformation to vehicle position
S=[];
S(:,1)=Y(:,1);
for k1=1:N1    %  considere the followers
    S(:,k1+1)=Y(:,k1+1)-sbari(k1)*ones(size(Time))'; 
end
figure(17)
subplot(2,1,1)
plot(Time, S(:,2:N1+1), 'b', Time, S(:,1),'r--');
axis([0 100 -100 3000]);
%xlabel('{\it t}  in s');
xticks([0 25 50 75 100 125 150]);
latexxlabel('$$t$$ in s');
latexylabel('$$s_i$$ in m');   % in m
rotateY;
yticks([-500 0 1000 2000 3000]);
latextitle('$${\tt SyncIMPDemo:}$$ Platoon with neighbouring communication');
hold off


%%  figure(17) after figure(9)


%%
%   EPS-figure
%
epsfigc('SyncIMPDemo');
