%      SyncDemo.M
%   
%  Figures to demonstrate the synchronisation problem
%   Fig. 1 - 5: Elgersburg/OscillatorsPhDSchool.m
%  
% J. Lunze
% 
% 5.2.2018
% Version v. 23.12.2018
% for 2nd edition: 12.6.2021
%
echo off
clear
close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%
%%  Unstable oscillator network with N=2 nodes
figure(1)
%   Oscillator model
A=[0.1 3; -3 0.1];
B=[1; 1];      
C=[1 10];
D=0;
n=2;
%  in the synchronisation analysis the feedback gain is lambda2*k
%  for the coupled oscillators, lambda2=2 holds
Oscillator=ss(A, B, C, D);
rlocprintblue(Oscillator);
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-4 4 -4 4])
axis('square');
plot([-6 6],[0 0],'k:', [0 0],[-5 5],'k:');
latextitle('$${\tt SyncDemo:}$$ Root locus of the unstable oscillator');
% rlocfind(Oscillator)    %  stable for {\itk}=0.017 ... 0.321
Eig=rlocus(Oscillator,0.018);
plot(real(Eig), imag(Eig), 'bd');
latextext(0.2, 2.5, '$$k_{\rm crit}=0.018$$');
Eig=rlocus(Oscillator,0.32);
plot(real(Eig), imag(Eig), 'bd');
latextext(0.2, 0.4, '$$k_{\rm crit}=0.32$$');
ks=0.2;
Eig=rlocus(Oscillator,ks);
plot(real(Eig), imag(Eig), 'bx');
latextext(-3, 1.8, '$$k=0.2$$');
xticks([-4 -2 0 2 4]);
yticks([-4 -2 0 2 4]);
hold off
%

%%  Behaviour of the oscillator network for k=0.1  (stable modes)
%
figure(2)
%  Laplacian matrix
L=[1 -1; -1 1];
lambda2=2;
ks=ks/lambda2;
N=2;
Abar=kron(diag(ones(N,1)), A)-kron(ks*L, B*C);
Bbar=zeros(N*n, 1);
Cbar=kron(diag(ones(N,1)), C);
Dbar=zeros(N, 1);
OscillatorNetwork=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
x0=[0; 0; 1; 1];
Tend=10;
dt=0.01;
T=0:dt:Tend;
[Y, T]=initial(OscillatorNetwork, x0, T);
subplot(2,1,1)
plot(T, Y(:,1), 'b', T, Y(:,2), 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -13 13]);
latexxlabel('$$t$$');
%xleer;
latexylabel('$$y_i$$');
rotateY;
latextitle('$${\tt SyncDemo:}$$ 2 unstable oscillators');
xticks([0 5 10]);
yticks([-10 0 10]);
hold off
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Harmonic oscillator
figure(3)
%   New oscillator model
A=[0 3; -3 0];
Oscillator=ss(A, 2*B, C, D);
rlocprintblue(Oscillator);
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-4 4 -4 4])
axis('square');
plot([-6 6],[0 0],'k:', [0 0],[-5 5],'k:');
latextitle('$${\tt SyncDemo:}$$ Root locus of the stable oscillator');
% rlocfind(Oscillator)    %  stable for k=0 ... 0.334
Eig=rlocus(Oscillator,0.167);
plot(real(Eig), imag(Eig), 'kd');
latextext(0.2, 0.4, '$$k_{\rm crit}=0.167$$');
ks=0.1;
Eig=rlocus(Oscillator,ks);
plot(real(Eig), imag(Eig), 'kp');
latextext(-3, 2, '$$k=0.1$$');
ks2=0.15;
Eig=rlocus(Oscillator,ks2);
plot(real(Eig), imag(Eig), 'k*');
latextext(-3.8, 0.5, '$$k=0.15$$');
xticks([-4 -2 0 2 4]);
yticks([-4 -2 0 2 4]);
hold off
%


%%  Behaviour of the oscillator network for k=0.3  (stable modes)
%
figure(4)
Abar=kron(diag(ones(N,1)), A)-kron(ks*L, B*C);
Bbar=zeros(N*n, 1);
Cbar=kron(diag(ones(N,1)), C);
Dbar=zeros(N, 1);
OscillatorNetwork=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
x0=[0; 0; 1; 1];
Tend=10;
dt=0.01;
T=0:dt:Tend;
[Y, T]=initial(OscillatorNetwork, x0, T);
subplot(4,1,[1 2])
plot(T, Y(:,1), 'b', T, Y(:,2), 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -13 13]);
%latexxlabel('$$t$$');
latexylabel('$$y_i$$');
rotateY;
latextext(7, 10, '$$k=0.1$$');
latextitle('$${\tt SyncDemo:}$$ 2 harmonic oscillators');
xticks([0 5 10]);
xleer;
yticks([-10 0 10]);
hold off
%   other feedback gain: ks2
Abar=kron(diag(ones(N,1)), A)-kron(ks2*L, B*C);
OscillatorNetwork2=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
[Y2, T]=initial(OscillatorNetwork2, x0, T);
subplot(4,1,[3 4])
plot(T, Y2(:,1), 'b', T, Y2(:,2), 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -13 13]);
latexxlabel('$$t$$');
%xleer;
latexylabel('$$y_i$$');
rotateY;
latextext(7, 10, '$$k=0.15$$');
xticks([0 5 10]);
yticks([-10 0 10]);
hold off

