%
% Mixed Sensitivity
%
% SEM 5928 - Sistemas de Controle 
% Escola de Engenharia de São Carlos - USP
%
% Adriano Siqueira - 2016
%

clear all
close all
G = tf(200,conv([10 1],conv([0.05 1],[0.05 1])));
Gd = tf(100,[10 1]);

M =1.5;
wb = 10;
A=1.e-4;
Wp = tf([1/M wb],[1 wb*A]);

Wu = 1;

[khinf,ghinf,gopt]=mixsyn(G,Wp,Wu,[]);
Marg = allmargin(G*khinf)

[A,B,C,D]=ssdata(khinf);
[num,den]=ss2tf(A,B,C,D);
C = tf(num,den);

T = feedback(C*G,1);

S = 1/(1 +C*G);

figure(1)
sigma(S);hold on;sigma(1/Wp)
axis([1e-2 100 -70 10])

figure(2)
step(T,3)
hold on

figure(3)
step(S*Gd,3)
hold on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

M =1.5;
wb = 10;
A=1.e-4;
Wp2 = tf(conv([1/sqrt(M) wb],[1/sqrt(M) wb]),conv([1 wb*sqrt(A)],[1 wb*sqrt(A)]));
[khinf2,ghinf2,gopt2]=mixsyn(G,Wp2,Wu,[]);
Marg2 = allmargin(G*khinf2)

[A,B,C,D]=ssdata(khinf2);
[num,den]=ss2tf(A,B,C,D);
C2 = tf(num,den);

T2 = feedback(C2*G,1);

S2 = 1/(1 +C2*G);

figure(1)
sigma(S2);
sigma(1/Wp2)
axis([1e-2 100 -70 10])
legend('S','1/Wp','S_2','1/Wp_2',2)

figure(2)
step(T2,3)
legend('step(T)','step(T_2)')

figure(3)
step(S2*Gd,3)
legend('step(SG_d)','step(S_2G_d)')

[SV,W] = sigma(Wp*S)
figure(4)
semilogx(W,SV)
hold on
[SV,W] = sigma(Wp2*S2)
semilogx(W,SV,'g')
legend('WpS','Wp_2S_2')