%Illustration of stability margins in the Bode diagram
%   kja 070515

aminit;

% Transfer function
sys = tf([1], [1 0]) * tf([1], [1 1]) * tf([1], [1 2]);

% Numerical version for computing margins
g='1./(s.*(s+1).*(s+2))';
w=logspace(-1,1,100);s=i*w;
gv=eval(g);
gabs=abs(gv);
gphase=180*unwrap(angle(gv))/pi;
one=ones(size(w));
[amin,kg]=min(abs(gabs-1))
wgc=w(kg)
phasemarg=180+gphase(kg)
[pmin,kp]=min(abs(gphase+180))
w180=w(kp)
gainmarg=1/gabs(kp)

% Generate the bode plot
figure(1); clf; subplot(221);
[mh, ph] = ambode(sys, w);

% Gain curve
axes(mh); hold on;
loglog(w, one, 'k-', 'LineWidth', AM_ref_linewidth);
axis([0.1 10 0.1 10]);
set(gca, 'YTick', [0.1 1 10]);
ylabel('gain')

% Gain margin
plot([w180,w180],[gabs(kp),1], 'r-', 'LineWidth', AM_ref_linewidth);
plot(w180, gabs(kp), 'ro', 'MarkerSize', AM_bode_markersize);
text(w180+0.1,0.4,'gm')

axes(ph); hold on;
semilogx(w, -180*one, 'k-', 'LineWidth', AM_ref_linewidth);
set(gca,'Ytick',[-180 -150 -120 -90])

plot([wgc,wgc], [-180,gphase(kg)], 'r-', 'LineWidth', AM_ref_linewidth);
plot(wgc, gphase(kg), 'ro', 'MarkerSize', AM_bode_markersize);
text(wgc+0.025,-155,'pm')
axis([0.1 10 -200 -90])
ylabel('phase');xlabel('w')

amprint('bodemargins.eps');