%%   Change of the oscillators parameters bring about asymptotic stability
%
figure(21)
A1=[0 3; -3 0];
A2=[0 2.5; -2.5 0];
N1=2;
% ks=0.1
Abar=[A1 zeros(2,2); zeros(2,2) A2]-kron(ks*L, B*C);
Bbar=zeros(N1*n, 1);
Cbar=kron(diag(ones(N1,1)), C);
Dbar=zeros(N1,1);
OscillatorNetwork=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
x0=[0; 0; 1; 1];
Tend2=40;
dt=0.01;
Time2=0:dt:Tend2;
Y=initial(OscillatorNetwork, x0, Time2);
subplot(4,1,[1 2])
plot(Time2, Y(:,1), 'b', Time2, Y(:,2), 'b');
hold on
plot([0 Tend2],[0 0],'k:');
axis([0 Tend2 -13 13]);
%latexxlabel('$$t$$');
latexylabel('$$y_i$$');
rotateY;
latextitle('$${\tt SyncDemo:}$$ 2 harmonic oscillators');
xticks([0 10 20 30 40]);
latexxlabel('$$t$$');
yticks([-10 0 10]);
hold off

%%  Relation between the synchronous trajectory and the communication graph
%
figure(5)
% initial states  
x10=[1;1];
x20=[0;0];
x0=[x10; x20];
% bi-directional coupling
L=[1 -1;-1 1];
wT=ones(1,2)*inv(ones(2,2)-L);   % left eigenvector of L
xs0=wT(1,1)*x10+wT(1,2)*x20;
Reference=ss(A, B, C, D);
Ys=initial(Reference,xs0,T);
subplot(4,1,[1 2])
plot(T, Y2, 'b');    % agent outputs as before
hold on
plot(T, Ys, 'k--');
axis([0 Tend -13 13]);
latexxlabel('$$t$$');
xleer;
latexylabel('$$y_i$$');
rotateY;
xticksempty([0 5 10]);
yticks([-10 0 10]);
latextitle('$${\tt SyncDemo:}$$ 2 stable oscillators with different communication');
hold off
%
%  Series connection of both oscillators
L=[0 0; -1 1];
wT=ones(1,2)*inv(ones(2,2)-L);   % left eigenvector of L
xs0=wT(1,1)*x10+wT(1,2)*x20;
Reference=ss(A, B, C, D);
Ys=initial(Reference,xs0,T);
subplot(4,1,[3 4])
plot(T, Ys, 'k--');
hold on
Abar=kron(diag(ones(N,1)), A)-kron(ks*L, B*C);
OscillatorSeries=ss(Abar, Bbar, Cbar, Dbar);
Y3=initial(OscillatorSeries, x0, T);
plot(T, Y3,'b');  
latexxlabel('$$t$$');
%xleer;
latexylabel('$$y_i$$');
rotateY;
xticks([0 5 10]);
yticks([-10 0 10]);
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Oscillator trajectories in the two-dimensional state space
%
figure(6)
A=[0 3; -3 0];
B=[1; 1];      
C=[1 10];
D=0;
n=2;
L=[1 -1;-1 1];  % bidirectional couplings
ks=0.13;
Abar=kron(diag(ones(N,1)), A)-kron(ks*L, B*C);
Bbar=zeros(N*n, 1);
Cbar=kron(diag(ones(N,1)), C);
Dbar=zeros(N, 1);
OscillatorNetwork=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
x0=[0; 0.4; 1; -0.6];
Tend=10;
dt=0.01;
Time=0:dt:Tend;
Y=initial(OscillatorNetwork, x0, Time);
plot(Y(:,1), Y(:,2), 'b')
hold on
plot(Y(1,1), Y(1,2), 'bo');
plot([-11 11],[-11 11], 'k');
axis([-11 9 -11 9]);
axis('square');
yticks([-10 -5 0 5 10]);
xticks([-10 -5 0 5 10]);
latexxlabel('$$y_1$$');
latexylabel('$$y_2$$');
rotateY;
latextitle('$${\tt SyncDemo:}$$ Two coupled oscillators');
hold off





