%      ConsensusDemo.M
%   
%  Figures to demonstrate the consensus problem
%  
% J. Lunze
% 
% 27.11.2016
% Version v. 13.1.2019
% for 2nd edition: 16.4.2021
%
echo off
clear
close all
N1=100;                   % Number of vertices


%%  Consensus in networks with different couplings
%   
figure(1)
%  series connection   1-2-3-4-5
subplot(2,1,1)
N1=5;
A=zeros(N1, N1);
for k1=2:N1
    A(k1,k1-1)=1;
end
D=diag(sum(A'));
LPath=D-A;
%  Network model
APath=-LPath;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
NetworkPath=ss(APath, Bbar, Cbar, 0);
x0=[  0.9000
    1.6154
    0.8263
    0.5562
    1.220];
[YPath,TimePath]=initial(NetworkPath,x0);
tend=TimePath(end);
plot(TimePath,YPath,'b', [0 tend], [x0(1), x0(1)],'k:');
hold on
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
axis([0 6 0.4 1.7]);
yticks([0.5 1 1.5]);
latextitle('Consensus behaviour for path and ring connection');
hold off;
%
%  Ring connection
subplot(2,1,2)
N1=5;
A=zeros(N1, N1);
for k1=2:N1
    A(k1,k1-1)=1;
end
A(1, N1)=1;
D=diag(sum(A'));
LRing=D-A;
%  Network model
ARing=-LRing;
NetworkRing=ss(ARing, Bbar, Cbar, 0);
[YRing,TimeRing]=initial(NetworkRing,x0);
xbar=YRing(end,1);
tend=TimeRing(end);
plot(TimeRing,YRing,'b', [0 tend], [xbar, xbar],'k:');
hold on
%latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 6 0.4 1.7]);
yticks([0.5 1 1.5]);
hold off;

%%  Convergence bound by the second smallest eigenvalue of L
%
figure(2)
%  series connection   1-2-3-4-5
subplot(4,1,[1 2])
plot(TimePath,YPath,'b');
hold on
xbar=x0(1);
lambda2=-1;  % for path graph
%  Determination of the upper bound backwards from Tend
Tend=5;
[YPath2,TimePath2]=initial(NetworkPath, x0, Tend);
Yend=YPath2(end,:)';
dt2=TimePath2(2)-TimePath2(1);
kend=length(TimePath2);
Bound1Path=[];
Bound1Path(kend,:)=max(Yend)-xbar;
for k1=1:kend-1
    Bound1Path(kend-k1,:)=exp(-lambda2*dt2)*Bound1Path(kend-k1+1,:);
end
Bound1Path=Bound1Path+xbar;
%   Determination of the lower bound by starting in the minimum initial
%   state
Ymin=min(x0);
Bound2Path=(xbar-Ymin)*exp(lambda2*TimePath2);
Bound2Path=xbar-Bound2Path;
plot(TimePath2, Bound1Path, 'k--', TimePath2, Bound2Path, 'k--');
%latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 6 0.4 2.7]);
yticks([0 1 2 ]);
xleer;
latextitle('Consensus behaviour for path and ring connection');
hold off;
%
%  Ring connection
subplot(4,1,[3 4])
plot(TimeRing,YRing,'b');
hold on
xbar=sum(x0)/N1;
lambda2=-0.691;  % for ring graph
%  Determination of the upper bound backwards from Tend
Tend=6;
[YRing2,TimeRing2]=initial(NetworkPath, x0, Tend);
Yend=YRing2(end,:)';
dt2=TimeRing2(2)-TimeRing2(1);
kend=length(TimeRing2);
BoundRing=[];
BoundRing(kend,:)=[max(Yend)-xbar min(Yend)-xbar];
for k1=1:kend-1
    BoundRing(kend-k1,:)=exp(-lambda2*dt2)*BoundRing(kend-k1+1,:);
end
BoundRing(:,1)=BoundRing(:,1)+xbar;
BoundRing(:,2)=xbar-BoundRing(:,2);
%   Determination of the lower bound by starting in the minimum initial
%   state
Ymin=min(x0);
Bound2Path=(xbar-Ymin)*exp(lambda2*TimePath2);
Bound2Path=xbar-Bound2Path;
plot(TimeRing2, BoundRing,'k--');
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 6 0.4 2.7]);
yticks([0 1 2]);
hold off;


%%  Consensus in completely coupled networks
%   L = (l_ij)  with l_ii= N-1,  l_ij=-1
figure(3)
subplot(2,1,1)
N1=40;
tend=4/N1;
dt=tend/100;
kend=ceil(tend/dt);
Time=0:dt:tend;
L=-ones(N1, N1);
for k1=1:N1
    L(k1,k1)=N1-1;
end
%  Network model
Abar=-L;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Network=ss(Abar, Bbar, Cbar, 0);
x0=abs(rand(N1,1));   
x0=x0/sum(x0)*N1;        %  nonnegative initial state in [0, 1]
xbar=sum(x0)/N1;      %  consensus state
Y=initial(Network,x0,Time);
plot(Time,Y,'b');
hold on
%latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
xticks([0 0.05 0.1]);
yticks([0 1 2]);
axis([0 tend 0 2]);
%
%   Bounds determined with first-order representation
Abound=-N1;
Bbound=0;
Cbound=1;
Dbound=0;
Bound=ss(Abound, Bbound, Cbound, Dbound);
xbound=1;
ybound=initial(Bound, xbound, Time);
plot(Time, xbar+ybound*(min(x0)-xbar), 'w',...
     Time, xbar+ybound*(max(x0)-xbar), 'w');    % to erase the lines
plot(Time, xbar+ybound*(min(x0)-xbar), 'r--',...
     Time, xbar+ybound*(max(x0)-xbar), 'r--');
hold off
%   the same for N1=5;
subplot(2,1,2)
N1=5;
tend=4/N1;
dt=tend/100;
kend=ceil(tend/dt);
Time=0:dt:tend;
L=-ones(N1, N1);
for k1=1:N1
    L(k1,k1)=N1-1;
end
%  Network model
Abar=-L;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Network=ss(Abar, Bbar, Cbar, 0);
x0=abs(rand(N1,1));   
x0=x0/sum(x0)*N1;        %  nonnegative initial state in [0, 1]
xbar=sum(x0)/N1;      %  consensus state
Y=initial(Network,x0,Time);
plot(Time,Y,'b');
hold on
plot([0 tend],[Y(end,1), Y(end,1)],'k:');
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
xticks([0 0.2 0.4 0.6 0.8]);
yticks([0 1 2]);
axis([0 tend 0 2]);
hold off

%%  Consensus in switching networks
%   Switch between two networks both of which are weight-balanced
%   and strongly coupled
%   Graph 1: ring graph  1->2->3->4->5->1
%   Graph 2: ring graph  1->5->4->3->2->1
figure(4)
N1=5;
tend=6;
dt=0.01;
kend=ceil(tend/dt)-1;
Time=0:dt:kend*dt;
%
k3=0;
Xbar=[];
Tswitch=ceil([1.5 2 3 4.5 5 5.3 6.5 7 9]/dt);           
Sigma=[];
Sigma(1)=1;   
k2=1;
for k1=2:kend+1
    Sigma(k1)=Sigma(k1-1);
    if k1==Tswitch(k2)
        Sigma(k1)=Sigma(k1)+1;
        if Sigma(k1)==3
            Sigma(k1)=1;
        end
        k2=k2+1;
    end
end
subplot(4,1,1)
plot(Time, Sigma, 'b');
yticks([1 2]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 tend 0.8 2.2]);
%  Network model
L1=[1 0 0 0 -1; -1 1 0 0 0; 0 -1 1 0 0; 0 0 -1 1 0; 0 0 0 -1 1];
L2=[1 -1 0 0 0; 0 1 -1 0 0; 0 0 1 -1 0; 0 0 0 1 -1; -1 0 0 0 1];
Atilde1=expm(-L1*dt);
Atilde2=expm(-L2*dt);
%   deterministic initial state
x0(1,1)=2;
x0(2,1)=1;
x0(3,1)=0.5;
x0(4,1)=0.2;
x0(5,1)=1.3;
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=Atilde1*X(k1-1,:)';
    else
        X(k1,:)=Atilde2*X(k1-1,:)';
    end
end
subplot(4,1,[2,3]);
plot(Time,X,'b');
axis([0 tend 0 2]);
hold on
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
hold off
%  Lyapunov function
subplot(4,1,4)
V=[];
xbar=sum(x0)/N1;
Xtilde=X-xbar;
for k1=1:length(Xtilde)
    V(k1)=0.5*Xtilde(k1,:)*Xtilde(k1,:)';
