%% Prestaciones robustas, caso de estudio Matlab 5/(s+1)^2: mu síntesis
% © 2022, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Este código ejecutó sin errores en Matlab R2022a
% Presentaciones en vídeo:
% http://personales.upv.es/asala/YT/V/cerp4mu.html , http://personales.upv.es/asala/YT/V/cerp5mu.html .
% Objetivos: Comprender las ideas subyacentes a la teoría sobre prestaciones robustas y pequeña ganancia escalado con un ejemplo monovariable de segundo orden, ejecutado con mu-síntesis (búsqueda automática iterativa de multiplicadores) en vez de buscar "a mano" multiplicadores como en vídeos previos que sólo tenían interés didáctico/ilustrativo, pero no eran aconsejables como herramienta de diseño en un caso real.
% Tabla de Contenidos
% Modelo de planta a controlar
%     Cota deseada de prestaciones robustas (en frecuencia)
% Diseño Mu-Síntesis para prestaciones robustas
%         Objetivo teórico
%         Planta Generalizada 2x2 para musyn
%         Planta Generalizada 3x3 a partir de 2x2+incertidumbre (por comparar, esto NO es necesario)
%     Planta generalizada ponderada para musyn
%     Mu-síntesis
%     Controladores de orden reducido
%         Reducción de orden del regulador
%         Optimización directa de un regulador tipo PID
% Simulación prest. robustas en tiempo y frecuencia
%
%% Modelo de planta a controlar

s=tf('s');
G=5/(s+1)^2; %modelo NOMINAL

BoundDelta=0.5; %tamaño error modelado "Delta" aditivo no estructurado
Greal=G+BoundDelta*ultidyn("DeltaNormalized"); %modelo incierto para mu-síntesis

Greal_parasimular=G+BoundDelta*ultidyn("DeltaNormalized")*1/(0.03*s+1);
%añado "matar" Greal a partir de 33 rad/s. No usado en análisis ese
%paso-bajo final.

step(Greal_parasimular,G,12), grid on
bodemag(Greal_parasimular,logspace(-1,2)), grid on, hold on
bodemag(G,logspace(-1,2)), hold off, legend("cota Delta*Paso bajo", "Nominal",Location="best")

%
%% Cota deseada de prestaciones robustas (en frecuencia)

TargetBandwidth=1.01; %aprox. inversamente prop. tiempo de subida
templateErr=makeweight(0.01,TargetBandwidth,1.8);
bodemag(templateErr,tf(1)), grid on
title("Ref->Error Bode Diagram maximum modulus, template")
legend("plantilla maximo","0 dB (100% error)")

% Nota: por simplicidad no limitamos la acción de control por tema "prestaciones" (saturación, etc.), aunque en aplicaciones sí sería recomendable. De todos modos, la robustez ante incertidumbre aditiva se consigue limitando "u", con lo que implícitamente sí estamos en cierto modo limitando "u".
%
%% Diseño Mu-Síntesis para prestaciones robustas
%% Objetivo teórico
% Trataríamos de construir una planta generalizada 3x3 con incertidumbre para aplicar (por ejemplo) teorema de pequeña ganancia escalado si quisiéramos ejecutar la búsqueda de multiplicadores por nuestra cuenta (como se ha hecho en otros vídeos anteriores, "manualmente").
%
% Si nosotros, no obstante, construimos con un modelo incierto sólo la parte 2x2 que se refiere a los grupos 2 y 3, el Robust Control Toolbox de Matlab se encarga de extraer la incertidumbre y plantear (internamente) la planta 3x3. [Cuando digo 2x2 o 3x3 me refiero a "grupos" de señales, cada grupo tiene unas dimensiones diferentes dependiendo de cada problema en concreto a resolver].
% *Si queremos ver "explícitamente" la planta 3x3 que utiliza musyn, lo podemos hacer con el comando lftdata (ver abajo), pero no es necesario ese uso explícito en la mayoría de los casos sencillos ( ya se encarga musyn de hacerlo), y planteamos la planta generalizada con la incertidumbre "dentro".
%% Planta Generalizada 2x2 para musyn
%
% No es recomendable dos "occurrences" si físicamente sólo hay "una". Mejor escribimos  [err;err]=[1 -Greal;1 -Greal]*[ref;u]  sin multiincidencia de Greal:

PlantaGenMu=minreal([[1;1] [-1;-1]*Greal] )%if we execute "ss" then uncertainty is destroyed. Matlab does it in state-space form anyway.

%% Planta Generalizada 3x3 a partir de 2x2+incertidumbre (por comparar, esto NO es necesario)
% Extraemos la incertidumbre con lftdata como sigue (realmente NO es necesario hacerlo, ya lo hace musyn internamente, transparente a nosotros):

[M,Delta]=lftdata(PlantaGenMu);
size(M)
Delta
zpk(M)

% Coincide con lo que nosotros habíamos propuesto al buscar "a mano" el multiplicador en otros vídeos (bueno, incorporando además el peso de la incertidumbre). En efecto
%