%%  Unstable oscillator network with N=2 nodes
%   with individual dynamics due to different actuators
figure(7)
%   Oscillator models
A=[0.1 3; -3 0.1];
B=[1; 1];      
C=[1 10];
D=0;
n=2;
%   Actuator models
T1=1;     % time constant of actuator 1
T2=2;     % time constant of actuator 2
Aa1=-1/T1;
Ba1=1/T1;
Ca1=1;
Da1=0;
Aa2=-1/T2;
Ba2=1/T2;
Ca2=1;
Da2=0;
%   Agent models
A1=[A  B*Ca1; zeros(1,n) Aa1];
B1=[zeros(n,1); Ba1];
C1=[C 0];
D1=0;
A2=[A  B*Ca2; zeros(1,n) Aa2];
B2=[zeros(n,1); Ba2];
C2=[C 0];
D2=0;
nTS=3;
%  Overall system with leader-follower structure
ks=0.1;
Abar=[A1-B1*ks*C2 B1*ks*C2; zeros(nTS, nTS) A2];
Bbar=zeros(2*nTS, 1);
Cbar=[C1 zeros(1,nTS); zeros(1,nTS) C2];
Dbar=zeros(2,1);
OscillatorNetwork=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
x0=[0; 0; 0; 1; 1; 0];
Tend=13;
dt=0.01;
T=0:dt:Tend;
[Y, T]=initial(OscillatorNetwork, x0, T);
subplot(2,1,1)
plot(T, Y, 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -40 40]);
xlabel('{\it t}');
%xleer;
ylabel('{\it y_i}');
rotateY;
title('2 coupled oscillators');
hold off
%


%%  10 oscillators
%
figure(8)
N1=10;
A=[0 3; -3 0];
n=2;
%   complete interactions
L=-ones(N1,N1)+N1*eye(N1,N1);
Lnorm=L/(N1-1);
ks=0.1;
Abar=kron(diag(ones(N1,1)), A)-kron(ks*Lnorm, B*C);
Bbar=zeros(N1*n, 1);
Cbar=kron(diag(ones(N1,1)), C);
Dbar=zeros(N1, 1);
OscillatorN=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
x0=2*rand(n*N1,1)-1;
Tend=10;
dt=0.01;
T=0:dt:Tend;
Y3=initial(OscillatorN, x0, T);
axis([0 Tend -13 13]);
subplot(2,1,1)
plot(T, Y3, 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -13 13]);
latexxlabel('$$t$$');
%xleer;
latexylabel('$$y_i$$');
rotateY;
latextitle(['$${\tt SyncDemo:}$$ Coupled oscillators ($$N=$$', num2str(N1), ')']);
xticks([0 5 10]);
yticks([-10 0 10]);
hold off
%
%  series interconnection, same x0
L=-diag(ones(N1-1,1),-1)+eye(N1,N1);
L(1,1)=0;
Lnorm=L;
Abar=kron(diag(ones(N1,1)), A)-kron(ks*Lnorm, B*C);
Tend2=40;
dt=0.01;
T2=0:dt:Tend2;
OscillatorN=ss(Abar, Bbar, Cbar, Dbar);
Y4=initial(OscillatorN, x0, T2);
subplot(2,1,2)
plot(T2, Y4,'b');
hold on
plot([0 Tend2],[0 0],'k:');
axis([0 Tend2 -16 16]);
latexxlabel('$$t$$');
latexylabel('$$y_i$$');
rotateY;
xticks([0 10 20 30 40]);
yticks([-10 0 10]);
axis([0 Tend2 -13 13]);
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Double integrator agents
%   Root locus
%
figure(9)
AI=[0 1; 0 0];
BI=[0; 1];
CI=[1 0; 0 1];
DI=[0; 0];
n=2;
K=[1 1];
DIntegrator=ss(AI, BI, K*CI, 0);
rlocprintblue(DIntegrator);
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-3 1 -2 2])
axis('square');
plot([-6 6],[0 0],'k:', [0 0],[-5 5],'k:');
latextitle('$${\tt SyncDemo:}$$ Root locus of the double integrator');
% rlocfind(Oscillator)    %  stable for k=0 ... 0.334
Eig=rlocus(DIntegrator,2);
plot(real(Eig), imag(Eig), 'kd');
latextext(-1, 1.5, '$$k=2$$');
ks=4;
Eig=rlocus(DIntegrator,ks);
plot(real(Eig), imag(Eig), 'k*');
latextext(-2.8, 0.4, '$$k=4$$');
xticks([-4 -2 0 2 4]);
yticks([-4 -2 0 2 4]);
hold off



%%  Double integrator agents
%   Behaviour
figure(10)
N1=7;
%  complete interactions
L=-ones(N1,N1)+N1*eye(N1,N1);
ks=2;    % from figure(9)
ksn=ks*(N1-1)/N1;
Abar=kron(diag(ones(N1,1)), AI)-kron(ksn*L, BI*K*CI);
Bbar=zeros(N1*n, 1);
Cbar=kron(diag(ones(N1,1)), [1 0]);   % plot the agent positions
Dbar=zeros(N1, 1);
DoubleIntegrators=ss(Abar, Bbar, Cbar, Dbar);
% initial states if the agents 2, 3, 4 are initially synchronised  
%  with x_s0 = (1, 1)
%    x_i1  x_i2
x0=[  0.2;     0.7;
      1;     1;
      1;     1;
      1;     1;
      2;     2;
      1.8;   1.3;
      0;     0];
