% Glover McFarlane Design for 
% AIRCRAFT MODEL
close all
s=tf('s');

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

G = ss(A,B,C,D);
wmin = 1e-2;
wmax = 1e4;

figure(1);
sigma(G,{wmin,wmax});
axis([wmin wmax 1e-6 1e4])
grid on; hold on
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)

figure(2);
Wp = 8*(1+5/s)*eye(3);
sigma(G*Wp,{wmin,wmax});
axis([wmin wmax 1e-6 1e4])
grid on; hold on
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)

figure(3)
G10 = evalfr(G*Wp,i*10);
[u,d,v]=svd(G10);
W1 = eye(3) + real(20*v(:,3)*v(:,3)');
G10 = evalfr(G*Wp*W1,i*10);
[u,d,v]=svd(G10);
W1 = W1*(eye(3) + real(2*v(:,3)*v(:,3)'));
Gd = G*Wp*W1;
sigma(Gd,{wmin,wmax})
axis([wmin wmax 1e-6 1e4])
grid on; hold on
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)


[K,CL,GAM,INFO]=loopsyn(G,Gd);

S = minreal(feedback(eye(3),G*K));
T = minreal(feedback(G*K,eye(3)));

figure(4)
subplot(221)
sigma(S,{wmin,wmax})
axis([wmin wmax 1e-3 10])
grid on; hold on


subplot(222)
sigma(minreal(feedback(G,K)),{wmin,wmax})
axis([wmin wmax 1e-3 10])
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)
grid on; hold on

subplot(223)
sigma(minreal(feedback(K,G)),{wmin,wmax})
axis([wmin wmax 1e-3 1e3])
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)
grid on; hold on

subplot(224)
sigma(T,{wmin,wmax})
axis([wmin wmax 1e-3 10])
grid on; hold on
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)

figure(5)
step(T,2);
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)

figure(6)
step(feedback(K,G),2)
hline = findobj(gcf, 'type', 'line');
set(hline,'Linewidth',2)

% figure(1)
% print -depsc fig/air1.eps
% figure(2)
% print -depsc fig/air2.eps
% figure(3)
% print -depsc fig/air3.eps
% figure(4)
% print -depsc fig/air4.eps
% figure(5)
% print -depsc fig/air5.eps
% figure(6)
% print -depsc fig/air6.eps
% 
% 











