oldboxes
addpath('/usr/local/regler/matlab/lqgbox','-begin');


A =[
   -2.82e-8        0         0         0         0         0;
         0   -0.0068         0         0         0         0;
         0         0   -0.1220    1.5700         0         0;
         0         0   -1.5700   -0.1220         0         0;
         0         0         0         0   -0.7100   21.3000;
         0         0         0         0  -21.3000   -0.7100 ];
B2 = [-4.06e-4; -2.20e-1;8.84e-2;-3.08e-1;6.39e-2;-1.08];
B1 =[-1.65e-7 0 0;7.61e-5 0 0;3.05e-2 0 0;-1.05e-2 0 0;4.46e-3 0 0;-1.02e-2 0 0];
C=[-3.5e-4 -2.05e-4; -1.57e-1 -1.54e-1;-1.56e-1 2.81e-1; 4.61e-3 -9.85e-2; -3.37e-1 -1.02; -2.4e-2 -7.68e-2]';
D21 = [0 1e-3 0 ; 0 0 1e-3];

figure(1)
[b1,a1]=ss2tf(A,B1(:,1),C(1,:),0);
fr1=frc(b1,a1,0,-2,1.4,500);
phiyf=fadd(fmul(fr1,fr1),frc(D21(1,2)^2,1,0,-2,1.4,500));
[b2,a2]=ss2tf(A,B1(:,1),C(2,:),0);
fr2=frc(b2,a2,0,-2,1.4,500);
phiya=fadd(fmul(fr2,fr2),frc(D21(2,3)^2,1,0,-2,1.4,500));
plot(phiyf(:,1),abs(phiyf(:,2)),phiya(:,1),abs(phiya(:,2)),'Linewidth',2)
xlabel('Frequency rad/s','Fontsize',16,'Fontweight','bold')
ylabel('Power spectral density','Fontsize',16,'Fontweight','bold')
title('Open loop','Fontsize',16,'Fontweight','bold')
axis([0 25 0 2e-3]);
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
print -depsc aircfig1.eps