Tend=4;
dt=0.01;
TI=0:dt:Tend;
YI=initial(DoubleIntegrators, x0, TI);
subplot(4,1,[1 2])
for k1=[2,3,4]
    plot(TI, YI(:,k1),'b--');
    hold on
end
for k1=[1 5 6 7]
    plot(TI, YI(:,k1),'k');
end    
latexylabel('$$y_i\;$$');
rotateY;
latextitle(['$${\tt SyncDemo:}$$ Double integrator agents ($$N=$$', num2str(N1), ')']);
axis([0 2.5 -0.1 4]);
xticksempty([0 0.5 1 1.5 2 2.5]);
yticks([0 2 4 6]);
xleer;
hold off
%    the same situation for a not completely coupled network
% LRing=eye(N1,N1)-diag(ones(N1-1,1),-1);
% LRing(1,N1)=-1;
LPath=eye(N1,N1)-0.5*diag(ones(N1-1,1),-1)-0.5*diag(ones(N1-1,1),1);
LPath(1,2)=-1;
LPath(N1,N1-1)=-1;
ksn=ks/max(eig(LPath));
AbarN=kron(diag(ones(N1,1)), AI)-kron(ksn*LPath, BI*K*CI);
DoubleIntegratorsN=ss(AbarN, Bbar, Cbar, Dbar);
YI=initial(DoubleIntegratorsN, x0, TI);
subplot(4,1,[3 4])
for k1=[2,3,4]
    plot(TI, YI(:,k1),'b');
    hold on
end
for k1=[1 5 6 7]
    plot(TI, YI(:,k1),'k');
end    
latexxlabel('$$t$$');
latexylabel('$$y_i\;$$');
rotateY;
axis([0 2.5 -0.1 4]);
xticks([0 0.5 1 1.5 2 2.5]);
yticks([0 2 4 6]);
hold off


%%    Double integrators with random initial state
%
figure(11)
% random initial states  
%RandSync=rng;
%save('RandSync');
load('RandSync');       % Seed (initialisation) of the number generator
rng(RandSync);
x0=2*rand(n*N1,1)-1;
YI=initial(DoubleIntegrators, x0, TI);
subplot(4,1,[1 2])
plot(TI, YI, 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -1 1]);
%latexxlabel('$$t$$');
latexylabel('$$x_{i1}\;$$');
rotateY;
latextitle(['$${\tt SyncDemo:}$$ Double integrator agents ($$N=$$', num2str(N1), ')']);
xticksempty([0 1 2 3 4 5]);
yticks([-1 0 1]);
xleer;
hold off
%    velocity
CbarV=kron(diag(ones(N1,1)), [0 1]);   % plot the agent velocities
DoubleIntegratorsV=ss(Abar, Bbar, CbarV, Dbar);
YI=initial(DoubleIntegratorsV, x0, TI);
subplot(4,1,[3 4])
plot(TI, YI,'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -1 1]);
latexxlabel('$$t$$');
%xleer;
latexylabel('$$x_{i2}\;$$');
rotateY;
xticks([0 1 2 3 4 5]);
yticks([-1 0 1]);
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Vehicle platoon: velocity control
%
figure(12)
%      Data from PlatoonDiff.m  of 13.12.2010
%               Vehicle models differ in the mass m
n=2;            % dynamical order of a vehicle
c=200;          %  c=100 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
vbar=10;        % vbar=10 m/s   velocity of the platoon
sbar=20;        % 20 m nominal distance between vehicles
m=1000;      % m= 1000 kg  mass
N=10;           % N vehicles
%
%               PI controller
kI1=20;           % kI=5
kP1=70;          % kP=10
%               Controlled vehicle
%               input = set-point for velocity controller
%               output = velocity
Aa=[-c/m kI1/m; 0 0];
Ba=[kP1/m; 1];   %  input: command signal for velocity controller
Ca=[1 0];        %  output: velocity
Da=0;
na=2;            % dynamical order of the controlled vehicle
r=1;
%   Determination of a feedback gain k
OpenLoop=ss(Aa, Ba, Ca, 0);
rlocprintblue(OpenLoop);
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-0.55 0.05 -0.3 0.3])
axis('square');
plot([-6 6],[0 0],'k:', [0 0],[-5 5],'k:');
latextitle('$${\tt SyncDemo:}$$ Root locus of the vehicle');
Eig=rlocus(OpenLoop,4);
plot(real(Eig), imag(Eig), 'kd');
latextext(-0.3, 0.2, '$$k=4$$');
ks=9;                          % good choice for k
Eig=rlocus(OpenLoop,ks);
plot(real(Eig), imag(Eig), 'k*');
latextext(-0.4, 0.05, '$$k=9$$');
xticks([-0.5 -0.3 -0.1]);
yticks([-0.3 -0.1 0.1 0.3]);
hold off
%    Agent model
kI=kI1*ks;
kP=kP1*ks;
A=[-c/m kI/m; 0 0];
B=[kP/m; 1];   %  input: command signal for velocity controller
C=[1 0];        %  output: velocity
D=0;
na=2;            % dynamical order of the controlled vehicle
r=1;

