%       SelfOrgCopterDemo.m
%
%      Demosntrate the self-organising disturbance attenuation
%       similar to RobotSelforg.m of 19.8.2014
%
%   Identical multicopters
%
% J. Lunze
% 23.3.2018
%
% Version of 5.4.2018
% for 2nd edition: 29.3.2021
% 
echo off
clear
close all
%
N=7;         % number of multicopters: P0,..., P6
rbar=0.2;    % maximum tolerance of output due to disturbance
             % before communication is switched off
             
%%     Multicopter model
%
%
m=1;       % mass in kg
T1=0.2;    % time constant of motor in s
kP=2;
A=[-1/T1  -kP/T1;
     1/m     0];
B=[kP/T1; 0];
E=[0; -1/m];
C=[0 1];
D=0;
Multicopter=ss(A, B, C, D);
nTS=2;
MulticopterDist=ss(A, E, C, D);


%% State observers used to supervise the controller
%    from RobotSelforg.m, figure 4
%
figure(1)
%  Observer design
AObs=A;
EObs=E;
CObs=C;
DObs=D;
Observer=ss(AObs, EObs, CObs, DObs);
%
%  Disturbance: d_3(t) = 1 m/s^2 for tD1 < t < tD2
Tend=10;
dt=0.03;
tD1=2;
tD2=5;
T=[0:dt:Tend]';
Dist=zeros(size(T));
Dist(ceil(tD1/dt):ceil(tD2/dt))=1;
D3=Dist;     %  for analysis of observer
T3=T; 
%    Behaviour of the observer
xB0=0.9*[1; 1];    % initial observation error
YDist=lsim(Observer, Dist, T, xB0);
R=abs(YDist);    % observer residual
subplot(6,1,[1 2])
plot(T, Dist, 'b', [0 Tend], [0 0], 'k:');
latexylabel('$$d_3$$ in $$\frac{\rm m}{\rm s^2}$$');
rotateY;
yticks([0 1]);
xleer;
xticksempty([0 5 10]);
axis([0 Tend -0.1 1.3]);
%
subplot(6,1,[3 4]);
plot(T, YDist, 'b');
latexylabel('$$r_3$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
xleer;
axis([0 Tend -1 1]);
yticks([-1 0 1]);
xticks([0 5 10]);
hold on
%
%  Switching bound: rbar
plot([0 Tend], [rbar, rbar], 'k--');
plot([0 Tend], [-rbar, -rbar], 'k--');
hold off


%%    Leader-follower structure with communication to the two next vehicles
%      from RobotSelforg.m, figure 5
%
%  SS 1 is the leader
%
%figure(2)
%subplot(2,1,1)
%  Coupling matrix for original communication
K=[];
K=diag(ones(N-1,1),-1)+diag(ones(N-2,1),-2);
K=K-diag(sum(K'));
Kneighb=K;   %  store for future investigations
% normalisation
Kdiag=-diag(diag(K));
Kdiag(1,1)=1;  % make this matrix invertible
K=inv(Kdiag)*K;  
K=K-diag(diag(K)); % set the diagonal elements zero
Cu1=kron(K, C);    % output matrix of the overall model to see the inputs
%  Model of the robot formation
AbarOS=kron(diag(ones(N,1)),A) + kron(K, B*C);
Cbar=kron(diag(ones(N,1)), C);
%  Disturbance input matrix if SS 3 is disturbed
Ebar=[zeros(3*nTS,1); E; zeros((N-4)*nTS,1)];  % input matrix for disturbance
Multicopters=ss(AbarOS, Ebar, Cbar, 0);




%% Disturbance behaviour of the overall system with zero initial states
%      from RobotSelforg.m, figure 6
figure(3)
%
% initial states of the synchronised vehicles
x0=zeros(N*nTS,1);
[Ynon, Tnon, Xnon]=lsim(Multicopters, Dist, T, x0);
%
%  Draw the disturbance
subplot(8,1,[1 2])
plot(T, Dist, 'b', [0 Tend], [0 0], 'k:');
latexylabel('$$d_3\;$$');   %  in $$\frac{\rm m}{\rm s^2}$$
rotateY;
yticks([0 1]);
xleer;
xticksempty([0 5 10]);
axis([0 Tend -0.1 1.3]);
latextitle('Non-switching platoon control');
%  Draw the agent outputs
subplot(8,1,[5 6 7 8]);
plot(Tnon, Ynon, 'b');
axis([0 Tend -0.6 0.2]);
latexxlabel('$$t$$ in s');
xticks([0 5 10]);
latexylabel('$$y_i\;$$');   % in m
yticks([-0.6 -0.3 0 0.3 0.6]);
rotateY;


%%  Robot behaviour if the disturbed multicopter interrupts its communication
%    from RobotSelforg.m, figure 7
%
figure(4)
%  Coupling matrix when communication from SS 3 is switched off
K=Kneighb;
K(5,4)=0;
K(5,5)=-1;
K(6,4)=0;
K(6,6)=-1;
% normalisation
Kdiag=-diag(diag(K));
Kdiag(1,1)=1;  % make this matrix invertible
K=inv(Kdiag)*K;  
K=K-diag(diag(K));
%  model of the multicopters with interrupted communication from agent 3
Abar2=kron(diag(ones(N,1)),A) + kron(K, B*C);
Cu2=kron(K, C);    % output matrix of the overall model to see the agent inputs
Multicopters2=ss(Abar2, Ebar, Cbar, 0);
%
%  Transformation into a discrete-time model
Ts=dt;   % sampling time
Multicoptersd=c2d(Multicopters, Ts);
A1=Multicoptersd.A;
B1=Multicoptersd.B;
Multicopters2d=c2d(Multicopters2, Ts);
A2=Multicopters2d.A;
B2=Multicopters2d.B;
X=[];       % state of the overall system
Y=[];       % output of the agents
U=[];       % inputs to the agents
S=[];       % operation mode   s=1: full communication;  
            %                  s=0: with some communication switched off
X(1,:)=x0';
Y(1,:)=Cbar*X(1,:)';
U(1,:)=Cu1*X(1,:)';
S(1)=1;
for k1=1:length(T)-1
  if R(k1)<rbar     % use full communication
       S(k1+1)=1;
       X(k1+1,:)=A1*X(k1,:)'+B1*Dist(k1);
       Y(k1+1,:)=Cbar*X(k1+1,:)';
       U(k1+1,:)=Cu1*X(k1+1,:)';
   else               % use reduced communication
       S(k1+1)=0;
       X(k1+1,:)=A2*X(k1,:)'+B2*Dist(k1);
       Y(k1+1,:)=Cbar*X(k1+1,:)';
       U(k1+1,:)=Cu2*X(k1+1,:)';
  end
end
%
%  Draw the disturbance
subplot(8,1,[1 2])
plot(T, Dist, 'b',[0 Tend], [0 0], 'k:');
latexylabel('$$d_3\;$$');  %  in $$\frac{\rm m}{\rm s^2}$$
rotateY;
yticks([0 1]);
xleer;
xticksempty([0 5 10]);
axis([0 Tend -0.1 1.3]);
latextitle('Switching controller');
%    Draw the agent outputs
subplot(8,1,[5 6 7 8]);
%   Mark the time intervals in which the disturbed subsystem does not send
%   information
%colormap(gray(20))
%image([0 Tend],[-1.5 1.5], 10*S+15);   % cannot be combined with plot
%hold on
%   Draw the outputs
plot(T, Y, 'b');   % the minus sign is wrong,
                            % but without this sign the curves
                            % have the wrong sign???!!!
axis([0 Tend -0.6 0.2]);
latexxlabel('$$t$$ in s');
xticks([0 5 10]);
latexylabel('$$y_i\;$$');   % in m
yticks([-0.6 -0.3 0 0.3 0.6]);
rotateY;
hold off
%
%    Draw the operation mode of the controller into figure(4)
subplot(8,1,[3 4])
S1=ones(length(T),N);      % extend function S to the overall system
S1(:,4)=S;
colormap(gray(2))
image([0 Tend],[0 6], 2*S1');
axis([0 Tend 0 6]);
%latexxlabel('$$t$$ in s');
%xticks([0 100 200 300 400]);
xleer;
xticksempty([0 5 10]);
latexylabel('$$\mathbf{s}\quad$$');
yticks([1 3 5]);
rotateY;
%
Y7=Y;    % store Y for figure 7



%%  Several disturbances affect different multicopters
%      from RobotSelforg.m, figure 9
figure(5)
Tend1=40;
T2=[0:dt:Tend1]';
%  Disturbance vector Dist(t):  Dist(k1, k2) = disturbance to agent k2 at time k1
Distm=zeros(length(T2),N);
%  Disturbance: d_2(t) = 1 m/s^2 for tD1 < t < tD2
tD1=3;
tD2=4;
Distm(ceil(tD1/dt):ceil(tD2/dt),3)=1;
%  Disturbance d_3(t) = -1.1 m/s^2 for tD3 < t < tD4
tD3=12;
tD4=17;
Distm(ceil(tD3/dt):ceil(tD4/dt),4)=-1.1;
%  Disturbance d_5(t) = 0.5 m/s^2 for tD5 < t < tD6
tD5=13;
tD6=28;
Distm(ceil(tD5/dt):ceil(tD6/dt),6)=0.5;
%
subplot(8,1,[1 2])
plot(T2, Distm, 'b', [0 Tend], [0 0], 'k:');
latexylabel('$$d\;$$');  %  in $$\frac{\rm m}{\rm s^2}$$
rotateY;
yticks([-1 0 1]);
xleer;
xticksempty([0 10 20 30 40 50]);
axis([0 Tend1 -1.3 1.1]);
latextext(tD1-2.5, 0.5, '$$d_2$$');
latextext(tD3-2.5, -0.5, '$$d_3$$');
latextext(tD5-2.5, 0.5, '$$d_5$$');
latextitle('Switching controller');
%
%   Determination of the residuals of SS 2, SS3 and SS5
Dist2=Distm(:,3);
YDist2=lsim(Observer, Dist2, T2);
R2=abs(YDist2);    % observer residual of SS 2
Dist3=Distm(:,4);
YDist3=lsim(Observer, Dist3, T2);
R3=abs(YDist3);    % observer residual of SS 3
Dist5=Distm(:,6);
YDist5=lsim(Observer, Dist5, T2);
R5=abs(YDist5);    % observer residual of SS 5
%  Extension of the model for disturbances to all agents
Ebar=kron(eye(N,N), E);
%    Reset of signals
X=[];       % state of the overall platoon
Y=[];       % output of the vehicles
U=[];       % inputs to the vehicles
S=[];       % operation mode   s=1: full communication;  
            %                  s=0: with some communication switched off
X(1,:)=x0';
Y(1,:)=Cbar*X(1,:)';
U(1,:)=Cu1*X(1,:)';
S=ones(length(T2),N);
for k1=1:length(T2)-1
    %  Determine the coupling structure
   K=Kneighb;
   if R2(k1)>rbar     
      K(4,3)=0;
      K(4,4)=-1;
      K(5,3)=0;
      K(5,5)=-1;
      S(k1+1,3)=0;
   end
   if R3(k1)>rbar     
      K(5,4)=0;
      K(5,5)=-1;
      K(6,4)=0;
      K(6,6)=-1;
      S(k1+1,4)=0;
   end 
   if R5(k1)>rbar     
      K(7,6)=0;
      K(7,7)=-1;
      S(k1+1,6)=0;
   end
   Kdiag=-diag(diag(K));    % normalisation
   Kdiag(1,1)=1;            % make this matrix invertible
   K=inv(Kdiag)*K;
   K=K-diag(diag(K));
     % Determine the model
   Abar=kron(diag(ones(N,1)),A) + kron(K, B*C);  %  model of the platoon
   OverallSyst=ss(Abar, Ebar, Cbar, 0);
   OverallSystd=c2d(OverallSyst, Ts);
   A0=OverallSystd.A;
   B0=OverallSystd.B;
      %  Determine the next signal values
   X(k1+1,:)=A0*X(k1,:)'+B0*Distm(k1,:)';
   Y(k1+1,:)=Cbar*X(k1+1,:)';
   U(k1+1,:)=Cu1*X(k1+1,:)';
end
%    Draw the operation mode of the controller 
subplot(8,1,[3 4])
colormap(gray(2))
image([0 Tend1],[0 6], 20*S');
axis([0 Tend1 0 6]);
%latexxlabel('$$t$$ in s');
xleer;
xticksempty([0 10 20 30 40 50]);
latexylabel('$$s \quad$$');
yticks([2  5 ]);
rotateY;
%
%  Draw the sync result
subplot(8,1,[5 6 7 8]);
plot(T2, Y, 'b');
hold on;
axis([0 Tend1 -0.55 0.6]);
latexxlabel('$$t$$ in s');
xticks([0 10 20 30 40 50]);
latexylabel('$$y_i\;$$');   % in m
yticks([-0.6 -0.3 0 0.3 0.6]);
rotateY;
hold off



%%  The same disturbance to the overall system with fixed controller
%      from RobotSelforg.m, figure 10
%
figure(6)
%  Draw the disturbance
subplot(8,1,[1 2]);
plot(T2, Distm, 'b', [0 Tend], [0 0], 'k:');
latexylabel('$$d\;$$');   %  in $$\frac{\rm m}{\rm s^2}$$
rotateY;
yticks([-1 0 1]);
xticks([0 100 200 300 400]);
xleer;
xticksempty([0 10 20 30 40 50]);
axis([0 Tend1 -1.3 1.1]);
latextext(tD1-2.5, 0.5, '$$d_2$$');
latextext(tD3-2.5, -0.5, '$$d_3$$');
latextext(tD5-2.5, 0.5, '$$d_5$$');
latextitle('Non-switching platoon control');
%
% subplot(4,1,2)
% colormap(gray(2))
% axis([0 Tend1 0 6]);
% latexxlabel('$$t$$ in s');
% xticks([0 100 200 300 400]);
% xleer;
% latexylabel('$$s$$');
% yticks([2 3 5 ]);
% rotateY;
% 
SystemFull=ss(AbarOS, Ebar, Cbar, 0);
[YFull, TFull, XFull]=lsim(SystemFull, Distm, T2, x0);
%
%   Transformation to get the control error (distance from the line)
subplot(8,1,[5 6 7 8]);
plot(TFull, YFull, 'b');
hold on;
axis([0 Tend1 -0.55 0.6]);
latexxlabel('$$t$$ in s');
xticks([0 10 20 30 40 50]);
latexylabel('$$y_i\;$$');  % in m
yticks([-0.6 -0.3 0 0.3 0.6]);
rotateY;
hold off


%%  Determination of Myy = int_0^\infty |g_yy| dt
%     from RobotSelforg.m, figure 13
%
figure(7)
[YImp, TImp]=impulse(Multicopter);
deltaT=TImp(2)-TImp(1);
Myy=sum(abs(YImp))*deltaT;
%   Maximum effect of the disturbance on the direct neighbour: 1/2*Myy
ybar=Myy/2*rbar;
%   Use the result of disturbance behaviour with d_3\not=0:
%   Draw the disturbance
subplot(6,1,[1 2])
plot(T, Dist, 'b', [0 Tend], [0 0], 'k:');
latexylabel('$$d_3$$ in $$\frac{\rm m}{\rm s^2}$$');
rotateY;
yticks([0 1]);
xleer;
xticksempty([0 5 10 20 30 40 50]);
axis([0 Tend -0.1 1.3]);
latextitle('Switching controller');
%
%    Draw the output of Agent 4 (neighbouring the disturbed agent)
subplot(6,1,[3 4 5 6]);
plot(T, Y7(:,5), 'b');   % Agent 4 with self-org. controller
hold on
plot(T, Ynon(:,5), 'b--');  % For comparison: behaviour of Agent 4
                                    % for fixed control structure
plot([0 Tend],[ybar, ybar],'k--', [0 Tend],[-ybar, -ybar],'k--');
%axis([0 Tend -5 5]);
latexxlabel('$$t$$ in s');
xticks([0 5 10 20 30 40 50]);
latexylabel('$$y_4$$ in m');   % in m
yticks([-0.6 -0.3 0 0.3 0.6]);
axis([0 Tend -0.4 0.2]);
rotateY;
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Several disturbances distroy the multicopter formation
%   but with changed local controllers the connectivity is retained
%     
figure(8)
Tend1=40;
T2=[0:dt:Tend1]';
%  Disturbance vector Dist(t):  Dist(k1, k2) = disturbance to agent k2 at time k1
Distm=zeros(length(T2),N);
%  Disturbance: d_1(t) = 1 m/s^2 for tD1 < t < tD3
tD1=3;
tD3=11;
Distm(ceil(tD1/dt):ceil(tD3/dt),2)=1;
%  Disturbance d_2(t) = 0.7 m/s^2 for tD2 < t < tD5
tD2=7;
tD5=23;
Distm(ceil(tD2/dt):ceil(tD5/dt),3)=0.7;
%  Disturbance d_3(t) = 1.5 m/s^2 for tD4 < t < tD6
tD4=18;
tD6=35;
Distm(ceil(tD4/dt):ceil(tD6/dt),4)=1.5;
%
subplot(8,1,[1 2])
plot(T2, Distm, 'b', [0 Tend1], [0 0], 'k:');
latexylabel('$$d\;$$');  %  in $$\frac{\rm m}{\rm s^2}$$
rotateY;
yticks([0 0.7 1.5]);
xleer;
xticksempty([0 10 20 30 40 50]);
axis([0 Tend1 -0.1 1.7]);
latextext(tD1-2, 0.5, '$$d_1$$');
latextext(tD2-2.5, 0.5, '$$d_2$$');
latextext(tD4-2.5, 1.1, '$$d_3$$');
latextitle('Switching controller');
%
%   Determination of the residuals  
Dist2=Distm(:,2);
YDist2=lsim(Observer, Dist2, T2);
R2=abs(YDist2);    % observer residual r1
Dist3=Distm(:,3);
YDist3=lsim(Observer, Dist3, T2);
R3=abs(YDist3);    % observer residual r2
Dist4=Distm(:,4);
YDist4=lsim(Observer, Dist4, T2);
R4=abs(YDist4);    % observer residual r3
%  Extension of the model for disturbances to all agents
Ebar=kron(eye(N,N), E);
%    Reset of signals
X=[];       % state of the overall system
Y=[];       % height of the multicopters
S=[];       % operation mode   s=1: full communication;  
            %                  s=0: with some communication switched off
X(1,:)=x0';
Y(1,:)=Cbar*X(1,:)';
S=ones(length(T2),N);
for k1=1:length(T2)-1
    %  Determine the coupling structure
   K=Kneighb;
   if R2(k1)>rbar     
      K(3,2)=0;
      K(3,3)=K(3,3)+1;
      K(4,2)=0;
      K(4,4)=K(4,4)+1;
      S(k1+1,2)=0;
   end
   if R3(k1)>rbar     
      K(4,3)=0;
      K(4,4)=K(4,4)+1;
      K(5,3)=0;
      K(5,5)=K(5,5)+1;
      S(k1+1,3)=0;
   end 
   if R4(k1)>rbar     
      K(5,4)=0;
      K(5,5)=K(5,5)+1;
      K(6,4)=0;
      K(6,6)=K(6,6)+1;
      S(k1+1,4)=0;
   end
   % normalisation
   for k3=2:N
       if K(k3,k3) ~= 0
          for k4=1:k3-1
              K(k3,k4)=K(k3,k4)/abs(K(k3,k3));
          end
       end
   end
     % Determine the model
   Abar=kron(diag(ones(N,1)),A) + kron(K, B*C);  %  model of the overall system
   OverallSyst=ss(Abar, Ebar, Cbar, 0);
   OverallSystd=c2d(OverallSyst, Ts);    % discrete-time version
   A0=OverallSystd.A;
   B0=OverallSystd.B;
      %  Determine the next signal values
   X(k1+1,:)=A0*X(k1,:)'+B0*Distm(k1,:)';
   Y(k1+1,:)=Cbar*X(k1+1,:)';
end
%    Draw the operation mode of the controller 
subplot(8,1,[3 4])
colormap(gray(2))
image([0 Tend1],[0 6], 20*S');
axis([0 Tend1 0 6]);
%latexxlabel('$$t$$ in s');
xticks([0 100 200 300 400]);
xleer;
xticksempty([0 10 20 30 40 50]);
latexylabel('$$s \quad$$');
yticks([1 3]);
rotateY;
%
%  Draw the sync result
subplot(8,1,[5 6 7 8]);
plot(T2, Y, 'b');
hold on;
axis([0 Tend1 -0.6 0.1]);
latexxlabel('$$t$$ in s');
xticks([0 10 20 30 40 50]);
latexylabel('$$y_i\;$$');   % in m
yticks([-0.9 -0.6 -0.3 0 0.3 0.6]);
rotateY;
hold off


%%  The same scenario with PI-controlled multicopters
%   Design the PI controller
%%     Multicopter model
%
figure(9)
subplot(4,1,[1 2])
TendD=10;
[YDist,Time]=step(MulticopterDist, TendD);
plot(Time,YDist, 'b');
hold on
%
m=1;       % mass in kg
T1=0.2;    % time constant of motor in s
kP=3;
kI=0.3*kP;
API=[-1/T1  -kP/T1  -kI/T1;
     1/m     0      0;
     0       1      0];
BPI=[kP/T1; 0; -1];       % input: ysi
EPI=[0; -1/m; 0];         % disturbance input
CPI=[0 1 0];
DPI=0;
nTS=3;
MulticopterPI=ss(API, BPI, CPI, DPI);
%  Disturbance behaviour
MulticopterPIDist=ss(API, EPI, CPI, DPI);
[YDist,Time]=step(MulticopterPIDist, TendD);
plot(Time,YDist, 'b');
axis([0 TendD -0.6 0.05]);
latexxlabel('$$t$$ in s');
xticks([0 5 10]);
latexylabel('$$y_{{\rm d}i}$$ in m');   % in m
yticks([-0.9 -0.6 -0.3 0 0.3 0.6]);
rotateY;
hold off


%% State observers used to supervise the controller
%
figure(10)
rbar=0.2;
%  Observer design
AObsPI=API;
EObsPI=EPI;
CObsPI=CPI;
DObsPI=DPI;
ObserverPI=ss(AObsPI, EObsPI, CObsPI, DObsPI);
%
%  Disturbance: d_3(t) = 1 m/s^2 for tD1 < t < tD2
Tend=10;
dt=0.03;
tD1=2;
tD2=5;
T=[0:dt:Tend]';
Dist=zeros(size(T));
Dist(ceil(tD1/dt):ceil(tD2/dt))=1;
D3=Dist;     %  for analysis of observer
T3=T; 
%    Behaviour of the observer
xB0=0*[1; 1; 0];    % initial observation error
YDist=lsim(ObserverPI, Dist, T, xB0);
R=abs(YDist);    % observer residual
%
subplot(6,1,[1 2])
plot(T, Dist, 'b', [0 Tend], [0 0], 'k:');
latexylabel('$$d_3$$ in $$\frac{\rm m}{\rm s^2}$$');
rotateY;
yticks([0 1]);
xleer;
xticksempty([0 5 10]);
axis([0 Tend -0.1 1.3]);
%
subplot(6,1,[3 4]);
plot(T, YDist, 'b');
latexylabel('$$r_3$$ in m');
rotateY;
latexxlabel('$$t$$ in s');
xleer;
axis([0 Tend -1 1]);
yticks([-1 0 1]);
xticks([0 5 10]);
hold on
%
%  Switching bound: rbar
plot([0 Tend], [rbar, rbar], 'k--');
plot([0 Tend], [-rbar, -rbar], 'k--');
hold off


%%  Several disturbances do not destroy the PI-multicopter formation
%     
figure(11)
Tend1=40;
T2=[0:dt:Tend1]';
%  Disturbance vector Dist(t):  Dist(k1, k2) = disturbance to agent k2 at time k1
Distm=zeros(length(T2),N);
%  Disturbance: d_1(t) = 1 m/s^2 for tD1 < t < tD3
tD1=3;
tD3=11;
Distm(ceil(tD1/dt):ceil(tD3/dt),2)=1;
%  Disturbance d_2(t) = 1 m/s^2 for tD2 < t < tD5
tD2=7;
tD5=23;
Distm(ceil(tD2/dt):ceil(tD5/dt),3)=0.7;
%  Disturbance d_3(t) = 0.5 m/s^2 for tD4 < t < tD6
tD4=18;
tD6=35;
Distm(ceil(tD4/dt):ceil(tD6/dt),4)=1.5;
%
subplot(8,1,[1 2])
plot(T2, Distm, 'b', [0 Tend1], [0 0], 'k:');
latexylabel('$$d\;$$');  %  in $$\frac{\rm m}{\rm s^2}$$
rotateY;
yticks([0 0.7  1.5]);
xleer;
xticksempty([0 10 20 30 40 50]);
axis([0 Tend1 -0.1 1.7]);
latextext(tD1-2, 0.5, '$$d_1$$');
latextext(tD2-2.5, 0.5, '$$d_2$$');
latextext(tD4-2.5, 1.1, '$$d_3$$');
latextitle('Switching controller');
%
%   Determination of the residuals of SS 1, SS2 and SS3
Dist2=Distm(:,2);
YDist2=lsim(ObserverPI, Dist2, T2);
R2=abs(YDist2);    % observer residual of SS 1
Dist3=Distm(:,3);
YDist3=lsim(ObserverPI, Dist3, T2);
R3=abs(YDist3);    % observer residual of SS 2
Dist4=Distm(:,4);
YDist4=lsim(ObserverPI, Dist4, T2);
R4=abs(YDist4);    % observer residual of SS 3
%  Extension of the model for disturbances to all agents
Cbar=kron(diag(ones(N,1)), CPI);
Ebar=kron(eye(N,N), EPI);
%    Reset of signals
X=[];       % state of the overall system
Y=[];       % output of the multicopters
S=[];       % operation mode   s=1: full communication;  
            %                  s=0: with some communication switched off
X(1,:)=zeros(N*nTS,1)';
Y(1,:)=Cbar*X(1,:)';
S=ones(length(T2),N);
for k1=1:length(T2)-1
    %  Determine the coupling structure
   K=Kneighb;
   if R2(k1)>rbar     
      K(3,2)=0;
      K(3,3)=K(3,3)+1;
      K(4,2)=0;
      K(4,4)=K(4,4)+1;
      S(k1+1,2)=0;
   end
   if R3(k1)>rbar     
      K(4,3)=0;
      K(4,4)=K(4,4)+1;
      K(5,3)=0;
      K(5,5)=K(5,5)+1;
      S(k1+1,3)=0;
   end 
   if R4(k1)>rbar     
      K(5,4)=0;
      K(5,5)=K(5,5)+1;
      K(6,4)=0;
      K(6,6)=K(6,6)+1;
      S(k1+1,4)=0;
   end
   % normalisation
   for k3=2:N
       if K(k3,k3) ~= 0
          for k4=1:k3-1
              K(k3,k4)=K(k3,k4)/abs(K(k3,k3));
          end
       end
   end
     % Determine the model
   Abar=kron(diag(ones(N,1)),API) + kron(K, BPI*CPI);  %  model of the overall system
   OverallSyst=ss(Abar, Ebar, Cbar, 0);
   OverallSystd=c2d(OverallSyst, Ts);    % discrete-time version
   A0=OverallSystd.A;
   B0=OverallSystd.B;
      %  Determine the next signal values
   X(k1+1,:)=A0*X(k1,:)'+B0*Distm(k1,:)';
   Y(k1+1,:)=Cbar*X(k1+1,:)';
end
%    Draw the operation mode of the controller 
subplot(8,1,[3 4])
colormap(gray(2))
image([0 Tend1],[0 6], 20*S');
axis([0 Tend1 0 6]);
%latexxlabel('$$t$$ in s');
xticks([0 100 200 300 400]);
xleer;
xticksempty([0 10 20 30 40 50]);
latexylabel('$$s \quad$$');
yticks([1 3]);
rotateY;
%
%  Draw the behaviour
subplot(8,1,[5 6 7 8]);
plot(T2, Y, 'b');
hold on
axis([0 Tend1 -0.3 0.3]);
latexxlabel('$$t$$ in s');
xticks([0 10 20 30 40 50]);
latexylabel('$$y_i\;$$');   % in m
yticks([-0.9 -0.6 -0.3 0 0.3 0.6]);
rotateY;
hold off


%%  Draw the disturbance in a separate diagram
%
figure(12)
subplot(4,1,[1 2])
plot(T2, Distm, 'b', [0 Tend1], [0 0], 'k:');
latexylabel('$$d$$ in $$\frac{\rm m}{\rm s^2}$$');
rotateY;
latexxlabel('$$t$$ in s');
axis([0 Tend1 -0.1 1.7]);
latextext(tD1-2, 0.5, '$$d_1$$');
latextext(tD2-2.5, 0.5, '$$d_2$$');
latextext(tD4-2.5, 1.1, '$$d_3$$');
xticks([tD1 tD2 tD3 tD4 tD5 tD6]);
yticks([0 0.7 1 1.5]);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%%  The same scenario with PID-controlled multicopters
%   Design the PID controller
%%     Multicopter model
%
figure(13)
subplot(4,1,[1 2])
TendD=8;
[YDist,Time]=step(MulticopterPIDist, TendD);
plot(Time,YDist, 'b');
hold on
%
m=1;       % mass in kg
T1=0.2;    % time constant of motor in s
kP=8;
kI=0.3*kP;
kD=4;
APID=[-1/T1-kD/T1/m  -kP/T1  -kI/T1;
     1/m     0      0;
     0       1      0];
BPID=[kP/T1; 0; -1];       % input: ysi
EPID=[kD/T1/m; -1/m; 0];         % disturbance input
CPID=[0 1 0];
DPID=0;
nTS=3;
MulticopterPID=ss(APID, BPID, CPID, DPID);
%  Disturbance behaviour
MulticopterPIDDist=ss(APID, EPID, CPID, DPID);
[YDist,Time]=step(MulticopterPIDDist, TendD);
plot(Time,YDist, 'b');
axis([0 TendD -0.6 0.05]);
%latexxlabel('$$t$$ in s');
%xticks([0 5 10]);
xleer;
xticksempty([0 5 10]);
latexylabel('$$y_{{\rm d}i}$$ in m');   % in m
yticks([-0.9 -0.6 -0.3 0 0.3 0.6]);
rotateY;
hold off
%
%   Command behaviour
subplot(4,1,[3 4])
[YComm, Time]=step(MulticopterPI, TendD);
plot(Time,YComm, 'b');
hold on
[YComm,Time]=step(MulticopterPID, TendD);
plot(Time,YComm, 'b');
axis([0 TendD -0.05 1.2]);
latexxlabel('$$t$$ in s');
xticks([0 5 10]);
latexylabel('$$y_i$$ in m');   % in m
yticks([0 0.5 1]);
rotateY;
hold off





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%
epsfigc('SelfOrgCopterDemo');

