%% Control Hinfinito: feedforward + retardo (Padé)
% © 2020, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados
% Presentación en video: http://personales.upv.es/asala/YT/V/ffhinfd.html
% Este código funcionó correctamente con Matlab R2020b
% Objetivo: plantear una planta generalizada para control feedforward. Comprobar la influencia del retardo en las soluciones \mathcal H_\infty obtenidas, mediante la aproximación de Padé.
% Tabla de Contenidos
% Modelado y preliminares
%         Aproximación de Padé
%     Prealimentación ideal (no realizable, + no causal en caso 2)
% Control por prealimentación: planta generalizada
%      Planta NO ponderada:
%      Planta generalizada ponderada
%         determinación de pesos
%         Construcción de la planta generalizada ponderada
%     Control óptimo H-infinito
%         Evaluación de las prestaciones
%%
%% Modelado y preliminares
% \bullet Modelo de planta (vble. manipulada a salida variable controlada):

caso=3
if(caso==1) %sin retardo
    ret_planta=0;  ret_pert=0;
elseif(caso==2) %difícil: la planta tarda en responder
    ret_planta=0.65;
    ret_pert=.0;
elseif(caso==3) %fácil: conocemos efecto pert. con antelación
    ret_planta=0;
    ret_pert=0.65;    
end

s=tf('s');
Gsinretardo=2/(2.5*s+1)^2;
G0=exp(-ret_planta*s)*Gsinretardo

%
% \bullet Modelo de efecto perturbaciones a variable controlada:

Gpert_sinretardo=1.1/(s+1);
Gpert0=exp(-ret_pert*s)*Gpert_sinretardo;

%% Aproximación de Padé
% Aproximaremos con Padé, por ej. la de orden 1 es e^{-ds}=\frac{e^{-ds/2}}{e^{ds/2}}\approx \frac{1-\frac{d}{2}s}{1+\frac{d}{2}s}, la de orden 2 es e^{-ds}=\frac{e^{-ds/2}}{e^{ds/2}}\approx \frac{1-\frac{d}{2}s+\frac{d^2}{8}s^2}{1+\frac{d}{2}s+\frac{d^2}{8}s^2}, etc., dado que las rutinas \mathcal H_2 o \mathcal H_\infty no permiten retardos puros (deben usarse FdT racionales de orden finito):

step(pade(exp(-s),1),pade(exp(-s),2),pade(exp(-s),9),exp(-s)), grid on
legend("aprox. Padé 1","aprox. Padé 2","aprox. Padé 9","retardo puro")
ordenPade=2;
G_pade=pade(G0,ordenPade);zpk(G_pade)

% También necesitamos aproximar con Padé el modelo de las perturbaciones:

Gpert_pade=pade(Gpert0,ordenPade);zpk(Gpert_pade)
step(Gpert_pade,Gpert0),grid on

%% Prealimentación ideal (no realizable, + no causal en caso 2)

FF_ideal=inv(Gsinretardo)*Gpert_sinretardo 

% aparte, habría que retrasar/adelantar, pero el bodemag coindice (retardo sólo desfasa); el FF_ideal debería ser lo de abajo, pero da error cuando ret_planta>ret_pert (caso 2)

%FF_ideal_conretardo=FF_ideal*exp((-ret_pert+ret_planta)*s)

%% Control por prealimentación: planta generalizada
%% \bullet Planta NO ponderada:

PG0=[Gpert0 G0;0 1;1 0]; %para la simulación con retardo sin aproximar
PG0.InputName={'d','u'};
PG0.OutputName={'y','u','dmed'};

% Para la síntesis hinfsyn (con modelos Padé) usaremos:

PGpade=[Gpert_pade G_pade;0 1;1 0]; %para diseño hinfsyn con Padé
PGpade.InputName=PG0.InputName;
PGpade.OutputName=PG0.OutputName;

%% \bullet Planta generalizada ponderada
%% determinación de pesos
% Peso de Entrada: Consideremos un filtro de entrada

Wpert=1/(0.2*s+1)+0.05;

% En caso \mathcal H_\infty se interpretaría como que la entrada podría tener componentes senoidales por debajo de ese límite de respuesta en frecuencia:

