A=[-2 -2 0 0; 1 0 0 0;0 0 -2 -2; 0 0 1 0];
B=[1 0 0 0;0 0 1 0]';
C=[-0.01 -10 5 0; 0 20 0.01 20];
Plant=ss(A,B,C,zeros(2,2));

wB=200; % desired closed-loop bandwidth
A=1e-3; % desired disturbance attenuation inside bandwidth
M=1.05; % desired bound on hinfnorm(S) & hinfnorm(T)
n=1;    % the slope of the weight function
WP=zpk([-wB*M^(1/n)*ones(n,1)],[-wB*A^(1/n)*ones(n,1)],1/M);
WT=zpk([-wB/(M^(1/n))*ones(n,1)],[-wB/(A^(1/n))*ones(n,1)],1/A);

WPm=[WP 0;0 WP];
WTm=[WT 0;0 WT];
Unit=eye(2);

P11=[WPm; zeros(2,2); zeros(2,2)];
P12=[-WPm*Plant;WTm/100;WTm*Plant];
P21=Unit;
P22=-Plant;

P=[P11 P12;P21 P22];


[K,CL,GAM,INFO]=hinfsyn(P,2,2);

L = Plant*K;
T=feedback(L,Unit);
S=Unit-T;

figure(5)
bodemag(T,'b');
hold on;
bodemag(S,'r');
hold off;
title('Sensitivity Functions')

figure(6)
bodemag(K);
title('Controller Freq Response')

figure(7)
step(T,10/wB);
title('Close Loop Plant Step Responses')

figure(8)
step(K*S,10/wB);
title('Input signals')
