%% Controllability analysis when gas-lift choke is the manipulated variable
clear all;
clc;
warning off
global u0;
%% Inputs to model 
% Manipulated variables
z1=0.6;    % production choke opening (u1) 
z2=0.4;    % injection valve opening(u2) 
% Disturbances
GOR = 0;                     % Mass GOR from reservoir, NB: MASS, increasing gives: lower BH, stable
P_r = 160;                   % Rservoir pressure [bar]
P_gs = 140;                  % Gass source pressure [bar]
P0 = 15;                     % Separator Pressure [bar]
d = [GOR;P_r;P_gs;P0];
u0=[z1;z2;d]; %Only u(1) and u(2) are MVs

%% ***** Parameter Constants*******
par = v1_gaslift_parameters();
%% ******** Initializing *************
disp('Initializing...')
tic
[x0,flag,yss] = v1_gaslift_initialize(u0,par);
toc
disp('Initialization completed.');
disp('#########################');
%% ******** Linearization *************
[A,B,C,D]=linmod('linerazation_simulink',x0,u0); % NB: uses S-function which needs xeq set in own file
[De,Dd,Du] = scaling(z1,z2);
sys =(ss(A,B,C,D));
set(sys,'inputname',{'Z1','Z2','GOR','P_r','P_gs','P0'},'outputname',{'Wg_in','P_at','P_rt','P_ab','P_bh','W','Rho','Alpha_L'},'statename',{'m_G1','m_G2','m_L2'});
%% ****Scaling****

Bs1 = B(:,1:2)*Du;
Bs2 = B(:,3:6)*Dd;
Bs = [Bs1 Bs2];

Cs = (De^-1)*C;

Ds1 = (De^-1)*D(:,1:2)*Du;
Ds2 = (De^-1)*D(:,3:6)*Dd;
Ds = [Ds1 Ds2];
sys =(ss(A,Bs,Cs,Ds));
set(sys,'inputname',{'Z1','Z2','GOR','P_r','P_gs','P0'},'outputname',{'Wg_in','P_at','P_rt','P_ab','P_bh','W','Rho','Alpha_L'},'statename',{'m_G1','m_G2','m_L2'});
Gu = tf(sys({'Wg_in','P_at','P_rt','P_ab','P_bh','W','Rho','Alpha_L'},'Z2'));
Gd = tf(sys({'Wg_in','P_at','P_rt','P_ab','P_bh','W','Rho','Alpha_L'},{'GOR','P_r','P_gs','P0'}));
%% ****Tranfer function and steady state values ****
G1 =(Gu('Wg_in','Z2')); G2 =(Gu('P_at','Z2')); G3 =(Gu('P_rt','Z2')); G4 =(Gu('P_ab','Z2')); 
G5 =(Gu('P_bh','Z2'));  G6 =(Gu('W','Z2'));    G7 =(Gu('Rho','Z2'));  G8 =(Gu('Alpha_l','Z2'));
G10 = abs(freqresp(G1,0)); G20 = abs(freqresp(G2,0)); G30 = abs(freqresp(G3,0)); G40 = abs(freqresp(G4,0)); 
G50 = abs(freqresp(G5,0)); G60 = abs(freqresp(G6,0)); G70 = abs(freqresp(G7,0)); G80 = abs(freqresp(G8,0)); 
%Have to have the disturbance matrix Gd for each output,the gas and liquid flow rate into the riser are going
%to be analysed as two separately disturbances.
Gd11=(Gd('Wg_in','P_r'));  Gd12=(Gd('Wg_in','P_gs'));
Gd21=(Gd('P_at','P_r'));   Gd22=(Gd('P_at','P_gs'));
Gd31=(Gd('P_rt','P_r'));   Gd32=(Gd('P_rt','P_gs'));
Gd41=(Gd('P_ab','P_r'));   Gd42=(Gd('P_ab','P_gs'));
Gd51=(Gd('P_bh','P_r'));   Gd52=(Gd('P_bh','P_gs'));
Gd61=(Gd('W','P_r'));      Gd62=(Gd('W','P_gs'));
Gd71=(Gd('Rho','P_r'));    Gd72=(Gd('Rho','P_gs'));
Gd81=(Gd('Alpha_L','P_r')); Gd82=(Gd('Alpha_L','P_gs'));

%Both G1, G2 and G3 have the same poles.
disp('poles:')
p=pole(G1);
disp(p)
disp('zeros of Wg_in:')
z1=zero(G1);
disp(z1)
disp('zeros of P_at:')
z2=zero(G2);
disp(z2)
disp('zeros of P_rt:')
z3=zero(G3);
disp(z3)

disp('zeros of P_ab:')
z4=zero(G4);
disp(z4)

disp('zeros of P_bh:')
z5=zero(G5);
disp(z5)

disp('zeros of W:')
z6=zero(G6);
disp(z6)

disp('zeros of Rho:')
z7=zero(G7);
disp(z7)

disp('zeros of AlphaL:')
z8=zero(G8);
disp(z8)


% Stable phase, minimum phase and minimum stable phase of all TF, Functions
% are made to performed this.
[Gs1 Gm1 Gms1]=gmsi(G1);
[Gs2 Gm2 Gms2]=gmsi(G2);
[Gs3 Gm3 Gms3]=gmsi(G3);
[Gs4 Gm4 Gms4]=gmsi(G4);
[Gs5 Gm5 Gms5]=gmsi(G5);
[Gs6 Gm6 Gms6]=gmsi(G6);
[Gs7 Gm7 Gms7]=gmsi(G7);
[Gs8 Gm8 Gms8]=gmsi(G8);

[Gds11 Gdm11 Gdms11]=gmsdij(Gd11);
[Gds12 Gdm12 Gdms12]=gmsdij(Gd12);

[Gds21 Gdm21 Gdms21]=gmsdij(Gd21);
[Gds22 Gdm22 Gdms22]=gmsdij(Gd22);

[Gds31 Gdm31 Gdms31]=gmsdij(Gd31);
[Gds32 Gdm32 Gdms32]=gmsdij(Gd32);

[Gds41 Gdm41 Gdms41]=gmsdij(Gd41);
[Gds42 Gdm42 Gdms42]=gmsdij(Gd42);

[Gds51 Gdm51 Gdms51]=gmsdij(Gd51);
[Gds52 Gdm52 Gdms52]=gmsdij(Gd52);

[Gds61 Gdm61 Gdms61]=gmsdij(Gd61);
[Gds62 Gdm62 Gdms62]=gmsdij(Gd62);

[Gds71 Gdm71 Gdms71]=gmsdij(Gd71);
[Gds72 Gdm72 Gdms72]=gmsdij(Gd72);

[Gds81 Gdm81 Gdms81]=gmsdij(Gd81);
[Gds82 Gdm82 Gdms82]=gmsdij(Gd82);

% ******** Controllability analysis*************
%*** Sensitivity and complementery sensitivity peak***
%% Sensitivity peak for Wg_in.
sys1 = sys('Wg_in','Z2');
[p, z1]=pzmap(sys1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));
l=1;
if (isempty(RHPp1) || isempty(RHPz1))
    Msmin1=1;
