%% Control Hinfinito: ejemplo feedforward (prealimentación)
% © 2020, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados
% Presentación en vídeo: http://personales.upv.es/asala/YT/V/ffhinf.html
% Este código funcionó correctamente con Matlab R2020b
% Objetivo: plantear una planta generalizada para control feedforward y haremos hinfsyn.
% Tabla de Contenidos
% Modelado
% Control por prealimentación: planta generalizada
%      Planta NO ponderada:
%      Planta generalizada ponderada
%         Determinación de pesos
%         Construcción de la planta ponderada:
%     Control óptimo:
%     Evaluación de las prestaciones
%%
%% Modelado
% \bullet Modelo de planta (vble. manipulada a salida variable controlada):

s=tf('s');
G=2*(-0.1*s+1)/(2.5*s+1)^2/(0.02*s+1);

% \bullet Modelo de efecto perturbaciones a variable controlada:

Gpert=1.1/(s+1);

% y=Gu+G_{pert}d

step(G,Gpert,19), grid on, ylim([-.025 2]), legend

%
%% Control por prealimentación: planta generalizada
%% \bullet Planta NO ponderada:
% y=G*u+G_{pert}d, añadiendo u como salida (para limitarla), y d como información al controlador, tenemos:

PG=[Gpert G; 
    0 1;
    1 0]; 
PG.InputName={'d','u'};
PG.OutputName={'y','u','dmed'};

% La prealimentación "perfecta" no es ni realizable ni estable:

FF_ideal=zpk(-inv(G)*Gpert)

%% \bullet Planta generalizada ponderada
%% Determinación de pesos
% Peso de Entrada "d": 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,Gpert*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.04; LFgain=0.03; HFgain=0.6; qu=HFgain/LFgain;
PlantillaY=zpk(LFgain*(s/bw+1)/(s/bw/qu+1)*1/(0.25*s+1))

bodemag(PlantillaY,"b--",Gpert*Wpert), grid on,xline(bw,'-.g'),yline(20*log10(HFgain),':'),yline(20*log10(LFgain),':')
legend('Plantilla cota salida','Gpert*Wpert Bucle Abierto')

% El peso es la inversa de la plantilla:

Wy=1/PlantillaY;

% Sin control tendríamos:

[GAMinf_OpenLoop,wc_freq]=norm(Wy*Gpert*Wpert,"inf")

% y el optimizador \mathcal H_\infty debería evidentemente mejorar sobre ese valor.
% Peso sobre variable manipulada (acción de control u)
% En \mathcal H_\infty, lo interpretamos como límite de amplitud ante una excitación senoidal de amplitud dada por Wpert.

PlantillaU=tf(7.5)*Wpert; 
%la ganancia máxima del bloque FeedForward será 7.5
bodemag(PlantillaU), grid on
Wu=1/PlantillaU; %pesos de salidas generalizadas son inversa de la plantilla

%% Construcción de la planta ponderada:

Wout=blkdiag(Wy,Wu,1); %pesos salidas generalizadas
Win=blkdiag(Wpert,1); %pesos entradas generalizadas
PGPond=Wout*PG*Win; %planta generalizada ponderada
norm(PGPond(1:2,1),inf) %con K=0 coincide con lo de arriba

%% Control óptimo:

[K,CL,GAMinf,~]=hinfsyn(PGPond,1,1);
GAMinf
sigma(CL,logspace(-2,3)), grid on
K_FF=minreal(K);
zpk(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,3)), grid on,
legend('K_{FF}*Wpert','prealim. ideal sin retardo','Plantilla U')
title("Bode cota máxima de u ante entrada Wpert")

% Cerremos el bucle para evaluar prestaciones

CLNW=lft(PG,K_FF); 
CLWpert=CLNW*Wpert;
%Sólo peso "entrada" para simular ante entrada "física" (no amplitud 1)
step(CLWpert,[Gpert*Wpert;0]), grid on
legend("Hinf feedforward","Bucle Abierto","location","best")
bodemag(CLWpert,[Gpert*Wpert;0],[PlantillaY;PlantillaU],'g-.'), grid on
legend("FF","B. Abto","Plantillas")