%Bode plot for the steering example high bandwidth
%cancellation of process zero
% kja 060714
aminit;

%High bandwidth design without cancellation
a=0.5;
A=[0 1;0 0];
B=[a;1];
C=[1 0];
w=logspace(-1,3,500);
wc=10;zc=0.707;
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);
L1=Guy*Gyu;
T1=feedback(L1,1);
S1=feedback(1,L1);
[gaint1,phaset1]=bode(T1,w);
gaint1=gaint1(:);
Mt1=max(gaint1)
[gains1,phases1]=bode(S1,w);
gains1=gains1(:);
Ms1=max(gains1)
[nump1,denp1]=ss2tf(A,B,C,0);
[numc1,denc1]=ss2tf(A-B*K-L*C,L,K,0);
%
%Design with cancellation
wc=10;zc=(wc*a+1/(wc*a))/2
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];
Guy=ss(A-B*K-L*C,L,K,0);
L2=Guy*Gyu;
T2=feedback(L2,1);
S2=feedback(1,L2);
[gaint2,phaset2]=bode(T2,w);
gaint2=gaint2(:);
Mt2=max(gaint2)
[gains2,phases2]=bode(S2,w);
gains2=gains2(:);
Ms2=max(gains2)
[nump2,denp2]=ss2tf(A,B,C,0);
[numc2,denc2]=ss2tf(A-B*K-L*C,L,K,0);

%Plotting
subplot(321)
pl=loglog(w,gaint1,'-',w,gaint2,'--');
set(gca,'Ytick',[0.01 1]);
set(gca,'Xtick',[1 100]);
set(pl,'Linewidth', AM_bode_linewidth);
hold on
loglog([0.1 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth);
ylabel('t');
xlabel('w');
amaxis([0.1 1000 1e-3 20])

lgh = legend('Original', 'Improved', 'Location', 'southwest');
legend(lgh, 'boxoff');

subplot(322)
pl=loglog(w,gains1,'-',w,gains2,'--');
set(gca,'Ytick',[0.01 1]);
set(gca,'Xtick',[1 100]);
set(pl,'Linewidth', AM_bode_linewidth);
hold on
loglog([0.1 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth);
ylabel('s');
xlabel('w');
amaxis([0.1 1000 1e-3 20])

amprint('steering-senscomp.eps');
