%% Cancelación de perturbaciones deterministas por asignación de polos
% (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/drgenml.html
%
%% Modelado
% modelo del proceso (masa-muelle-amortiguador):

A=[0 1;-5 -1]; B=[0;2];C=[1 0];D=0;

% efecto de las perturbaciones sobre el estado:

F=[0.2;1]; E=0;%efecto colineal con la entrada.
sys_conpertentrada=ss(A,[B F],C,[D E]);
tf(sys_conpertentrada)

%% Resolución de ecuaciones estacionarias
% Se harán casos constante (acción integral) y un caso de cancelación de vibración senoidal.

ejemplo=2;%para seleccionar 1 cte, 2 senoidal.

%caso constante
if(ejemplo==1)
syms qx1 qx2 qu1 real
M=0;N=1; %modelo del GENERADOR, de orden 1.
Qx=[qx1;qx2];Qu=[qu1];
else
%caso senoidal ejemplo=2
syms qx11 qx12 qx21 qx22 qu1 qu2 real
omega=3;
M=[0 -omega;omega 0];N=[1 0]; %senoidal, modelo del generador orden 2.
Qx=[qx11 qx12;qx21 qx22];
Qu=[qu1 qu2];
end

%ecuaciones de la cancelación
ec1= (Qx*M-A*Qx-B*Qu==F*N);
ec2= (C*Qx+D*Qu==-E*N);
ecs=[ec1(:);ec2(:)]
sol=solve(ecs);
if(ejemplo==1)
    Qu_num=eval([sol.qu1])
    Qx_num=eval([sol.qx1;sol.qx2])
else
    Qu_num=eval([sol.qu1 sol.qu2])
Qx_num=eval([sol.qx11 sol.qx12;sol.qx21 sol.qx22])
end

%
%% Diseño del control por asignación de polos
% Modelo ampliado:

numestadosgenerador=size(M,1);
Abig=[A F*N;zeros(numestadosgenerador,2) M];
Bbig=[B;zeros(numestadosgenerador,1)];
Cbig=[C  E*N];

% Ganancia observador:

if(ejemplo==1)
    polosobs=[-8.5 -9 -9.5];
else
    polosobs=[-8 -8.5 -9 -9.5];
end
Lbig=place(Abig',Cbig',polosobs)'

% Ganancia controlador:

K=place(A,B,[-3.5 -3.75])

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

Kbig=[K -(Qu_num+K*Qx_num)]

% Formamos el regulador, de orden 3:

sysbig=ss(Abig,Bbig,Cbig,D);
regulador=reg(sysbig,Kbig,Lbig);
zpk(regulador)

%% Simulación del resultado:
%

clp=feedback([1; sysbig],regulador,1,2,+1);
clp.OutputName={'u','y'};

% Comprobamos que los polos de bucle cerrado están donde queríamos:

eig(clp)

% Los polos de parte real cero corresponden a los del generador de perturbaciones, no controlables, por lo que no podemos moverlos.
%

%vamos a ver la respuesta ante condiciones iniciales no nulas, a ver...
xinicialproceso=[4;0;15*ones(numestadosgenerador,1)];
%perturbacion es 15cos(3t), posicion inicial 4, vel inicial 0.
xinicialobservador=[0;0;zeros(numestadosgenerador,1)]; %ni idea, empiezo en 0.

initial(clp,[xinicialproceso;xinicialobservador],20)
%comparamos con un diseño sin tener en cuenta la perturbacion:
L=place(A',C',[-9 -8])';
%K no cambia:
sysnopert=ss(A,B,C,D);
regnopert=reg(sysnopert,K,L);
zpk(regnopert)
clp2=feedback([1;sysbig],regnopert,1,2,+1);
eig(clp2)
%superponemos simulaciones
initial(clp,[xinicialproceso;xinicialobservador],20)
hold on
initial(clp2,[xinicialproceso;[0;0]],20)
hold off
legend('Con cancelación estacionaria','Sin modelo ampliado')

% Nota: simulamos también el generador de ondas, por brevedad, aunque en Simulink hubieramos cerrado el bucle con "sysconpertentrada", no "sysbig" y alimentado a ese sistema "sysconpertentrada" con un generador de ondas constante/senoidal.
% Según la nota anterior, no  "existirían" en la realidad esos polos marginalmente inestables (cerrando con sysconpertentrada).  En efecto:

clp_real=feedback([1 0;sys_conpertentrada],regulador,1,2,+1);
%la entrada 1 es "perturbación en control", que no hemos considerado aquí.
%salidas son control y salida del proceso.
clp_real.OutputName={'Acc. control','Salida'};
size(clp_real)
eig(clp_real)

%
% Vemos que la respuesta en frecuencia de clp_real es cero en el error, y Qu en acción de control:

freqresp(clp_real(:,2),omega)
bodemag(clp_real(:,2))

%
%