%LQG 1
D12=sqrt(0.2);
[L,S]=lqrc(A,B2,C'*C,D12'*D12);
[K,P] = lqec(A,C,B1*B1',eye(2)*1e-6);

figure(2)
[Ac,By,Byr,Cc,Dy,Dyr] = lqgc(A,B2,C,zeros(2,1),L,1,K);
[s1,r]=ss2tf(Ac,By(:,1),Cc,Dy(:,1));
[s2,r]=ss2tf(Ac,By(:,2),Cc,Dy(:,2));
fr5=frc(s1,r,0,-2,1.4,500);
fr6=frc(s2,r,0,-2,1.4,500);
plot(fr5(:,1),abs(fr5(:,2)),fr6(:,1),abs(fr6(:,2)),'Linewidth',2)
xlabel('Frequency rad/s','Fontsize',16,'Fontweight','bold')
ylabel('Controller gain','Fontsize',16,'Fontweight','bold')
title('Design 1','Fontsize',16,'Fontweight','bold')
axis([0 25 0 10]);
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
print -depsc aircfig2.eps


figure(3);  %  calculation can be simplified
Acl = [A-B2*L B2*L ; 0*A A-K*C];
Bcl = [B1; B1-K*D21];
Ccl1 = [C(1,:) 0*C(1,:)];
Ccl2 = [C(2,:) 0*C(1,:)];
Ccl3 = [-D12*L D12*L];
Dcl = 0*Ccl1*Bcl;
[aa,bb,cc,dd]=series(-Acl,-Ccl1',Bcl',Dcl',Acl,Bcl,Ccl1,Dcl);
[bb1,aa1]=ss2tf(aa,bb,cc,dd);
fr7 = frc(bb1,aa1,0,-2,1.4,500);
[aa,bb,cc,dd]=series(-Acl,-Ccl2',Bcl',Dcl',Acl,Bcl,Ccl2,Dcl);
[bb2,aa2]=ss2tf(aa,bb,cc,dd);
fr8 = frc(bb2,aa2,0,-2,1.4,500);
plot(fr7(:,1),abs(fr7(:,2)),fr8(:,1),abs(fr8(:,2)),'Linewidth',2)
xlabel('Frequency rad/s','Fontsize',16,'Fontweight','bold')
ylabel('Power Spectral Density','Fontsize',16,'Fontweight','bold')
title('Design 1','Fontsize',16,'Fontweight','bold')
axis([0 25 0 2e-3]);
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
print -depsc aircfig3.eps

figure(4); %spectral density of u
[aa,bb,cc,dd]=series(-Acl,-Ccl3',Bcl',Dcl',Acl,Bcl,Ccl3,Dcl);
[bb3,aa3]=ss2tf(aa,bb,cc,dd);
fr9 = frc(bb3,aa3,0,-2,1.4,500);
plot(fr9(:,1),abs(fr9(:,2)),'Linewidth',2)
axis([0 25 0 5e-3]);
hold on

fr10=[];
for w=logspace(-2,2,5000);
  s=i*w;
  fr10=[fr10;w L*inv(s*eye(size(A))-A+B2*L+K*C)*K*C*inv(s*eye(size(A))-A)*B2];
end

%LQG 2
D12=sqrt(1);
Q1 = C'*C+diag([0,0,4,4,0,0]);
[L,S]=lqrc(A,B2,Q1,D12'*D12);

figure(5)
[Ac,By,Byr,Cc,Dy,Dyr] = lqgc(A,B2,C,zeros(2,1),L,1,K);
[s1,r]=ss2tf(Ac,By(:,1),Cc,Dy(:,1));
[s2,r]=ss2tf(Ac,By(:,2),Cc,Dy(:,2));
fr5=frc(s1,r,0,-2,1.4,500);
fr6=frc(s2,r,0,-2,1.4,500);
plot(fr5(:,1),abs(fr5(:,2)),fr6(:,1),abs(fr6(:,2)),'Linewidth',2)
axis([0 25 0 10]);
xlabel('Frequency rad/s','Fontsize',16,'Fontweight','bold')
ylabel('Controller Gain','Fontsize',16,'Fontweight','bold')
title('Design 2','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
print -depsc aircfig5.eps

figure(6);
Acl = [A-B2*L B2*L ; 0*A A-K*C];
Bcl = [B1; B1-K*D21];
Ccl1 = [C(1,:) 0*C(1,:)];
Ccl2 = [C(2,:) 0*C(1,:)];
Ccl3 = [-D12*L D12*L];
Dcl = 0*Ccl1*Bcl;
[aa,bb,cc,dd]=series(-Acl,-Ccl1',Bcl',Dcl',Acl,Bcl,Ccl1,Dcl);
[bb1,aa1]=ss2tf(aa,bb,cc,dd);
fr7 = frc(bb1,aa1,0,-2,1.4,500);
[aa,bb,cc,dd]=series(-Acl,-Ccl2',Bcl',Dcl',Acl,Bcl,Ccl2,Dcl);
[bb2,aa2]=ss2tf(aa,bb,cc,dd);
fr8 = frc(bb2,aa2,0,-2,1.4,500);
plot(fr7(:,1),abs(fr7(:,2)),fr8(:,1),abs(fr8(:,2)),'Linewidth',2)
axis([0 25 0 2e-3]);
xlabel('Frequency rad/s','Fontsize',16,'Fontweight','bold')
ylabel('Power Spectral Density','Fontsize',16,'Fontweight','bold')
title('Design 2','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
print -depsc aircfig6.eps


figure(4);
[aa,bb,cc,dd]=series(-Acl,-Ccl3',Bcl',Dcl',Acl,Bcl,Ccl3,Dcl);
[bb3,aa3]=ss2tf(aa,bb,cc,dd);
fr9 = frc(bb3,aa3,0,-2,1.4,500);
plot(fr9(:,1),abs(fr9(:,2)),'r','Linewidth',2)
xlabel('Frequency rad/s','Fontsize',16,'Fontweight','bold')
ylabel('Power Spectral Density u','Fontsize',16,'Fontweight','bold')
title('Design 1(blue) vs 2(red)','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
print -depsc aircfig4.eps

figure(8)
plot(fr10(:,2),'Linewidth',2)
title('Nyquist plot - Design 1','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
print -depsc aircfig8.eps

figure(9)
fr11=[];
for w=logspace(-2,2,5000);
  s=i*w;
  fr11=[fr11;w L*inv(s*eye(size(A))-A+B2*L+K*C)*K*C*inv(s*eye(size(A))-A)*B2];
end
plot(fr11(:,2),'Linewidth',2)
title('Nyquist plot - Design 2','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16)
set(gca,'Fontweight','bold')
hold on
print -depsc aircfig9.eps