% Doyle and Stein AC 79
% LQG/LTR design 
A=[-4 -3 ; 1 0];
B=[1;0];
Bv=[-61;35];
C=[1 2];
D=0;
sys=ss(A,B,C,D);
wv = logspace(-2,3,1000);
sysf = squeeze(freqresp(sys,wv));

Q1=80*[1 sqrt(35)]'*[1 sqrt(35)];
Q2=1;
[K,S,E]=lqr(A,B,Q1,Q2);

G=eye(size(A,1));
H=zeros(size(C,1),size(A,1));
syse=ss(A,[B G],C,[D H]);

q=0;
R1 = Bv*Bv'+q*B*B';
R2 = 1;
[kest,L,P] = kalman(syse,R1,R2);
reg = -lqgreg(kest,K);
PC=sys*reg;
PCf = squeeze(freqresp(PC,wv));
figure(1)
loglog(wv,abs(PCf),'b','Linewidth',2)
title('Controller Gain q=0(blue),1000(red),10000(green)','Fontsize',16,'Fontweight','bold')
hold on; grid on
axis([1e-2 1e3 1e-3 1e2])
figure(2)
[re,im]=nyquist(PC,wv);
re=re(:);im=im(:);
plot(re,im,'b','Linewidth',2)
hold on; grid on
axis([-3 3 -3 3])
REG0=zpk(reg)


q=1000;
R1 = Bv*Bv'+q*B*B';
R2 = 1;
[kest,L,P] = kalman(syse,R1,R2);
reg = -lqgreg(kest,K);
PC=sys*reg;
PCf = squeeze(freqresp(PC,wv));
figure(1)
loglog(wv,abs(PCf),'r','Linewidth',2)
title('Controller Gain q=0(blue),1000(red),10000(green)','Fontsize',16,'Fontweight','bold')
hold on; grid on
figure(2)
[re,im]=nyquist(PC,wv);
re=re(:);im=im(:);
plot(re,im,'r','Linewidth',2)
REG1000=zpk(reg)


q=10000;
R1 = Bv*Bv'+q*B*B';
R2 = 1;
[kest,L,P] = kalman(syse,R1,R2);
reg = -lqgreg(kest,K);
PC=sys*reg;
PCf = squeeze(freqresp(PC,wv));
figure(1)
loglog(wv,abs(PCf),'g','Linewidth',2)
title('Controller Gain q=0(blue),1000(red),10000(green)','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16,'Fontweight','bold')
hold on; grid on
figure(2)
[re,im]=nyquist(PC,wv);
re=re(:);im=im(:);
plot(re,im,'g','Linewidth',2)
REG10000=zpk(reg)
set(gca,'Fontsize',16,'Fontweight','bold')
figure(1)
grid off
print -depsc doylesteinfig1.ps
set(gca,'Fontsize',16,'Fontweight','bold')
figure(2)
title('Nyquist plot','Fontsize',16,'Fontweight','bold')
print -depsc doylesteinfig2.ps
