% Example 7.6
close all;clear all;
% This is a simple second-order plant with RHP-zero at z=2.
omega=logspace(-3,1,201);
G=nd2sys([-2,1],[50 15 1],3);
% PI control
Kc1=1.13; ti=12.7; 
K1=nd2sys([ti,1],[ti 1.e-4],Kc1);

% perturbed plant
Gp=nd2sys([-3,1],[16 8 1],4);

% frequency response of G
Gf=frsp(G,omega);
Gpf=frsp(Gp,omega);

% Relative difference
li = msub(1,mmult(minv(Gf),Gpf));

% Uncertainty weight
wi = nd2sys([10 0.33],[10/5.25 1]);
wif=frsp(wi,omega);
figure(1);
vplot('liv,lm',li,wif,'--',1,':'); %Frequency responses of LI and WI.

%Now evaluate complementary sensitivity.
L1=mmult(G,K1); S1=minv(madd(1,L1)); T1=msub(1,S1);
disp('Closed-loop poles of T1 (nominal plant, stable):');disp(spoles(T1));
T1f=frsp(T1,omega);
figure(2);
vplot('liv,lm',T1f,minv(wif),'--',1,':'); %It is not RS. 
disp('Check with actual perturbed plant:');
L1p=mmult(Gp,K1); S1p=minv(madd(1,L1p)); T1p=msub(1,S1p);
disp('Closed-loop poles of T1p (perturbation plant, unstable):');
disp(spoles(T1p));

%Need to reduce controller gain.
Kc2=0.3103;    ti=12.7; 
K2=nd2sys([ti,1],[ti 1.e-4],Kc2);
L2=mmult(G,K2); S2=minv(madd(1,L2)); T2=msub(1,S2);
T2f=frsp(T2,omega);
%Figure 7.12
figure(3);
vplot('liv,lm',T1f,T2f,minv(wif),'--',1,':'); 
axis([.001,10,.03,7]);
xlabel('Frequency'); ylabel('Magnitude');
text(.002,4,'1 / wI'); text(0.2,4,'T1 (NOT RS)');
text (0.2,.07,'T2 (RS)');



