% Model-based control systems
% Exercise 10
clearvars
close all
clc

%
% Implement the controller in Matlab/Simulink and plot the responses of the outputs when
% a. a unit step enters the reference of channel 1,
% b. a unit step enters the reference of channel 2 and
% c. unit steps enter at both channels simultaneously.

s=tf('s');
G=1/((0.2*s+1)*(s+1))*[1 1;1+2*s 2];
G0=[1 1;1 2];
RGAG0=G0.*(inv(G0))';
sigma(G);

%
% At the zero angular frequency the preferable pairing would seem to be u_1 \leftrightarrow y_1 and u_2 \leftrightarrow y_2, although neither one is really good.  But at the gain crossover frequency the pairing  u_1 \leftrightarrow y_2 and u_2 \leftrightarrow y_1 should be preferred.

% Let us try a pairing u1<->y1, u2<->y2 first.  
% PID control.
clc
[Fy1,Info1]=pidtune(G(1,1),'pid')
[Fy2,Info2]=pidtune(G(2,2),'pid')

% The solution is unstable.
%

% Let us try a pairing u1<->y2, u2<->y1.
% PID control.
clc
[Fy1,Info1]=pidtune(G(1,2),'pid')
[Fy2,Info2]=pidtune(G(2,1),'pid')

% The solution is unstable.
%
% Run this section before simulating

% 1 DOF controllers
Fy=[Fy1 0;0 Fy2]
Fr=Fy

% Conclusion:  pidtune is for SISO tuning only.  There is no guarantee that it would be good for MIMO systems.  The RGA analysis showed that at zero frequency the coupling u_1 \leftrightarrow y_1, u_2 \leftrightarrow y_2 should be preferred.  For step responses that seems to be so.  At the gain crossover frequency the pairing  u_1 \leftrightarrow y_2, u_2 \leftrightarrow y_1 seemed to be preferable, but that could not be observed by simulations with step reference inputs.
%
% b.  Design a decoupling controller using the singular value decomposition at zero frequency and choosing the weight matrices W1 and W2  accordingly (see Chapter 6 in the lecture slides).  Use PID-controllers in the decoupled system.  Simulate as in part a.
% There are many ways to solve this.  The solution below is based on the below figure, from which the program code becomes understandable.  See also Chapter 6 in the lecture slides
%
%
% The last part of the figure can be explained by noting that y=W_2^{-1} \overset{~}{y} since then also r=W_2^{-1} \overset{~}{r}.  The reference signal has to be scaled to get the static gain from r to y to be 1.  The following results are obtained by the below program code.  The result is acceptable but a bit slow.  Again, the PID tuning could be improved.

 % SVD and construction of pre- and post compensators
[U,S,V]=svd(G0);
W1=V; W2=U';
% Diagonalazied plant; design of controller
Gworm=minreal(W2*G*W1);
[Fp1worm,Info1]=pidtune(Gworm(1,1),'pid');
[Fp2worm,Info2]=pidtune(Gworm(2,2),'pid');
% 1 DOF controller in "worm" domain
Fpworm=[Fp1worm 0;0 Fp2worm];
% 2 DOF
Fy=Fpworm*W2;
Fr=Fpworm*inv(W2);
G=G*W1; % Modified plant
 

%
% Use this section to plot the results of a and b.

%Sensitivity functions
L=minreal(G*Fy);
S=minreal(inv(eye(2)+L));
Tcomp=minreal(eye(2)-S);
Gc=S*G*Fr;
Gc2=Gc;
 
figure(1)
sigma(L,S,Tcomp)
title('Functions L, S, T')
%
%
%Simulation
T=0:0.01:10;
figure(2)
Uinp=ones(length(T),2);
subplot(221)
lsim(Gc2,Uinp,T)
title('Step(1,1)')
%
Uinp=[ones(length(T),1) zeros(length(T),1)];
subplot(222)
lsim(Gc2,Uinp,T)
title('Step(1,0)')
%
Uinp=[zeros(length(T),1) ones(length(T),1)];
subplot(223)
lsim(Gc2,Uinp,T)
title('Step(0,1)')
%
Uinp=[zeros(length(T),1) zeros(length(T),1)];
subplot(224)
lsim(Gc2,Uinp,T)
title('Step(0,0)')

