%   Script DistAtten.m
%
%   Disturbance attenuation by feedback control
%
% J. Lunze
% 29.11.2021
% homepage version of 27.2.2023
%
%   
clear
close all

%%   PI controller to attenuate constant disturbance
%
figure(1)
T=1;  % time constant
ks=1; % static reinforcement
kP=0.2; 
kI=0.8;
Tend=4;
Time=0:0.01:Tend;
Abar=[-(1+ks*kP)/T kI/T -kP/T; -ks 0 -1; 0 0 0];
Bbar=zeros(3,1);
Cbar=[ks 0 1];
Dbar=0;
SigmaP=ss(Abar, Bbar, Cbar, Dbar);
x0=[0; 0; 1];
[Y, Time, X]=initial(SigmaP, x0, Time);
%
subplot(6,1,[1,2]);
D=[0 0 1]*X';
plot(Time, D, 'b', [-1 0 0],[0 0 1],'b');
title('Attenuation of constant disturbances');
ylabel('d');

xlabel('');
axis([-1, Tend, -0.3 1.3]);
%
subplot(6,1,[3 4]);
plot(Time, Y, 'b', [-1 0 0],[0 0 1],'b');
ylabel('y');

xlabel('');
axis([-1, Tend, -1.3 1.3]);
%
subplot(6,1,[5 6]);
U=[-kP*ks kI -kP]*X';
plot(Time, U, 'b', [-1 0 0],[0 0 U(1)],'b');
ylabel('u');

xlabel('t ');
axis([-1, Tend, -1.3 1.3]);


%%  Attenuation of oscillating disturbances
%
figure(2)
omega=0.6;
T=0.1;
ks=1;
kP=1;
kI1=0.2;
kI2=0.2;
br1=1;
br2=1;
cd1=1;
cd2=10;
Tend=100;
Time=[0:0.1:Tend];
Abar=[-(1+ks*kP)/T kI1/T kI2/T -kP/T*cd1 -kP/T*cd2;
      -br1*ks        0     1   -br1*cd1  -br1*cd2;
      -br2*ks     -omega^2 0   -br2*cd1  -br2*cd2;
         0           0     0       0         1;
         0           0     0    -omega^2     0];
Bbar=zeros(5,1);
Cbar=[ks 0 0 cd1 cd2];
Dbar=0;
SigmaP=ss(Abar, Bbar, Cbar, Dbar);
x0=[0; 0; 0; 1; 1];
[Y, Time, X]=initial(SigmaP, x0, Time);
plot(Time, Y);
subplot(6,1,[1,2]);
D=[0 0 0 cd1 cd2]*X';
plot(Time, D, 'b');
title('Attenuation of oscillating disturbances');
ylabel('d');

xlabel('');
axis([0, Tend, -13 13]);
%
subplot(6,1,[3 4]);
plot(Time, Y, 'b');
ylabel('y');

xlabel('');
axis([0, Tend, -5 5]);
%
subplot(6,1,[5 6]);
U=[-kP*ks kI1 kI2 -kP*cd1 -kP*cd2]*X';
plot(Time, U, 'b');
ylabel('u');

xlabel('t ');
axis([0, Tend, -13 13]);


%%  Behaviour of the feedback loop for modified disturbance frequency
%
figure(3)
omegad=0.608;
Abar=[-(1+ks*kP)/T kI1/T kI2/T -kP/T*cd1 -kP/T*cd2;
      -br1*ks        0     1   -br1*cd1  -br1*cd2;
      -br2*ks     -omega^2 0   -br2*cd1  -br2*cd2;
         0           0     0       0         1;
         0           0     0    -omegad^2     0];
SigmaPd=ss(Abar, Bbar, Cbar, Dbar);
x0=[0; 0; 0; 1; 1];
[Y, Time, X]=initial(SigmaPd, x0, Time);
plot(Time, Y);
subplot(6,1,[1,2]);
D=[0 0 0 cd1 cd2]*X';
plot(Time, D, 'b');
title('Attenuation of oscillating disturbances');
ylabel('d');

xlabel('');
axis([0, Tend, -13 13]);
%
subplot(6,1,[3 4]);
plot(Time, Y, 'b');
ylabel('y');

xlabel('');
axis([0, Tend, -5 5]);
%
subplot(6,1,[5 6]);
U=[-kP*ks kI1 kI2 -kP*cd1 -kP*cd2]*X';
plot(Time, U, 'b');
ylabel('u');

xlabel('t ');
axis([0, Tend, -13 13]);

