% Model-based control systems
% Ex9 - Problem 2
clear all; clc; close all
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);
%
% Let us try a pairing u1-y1, u2-y2 first.  PID control.
% [Fy1,Info1]=pidtune(G(1,1),'pid');
% [Fy2,Info2]=pidtune(G(2,2),'pid');
%  
% %1 DOF controllers
% Fy=[Fy1 0;0 Fy2];
% Fr=Fy;
%  
% Let us then try a pairing u1-y2, u2-y1.  PID control.
%[Fy1,Info1]=pidtune(G(1,2),'pid');
%[Fy2,Info2]=pidtune(G(2,1),'pid');
 
%1 DOF controllers
%Fy=[0 Fy1;Fy2 0];
%Fr=Fy;
 
%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
 
%Sensitivity functions
L=minreal(G*Fy);
S=minreal(inv(eye(2)+L));
Tcomp=minreal(eye(2)-S);
Gc=S*G*Fr;
Gc2=Gc;
 
figure
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)')