end
plot(Time, V, 'b', [0 tend], [0 0], 'k:');
latexxlabel('$$t$$');
latexylabel('$$V$$');
rotateY;
axis([0 tend -0.1 1.1]);
xticks([0 2 4 6 8]);
yticks([0 1]);



%%  Consensus in switching networks
%   Switch between two networks both of which are not weight-balanced
%   but strongly coupled
%   Graph 1: ring graph  1->2->3->4->5->1
%   Graph 2: ring graph  1->5->4->3->2->1
figure(5)
%
k3=0;
Xbar=[];
subplot(4,1,1)
plot(Time, Sigma, 'b');
yticks([1 2]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 tend 0.8 2.2]);
%  Network model
L1=[1 0 0 0 -1; -2 2 0 0 0; 0 -1 1 0 0; 0 0 -1 1 0; 0 0 0 -1 1];
L2=[1 -1 0 0 0; 0 1 -1 0 0; 0 0 2 -2 0; 0 0 0 3 -3; -1 0 0 0 1];
Atilde1=expm(-L1*dt);
Atilde2=expm(-L2*dt);
%   deterministic initial state
x0(1,1)=2;
x0(2,1)=1;
x0(3,1)=0.5;
x0(4,1)=0.2;
x0(5,1)=1.3;
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=Atilde1*X(k1-1,:)';
    else
        X(k1,:)=Atilde2*X(k1-1,:)';
    end
end
subplot(4,1,[2,3]);
plot(Time,X,'b');
axis([0 tend 0 2]);
hold on
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
xticks([0 2 4 6 8]);
hold off
%
%  the same with different order of the switching modes
figure(6)
Sigma=[];
Sigma(1)=2;   % Switching stars with 2   
k2=1;
for k1=2:kend+1
    Sigma(k1)=Sigma(k1-1);
    if k1==Tswitch(k2)
        Sigma(k1)=Sigma(k1)+1;
        if Sigma(k1)==3
            Sigma(k1)=1;
        end
        k2=k2+1;
    end
end
subplot(4,1,1)
plot(Time, Sigma, 'b');
yticks([1 2]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 tend 0.8 2.2]);
%  behaviour of the overall system
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=Atilde1*X(k1-1,:)';
    else
        X(k1,:)=Atilde2*X(k1-1,:)';
    end
end
subplot(4,1,[2,3]);
plot(Time,X,'b');
axis([0 tend 0 2]);
hold on
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
xticks([0 2 4 6 8]);
hold off


