%==========================================================================
%                       Frequency dependent RGA analysis
%==========================================================================

clc
clear all
close all

% Loading the state-space models
s = tf('s');
load u2_to_y1.mat
load u2_to_y2.mat
load u7_to_y1.mat
load u7_to_y2.mat

% Gain matrix
G = [u2y1 u7y1;
     u2y2 u7y2];

% Computing the RGA matrix as a function of frequency
omega = logspace(-4,2,150);
RGAw  = zeros(2,2,length(omega));
RGAno_d = zeros(1,length(omega));
RGAno_od = zeros(1,length(omega));
for i = 1:length(omega)
    Gf = freqresp(G,omega(i));
    RGAw(:,:,i) = Gf.*inv(Gf).';
    RGAno_d(i) = sum(sum(abs(RGAw(:,:,i) - [1 0;0 1])));
    RGAno_od(i) = sum(sum(abs(RGAw(:,:,i) - [0 1;1 0])));
end

% Plotting
RGA = frd(RGAw,omega);
figure(1)
plot(RGA(1,1),'k',RGA(2,1),'k:')
set(gca,'XScale','log','FontName','Arial')
xlabel('\fontsize{12}Frequency [rad/s]')
ylabel('\fontsize{12}\lambda\rm_{ij}')
legend('\fontsize{12}\lambda\rm_{11}','\fontsize{12}\lambda\rm_{21}')
figure(2)
plot(omega,RGAno_d,'k',omega,RGAno_od,'k:')
set(gca,'XScale','log','FontName','Arial')
xlabel('\fontsize{12}Frequency [rad/s]')
ylabel('\fontsize{12}||\Lambda - I\rm||_{sum}')
figure(3)
subplot(2,1,1)
plot(RGA(1,1),'k',RGA(2,1),'k:')
set(gca,'XScale','log','FontName','Arial')
ylabel('\fontsize{14}\lambda\rm_{ij}')
subplot(2,1,2)
plot(omega,RGAno_d,'k',omega,RGAno_od,'k:')
set(gca,'XScale','log','FontName','Arial')
ylabel('\fontsize{14}||\Lambda - I\rm||_{sum}')