%  steering_nonrobust.m
%Bode and Nyquist plots for the steering example high bandwidth and no cancellation
%We compute it in two ways to check the calculations
%   kja 060714
aminit;

a=0.5;
A=[0 1;0 0];
B=[a;1];
C=[1 0];
wc=10;zc=0.707;%wc=7.2; zc=0.63;
wo=20;zo=0.707;
k1=wc^2;k2=2*zc*wc-a*wc^2;K=[k1 k2];
l1=2*zo*wo;l2=wo^2;L=[l1;l2];
Gyu=ss(A,B,C,0);
Guy=ss(A-B*K-L*C,L,K,0);
[numc,denc]=ss2tf(A-B*K-L*C,L,K,0);
Ltf=Guy*Gyu;
w=logspace(-1,3,500);s=i*w;
[real1,imag1]=nyquist(Ltf,w);
real1=real1(:);
imag1=imag1(:);
P=(a*s+1)./s.^2;
den=s.^2+s*(l1+a*k1+k2)+k1+l2+k2*l1-a*k2*l2;
num=s*(k1*l1+k2*l2)+k1*l2;
Cr=num./den;
PC=P.*Cr;
sm=min(abs(1+PC));

%bode plot
figure(1); clf; subplot(221)
[magh, phaseh] = ambode(Ltf, w)
% [gain,phase]=bode(Ltf,w);
% gain=gain(:);
% phase=phase(:);

subplot(magh); hold on;
loglog([0.1 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth);
ylabel('gain');
amaxis([0.1 1000 1e-3 1e3])

subplot(phaseh); hold on;
semilogx([0.1 1000], [-180 -180], 'k-', 'LineWidth', AM_ref_linewidth);
set(gca,'Ytick',[-360 -270 -180 -90])
ylabel('phase');
xlabel('w');
amaxis([0.1 1000 -360 -150])

amprint('steering-bode2.eps');

figure(2); clf;

%Sensitivity functions
S=1./(1+PC);
T=PC.*S;
subplot(221);
loglog(w,abs(S));
axis([0.1 1000 0.01 10]);
subplot(222);
loglog(w,abs(T));
axis([0.1 1000 0.01 10]);

%Nyquist plot for the steering example
%We compute it in two ways to check the calculations

w=logspace(-2,2,100);s=i*w;
[real1,imag1]=nyquist(Ltf);
real1=real1(:);
imag1=imag1(:);
P=(a*s+1)./s.^2;
den=s.^2+s*(l1+a*k1+k2)+k1+l2+k2*l1-a*k2*l2;
num=s*(k1*l1+k2*l2)+k1*l2;
Cr=num./den;
PC=P.*Cr;

%pl=plot(real1,imag1,'b',real(PC),imag(PC),'r--');

figure(3); clf; xsubplot(221);
ampolezero(tf([1], [1]), [-2 1.5 -1.5 2]); hold on;

pl=plot(real1,imag1,'b');
set(pl,'Linewidth', AM_data_linewidth);
axis([-2 1.5 -1.5 2]);
axis square;
hold on
dfi=pi/500;
fi=0:dfi:2*pi;
plot(cos(fi), sin(fi), 'k--', 'LineWidth', AM_ref_linewidth);
[xxx,iii]=min(abs(abs(PC)-1));
pm=180*(pi+angle(PC(iii)))/pi;

amprint('steering-nyq2.eps');
