% Example in "Limits of Disturbance Rejection for Indirect Control" -
% by Kariwala and Skogestad (CDC-ECC 2005, Seville, Spain)

% Written by Vinay Kariwala
%            NTNU, Trondheim, Norway
%            March 2005 (modified July 2005)

s = tf('s');
Gd = 1/(s+1); Gdy = 1;

% G = (s-1)/(s+p1);
% Gy = 1/(s+p1)/(s+p2);

% Case 1: p1 = 2 and p2 = 3
% Limitations for direct control are the same

alpha = 1;
Interp1 = evalfr(Gd,alpha);
Hinf1_Ind = abs(Interp1)

Q1_1 = 1/(alpha+alpha');
H21_Ind = sqrt(Interp1*inv(Q1_1)*Interp1')

% ========================
% Case 2: p1 = - 2 and p2 = 3

gamma = 2;
Gs = (s-1)/(s+2); 
Gys1 = 1/(s+2)/(s+3);

Interp2 = evalfr(Gd,gamma) - evalfr(Gs,gamma)/evalfr(Gys1,gamma); 

F1_2 = diag([Interp1 Interp2]);
Q1_2 = [1/(alpha+alpha') 1/(alpha+gamma');1/(gamma+alpha') 1/(gamma+gamma')];

Hinf2_Ind = norm(inv(sqrtm(Q1_2))*F1_2*sqrtm(Q1_2))

H22_Ind = sqrt([Interp1 Interp2]*inv(Q1_2)*[Interp1 Interp2]')

% Direct Control : Case 2

Interp2d = 0;
Hinf2_d = norm(inv(sqrtm(Q1_2))*diag([Interp1 Interp2d])*sqrtm(Q1_2))
H22_d = sqrt([Interp1 Interp2d]*inv(Q1_2)*[Interp1 Interp2d]')


% ========================
% Case 3: p1 = - 2 and p2 = - 3
% Ofcourse, limitations for direct control are the same as Case 2

epsilon = 3;
Gys1_3 = 1/(s+2)/(s-3);     % Stable version of Gys considering only gamma
Gys = 1/(s-2)/(s+3);        % and considering only epsilon
G = (s-1)/(s-2);

% Since Gys1_3 is different from Gys1, Interpolation constraint 2 changes

Interp2_3 = evalfr(Gd,gamma) - evalfr(Gs,gamma)/evalfr(Gys1_3,gamma); 

Interp3 = evalfr(Gd,epsilon);

dGd = -1/(s+1)^2;

Interp4 = evalfr(dGd,epsilon) - 1/(2*epsilon)*evalfr(G,epsilon)/evalfr(Gys,epsilon); % Derivative Constraint

X = [1/(epsilon+epsilon') -1/(epsilon+epsilon')^2;-1/(epsilon+epsilon')^2 2/(epsilon+epsilon')^3];
Z1 = [1/(epsilon+alpha); -1/(epsilon+alpha)^2];
Z2 = [1/(epsilon+gamma); -1/(epsilon+gamma)^2];
Z = [Z1 Z2];

Y = [X Z; Z' Q1_2];

F1_3 = diag([Interp1 Interp2_3]);

R = [Interp3 0;Interp4 Interp3];
F = [R zeros(2,2);zeros(2,2) F1_3];

Hinf3_Ind = norm(inv(sqrtm(Y))*F*sqrtm(Y))

Q1_3 = [1/(alpha+alpha') 1/(alpha+gamma') 1/(alpha+epsilon');
        1/(gamma+alpha') 1/(gamma+gamma') 1/(gamma+epsilon');
        1/(epsilon+alpha') 1/(epsilon+gamma') 1/(epsilon+epsilon')];
    
H23_Ind = sqrt([Interp1 Interp2_3 Interp3]*inv(Q1_3)*[Interp1 Interp2_3 Interp3]')

% The resulting value for acheivable H2 norm is the same as Case 2
% indicating conservativeness (due to omission of Derivative constraint)


% ========================
% Case 4: Feedback + Feedforward control (p1 = - 2 and p2 = - 3)

% gamma does not poses limitation and derivative constraint is also absent

F1_4 = diag([Interp1 Interp3]);
Q1_4 = [1/(alpha+alpha') 1/(alpha+epsilon');1/(epsilon+alpha') 1/(epsilon+epsilon')];

Hinf_Ind_Twodeg = norm(inv(sqrtm(Q1_4))*F1_4*sqrtm(Q1_4))

H2_Ind_Twodeg = sqrt([Interp1 Interp3]*inv(Q1_4)*[Interp1 Interp3]')

% For direct 2 DOF control, only RHP zero of G poses limitation and thus
% Hinf and H2 optimal acheivable performances are same as Case 1 for all cases

