s=tf('s');
A=[0 1 0;0 0 1;0 0 -2];
B=[0;0;1];
C=[2 -1 0];
K=place(A,B,[-4,-13,-25])
L=place(A',C',[-50,-51,-52])'
[b,a]=ss2tf(A,B,C,0);
Gp=tf(b,a)
Ah = A-B*K-L*C;
[b,a]=ss2tf(Ah,L,-K,0);
Gc=-tf(b,a)
pole(Gr);
[b,a]=ss2tf(Ah,B,-K,1);
Gr=tf(b,a)
r=0.001
%Gp = minreal(s^2*Gp)/((s+r)*(s+r))
nyquistplot(Gp*Gc)
pole(Gp)
pole(Gc)
pole(1/(1+Gp*Gc))
Af = vertcat(horzcat(A-B*K,-B*K),horzcat(zeros(3,3),A-L*C));
Bf = vertcat(B,zeros(3,1));
Cf = horzcat(C,zeros(1,3));
[b,a] = ss2tf(Af,Bf,Cf,0);
R2Y = tf(b,a)
pole(R2Y)
zero(R2Y)
dcgain(R2Y);
[mag,phase,w]=bode(Gp*Gc,10^7);
margin(Gp*Gc)