%%  Consensus in networks with different couplings
%   
figure(7)
%  Ring connection
subplot(3,1,1)
N1=5;
A1=[0 1 0 0 0; 1 0 1 0 0; 0 1 0 1 0; 0 0 1 0 1; 0 0 0 1 0];
A2=[0 1 1 0 0; 1 0 1 1 0; 1 1 0 1 1; 0 1 1 0 1; 0 0 1 1 0];
A3=[0 1 1 1 1; 1 0 1 1 1; 1 1 0 1 1; 1 1 1 0 1; 1 1 1 1 0];
D1=diag(sum(A1'));
L1=D1-A1;
D2=diag(sum(A2'));
L2=D2-A2;
D3=diag(sum(A3'));
L3=D3-A3;
%  Network model
Abar1=-L1;
Abar2=-L2;
Abar3=-L3;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Network1=ss(Abar1, Bbar, Cbar, 0);
Network2=ss(Abar2, Bbar, Cbar, 0);
Network3=ss(Abar3, Bbar, Cbar, 0);
x0=[  0.9000
    1.6154
    0.8263
    0.5562
    1.220];
%x0=abs(rand(N1,1));   
x0=x0/sum(x0)*N1;        %  nonnegative initial state in [0, 1]
[Y1,Time1]=initial(Network1,x0);
Y2=initial(Network2,x0, Time1);
Y3=initial(Network3,x0, Time1);
%
subplot(6,1,[1, 2])
xbar=Y1(end,1);
tend=Time1(end);
plot(Time1,Y1,'b', [0 tend], [xbar, xbar],'k:');
hold on
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
axis([0 2 0.4 1.7]);
yticks([0.5 1 1.5]);
%
subplot(6,1,[3, 4])
plot(Time1,Y2,'b', [0 tend], [xbar, xbar],'k:');
hold on
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
axis([0 2 0.4 1.7]);
yticks([0.5 1 1.5]);
%
subplot(6,1,[5, 6])
plot(Time1,Y3,'b', [0 tend], [xbar, xbar],'k:');
hold on
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 2 0.4 1.7]);
yticks([0.5 1 1.5]);
%   
%  Determination of the upper bounds
subplot(6,1,[1, 2])
lambda2=0.382;
Bound1=[];
Bound1(:,1)=xbar+(max(x0)-xbar)*exp(-lambda2*Time1);
Bound1(:,2)=xbar-(xbar-min(x0))*exp(-lambda2*Time1);
plot(Time1, Bound1, 'k--');
hold off
%
subplot(6,1,[3, 4])
lambda2=1.5858;
Bound2=[];
Bound2(:,1)=xbar+(max(x0)-xbar)*exp(-lambda2*Time1);
Bound2(:,2)=xbar-(xbar-min(x0))*exp(-lambda2*Time1);
plot(Time1, Bound2, 'k--');
hold off
%
subplot(6,1,[5, 6])
lambda2=5;
Bound3=[];
Bound3(:,1)=xbar+(max(x0)-xbar)*exp(-lambda2*Time1);
Bound3(:,2)=xbar-(xbar-min(x0))*exp(-lambda2*Time1);
plot(Time1, Bound3, 'k--');
hold off


%%  High-gain consensus
%   
%   Use a graph with local couplings among 1=2=3=4=5
%   and use the feedback gain k=1, k=2, k=5
%   to get quicker system responses
figure(8)
subplot(3,1,1)
N1=5;
A=[0 1 0 0 0; 1 0 1 0 0; 0 1 0 1 0; 0 0 1 0 1; 0 0 0 1 0];
D=diag(sum(A'));
L=D-A;
%  Network model
Abar1=-L;
Abar2=-2*L;
Abar3=-5*L;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Network1=ss(Abar1, Bbar, Cbar, 0);
Network2=ss(Abar2, Bbar, Cbar, 0);
Network3=ss(Abar3, Bbar, Cbar, 0);
x0=[0.7
    1.6
    0.5
    1.3
    0.9];
x0=x0/sum(x0)*N1;        %  nonnegative initial state in [0, 1]
[Y1,Time1]=initial(Network1,x0);
Y2=initial(Network2,x0, Time1);
Y3=initial(Network3,x0, Time1);
%
subplot(6,1,[1, 2])
xbar=Y1(end,1);
tend=Time1(end);
plot(Time1,Y1,'b', [0 tend], [xbar, xbar],'k--');
hold on
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
axis([0 1 0.4 1.7]);
yticks([0.5 1 1.5]);
latextext(0.8, 1.35, '$$k=1$$');
hold off;
%
subplot(6,1,[3, 4])
plot(Time1,Y2,'b', [0 tend], [xbar, xbar],'k--');
hold on
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
axis([0 1 0.4 1.7]);
yticks([0.5 1 1.5]);
latextext(0.8, 1.35, '$$k=2$$');
hold off;
%
subplot(6,1,[5, 6])
plot(Time1,Y3,'b', [0 tend], [xbar, xbar],'k--');
hold on
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 1 0.4 1.7]);
yticks([0.5 1 1.5]);
latextext(0.8, 1.35, '$$k=5$$');
hold off;


%%  Consensus for systems that are not strongly connected
%
%
figure(9)
N1=6;
A1=[0 1 1 0 0 0; 1 0 1 0 0 0; 1 1 0 0 0 0; 0 0 1 0 1 1; 0 0 0 1 0 1; 0 0 0 1 1 0];
A2=[0 1 1 0 0 0; 1 0 1 0 0 0; 1 1 0 0 0 0; 0 0 0 0 1 1; 0 0 0 1 0 1; 0 0 0 1 1 0];
A3=[0 1 0 0 0 0; 0 0 1 0 0 0; 1 0 0 0 0 0; 0 0 1 0 0 1; 0 0 0 1 0 0; 0 0 0 0 1 0];
A4=[0 1 0 0 0 0; 0 0 1 0 0 0; 1 0 0 0 0 0; 0 0 1 0 0 1; 0 0 0 0 0 0; 0 0 0 0 1 0];
D1=diag(sum(A1'));
L1=D1-A1;
D2=diag(sum(A2'));
L2=D2-A2;
D3=diag(sum(A3'));
L3=D3-A3;
D4=diag(sum(A4'));
L4=D4-A4;
%  Network model
Abar1=-L1;
Abar2=-L2;
Abar3=-L3;
Abar4=-L4;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Network1=ss(Abar1, Bbar, Cbar, 0);
Network2=ss(Abar2, Bbar, Cbar, 0);
Network3=ss(Abar3, Bbar, Cbar, 0);
Network4=ss(Abar4, Bbar, Cbar, 0);
x0=[1; 0.5; 0; 1.2; 1.5; 2];
[Y1,Time1]=initial(Network1,x0);
Y2=initial(Network2,x0, Time1);
Y3=initial(Network3,x0, Time1);
Y4=initial(Network4,x0, Time1);
%
subplot(8,1,[1,2])
xbar=Y1(end,1);
tend=Time1(end);
plot(Time1,Y1,'b');
hold on
%latexxlabel('$$t$$');
xleer
latexylabel('$$x_i$$');
rotateY;
axis([0 5 -0.2 2.2]);
yticks([0 1 2]);
latextext(5.2,1,'$$\overrightarrow{{\cal G}_1}$$');
hold off;
%
subplot(8,1,[3,4])
plot(Time1,Y2,'b');
hold on
%latexxlabel('$$t$$');
xleer
latexylabel('$$x_i$$');
rotateY;
axis([0 5 -0.2 2.2]);
yticks([0 1 2]);
latextext(5.2,1,'$$\overrightarrow{{\cal G}_2}$$');
hold off;
%
subplot(8,1,[5,6])
plot(Time1,Y3,'b');
hold on
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
axis([0 5 -0.2 2.2]);
yticks([0 1 2]);
latextext(5.2,1,'$$\overrightarrow{{\cal G}_3}$$');
hold off;
%
subplot(8,1,[7,8])
plot(Time1,Y4,'b');
hold on
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 5 -0.2 2.2]);
yticks([0 1 2]);
latextext(5.2,1,'$$\overrightarrow{{\cal G}_4}$$');
hold off;

%% Consensus in a network with time-varying number of agents
%
figure(10)
subplot(3,1,[1,2]);
N1=5;
A=zeros(N1, N1);
for k1=2:N1
    A(k1,k1-1)=1;
end
A(1, N1)=1;
D=diag(sum(A'));
LRing=D-A;
%  Network model
ARing=-LRing;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
NetworkRing=ss(ARing, Bbar, Cbar, 0);
x0=zeros(N1,1);
x0(3,1)=0.6;
x0(4,1)=0.4;
x0(1,1)=0.8;
T1=0.8;
[YRing,TimeRing]=initial(NetworkRing,x0,T1);
xbar=sum(x0)/N1;
plot(TimeRing,YRing,'b', [0 T1], [xbar, xbar],'k--');
hold on
plot(T1, YRing(end,5),'b+', [T1, T1], [0 1], 'k:'); 
%   Delete Agent~5;
x0=YRing(end,1:4);    % use as initial state for next time interval
N1=4;
A=zeros(N1, N1);
for k1=2:N1
    A(k1,k1-1)=1;
end
A(1, N1)=1;
D=diag(sum(A'));
LRing=D-A;
%  New network model
ARing=-LRing;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
NetworkRing=ss(ARing, Bbar, Cbar, 0);
T2=1.5;
[YRing,TimeRing]=initial(NetworkRing,x0,T2);
xbar=sum(x0)/N1;
plot(TimeRing+T1,YRing,'b', [T1 T1+T2], [xbar, xbar],'k--');
plot([T1+T2, T1+T2], [0 1], 'k:'); 
%  Insert new Agent~5
x0=YRing(end,:)';
x0(5,1)=0.05;         % initial state of new agent
N1=5;
A=zeros(N1, N1);
for k1=2:N1
    A(k1,k1-1)=1;
end
A(1, N1)=1;
D=diag(sum(A'));
LRing=D-A;
%  Network model
ARing=-LRing;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
NetworkRing=ss(ARing, Bbar, Cbar, 0);
T3=3.7;
[YRing,TimeRing]=initial(NetworkRing,x0,T3);
xbar=sum(x0)/N1;
plot(TimeRing+T1+T2,YRing,'b', [T1+T2 T1+T2+T3], [xbar, xbar],'k--');
plot(T1+T2, x0(5,1), 'bo');
latexxlabel('$$t$$');
xticks([0 T1 T1+T2 4 6]);
latexylabel('$$x_i$$');
rotateY;
axis([0 T1+T2+T3 0 0.8]);
yticks([0 0.2 0.4 0.6 0.8]);
hold off;
latextitle('Network with changing number of agents');


%%  Consensus with two leaders
%
%
figure(11)
N1=5;
L1=[0 0 0 0 0; -1 2 -1 0 0; 0 -1 2 -1 0; 0 0 -1 2 -1; 0 0 0 0 0];
L2=[0 0 0 0 0; 
    -1 3 -1 -1 0; 
    -1 -1 3 -1 0; 
    0 -1 -1 3 -1; 
    0 0 0 0 0];
%  Network model
Abar1=-L1;
Abar2=-L2;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Network1=ss(Abar1, Bbar, Cbar, 0);
Network2=ss(Abar2, Bbar, Cbar, 0);
x0=[0; 
    0.2; 
    0.9; 
    1.2; 
    6];
[Y1,Time1]=initial(Network1,x0);
Y2=initial(Network2,x0, Time1);
%
subplot(4,1,[1 2])
xbar=Y1(end,1);
tend=Time1(end);
plot(Time1,Y1,'b');
hold on
plot(3, Y1(end,:), 'bo');
%latexxlabel('$$t$$');
xleer
latexylabel('$$x_i$$');
rotateY;
axis([0 3 -0.3 6.3]);
yticks([0 2 4 6]);
latextext(3.2,3,'$$\overrightarrow{{\cal G}_1}$$');
hold off;
%
subplot(4,1,[3 4])
plot(Time1,Y2,'b');
hold on
plot(3, Y2(end,:), 'bo');
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 3 -0.3 6.3]);
yticks([0 2 4 6]);
xticks([0 1 2 3 4]);
latextext(3.2,3,'$$\overrightarrow{{\cal G}_2}$$');
hold off;



%%     Linear formation control
%
figure(12)
%subplot(2,2,1)
subplot(4,4,[1 2 5 6]);
N1=4;
Tend=6;
% L=[0 0 0 0;
%     -1 1 0 0;
%     0 -1 2 -1;
%     0 -1 -1 2];
L=[2 -1 0 -1;
    -1 2 -1 0;
    0 -1 2 -1;
    -1 -1 0 2];
%   Definition of the a_i to fix the position
a1=[-1; 1];
a2=[1; -1];
a3=[1; 1];
a4=[-1; -1];
Offset=[a1 a2 a3 a4];
Abar=-kron(L,eye(2,2));
Bbar=zeros(2*N1,1);
Cbar=eye(2*N1,2*N1);
Formation=ss(Abar, Bbar, Cbar, 0);
%
%load('RandConsensDemo');
%rng(RandConsensDemo);
%x0=2*rand(2*N1,1);
x0=[0.7869
    1.3429
    1.4825
    1.0401
    0.6954
    0.7000
    1.1722
    0.5243];
y0=x0-[a1;a2;a3;a4];
%
Y=[];
Y=initial(Formation,y0,Tend);   % y_i = x_i-a_i
plot(Y(:,1)+a1(1), Y(:,2)+a1(2), 'b');
hold on
plot(Y(:,3)+a2(1), Y(:,4)+a2(2), 'b');
plot(Y(:,5)+a3(1), Y(:,6)+a3(2), 'b');
plot(Y(:,7)+a4(1), Y(:,8)+a4(2), 'b');
%  initial positions
plot(Y(1,1)+a1(1), Y(1,2)+a1(2),'bo');
plot(Y(1,3)+a2(1), Y(1,4)+a2(2),'bo');
plot(Y(1,5)+a3(1), Y(1,6)+a3(2),'bo');
plot(Y(1,7)+a4(1), Y(1,8)+a4(2),'bo');
latextext(Y(1,1)+a1(1)+0.2, Y(1,2)+a1(2)+0.05,'1');
latextext(Y(1,3)+a2(1)+0.2, Y(1,4)+a2(2),'2');
latextext(Y(1,5)+a3(1)+0.2, Y(1,6)+a3(2),'3');
latextext(Y(1,7)+a4(1)+0.1, Y(1,8)+a4(2)-0.07,'4');
%   final positions
plot(Y(end,1)+a1(1), Y(end,2)+a1(2),'b+');
plot(Y(end,3)+a2(1), Y(end,4)+a2(2),'b+');
plot(Y(end,5)+a3(1), Y(end,6)+a3(2),'b+');
plot(Y(end,7)+a4(1), Y(end,8)+a4(2),'b+');
%   Center of formation
wT=ones(1,N1)*inv(L+ones(N1,N1));
xbar=wT(1)*y0(1:2)+wT(2)*y0(3:4)+wT(3)*y0(5:6)+wT(4)*y0(7:8);
plot(xbar(1), xbar(2),'b*');
latexxlabel('$$x_i$$');
latexylabel('$$y_i$$');
rotateY;
axis([-0.1 2.2 -0.1 2.2]);
axis('square');
yticks([0 1 2]);
xticks([0 1 2]);
latextitle('Formation behaviour');
hold off
%
%subplot(2,2,2)
subplot(4,4,[3, 4 7 8]);
plot([Y(end,1)+a1(1), a1(1)], [Y(end,2)+a1(2), a1(2)],'b--');
hold on;
plot([Y(end,3)+a2(1), a2(1)], [Y(end,4)+a2(2), a2(2)],'b--');
plot([Y(end,5)+a3(1), a3(1)], [Y(end,6)+a3(2), a3(2)],'b--');
plot([Y(end,7)+a4(1), a4(1)], [Y(end,8)+a4(2), a4(2)],'b--');
plot(Y(end,1)+a1(1), Y(end,2)+a1(2),'b+');
plot(Y(end,3)+a2(1), Y(end,4)+a2(2),'b+');
plot(Y(end,5)+a3(1), Y(end,6)+a3(2),'b+');
plot(Y(end,7)+a4(1), Y(end,8)+a4(2),'b+');
plot(a1(1), a1(2),'b*');
plot(a2(1), a2(2),'b*');
plot(a3(1), a3(2),'b*');
plot(a4(1), a4(2),'b*');
latextext(Y(end,1)+a1(1), Y(end,2)+a1(2)-0.5,'1');
latextext(Y(end,3)+a2(1)-0.2, Y(end,4)+a2(2)-0.5,'2');
latextext(Y(end,5)+a3(1)-0.2, Y(end,6)+a3(2)-0.5,'3');
latextext(Y(end,7)+a4(1), Y(end,8)+a4(2)-0.5,'4');
latexxlabel('$$x_i$$');
%latexylabel('$$y_i$$');
%rotateY;
axis([-1.5 2.5 -1.5 2.5]);
axis('square');
yticks([-1 0 1 2]);
xticks([-1 0 1 2]);
hold off



%%     Linear formation control (II)
%
figure(13)
N1=8;
Tend=15;
L=zeros(N1,N1);
for k1=1:N1-1
    L(k1+1,k1)=-1;
end
L(1,N1)=-1;
L=L+diag(ones(N1,1));
%   Definition of the a_i to fix the position
a1=[cos(0); sin(0)];
a2=[cos(pi/4); sin(pi/4)];
a3=[cos(2*pi/4); sin(2*pi/4)];
a4=[cos(3*pi/4); sin(3*pi/4)];
a5=[cos(4*pi/4); sin(4*pi/4)];
a6=[cos(5*pi/4); sin(5*pi/4)];
a7=[cos(6*pi/4); sin(6*pi/4)];
a8=[cos(7*pi/4); sin(7*pi/4)];
Abar=-kron(L,eye(2,2));
Bbar=zeros(2*N1,1);
Cbar=eye(2*N1,2*N1);
Formation=ss(Abar, Bbar, Cbar, 0);
%
%load('RandConsensDemo');
%rng(RandConsensDemo);
%x0=2*rand(2*N1,1);
x0=0.15*[a1; a2; a3; a4; a5; a6; a7; a8];
y0=x0-[a1; a2; a3; a4; a5; a6; a7; a8];
%
Y=[];
Y=initial(Formation,y0,Tend);   % y_i = x_i-a_i
%
%subplot(2,2,1)
plot(Y(:,1)+a1(1), Y(:,2)+a1(2), 'b');
hold on
plot(Y(:,3)+a2(1), Y(:,4)+a2(2), 'b');
plot(Y(:,5)+a3(1), Y(:,6)+a3(2), 'b');
plot(Y(:,7)+a4(1), Y(:,8)+a4(2), 'b');
plot(Y(:,9)+a5(1), Y(:,10)+a5(2), 'b');
plot(Y(:,11)+a6(1), Y(:,12)+a6(2), 'b');
plot(Y(:,13)+a7(1), Y(:,14)+a7(2), 'b');
plot(Y(:,15)+a8(1), Y(:,16)+a8(2), 'b');
%  initial positions
plot(Y(1,1)+a1(1), Y(1,2)+a1(2),'bo');
plot(Y(1,3)+a2(1), Y(1,4)+a2(2),'bo');
plot(Y(1,5)+a3(1), Y(1,6)+a3(2),'bo');
plot(Y(1,7)+a4(1), Y(1,8)+a4(2),'bo');
plot(Y(1,9)+a5(1), Y(1,10)+a5(2),'bo');
plot(Y(1,11)+a6(1), Y(1,12)+a6(2),'bo');
plot(Y(1,13)+a7(1), Y(1,14)+a7(2),'bo');
plot(Y(1,15)+a8(1), Y(1,16)+a8(2),'bo');
%   final positions
plot(Y(end,1)+a1(1), Y(end,2)+a1(2),'b+');
plot(Y(end,3)+a2(1), Y(end,4)+a2(2),'b+');
plot(Y(end,5)+a3(1), Y(end,6)+a3(2),'b+');
plot(Y(end,7)+a4(1), Y(end,8)+a4(2),'b+');
plot(Y(end,9)+a5(1), Y(end,10)+a5(2),'b+');
plot(Y(end,11)+a6(1), Y(end,12)+a6(2),'b+');
plot(Y(end,13)+a7(1), Y(end,14)+a7(2),'b+');
plot(Y(end,15)+a8(1), Y(end,16)+a8(2),'b+');
%   Center of formation
wT=ones(1,N1)*inv(L+ones(N1,N1));
xbar=wT(1)*y0(1:2)+wT(2)*y0(3:4)+wT(3)*y0(5:6)+wT(4)*y0(7:8)...
    +wT(5)*y0(9:10)+wT(6)*y0(11:12)+wT(7)*y0(13:14)+wT(8)*y0(15:16);
%plot(xbar(1), xbar(2),'r*');
latextext(a1(1), a1(2)+0.2,'1');
latextext(a2(1)-0.2, a2(2)+0.2,'2');
latexxlabel('$$x_i$$');
latexylabel('$$y_i$$');
rotateY;
axis([-1.4 1.4 -1.4 1.4]);
axis('square');
xticks([-1 0 1]);
yticks([-1 0 1]);
latextitle('Formation behaviour');
%
figure(14)
%subplot(2,2,2)
%   final positions
plot(Y(end,1)+a1(1), Y(end,2)+a1(2),'b+');
hold on
plot(Y(end,3)+a2(1), Y(end,4)+a2(2),'b+');
plot(Y(end,5)+a3(1), Y(end,6)+a3(2),'b+');
plot(Y(end,7)+a4(1), Y(end,8)+a4(2),'b+');
plot(Y(end,9)+a5(1), Y(end,10)+a5(2),'b+');
plot(Y(end,11)+a6(1), Y(end,12)+a6(2),'b+');
plot(Y(end,13)+a7(1), Y(end,14)+a7(2),'b+');
plot(Y(end,15)+a8(1), Y(end,16)+a8(2),'b+');
plot(xbar(1), xbar(2),'b*');
latexxlabel('$$x_i$$');
%latexylabel('$$y_i$$');
%rotateY;
plot([-2 2],[0 0],'k:', [0 0],[-2 2],'k:');
axis([-1.4 1.4 -1.4 1.4]);
axis('square');
xticks([-1 0 1]);
yticks([-1 0 1]);

%%     Linear formation control (III)
%
figure(15)
N1=8;
Tend=40;
A=[0 1 0 0 0 0 0 0;
   1 0 1 0 0 0 0 0;
   0 1 0 1 0 0 0 0;
   0 0 1 0 1 0 0 0;
   0 0 0 1 0 1 0 0;
   0 0 0 0 1 0 1 0;
   0 0 0 0 0 1 0 1;
   0 0 0 0 0 0 1 0];
L=-A+2*diag(ones(N1,1));
%  formation as above
Abar=-kron(L,eye(2,2));
Bbar=zeros(2*N1,1);
Cbar=eye(2*N1,2*N1);
Formation=ss(Abar, Bbar, Cbar, 0);
%
Y=[];
Y=initial(Formation,y0,Tend);   % y_i = x_i-a_i
%
%subplot(2,2,1)
plot(Y(:,1)+a1(1), Y(:,2)+a1(2), 'b');
hold on
plot(Y(:,3)+a2(1), Y(:,4)+a2(2), 'b');
plot(Y(:,5)+a3(1), Y(:,6)+a3(2), 'b');
plot(Y(:,7)+a4(1), Y(:,8)+a4(2), 'b');
plot(Y(:,9)+a5(1), Y(:,10)+a5(2), 'b');
plot(Y(:,11)+a6(1), Y(:,12)+a6(2), 'b');
plot(Y(:,13)+a7(1), Y(:,14)+a7(2), 'b');
plot(Y(:,15)+a8(1), Y(:,16)+a8(2), 'b');
%  initial positions
plot(Y(1,1)+a1(1), Y(1,2)+a1(2),'bo');
plot(Y(1,3)+a2(1), Y(1,4)+a2(2),'bo');
plot(Y(1,5)+a3(1), Y(1,6)+a3(2),'bo');
plot(Y(1,7)+a4(1), Y(1,8)+a4(2),'bo');
plot(Y(1,9)+a5(1), Y(1,10)+a5(2),'bo');
plot(Y(1,11)+a6(1), Y(1,12)+a6(2),'bo');
plot(Y(1,13)+a7(1), Y(1,14)+a7(2),'bo');
plot(Y(1,15)+a8(1), Y(1,16)+a8(2),'bo');
%   final positions
plot(Y(end,1)+a1(1), Y(end,2)+a1(2),'b+');
plot(Y(end,3)+a2(1), Y(end,4)+a2(2),'b+');
plot(Y(end,5)+a3(1), Y(end,6)+a3(2),'b+');
plot(Y(end,7)+a4(1), Y(end,8)+a4(2),'b+');
plot(Y(end,9)+a5(1), Y(end,10)+a5(2),'b+');
plot(Y(end,11)+a6(1), Y(end,12)+a6(2),'b+');
plot(Y(end,13)+a7(1), Y(end,14)+a7(2),'b+');
plot(Y(end,15)+a8(1), Y(end,16)+a8(2),'b+');
%   Center of formation
wT=ones(1,N1)*inv(L+ones(N1,N1));
xbar=wT(1)*y0(1:2)+wT(2)*y0(3:4)+wT(3)*y0(5:6)+wT(4)*y0(7:8)...
    +wT(5)*y0(9:10)+wT(6)*y0(11:12)+wT(7)*y0(13:14)+wT(8)*y0(15:16);
%plot(xbar(1), xbar(2),'r*');
latextext(a1(1), a1(2)+0.2,'1');
latextext(a2(1), a2(2)+0.2,'2');
latexxlabel('$$x_i$$');
latexylabel('$$y_i$$');
rotateY;
axis([-1.4 1.4 -1.4 1.4]);
axis('square');
xticks([-1 0 1]);
yticks([-1 0 1]);
latextitle('Formation behaviour');
%
figure(16)
%subplot(2,2,2)
%   final positions
plot(Y(end,1)+a1(1), Y(end,2)+a1(2),'b+');
hold on
plot(Y(end,3)+a2(1), Y(end,4)+a2(2),'b+');
plot(Y(end,5)+a3(1), Y(end,6)+a3(2),'b+');
plot(Y(end,7)+a4(1), Y(end,8)+a4(2),'b+');
plot(Y(end,9)+a5(1), Y(end,10)+a5(2),'b+');
plot(Y(end,11)+a6(1), Y(end,12)+a6(2),'b+');
plot(Y(end,13)+a7(1), Y(end,14)+a7(2),'b+');
plot(Y(end,15)+a8(1), Y(end,16)+a8(2),'b+');
plot(xbar(1), xbar(2),'b*');
latexxlabel('$$x_i$$');
%latexylabel('$$y_i$$');
%rotateY;
plot([-2 2],[0 0],'k:', [0 0],[-2 2],'k:');
axis([-1.4 1.4 -1.4 1.4]);
axis('square');
xticks([-1 0 1]);
yticks([-1 0 1]);



%%  Discrete-time consensus
%  series connection   1-2-3-4-5
%
figure(17)
N1=5;
q=0.55;
P1=[1 0 0 0 0; q 1-q 0 0 0; 0 q 1-q 0 0; 0 0 q 1-q 0; 0 0 0 q 1-q];
L2=[0 0 0 0 0; 
    -1 1 0 0 0; 
    0 -1 1 0 0; 
    0 0 -1 1 0; 
    0 0 0 -1 1];
Ts=0.8;
P2=expm(-L2*Ts);
%  Network model
Abar1=P1;
Abar2=P2;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Dbar=zeros(N1,1);
Network1=ss(Abar1, Bbar, Cbar, Dbar);
Network2=ss(Abar2, Bbar, Cbar, Dbar);
x0=[1; 
    0; 
    0; 
    0; 
    0];
Tend=16;
[Y1,X1]=dinitial(Abar1, Bbar, Cbar, Dbar, x0, Tend);
[Y2,X2]=dinitial(Abar2, Bbar, Cbar, Dbar, x0, Tend);
%
subplot(2,1,1)
%xbar=Y1(end,1);
Time1=[0:1:Tend-1];
stairs(Time1,X1,'Color','b','LineWidth',1.5);
hold on
plot(Time1,X1,'bo');
%latexxlabel('$$k$$');
xleer
latexylabel('$$x_i$$');
rotateY;
axis([0 15 -0.05 1.05]);
yticks([0 0.5 1]);
%xticks([0 5 10 15]);
hold off;

%
subplot(2,1,2)
plot(Time1,X2,'b');
hold on
plot(Time1,X2,'bo');
latexxlabel('$$k$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 15 -0.05 1.05]);
yticks([0 0.5 1]);
xticks([0 5 10 15]);
hold off;


%%  Load-balancing of multiprocessors
%  bi-directional links in a grid
%  weightings so as to make the matrix P primitive
%
figure(18)
N1=9;
A=[0 1 0 1 0 0 0 0 0;
   1 0 1 0 1 0 0 0 0;
   0 1 0 0 0 1 0 0 0;
   1 0 0 0 1 0 1 0 0;
   0 1 0 1 0 1 0 1 0;
   0 0 1 0 1 0 0 0 1;
   0 0 0 1 0 0 0 1 0;
   0 0 0 0 1 0 1 0 1;
   0 0 0 0 0 1 0 1 0];
D=diag(sum(A));
L=D-A;
ktilde=0.125;
P1=eye(N1,N1)-ktilde*L;
%
Abar1=P1;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Dbar=zeros(N1,1);
Network1=ss(Abar1, Bbar, Cbar, Dbar);
%  Random initial state
%RandLoad=rng;
%save('RandLoad');
load('RandLoad');       % Seed (initialisation) of the number generator
rng(RandLoad);
x0=100*rand(1,N1);
Tend=20;
[Y1,X1]=dinitial(Abar1, Bbar, Cbar, Dbar, x0, Tend);
%
subplot(4,1,[1,2])
%xbar=Y1(end,1);
Time1=[0:1:Tend-1];
stairs(Time1,X1,'Color','b','LineWidth',1.5);
hold on
plot(Time1,X1,'bo');
plot(0, X1(1,:), 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
plot(20, sum(x0)/N1, 'bo');
latextext(21, 50, '$$\bar{x}$$');
%latexxlabel('$$k$$');
xleer
latexylabel('$$x_i$$');
rotateY;
%axis([0 15 -0.05 1.05]);
yticks([0 50 100]);
%xticks([0 5 10 15]);
hold off;
%
%   normal distribution of workload eta_i(k) - c
sigma=3;
Eta=randn(Tend,N1);    % normally distributed with sigma^2=1
Eta=Eta*sigma;         % extension to get the new variance sigma^2
X=[];
X(1,:)=x0;
for k1=1:Tend-1
    X(k1+1,:)=P1*X(k1,:)'+Eta(k1,:)';
end
subplot(4,1,[3,4])
stairs(Time1,X,'Color','b','LineWidth',1.5);
hold on
plot(Time1,X,'bo');
plot(0, X1(1,:), 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
plot(20, sum(x0)/N1, 'bo');
latextext(21, 50, '$$\bar{x}$$');
latexxlabel('$$k$$');
latexylabel('$$x_i$$');
rotateY;
%axis([0 15 -0.05 1.05]);
yticks([0 50 100]);
xticks([0 5 10 15 20]);
hold off;

%%  Performance with path-graph and additional edges
%
figure(19)
N1=5;
A1=[0 0 0 0 0; 1 0 0 0 0; 0 1 0 0 0; 0 0 1 0 0; 0 0 0 1 0];
A2=[0 0 0 0 0; 1 0 0 0 0; 1 1 0 0 0; 0 1 1 0 0; 0 1 0 1 0];
D1=diag(sum(A1'));
L1=D1-A1;
D2=diag(sum(A2'));
L2=D2-A2;
%
Abar1=-L1;
Abar2=-L2;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Dbar=zeros(N1,1);
Network1=ss(Abar1, Bbar, Cbar, Dbar);
Network2=ss(Abar2, Bbar, Cbar, Dbar);
x0=[  0.9000     % the same as for figure 7
    1.6154
    0.8263
    0.5562
    1.220];
%x0=abs(rand(N1,1));   
x0=x0/sum(x0)*N1;        %  nonnegative initial state in [0, 1]
Tend=7;
[Y1,Time1,X1]=initial(Network1, x0, Tend);
subplot(4,1,[1 2])
plot(Time1,X1,'b');
%latexxlabel('$$t$$');
xleer
latexylabel('$$x_i$$');
rotateY;
axis([0 6 0.5 1.7]);
yticks([0.5 1 1.5]);
%xticks([0 5 10 15]);
hold off;
%
%  Network with additional links
[Y2,Time2,X2]=initial(Network2, x0, Tend);
subplot(4,1,[3 4])
plot(Time2,X2,'b');
latexxlabel('$$t$$');
latexylabel('$$x_i$$');
rotateY;
axis([0 6 0.5 1.7]);
yticks([0.5 1 1.5]);
xticks([0 2 4 6]);
hold off;


%%  Switching system consensus
%   periodic switching among 3 topologies
%
figure(20)
N1=5;
A1=[0 0 0 0 1; 0 0 0 0 0; 0 0 0 0 0; 0 0 0 0 0; 1 0 0 0 0];
A2=[0 0 0 1 0; 0 0 0 0 0; 0 0 0 0 1; 1 0 0 0 0; 0 0 1 0 0];
A3=[0 1 1 0 0; 1 0 1 0 0; 1 1 0 0 0; 0 0 0 0 0; 0 0 0 0 0];
D1=diag(sum(A1));
D2=diag(sum(A2));
D3=diag(sum(A3));
L1=D1-A1;
L2=D2-A2;
L3=D3-A3;
Ts=0.05;
P1=expm(-L1*Ts);
P2=expm(-L2*Ts);
P3=expm(-L3*Ts);
%
x0=[0; 1; 0; 1; 0];
kend=31;
Time=[0:(kend-1)*3];   % overall 90 steps
X=[];
X(1,:)=x0';
k1=1;
V=[];
VS=[];
Lc=N1*eye(N1,N1)-ones(N1,N1);
V(1)=x0'*Lc*x0;
VS(1)=V(1);
for ktilde=1:kend-1
    X(k1+1,:)=P1*X(k1,:)';
    V(k1+1)=X(k1+1,:)*Lc*X(k1+1,:)';
    X(k1+2,:)=P2*X(k1+1,:)';
    V(k1+2)=X(k1+2,:)*Lc*X(k1+2,:)';
    X(k1+3,:)=P3*X(k1+2,:)';
    V(k1+3)=X(k1+3,:)*Lc*X(k1+3,:)';
    VS(ktilde+1)=V(k1+3);
    k1=k1+3;
end
%
subplot(4,3,[1 2 4 5])
%xbar=Y1(end,1);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
%plot(Time,X,'ko');
%latexxlabel('$$k$$');
xleer
latexylabel('$$x_i$$');
rotateY;
axis([0 91 -0.05 1.05]);
yticks([0 0.5 1]);
xticksempty([0 20 40 60 80]);
% consensus value
xbar=sum(x0)/N1;
plot(91, xbar, 'bo');
latextext(94, xbar, '$$\bar{x}$$');
hold off;
%
subplot(4,3,[7 8 10 11])
Time2=[0:3:(kend-1)*3];
stairs(Time,V,'Color','b','LineWidth',1.5);
hold on
plot(Time2,VS,'bo');
latexxlabel('$$k$$');
latexylabel('$$V$$');
rotateY;
axis([0 90 -0.05 6.1]);
yticks([0 2 4 6]);
xticks([0 20 40 60 80]);
hold off;
%
subplot(4,3,[9 12])
Time2=[0:3:(kend-1)*3];
stairs(Time,V,'Color','b','LineWidth',1.5);
hold on
plot(Time2,VS,'bo');
for k1=1:6
   plot([k1 k1],[0 7],'k:'); 
end
latexxlabel('$$k$$');
%latexylabel('$$V$$');
rotateY;
axis([0 7 3.5 6.1]);
yticks([4 5 6]);
xticks([0 3 6 9]);
hold off;



%%  Deterministic gossiping
%  
%
figure(21)
N1=5;
A1=[0 0 0 0 1; 0 0 0 0 0; 0 0 0 0 0; 0 0 0 0 0; 1 0 0 0 0];
A2=[0 1 0 0 0; 1 0 0 0 0; 0 0 0 1 0; 0 0 1 0 0; 0 0 0 0 0];
A3=[0 0 0 0 0; 0 0 1 0 0; 0 1 0 0 0; 0 0 0 0 1; 0 0 0 1 0];
D1=diag(sum(A1));
D2=diag(sum(A2));
D3=diag(sum(A3));
L1=D1-A1;
L2=D2-A2;
L3=D3-A3;
Ts=0.05;
P1=expm(-L1*Ts);
P2=expm(-L2*Ts);
P3=expm(-L3*Ts);
%
x0=[1; 0.2; 0; 0.7; 0];
xbar=sum(x0)/N1;
kend=27;
Time=[0:(kend-1)*3];
X=[];
X(1,:)=x0';
k1=1;
V=[];
V(1)=max(x0)-min(x0);            % width of the trajectory bundle
for ktilde=1:kend-1
    X(k1+1,:)=P1*X(k1,:)';
    V(k1+1)=max(X(k1+1,:)')-min(X(k1+1,:)');
    X(k1+2,:)=P2*X(k1+1,:)';
    V(k1+2)=max(X(k1+2,:)')-min(X(k1+2,:)');
    X(k1+3,:)=P3*X(k1+2,:)';
    V(k1+3)=max(X(k1+3,:)')-min(X(k1+3,:)');
    k1=k1+3;
end
%
subplot(4,1,[1,2])
%xbar=Y1(end,1);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
plot(80, sum(x0)/N1, 'bo');
latextext(83, xbar, '$$\bar{x}$$');
xleer
latexylabel('$$x_i$$');
rotateY;
axis([0 80 -0.05 1.05]);
yticks([0 0.5 1]);
hold off;
%
subplot(4,1,[3 4])
stairs(Time,V,'Color','b','LineWidth',1.5);
hold on
latexxlabel('$$k$$');
latexylabel('$$\Delta x$$');
rotateY;
axis([0 80 -0.05 1.05]);
yticks([0 0.5 1]);
xticks([0 20 40 60 80]);
hold off;


%%  Deterministic gossiping: vehicle example
%
%   Leader should distribute its velocity to all followers
figure(22)
N1=5;
NG=4;      % number of graphs
kend=40; 
Time=0:1:kend;
%
k3=0;
Xbar=[];
Sigma=[];
Sigma(1)=0;   
k2=0;
for k1=2:kend+1
    k2=k2+1;
    Sigma(k1)=mod(Sigma(k1-1)+1,NG);
end
Sigma=Sigma+1;
subplot(3,1,1)
plot(Time, Sigma,'bo');
hold on
stairs(Time,Sigma,'Color','b','LineWidth',1.5);
yticks([1 2 3 4]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 kend 0.8 NG+0.2]);
hold off
%
%  Network model
P1=diag(ones(1,N1));
P1(5,4)=0.5;
P1(5,5)=0.5;
P2=diag(ones(1,N1));
P2(4,3)=0.5;
P2(4,4)=0.5;
P3=diag(ones(1,N1));
P3(3,2)=0.5;
P3(3,3)=0.5;
P4=diag(ones(1,N1));
P4(2,1)=0.5;
P4(2,2)=0.5;
%   deterministic initial state
x0=zeros(N1,1);
x0(1,1)=1;
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=P1*X(k1-1,:)';
    end
    if Sigma(k1)==2
        X(k1,:)=P2*X(k1-1,:)';
    end
    if Sigma(k1)==3
        X(k1,:)=P3*X(k1-1,:)';
    end
    if Sigma(k1)==4
        X(k1,:)=P4*X(k1-1,:)';
    end
end
subplot(3,1,[2,3]);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
plot(Time, X,'bo');
axis([0 kend -0.1 1.1]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
latexxlabel('$$k$$');
xticks([0 10 20 30 40]);
hold off

%%  reversed order of the communication graphs
%
figure(23)
Sigma=[];
Sigma(1)=3;   
k2=0;
for k1=2:kend+1
    k2=k2+1;
    Sigma(k1)=mod(Sigma(k1-1)-1,NG);
end
Sigma=Sigma+1;
subplot(3,1,1)
plot(Time, Sigma,'bo');
hold on
stairs(Time,Sigma,'Color','b','LineWidth',1.5);
yticks([1 2 3 4]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 kend 0.8 NG+0.2]);
hold off
%
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=P1*X(k1-1,:)';
    end
    if Sigma(k1)==2
        X(k1,:)=P2*X(k1-1,:)';
    end
    if Sigma(k1)==3
        X(k1,:)=P3*X(k1-1,:)';
    end
    if Sigma(k1)==4
        X(k1,:)=P4*X(k1-1,:)';
    end
end
subplot(3,1,[2,3]);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
plot(Time, X,'bo');
axis([0 kend -0.1 1.1]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
latexxlabel('$$k$$');
xticks([0 10 20 30 40]);
hold off

%%  Random gossiping
%
figure(24)
%RandGoss=rng;
%save('RandGoss');
load('RandGoss');       % Seed (initialisation) of the number generator
rng(RandGoss);
Sigma=[];
P=rand(1,kend+1);
for k1=1:kend+1
    if P(k1)<=0.25
        Sigma(k1)=1;
    end
    if P(k1)>0.25 && P(k1) <=0.5
        Sigma(k1)=2;
    end
    if P(k1)>0.5 && P(k1) <=0.75
        Sigma(k1)=3;
    end
    if P(k1)>0.75 && P(k1) <=1
        Sigma(k1)=4;
    end
end
subplot(3,1,1)
plot(Time, Sigma,'bo');
hold on
stairs(Time,Sigma,'Color','b','LineWidth',1.5);
yticks([1 2 3 4]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 kend 0.8 NG+0.2]);
hold off
%
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=P1*X(k1-1,:)';
    end
    if Sigma(k1)==2
        X(k1,:)=P2*X(k1-1,:)';
    end
    if Sigma(k1)==3
        X(k1,:)=P3*X(k1-1,:)';
    end
    if Sigma(k1)==4
        X(k1,:)=P4*X(k1-1,:)';
    end
end
subplot(3,1,[2,3]);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
plot(Time, X,'bo');
axis([0 kend -0.1 1.1]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
latexxlabel('$$k$$');
xticks([0 10 20 30 40]);
hold off
%
%  Eigenvalues of the expected matrix E(P)
EP=1/4*(P1+P2+P3+P4);
Lambda=eig(EP);
Lambdasorted=sort(Lambda, 'descend');
Lambdasorted(2)


%%  Random gossiping (repetition with other random sequence)
%
figure(25)
%RandGoss2=rng;
%save('RandGoss2');
load('RandGoss2');       % Seed (initialisation) of the number generator
rng(RandGoss2);
Sigma=[];
P=rand(1,kend+1);
for k1=1:kend+1
    if P(k1)<=0.25
        Sigma(k1)=1;
    end
    if P(k1)>0.25 && P(k1) <=0.5
        Sigma(k1)=2;
    end
    if P(k1)>0.5 && P(k1) <=0.75
        Sigma(k1)=3;
    end
    if P(k1)>0.75 && P(k1) <=1
        Sigma(k1)=4;
    end
end
subplot(3,1,1)
plot(Time, Sigma,'bo');
hold on
stairs(Time,Sigma,'Color','b','LineWidth',1.5);
yticks([1 2 3 4]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 kend 0.8 NG+0.2]);
hold off
%
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=P1*X(k1-1,:)';
    end
    if Sigma(k1)==2
        X(k1,:)=P2*X(k1-1,:)';
    end
    if Sigma(k1)==3
        X(k1,:)=P3*X(k1-1,:)';
    end
    if Sigma(k1)==4
        X(k1,:)=P4*X(k1-1,:)';
    end
end
subplot(3,1,[2,3]);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
plot(Time, X,'bo');
axis([0 kend -0.1 1.1]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
latexxlabel('$$k$$');
xticks([0 10 20 30 40]);
hold off

%%  Random gossiping
%  the same sequence of graphs, but with extended graphs
%
figure(26)
%  Network model
P1=diag(ones(1,N1));
P1(5,4)=0.5;
P1(5,5)=0.5;
P2=diag(ones(1,N1));
P2(4,3)=0.5;
P2(4,4)=0.5;
P3=diag(ones(1,N1));
P3(3,2)=0.5;
P3(3,3)=0.5;
P4=diag(ones(1,N1));
P4(2,1)=0.5;
P4(2,2)=0.5;
%  Network model extended
P2(5,3)=0.5;
P2(5,5)=0.5;
P3(4,2)=0.5;
P3(4,4)=0.5;
P4(3,1)=0.5;
P4(3,3)=0.5;
subplot(3,1,1)
plot(Time, Sigma,'bo');
hold on
stairs(Time,Sigma,'Color','b','LineWidth',1.5);
yticks([1 2 3 4]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 kend 0.8 NG+0.2]);
hold off
%
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=P1*X(k1-1,:)';
    end
    if Sigma(k1)==2
        X(k1,:)=P2*X(k1-1,:)';
    end
    if Sigma(k1)==3
        X(k1,:)=P3*X(k1-1,:)';
    end
    if Sigma(k1)==4
        X(k1,:)=P4*X(k1-1,:)';
    end
end
subplot(3,1,[2,3]);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
plot(Time, X,'bo');
axis([0 kend -0.1 1.1]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
latexxlabel('$$k$$');
xticks([0 10 20 30 40]);
hold off
%
%  Eigenvalues of the expected matrix E(P)
EP=1/4*(P1+P2+P3+P4);
Lambda=eig(EP);
Lambdasorted=sort(Lambda, 'descend');
Lambdasorted(2)


%%  Averaging with random noise
%
figure(27)
%  Network model
P1=diag(ones(1,N1));
P1(5,4)=0.5;
P1(5,5)=0.5;
P2=diag(ones(1,N1));
P2(4,3)=0.5;
P2(4,4)=0.5;
P3=diag(ones(1,N1));
P3(3,2)=0.5;
P3(3,3)=0.5;
P4=diag(ones(1,N1));
P4(2,1)=0.5;
P4(2,2)=0.5;
RandGoss3=rng;
save('RandGoss3');
%load('RandGoss3');       % Seed (initialisation) of the number generator
%rng(RandGoss3);
%                  random noise added to the transferred information
dmax=0.2;
D=dmax*(rand(kend+1,1)-0.5);  
%   Switching sequences as before
subplot(3,1,1)
plot(Time, Sigma,'bo');
hold on
stairs(Time,Sigma,'Color','b','LineWidth',1.5);
yticks([1 2 3 4]);
latexylabel('$$\sigma$$');
xleer;
rotateY;
axis([0 kend 0.8 NG+0.2]);
hold off
%
X=[];
X(1,:)=x0';
for k1=2:kend+1
    if Sigma(k1)==1
        X(k1,:)=P1*X(k1-1,:)';
        X(k1,5)=X(k1,5)+0.5*D(k1);
    end
    if Sigma(k1)==2
        X(k1,:)=P2*X(k1-1,:)';
        X(k1,4)=X(k1,4)+0.5*D(k1);
    end
    if Sigma(k1)==3
        X(k1,:)=P3*X(k1-1,:)';
        X(k1,3)=X(k1,3)+0.5*D(k1);
    end
    if Sigma(k1)==4
        X(k1,:)=P4*X(k1-1,:)';
        X(k1,2)=X(k1,2)+0.5*D(k1);
    end
end
subplot(3,1,[2,3]);
stairs(Time,X,'Color','b','LineWidth',1.5);
hold on
plot(Time, X,'bo');
axis([0 kend -0.1 1.1]);
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
yticks([0 1 2]);
latexxlabel('$$k$$');
xticks([0 10 20 30 40]);
hold off



%%  Rendesvous problem of robots
%   6 robots with initial states x_{i0}=(x_{i0}, y_{i0})'
%   should meet in the average position xbar
figure(28)
%  Network model
%  Adjacency matrix
N1=6;
A=[0 1 1 1 0 0;
   1 0 1 0 0 0;
   1 1 0 1 1 1;
   1 0 1 0 0 0;
   0 0 1 0 0 1;
   0 0 1 0 1 0];
D=diag(sum(A'));
LRobot=D-A;
%  Network model
ARobot=-kron(LRobot, eye(2,2));
Bbar=zeros(2*N1,1);
Cbar=eye(2*N1,2*N1);
Robots=ss(ARobot, Bbar, Cbar, 0);
%  initial positions
x1=[1; 4];
x2=[3; 5];
x3=[2; 3];
x4=[1; 1];
x5=[4; 2];
x6=[5; 4];
x0=[x1; x2; x3; x4; x5; x6];
xbar=1/N1*(x1+x2+x3+x4+x5+x6);
[Y,Time]=initial(Robots,x0);
tend=Time(end);
for k1=1:N1
    plot(Y(:,2*(k1-1)+1), Y(:,2*(k1-1)+2),'b');
    hold on
    plot(Y(1,2*(k1-1)+1), Y(1,2*(k1-1)+2),'b+');
    plot(Y(end,2*(k1-1)+1), Y(end,2*(k1-1)+2),'bo');
end
latexxlabel('$$x$$');
latexylabel('$$y$$');
rotateY;
axis([0.7 5.3 0.7 5.3]);
axis('square');
yticks([1 2 3 4 5]);
xticks([1 2 3 4 5]);
hold off;

%%  Temporal behaviour of the rendezvous problem
%
figure(29)
dt=Time(2)-Time(1);
ke1=10;
ke2=50;
ke3=150;
subplot(2,3,1)
for k1=1:N1
    plot(Y(ke1,2*(k1-1)+1), Y(ke1,2*(k1-1)+2),'b+');
    hold on
end
Formation=[Y(ke1,1:2)' Y(ke1,5:6)' Y(ke1,7:8)' Y(ke1,9:10)'...
         Y(ke1,11:12)' Y(ke1,3:4)' Y(ke1,1:2)'];
plot(Formation(1,:), Formation(2,:), 'b');
axis([0.7 5.3 0.7 5.3]);
axis('square');
yticks([1 2 3 4 5]);
xticks([1 2 3 4 5]);
latextext(2, 6, ['$$t=$$', num2str(ke1*dt,2)]);
hold off
%
subplot(2,3,2)
for k1=1:N1
    plot(Y(ke2,2*(k1-1)+1), Y(ke2,2*(k1-1)+2),'b+');
    hold on
end
Formation=[Y(ke2,1:2)' Y(ke2,5:6)' Y(ke2,7:8)' Y(ke2,9:10)'...
         Y(ke2,11:12)' Y(ke2,3:4)' Y(ke2,1:2)'];
plot(Formation(1,:), Formation(2,:), 'b');
latextext(2, 6, ['$$t=$$', num2str(ke2*dt,2)]);
axis([0.7 5.3 0.7 5.3]);
axis('square');
yticks([1 2 3 4 5]);
xticks([1 2 3 4 5]);
hold off
%
subplot(2,3,3)
for k1=1:N1
    plot(Y(ke3,2*(k1-1)+1), Y(ke3,2*(k1-1)+2),'b+');
    hold on
end
Formation=[Y(ke3,1:2)' Y(ke3,5:6)' Y(ke3,7:8)' Y(ke3,9:10)'...
         Y(ke3,11:12)' Y(ke3,3:4)' Y(ke3,1:2)'];
plot(Formation(1,:), Formation(2,:), 'b');
latextext(2, 6, ['$$t=$$', num2str(ke3*dt,2)]);

axis([0.7 5.3 0.7 5.3]);
axis('square');
yticks([1 2 3 4 5]);
xticks([1 2 3 4 5]);
hold off


%%  Consensus over delayed network
%
figure(30)
%  bidirectional connections   1-2-3-4-5
N1=5;
tau=0.3;                 % dead time
Omega=logspace(-1, 1.3, 600);
A=zeros(N1, N1);
for k1=2:N1
    A(k1,k1-1)=1;
    A(k1-1,k1)=1;
end
D=diag(sum(A'));
LPath=D-A;
L22tilde=LPath(2:end,2:end)-ones(N1-1,1)*LPath(1,2:end);
[V, D]=eig(L22tilde);
for k1=1:N1-1
    lambda(k1)=D(k1,k1);
end
%  draw the Nyquist plot of the transfer functions with delay
%  determine the Nyquist plot
komega=0;
G=[];
for omega=Omega
    komega=komega+1;
    G(komega)=exp(-j*omega*tau)/(j*omega);
end
for k2=1:N1-1
    plot(real(lambda(k2)*G), imag(lambda(k2)*G), 'b')
    hold on
end
plot([-2 2],[0 0],'k--', [0 0],[-2 2],'k--');
plot([-1 -1], [-0.05 0.05], 'k');
latextext(-1.2, 0.2, '$$-1$$');
axis('square');
axis([-2 1 -2 1]);
latexxlabel('Re');
latexylabel('Im');
rotateY;
yticks([-2 -1 0 1]);
xticks([-2 -1 0 1]);
latextitle('$${\tt ConsensusDemo:}$$ Consensus test');
hold off
    
%%   Simulation and comparison with figure(7)
%
figure(31)
Tend=3;
Ts=0.01;                  % sampling time for simulation
Time=[0:Ts:Tend];
Abar1=-LPath;
Bbar=zeros(N1,1);
Cbar=eye(N1,N1);
Network1=ss(Abar1, Bbar, Cbar, 0);
x0=[  0.9000
    1.6154
    0.8263
    0.5562
    1.220];
%x0=abs(rand(N1,1));   
x0=x0/sum(x0)*N1;        %  nonnegative initial state in [0, 1]
Y1=initial(Network1,x0,Time);
%
subplot(4,1,[1 2])
xbar=Y1(end,1);
tend=Time(end);
plot(Time,Y1,'b', [0 tend], [xbar, xbar],'k:');
hold on
xleer;
latexylabel('$$x_i$$');
rotateY;
axis([0 2.7 0.4 1.7]);
yticks([0.5 1 1.5]);
latextitle('$${\tt ConsensusDemo:}$$ Delay-free and delayed consensus');
%
%  Delayed consensus
%Ad=expm(-L*Ts);
ktau=tau/Ts;
X=[];
for k1=1:ktau    % assume that the system has a constant state for t<0
   X(k1,:)=x0;
end
for k1=ktau+1:Tend/Ts+1
    Xdot=-LPath*X(k1-ktau,:)';
    X(k1,:)=X(k1-1,:)+Xdot'*Ts;
end
subplot(4,1,[3 4])
plot(Time(ktau:end)-tau, X(ktau:end,:),'b');
plot(Time, X,'b');
hold on
latexylabel('$$x_i$$');
rotateY;
axis([0 2.7 0.4 1.7]);
yticks([0.5 1 1.5]);
xticks([0 1 2 3]);
hold off

%%  Discrete-time consensus
%   distributed estimation in sensor networks
%
figure(32)
%  undirected path graph   1-2-3-4-5
subplot(3,1,[1,2])
N1=5;
A=zeros(N1, N1);
for k1=2:N1
    A(k1,k1-1)=1;
    A(k1-1,k1)=1;
end
L=diag(sum(A))-A;
%  Network model
q=0.15;
P=eye(N1,N1) - q*L;
x0=[  0.9000
    1.6154
    0.8263
    0.5562
    1.220];
kend=20;
%x0=abs(rand(N1,1));   
x0=x0/sum(x0)*N1;        %  nonnegative initial state in [0, 1]
YDiskr=dinitial(P, zeros(N1,1), eye(N1,N1), zeros(N1,1),x0,kend);
stairs([0:kend-1],YDiskr,'Color','b','LineWidth',1.5);
hold on
plot([0:kend-1], YDiskr, 'bo');
plot(0, YDiskr(1,:), 'bo',...
            'MarkerEdgeColor','b',...
            'MarkerFaceColor','b');
xbar=sum(x0)/N1;
plot(kend, xbar, 'bo');
latextext(20.7, xbar, '$$\bar{x}$$');
%latexxlabel('$$t$$');
xleer;
latexylabel('$$x_i$$');
rotateY;
latexxlabel('$$k$$');
axis([0 kend 0.4 1.7]);
yticks([0.5 1 1.5]);
xticks([0 5 10 15 20]);
latextitle('Distributed estimation algorithm');
hold off;



%%   Figures
%
epsfigc15('ConsensusDemo');

