%% Ejemplo IMC con distintos modelos de referencia: comparación rapidez, saturación, amplificación ruido...
% © 2020, A. Sala , C. Ramos, X. Blasco, J. M. Herrero, Universitat Politècnica de València. Todos los derechos reservados.
% Presentaciones en vídeo:
% http://personales.upv.es/asala/YT/V/imc4ca1.html
% http://personales.upv.es/asala/YT/V/imc4ca2.html
%
% Este código funcionó correctamente en Matlab R2020a
% Objetivo: comparar para un mismo proceso y constante de tiempo dominante, distintas alternativas de modelo de referencia para IMC. La comparación se hará tanto en salida ante referencia escalón, como en acción de control . Se simulará con ruido de medida y se hará un análisis de la respuesta en frecuencia de las diferentes funciones de transferencia de bucle cerrado.
% Tabla de contenidos
% 1. Modelo del proceso a controlar
% 2. Diseño IMC
%     0.- Diseño inválido (no realizable)
%     Diseño 1: polo doble en 1/tau_c
%     Diseño 2: polo dominante en 1/tau_c y otro 10 veces más rápido
%     Diseño 3: polo doble, pero 1.5 veces más rápido que 1/tau_c para compensar el mayor tiempo de est. del diseño 2
%     Diseño 4, simplificado: modificar sólo polos más lentos que constante de tiempo deseada
% 3. Simulación de respuesta temporal con ruido de medida
% 5. Respuesta en frecuencia en bucle cerrado
% Conclusiones
%%
%% 1. Modelo del proceso a controlar

