%Example 5.1
% Effect of rhp-zeros. Try different sign of gain at high frequency
%Plant (5.40) is -s+1/ s+1. 
%Try controller c = Kc * (s+1)/s * 1/(0.05s+1) with different gains.

omega=logspace(-2,2,41);
g = nd2sys([-1 1],[1 0]);
f = nd2sys(1,[0.05,1]);
Kc=0.2;
gk=mmult(g,f,Kc);
s = minv(madd(1,gk));
t = msub(1,s);
r1=frsp(s,omega);
y1=trsp(t,1,5);

Kc=0.5;
gk=mmult(g,f,Kc);
s = minv(madd(1,gk));
t = msub(1,s);
r2=frsp(s,omega);
y2 = trsp(t,1,5);

Kc=0.8;
gk=mmult(g,f,Kc);
s = minv(madd(1,gk));
t = msub(1,s);
r3=frsp(s,omega);
y3 = trsp(t,1,5);

figure(1)     %Figure 5.8
subplot(221),vplot('liv,lm',r1,r2,r3,1,'w:');axis([0.01 100 0.01 10]);
text(0.04,0.03,'Kc=0.8'); text(0.1,2,'Kc=0.5'); text(.015,0.5,'Kc=0.2');
xlabel('Frequency');ylabel('|S|');
title('SENSITIVITY FUNCTION');
subplot(222),vplot(y1,y2,y3,1,'w:',0,'w:')
axis([0 5 -2 2]);
text(0.25,1.7,'Kc=0.8'); text(0.8,0.7,'Kc=0.5'); text(2,0.55,'Kc=0.2');
text(3.5,1.3,'Setpoint');
title('STEP RESPONSE');
xlabel('Time');ylabel('y');

% Effect of rhp-zeros. Try different sign of gain at high frequency
%Plant is -s+1/ s+1. Try controller c = K s with different gains

omega=logspace(-2,2,41);
g = nd2sys([-1 1],[1 1]);
%Controller
f = nd2sys([1 0], conv([0.02 1],[0.05 1]));

% Transient step change
r = [1 0; 1 0.10; 0 0.10; 0 0.2; 4 Inf];
time=0.2;

K=-0.1;
S = minv(madd(1,mmult(g,K,f)));
r1=frsp(S,omega);
y1 = trsp(msub(1,S),r,time,time/200);

K=-0.5;
S = minv(madd(1,mmult(g,K,f)));
r2=frsp(S,omega);
y2 = trsp(msub(1,S),r,time,time/200);

K=-0.9;
S = minv(madd(1,mmult(g,K,f)));
r3=frsp(S,omega);
y3 = trsp(msub(1,S),r,time,time/200);

figure(2),           %Figure 5.9
subplot(221),vplot('liv,lm',r1,r2,r3,1,'w:')
axis([.01,100,.01,10]);
text(1.5,5,'Kc=0.9'); text(2,2,'Kc=0.5'); text(15,.3,'Kc=0.1');
xlabel('Frequency');ylabel('|S|');
title('SENSIVITY FUNCTION');
% Phase:subplot(212),vplot('liv,p',x_f)
subplot(222),vplot(y1,y2,y3,r,'w:')
axis([0 0.14 -0.3 1.2]);
text(0.06,0.95,'Kc=0.9'); text(.06,0.65,'Kc=0.5'); text(.06,.3,'Kc=0.1');
text(0.06,1.1,'Setpoint');
title('STEP RESPONSE');
xlabel('Time');ylabel('y');