%%    Platoon behaviour
figure(13)
N1=10;
%  Communication to the next two vehicles
L=2*diag(ones(N1,1))-diag(ones(N1-1,1),-1)-diag(ones(N1-2,1),-2);
L(1,1)=0;
L(2,2)=1;
Lnorm=[];
for k1=1:N1
    if L(k1,k1)~=0
        for k2=1:N1
        Lnorm(k1,k2)=L(k1,k2)/L(k1,k1);
        end
    end
end
Abar=kron(diag(ones(N1,1)), A)-kron(Lnorm, B*C);
Bbar=zeros(N1*n, 1);
Cbar=kron(diag(ones(N1,1)), C);    % plot the agent velocity
Dbar=zeros(N1, 1);
Platoon=ss(Abar, Bbar, Cbar, Dbar);
% initial states  
v0=ones(N1,1)*5;    % initial velocities
v0(1)=10;           % velocity of the leader in m/s
for k1=1:N1
    x0(2*(k1-1)+1)=v0(k1);
    x0(2*(k1-1)+2)=c/kI*v0(k1);
                   % determine the initial state of the PI controller
end
Tend=18;
dt=0.02;
TI=0:dt:Tend;
YI=initial(Platoon, x0, TI);  % Free motion
subplot(4,1,[1 2])
plot(TI, YI(:,2:N1), 'b');
hold on
plot(TI, YI(:,1), 'k--');  % leader velocity
%latexxlabel('$$t$$');
%latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$'); % .eps figure wrong
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
latextitle(['$${\tt SyncDemo:}$$ Vehicle platoon ($$N=$$', num2str(N1), ')']);
axis([0 Tend 3 11]);
xticksempty([0 5 10 15 20]);
yticks([0 5 10]);
xleer;
hold off
%
%    Communication only to the next vehicles
L=diag(ones(N1,1))-diag(ones(N1-1,1),-1);
L(1,1)=0;
Lnorm=L;
Abar=kron(diag(ones(N1,1)), A)-kron(Lnorm, B*C);
Platoon=ss(Abar, Bbar, Cbar, Dbar);
YI=initial(Platoon, x0, TI);  % Free motion
subplot(4,1,[3 4])
plot(TI, YI(:,2:N1), 'b');
hold on
plot(TI, YI(:,1), 'k--');  % leader velocity
plot([0 Tend],[0 0],'k:');
axis([0 Tend 0 1.1]);
latexxlabel('$$t$$ in s');
%latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$');
latexylabel('$$v_i$$ in $$\frac{\rm m}{\rm s}$$');
rotateY;
axis([0 Tend 3 11]);
xticks([0 5 10 15 20]);
yticks([0 5 10]);
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Nyquist test for the synchronisation of unstable oscillators
figure(14)
%   Oscillator model
A=[0.1 3; -3 0.1];
B=[1; 1];      
C=[1 10];
D=0;
n=2;
Oscillator=ss(A, B, C, D);
N1=5;
L=[2 -1 0 0 -1;
   -1 2 -1 0 0;
   0 -1 2 -1 0;
   0 0 -1 2 -1;
   -1 0 0 -1 2];
ks=30;
Lambda=sort(eig(L)/ks);    
[R,I]=nyquist(Lambda(2)*Oscillator);
for k1=1:length(R)
    Re(k1)=R(1,1,k1);
    Im(k1)=I(1,1,k1);
end
plot(Re, Im, 'b', Re, -Im, 'b');
hold on
[R,I]=nyquist(Lambda(4)*Oscillator);
for k1=1:length(R)
    Re(k1)=R(1,1,k1);
    Im(k1)=I(1,1,k1);
end
plot(Re, Im, 'r--', Re, -Im, 'r--');
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-13 3 -8 8])
axis('square');
plot([-13 6],[0 0],'k:', [0 0],[-8 8],'k:');
plot(-1, 0, 'k+');
latextext(-9, -2.5, '$$\downarrow$$');
latextitle('$${\tt SyncDemo:}$$ Nyquist plot of the oscillator');
xticks([-10 -5 0]);
yticks([-5 0 5]);
hold off

figure(15)     % zoom of the nyquist plot
[R,I]=nyquist(Lambda(2)*Oscillator);
for k1=1:length(R)
    Re(k1)=R(1,1,k1);
    Im(k1)=I(1,1,k1);
end
plot(Re, Im, 'b', Re, -Im, 'b');
hold on
plot(-C*inv(A)*B*Lambda(2), 0, 'bo');
hold on
[R,I]=nyquist(Lambda(4)*Oscillator);
for k1=1:length(R)
    Re(k1)=R(1,1,k1);
    Im(k1)=I(1,1,k1);
end
plot(Re, Im, 'r--', Re, -Im, 'r--');
plot(-C*inv(A)*B*Lambda(4), 0, 'ro');
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-2.1 0.9 -1.5 1.5])
axis('square');
plot([-6 6],[0 0],'k:', [0 0],[-5 5],'k:');
plot(-1, 0, 'k+');
latextitle('$${\tt SyncDemo:}$$ Nyquist plot of the oscillator');
xticks([-2 -1 0]);
yticks([-1 0 1]);
hold off

