%      DelayDemo.M
%
%   Properties of the delay measured
%   introduced for communication structure design
%    from DelayMeasure.m of 24.7.2013
%
% J. Lunze
% 8.3.2018
%
% Version of 14.1.2019
% for 2nd edition: 23.3.2021
% 
close all
clear all


%%  PT_n systems with identical time constants T1
%   At what time t does the system reach the delay
%   D = n*T1
figure(1)
subplot(2,1,1)
T1=1;
N=10;   % highest dynamical order
dt=0.01;  % simulation time step
Time=0:dt:2*N*T1;
PT1Syst=tf(1,[T1 1]);
System=1;
yD=[];         % y(n*T1) - output value at time t=D
nD=[];         % n - dynamical order (number of time constants)
for k1=1:N
    System=System*PT1Syst;     % series connection expands the system by one PT1-system
    [Y, Time]=step(System, Time);
    kT=k1*T1/dt;               % Number of time point at which t=n*T1
    nD=[nD; k1];               % dynamical order of system
    yD=[yD; Y(kT)];            % output at time t=n*T1
    plot(Time/k1, Y, 'b');
    hold on
end
plot([1 1],[-1 2],'k--', [0 5], [1 1], 'k:');
latextitle('$${\tt DelayDemo:}$$ PT$$_n$$ system with $$T_1=1$$');
latextext(0.9, -0.2, '$$\Delta$$');
axis([0 5 0 1.1]);
latexylabel('$$y\;$$');
rotateY;
latexxlabel('$$\frac{t}{n}$$');
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(2)
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('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

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Robot formation problem
%   
%  new robot parameters
%  Vehicle model with velocity and position control loops
m=10;           % m= 10 kg  mass
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
Ac=[-c/m-kP/m -kI/m; 1 0];
Bc=[kP/m; -1];
Cc=[1 0];
Dc=0;
na=2;            % dynamical order of vehicle with velocity controller
%
%
%  Vehicle with output=position
A=[0 Cc; zeros(na,1) Ac];
B=[0; Bc];
C=[1 zeros(1,na)];
D=0;
Robot=ss(A, B, C, D);


%% Design of the feedback gain k
%
figure(3)
rlocprintblue(Robot);
hold on
axis([-1 0.2 -0.6 0.6]);
axis('square');
latextitle('$${\tt DelayDemo:}$$ Root locus of controlled vehicle');
% kP=rlocfind(vehicle);
kP=0.3;          % feedback gain 0.03
Poles=rlocus(Robot, kP);
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(4)
%  Step response of the controlled vehicle
Abar=A-kP*B*C;
Bbar=kP*B;
Cbar=C;
Dbar=D;
n=na+1;
contrVehicle=ss(Abar, Bbar, Cbar, Dbar);
Tsim=25;      % simulation time long enough to determine the delay with 
              % sufficient accuracy
dt=0.005;
Time=0:dt:Tsim;
Y=step(contrVehicle,Time);
subplot(4,1,[1 2]);
plot(Time, Y,'b');
latexylabel('$$s_i$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
latextitle('$${\tt DelayDemo:}$$ Step response of the controlled vehicle');
axis([0 15 0 1.1]);
hold on
ks=dcgain(contrVehicle);
plot([0 Tsim],[ks ks],'k:');
% 
%  Determination of the delay
DVeh=trapz(1-Y)*dt;
latextext(0.5, 0.8, ['$$\Delta= $$', num2str(DVeh,'%.3g')]);
yticks([0 0.5 1]);
xticks([0 5 10 15 20]);
hold off;
 
%%  Vehicles with neighbouring coupling
%
%   yref=1;
%   all vehicles initially at s_i(0)=0;  v_i(0)=0;
figure(5);
subplot(4,1,[1,2])
N=6;
%  Coupling matrix
%    N = number of vehicles including the reference vehicle
K=diag(ones(N-1,1),-1);
K=K-diag(sum(K'));
Ktilde=K(2:N, 2:N);   % N-1 followers
Kneighb=K;     % store the neighbouring couplings
%  Model of all followers in the vehicle row
AF=kron(Ktilde, kP*B*C)+kron(diag(ones(N-1,1)), A);
BF=kron(K(2:N,1), kP*B);                  % not used in step(VehicleRow...)
CF=kron(diag(ones(N-1,1)), C);
DF=zeros(N-1,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
for k=1:N-1
    plot(k*DVeh, 0.63, 'kx'); 
end
yticks([0 0.5 1]);
%xticks([0 100 200 300 400]);
hold off
 
%%  Vehicles with communication structure (c)
%
%  adjacency matrix:
figure(5)
subplot(4,1,[3,4])
N=6;
K=[0 0 0 0 0 0;
   1 0 0 0 0 0;
   0 1 0 0 0 0;
   0 0 1 0 0 0;
   0 1 0 0 0 0;
   0 0 0 0 1 0];
%  Laplace matrix
K=K-diag(sum(K'));
Ktilde=K(2:N, 2:N);   % N-1 followers
Kneighb=K;     % store the neighbouring couplings
%  Model of all followers in the vehicle row
AF=kron(Ktilde, kP*B*C)+kron(diag(ones(N-1,1)), A);
BF=kron(K(2:N,1), kP*B);                  % not used in step(VehicleRow...)
CF=kron(diag(ones(N-1,1)), C);
DF=zeros(N-1,1);
VehicleRow=ss(AF, BF, CF, DF);
[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;
%latextitle('$${\tt DelayDemo:}$$ Unidirectional neighbouring couplings');
%   Marking of the delay:  D_{i, ref} = i*DVeh
for k=1:3
    plot(k*DVeh, 0.63, 'kx'); 
end
yticks([0 0.5 1]);
xticks([0 10 20 30 40]);
hold off
 

%%  Vehicles with communication structure (c) with dashed lines
%
%  adjacency matrix:
figure(6)
subplot(4,1,[1,2])
N=6;
K=[0 0 0 0 0 0; 
   1 0 0 0 0 0;
   0 1 0 0 0 0;
   0 0 1 0 0 0;
   0 1 0 1 0 0;
   0 0 1 0 1 0];
%  Laplace matrix
K=K-diag(sum(K'));
K(1,1)=1;     % this element is necessary to avoid singular matrix
K=inv(diag(-diag(K)))*K;
Ktilde=K(2:N, 2:N);   % N-1 followers
%  Model of all followers in the vehicle row
AF=kron(Ktilde, kP*B*C)+kron(diag(ones(N-1,1)), A);
BF=kron(K(2:N,1), kP*B);                  % not used in step(VehicleRow...)
CF=kron(diag(ones(N-1,1)), C);
DF=zeros(N-1,1);
VehicleRow=ss(AF, BF, CF, DF);
[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
for k=[1 2 3 3.5]
    plot(k*DVeh, 0.63, 'kx'); 
end
yticks([0 0.5 1]);
xticksempty([0 10 20 30 40]);
hold off
 

%%  Vehicles with communication structure (d)
%
%  adjacency matrix:
figure(6)
subplot(4,1,[3,4])
N=6;
K=[0 0 0 0 0 0; 
   1 0 0 0 0 0;
   0 1 0 0 0 0;
   1 0 0 0 0 0;
   0 0 1 1 0 0;
   0 0 0 0 1 0];
%  Laplace matrix
K=K-diag(sum(K'));
K(1,1)=1;     % this element is necessary to avoid singular matrix
K=inv(diag(-diag(K)))*K;
Ktilde=K(2:N, 2:N);   % N-1 followers
%  Model of all followers in the vehicle row
AF=kron(Ktilde, kP*B*C)+kron(diag(ones(N-1,1)), A);
BF=kron(K(2:N,1), kP*B);                  % not used in step(VehicleRow...)
CF=kron(diag(ones(N-1,1)), C);
DF=zeros(N-1,1);
VehicleRow=ss(AF, BF, CF, DF);
[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;
%latextitle('$${\tt DelayDemo:}$$ Unidirectional neighbouring couplings');
%   Marking of the delay:  D_{i, ref} = i*DVeh
for k=[1 2 2.5 3.5]
    plot(k*DVeh, 0.63, 'kx'); 
end
yticks([0 0.5 1]);
xticks([0 10 20 30 40]);
hold off



%%  More information makes the performance worse
%
%     similar to RandomGraphBosch.m   figure(10)
figure(7)
%   1. yref(t): sprungförmige Führungsgröße
%   2. Verhalten des Agenten 5 für Informationsstruktur a)
%   3. Verhalten des Agenten 5 für Informationsstruktur b)
%
%   a) 0 -> 1, 2;    1 -> 3;   2 -> 4;   3 -> 5
%   b) 0 -> 1, 2;    1 -> 2, 3;  2 -> 3, 4;  3 -> 4, 5;   4 -> 5
%  wobei Nummerierung der Agenten 0...5 zu den Indizes 1...6 gehört
Ka=[0  0 0 0 0 0; 
    1 -1 0 0 0 0;
    1  0 -1 0 0 0;
    0  1 0 -1 0 0;
    0  0 1 0 -1 0;
    0  0 0 1 0 -1];
%
Kb=[0  0 0 0 0 0; 
    1 -1 0 0 0 0;
    1  1 -2 0 0 0;
    0  1 1 -2 0 0;
    0  0 1 1 -2 0;
    0  0 0 1 1 -2];
Kb=inv(diag([1 1 2 2 2 2]))*Kb;
%
AF=kron(Ka, kP*B*C)+kron(diag(ones(N,1)), A);
BF=kron(Ka(1:N,1), kP*B);                 
CF=[zeros(1, (N-1)*n) C];           % output of the last agent
DF=zeros(1,1);
VehicleRow=ss(AF, BF, CF, DF);
Tend=30;
[Y, Time]=step(VehicleRow, Tend);
subplot(2,1,1)
plot(Time, 10*Y, 'b');
hold on
axis([-3 Tend -1 11]);
plot([-3 0 0 Tend],[0 0 10 10],'b');
latextext(2, 8.5, '$$y_{\rm ref}$$');
latexxlabel('$$t$$');   %  in s
latexylabel('$$y_5\;$$');   % in m
rotateY;
latextitle('$${\tt DelayDemo:}$$ Unidirectional neighbouring couplings');
yticks([0 5 10]);
xticks([0 10 20 30]);
%  Coupling structure b)
AF=kron(Kb, kP*B*C)+kron(diag(ones(N,1)), A);
BF=kron(Kb(1:N,1), kP*B);                 
VehicleRow=ss(AF, BF, CF, DF);
Tend=380;
[Y, Time]=step(VehicleRow, Tend);
plot(Time, 10*Y, 'b--');
hold off
%





%%
%
epsfigc15('DelayDemo');
