% Problem 8.21

clear
disp('F-15 Aircraft model for supersonic conditions:')
A=[-0.01172 -95.91071  0       -32.11294
    -0.00011  -1.87942  1         0
     0.00056  -3.61627 -3.44478   0
     0         0        1         0]
B=[-25.40405 -0.22042 -53.4246 0]'
%C=[1 1 1 1]
C=eye(4)
%D=0
D=[0 0 0 0]'

% Transfer function of the system
disp('21(a) Transfer function of the system:')
[num,den]=ss2tf(A,B,C,D,1)
for i=1:4,
  for j=1:5,
    if abs(num(i,j)) < 1.e-8
       num(i,j)=0;
    end
  end
end

% Root locus of the system
disp('21(b) Root loci of the system')
rlocus(num,den)

% Closed-loop transfer functions with unit feedback
disp('21(c) Unit feedback closed-loop transfer functions:')
[nc1,dc1]=cloop(num(1,:),den,-1);
[nc2,dc2]=cloop(num(2,:),den,-1);
[nc3,dc3]=cloop(num(3,:),den,-1);
[nc4,dc4]=cloop(num(4,:),den,-1);

disp('G1(s)=y1(s)/U(s)')
printsys(nc1,dc1,'s')
[p1,z1]=pzmap(nc1,dc1)
disp('G2(s)=y2(s)/U(s)')
printsys(nc2,dc2,'s')
[p2,z2]=pzmap(nc2,dc2)
disp('G3(s)=y3(s)/U(s)')
printsys(nc3,dc3,'s')
[p3,z3]=pzmap(nc3,dc3)
disp('G4(s)=y4(s)/U(s)')
printsys(nc4,dc4,'s')
[p4,z4]=pzmap(nc4,dc4)

% Nominal point
sd=-1+1i;
xi=sqrt(2)/2; om_n=sqrt(2);

% (a) Compensator Gc1
th_p1=angle(sd-p1(1));
th_p2=angle(sd-p1(2));
th_p3=angle(sd-p1(3));
th_p4=angle(sd-p1(4));
th_z1=angle(sd-z1(1));
th_z2=angle(sd-z1(2));
th_z3=angle(sd-z1(3));
th_c=pi-th_z1-th_z2-th_z3+th_p1+th_p2+th_p3+th_p4
while th_c - pi > 0,
      th_c=th_c-pi;
end

zc1=0.1;
phi=angle(sd+zc1)
pc1=xi*om_n+om_n*sqrt(1-xi^2)*tan(th_c-phi+pi/2)

nc_lead=[1 zc1];
dc_lead=[1 pc1];
disp('Phase-lead compensator:')
printsys(nc_lead,dc_lead,'s')

n_c1=conv(nc_lead,nc1)
d_c1=conv(dc_lead,dc1)
figure(2)
subplot(221)
rlocus(n_c1,d_c1)

% (b) Compensator Gc2
th_p1=angle(sd-p2(1));
th_p2=angle(sd-p2(2));
th_p3=angle(sd-p2(3));
th_p4=angle(sd-p2(4));
th_z1=angle(sd-z2(1));
th_z2=angle(sd-z2(2));
th_z3=angle(sd-z2(3));
th_c=pi-th_z1-th_z2-th_z3+th_p1+th_p2+th_p3+th_p4
while th_c - pi > 0,
      th_c=th_c-pi;
end

zc1=0.2;
phi=angle(sd+zc1)
pc1=xi*om_n+om_n*sqrt(1-xi^2)*tan(th_c-phi+pi/2)

nc_lead=[1 zc1];
dc_lead=[1 pc1];
disp('Phase-lead compensator:')
printsys(nc_lead,dc_lead,'s')

n_c1=conv(nc_lead,nc1)
d_c1=conv(dc_lead,dc1)
figure(2)
subplot(222)
rlocus(n_c1,d_c1)
