% Pole Placement
% By Jorge Duque
% Control2 CUTB




%
% planta:
% 	         10K
% G(s)=      ------------
%	     S(S+10)(s+5)
%
num=[10];
den=conv([1 0],conv([1 10],[1 5]));
disp('El sistema es:')
printsys(num,den,'s')
%  Las especificaciones para la respuesta transitoria de 
%  éste sistema se dan originalmente en términos del
%  sobrepico, tiempo de estabilización y error en estado 
%  estacionario para una entrada rampa:  
%
%  OS   = 5 %  (Sobrepico)
%  essr = 0.01 (error en estado estacionario para entrada
%               rampa )
%  ts   = 2 seg ( tiempo de estabilización 

disp('Especificaciones de desempeño:') 

ts=2
Kv=100
OS=5,disp('%')
pause
disp('Esta especificaciones en términos de zita y Wn:')
a=(log(OS/100))/pi;
zita=sqrt(a/(a-1))
wn=4.6/(zita*ts), disp('rad/seg')
wd=wn*sqrt(1-zita^2), disp('rad/seg')
disp('Ahora la representación en espacio de estado')
[A,B,C,D]=tf2ss(num,den)
pause


disp(' Obtenemos la matriz de controlabilidad')

Co=ctrb(A,B)
pause
disp('Probamos si es no singular')

det(Co)
disp('det(Co) debe ser diferente de cero')

disp(' Observamos que si es controlable, debido a que det(Co) =1')
pause


disp(' Dibujando polos y ceros para A,B,C,D')

figure(1)
pzmap(A,B,C,D)
hold on
x=[-wn:.1:-zita*wn];
y=-(sqrt(1-zita.^2)/zita)*x;
xc=[-wn:.1:-zita*wn];
c=sqrt(wn.^2-xc.^2);
plot(x,y,':',x,-y,':',xc,c,':',xc,-c,':')
hold off
pause

disp(' Calculo del polinomio característico deseado')
disp(' (s^2 + 2*zita*wn s + wn^2 )(s+ wn)')
Pc=conv([1 2*zita*wn wn.^2],[1 wn])
pause 
disp('Aplicamos la formula de Ackerman para hallar la matriz K de ')
disp(' realimentacion de estado')

disp(' El inverso de la matriz de controlabilidad')

inv(Co)

disp(' El polinomio caracteristico evaluado en A')

polyvalm(Pc,A)
pause
disp(' EL valor de la matriz de realimentacion sera')
disp('K=[0 0 ...0 1]*inv(Co)*q(A)')
disp('donde K= matriz de ganancia realimentación del estado K')
disp('inv(Co) es la inversa de la matriz de controlabilidad')
disp('q(A) es el polinomio característico evaluado en K')

K=[0 0 1]*inv(Co)*polyvalm(Pc,A)

pause

disp(' Calculamos el valor de Nx y Nu')
N=inv([A B;C 0])*[0;0;0;1]
Nx=N(1:3)
Nu=N(4)
disp('No se necesita Feedforward porque se tiene un integrador')
disp('Nu es igual a cero')
pause
disp('El nuevo sistema con controlador sera')

Ac=A-B*K
Bc=B*K*Nx
Cc=C
Dc=D
pause
figure(2)

disp(' Dibujando la ubicación de los polos de Ac,Bc,Cc,Dc')
pzmap(Ac,Bc,Cc,Dc);
hold on
x=[-wn:.1:-zita*wn];
y=-(sqrt(1-zita.^2)/zita)*x;
xc=[-wn:.1:-zita*wn];
c=sqrt(wn.^2-xc.^2);
plot(x,y,':',x,-y,':',xc,c,':',xc,-c,':')
hold off
pause
figure(3)
disp(' Observamos la respuesta paso')
step(Ac,Bc,Cc,Dc)