rho=0.1;ep1=0.0001;ep2=0.0001;sigma=0.01;
A=[0 1 0 0 0; 0 0 0 0 0 ; 0 0 -ep1 1 0;0 0 -1 -ep1 0 ; 1 0 0 0 -ep2];
B1=[0 0 0  ; 0 0 sigma ; 0 0 0 ; 0 1 0 ; 1 0 0];
B2=[0 ; 1 ; 0 ; 0 ; 0];
C1=[0 0 0 0 1; 0 0 0 0 0 ];
C2=[1 0 0 1 0];
D11=[0 0 0; 0 0 0 ];
D12=[0; rho];
D21=[1 0 0];
D22=0;

Q=C1'*C1;
R=D12'*D12;
N=C1'*D12;
[k,s,e] = lqr(A,B2,Q,R,N);
G=eye(length(A));
H=zeros(1,length(A));
syse = ss(A,[B2 G],C2,[D22 H])
R11 = B1*B1';
R22 = D21*D21';
R12 = B1*D21';
[kest,l,p]=kalman(syse,R11,R22,R12)

reg = zpk(-lqgreg(kest,k));
frv=logspace(-3,3,1000);
fr1 = squeeze(freqresp(reg,frv));
figure(1)
loglog(frv,abs(fr1),'r','Linewidth',2)
hold on
title('Controller Gain','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16,'Fontweight','bold')
print -depsc techcondres1.eps

sys=ss(A,B2,C2,0*C2*B2);
PC=sys*reg;
figure(2)
[re,im]=nyquist(PC);re=re(:);im=im(:);
plot(re(:),im(:),'Linewidth',2)
axis([-2 2 -2 2]);grid on;hold on
title('Nyquist Plot','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16,'Fontweight','bold')
print -depsc techcondres2.eps

sysye=ss(A,B1,C2,D21);
zeroes_ye=tzero(sysye)
