%% Cancelación de perturbaciones deterministas por asignación de polos.
%% Seguimiento de referencia con 1 grado libertad.
% (c) 2018, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Presentación en vídeo en http://personales.upv.es/asala/YT/V/drconstrml.html
%
%% Modelado
% modelo del proceso (masa-muelle-amortiguador):

A=[0 1;-5 -1]; B=[0;2];C=[1 0];D=0;
sys=ss(A,B,C,D);
tf(sys)

% efecto de las perturbaciones sobre el estado:

F=[0;0]; E=-1;%efecto en la salida, se desea y-r=0.
%sólo hay un "C": objetivos de control y sensores idénticos.
%modelo con 2 entradas:
syspert=ss(A,[B F],C,[D E]);
%las entradas son u, ref; la salida es error Cx-ref.
step(syspert)

% Cancelación de perturbaciones si fuera conocida:

Qu=inv(dcgain(sys)) %la perturbación r se cancela con u=inv(ganancia)*r, 
%el estado final, iria a:
Qx=-inv(A)*B*Qu 

% Modelo ampliado:

Abig=[A F;zeros(1,2) 0]
Bbig=[B;0]
Cbig=[C  E]
sysbig=ss(Abig,Bbig,Cbig,0);
rank(ctrb(Abig,Bbig))
rank(obsv(Abig,Cbig))

%% Diseño del control por asignación de polos
% Ganancia controlador:

t_est_deseado=0.5;
polosdeseadosc=-4/t_est_deseado*[1 1.25]
K=place(A,B,polosdeseadosc)

% Ganancia ampliada, incorporando cancelación de perturbación estacionaria:

%K=[0 0];%bucle abierto
Kbig=[K -(Qu+K*Qx)]

%% Diseño del observador

polosdeseadosobs=-4/t_est_deseado*[1.5 1.75 2]
Lbig=place(Abig',Cbig',polosdeseadosobs)';

% formamos el controlador por realimentación de la salida:

controlador=reg(sysbig,Kbig,Lbig);
zpk(controlador)

% tiene un integrador, como se esperaba intuitivamente.
%% Simulación del resultado:

clp=feedback(sys*(-controlador),1);
zpk(clp)
eig(clp)'

% Ante un escalón en esa perturbación de entrada, la respuesta es:

    dcgain(clp)
   step(clp), grid on