% LQTLTR.M

% echo on

% This file includes the design example in section 5.8 in Maciejowski.
% It is written in Matlab4 at july 9th, 1992.

% Bandwidth specified to about 10 rad/s

clg
hold off



% AIRCRAFT MODEL

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';


% FIRST TRY

Gamma=B;
W=eye(3);
V=eye(3);

Kf1=lqe(A,Gamma,C,W,V);

w=logspace(-3,2);
figure(1);
sigma(A,Kf1,C,D,w);
title('Open-loop princ. gains with filter gain Kf1');
grid



% 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      Gamma*Cw;
    zeros(3,5)    Aw];

Ba=[   B;
    zeros(3)];

Ca=[C zeros(3)];

Da=zeros(3);

Gammaa=[Gamma*Dw;
           Bw    ];

Kf2=lqe(Aa,Gammaa,Ca,W,V);

 figure(2);
sigma(Aa,Kf2,Ca,Da,w);
grid
title('Open-loop princ. gains with filter gain Kf2');

% INCREASE SMALLEST PRINCIPAL GAIN AT w=0.001

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

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

Kf3=lqe(Aa,Gammaa,Ca,W3,V);

 figure(3);
sigma(Aa,Kf3,Ca,Da,w);
grid on
title('Open-loop princ. gains with filter gain Kf3');

% INCREASE LARGEST PRINCIPAL GAIN 

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

Kf4=lqe(Aa,Gammaa,Ca,100*W3,V);

 figure(4);
sigma(Aa,Kf4,Ca,Da,w);
grid on
title('Open-loop princ. gains with filter gain Kf4');

% 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)*Kf4));
end
for k=1:max(size(w))
  Tsv(:,k)=svd(eye(3)-inv(eye(3)+Ca*inv(w(k)*i*eye(size(Aa))-Aa)*Kf4));
end

figure(5);
semilogx(w,20*log10(Tsv),w,zeros(1,length(w)),'w:');
grid
hold on
semilogx(w,20*log10(Ssv),w,zeros(1,length(w)),'w:');
title('Sensitivity and complementary sensitivity with Kf4');
hold off


% 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)*Gammaa;
[u5,s5,v5]=svd(Gf5*sqrtm(100*W3));

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

% Principal gains of Gf*sqrt(W3) is given by Equation (5.70) as
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);

W5sqrt=10*W3sqrt*(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 the book:
% 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)'));

W5=W5sqrt*W5sqrt';

Kf5=lqe(Aa,Gammaa,Ca,W5,V);

w=logspace(0,2);
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)*Kf5));
end
for k=1:max(size(w))
  Tsv5(:,k)=svd(eye(3)-inv(eye(3)+Ca*inv(w(k)*i*eye(size(Aa))-Aa)*Kf5));
end

 figure(6)
semilogx(w,20*log10(Tsv5),w,zeros(1,length(w)),'w:');
grid
hold on
semilogx(w,20*log10(Ssv5),w,zeros(1,length(w)),'w:');
title('Sensitivity and complementary sensitivity with Kf5');
hold off

 figure(7)
wlog=logspace(-3,2);
sigma(Aa,Kf5,Ca,Da,wlog);
grid
title('Open-loop princ. gains with filter gain Kf5');

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


% RECOVERY AT THE PLANT OUTPUT
%------------------------------

KB=Kf5;
KD=zeros(3);

% To obtain LTR we solve Riccati and let rho -> 0.
Q=Ca'*Ca;
rho=logspace(-8,-2,4);
for k=4:-1:1
  clear KA KC Kc R 
  R=rho(k)*eye(3);
  Kc=lqr(Aa,Ba,Q,R);
  KA=Aa-Ba*Kc-Kf5*Ca;
  KC=-Kc;
  [GKA,GKB,GKC,GKD]=series(KA,KB,KC,KD,Aa,Ba,Ca,Da);
   figure(13-k);
   sigma(GKA,GKB,GKC,GKD,wlog);
  title(['rho = ',num2str(rho(k))]);
end
%k=1 <-> rho=1E-8

% %figure();hold on;
%%sigma(Aa,Kf5,Ca,Da,wlog);  % Desired open-loop princ. gains as above 

%rho=1E-6 and 1E-8 seem good


% CHARACTERISTIC LOCI AT THE PLANT OUTPUT

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 -GKC!
end;
figure(13);
plot(lambda2.');
hold on
xlabel('Re'); ylabel('Im');grid;
title('Characteristic loci at the plant output (rho = 1e-8)');
circle(-1,1);
hold off


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

wlog2=logspace(0,2);
Ssvf=zeros(3,max(size(wlog2)));
Tsvf=zeros(3,max(size(wlog2)));
for k=1:max(size(wlog2))
  Ssvf(:,k)=svd(inv(eye(3)-GKC*inv(wlog2(k)*i*eye(16)-GKA)*GKB));
end
for k=1:max(size(wlog2))
  Tsvf(:,k)=svd(eye(3)-inv(eye(3)-GKC*inv(wlog2(k)*i*eye(16)-GKA)*GKB));
end

 figure(14)
 semilogx(wlog2,20*log10(Tsvf),wlog2,zeros(1,length(wlog2)),'w:');
 grid
 hold on
 semilogx(wlog2,20*log10(Ssvf),wlog2,zeros(1,length(wlog2)),'w:');
 title('Final sensitivity and complementary sensitivity');
 hold off


% CLOSED-LOOP STEP RESPONSES

% Let the augmented plant include true integration

Aa2=[    A      Gamma*Cw;
    zeros(3,5)   zeros(3)];
KA2=Aa2-Ba*Kc-Kf5*Ca;

[As,Bs,Cs,Ds]=series(KA2,KB,KC,KD,A,B,C,D);
[AA,BB,CC,DD]=cloop(As,Bs,Cs,Ds,+1);        % Positive feedback!! (fig 5.2)

 figure(15);
 pzmap(AA,BB,CC,DD);
 title('Closed-loop system');

 figure(16);
 pzmap(A,B,C,D);
 title('Original plant');

figure(17);
d=[-ones(300,1) zeros(300,2)];  % Gives an output 1 step disturbance 
t=.01:.01:3;
lsim(AA,BB,CC,DD,d,t); 
title('Output 1 step disturbance')
grid

figure(18);
d=[zeros(300,1) -ones(300,1) zeros(300,1)];%Gives an output 2 step disturbance 
lsim(AA,BB,CC,DD,d,t); 
title('Output 2 step disturbance')
grid

figure(19);
d=[zeros(300,2) -ones(300,1) ];  % Gives an output 3 step disturbance 
lsim(AA,BB,CC,DD,d,t); 
title('Output 3 step disturbance')
grid

spread;

% echo off