%%  Oscillator network with time delay
%
figure(16)
Tt=0.6;     % delay time
Omega=logspace(-1, 2, 2000);
[Mag, Phase, Omega]=bode(Lambda(2)*Oscillator, Omega);  % Phase in degrees
for k1=1:length(Omega)
    G(k1)=Mag(1,1,k1)*exp(j*Phase(1,1,k1)/360*2*pi);
end
plot(real(G), imag(G), 'b', real(G), -imag(G),'b');  % without time delay
hold on
PhaseTt=[];
for k1=1:length(Omega)
    PhaseTt(1,1,k1)=-(Omega(k1)*Tt)*360/2/pi;   % in degrees
end;
Phase=Phase+PhaseTt;
for k1=1:length(Omega)
    G(k1)=Mag(1,1,k1)*exp(j*Phase(1,1,k1)/360*2*pi);
end
plot(real(G), imag(G), 'r--', real(G), -imag(G),'r--');
plot(-C*inv(A)*B*Lambda(2), 0, 'bo');
latexxlabel('Re');
latexylabel('Im');
rotateY;
axis([-5 2 -3.5 3.5])
axis('square');
plot([-6 6],[0 0],'k:', [0 0],[-5 5],'k:');
latextitle('$${\tt SyncDemo:}$$ Oscillator with time delay');
plot(-1, 0, 'k+');
xticks([-4 -2 0]);
yticks([-2 0 2]);
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Demonstrate that two synchronised platoons can be united
%   by a completely coupled network
% 
figure(17)
N1=10;
%   Vehicle with velocity controller: PT_1 with time constant T1
T1=3;    % seconds
%   PI controller for distance y_i = s_i - sbar_i
%   with sbar_i denoting the reference distance
kI=0.007;     % 0.009
kP=0.2;      % 0.2
%  Agent model
A=[-1/T1    0    kI/T1;
    1       0       0;
    0       0       0];
B=[kP/T1; 0 ; 1];
C=[0 1 0];       % position output
D=0;
n=3;
Vehicle=0.1*ss(A, B, C, D);
%      unity feedback results in stable vehicle
%
%  complete interactions
L=-ones(N1,N1)+N1*eye(N1,N1);
Lnorm=L/(N1-1);
ks=1;
Abar=kron(diag(ones(N1,1)), A)-kron(ks*Lnorm, B*C);
Bbar=zeros(N1*n, 1);
Cbar=kron(eye(N1,N1), C);   % plot the agent positions
Dbar=zeros(N1, 1);
Platoon=ss(Abar, Bbar, Cbar, Dbar);
%   Initial states of the agents
for k1=1:6
    s0(k1)=200;
    v0(k1)=25;
end
for k1=7:10
    s0(k1)=0;
    v0(k1)=10;
end
Tend=20;
saverage=sum(s0)/N1;
vaverage=sum(v0)/N1;
smax=saverage+vaverage*Tend;
x0=[];
for k1=1:N1
    x0((k1-1)*n+1)=v0(k1);
    x0((k1-1)*n+2)=s0(k1);
    x0((k1-1)*n+3)=v0(k1)/kI;
end
[Y, T]=initial(Platoon, x0, Tend);
subplot(4,1,[1 2])
plot(T, Y,'b');
hold on
axis([0 Tend -200 700]);
%latexxlabel('$$t$$ in s');
latexylabel('$$y_i$$ in m');
rotateY;
yticks([0 300 600 900]);
latextitle('$${\tt SyncDemo:}$$ Platoon with complete couplings');
xleer;
hold off
%
%   Transformation to get the original vehicle position
subplot(4,1,[3 4])
sbar=30;
for k1=1:N1
    sbari(k1)=(k1-1)*sbar;
end
for k1=1:6
    Ytilde(:,k1)=Y(:,k1)+sbari(k1)*ones(size(T)); 
end
for k1=7:10
    Ytilde(:,k1)=Y(:,k1)-sbari(k1-5)*ones(size(T)); 
end
plot(T, Ytilde,'b');
axis([0 Tend -200 700]);
latexxlabel('$$t$$ in s');
xticks([0 10 20]);
latexylabel('$$s_i$$ in m');
yticks([-300 0 300 600 900]);
rotateY;
hold off