bodemag(Wpert,Gpert0*Wpert,logspace(-2,3)), grid on
legend("cota frec. Wpert","cota efecto sobre salida")

% Peso sobre la salida:
% En \mathcal H_\infty, consideraremos un límite para la amplitud de la salida dado por una plantilla:

bw=0.082; LFgain=0.03; HFgain=0.6; 
qu=HFgain/LFgain;
Plantilla{1}=zpk(LFgain*(s/bw+1)/(s/bw/qu+1)*1/(0.09*s+1));
bw=0.0157;LFgain=0.03;HFgain=0.8; 
qu=HFgain/LFgain;
Plantilla{2}=zpk(LFgain*(s/bw+1)/(s/bw/qu+1)*1/(0.09*s+1));
bw=0.35;LFgain=0.03;HFgain=0.49; 
qu=HFgain/LFgain;
Plantilla{3}=zpk(LFgain*(s/bw+1)/(s/bw/qu+1)*1/(0.15*s+1));
bodemag(Plantilla{1},'-.',Plantilla{2},Plantilla{3},Gpert0*Wpert,'-.r'), grid on,
legend("Sin retardo","retardo en planta","retardo en perturbación","B.Abto","Location","Best")
if(caso==2) 
    xline(pi/ret_planta);
end
PlantillaY=Plantilla{caso};
Wy=1/PlantillaY;
bodemag(PlantillaY,"b--",Gpert0*Wpert), grid on,xline(bw,'-.g'),yline(20*log10(HFgain),':'),yline(20*log10(LFgain),':')
legend('Plantilla cota salida','Gpert*Wpert Bucle Abierto')

% Peso sobre variable controlada.
% En \mathcal H_\infty, lo interpretamos como límite de amplitud ante una excitación senoidal dada por Wpert, dado por una plantilla.

PlantillaU=tf(7.5)*Wpert; %la ganancia máxima del FF será 7.5
bodemag(PlantillaU), grid on
Wu=1/PlantillaU;

%% Construcción de la planta generalizada ponderada

Wout=blkdiag(Wy,Wu,1);
Win=blkdiag(Wpert,1);
PGPond=Wout*PGpade*Win;
norm(PGPond(1:2,1),inf) %con K=0

%% Control óptimo H-infinito

[K_FF,~,GAMinf,~]=hinfsyn(PGPond,1,1);
GAMinf
K_FF=minreal(K_FF);
bodemag(K_FF,FF_ideal,PlantillaU/Wpert,'b--',logspace(-1,3)), grid on,
legend('K_{FF}','prealim. ideal sin retardo','Plantilla U/Wpert (máx. ganancia)')
title("Bode de controlador")

%%
%
%% Evaluación de las prestaciones

bodemag(K_FF*Wpert,FF_ideal*Wpert,PlantillaU,'b--',logspace(-1,2)), grid on,
legend('K_{FF} H_\infty*Wpert','prealim. ideal sin retardo','Plantilla U')
CLNWpade=lft(PGpade,K_FF); %bucle cerrado con aproximaciones de Padé
CLNW=lft(PG0,K_FF);
CLWpert=minreal(CLNW*Wpert);
CLcasos{caso}=CLWpert;
step(CLWpert,[Gpert0*Wpert;0],16), grid on
legend("Hinf","Bucle Abierto","Location","best")
step(CLWpert(1),Gpert0*Wpert,4), grid on
xline(ret_planta,'-.k')
xline(ret_pert,'-.r')
legend("Hinf","Bucle Abierto","Wpert input","retardo planta","Location","best")

figure()
bodemag(CLWpert(1),Gpert0*Wpert,PlantillaY,'g-.'), grid on
legend("FF","B. Abto","Plantilla")

return

%

step(CLcasos{1}*exp(-0.65*s),CLcasos{2}*exp(-0.65*s),CLcasos{3},Gpert0*Wpert,'-.'), grid on
legend("Sin retardo","retardo planta","retardo perturb.","B. Abto","Location","best")
bodemag(CLcasos{1},CLcasos{2},CLcasos{3},Gpert0*Wpert,'-.',[0;PlantillaU],'-.r'), grid on
legend("Sin retardo","retardo planta","retardo perturb.","B. Abto","Location","best")