PlantGenOtherVideo=minreal(ss([0 0 1;-1 1 -G;-1 1 -G])); %NOT NEEDED when using musyn
zpk(PlantGenOtherVideo*blkdiag(BoundDelta,1,1)) %We add input weight with size of uncertainty %NOT NEEDED when using musyn

%% Planta generalizada ponderada para musyn

Win=blkdiag(1,1);
Wout=minreal(blkdiag(1/templateErr,1));
PlantaGenPond=Wout*PlantaGenMu*Win %la incertidumbre está dentro de PlantaGenMu

%% Mu-síntesis
% Minimizar la norma infinito con búsqueda de multiplicadores lo haríamos con:

[Kmu,GAM]=musyn(PlantaGenPond,1,1); GAM %no gastamos ni "M" ni "Delta" del lftdata, ya lo construye musyn internamente

% *El multiplicador para un ultidyn SISO es un S(s) dinámico, que mejora las prestaciones que se podrían probar respecto a un multiplicador constante visto en otros vídeos preliminares. Bueno, se le denomina   "D(s)" en la salida de musyn, cuando presenta D fit, Fit Order.
% Sería una opción "teóricamente mejor" si la incertidumbre es lineal e invariante en el tiempo... pero observad cómo el multiplicador dinámico sube el orden del regulador resultante. A cambio, no aseguraría robustez ante no-linealidad con distorsión armónica o variante en el tiempo.

size(Kmu) %lti uncertainty permite pesos en frecuencia, por eso sube el orden.

%% Controladores de orden reducido
% Tenemos varias opciones:
% reducir el orden regulador obtenido por musyn,
% Indicar a musyn mediante ciertas opciones que el orden del escalado "D" no sea tan elevado
% Directamente optimizar algo de orden menor (como un PID)
% Vamos a ver la primera y tercera.
%% Reducción de orden del regulador

hsvd(Kmu) %muchos valores singulares son muy pequeños, podemos reducir con (esperemos) poca pérdida de prestaciones
Kmu_reduced=balred(Kmu,3);
wcgain(lft(PlantaGenPond,Kmu_reduced))
zpk(Kmu_reduced)

% Como hay un polo "rápido" vamos a quitarlo con freqsep, independientemente de su efecto hsvd, a ver qué podemos obtener:

[Kslow,Kfast]=freqsep(Kmu_reduced,150);
zpk(Kslow)
zpk(Kfast)
Kmu_reduced2=Kslow+dcgain(Kfast);
wcgain(lft(PlantaGenPond,Kmu_reduced2)) %seguimos teniendo prestaciones robustas
zpk(Kmu_reduced2) %it's a PID+'noise filter' controller

% El controlador resultante se parece mucho a un PID con filtro de ruido... ello sugiere la optimización directa de un PID, que vemos a continuación.
%
%% Optimización directa de un regulador tipo PID

PID=tunablePID('PIDtest','PID');
ClosedLoopWithPID=lft(PlantaGenPond,PID);tic
[TunedCL,GAM]=musyn(ClosedLoopWithPID); GAM,toc

% Pues no hemos perdido prácticamente nada respecto al regulador "state space" genérico. Bueno, igual habría que reducir un poquito el ancho de banda para estar "debajo" de 1, pero un 1 por mil no va a importar lo más mínimo en la práctica (la tolerancia interna de los solvers h-infinito por defecto ya es del 1 por cien)...

TunedPID=pid(TunedCL.Blocks.PIDtest)
zpk(TunedPID) %se parece al Kmu_reduced2 de arriba, tolerancias aparte consiguen básicamente lo mismo

%
%% Simulación prest. robustas en tiempo y frecuencia

Kfinal=Kmu_reduced2;
%Kfinal=TunedPID;
step(feedback(Greal*Kfinal,1),feedback(Greal_parasimular*Kfinal,1),8), grid on
legend("Cota Delta 0.5","Cota Delta 0.5*PasoBajo")
bodemag(feedback(1,Greal*Kfinal),feedback(1,Greal_parasimular*Kfinal),templateErr,logspace(-2.5,2.5)), grid on
h_line=findobj(gcf,'type','line');
h_line(46).LineWidth=4; %trial and error to get the right line to make thicker
legend("Cota Delta 0.5","Cota Delta 0.5*PasoBajo","cota máx. PR",Location="best") %error

% \bullet¿Acción de control? Todo control "final" debe tener una acción de control "razonable". No lo hemos limitado teóricamente, pero es importante, por lo que vamos a ver cuál es:

step(feedback(Kfinal,Greal_parasimular),6), grid on
title("ref->u en tiempo (step)")
Fu=feedback(Kfinal,Greal_parasimular);
bodemag(Fu,logspace(-2.5,2.5)), hold on
bodemag(Fu.NominalValue,tf(1/BoundDelta),logspace(-2.5,2.5)), hold off
grid on, title("ref->u en frecuencia"),legend("Bucle cerrado","Nom","Cota Pequeña ganancia estab. robusta")

% Obviamente, está por debajo de la cota de estabilidad robusta (pequeña ganancia clásico, no escalado) porque asegurar prestaciones robustas es más exigente que asegurar sólo estabilidad robusta.
%