s=tf('s');
tau_1=1; %cte. tiempo dominante
tau_2=0.15; %cte. tiempo secundaria NO dominante
G=5/(tau_1*s+1)/(tau_2*s+1);
zpk(G)
step(G), grid on
yline(0.98*dcgain(G),'r','98% ganancia','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
title('Resp. Escalón en bucle abierto de G')
xline(4,'-.','t_{est} bucle abierto')

%%
%
%% 2. Diseño IMC
% Deseamos t_{est}=1.2 s. Con la fórmula de primer orden (98%), saldría una constante de tiempo deseada:

t_est_objetivo=1.2;
tau_c=t_est_objetivo/4

% O, equivalentemente, un polo deseado:

polo_deseado=-1/tau_c

% El proceso no es de primer orden, por lo tanto, en vez de "constante de tiempo deseada" deberemos hablar de "constante de tiempo dominante deseada" o "polo dominante deseado".
% Vamos a generar algunas variaciones de modelos de referencia con esa constante de tiempo dominante...
%% 0.- Diseño inválido (no realizable)
% Este sería el diseño más "directo", buscar transformar a un modelo de primer orden con la constante de tiempo deseada:

Gdeseado_novale=1/(tau_c*s+1) 
step(G/dcgain(G),Gdeseado_novale), grid on
yline(0.98,'r','98%','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
legend('Control B.A.','Gdeseado_ novale')
xline(t_est_objetivo,'-.','4/\tau_c'), xline(4,'-.','t_{est} bucle abierto','Color',[.67 .75 .67])
Qnovale=inv(G)*Gdeseado_novale %NO realizable, no puede ser implementado... 

% Por tanto, Q debe tener más polos, esto es, el modelo de referencia debe tener más polos (ser de grado relativo 2) para que Q sea realizable.
% Se van a proponer cuatro diseños con Q realizable, que por tanto son teóricamente válidos; se comprobará viendo respuestas temporales/en frecuencia su desempeño.
%% Diseño 1: polo doble en 1/tau_c

Gdeseado{1}=1/(tau_c*s+1)^2;
zpk(Gdeseado{1})
step(G/dcgain(G),Gdeseado{1}), grid on
yline(0.98,'r','98%','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
xline(t_est_objetivo,'-.','4/\tau_c','LabelVerticalAlignment','bottom')
xline(t_est_objetivo*1.5,'-.','4/\tau_c*1.5','Color',[.5 .4 0.1],'LabelVerticalAlignment','bottom')
xline(4,'-.','t_{est} bucle abierto','Color',[.6 .7 .6],'LabelVerticalAlignment','bottom')
legend('Control B.A.','Gdeseado1')

% El polo doble influye en el tiempo de establecimiento calculado con la fórmula de 1er orden (se multiplica por 1.5).
% El IMC tendrá parámetro de Youla Q:

Q{1}=inv(G)*Gdeseado{1};zpk(Q{1})

%% Diseño 2: polo dominante en 1/tau_c y otro 10 veces más rápido

Gdeseado{2}=1/(tau_c*s+1)/(0.1*tau_c*s+1);zpk(Gdeseado{2})
Q{2}=inv(G)*Gdeseado{2};zpk(Q{2})
step(G/dcgain(G),Gdeseado{2}), grid on
yline(0.98,'r','98%','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
legend('Control B.A.','Gdeseado2')
xline(t_est_objetivo,'-.','4/\tau_c'), xline(4,'-.','t_{est} bucle abierto','Color',[.67 .75 .67])

% En efecto, el polo 10 veces más rápido no influye en el tiempo de establecimiento calculado con la fórmula de 1er orden.
%% Diseño 3: polo doble, pero 1.5 veces más rápido que 1/tau_c para compensar el mayor tiempo de est. del diseño 2

Gdeseado{3}=1/(tau_c/1.5*s+1)^2;
Q{3}=inv(G)*Gdeseado{3};zpk(Q{3}) 
step(G/dcgain(G),Gdeseado{3}), grid on
yline(0.98,'r','98%','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
legend('Control B.A.','Gdeseado3')
xline(t_est_objetivo,'-.','4/\tau_c'), xline(4,'-.','t_{est} bucle abierto','Color',[.67 .75 .67])

%% Diseño 4, simplificado: modificar sólo polos más lentos que constante de tiempo deseada
% No tocamos lo que ya tiene una dinámica más rápida que la deseada de forma "natural" en G.

zpk(G)
Q{4}=1/dcgain(G)*(s+1)/(tau_c*s+1);

% Esto hace que la respuesta ante referencia, sin ruido, sea:

G_implicitamentedeseado=zpk(minreal(G*Q{4}))
Gdeseado{4}=G_implicitamentedeseado;
step(G/dcgain(G),G_implicitamentedeseado), grid on
yline(0.98,'r','98%','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
legend('Control B.A.','Gdeseado4')
xline(t_est_objetivo,'-.','4/\tau_c'), xline(4,'-.','t_{est} bucle abierto','Color',[.67 .75 .67])
step(G/dcgain(G),Gdeseado{:}), grid on
yline(0.98,'r','98%','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
xline(t_est_objetivo,'-.','4/\tau_c'), xline(4,'-.','t_{est} bucle abierto','Color',[.67 .75 .67])
legend('Control B.A.',"[1] 1/(0.3s+1)^2","[2] 1/(0.3s+1)/(0.03*s+1)", "[3] 1/(0.2s+1)^2","[4] Simplificado 1/(0.3s+1)/(0.15*s+1)",'Location','best')

%% 3. Simulación de respuesta temporal con ruido de medida
% La respuesta en bucle cerrado de un IMC de parámetro Q sin error de modelado es:
% y=GQr+(1-GQ)\cdot(G d_u+d_y), siendo d_u perturbación a la entrada (o ruido de proceso) y d_y perturbación a la salida (o ruido de medida).
% u=Q(r-Gd_u-d_y)
% Simulando únicamente ante ruido de medida aleatorio, haremos d_u=0.
% Usaremos T=GQ, S=1-T, con lo que y=Tr+Sd_y, u=Q(r-d_y)

for i=1:4 %simulamos 4 diseños
   T{i}=minreal(G*Q{i}); %es la FdT de r o ruido de medida ->"y sin ruido"
   S{i}=1-T{i}; %es la FdT de ruido de medida a salida con ruido...
   tiemposim=(0:0.001:1)*4;
   ruidomedida=randn(1,length(tiemposim))*.02;
   referencia=ones(1,length(tiemposim));
   saliday{i}=lsim(T{i},referencia,tiemposim) ...
      + lsim(S{i},ruidomedida,tiemposim);
   accontrol{i}=lsim(Q{i},-ruidomedida+referencia,tiemposim);
end
plot(tiemposim,saliday{2},'Color',[.58 .74 .9]), grid on
hold on
plot(tiemposim,[saliday{[1 3 4]}])
step(G/dcgain(G),'-.',4);
hold off
grid on
title('Respuesta ante escalón referencia (salida, vble. controlada)')
yline(1,'k','ref','LineStyle','--','LabelHorizontalAlignment','left');
yline(0.98,'r','98% ref.','LineStyle','-.','LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom');
legend(["[2] 1/(0.3s+1)/(0.03*s+1)","[1] 1/(0.3s+1)^2", "[3] 1/(0.2s+1)^2","[4] Simplificado 1/(0.3s+1)/(0.15*s+1)","Bucle Abto. G/dcgain(G)"],'Location','best')
xline(t_est_objetivo,'-.','4/\tau_c')
plot(tiemposim,accontrol{2},'Color',[.58 .74 .85]), grid on
hold on
plot(tiemposim,[accontrol{[1 3 4]}]), grid on
hold off
yline(1/dcgain(G),'-.b',"B. abto: 1/Ganancia",'LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom')
xline(t_est_objetivo,'-.','4/\tau_c'), xlim([0 3])
legend(["[2] 1/(0.3s+1)/(0.03*s+1)","[1] 1/(0.3s+1)^2", "[3] 1/(0.2s+1)^2","[4] Simplificado 1/(0.3s+1)/(0.15*s+1)"],'Location','best')
title('Acción de control (vble. manipulada) ante escalón referencia')

%los diseños razonables son 1, 3, 4
plot(tiemposim,accontrol{2},'Color',[.74 .82 .9]), grid on
hold on
plot(tiemposim,[accontrol{[1 3 4]}],'LineWidth',2), grid on
ylim([0 .77]), xlim([0 2])
hold off
yline(1/dcgain(G),'-.b',"B. abto: 1/Ganancia",'LabelHorizontalAlignment','left','LabelVerticalAlignment','bottom')
xline(t_est_objetivo,'-.','4/\tau_c','LabelVerticalAlignment','bottom')
legend(["[2] 1/(0.3s+1)/(0.03*s+1) amplif. ruido inaceptable","[1] 1/(0.3s+1)^2", "[3] 1/(0.2s+1)^2","[4] Simplificado 1/(0.3s+1)/(0.15*s+1)"],'Location','best')
title('Acción de control (detalle)')

% La respuesta ante perturbaciones a la entrada (ruido de proceso):

for i=1:4
   GS{i}=G*S{i};
end
step(GS{:},G, 7), grid on
xline(t_est_objetivo,'-.','4/\tau_c'),xline(4,'-.','t_{est} bucle abierto','LabelVerticalAlignment','middle','Color',[.6 .75 .6])
legend(["[1] 1/(0.3s+1)^2","[2] 1/(0.3s+1)/(0.03*s+1)", "[3] 1/(0.2s+1)^2","[4] Simplificado 1/(0.3s+1)/(0.15*s+1)","Bucle Abto. G"])
title('Resp. escalón unitario perturb. entrada')

%% 5. Respuesta en frecuencia en bucle cerrado
% El ruido de medida "totalmente aleatorio, ruido blanco" puede no ser el peor caso de perturbación... podría haber un "ruido coloreado" que excitara algún tipo de vibración o resonancia.
% Vamos a examinar el diagrama de Bode de amplitud completo, para tener más información de cara a evaluar los diseños.
% Referencia a salida

bodemag(T{:},G/dcgain(G)), grid on, 
legend(["[1] 1/(0.3s+1)^2","[2] 1/(0.3s+1)/(0.03*s+1)", "[3] 1/(0.2s+1)^2","[4] Simplificado  1/(0.3s+1)/(0.15*s+1)","Bucle Abto. G/dcgain(G)"],'Location','best')
title('Resp. frec (amplitud) referencia a salida')

% Ref. (o ruido med.) a error

bodemag(S{:},tf(1)), grid on
legend(["[1] 1/(0.3s+1)^2","[2] 1/(0.3s+1)/(0.03*s+1)", "[3] 1/(0.2s+1)^2","[4] Simplificado  1/(0.3s+1)/(0.15*s+1)","B. abto 1"],'Location','best')
title('Resp. frec (amplitud) referencia o ruido medida a error de bucle')

% (ref o ruido) -> acc. control

bodemag(Q{:},tf(1/dcgain(G))), grid on
legend(["[1] 1/(0.3s+1)^2","[2] 1/(0.3s+1)/(0.03*s+1)", "[3] 1/(0.2s+1)^2","[4] Simplificado  1/(0.3s+1)/(0.15*s+1)","Bucle Abto. 1/dcgain(G)"],'Location','best')
title('Resp. frec (amplitud) referencia o ruido medida a acción de control')

% (pert_entrada) -> salida

bodemag(GS{:},G), grid on
legend(["[1] 1/(0.3s+1)^2","[2] 1/(0.3s+1)/(0.03*s+1)", "[3] 1/(0.2s+1)^2","[4] Simplificado  1/(0.3s+1)/(0.15*s+1)","Bucle Abto. G"],'Location','best')
title('Resp. frec (amplitud) ruido de proceso (pert. entrada) a salida')
xlim([1e-2 1e2])

% Se observa que si existiera un ruido de proceso de contenido en frecuencias alrededor de 2 rad/s ( \approx  sube y baja con período de más o menos 3 segundos) o más rápido, entonces los diseños IMC no serían muy efectivos comparados con el bucle abierto (incluso peores que no hacer nada).
%
%% Conclusiones
% En este material se han simulado cuatro posibles diseños con la misma constante de tiempo dominante para un proceso de segundo orden. El modelo de referencia necesita dos polos para que el IMC sea realizable: se ha comprobado que cuanto más rápida es la segunda constante de tiempo, más rápido es el seguimiento de referencia escalón, mejor se cancelan (pero no más rápidamente) las perturbaciones de entrada (ruido de proceso) tipo escalón; los inconvenientes son que cuanto más rápida es la segunda constante de tiempo más se amplifica el ruido de medida y más problemas de saturación se tendrán.
%%