%% Doble integrador: diseño \mathcal H_\infty para rechazo de perturbaciones en entrada y salida (hinfsyn), comparativa entre diferentes posibilidades de diseño
% © 2021, Antonio Sala Piqueras. Universitat Politècnica de València. Todos los derechos reservados.
% Presentaciones en vídeo:
% http://personales.upv.es/asala/YT/V/dinthi1.html , http://personales.upv.es/asala/YT/V/dinthi2.html , http://personales.upv.es/asala/YT/V/dinthi3.html .
% Este código ejecutó sin errores en Matlab R2021b
% Objetivo: el doble integrador modela sistemas mecánicos "libres" con poca fricción (control de movimientos, drones, ...). Como todos los modelos, es "aproximado" hasta un cierto "ancho de banda" donde entrarían vibraciones estructurales del móvil, control en cascada/giro-estabilización de drones, dinámica de actuadores electromecánicos o hélices, aparte de posibles no-linealidades o errores paramétricos (masa/inercia/fricción mal modeladas)...
% Este vídeo plantea posibilidades del control \mathcal H_\infty sobre dicho proceso, analizando prestaciones nominales, márgenes de robustez, etc. Dado que \mathcal H_\infty no tiene como objetivo reducir la varianza ante ruidos, si las perturbaciones fueran ruidos coloreados (o si hubiera mucho ruido de medida de alta frecuencia), podría requerir en la práctica un rediseño de los filtros o/y comparación con el \mathcal H_2 en términos de prestaciones/márgenes de robustez en simulación o/y experimentación real. El control, implementado por computador en un 99.9% de los casos, lo supondremos implementado a un período de muestreo suficientemente pequeño para que los efectos de la discretización sean despreciables.
% Tabla de Contenidos
% 1.- Modelado
% 2.- Planteamiento del problema de control como planta generalizada
% 3.- Planta generalizada ponderada y controlador hinfsyn
% 4.- Análisis tiempo/frecuencia del bucle resultante
%     Polos, ceros, resp. frecuencia del regulador
%     Análisis bucle cerrado nominal en tiempo y frecuencia
% 5.- Análisis de robustez
%     Márgenes clásicos SISO
%     Márgenes norma infinito
%         Elemento a elemento
%         Conjunto
%         Comparación con el máximo margen nfc posible:
% 6.- Simulación con posible proceso "real"
% Anexo
%     Análisis comparativo
%         Prestaciones nominales tiempo/frecuencia
%         Márgenes de robustez
%         Simulación con error de modelado
%     Funciones auxiliares
%
%% 1.- Modelado

s=tf('s');
G=ss(1/s^2);

% El límite de saturación en U implica también un límite de ancho de banda para seguir referencias (aceleración limitada):

satU=4.5;
sigma(G*satU,tf(1)), grid on

% No podremos seguir referencias senoidales de amplitud 1 y frecuencia superior a \omega^*=sqrt(satU). En efecto, la aceleración de r=\sin(\omega^*t) es -(\omega^*)^2\sin(\omega^*t), y por tanto (\omega^*)^2\leqsatU.

omega_lim=sqrt(satU)

% En este caso, el semiperíodo (tiempo de tope inferior al superior será):

pi/omega_lim

% y es un estimado grosso modo del tiempo de establecimiento más rápido sin saturar u.
% Aparte, podría necesitar hacerse más lento por cuestiones de robustez, filtrado de ruido de medida, dinámica (conocida o no) de sensores y actuadores, retardos de comunicación/discretización ...
% Quizás, debido a esas necesidades de filtrado/robustez, etc. 1.5 segundos sea un estimado del "tiempo de subida", más que del "tiempo de establecimiento".
%% 2.- Planteamiento del problema de control como planta generalizada
% El problema de control de un "móvil" por ejemplo no es sólamente "seguimiento de referencia": un "dron" debería  mantener posición pese al viento, por ejemplo, o pese a incertidumbre en el modelo (tanto en parámetros como masa o rozamiento, como en dinámica no modelada de las hélices, movimiento angular,  etc.). Debemos contemplar perturbaciones en salida y entrada, y errores de modelado.
%
% Planta generalizada pert. entrada y salida (realimentación negativa para no andar cambiando signos al comparar con PIDs):

PG=ConstruyePlantaGen(G);
size(PG)
PG.InputName={'dy','du','u'};
PG.OutputName={'y','u','ym'};

%% 3.- Planta generalizada ponderada y controlador hinfsyn

ejemplo=2
if ejemplo==1 %puro mixed sensitivity... cancelación polo-cero, fatal "du"... sólo válido si G no tiene polos "lentos"
    Wdy=tf(1); Wdu=tf(0.001); %Dará problemas
    bandwidth=1.17;
elseif ejemplo==2
    Wdy=tf(1); Wdu=tf(1); 
    bandwidth=0.15;
elseif ejemplo==3 %algo más de énfasis en "input disturbance" que caso 2
    Wdy=tf(0.5); Wdu=tf(2); 
    bandwidth=0.147;
