% Problem 8.7
disp('System model')
A=[-0.71  0     0     0      0
    0    -2     0     0      0
    0.61  1.28 -1.46  0.566  0
   -0.18 -0.37 0.56  -0.594 -0.23
    0     0    0    314.16   0]
B=[0.71 2 0 0 0]'
C=[0 0 1 0 1]
D=0

disp('(a.1) Open-loop transfer function:')
[n,d]=ss2tf(A,B,C,D,1);
for i=1:length(n),
    if n(i)<1.e-10
       n(i)=0;
    end;
end
for i=1:length(d),
    if d(i)<1.e-10
       d(i)=0;
    end;
end
printsys(n,d,'s')

disp('(a.2) Steady-state error:')
Kp=n(length(n))/d(length(d));
ess_step=1/(1+Kp)
if isinf(Kp) ==0
   Kv=0;
   Ka=0;
   ess_ramp=inf
   ess_parab=inf
else
   Kv=n(length(n))/d(length(d)-1);
   ess_ramp=1/Kv
end
if isinf(ess_ramp) == 0
   Ka=n(length(n))/d(length(d)-2);
   ess_parab=2/Ka
end

disp('Root locus of the system')
rlocus(n,d)
axis([-4 4 -11 11])
grid

% Lag compensator
nc=[1 0.1];
dc=[1 0.01];

% Compensated system - G(s)Gc(s)
num_c=conv(n,nc);
den_c=conv(d,dc);
len_n=length(num_c);
len_d=length(den_c);
K_c=num_c(len_n)/den_c(len_d);

while (K_c-10*Kp-9) < 0,
   dc(2)=0.9*dc(2);
   den_c=conv(d,dc);
   K_c=num_c(len_n)/den_c(len_d);
end

disp('(b.1) Lag Compensator:')
printsys(nc,dc,'s')

disp('(b.2) Steady state errors of the compensated system:')
ess_step_c=1/(1+K_c)
Kv_c=Kv*nc(2)/dc(2);
ess_ramp_c=1/Kv_c
Ka_c=Ka*nc(2)/dc(2);
ess_parab_c=2/Ka_c

disp('(b.3) Compensated system:')
printsys(num_c,den_c,'s')
figure(2)
rlocus(num_c,den_c)
axis([-4 4 -11 11])
grid

disp(' ')
disp('(c.1) Static gain and poles for uncomp.system at sd=-1.5+j1 (appr.)')
figure(1)
[K,p]=rlocfind(n,d)
unst_p=0;
for j=1:length(p),
    if real(p(j)) > 0
       unst_p=unst_p+1;
    end
end
if unst_p > 0
   disp('(d.1) System unstable at this operating point!')
else
   disp('(d.1) System stable at this operating point!')
end

disp(' ')
disp('(c.2) Static gain and poles for comp.system at sd=-1.5+j1 (appr.)')
figure(2)
[K_c,p_c]=rlocfind(num_c,den_c)
unst_pc=0;
for j=1:length(p_c),
    if real(p_c(j)) > 0
       unst_pc=unst_pc+1;
    end
end
