clear all;
close all;

s = tf('s');

P0 = 1/((s - 1)*(s + 1)); % nominal plant
WT = 0.4*s/(s + 1); % weighting for complementary sensitivity function
WS = 0.1/(s+0.1); % weighting for sensitivity function

WS = ss(WS); % at least one sub-system is given in SSR to get G in SSR

systemnames = 'P0 WT WS';
inputvar = '[ w; u ]';
outputvar = '[ WT; WS; -w-P0 ]';
input_to_P0 = '[ u ]';
input_to_WT = '[ P0 ]';
input_to_WS = '[ P0 + w ]';
cleanupsysic = 'yes';
G1 = sysic;

gam_low = 1e-5;
gam_high = 10.0;
while gam_high - gam_low > 1e-4
  gam = (gam_low + gam_high) / 2.;
  [K, clp, gopt] = hinfsyn(mdiag(1,1/gam,1)*G1, 1, 1, 'GMIN', 1., 'GMAX', 1, 'DISPLAY', 'off');
  fprintf('gam = %f ... ', gam);
  if isempty(K)
    fprintf('infeasible\n');
    gam_low = gam;
  else
    fprintf('feasible\n');
    gam_high = gam;
    K_opt = K;
    gam_opt = gam;
  end
end

figure(1);
S = 1/(1 + P0 * K_opt); % gain plot of sensitivity function
bodemag(S, 'b', gam_opt/WS, 'g');
legend('S', 'gamma/WS');

figure(2);
T = 1 - S; % gain plot of complementary sensitivity function
bodemag(T, 'b', 1/WT, 'g');
legend('T', '1/WT');
