%% LTR design on the AIRCRAFT MODEL in Maciejowski app A.1

rho=1e-6;  % LTR parameter
% rho=1E-5 or 1E-6 seems best (Maciejowski. thinks 1E-8)

A=[0         0    1.1320         0   -1.0000;
   0   -0.0538   -0.1712         0    0.0705;
   0         0         0    1.0000         0;
   0    0.0485         0   -0.8556   -1.0130;
   0   -0.2909         0    1.0532   -0.6859];

B=[      0         0         0;
   -0.1200    1.0000         0;
         0         0         0;
    4.4190         0   -1.6650;
    1.5750         0   -0.0732];

C=[1     0     0     0     0;
   0     1     0     0     0;
   0     0     1     0     0];

D=zeros(3);

states = 'altitude forw-speed pitch-angle pitch-rate vert-speed';
inputs = 'spoiler-angl forw-acc elevator-ang';
outputs= 'altitude forw-speed pitch-angle';
sys=ss(A,B,C,D);

% FIRST TRY

B1=B;
R1=eye(3);
R2=eye(3);

L1=(lqr(A',C',B1*B1',R2))';

wv=logspace(-3,2);
figure(1);
sigma(ss(A,L1,C,D),wv);
title('Singular values of C(sI-A)\^(-1)L, R1=BB^T,R2=1','Fontsize',16,'Fontweight','bold');
grid on
axis([1e-3 1e2 -40 80])
%print -depsc mac1.eps


% INTRODUCE INTEGRATORS

% To avoid trouble in the recovery step, we place poles at -0.001
Aw=-0.0001*eye(3);
Bw=eye(3);
Cw=eye(3);
Dw=zeros(3);

Aa=[    A      B1*Cw;
    zeros(3,5)    Aw];

Ba=[   B;
    zeros(3)];

Ca=[C zeros(3)];

Da=zeros(3);

B1a=[B1*Dw;
        Bw    ];

L2=(lqr(Aa',Ca',B1a*B1a',R2))';

figure(2);
sigma(ss(Aa,L2,Ca,Da),wv);
title('Extended with integrators','Fontsize',16,'Fontweight','bold');
grid on
axis([1e-3 1e2 -40 80])
%print -depsc mac2.eps

% INCREASE SMALLEST PRINCIPAL GAIN AT w=0.001

s=0.001*i;
Gf=Ca*inv(s*eye(size(Aa))-Aa)*B1a;
[u3,s3,v3]=svd(Gf);

v3real=real(v3(:,3));           % real approximation 
R1sqrt=eye(3)+9*v3real*v3real'; % alfa=9, cause we want to change the gain 
R1 = R1sqrt*R1sqrt';            % of sigma_smallest with a factor 10
R1 = B1a*R1*B1a';

L3=(lqr(Aa',Ca',R1,R2))';

figure(3);
sigma(ss(Aa,L3,Ca,Da),wv);
grid on
title('Changed R1 to increase lowest singular value','Fontsize', ...
      16,'Fontweight','bold');
axis([1e-3 1e2 -40 80])
%print -depsc mac3.eps

% INCREASE LARGEST PRINCIPAL GAIN 

% R1:=100R1 to give a 0 dB crossover at 10 rad/s

R1=100*R1;
L4=(lqr(Aa',Ca',R1,R2))';

figure(4);
sigma(ss(Aa,L4,Ca,Da),wv);
axis([1e-3 1e2 -40 80]);
grid on
title('R1=100*R1 to increase bandwidh to 10rad/s','Fontsize',16,'Fontweight','bold');
%print -depsc mac4.eps

% CONTROL OF THE BANDWIDTH SPECIFICATION
 
w=logspace(0,2);
Ssv=zeros(3,max(size(w)));
Tsv=zeros(3,max(size(w)));
for k=1:max(size(w))
  Ssv(:,k)=svd(inv(eye(3)+Ca*inv(w(k)*i*eye(size(Aa))-Aa)*L4));
end
for k=1:max(size(w))
  Tsv(:,k)=svd(eye(3)-inv(eye(3)+Ca*inv(w(k)*i*eye(size(Aa))-Aa)*L4));
end

figure(5);
semilogx(w,20*log10(Tsv),'Linewidth',2);
grid
hold on
semilogx(w,20*log10(Ssv),'Linewidth',2);
title('Sensitivity and complementary sensitivity with L4','Fontsize',16,'Fontweight','bold');
axis([1 1e2 -20 10])
set(gca,'Fontsize',16,'Fontweight','bold')
%print -depsc mac5.eps

% ONE CROSSOVER POINT FOR Sf AT -3 dB 

% Give svd(Sf) the same value at 5.225 rad/s (error in the book), i.e. -3 dB.

Gf5=Ca*inv(5.225*i*eye(size(Aa))-Aa)*B1a*R1sqrt;
[u5,s5,v5]=svd(Gf5);

% Principal gains of the return difference with the current design is
Ff=svd(eye(3)+Ca*inv(5.225*i*eye(size(Aa))-Aa)*L4); 
% We want these to be sqrt(2)*[1 1 1]'. 

% Principal gains of Gf is given by 
Gfsv=sqrt(Ff.^2-1);
% and Ff=sqrt(2)*[1 1 1]' is equivalent to Gf=[1 1 1]', so
alfa=[1 1 1]'./Gfsv-ones(3,1);

R1sqrt=10*R1sqrt*(eye(3)+alfa(1)*real(v5(:,1))*real(v5(:,1))')*...
      (eye(3)+alfa(2)*real(v5(:,2))*real(v5(:,2))')*...
      (eye(3)+alfa(3)*real(v5(:,3))*real(v5(:,3))');
% Notice the error in Maciejowski:
% W5sqrt=10*W3sqrt*(eye(3)+alfa(1)*real(v5(:,1)*v5(:,1)'))*...
      (eye(3)+alfa(2)*real(v5(:,2)*v5(:,2)'))*...
      (eye(3)+alfa(3)*real(v5(:,3)*v5(:,3)'));

R1=R1sqrt*R1sqrt';

L5=lqe(Aa,B1a,Ca,R1,R2);

w=logspace(0,2,500);
Ssv5=zeros(3,max(size(w)));
Tsv5=zeros(3,max(size(w)));
for k=1:max(size(w))
  Ssv5(:,k)=svd(inv(eye(3)+Ca*inv(w(k)*i*eye(size(Aa))-Aa)*L5));
end
for k=1:max(size(w))
  Tsv5(:,k)=svd(eye(3)-inv(eye(3)+Ca*inv(w(k)*i*eye(size(Aa))-Aa)*L5));
end

figure(6)
semilogx(w,20*log10(Tsv5),'Linewidth',2);
grid
hold on
semilogx(w,20*log10(Ssv5),'Linewidth',2);
title('Sensitivity and complementary sensitivity with L5','Fontsize',16,'Fontweight','bold');
axis([1 1e2 -20 10])
set(gca,'Fontsize',16,'Fontweight','bold')
%print -depsc mac6.eps

figure(7)
sigma(ss(Aa,L5,Ca,Da),wv);
grid
title('Open-loop princ. gains with filter gain L5','Fontsize',16,'Fontweight','bold');
axis([1e-3 1e2 -40 80])
%print -depsc mac7.eps


figure(8);
wlin=linspace(3,500,1000);
lambda=zeros(3,size(wlin));
for k=1:max(size(wlin))
  lambda(:,k)=eig(Ca*inv(wlin(k)*i*eye(8)-Aa)*L5);
end;
plot(lambda.');
xlabel('Re'); ylabel('Im');
title('Characteristic loci of the return ratio')
hold on
circle(-1,1);


% RECOVERY AT THE PLANT OUTPUT
%------------------------------
% compensator = ss(KA,KB,KC,KD)

figure(9);

KB=L5;
KD=zeros(3);
% To obtain LTR we solve Riccati and let rho -> 0.
Q1=Ca'*Ca;
  
KC=lqr(Aa,Ba,Q1,rho*eye(3));
KA=Aa-Ba*KC-L5*Ca;
[GKA,GKB,GKC,GKD]=series(KA,KB,KC,KD,Aa,Ba,Ca,Da);
sigma(ss(GKA,GKB,GKC,GKD),wv);
hold on;grid on;
sigma(ss(Aa,L5,Ca,Da),wv);
axis([1e-3 1e2 -40 80])
title(['rho = ',num2str(rho)],'Fontsize',16,'Fontweight','bold');

%print -depsc mac9.eps


% CHARACTERISTIC LOCI AT THE PLANT OUTPUT

figure(10);
clear lambda2
lambda2=zeros(3,size(wlin));
for k=1:max(size(wlin))
  lambda2(:,k)=eig(GKC*inv(wlin(k)*i*eye(16)-GKA)*GKB); % Notice 
end;
plot(lambda2.');
hold on
xlabel('Re'); ylabel('Im');grid;
title(['Characteristic loci at the plant output rho = ' num2str(rho)],'Fontsize',16,'Fontweight','bold');
circle(-1,1);


% S(s) AND T(s) FOR THE FINAL DESIGN (rho=1E-6)

figure(11)
Ssvf=zeros(3,max(size(w)));
Tsvf=zeros(3,max(size(w)));
for k=1:max(size(w))
  Ssvf(:,k)=svd(inv(eye(3)+GKC*inv(w(k)*i*eye(16)-GKA)*GKB));
end
for k=1:max(size(w))
  Tsvf(:,k)=svd(eye(3)-inv(eye(3)+GKC*inv(w(k)*i*eye(16)-GKA)*GKB));
end
semilogx(w,20*log10(Tsvf),'Linewidth',2);
grid
hold on
semilogx(w,20*log10(Ssvf),'Linewidth',2);
title(['Final S and T, rho=' num2str(rho)],'Fontsize',16,'Fontweight','bold');
grid on
axis([1 1e2 -20 10])
set(gca,'Fontsize',16,'Fontweight','bold')
%print -depsc mac11.eps

% CLOSED-LOOP SYSTEM

% Let the augmented plant include true integration

Aa2=[    A         B*Cw;
     zeros(3,5)   zeros(3)];
KA2=Aa2-Ba*KC-L5*Ca;
LQGreg = ss(KA2,KB,KC,KD);
PC = series(LQGreg,sys);
syscl = feedback(PC,ss([],[],[],eye(3)),-1);


%%%%% Closed loop step responses

figure(12);
t=0.01:0.01:3;
N=length(t);
r=[ones(N,1) zeros(N,2)];  % Gives an output 1 step disturbance 
[y,t]=lsim(syscl,r,t); 
plot(t,y(:,1),'b','Linewidth',2)
hold on; grid on
plot(t,y(:,2),'r','Linewidth',2)
plot(t,y(:,3),'g','Linewidth',2)

r=[zeros(N,1) ones(N,1) zeros(N,1)];%Gives an output 2 step disturbance 
[y,t]=lsim(syscl,r,t); 
plot(t,y(:,1),'b','Linewidth',2)
hold on; grid on
plot(t,y(:,2),'r','Linewidth',2)
plot(t,y(:,3),'g','Linewidth',2)

r=[zeros(N,2) ones(N,1) ];  % Gives an output 3 step disturbance 
[y,t]=lsim(syscl,r,t); 
plot(t,y(:,1),'b','Linewidth',2)
hold on; grid on
plot(t,y(:,2),'r','Linewidth',2)
plot(t,y(:,3),'g','Linewidth',2)

title('Closed-loop y1(blue), y2(red), y3(green)','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16,'Fontweight','bold')
%print -depsc mac12.eps


%%%%% Closed loop step responses

sys12 = feedback(LQGreg,sys);
sys21 = feedback(sys,LQGreg);

figure(13);
t=0.01:0.001:3;
N=length(t);
r=[ones(N,1) zeros(N,2)];  % Gives an output 1 step disturbance 
[y,t]=lsim(sys12,r,t); 
plot(t,y(:,1),'b','Linewidth',2)
hold on; grid on
plot(t,y(:,2),'r','Linewidth',2)
plot(t,y(:,3),'g','Linewidth',2)
title('Closed-loop u1(blue), u2(red), u3(green)','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16,'Fontweight','bold')
%print -depsc mac13.eps

figure(14);
r=[zeros(N,1) ones(N,1) zeros(N,1)];%Gives an output 2 step disturbance 
[y,t]=lsim(sys12,r,t); 
plot(t,y(:,1),'b','Linewidth',2)
hold on; grid on
plot(t,y(:,2),'r','Linewidth',2)
plot(t,y(:,3),'g','Linewidth',2)
title('Closed-loop u1(blue), u2(red), u3(green)','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16,'Fontweight','bold')
%print -depsc mac14.eps

figure(15);
r=[zeros(N,2) ones(N,1) ];  % Gives an output 3 step disturbance 
[y,t]=lsim(sys12,r,t); 
plot(t,y(:,1),'b','Linewidth',2)
hold on; grid on
plot(t,y(:,2),'r','Linewidth',2)
plot(t,y(:,3),'g','Linewidth',2)

title('Closed-loop u1(blue), u2(red), u3(green)','Fontsize',16,'Fontweight','bold')
set(gca,'Fontsize',16,'Fontweight','bold')
%print -depsc mac15.eps


figure(16)
wv2=logspace(-3,5,1000);
sigma(LQGreg,wv2)
title('Controller Gain','Fontsize',16,'Fontweight','bold')
hold on
%print -depsc mac16.eps

figure(17)
sigma(sys12,wv2)
axis([1e-3 1e5 -80 80])
title('(I+CP)\^(-1)C','Fontsize',16,'Fontweight','bold')
hold on
%print -depsc mac17.eps

figure(18)
sigma(sys21,wv2)
axis([1e-3 1e5 -100 0])
title('P(I+CP)\^(-1)','Fontsize',16,'Fontweight','bold')
hold on
%print -depsc mac18.eps


for fignr = 1:18
  str = ['print -depsc mac' num2str(fignr) '.eps'];
  figure(fignr)
  eval(str)
end