else
    Wdy=tf(0.001); Wdu=tf(2.6); %optimal input disturbance rejection
    bandwidth=87.8; %Dará problemas
end

PlantillaY=makeweight(0.025,bandwidth,2.5);

% Queremos limitar algo más la alta frecuencia de "u" aparte de la mera "saturación" para aumentar robustez y comportamiento ante ruidos a alta frecuencia (más alla de 12 rad/s) reduciendo actividad de control.

PlantillaU=ss(satU*((1+s/240)/(s/12+1))); 
bodemag(PlantillaY,PlantillaU), grid on, legend("Plantilla y","Plantilla U")
Win=blkdiag(Wdy,Wdu); Win.InputName={'dy_esc','du_esc'};
Wy=1/PlantillaY;Wu=1/PlantillaU;
Wout=minreal(blkdiag(Wy,Wu));
PGPond=blkdiag(Wout,1)*PG*blkdiag(Win,1);
size(PGPond)
tf(PGPond(3,3)) %seguimos controlando un doble integrador
[K,CL,GAM,INFO]=hinfsyn(PGPond,1,1); GAM
eee=eig(CL) 

% Estos eee son todos los polos, incluyendo los de filtros entrada (no controlables por u) y filtros de salida (no observables desde y_m).

eig(feedback(G*K,1)) %estos, los polos de bucle cerrado "de toda la vida".

%
%% 4.- Análisis tiempo/frecuencia del bucle resultante
%% Polos, ceros, resp. frecuencia del regulador

zpk(K)
tzero(K)
pole(K)

% Podríamos también considerar una reducción de orden (aproximar a PID con filtro de ruido si orden 2):

[Kl,Kh]=freqsep(K,20); %cortar dinámica a partir de 20 radianes
Klb=Kl+dcgain(Kh); %Khb=Kh-dcgain(Kh); parte alta frecuencia
zpk(Klb) %PID con filtro de ruido...
bodemag(K,Klb), grid on, legend("K","aprox baja frec.")

%
%% Análisis bucle cerrado nominal en tiempo y frecuencia

BC=lft(PG,K); BCred=lft(PG,Klb);
eig(BC)
step(BC(:,1),BCred(:,1)), grid on, title("Step, no ponderado, ante pert. salida/referencia tamaño 1")
step(BC(:,2),BCred(:,2)), grid on, title("Step, no ponderado, ante pert. entrada tamaño 1")
step(BC*Win), grid on, title("Step, ponderando entradas por su tamaño")
bodemag(BC), grid on, title("Diag. Bode, no ponderado")
bodemag(BC*Win), grid on
hold on
bodemag([PlantillaY;PlantillaU]*[1 1])
hold off, title("Diag. Bode ponderando entradas por tamaño")

% En teoría también garantizamos norma ponderada<1 ante la "peor combinación de dy y du a cada frecuencia":

sigma(BC(1,:)*Win,PlantillaY) %la peor "salida y" posible ante la peor "dy, du"
grid on
sigma(BC(2,:)*Win,PlantillaU) %la peor "acción u" posible ante la peor "dy, du"
grid on

%
%% 5.- Análisis de robustez
%% Márgenes clásicos SISO
% *Obviamente, este análisis se puede hacer únicamente porque el proceso controlado es monovariable.

nyquist(G*K), axis([-8 1 -1.5 1.5]),yline(0)
stab_marg_clasico=allmargin(G*K)

%% Márgenes norma infinito
%% Elemento a elemento
% a) Margen ante incertidumbre aditiva en proceso G_{real}=G+\Delta, o aditiva inversa en el controlador K_{real}=(K^{-1}+\Delta)^{-1}, ante errores de implementación por discretización, precisión finita, etc. de K.

marg_aditivo= 1/norm(BC(2,1),'inf') %de dy a u, pico KS

% b) Margen ante incertidumbre multiplicativa en proceso o controlador G_{real}=G(1+\Delta), K_{real}=K(1+\Delta).

marg_multiplicativo=1/norm(BC(2,2),'inf') %de du a u, pico T... en SISO

% c) Margen ante incertidumbre multiplicativa inversa proceso o controlador G_{real}=G(1+\Delta)^{-1}, K_{real}=K(1+\Delta)^{-1}.

marg_multiplicativo_inv=1/norm(BC(1,1),'inf') %de dy a y, pico S

% d) Margen ante incertidumbre aditiva inversa en proceso G_{real}=(G^{-1}+\Delta)^{-1}, o aditiva en el controlador K_{real}=K+\Delta.

marg_aditivo_inv=1/norm(BC(1,2),'inf') %de du a y, pico GS

%% Conjunto
% Márgen ante incertidumbre G_{real}=\frac{N+\Delta_N}{D+\Delta_D}, siendo G=\frac{N}{D} normalizado N(j\omega)^2+D(j\omega)^2=1. En este caso del doble integrador, N=\frac{1}{s^2+\sqrt{2}s+1}, D=\frac{s^2}{{s^2+\sqrt{2}s+1}}.

ncfrobmarg=ncfmargin(G,K) %realim negativa supone ncfmargin

