%Nyquist curve and sensitivity
%   kja070624
%   Defining the loop transfer function

aminit;

den=[1 1];den=conv(den,den);den=conv(den,den);
den=conv(den,[1 0]);
num=[0.78 0.39];
L=tf(num,den);

%   The Nyquist plot
w=0.1:0.005:2;
[re,im]=nyquist(L,w);re=re(:);im=im(:);

figure(1); clf; xsubplot(221);
ampolezero(tf([1], [1]), [-2.7 0.3 -1.8 1.2]); axis square
plot(re, im, 'Linewidth', AM_bode_linewidth);

dphi=pi/100;phi=-pi:dphi:pi;
x=cos(phi)-1; y=sin(phi);
uc=plot(x, y, 'k--', 'Linewidth', AM_bode_linewidth)

% x=-2.2:0.1:2;null=zeros(size(x));plot(x,null);plot(null,x);
re=re(1:100); im=im(1:100);
[v,ii]=min(abs(sqrt((re+1).^2+im.^2)-1));%ws
plot(re(ii),im(ii),'ro', 'MarkerSize', AM_data_markersize);
text(re(ii)+0.1,im(ii)-0.1,'ws')

[v,ii]=min(abs(sqrt(re+1).^2+im.^2));%wms
plot(re(ii), im(ii), 'ro', 'MarkerSize', AM_data_markersize);
text(re(ii)+0.1,im(ii),'wms');
text(re(ii)-0.4,0.85*im(ii),'ms');
% plot(-1, 0, 'ro', 'MarkerSize', AM_data_markersize);
text(-1.1,0.15,'-1')
s=0:0.01:1;
xx=s*(re(ii)+1);yy=s*im(ii);
plot(xx-1, yy, 'r-', 'LineWidth', AM_ref_linewidth);

% Add a "tick mark" at -1
plot(-1, 0, 'k+', 'LineWidth', AM_thin_linewidth);

amprint('sensitivity-nyquist.eps');

%Plotting L and S
figure(2); clf;
w=logspace(-1,1,200);
[re,im]=nyquist(L,w);re=re(:);im=im(:);

subplot(221);
[magh, phaseh] = ambode(L);

subplot(magh);
Lv=re+i*im;
S=1./(1+Lv);
loglog([0.1 10], [1 1], 'k', 'LineWidth', AM_ref_linewidth); hold on;
loglog(w, abs(Lv), 'b', 'LineWidth', AM_bode_linewidth);

% Mark the the crossover frequency
% [v,ii] = min(abs(sqrt(re.^2 + im.^2) - 1));%ws
% loglog(w(ii), 1, 'ro', 'MarkerSize', AM_data_markersize);

set(gca, 'XTick', [0.1 1 10], 'Ytick', [0.01 0.1 1 10]);
set(gca, 'XTickLabel', []);
amaxis([0.1 2.5 0.1 10]);
ylabel('L');

subplot(phaseh);
loglog([0.1 10], [1 1], 'k', 'LineWidth', AM_ref_linewidth); hold on;
loglog(w, abs(S), 'Linewidth', AM_data_linewidth);

% Mark the maximum sensitivity frequency
[v,ii]=max(abs(S));
plot(w(ii), abs(S(ii)), 'ro', 'MarkerSize', AM_data_markersize);

% Mark the the crossover frequency
[v,ii] = min((abs(S(1:ii)) - 1).^2); %wsc
loglog(w(ii), 1, 'ro', 'MarkerSize', AM_data_markersize);

set(gca, 'XTick', [0.1 1 10], 'XMinorGrid', 'off');
amaxis([0.1 2.5 0.1 10]);
xlabel('w');ylabel('S');

amprint('sensitivity-bode.eps');
