%% Limitations in performance
% Based on examples from Skogestad's book.

%% Definition of bandwidth
% Should we use T or S for the definition of bandwidth? If we use the former, the 
% bandwidth is defined as the frequency from which the gain starts rolling off. 
% If we use the latter, we define the bandwidth as the frequency up to which the gain is small. 
% Mostly it does not really matter which one we choose. But the following example shows 
% that there could be a significant difference. 

figure(1)
L = tf([-1, 0.1],[1, 2.1, 0]);
S = 1/(1+L); T = L/(1+L);
bode(S,T), grid on, legend("S,","T")
% print -depsc2 ../../exams/2019/bandwidth.eps
% !epstopdf ../../exams/2019/bandwidth.eps

%%
% In particular, see the rise time in the step responses below. It turns
% out that even in the step response, the rise time corresponds to the
% (inverse of) the bandwidth defined with respect to S.

figure(2)
step(T,50), grid on

%% Water-bed effect for a minimum phase system with the relative degree >= 2
% Provided the open-loop transfer function as at least two more poles than
% zeros, the total area between the magnitude frequency response and the
% horizontal (0dB) axis, taking the sign into consideration, should equal zero.

L = tf([1],[1 1 0]);
S = 1/(1+L);

figure(3)
bodemag(S)
grid on

%% Waterbed effect for a system with a RHP zero
% Provided that the open-loop transfer function contains a zero in the
% right half plane.

G = tf([-1 2], [1, 2]);
k = [0.1, 0.25, 0.5, 1, 2];
for ii=1:length(k)
    K = k(ii)*tf(1,[1 0]);
    L(:,:,ii) = K*G;
end
S = 1/(1+L);

figure(4)
bodemag(S), grid on

%% Maximum modulus theorem
% Just a visualization of the fundamental fact from complex analysis:
% a complex function of a complex variable that is holomorphic (or,
% equivalently, analytic) in the right half plance, assumes its maximum (in magnitude) 
% somewhere on the imaginary axis.

x = linspace(-1,1,50);
X = repmat(x,50,1);
Y = repmat(x',1,50);

S = X+j*Y;
G = abs((S+1)./(S+1/3));

figure(5)
mesh(x,x,G)
xlabel('Real'), ylabel('Imaginary')

%% Functional controllability
% Transfer functions for MIMO systems when evaluated at a given frequencies
% are just constant matrices. As such, they relate the (Fourier transforms of) inputs and 
% (Fourier transforms) ofthe output in the linear fashion: y(jw) = G(jw) u(jw). 
% Therefore, it makes sense to analyze the rank of such matrix:  

G = tf({1,2;2,4},{[1 1],[1 1];[1 2],[1 2]})

%%
% Evaluate the matrix transfer function at steady state:

G0 = evalfr(G,0)

%%
% Evaluate the matrix transfer function at "some large" frequency:

Ginf = evalfr(G,1e5)

%%
% We can now analyze the two matrices. SVD is the right tool.

[U0,S0,V0] = svd(G0)
[Uinf,Sinf,Vinf] = svd(Ginf)

%% Limitations due to disturbance
% Provided the model of the system is normalized, that is, the disturbance
% is assumed to be bounded by 1 in the magnitude, and the regulation error
% is only tolerated if below 1 in the magnitude, the bandwidth is given by
% the frequency range over which Gd is significant (above 1). 

Gd = tf(10,[100 1]);

figure(6)
bodemag(inv(Gd))
grid on

%% Limitations due to saturation
% Again, we assume that the provided model has already been scaled. Then we
% check if G has higher gain than Gd over the intended bandwidth. Now,
% since the magnitude of Gd is above 1 up to some 15 rad/s, we need to 
% aim at making the bandwidth at least this high. But this introduces a
% problem. From some 0.38 rad/s, the gain of Gd is higher that that of G,
% which signals possible problems with saturation.

G = 40*tf(1,[5 1])*tf(1,[2.5 1]);
Gd = 3*tf([50 1],[10 1])*tf(1,[1 1]);

figure(7)
bodemag(G,Gd)
grid on

%% Limitations due to saturation and instability
% The same assumption and task as above. But this time note that the gain
% of Gd is below 1, hence one is tempted to conclude that there is no need
% to introduce a feedback controller at all. However, disturbance rejection is not the only 
% the only motivation for introduction of feedback; stabilization is another. And indeed, 
% this system needs stabilization as it is unstable. We know (from the
% lecture) that the bandwidth (defined using T) should be at least twice
% the frequency of the unstable pole, that is, it should be greated than 2.
% But this brings a problem! At the frequencies above 0.6 rad/s, the gain
% of Gd is greated than that of G, which means that there is a possibility
% of saturation.

G = 5*tf(1,[10 1])*tf(1,[1 -1]);

k = 0.8; % should be less than 1
Gd = k*tf(1,[1 1])*tf(1,[0.2 1]);

figure(8)
bodemag(G,Gd)
grid on

%% MIMO zeros

s = tf('s');

G = [1, 1; 1+2*s, 2]/((0.2*s+1)*(s+1))
G = tf(G);

z = tzero(G)
Gz = evalfr(G,z)
rank(Gz)
[U,S,V] = svd(Gz);
Uz = U(:,2), Vz = V(:,2),

D1 = [1; 0];
D2 = [0; 1];
D12 = [1; -1];

figure(9);

subplot(1,3,1), s1 = stepplot(G*D1); set(s1,'IOGrouping','outputs'), grid on, title('(a) step in u_1'), xlabel('Time'), ylabel('Response')
subplot(1,3,2), s2 = stepplot(G*D2); set(s2,'IOGrouping','outputs'), grid on, title('(b) step in u_2'), xlabel('Time'), ylabel('')
subplot(1,3,3), s3 = stepplot(G*D12); set(s3,'IOGrouping','outputs'), grid on, title('(c) combined step in [u_1,u_2]^T=[1,-1]^T'), xlabel('Time'), ylabel('')

M1 = 1.5; M2 = M1; wb1 = abs(z)/2; wb2 = wb1; A1 = 10e-4; A2 = A1;
wp1 = (s/M1+wb1)/(s+wb1*A1); wp2 = (s/M2+wb2)/(s+wb2*A2); 
W1 = [wp1, 0; 0, wp2];W2 = eye(2,2); W3 = [];

[K,CL,GAM,INFO]=mixsyn(ss(G),W1,W2,W3);
CL1 = loopsens(G,K);

M1 = 1.5; M2 = M1; wb1 = abs(z)/2; wb2 = 25; A1 = 10e-4; A2 = A1;
wp1 = (s/M1+wb1)/(s+wb1*A1); wp2 = (s/M2+wb2)/(s+wb2*A2); 
W1 = [wp1, 0; 0, wp2];W2 = eye(2,2); W3 = [];

[K,CL,GAM,INFO]=mixsyn(ss(G),W1,W2,W3);
CL2 = loopsens(G,K);

figure(10)
sigma(CL1.So,'b',CL1.To,'r',CL2.So,'b-.',CL2.To,'r-.'), grid on,
legend('S1','T1','S2','T2')
title(''), xlabel('Frekvence'), ylabel('Zesileni')

figure(11)
s4 = stepplot(CL1.To*D12,'b',CL2.To*D12,'r'); set(s4,'IOGrouping','outputs'), grid on, title(''), xlabel('Time'), ylabel('Response to [1; -1]')