% Dada la dificultad de intepretación de la "factorización normalizada" (más aún en el caso MIMO), su interés "práctico" es más bien que garantiza que cada uno de los 4 márgenes individuales de arriba es mayor que el ncfmargin.

1/norm(BC,'inf')
ncfmargin(G,Klb) %veamos si perdemos mucho o no con el "reducido"

%% Comparación con el máximo margen nfc posible:

[K2,CL,GAM2]=ncfsyn(G); 1/GAM2 %sin "pesos" este es el márgen máximo ncf que se podría conseguir.

% Este controlador da el ncfmargin más grande posible, aunque con malas prestaciones en la mayoría de casos (se requerirá "pesos en frecuencia", obviamente, metodología Glover Mc-Farlane, no objetivo de este material).

ncfmargin(G,-K2)

% Nos puede "guiar" en la forma del regulador, los márgenes conseguibles, o para un "primer experimento" en un proceso muy incierto...

zpk(-K2)
allmargin(-G*K2)
step(BC,lft(PG,-K2),20), grid on, legend('Diseño Planta GEn','ncfsyn sin pesos') %ncfsyn supone realim. positiva

%
%% 6.- Simulación con posible proceso "real"
% Para comprobar si andabamos "acertados" con el análisis de robustez sobre el modelo nominal, vamos a inventarnos una posible planta real, a ver cómo nos funciona.

Greal=ss(1.04/s/(s+0.018)/(s/15+1)^2)*exp(-0.015*s); %ancho banda limitado actuador, retardo/discretización
gapmetric(G,pade(Greal,2)) %estable si menor a ncfmargin, aunque podría ser que no tuviéramos prestaciones robustas
PGreal=ConstruyePlantaGen(Greal);
BCreal=lft(PGreal,K); 

% Con esta planta tendríamos "prestaciones robustas" si lo que sigue diera menor a 1:

PGPondReal=blkdiag(Wout,1)*PGreal*blkdiag(Win,1);
norm(pade(lft(PGPondReal,K),2),'inf')
norm(pade(lft(PGPondReal,Klb),2),'inf')

% *deberíamos hacer \mu-síntesis (no convexo) para intentar obtener prestaciones robustas en caso de no tenerlas... o para garantizarlas para toda una familia de plantas con cotas de error de modelado; eso no es objetivo de este material.
% Como lo implementaremos por computador, simularemos BCreal y también en discreto, y con un controlador de "orden reducido", por comparar:

Ts=0.05; nyquistfreq=pi/Ts
PGrealDisc=c2d(PGreal,Ts,'zoh'); 

% Nota: la discretización "zoh" no considera la respuesta intermuestreo de perturbaciones y salidas. Si el período de muestreo no fuera "muy pequeño", debería considerarse sdlsim en vez del código aquí discutido.

KDisc=c2d(Klb,Ts,'Tustin'); %la aproximación de orden 2, discreto
BCrealDiscRed=lft(PGrealDisc,KDisc); %ponemos también el regulador de orden 2...
step(BC,BCreal,BCrealDiscRed), legend("nominal","con error de modelado","K discr. red."), grid on
bodemag(BC*Win,BCreal*Win,BCrealDiscRed*Win,[PlantillaY;PlantillaU]*[1 1]), legend("nominal","con error de modelado","K discr. red.", "COTA",Location='best'), grid on
Paracompara.margenretardo(ejemplo)=stab_marg_clasico.DelayMargin;
Paracompara.BCnominal{ejemplo}=BC;
Paracompara.BCreal{ejemplo}=BCrealDiscRed;
Paracompara.K{ejemplo}=zpk(K);
Paracompara.ncfmarg(ejemplo)=ncfrobmarg;
return

%
%% Anexo
%% Análisis comparativo
% Cuando ya hayamos ejecutado todos los ejemplos, podemos compararlos...

%load paracomparar.mat

%% Prestaciones nominales tiempo/frecuencia

step(Paracompara.BCnominal{1:4},8), grid on, legend("sólo dy","2","3","sólo du",Location="best")
bodemag(Paracompara.BCnominal{:},logspace(-3.5,2.5,400)), grid on, legend("dy","2","3","du",Location="best")

%% Márgenes de robustez

Paracompara.ncfmarg %los márgenes de robustez ncf de cada diseño
Paracompara.margenretardo
Paracompara.K{:}

%% Simulación con error de modelado

step(Paracompara.BCreal{[1 2 3]},8), grid on, legend("dy","2","3",Location="best")
bodemag(Paracompara.K{:},logspace(-3,3,400)), grid on, legend("dy","2","3","du",Location="best")

%
%
%% Funciones auxiliares

function PGen=ConstruyePlantaGen(Proceso)
%construimos elementos de diagrama de bloques
Proceso.InputName={'u_real'}; Proceso.OutputName={'y_limpia'};
sumblk1=sumblk('u_real = u + du');
sumblk2=sumblk('y = y_limpia + dy');
sumblk3=sumblk('ym = -y');
%interconectamos todo.
PGen=connect(Proceso,sumblk1,sumblk2,sumblk3,{'dy','du','u'},{'y','u','ym'});
end

%