%%  Comparison with the path communication structure
%
figure(23)
%  path graph
L=diag(ones(N1,1))-diag(ones(N1-1,1),-1)
L(1,1)=0;
Lnorm=L;
ks=1;
Abar=kron(diag(ones(N1,1)), A)-kron(ks*Lnorm, B*C);
Bbar=zeros(N1*n, 1);
Cbar=kron(eye(N1,N1), C);   % plot the agent positions
Dbar=zeros(N1, 1);
Platoon=ss(Abar, Bbar, Cbar, Dbar);
[Y, T]=initial(Platoon, x0, Tend);
subplot(4,1,[1 2])
plot(T, Y,'b');
hold on
axis([0 Tend -200 700]);
%latexxlabel('$$t$$ in s');
latexylabel('$$y_i$$ in m');
rotateY;
yticks([0 300 600 900]);
latextitle('$${\tt SyncDemo:}$$ Platoon with ACC');
xleer;
hold off
%
%   Transformation to get the original vehicle position
subplot(4,1,[3 4])
sbar=30;
for k1=1:N1
    sbari(k1)=(k1-1)*sbar;
end
for k1=1:6
    Ytilde(:,k1)=Y(:,k1)+sbari(k1)*ones(size(T)); 
end
for k1=7:10
    Ytilde(:,k1)=Y(:,k1)-sbari(k1-5)*ones(size(T)); 
end
plot(T, Ytilde,'b');
axis([0 Tend -200 700]);
latexxlabel('$$t$$ in s');
xticks([0 10 20]);
latexylabel('$$s_i$$ in m');
yticks([-300 0 300 600 900]);
rotateY;
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Linear Kuramoto oscillator for demonstration of synchronisation
%    from KuramotoIMP.m
%
figure(18)
N1=10;
%  Coupling matrix
L=-ones(N1,N1)+N1*eye(N1,N1);
Lnorm=L/(N1-1);
%  Oscillator model
kr=1;
thetai0=-pi/2;
omegai=1;
xri0=0;
n=2;
x10=[thetai0; omegai+xri0];
br1=4;
ke1=4;
x10=[thetai0; omegai+xri0];
A0=[0 1; 0 0];
B0=[ke1; kr*br1];
C0=[1 0];
D0=0;
%  Controller selection to ensure the stability of 
%  the interconnected followers
Abar=kron(diag(ones(N,1)), A0)-kron(Lnorm, B0*C0);
Bbar=zeros(N*n, 1);
Cbar=kron(diag(ones(N,1)), C0);
Dbar=zeros(N, 1);
Oscillators=ss(Abar, Bbar, Cbar, Dbar);
%  initial states
%theta0=2*pi*rand(N,1);        % cf. thetas0=1;
%thetaold=theta0;
theta0=[    
    3.7378
    1.6475
    3.7878
    4.4687
    1.3933
    0.7378
    1.8641
    2.0029
    2.6651
    3.1910];
omega0=4+1*rand(N,1);  % cf. omegas=1;
%omegaold=omega0;
% omega0=[    
%     0.5086
%     0.5262
%     0.5801
%     0.5029
%     0.5929
%     0.5730
%     0.5489
%     0.5579
%     0.5237
%     0.5459];
%xr0=zeros(N,1);
%xr0=rand(N,1);
%xr0old=xr0;
xr0=[
    0.9631
    0.5468
    0.5211
    0.2316
    0.4889
    0.6241
    0.6791
    0.3955
    0.3674
    0.9880];
x0=zeros(N*n,1);
for k=1:N
    x0((k-1)*n+1)=theta0(k);
    x0((k-1)*n+2)=omega0(k)+xr0(k);
end
%  determination of the common frequency
xsbar=[0; 0];
for k=1:N
    xsbar=xsbar+x0((k-1)*n+1:(k-1)*n+2);
end
xsbar=1/N*xsbar
Tend=4;
dt=0.003;
T=0:dt:Tend;
[Y, T]=initial(Oscillators, x0, T);
subplot(2,1,1)
plot(T, sin(Y),'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -1.1 1.1]);
xticks([0 1 2 3 4 5 6]);
yticks([-1 0 1]);
latexxlabel('$$t$$');
latexylabel('$$y_i$$');
rotateY;
latextitle('$${\tt SyncDemo:}$$ Completely coupled Kuramoto oscillators');
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Cluster synchronisation
%
figure(19)
%  harmonic oscillators as in figure(3)
A=[0 3; -3 0];
B=[1; 1];      
C=[1 10];
ks=0.3;
n=2;
%  Graph with 4 strongly connected components
N1=7;
AG=[0 1 0 0 0 0 0;
    1 0 0 0 0 0 0;
    0 1 0 0 0 0 0;
    0 0 1 0 1 1 0;
    0 0 0 1 0 0 0;
    0 0 0 0 0 0 0;
    0 0 0 0 0 1 0];
D=diag(AG*ones(N1,1));
L=D-AG;
Abar=kron(diag(ones(N1,1)), A)-kron(ks*L/5, B*C);
Bbar=zeros(N1*n, 1);
Cbar=kron(diag(ones(N1,1)), C);
Dbar=zeros(N1, 1);
OscillatorNetwork=ss(Abar, Bbar, Cbar, Dbar);
% initial states
x10=[1; 1];
x20=[0; 0];
x30=[0; 0];
x40=[0; 0];
x50=[0; 0];
x60=[-1; 1.5];
x70=[0; 0];
x0=[x10; x20; x30; x40; x50; x60; x70];
Tend=15;
dt=0.01;
T=0:dt:Tend;
[Y, T]=initial(OscillatorNetwork, x0, T);
subplot(4,1,[1,2])
plot(T, Y(:,1), 'b',...
     T, Y(:,2), 'b',...   
     T, Y(:,3), 'b',...
     T, Y(:,4), 'r',... %  T, Y(:,5), 'b--',...
     T, Y(:,5), 'r');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -20 20]);