else
    np1=length(RHPp1); nz1=length(RHPz1);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp1 = zeros(l,np1);
    Yz1 = zeros(l,nz1);
    for i=1:np1
        Yp1(:,i) = Cs(1,:)*V(:,i)/norm(Cs(1,:)*V(:,i)); %Pole direction
    end
    for j=1:nz1
        [U,S,V]=svd(evalfr(G1,RHPz1(j))); Yz1(:,j)=U(:,end); %zero direction
    end
    Qz1 = zeros(nz1,nz1); Qp1 = zeros(np1,np1); Qzp1 = zeros(nz1,np1);
    for i=1:nz1
        for j=1:nz1
            Qz1(i,j) = ctranspose(Yz1(:,i))*Yz1(:,j)/(RHPz1(i)+conj(RHPz1(j)));
        end
    end
    for i=1:np1
        for j=1:np1
            Qp1(i,j) = ctranspose(Yp1(:,i))*Yp1(:,j)/(conj(RHPp1(i))+RHPp1(j));
        end
    end
    for i=1:nz1
        for j=1:np1
            Qzp1(i,j) = ctranspose(Yz1(:,i))*Yp1(:,j)/(RHPz1(i)-RHPp1(j));
        end
    end
    %     Qp1=(Yp'*Yp).*(1./(diag(RHPp1')*ones(np1)+ones(np1)*diag(RHPp1)));
    %     Qz1=(Yz'*Yz).*(1./(diag(RHPz1)*ones(nz1)+ones(nz1)*diag(RHPz1')));
    %     Qzp1=(Yz'*Yp).*(1./(diag(RHPz1)*ones(nz1,np1)-ones(nz1,np1)*diag(RHPp1)));
    Msmin1=sqrt(1+norm(sqrtm(inv(Qz1))*Qzp1*sqrtm(inv(Qp1)))^2);
end

%% Sensitivity peak for P_at
sys2 = sys('P_at','Z2');
l=1;
[p, z2]=pzmap(sys2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if (isempty(RHPp2) || isempty(RHPz2))
    Msmin2=1;
else
    np2=length(RHPp2); nz2=length(RHPz2);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp2 = zeros(l,np2);
    Yz2 = zeros(l,nz2);
    for i=1:np2
        Yp2(:,i) = Cs(2,:)*V(:,i)/norm(Cs(2,:)*V(:,i)); %Pole direction
    end
    for j=1:nz2
        [U,S,V]=svd(evalfr(G2,RHPz2(j))); Yz2(:,j)=U(:,end); %zero direction
    end
    Qz2 = zeros(nz2,nz2); Qp2 = zeros(np2,np2); Qzp2 = zeros(nz2,np2);
    for i=1:nz2
        for j=1:nz2
            Qz2(i,j) = ctranspose(Yz2(:,i))*Yz2(:,j)/(RHPz2(i)+conj(RHPz2(j)));
        end
    end
    for i=1:np2
        for j=1:np2
            Qp2(i,j) = ctranspose(Yp2(:,i))*Yp2(:,j)/(conj(RHPp2(i))+RHPp2(j));
        end
    end
    for i=1:nz2
        for j=1:np2
            Qzp2(i,j) = ctranspose(Yz2(:,i))*Yp2(:,j)/(RHPz2(i)-RHPp2(j));
        end
    end
    
    %     Qp2=(Yp2'*Yp2).*(1./(diag(RHPp2')*ones(np2)+ones(np2)*diag(RHPp2)));
    %     Qz2=(Yz2'*Yz2).*(1./(diag(RHPz2)*ones(nz2)+ones(nz2)*diag(RHPz2')));
    %     Qzp2=(Yz2'*Yp2).*(1./(diag(RHPz2)*ones(nz2,np2)-ones(nz2,np2)*diag(RHPp2)));
    Msmin2=sqrt(1+norm((Qz2^(-0.5))*Qzp2*(Qp2^(-0.5)))^2);
end


%% Sensitivity peak for P_rt
sys3 = sys('P_rt','Z2');
l=1;
[p, z3]=pzmap(sys3);
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if (isempty(RHPp3) || isempty(RHPz3))
    Msmin3=1;
else
    np3=length(RHPp3); nz3=length(RHPz3);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp3 = zeros(l,np3);
    Yz3 = zeros(l,nz3);
    for i=1:np3
        Yp3(:,i) = Cs(3,:)*V(:,i)/norm(Cs(3,:)*V(:,i)); %Pole direction
    end
    for j=1:nz3
        [U,S,V]=svd(evalfr(G3,RHPz3(j))); Yz3(:,j)=U(:,end); %zero direction
    end
    
    Qz3 = zeros(nz3,nz3); Qp3 = zeros(np3,np3); Qzp3 = zeros(nz3,np3);
    for i=1:nz3
        for j=1:nz3
            Qz3(i,j) = ctranspose(Yz3(:,i))*Yz3(:,j)/(RHPz3(i)+conj(RHPz3(j)));
        end
    end
    for i=1:np3
        for j=1:np3
            Qp3(i,j) = ctranspose(Yp3(:,i))*Yp3(:,j)/(conj(RHPp3(i))+RHPp3(j));
        end
    end
    for i=1:nz3
        for j=1:np3
            Qzp3(i,j) = ctranspose(Yz3(:,i))*Yp3(:,j)/(RHPz3(i)-RHPp3(j));
        end
    end
    
    %     Qp3=(Yp3'*Yp3).*(1./(diag(RHPp3')*ones(np3)+ones(np3)*diag(RHPp3)));
    %     Qz3=(Yz3'*Yz3).*(1./(diag(RHPz3)*ones(nz3)+ones(nz3)*diag(RHPz3')));
    %     Qzp3=(Yz3'*Yp3).*(1./(diag(RHPz3)*ones(nz3,np3)-ones(nz3,np3)*diag(RHPp3)));
    Msmin3=sqrt(1+norm((Qz3^(-0.5))*Qzp3*(Qp3^(-0.5)))^2);
end



%% Sensitivity peak for P_ab
sys4 = sys('P_ab','Z2');
l=1;
[p, z4]=pzmap(sys4);
RHPp4=p(find(p>0));
RHPz4=z4(find(z4>0));

if (isempty(RHPp4) || isempty(RHPz4))
    Msmin4=1;
else
    np4=length(RHPp4); nz4=length(RHPz4);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp4 = zeros(l,np4);
    Yz4 = zeros(l,nz4);
    for i=1:np4
        Yp4(:,i) = Cs(4,:)*V(:,i)/norm(Cs(4,:)*V(:,i)); %Pole direction
    end
    for j=1:nz4
        [U,S,V]=svd(evalfr(G4,RHPz4(j))); Yz4(:,j)=U(:,end); %zero direction
    end
    
    Qz4 = zeros(nz4,nz4); Qp4 = zeros(np4,np4); Qzp4 = zeros(nz4,np4);
    for i=1:nz4
        for j=1:nz4
            Qz4(i,j) = ctranspose(Yz4(:,i))*Yz4(:,j)/(RHPz4(i)+conj(RHPz4(j)));
        end
    end
    for i=1:np4
        for j=1:np4
            Qp4(i,j) = ctranspose(Yp4(:,i))*Yp4(:,j)/(conj(RHPp4(i))+RHPp4(j));
        end
    end
    for i=1:nz4
        for j=1:np4
            Qzp4(i,j) = ctranspose(Yz4(:,i))*Yp4(:,j)/(RHPz4(i)-RHPp4(j));
        end
    end
    
    %     Qp4=(Yp4'*Yp4).*(1./(diag(RHPp4')*ones(np4)+ones(np4)*diag(RHPp4)));
    %     Qz4=(Yz4'*Yz4).*(1./(diag(RHPz4)*ones(nz4)+ones(nz4)*diag(RHPz4')));
    %     Qzp4=(Yz4'*Yp4).*(1./(diag(RHPz4)*ones(nz4,np4)-ones(nz4,np4)*diag(RHPp4)));
    Msmin4=sqrt(1+norm((Qz4^(-0.5))*Qzp4*(Qp4^(-0.5)))^2);
end

%% Sensitivity peak for P_bh.
sys5 = sys('P_bh','Z2');
[p, z5]=pzmap(sys5);
RHPp5=p(find(p>0));
RHPz5=z5(find(z5>0));

if (isempty(RHPp5) || isempty(RHPz5))
    Msmin5=1;
else
    np5=length(RHPp5); nz5=length(RHPz5);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp5 = zeros(l,np5);
    Yz5 = zeros(l,nz5);
    for i=1:np5
        Yp5(:,i) = Cs(5,:)*V(:,i)/norm(Cs(5,:)*V(:,i)); %Pole direction
    end
    for j=1:nz5
        [U,S,V]=svd(evalfr(G5,RHPz5(j))); Yz5(:,j)=U(:,end); %zero direction
    end
    Qz5 = zeros(nz5,nz5); Qp5 = zeros(np5,np5); Qzp5 = zeros(nz5,np5);
    for i=1:nz5
        for j=1:nz5
            Qz5(i,j) = ctranspose(Yz5(:,i))*Yz5(:,j)/(RHPz5(i)+conj(RHPz5(j)));
        end
    end
    for i=1:np5
        for j=1:np5
            Qp5(i,j) = ctranspose(Yp5(:,i))*Yp5(:,j)/(conj(RHPp5(i))+RHPp5(j));
        end
    end
    for i=1:nz5
        for j=1:np5
            Qzp5(i,j) = ctranspose(Yz5(:,i))*Yp5(:,j)/(RHPz5(i)-RHPp5(j));
        end
    end
    %     Qp5=(Yp'*Yp).*(1./(diag(RHPp5')*ones(np5)+ones(np5)*diag(RHPp5)));
    %     Qz5=(Yz'*Yz).*(1./(diag(RHPz5)*ones(nz5)+ones(nz5)*diag(RHPz5')));
    %     Qzp5=(Yz'*Yp).*(1./(diag(RHPz5)*ones(nz5,np5)-ones(nz5,np5)*diag(RHPp5)));
    Msmin5=sqrt(1+norm(sqrtm(inv(Qz5))*Qzp5*sqrtm(inv(Qp5)))^2);
end


%% Sensitivity peak for W.
sys6 = sys('W','Z2');
[p, z6]=pzmap(sys6);
RHPp6=p(find(p>0));
RHPz6=z6(find(z6>0));

if (isempty(RHPp6) || isempty(RHPz6))
    Msmin6=1;
else
    np6=length(RHPp6); nz6=length(RHPz6);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp6 = zeros(l,np6);
    Yz6 = zeros(l,nz6);
    for i=1:np6
        Yp6(:,i) = Cs(6,:)*V(:,i)/norm(Cs(6,:)*V(:,i)); %Pole direction
    end
    for j=1:nz6
        [U,S,V]=svd(evalfr(sys6,RHPz6(j))); Yz6(:,j)=U(:,end); %zero direction
    end
    Qz6 = zeros(nz6,nz6); Qp6 = zeros(np6,np6); Qzp6 = zeros(nz6,np6);
    for i=1:nz6
        for j=1:nz6
            Qz6(i,j) = ctranspose(Yz6(:,i))*Yz6(:,j)/(RHPz6(i)+conj(RHPz6(j)));
        end
    end
    for i=1:np6
        for j=1:np6
            Qp6(i,j) = ctranspose(Yp6(:,i))*Yp6(:,j)/(conj(RHPp6(i))+RHPp6(j));
        end
    end
    for i=1:nz6
        for j=1:np6
            Qzp6(i,j) = ctranspose(Yz6(:,i))*Yp6(:,j)/(RHPz6(i)-RHPp6(j));
        end
    end
    %     Qp6=(Yp'*Yp).*(1./(diag(RHPp6')*ones(np6)+ones(np6)*diag(RHPp6)));
    %     Qz6=(Yz'*Yz).*(1./(diag(RHPz6)*ones(nz6)+ones(nz6)*diag(RHPz6')));
    %     Qzp6=(Yz'*Yp).*(1./(diag(RHPz6)*ones(nz6,np6)-ones(nz6,np6)*diag(RHPp6)));
    Msmin6=sqrt(1+norm(sqrtm(inv(Qz6))*Qzp6*sqrtm(inv(Qp6)))^2);
end


%% Sensitivity peak for Rho.
sys7 = sys('Rho','Z2');
[p, z7]=pzmap(sys7);
RHPp7=p(find(p>0));
RHPz7=z7(find(z7>0));

if (isempty(RHPp7) || isempty(RHPz7))
    Msmin7=1;
else
    np7=length(RHPp7); nz7=length(RHPz7);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp7 = zeros(l,np7);
    Yz7 = zeros(l,nz7);
    for i=1:np7
        Yp7(:,i) = Cs(7,:)*V(:,i)/norm(Cs(7,:)*V(:,i)); %Pole direction
    end
    for j=1:nz7
        [U,S,V]=svd(evalfr(sys7,RHPz7(j))); Yz7(:,j)=U(:,end); %zero direction
    end
    Qz7 = zeros(nz7,nz7); Qp7 = zeros(np7,np7); Qzp7 = zeros(nz7,np7);
    for i=1:nz7
        for j=1:nz7
            Qz7(i,j) = ctranspose(Yz7(:,i))*Yz7(:,j)/(RHPz7(i)+conj(RHPz7(j)));
        end
    end
    for i=1:np7
        for j=1:np7
            Qp7(i,j) = ctranspose(Yp7(:,i))*Yp7(:,j)/(conj(RHPp7(i))+RHPp7(j));
        end
    end
    for i=1:nz7
        for j=1:np7
            Qzp7(i,j) = ctranspose(Yz7(:,i))*Yp7(:,j)/(RHPz7(i)-RHPp7(j));
        end
    end
    %     Qp7=(Yp'*Yp).*(1./(diag(RHPp7')*ones(np7)+ones(np7)*diag(RHPp7)));
    %     Qz7=(Yz'*Yz).*(1./(diag(RHPz7)*ones(nz7)+ones(nz7)*diag(RHPz7')));
    %     Qzp7=(Yz'*Yp).*(1./(diag(RHPz7)*ones(nz7,np7)-ones(nz7,np7)*diag(RHPp7)));
    Msmin7=sqrt(1+norm(sqrtm(inv(Qz7))*Qzp7*sqrtm(inv(Qp7)))^2);
end
%% Sensitivity peak for AlphaL.
sys8 = sys('Alpha_L','Z2');
[p, z8]=pzmap(sys8);
RHPp8=p(find(p>0));
RHPz8=z8(find(z8>0));

if (isempty(RHPp8) || isempty(RHPz8))
    Msmin8=1;
else
    np8=length(RHPp8); nz8=length(RHPz8);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp8 = zeros(l,np8);
    Yz8 = zeros(l,nz8);
    for i=1:np8
        Yp8(:,i) = Cs(8,:)*V(:,i)/norm(Cs(8,:)*V(:,i)); %Pole direction
    end
    for j=1:nz8
        [U,S,V]=svd(evalfr(sys8,RHPz8(j))); Yz8(:,j)=U(:,end); %zero direction
    end
    Qz8 = zeros(nz8,nz8); Qp8 = zeros(np8,np8); Qzp8 = zeros(nz8,np8);
    for i=1:nz8
        for j=1:nz8
            Qz8(i,j) = ctranspose(Yz8(:,i))*Yz8(:,j)/(RHPz8(i)+conj(RHPz8(j)));
        end
    end
    for i=1:np8
        for j=1:np8
            Qp8(i,j) = ctranspose(Yp8(:,i))*Yp8(:,j)/(conj(RHPp8(i))+RHPp8(j));
        end
    end
    for i=1:nz8
        for j=1:np8
            Qzp8(i,j) = ctranspose(Yz8(:,i))*Yp8(:,j)/(RHPz8(i)-RHPp8(j));
        end
    end
    %     Qp8=(Yp'*Yp).*(1./(diag(RHPp8')*ones(np8)+ones(np8)*diag(RHPp8)));
    %     Qz8=(Yz'*Yz).*(1./(diag(RHPz8)*ones(nz8)+ones(nz8)*diag(RHPz8')));
    %     Qzp8=(Yz'*Yp).*(1./(diag(RHPz8)*ones(nz8,np8)-ones(nz8,np8)*diag(RHPp8)));
    Msmin8=sqrt(1+norm(sqrtm(inv(Qz8))*Qzp8*sqrtm(inv(Qp8)))^2);
end

%% *** Bounds on KS ****
%To find the bound for KS the mirror image of the antistable part of the
%G`s are used. If a zero is very close to be negativ, this zero will be
%implemented in the antistable part.

%Split G into stable and anti stable part, by using a matlab
%function.
[GSu1, GNSu1]=stabsep(G1);
[GSu2, GNSu2]=stabsep(G2);
[GSu3, GNSu3]=stabsep(G3);
[GSu4, GNSu4]=stabsep(G4);
[GSu5, GNSu5]=stabsep(G5);
[GSu6, GNSu6]=stabsep(G6);
[GSu7, GNSu7]=stabsep(G7);
[GSu8, GNSu8]=stabsep(G8);


% [GSu1, GNSu1]=stabproj(G1);
% [GSu2, GNSu2]=stabproj(G2);
% [GSu3, GNSu3]=stabproj(G3);
% [GSu4, GNSu4]=stabproj(G4);
% [GSu5, GNSu5]=stabproj(G5);
% [GSu6, GNSu6]=stabproj(G6);
% [GSu7, GNSu7]=stabproj(G7);
% [GSu8, GNSu8]=stabproj(G8);


%Lower bound on KS for the output Wg_in
tf1=(GNSu1)';
h1=hsvd(tf1);
KSmin1=1/min(h1);

%Lower bound on KS for the output P_at
tf2=(GNSu2)';
h2=hsvd(tf2);
KSmin2=1/min(h2);

%Lower bound on KS for the output P_rt
tf3=(GNSu3)';
h3=hsvd(tf3);
KSmin3=1/min(h3);

%Lower bound on KS for the output P_ab
tf4=(GNSu4)';
h4=hsvd(tf4);
KSmin4=1/min(h4);

%Lower bound on KS for the output P_bh
tf5=(GNSu5)';
h5=hsvd(tf5);
KSmin5=1/min(h5);

%Lower bound on KS for the output W
tf6=(GNSu6)';
h6=hsvd(tf6);
KSmin6=1/min(h6);

%Lower bound on KS for the output Rho
tf7=(GNSu7)';
h7=hsvd(tf7);
KSmin7=1/min(h7);

%Lower bound on KS for the output AlphaL
tf8=(GNSu8)';
h8=hsvd(tf8);
KSmin8=1/min(h8);

%% *** bounds on KSGd ****
s=tf('s') ;
%There are two disturbances for each output, and they will be considered
%separately. d1=wG_in and d2=wL_in

%Lower bound on KSGd1 for the output Wg_in
solve1=((inv(Gdms11))*G1);
[t11s t11ns]=gnsi(solve1);
tfKSd11=(t11ns)';
hd11=hsvd(tfKSd11);
KSGd11=1/min(hd11);

%Lower bound on KSGd2 for the output Wg_in
solve2=((inv(Gdms12))*G1);
[t12s t12ns]=gnsi(solve2);
tfKSd12=(t12ns)';
hd12=hsvd(tfKSd12);
KSGd12=1/min(hd12);

%Lower bound on KSGd1 for the output P_at
solve3=((inv(Gdms21))*G2);
[t21s t21ns]=gnsi(solve3);
tfKSd21=(t21ns)';
hd21=hsvd(tfKSd21);
KSGd21=1/min(hd21);

%Lower bound on KSGd2 for the output P_at
solve4=((inv(Gdms22))*G2);
[t22s t22ns]=gnsi(solve4);
tfKSd22=(t22ns)';
hd22=hsvd(tfKSd22);
KSGd22=1/min(hd22);

%Lower bound on KSGd1 for the output P_rt
%Put in one stable zero, so gnsi works
solve5=((inv(Gdms31))*G3);
[t31s t31ns]=gnsi(solve5);
tfKSd31=(t31ns)';
hd31=hsvd(tfKSd31);
KSGd31=1/min(hd31);

%Lower bound on KSGd2 for the output P_rt
solve6=((inv(Gdms32))*G3);
[t32s t32ns]=gnsi(solve6);
tfKSd32=(t32ns)';
hd32=hsvd(tfKSd32);
KSGd32=1/min(hd32);

%Lower bound on KSGd1 for the output P_ab
solve7=(inv(Gdms41))*G4;
[t41s t41ns]=gnsi(solve7);
tfKSd41=(t41ns)';
hd41=hsvd(tfKSd41);
KSGd41=1/min(hd41);

%Lower bound on KSGd2 for the output P_ab
solve8=(inv(Gdms42))*G4;
[t42s t42ns]=gnsi(solve8);
tfKSd42=(t42ns)';
hd42=hsvd(tfKSd42);
KSGd42=1/min(hd42);

%Lower bound on KSGd1 for the output P_bh
solve9=((inv(Gdms51))*G5);
[t51s t51ns]=gnsi(solve9);
tfKSd51=(t51ns)';
hd51=hsvd(tfKSd51);
KSGd51=1/min(hd51);

%Lower bound on KSGd2 for the output P_bh
solve10=((inv(Gdms52))*G5);
[t52s t52ns]=gnsi(solve10);
tfKSd52=(t52ns)';
hd52=hsvd(tfKSd52);
KSGd52=1/min(hd52);

%Lower bound on KSGd1 for the output W
solve11=((inv(Gdms61))*G6);
[t61s t61ns]=gnsi(solve11);
tfKSd61=(t61ns)';
hd61=hsvd(tfKSd61);
KSGd61=1/min(hd61);

%Lower bound on KSGd2 for the output W
solve12=((inv(Gdms62))*G6);
[t62s t62ns]=gnsi(solve12);
tfKSd62=(t62ns)';
hd62=hsvd(tfKSd62);
KSGd62=1/min(hd62);

%Lower bound on KSGd1 for the output Rho
solve13=((inv(Gdms71))*G7);
[t71s t71ns]=gnsi(solve13);
tfKSd71=(t71ns)';
hd71=hsvd(tfKSd71);
KSGd71=1/min(hd71);

%Lower bound on KSGd2 for the output Rho
solve14=((inv(Gdms72))*G7);
[t72s t72ns]=gnsi(solve14);
tfKSd72=(t72ns)';
hd72=hsvd(tfKSd72);
KSGd72=1/min(hd72);

%Lower bound on KSGd1 for the output AlpaL
solve15=((inv(Gdms81))*G8);
[t81s t81ns]=gnsi(solve15);
tfKSd81=(t81ns)';
hd81=hsvd(tfKSd81);
KSGd81=1/min(hd81);

%Lower bound on KSGd2 for the output AlpaL
solve16=((inv(Gdms82))*G8);
[t82s t82ns]=gnsi(solve16);
tfKSd82=(t82ns)';
hd82=hsvd(tfKSd82);
KSGd82=1/min(hd82);

%% SG *****
%Used the poles and zeros that made the highest bound, since this bound is
%only tight for one RHP-pole and one RHP-zero.
%% SG for Wg_in
[p, z1]=pzmap(sys1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));
l = 1;
if (length(RHPp1)>0 && length(RHPz1)>0)
    np1=length(RHPp1); nz1=length(RHPz1);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp1 =  zeros(l,np1);
    Yz1 = zeros(l,nz1);
    for i=1:np1
        Yp1(:,i) = Cs(1,:)*V(:,i)/norm(Cs(1,:)*V(:,i)); %Pole direction
    end
    for j=1:nz1
        [U,S,V]=svd(evalfr(sys1,RHPz1(j))); Yz1(:,j)=U(:,end); %zero direction
    end
    
    SGmin = zeros(np1,nz1);
    for i=1:np1
        for j=1:nz1
            num1=abs((ctranspose(Yz1(:,j))*(evalfr(Gms1,RHPz1(j))))*((evalfr(Gms1,RHPp1(i)))^-1)*Yp1(:,i));
            den1=norm(ctranspose(Yz1(:,j))*(evalfr(Gms1,RHPz1(j))),2)*norm(((evalfr(Gms1,RHPp1(i)))^-1)*Yp1(:,i),2);
            cos2=(num1/den1)^2;
            sin2=1-cos2;
            
            term1 = (abs(RHPz1(j)+conj(RHPp1(i))))^2/(abs(RHPz1(j)-RHPp1(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz1(:,j))*(evalfr(Gms1,RHPz1(j)))),2)*sqrt(sin2+(term1*cos2));
        end
    end
    SGmin1 = max(max((SGmin)));
else
    SGmin1=0;
end

%% SG for P_at
[p, z2]=pzmap(sys2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if length(RHPp2)>0 && length(RHPz2)>0
    np2=length(RHPp2); nz2=length(RHPz2);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp2 =  zeros(l,np2);
    Yz2 = zeros(l,nz2);
    for i=1:np2
        Yp2(:,i) = Cs(2,:)*V(:,i)/norm(Cs(2,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz2
        [U,S,V]=svd(evalfr(sys2,RHPz2(j))); Yz2(:,j)=U(:,end); %zero direction
    end
    SGmin = zeros(np2,nz2);
    for i=1:np2
        for j=1:nz2
            num2=abs((ctranspose(Yz2(:,j))*(evalfr(Gms2,RHPz2(j))))*((evalfr(Gms2,RHPp2(i)))^-1)*Yp2(:,i));
            den2=norm(ctranspose(Yz2(:,j))*(evalfr(Gms2,RHPz2(j))),2)*norm(((evalfr(Gms2,RHPp2(i)))^-1)*Yp2(:,i),2);
            cos2=(num2/den2)^2;
            sin2=1-cos2;
            
            term2 = (abs(RHPz2(j)+conj(RHPp2(i))))^2/(abs(RHPz2(j)-RHPp2(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz2(:,j))*(evalfr(Gms2,RHPz2(j)))),2)*sqrt(sin2+(term2*cos2));
        end
    end
    SGmin2 = max(max((SGmin)));
else
    SGmin2=0;
end

%% SG for P_rt
[p, z3]=pzmap(sys3);
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if length(RHPp3)>0 && length(RHPz3)>0
    np3=length(RHPp3); nz3=length(RHPz3);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp3 =  zeros(l,np3);
    Yz3 = zeros(l,nz3);
    for i=1:np3
        Yp3(:,i) = Cs(3,:)*V(:,i)/norm(Cs(3,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz3
        [U,S,V]=svd(evalfr(sys3,RHPz3(j))); Yz3(:,j)=U(:,end); %zero direction
    end
    SGmin = zeros(np3,nz3);
    for i=1:np3
        for j=1:nz3
            num3=abs((ctranspose(Yz3(:,j))*(evalfr(Gms3,RHPz3(j))))*((evalfr(Gms3,RHPp3(i)))^-1)*Yp3(:,i));
            den3=norm(ctranspose(Yz3(:,j))*(evalfr(Gms3,RHPz3(j))),2)*norm(((evalfr(Gms3,RHPp3(i)))^-1)*Yp3(:,i),2);
            cos2=(num3/den3)^2;
            sin2=1-cos2;
            
            term3 = (abs(RHPz3(j)+conj(RHPp3(i))))^2/(abs(RHPz3(j)-RHPp3(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz3(:,j))*(evalfr(Gms3,RHPz3(j)))),2)*sqrt(sin2+(term3*cos2));
        end
    end
    SGmin3 = max(max((SGmin)));
else
    SGmin3=0;
end

%% SG for P_ab
[p, z4]=pzmap(sys4);
RHPp4=p(find(p>0));
RHPz4=z4(find(z4>0));

if length(RHPp4)>0 && length(RHPz4)>0
    np4=length(RHPp4); nz4=length(RHPz4);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp4 =  zeros(l,np4);
    Yz4 = zeros(l,nz4);
    for i=1:np4
        Yp4(:,i) = Cs(4,:)*V(:,i)/norm(Cs(4,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz4
        [U,S,V]=svd(evalfr(sys4,RHPz4(j))); Yz4(:,j)=U(:,end); %zero direction
    end
    SGmin = zeros(np4,nz4);
    for i=1:np4
        for j=1:nz4
            num4=abs((ctranspose(Yz4(:,j))*(evalfr(Gms4,RHPz4(j))))*((evalfr(Gms4,RHPp4(i)))^-1)*Yp4(:,i));
            den4=norm(ctranspose(Yz4(:,j))*(evalfr(Gms4,RHPz4(j))),2)*norm(((evalfr(Gms4,RHPp4(i)))^-1)*Yp4(:,i),2);
            cos2=(num4/den4)^2;
            sin2=1-cos2;
            
            term4 = (abs(RHPz4(j)+conj(RHPp4(i))))^2/(abs(RHPz4(j)-RHPp4(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz4(:,j))*(evalfr(Gms4,RHPz4(j)))),2)*sqrt(sin2+(term4*cos2));
        end
    end
    SGmin4 = max(max((SGmin)));
else
    SGmin4=0;
end

%% SG for P_bh
[p, z5]=pzmap(sys5);
RHPp5=p(find(p>0));
RHPz5=z5(find(z5>0));

if length(RHPp5)>0 && length(RHPz5)>0
    np5=length(RHPp5); nz5=length(RHPz5);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp5 =  zeros(l,np5);
    Yz5 = zeros(l,nz5);
    for i=1:np5
        Yp5(:,i) = Cs(5,:)*V(:,i)/norm(Cs(5,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz5
        [U,S,V]=svd(evalfr(sys5,RHPz5(j))); Yz5(:,j)=U(:,end); %zero direction
    end
    SGmin = zeros(np5,nz5);
    for i=1:np5
        for j=1:nz5
            num5=abs((ctranspose(Yz5(:,j))*(evalfr(Gms5,RHPz5(j))))*((evalfr(Gms5,RHPp5(i)))^-1)*Yp5(:,i));
            den5=norm(ctranspose(Yz5(:,j))*(evalfr(Gms5,RHPz5(j))),2)*norm(((evalfr(Gms5,RHPp5(i)))^-1)*Yp5(:,i),2);
            cos2=(num5/den5)^2;
            sin2=1-cos2;
            
            term5 = (abs(RHPz5(j)+conj(RHPp5(i))))^2/(abs(RHPz5(j)-RHPp5(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz5(:,j))*(evalfr(Gms5,RHPz5(j)))),2)*sqrt(sin2+(term5*cos2));
        end
    end
    SGmin5 = max(max((SGmin)));
else
    SGmin5=0;
end

%% SG for W
[p, z6]=pzmap(sys6);
RHPp6=p(find(p>0));
RHPz6=z6(find(z6>0));

if length(RHPp6)>0 && length(RHPz6)>0
    np6=length(RHPp6); nz6=length(RHPz6);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp6 =  zeros(l,np6);
    Yz6 = zeros(l,nz6);
    for i=1:np6
        Yp6(:,i) = Cs(6,:)*V(:,i)/norm(Cs(6,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz6
        [U,S,V]=svd(evalfr(sys6,RHPz6(j))); Yz6(:,j)=U(:,end); %zero direction
    end
    SGmin = zeros(np6,nz6);
    for i=1:np6
        for j=1:nz6
            num6=abs((ctranspose(Yz6(:,j))*(evalfr(Gms6,RHPz6(j))))*((evalfr(Gms6,RHPp6(i)))^-1)*Yp6(:,i));
            den6=norm(ctranspose(Yz6(:,j))*(evalfr(Gms6,RHPz6(j))),2)*norm(((evalfr(Gms6,RHPp6(i)))^-1)*Yp6(:,i),2);
            cos2=(num6/den6)^2;
            sin2=1-cos2;
            
            term6 = (abs(RHPz6(j)+conj(RHPp6(i))))^2/(abs(RHPz6(j)-RHPp6(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz6(:,j))*(evalfr(Gms6,RHPz6(j)))),2)*sqrt(sin2+(term6*cos2));
        end
    end
    SGmin6 = max(max((SGmin)));
else
    SGmin6=0;
end

%% SG for Rho
[p, z7]=pzmap(sys7);
RHPp7=p(find(p>0));
RHPz7=z7(find(z7>0));

if length(RHPp7)>0 && length(RHPz7)>0
    np7=length(RHPp7); nz7=length(RHPz7);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp7 =  zeros(l,np7);
    Yz7 = zeros(l,nz7);
    for i=1:np7
        Yp7(:,i) = Cs(7,:)*V(:,i)/norm(Cs(7,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz7
        [U,S,V]=svd(evalfr(sys7,RHPz7(j))); Yz7(:,j)=U(:,end); %zero direction
    end
    SGmin = zeros(np7,nz7);
    for i=1:np7
        for j=1:nz7
            num7=abs((ctranspose(Yz7(:,j))*(evalfr(Gms7,RHPz7(j))))*((evalfr(Gms7,RHPp7(i)))^-1)*Yp7(:,i));
            den7=norm(ctranspose(Yz7(:,j))*(evalfr(Gms7,RHPz7(j))),2)*norm(((evalfr(Gms7,RHPp7(i)))^-1)*Yp7(:,i),2);
            cos2=(num7/den7)^2;
            sin2=1-cos2;
            
            term7 = (abs(RHPz7(j)+conj(RHPp7(i))))^2/(abs(RHPz7(j)-RHPp7(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz7(:,j))*(evalfr(Gms7,RHPz7(j)))),2)*sqrt(sin2+(term7*cos2));
        end
    end
    SGmin7 = max(max((SGmin)));
else
    SGmin7=0;
end

%% SG for AlphaL
[p, z8]=pzmap(sys8);
RHPp8=p(find(p>0));
RHPz8=z8(find(z8>0));

if length(RHPp8)>0 && length(RHPz8)>0
    np8=length(RHPp8); nz8=length(RHPz8);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp8 =  zeros(l,np8);
    Yz8 = zeros(l,nz8);
    for i=1:np8
        Yp8(:,i) = Cs(8,:)*V(:,i)/norm(Cs(8,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz8
        [U,S,V]=svd(evalfr(sys8,RHPz8(j))); Yz8(:,j)=U(:,end); %zero direction
    end
    SGmin = zeros(np8,nz8);
    for i=1:np8
        for j=1:nz8
            num8=abs((ctranspose(Yz8(:,j))*(evalfr(Gms8,RHPz8(j))))*((evalfr(Gms8,RHPp8(i)))^-1)*Yp8(:,i));
            den8=norm(ctranspose(Yz8(:,j))*(evalfr(Gms8,RHPz8(j))),2)*norm(((evalfr(Gms8,RHPp8(i)))^-1)*Yp8(:,i),2);
            cos2=(num8/den8)^2;
            sin2=1-cos2;
            
            term8 = (abs(RHPz8(j)+conj(RHPp8(i))))^2/(abs(RHPz8(j)-RHPp8(i)))^2;
            SGmin(i,j)=norm((ctranspose(Yz8(:,j))*(evalfr(Gms8,RHPz8(j)))),2)*sqrt(sin2+(term8*cos2));
        end
    end
    SGmin8 = max(max((SGmin)));
else
    SGmin8=0;
end

%% ***** SGd ********
%% SGd1 for Wg_in
[p, z1]=pzmap(sys1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));
l = 1;
if (length(RHPp1)>0 && length(RHPz1)>0)
    np1=length(RHPp1); nz1=length(RHPz1);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp1 =  zeros(l,np1);
    Yz1 = zeros(l,nz1);
    for i=1:np1
        Yp1(:,i) = Cs(1,:)*V(:,i)/norm(Cs(1,:)*V(:,i)); %Pole direction
    end
    for j=1:nz1
        [U,S,V]=svd(evalfr(sys1,RHPz1(j))); Yz1(:,j)=U(:,end); %zero direction
    end
    
    SGdmin = zeros(np1,nz1);
    for i=1:np1
        for j=1:nz1
            num1=abs((ctranspose(Yz1(:,j))*(evalfr(Gdms11,RHPz1(j))))*((evalfr(Gdms11,RHPp1(i)))^-1)*Yp1(:,i));
            den1=norm(ctranspose(Yz1(:,j))*(evalfr(Gdms11,RHPz1(j))),2)*norm(((evalfr(Gdms11,RHPp1(i)))^-1)*Yp1(:,i),2);
            cos2=(num1/den1)^2;
            sin2=1-cos2;
            
            term1 = (abs(RHPz1(j)+conj(RHPp1(i))))^2/(abs(RHPz1(j)-RHPp1(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz1(:,j))*(evalfr(Gdms11,RHPz1(j)))),2)*sqrt(sin2+(term1*cos2));
        end
    end
    SGdmin11 = max(max((SGdmin)));
else
    SGdmin11=0;
end

%% SGd2 for Wg_in
[p, z1]=pzmap(sys1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));
l = 1;
if (length(RHPp1)>0 && length(RHPz1)>0)
    np1=length(RHPp1); nz1=length(RHPz1);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp1 =  zeros(l,np1);
    Yz1 = zeros(l,nz1);
    for i=1:np1
        Yp1(:,i) = Cs(1,:)*V(:,i)/norm(Cs(1,:)*V(:,i)); %Pole direction
    end
    for j=1:nz1
        [U,S,V]=svd(evalfr(sys1,RHPz1(j))); Yz1(:,j)=U(:,end); %zero direction
    end
    
    SGdmin = zeros(np1,nz1);
    for i=1:np1
        for j=1:nz1
            num1=abs((ctranspose(Yz1(:,j))*(evalfr(Gdms12,RHPz1(j))))*((evalfr(Gdms12,RHPp1(i)))^-1)*Yp1(:,i));
            den1=norm(ctranspose(Yz1(:,j))*(evalfr(Gdms12,RHPz1(j))),2)*norm(((evalfr(Gdms12,RHPp1(i)))^-1)*Yp1(:,i),2);
            cos2=(num1/den1)^2;
            sin2=1-cos2;
            
            term1 = (abs(RHPz1(j)+conj(RHPp1(i))))^2/(abs(RHPz1(j)-RHPp1(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz1(:,j))*(evalfr(Gdms12,RHPz1(j)))),2)*sqrt(sin2+(term1*cos2));
        end
    end
    SGdmin12 = max(max((SGdmin)));
else
    SGdmin12=0;
end
%% SGd1 for P_at
[p, z2]=pzmap(sys2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if length(RHPp2)>0 && length(RHPz2)>0
    np2=length(RHPp2); nz2=length(RHPz2);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp2 =  zeros(l,np2);
    Yz2 = zeros(l,nz2);
    for i=1:np2
        Yp2(:,i) = Cs(2,:)*V(:,i)/norm(Cs(2,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz2
        [U,S,V]=svd(evalfr(sys2,RHPz2(j))); Yz2(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np2,nz2);
    for i=1:np2
        for j=1:nz2
            num2=abs((ctranspose(Yz2(:,j))*(evalfr(Gdms21,RHPz2(j))))*((evalfr(Gdms21,RHPp2(i)))^-1)*Yp2(:,i));
            den2=norm(ctranspose(Yz2(:,j))*(evalfr(Gdms21,RHPz2(j))),2)*norm(((evalfr(Gdms21,RHPp2(i)))^-1)*Yp2(:,i),2);
            cos2=(num2/den2)^2;
            sin2=1-cos2;
            
            term2 = (abs(RHPz2(j)+conj(RHPp2(i))))^2/(abs(RHPz2(j)-RHPp2(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz2(:,j))*(evalfr(Gdms21,RHPz2(j)))),2)*sqrt(sin2+(term2*cos2));
        end
    end
    SGdmin21 = max(max((SGdmin)));
else
    SGdmin21=0;
end
%% SGd2 for P_at
[p, z2]=pzmap(sys2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if length(RHPp2)>0 && length(RHPz2)>0
    np2=length(RHPp2); nz2=length(RHPz2);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp2 =  zeros(l,np2);
    Yz2 = zeros(l,nz2);
    for i=1:np2
        Yp2(:,i) = Cs(2,:)*V(:,i)/norm(Cs(2,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz2
        [U,S,V]=svd(evalfr(sys2,RHPz2(j))); Yz2(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np2,nz2);
    for i=1:np2
        for j=1:nz2
            num2=abs((ctranspose(Yz2(:,j))*(evalfr(Gdms22,RHPz2(j))))*((evalfr(Gdms22,RHPp2(i)))^-1)*Yp2(:,i));
            den2=norm(ctranspose(Yz2(:,j))*(evalfr(Gdms22,RHPz2(j))),2)*norm(((evalfr(Gdms22,RHPp2(i)))^-1)*Yp2(:,i),2);
            cos2=(num2/den2)^2;
            sin2=1-cos2;
            
            term2 = (abs(RHPz2(j)+conj(RHPp2(i))))^2/(abs(RHPz2(j)-RHPp2(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz2(:,j))*(evalfr(Gdms22,RHPz2(j)))),2)*sqrt(sin2+(term2*cos2));
        end
    end
    SGdmin22 = max(max((SGdmin)));
else
    SGdmin22=0;
end
%% SGd1 for P_rt
[p, z3]=pzmap(sys3);
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if length(RHPp3)>0 && length(RHPz3)>0
    np3=length(RHPp3); nz3=length(RHPz3);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp3 =  zeros(l,np3);
    Yz3 = zeros(l,nz3);
    for i=1:np3
        Yp3(:,i) = Cs(3,:)*V(:,i)/norm(Cs(3,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz3
        [U,S,V]=svd(evalfr(sys3,RHPz3(j))); Yz3(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np3,nz3);
    for i=1:np3
        for j=1:nz3
            num3=abs((ctranspose(Yz3(:,j))*(evalfr(Gdms31,RHPz3(j))))*((evalfr(Gdms31,RHPp3(i)))^-1)*Yp3(:,i));
            den3=norm(ctranspose(Yz3(:,j))*(evalfr(Gdms31,RHPz3(j))),2)*norm(((evalfr(Gdms31,RHPp3(i)))^-1)*Yp3(:,i),2);
            cos2=(num3/den3)^2;
            sin2=1-cos2;
            
            term3 = (abs(RHPz3(j)+conj(RHPp3(i))))^2/(abs(RHPz3(j)-RHPp3(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz3(:,j))*(evalfr(Gdms31,RHPz3(j)))),2)*sqrt(sin2+(term3*cos2));
        end
    end
    SGdmin31 = max(max((SGdmin)));
else
    SGdmin31=0;
end
%% SGd2 for P_rt
[p, z3]=pzmap(sys3);
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if length(RHPp3)>0 && length(RHPz3)>0
    np3=length(RHPp3); nz3=length(RHPz3);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp3 =  zeros(l,np3);
    Yz3 = zeros(l,nz3);
    for i=1:np3
        Yp3(:,i) = Cs(3,:)*V(:,i)/norm(Cs(3,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz3
        [U,S,V]=svd(evalfr(sys3,RHPz3(j))); Yz3(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np3,nz3);
    for i=1:np3
        for j=1:nz3
            num3=abs((ctranspose(Yz3(:,j))*(evalfr(Gdms32,RHPz3(j))))*((evalfr(Gdms32,RHPp3(i)))^-1)*Yp3(:,i));
            den3=norm(ctranspose(Yz3(:,j))*(evalfr(Gdms32,RHPz3(j))),2)*norm(((evalfr(Gdms32,RHPp3(i)))^-1)*Yp3(:,i),2);
            cos2=(num3/den3)^2;
            sin2=1-cos2;
            
            term3 = (abs(RHPz3(j)+conj(RHPp3(i))))^2/(abs(RHPz3(j)-RHPp3(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz3(:,j))*(evalfr(Gdms32,RHPz3(j)))),2)*sqrt(sin2+(term3*cos2));
        end
    end
    SGdmin32 = max(max((SGdmin)));
else
    SGdmin32=0;
end
%% SGd1 for P_ab
[p, z4]=pzmap(sys4);
RHPp4=p(find(p>0));
RHPz4=z4(find(z4>0));

if length(RHPp4)>0 && length(RHPz4)>0
    np4=length(RHPp4); nz4=length(RHPz4);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp4 =  zeros(l,np4);
    Yz4 = zeros(l,nz4);
    for i=1:np4
        Yp4(:,i) = Cs(4,:)*V(:,i)/norm(Cs(4,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz4
        [U,S,V]=svd(evalfr(sys4,RHPz4(j))); Yz4(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np4,nz4);
    for i=1:np4
        for j=1:nz4
            num4=abs((ctranspose(Yz4(:,j))*(evalfr(Gdms41,RHPz4(j))))*((evalfr(Gdms41,RHPp4(i)))^-1)*Yp4(:,i));
            den4=norm(ctranspose(Yz4(:,j))*(evalfr(Gdms41,RHPz4(j))),2)*norm(((evalfr(Gdms41,RHPp4(i)))^-1)*Yp4(:,i),2);
            cos2=(num4/den4)^2;
            sin2=1-cos2;
            
            term4 = (abs(RHPz4(j)+conj(RHPp4(i))))^2/(abs(RHPz4(j)-RHPp4(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz4(:,j))*(evalfr(Gdms41,RHPz4(j)))),2)*sqrt(sin2+(term4*cos2));
        end
    end
    SGdmin41 = max(max((SGdmin)));
else
    SGdmin41 = 0;
end
%% SGd2 for P_ab
[p, z4]=pzmap(sys4);
RHPp4=p(find(p>0));
RHPz4=z4(find(z4>0));

if length(RHPp4)>0 && length(RHPz4)>0
    np4=length(RHPp4); nz4=length(RHPz4);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp4 =  zeros(l,np4);
    Yz4 = zeros(l,nz4);
    for i=1:np4
        Yp4(:,i) = Cs(4,:)*V(:,i)/norm(Cs(4,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz4
        [U,S,V]=svd(evalfr(sys4,RHPz4(j))); Yz4(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np4,nz4);
    for i=1:np4
        for j=1:nz4
            num4=abs((ctranspose(Yz4(:,j))*(evalfr(Gdms42,RHPz4(j))))*((evalfr(Gdms42,RHPp4(i)))^-1)*Yp4(:,i));
            den4=norm(ctranspose(Yz4(:,j))*(evalfr(Gdms42,RHPz4(j))),2)*norm(((evalfr(Gdms42,RHPp4(i)))^-1)*Yp4(:,i),2);
            cos2=(num4/den4)^2;
            sin2=1-cos2;
            
            term4 = (abs(RHPz4(j)+conj(RHPp4(i))))^2/(abs(RHPz4(j)-RHPp4(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz4(:,j))*(evalfr(Gdms42,RHPz4(j)))),2)*sqrt(sin2+(term4*cos2));
        end
    end
    SGdmin42 = max(max((SGdmin)));
else
    SGdmin42 = 0;
end
%% SGd1 for P_bh
[p, z5]=pzmap(sys5);
RHPp5=p(find(p>0));
RHPz5=z5(find(z5>0));

if length(RHPp5)>0 && length(RHPz5)>0
    np5=length(RHPp5); nz5=length(RHPz5);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp5 =  zeros(l,np5);
    Yz5 = zeros(l,nz5);
    for i=1:np5
        Yp5(:,i) = Cs(5,:)*V(:,i)/norm(Cs(5,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz5
        [U,S,V]=svd(evalfr(sys5,RHPz5(j))); Yz5(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np5,nz5);
    for i=1:np5
        for j=1:nz5
            num5=abs((ctranspose(Yz5(:,j))*(evalfr(Gdms51,RHPz5(j))))*((evalfr(Gdms51,RHPp5(i)))^-1)*Yp5(:,i));
            den5=norm(ctranspose(Yz5(:,j))*(evalfr(Gdms51,RHPz5(j))),2)*norm(((evalfr(Gdms51,RHPp5(i)))^-1)*Yp5(:,i),2);
            cos2=(num5/den5)^2;
            sin2=1-cos2;
            
            term5 = (abs(RHPz5(j)+conj(RHPp5(i))))^2/(abs(RHPz5(j)-RHPp5(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz5(:,j))*(evalfr(Gdms51,RHPz5(j)))),2)*sqrt(sin2+(term5*cos2));
        end
    end
    SGdmin51 = max(max((SGdmin)));
else
    SGdmin51=0;
end
%% SGd2 for P_bh
[p, z5]=pzmap(sys5);
RHPp5=p(find(p>0));
RHPz5=z5(find(z5>0));

if length(RHPp5)>0 && length(RHPz5)>0
    np5=length(RHPp5); nz5=length(RHPz5);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp5 =  zeros(l,np5);
    Yz5 = zeros(l,nz5);
    for i=1:np5
        Yp5(:,i) = Cs(5,:)*V(:,i)/norm(Cs(5,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz5
        [U,S,V]=svd(evalfr(sys5,RHPz5(j))); Yz5(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np5,nz5);
    for i=1:np5
        for j=1:nz5
            num5=abs((ctranspose(Yz5(:,j))*(evalfr(Gdms52,RHPz5(j))))*((evalfr(Gdms52,RHPp5(i)))^-1)*Yp5(:,i));
            den5=norm(ctranspose(Yz5(:,j))*(evalfr(Gdms52,RHPz5(j))),2)*norm(((evalfr(Gdms52,RHPp5(i)))^-1)*Yp5(:,i),2);
            cos2=(num5/den5)^2;
            sin2=1-cos2;
            
            term5 = (abs(RHPz5(j)+conj(RHPp5(i))))^2/(abs(RHPz5(j)-RHPp5(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz5(:,j))*(evalfr(Gdms52,RHPz5(j)))),2)*sqrt(sin2+(term5*cos2));
        end
    end
    SGdmin52 = max(max((SGdmin)));
else
    SGdmin52=0;
end
%% SGd1 for W
[p, z6]=pzmap(sys6);
RHPp6=p(find(p>0));
RHPz6=z6(find(z6>0));

if length(RHPp6)>0 && length(RHPz6)>0
    np6=length(RHPp6); nz6=length(RHPz6);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp6 =  zeros(l,np6);
    Yz6 = zeros(l,nz6);
    for i=1:np6
        Yp6(:,i) = Cs(6,:)*V(:,i)/norm(Cs(6,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz6
        [U,S,V]=svd(evalfr(sys6,RHPz6(j))); Yz6(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np6,nz6);
    for i=1:np6
        for j=1:nz6
            num6=abs((ctranspose(Yz6(:,j))*(evalfr(Gdms61,RHPz6(j))))*((evalfr(Gdms61,RHPp6(i)))^-1)*Yp6(:,i));
            den6=norm(ctranspose(Yz6(:,j))*(evalfr(Gdms61,RHPz6(j))),2)*norm(((evalfr(Gdms61,RHPp6(i)))^-1)*Yp6(:,i),2);
            cos2=(num6/den6)^2;
            sin2=1-cos2;
            
            term6 = (abs(RHPz6(j)+conj(RHPp6(i))))^2/(abs(RHPz6(j)-RHPp6(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz6(:,j))*(evalfr(Gdms61,RHPz6(j)))),2)*sqrt(sin2+(term6*cos2));
        end
    end
    SGdmin61 = max(max((SGdmin)));
else
    SGdmin61=0;
end
%% SGd2 for W
[p, z6]=pzmap(sys6);
RHPp6=p(find(p>0));
RHPz6=z6(find(z6>0));

if length(RHPp6)>0 && length(RHPz6)>0
    np6=length(RHPp6); nz6=length(RHPz6);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp6 =  zeros(l,np6);
    Yz6 = zeros(l,nz6);
    for i=1:np6
        Yp6(:,i) = Cs(6,:)*V(:,i)/norm(Cs(6,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz6
        [U,S,V]=svd(evalfr(sys6,RHPz6(j))); Yz6(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np6,nz6);
    for i=1:np6
        for j=1:nz6
            num6=abs((ctranspose(Yz6(:,j))*(evalfr(Gdms62,RHPz6(j))))*((evalfr(Gdms62,RHPp6(i)))^-1)*Yp6(:,i));
            den6=norm(ctranspose(Yz6(:,j))*(evalfr(Gdms62,RHPz6(j))),2)*norm(((evalfr(Gdms62,RHPp6(i)))^-1)*Yp6(:,i),2);
            cos2=(num6/den6)^2;
            sin2=1-cos2;
            
            term6 = (abs(RHPz6(j)+conj(RHPp6(i))))^2/(abs(RHPz6(j)-RHPp6(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz6(:,j))*(evalfr(Gdms62,RHPz6(j)))),2)*sqrt(sin2+(term6*cos2));
        end
    end
    SGdmin62 = max(max((SGdmin)));
else
    SGdmin62=0;
end

%% SGd1 for Rho
[p, z7]=pzmap(sys7);
RHPp7=p(find(p>0));
RHPz7=z7(find(z7>0));

if length(RHPp7)>0 && length(RHPz7)>0
    np7=length(RHPp7); nz7=length(RHPz7);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp7 =  zeros(l,np7);
    Yz7 = zeros(l,nz7);
    for i=1:np7
        Yp7(:,i) = Cs(7,:)*V(:,i)/norm(Cs(7,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz7
        [U,S,V]=svd(evalfr(sys7,RHPz7(j))); Yz7(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np7,nz7);
    for i=1:np7
        for j=1:nz7
            num7=abs((ctranspose(Yz7(:,j))*(evalfr(Gdms71,RHPz7(j))))*((evalfr(Gdms71,RHPp7(i)))^-1)*Yp7(:,i));
            den7=norm(ctranspose(Yz7(:,j))*(evalfr(Gdms71,RHPz7(j))),2)*norm(((evalfr(Gdms71,RHPp7(i)))^-1)*Yp7(:,i),2);
            cos2=(num7/den7)^2;
            sin2=1-cos2;
            
            term7 = (abs(RHPz7(j)+conj(RHPp7(i))))^2/(abs(RHPz7(j)-RHPp7(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz7(:,j))*(evalfr(Gdms71,RHPz7(j)))),2)*sqrt(sin2+(term7*cos2));
        end
    end
    SGdmin71 = max(max((SGdmin)));
else
    SGdmin71=0;
end

%% SGd2 for Rho
[p, z7]=pzmap(sys7);
RHPp7=p(find(p>0));
RHPz7=z7(find(z7>0));

if length(RHPp7)>0 && length(RHPz7)>0
    np7=length(RHPp7); nz7=length(RHPz7);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp7 =  zeros(l,np7);
    Yz7 = zeros(l,nz7);
    for i=1:np7
        Yp7(:,i) = Cs(7,:)*V(:,i)/norm(Cs(7,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz7
        [U,S,V]=svd(evalfr(sys7,RHPz7(j))); Yz7(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np7,nz7);
    for i=1:np7
        for j=1:nz7
            num7=abs((ctranspose(Yz7(:,j))*(evalfr(Gdms72,RHPz7(j))))*((evalfr(Gdms72,RHPp7(i)))^-1)*Yp7(:,i));
            den7=norm(ctranspose(Yz7(:,j))*(evalfr(Gdms72,RHPz7(j))),2)*norm(((evalfr(Gdms72,RHPp7(i)))^-1)*Yp7(:,i),2);
            cos2=(num7/den7)^2;
            sin2=1-cos2;
            
            term7 = (abs(RHPz7(j)+conj(RHPp7(i))))^2/(abs(RHPz7(j)-RHPp7(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz7(:,j))*(evalfr(Gdms72,RHPz7(j)))),2)*sqrt(sin2+(term7*cos2));
        end
    end
    SGdmin72 = max(max((SGdmin)));
else
    SGdmin72=0;
end

%% SGd1 for AlphaL
[p, z8]=pzmap(sys8);
RHPp8=p(find(p>0));
RHPz8=z8(find(z8>0));

if length(RHPp8)>0 && length(RHPz8)>0
    np8=length(RHPp8); nz8=length(RHPz8);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp8 =  zeros(l,np8);
    Yz8 = zeros(l,nz8);
    for i=1:np8
        Yp8(:,i) = Cs(8,:)*V(:,i)/norm(Cs(8,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz8
        [U,S,V]=svd(evalfr(sys8,RHPz8(j))); Yz8(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np8,nz8);
    for i=1:np8
        for j=1:nz8
            num8=abs((ctranspose(Yz8(:,j))*(evalfr(Gdms81,RHPz8(j))))*((evalfr(Gdms81,RHPp8(i)))^-1)*Yp8(:,i));
            den8=norm(ctranspose(Yz8(:,j))*(evalfr(Gdms81,RHPz8(j))),2)*norm(((evalfr(Gdms81,RHPp8(i)))^-1)*Yp8(:,i),2);
            cos2=(num8/den8)^2;
            sin2=1-cos2;
            
            term8 = (abs(RHPz8(j)+conj(RHPp8(i))))^2/(abs(RHPz8(j)-RHPp8(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz8(:,j))*(evalfr(Gdms81,RHPz8(j)))),2)*sqrt(sin2+(term8*cos2));
        end
    end
    SGdmin81 = max(max((SGdmin)));
else
    SGdmin81=0;
end

%% SGd2 for AlphaL
[p, z8]=pzmap(sys8);
RHPp8=p(find(p>0));
RHPz8=z8(find(z8>0));

if length(RHPp8)>0 && length(RHPz8)>0
    np8=length(RHPp8); nz8=length(RHPz8);
    [V,E]=eig(A); V=V(:,find(diag(E)>0));
    Yp8 =  zeros(l,np8);
    Yz8 = zeros(l,nz8);
    for i=1:np8
        Yp8(:,i) = Cs(8,:)*V(:,i)/norm(Cs(8,:)*V(:,i)); %Pole direction
    end
    
    for j=1:nz8
        [U,S,V]=svd(evalfr(sys8,RHPz8(j))); Yz8(:,j)=U(:,end); %zero direction
    end
    SGdmin = zeros(np8,nz8);
    for i=1:np8
        for j=1:nz8
            num8=abs((ctranspose(Yz8(:,j))*(evalfr(Gdms82,RHPz8(j))))*((evalfr(Gdms82,RHPp8(i)))^-1)*Yp8(:,i));
            den8=norm(ctranspose(Yz8(:,j))*(evalfr(Gdms82,RHPz8(j))),2)*norm(((evalfr(Gdms82,RHPp8(i)))^-1)*Yp8(:,i),2);
            cos2=(num8/den8)^2;
            sin2=1-cos2;
            
            term8 = (abs(RHPz8(j)+conj(RHPp8(i))))^2/(abs(RHPz8(j)-RHPp8(i)))^2;
            SGdmin(i,j)=norm((ctranspose(Yz8(:,j))*(evalfr(Gdms82,RHPz8(j)))),2)*sqrt(sin2+(term8*cos2));
        end
    end
    SGdmin82 = max(max((SGdmin)));
else
    SGdmin82=0;
end

%% *** Pole Vectors ****

i=1;
np1=length(RHPp1); nz1=length(RHPz1);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp1 = zeros(l,np1);
Yz1 = zeros(l,nz1);
for i=1:np1
    Yp1(:,i) = Cs(1,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end

np2=length(RHPp2); nz2=length(RHPz2);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp2 = zeros(l,np2);
Yz2 = zeros(l,nz2);
for i=1:np2
    Yp2(:,i) = Cs(2,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end


np3=length(RHPp3); nz3=length(RHPz3);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp3 = zeros(l,np3);
Yz3 = zeros(l,nz3);
for i=1:np3
    Yp3(:,i) = Cs(3,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end

np4=length(RHPp4); nz4=length(RHPz4);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp4 = zeros(l,np4);
Yz4 = zeros(l,nz4);
for i=1:np4
    Yp4(:,i) = Cs(4,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end

np5=length(RHPp5); nz5=length(RHPz5);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp5 = zeros(l,np5);
Yz5 = zeros(l,nz5);
for i=1:np5
    Yp5(:,i) = Cs(5,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end

np6=length(RHPp6); nz6=length(RHPz6);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp6 = zeros(l,np6);
Yz6 = zeros(l,nz6);
for i=1:np6
    Yp6(:,i) = Cs(6,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end

np7=length(RHPp7); nz7=length(RHPz7);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp7 = zeros(l,np7);
Yz7 = zeros(l,nz7);
for i=1:np7
    Yp7(:,i) = Cs(7,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end

np8=length(RHPp8); nz8=length(RHPz8);
[V,E]=eig(A); V=V(:,find(diag(E)>0));
Yp8 = zeros(l,np8);
Yz8 = zeros(l,nz8);
for i=1:np8
    Yp8(:,i) = Cs(8,:)*V(:,i)/norm(V(:,i)); %Pole vectors
end

%%
disp('Setady State gains');

disp(['Steady-state gain of G1 (P_at) is: ', num2str(evalfr(G1,0))]);
disp(['Steady-state gain of G2 (P_rt) is: ', num2str(evalfr(G2,0))]);
disp(['Steady-state gain of G3 (P_ab) is: ', num2str(evalfr(G3,0))]);
disp(['Steady-state gain of G4 (P_bh) is: ', num2str(evalfr(G4,0))]);
disp(['Steady-state gain of G5 (Q) is: ', num2str(evalfr(G5,0))]);
disp(['Steady-state gain of G6 (W) is: ', num2str(evalfr(G6,0))]);
disp(['Steady-state gain of G7 (Rho) is: ', num2str(evalfr(G7,0))]);
disp(['Steady-state gain of G8 (AlphaL) is: ', num2str(evalfr(G8,0))]);
disp('##########################################################');
disp('Pole vectors');
disp(['Pole vectors of (P_at) is: ', num2str(min(abs(Yp1)))]);
disp(['Pole vectors of (P_rt) is: ', num2str(min(abs(Yp2)))]);
disp(['Pole vectors of (P_ab) is: ', num2str(min(abs(Yp3)))]);
disp(['Pole vectors of (P_bh) is: ', num2str(min(abs(Yp4)))]);
disp(['Pole vectors of (Q) is: ', num2str(min(abs(Yp5)))]);
disp(['Pole vectors of (W) is: ', num2str(min(abs(Yp6)))]);
disp(['Pole vectors of (Rho) is: ', num2str(min(abs(Yp7)))]);
disp(['Pole vectors of (AlphaL) is: ', num2str(min(abs(Yp8)))]);
disp('##########################################################');
disp('Bounds on S and T');
disp(['The lowest achivable peak for Msmin1 (P_at) is: ', num2str(Msmin1)]);
disp(['The lowest achivable peak for Msmin2 (P_rt) is: ', num2str(Msmin2)]);
disp(['The lowest achivable peak for Msmin3 (P_ab) is: ', num2str(Msmin3)]);
disp(['The lowest achivable peak for Msmin4 (P_bh) is: ', num2str(Msmin4)]);
disp(['The lowest achivable peak for Msmin5 (Q) is: ', num2str(Msmin5)]);
disp(['The lowest achivable peak for Msmin6 (W) is: ', num2str(Msmin6)]);
disp(['The lowest achivable peak for Msmin7 (Rho) is: ', num2str(Msmin7)]);
disp(['The lowest achivable peak for Msmin8 (AlphaL) is: ', num2str(Msmin8)]);
disp('##########################################################');
disp('Bounds on KS');
disp(['The bound for KSmin1 (P_at) is: ', num2str(KSmin1)]);
disp(['The bound for KSmin2 (P_rt) is: ', num2str(KSmin2)]);
disp(['The bound for KSmin3 (P_ab) is: ', num2str(KSmin3)]);
disp(['The bound for KSmin4 (P_bh) is: ', num2str(KSmin4)]);
disp(['The bound for KSmin5 (Q) is: ', num2str(KSmin5)]);
disp(['The bound for KSmin6 (W) is: ', num2str(KSmin6)]);
disp(['The bound for KSmin7 (Rho) is: ', num2str(KSmin7)]);
disp(['The bound for KSmin8 (AlphaL) is: ', num2str(KSmin8)]);
disp('##########################################################');
disp('Bounds on KSGd1');
disp(['The bound for KSGd11 (P_at) is: ', num2str(KSGd11)]);
disp(['The bound for KSGd21 (P_rt) is: ', num2str(KSGd21)]);
disp(['The bound for KSGd31 (P_ab) is: ', num2str(KSGd31)]);
disp(['The bound for KSGd41 (P_bh) is: ', num2str(KSGd41)]);
disp(['The bound for KSGd51 (Q) is: ', num2str(KSGd51)]);
disp(['The bound for KSGd61 (W) is: ', num2str(KSGd61)]);
disp(['The bound for KSGd71 (Rho) is: ', num2str(KSGd71)]);
disp(['The bound for KSGd81 (AlphaL) is: ', num2str(KSGd81)]);
disp('Bounds on KSGd2');
disp(['The bound for KSGd12 (P_at) is: ', num2str(KSGd12)]);
disp(['The bound for KSGd22 (P_rt) is: ', num2str(KSGd22)]);
disp(['The bound for KSGd32 (P_ab) is: ', num2str(KSGd32)]);
disp(['The bound for KSGd42 (P_bh) is: ', num2str(KSGd42)]);
disp(['The bound for KSGd52 (Q) is: ', num2str(KSGd52)]);
disp(['The bound for KSGd61 (W) is: ', num2str(KSGd62)]);
disp(['The bound for KSGd71 (Rho) is: ', num2str(KSGd72)]);
disp(['The bound for KSGd82 (AlphaL) is: ', num2str(KSGd82)]);
disp('##########################################################');
disp('Bounds on SG');
disp(['The bound for SGmin1 (P_at) is: ', num2str(SGmin1)]);
disp(['The bound for SGmin2 (P_rt) is: ', num2str(SGmin2(1:1))]);
disp(['The bound for SGmin3 (P_ab) is: ', num2str(SGmin3)]);
disp(['The bound for SGmin4 (P_bh) is: ', num2str(SGmin4)]);
disp(['The bound for SGmin5 (Q) is: ', num2str(SGmin5)]);
disp(['The bound for SGmin6 (W) is: ', num2str(SGmin6)]);
disp(['The bound for SGmin7 (Rho) is: ', num2str(SGmin7)]);
disp(['The bound for SGmin8 (AlphaL) is: ', num2str(SGmin8)]);
disp('##########################################################');
disp('Bounds on SGd1');
disp(['The bound for SGd11 (P_at) is: ', num2str(SGdmin11)]);
disp(['The bound for SGd21 (P_rt) is: ', num2str(SGdmin21)]);
disp(['The bound for SGd31 (P_ab) is: ', num2str(SGdmin31)]);
disp(['The bound for SGd41 (P_bh) is: ', num2str(SGdmin41)]);
disp(['The bound for SGd51 (Q) is: ', num2str(SGdmin51)]);
disp(['The bound for SGd61 (W) is: ', num2str(SGdmin61)]);
disp(['The bound for SGd71 (Rho) is: ', num2str(SGdmin71)]);
disp(['The bound for SGd81 (AlphaL) is: ', num2str(SGdmin81)]);
disp('Bounds on SGd2');
disp(['The bound for SGd12 (P_at) is: ', num2str(SGdmin12)]);
disp(['The bound for SGd22 (P_rt) is: ', num2str(SGdmin22)]);
disp(['The bound for SGd32 (P_ab) is: ', num2str(SGdmin32)]);
disp(['The bound for SGd42 (P_bh) is: ', num2str(SGdmin42)]);
disp(['The bound for SGd52 (Q) is: ', num2str(SGdmin52)]);
disp(['The bound for SGd62 (W) is: ', num2str(SGdmin62)]);
disp(['The bound for SGd72 (Rho) is: ', num2str(SGdmin72)]);
disp(['The bound for SGd82 (AlphaL) is: ', num2str(SGdmin82)]);
%%
disp('-----------------------------------------------------------------------------------------------------------------')
disp('Measurement    Value   Dy   G(0)   Pole vector  |S|=|T|    |KS|     |SG|     |KSGd1|   |KSGd2|   |SGd1|    |SGd2|')
disp('-----------------------------------------------------------------------------------------------------------------')
disp(['W_gin [kg/s]' '  ' sprintf('%.2f',yss(1)) '  ' sprintf('%.1f',De(1,1)) '   ' sprintf('%.2f',G10) '     ' sprintf('%.4f',min(abs(Yp1))) '      ' sprintf('%.2f',Msmin1) '     '...
    sprintf('%.2f',KSmin1) '     ' sprintf('%.2f',SGmin1) '      ' sprintf('%.2f',KSGd11) '     ' sprintf('%.3f',KSGd12) '      ' sprintf('%.2f',SGdmin11) '     ' sprintf('%.2f',SGdmin12)]);
%---------------------------------------------------------------------------------------------------------------------------------------------------
disp(['P_at [bar]' '    ' sprintf('%.2f',yss(2)) '   ' sprintf('%.0f',De(2,2)) '    ' sprintf('%.2f',G20) '     ' sprintf('%.4f',min(abs(Yp2))) '      ' sprintf('%.2f',Msmin2) '      '...
    sprintf('%.2f',KSmin2) '      ' sprintf('%.2f',SGmin2) '      ' sprintf('%.2f',KSGd21) '      ' sprintf('%.3f',KSGd22) '      ' sprintf('%.2f',SGdmin21) '     ' sprintf('%.2f',SGdmin22)]);
%----------------------------------------------------------------------------------------------------------------------------------------------------
disp(['P_rt [bar]' '    ' sprintf('%.2f',yss(3)) '   ' sprintf('%.0f',De(3,3)) '    ' sprintf('%.2f',G30) '     ' sprintf('%.4f',min(abs(Yp3))) '      ' sprintf('%.2f',Msmin3) '      '...
    sprintf('%.2f',KSmin3) '     ' sprintf('%.2f',SGmin3) '      ' sprintf('%.2f',KSGd31) '      ' sprintf('%.3f',KSGd32) '      ' sprintf('%.2f',SGdmin31) '     ' sprintf('%.2f',SGdmin32)]);
%----------------------------------------------------------------------------------------------------------------------------------------------------
disp(['P_ab [bar]' '    ' sprintf('%.2f',yss(4)) '   ' sprintf('%.0f',De(4,4)) '    ' sprintf('%.2f',G40) '     ' sprintf('%.4f',min(abs(Yp4))) '      ' sprintf('%.2f',Msmin4) '      '...
    sprintf('%.2f',KSmin4) '      ' sprintf('%.2f',SGmin4) '      ' sprintf('%.2f',KSGd41) '      ' sprintf('%.3f',KSGd42) '      ' sprintf('%.2f',SGdmin41) '     ' sprintf('%.2f',SGdmin42)]);
%----------------------------------------------------------------------------------------------------------------------------------------------------
disp(['P_bh [bar]' '    ' sprintf('%.2f',yss(5)) '  ' sprintf('%.0f',De(5,5)) '    ' sprintf('%.2f',G50) '     ' sprintf('%.4f',min(abs(Yp5))) '      ' sprintf('%.2f',Msmin5) '      '...
    sprintf('%.2f',KSmin5) '     ' sprintf('%.2f',SGmin5) '      ' sprintf('%.2f',KSGd51) '      ' sprintf('%.3f',KSGd52) '      ' sprintf('%.2f',SGdmin51) '     ' sprintf('%.2f',SGdmin52)]);
%----------------------------------------------------------------------------------------------------------------------------------------------------
disp(['W_out [kg/s]' '  ' sprintf('%.2f',yss(6)) '   ' sprintf('%.0f',De(6,6)) '    ' sprintf('%.2f',G60) '     ' sprintf('%.4f',min(abs(Yp6))) '      ' sprintf('%.2f',Msmin6) '      '...
    sprintf('%.2f',KSmin6) '     ' sprintf('%.2f',SGmin6) '      ' sprintf('%.2f',KSGd61) '      ' sprintf('%.3f',KSGd62) '      ' sprintf('%.2f',SGdmin61) '     ' sprintf('%.2f',SGdmin62)]);
%----------------------------------------------------------------------------------------------------------------------------------------------------
disp(['Rho [kg/m^3]' ' ' sprintf('%.2f',yss(7)) '  ' sprintf('%.0f',De(7,7)) '    ' sprintf('%.2f',G70) '     ' sprintf('%.4f',min(abs(Yp7))) '      ' sprintf('%.2f',Msmin7) '      '...
    sprintf('%.2f',KSmin7) '     ' sprintf('%.2f',SGmin7) '      ' sprintf('%.2f',KSGd71) '      ' sprintf('%.3f',KSGd72) '      ' sprintf('%.2f',SGdmin71) '     ' sprintf('%.2f',SGdmin72)]);
%----------------------------------------------------------------------------------------------------------------------------------------------------
disp(['AlphaL [-]' '     ' sprintf('%.2f',yss(8)) '   ' sprintf('%.0f',De(8,8)) '    ' sprintf('%.2f',G80) '     ' sprintf('%.4f',min(abs(Yp8))) '      ' sprintf('%.2f',Msmin8) '      '...
    sprintf('%.2f',KSmin8) '      ' sprintf('%.2f',SGmin8) '      ' sprintf('%.2f',KSGd81) '      ' sprintf('%.3f',KSGd82) '      ' sprintf('%.2f',SGdmin81) '     ' sprintf('%.2f',SGdmin82)]);
disp('-----------------------------------------------------------------------------------------------------------------')