latexylabel('$$y_1, y_2, y_3, y_4, y_5$$');
rotateY;
latextitle('$${\tt SyncDemo:}$$ Five sets of strongly connected agents');
xticksempty([0 5 10 15]);
xleer;
yticks([-10 0 10]);
hold off
%
subplot(4,1,[3,4])
plot(T, Y(:,6), 'b',... % T, Y(:,4), 'b',...
     T, Y(:,7), 'b');
hold on
plot([0 Tend],[0 0],'k:');
axis([0 Tend -20 20]);
latexylabel('$$y_6, y_7$$');
rotateY;
xticks([0 5 10 15]);
latexxlabel('$$t$$');
yticks([-10 0 10]);
hold off



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Practical synchronisation of two oscillators with different eigenfrequencies
%   by using a voltage-controlled current source
%   from OscillatorNetwork.m, 11.7.2020
%   figures 15 and 16
%
figure(20)
%   Oscillators with current source 
%   Interesting parameter values: L02=0.12; delta=20; Tend2=0.03;
subplot(4,1,[1,2]);
Tend2=0.02;
dt=0.0001;
Time2=[0:dt:Tend2];
L=[1 -1;
  -1  1 ];    
N1=2;
kc1=0.005;
R=1/kc1;
Ltilde=L*kc1;  % hence, R1=1/kc = 200 Ohm
%  Oscillator 2 with different parameters
L02=0.12;   % L0 = 0.1 = 100 mH      L02=0.12 -> delta=3.817
L0=0.1;
C02=0.00001;   % 0.00001 = 10 muF
C0=C02;
delta=3.817;     %  L02=0.2,  delta=45.87 
A1=[2*delta 1/C0;
   -1/L0 0];
B1=[-1/C0; 0];
C1=[-1  0];
D1=0;
n=2;
A2=[2*delta 1/C02;
   -1/L02 0];
B2=[-1/C02; 0];
C2=[-1  0];
D2=0;
Abar=[A1 zeros(n,n); zeros(n,n) A2]-...
     [B1 zeros(n,1); zeros(n,1) B2]*Ltilde*[C1 zeros(1,n); zeros(1,n) C2];
Bbar=zeros(N1*n, 1);
Cbar=[C1 zeros(1,n); zeros(1,n) C2];
Network=ss(Abar, Bbar, Cbar, 0);
%x0=[x01; zeros((N1-1)*n, 1)];
x0=[1 0 0.9 0];   %  [1 0 0.9 0]
[Y,Time2,X]=initial(Network, x0, Time2);
plot(Time2, Y, 'b');
latexylabel('$$V_{{\rm C}i}$$ in V');
rotateY;
latextitle('$${\tt OscillatorNetwork:}$$ Network behaviour');
axis([0 Tend2 -1.1 1.4]);
yticks([-1 0 1]);
xleer
%latextext(0.015, 1.1, ['$$1/R=$$' num2str(kc1) '$$, \delta= $$' num2str(delta)]);
%
% Lyapunov function
V=[];
Vdot=[];
Vneg=[];
Vpos=[];
for k1=1:length(X)
    V(k1)=0.5*C0*X(k1,1)*X(k1,1)+0.5*L0*X(k1,2)*X(k1,2)+...
        0.5*C02*X(k1,3)*X(k1,3)+0.5*L02*X(k1,4)*X(k1,4);
    Vdot(k1)=-1/R*(X(k1,1)-X(k1,3))^2 +...
             2*delta*C0*(X(k1,1)*X(k1,1)+X(k1,3)*X(k1,3));
    Vneg(k1)=-1/R*(X(k1,1)-X(k1,3))^2;
    Vpos(k1)=2*delta*C0*(X(k1,1)*X(k1,1)+X(k1,3)*X(k1,3));
end
subplot(4,1,[3,4])
plot(Time2, V, 'b');
axis([0 Tend2 0.000008 0.00001]);
latexylabel('$$v$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0.000008 0.000009 0.00001]);
xticks([0 0.01 0.02 0.03 0.04 0.05 0.06]);
%
figure(22)
plot(Time2, Vneg, 'b--', Time2, Vpos, 'b--', Time2, Vdot, 'b');
hold on
plot([0 Tend2],[0 0],'k:');
latexylabel('$$\dot{v}\;$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend2 -0.00016 0.00016]);
yticks([-0.0001 0 0.0001]);
xticks([0 0.01 0.02 0.03 0.04 0.05 0.06]);
hold off


%%  figure(21) after figure(4)
%%  figure(23) after figure(17)

%%   Figures
%
epsfigc15('SyncDemo');