%% Control Robusto de un sistema masa-muelle-amortiguador incierto
% (c) 2023, Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Este código ejecutó sin errores con Matlab R2022b
% Objetivos: Modelar un sistema incierto con la Robust Control Toolbox, simplificar la incertidumbre, y comparar técnicas mixed sensitivity, mu-síntesis con reguladores arbitrarios o con reguladores fijando la estructura a tipo PID.
% Presentaciones en vídeo en:
% http://personales.upv.es/asala/YT/V/rcml1.html [modelado ureal, ultidyn]
% http://personales.upv.es/asala/YT/V/rcml2.html [simplificación incertidumbre ucover]
% http://personales.upv.es/asala/YT/V/rcml3.html [mixed sensitivity]
% http://personales.upv.es/asala/YT/V/rcml4.html [hinfstruct PID]
% http://personales.upv.es/asala/YT/V/rcml5.html [musyn (dksyn obsoleto)
% http://personales.upv.es/asala/YT/V/rcml6.html [PID musyn]
%
%
%
%
% Tabla de Contenidos
% 1.- Modelado de sistema incierto
% 2.- Simplificación de la incertidumbre a un único Delta
% 3.1- Diseño Hinfinito mixed sensitivity (sólo garantiza estabilidad robusta)
% 3.2- Diseño Hinfinito mixed sensitivity fijando estructura del regulador (PID)
% 4.1- Diseño iterativo multiplicador-controlador (mu-síntesis)
% 4.2- Diseño Musyn con PID estructurado
% Tabla resumen de resultados con diferentes opciones
%
%% 1.- Modelado de sistema incierto

%masa muelle amortiguador
M=2.5;
k=ureal('kmuelle',5,'Perc',20);
f=ureal('famortiguador',1,'Perc',30);
s=tf('s');
G=1/(M*s^2+f*s+k);
deltaact=ultidyn('deltaact',[1 1]);
Wdelta=makeweight(0.1,10,5);
sigma(Wdelta), grid on
act=8/(s+8)*(1+Wdelta*deltaact);
sigma(act), grid on
P=G*act;
P.Uncertainty
Pnominal=P.NominalValue;
zpk(Pnominal)
step(P), grid on
sigma(P,logspace(-1,1,150)), grid on
wcgain(P)





%% 2.- Simplificación de la incertidumbre a un único Delta

rng('default');% the random number generator is seeded for repeatability
NumSistemasPrueba=70;
muchosP=usample(P,NumSistemasPrueba);
step(muchosP), grid on
mPf=frd(muchosP,logspace(-1,3,60)); %frequency-response data model
[Pr,Info]=ucover(mPf,Pnominal,3);
Pr.Uncertainty
relerr = (Pnominal-muchosP)/Pnominal;

bodemag(relerr,'b--',Info.W1,'r',{0.1,1000}); grid on
TamanyoincertMultiplicativa=Info.W1;

[Pra,Infoa]=ucover(mPf,Pnominal,4,'Additive');
Pra.Uncertainty
adderr=Pnominal-muchosP;
bodemag(adderr,'b--',Infoa.W1,'r',{0.1,1000});, grid on
TamanyoincertAditiva=Infoa.W1;

%
%%
%% 3.1- Diseño Hinfinito mixed sensitivity (sólo garantiza estabilidad robusta)

%queremos que el control tenga un ancho de banda objetivo, lo más rápido...
w_objetivo=0.61; 
Werr=makeweight(40,w_objetivo,0.5);
sigma(Werr,[],1), grid on %inversa de Werr es plantilla de error (debe estar por debajo)
Wu=1/30; %que control no sature a 30 ante referencia 1.

%opción incertidumbre MULTIPLICATIVA 
GPNW=[1 0;0 1;0 0;1 0]+[0 -1;0 0;0 1;0 -1]*P;
GPW=blkdiag(Werr,Wu,TamanyoincertMultiplicativa,1)*GPNW*blkdiag(1,1);
tic
[K,CL,GAM,INFO]=hinfsyn(minreal(GPW.NominalValue),1,1);%robust stab garantizada si GAM<1
toc 
GAM
size(K)
CLinciertoHinfW=lft(GPW([1:2 4],:),K); %medida robust performance, lft con planta ponderada
%eliminamos fila 3 (salida r->y mixed sensitivity, porque GPW ya tiene la incertidumbre de P.
[stabmarg,wcu] = robstab(CLinciertoHinfW); %margen mayor de 1 --> estable. Mixed sens es conservativo por vectorizar y por incert. no estructurada.
stabmarg
[peorganancia,peorincert]=wcgain(CLinciertoHinfW);%peor caso de norma >1 --> no hay perf. robusta.
peorganancia

% Simulemos ahora el bucle diseñado.

CLinciertoHinf=feedback(P*K,1);%P es un modelo incierto
step(CLinciertoHinf,Pnominal/dcgain(Pnominal),8), grid on
title('Resp. Escalon SALIDA')
legend('Bucle cerrado','Bucle Abierto')
CL_u_incierto=feedback(K,P);
step(CL_u_incierto,tf(1/dcgain(Pnominal)), 8), grid on
title('Resp. Escalon CONTROL')
legend('Bucle cerrado','Bucle Abierto')

%



%% 3.2- Diseño Hinfinito mixed sensitivity fijando estructura del regulador (PID)

w_objetivo=0.69; 
Werr=makeweight(40,w_objetivo,0.5);

GPW2=blkdiag(Werr,Wu,TamanyoincertMultiplicativa,1)*GPNW*blkdiag(1,1);
%misma planta generalizada ponderada

tic
C0=tunablePID('ReguladorPID','pid')
TunableCL=lft(GPW2.NominalValue,C0);
opt = hinfstructOptions('Display','final','RandomStart',5);
[CL_PID,GAM,INFO] = hinfstruct(TunableCL,opt);
GAM
toc
PID_num=getValue(C0,CL_PID.Blocks);
tf(PID_num)
CLinciertoPIDW=lft(GPW2([1:2 4],:),PID_num); %medida robust performance, lft con planta ponderada
[stabmarg,wcu] = robstab(CLinciertoPIDW); %margen mayor de 1 --> estable. 
stabmarg
[peorganancia,~]=wcgain(CLinciertoPIDW);
peorganancia

%
% Pasamos a simular el bucle conseguido.

PIDfinal=getBlockValue(CL_PID,'ReguladorPID')
tf(PID_num)
CLinciertoPID=feedback(P*PID_num,1);
step(CLinciertoHinf,CLinciertoPID,9), grid on, legend("Hinf","PID-hinfstruct")

%
%% 4.1- Diseño iterativo multiplicador-controlador (mu-síntesis)

w_objetivo=0.68;
Werr=makeweight(40,w_objetivo,0.5);
GPNW=[1 0;0 1;1 0]+[0 -1;0 0;0 -1]*P;%No ponemos peso de incert. mulitiplicativa, ya está "dentro de P".
GPNW.InputName={'ref','u'};
GPNW.OutputName={'err','u_arbitro','err_copia'};
GPW=blkdiag(Werr,Wu,1)*GPNW*blkdiag(1,1);
GPW.Uncertainty

if(1) %esto tarda un monton y se raya... al menos en versiones antiguas (2017) de Matlab; las últimas (2022) van algo mejor.
tic
opt = musynOptions('MixedMu','on');
[Kmu_nored,wcGAM]=musyn(GPW,1,1,opt); %no necesito peso deltamultiplicativa, ya está todo en GPW.Uncertainty...
toc
wcGAM 
size(Kmu_nored)
wcgain(lft(GPW,Kmu_nored))
end

%

%con incertidumbre simplificada, más rápido en tiempo de cómputo
w_objetivo=0.65; 
Werr=makeweight(40,w_objetivo,0.5);

 

% Gastamos Pr en vez de P. No ponemos peso de incert. mulitiplicativa porque ya está en Pr y musyn debe estabilizarlo:

GPNWr=[1 0;0 1;1 0]+[0 -1;0 0;0 -1]*Pr;
GPWr=blkdiag(Werr,Wu,1)*GPNWr*blkdiag(1,1);
GPWr.Uncertainty
tic
[Kmu,wcGAM]=musyn(GPWr,1,1);
toc
wcGAM %es un escalado conjunto de incertidumbre y planta,
% no es la medida directa de robust performance (sí se garantiza si wcGAM<1).
size(Kmu)
CLinciertoW3r=lft(GPWr,Kmu);%con la planta "simplificada" de una incertidumbre.
wcgain(CLinciertoW3r)
GPNW=[1 0;0 1;1 0]+[0 -1;0 0;0 -1]*P;
GPW=blkdiag(Werr,Wu,1)*GPNW*blkdiag(1,1);
CLinciertoW3=lft(GPW,Kmu);%con la planta "original" de muelles, etc... 3 bloques de incertidumbre
robstab(CLinciertoW3)
wcgain(CLinciertoW3)

%


CLincierto3=feedback(P*Kmu,1);
step(CLinciertoHinf.NominalValue,CLinciertoPID.NominalValue,CLincierto3.NominalValue,7), grid on
title('Respuestas nominales de diseños con robust perf. garantizada')
legend('detuned hinfsyn','detuned pid-hinfstruct','dksyn')
step(CLinciertoHinf,CLinciertoPID,CLincierto3,7), grid on
legend('detuned hinfsyn','detuned pid','musyn')

%
%% 4.2- Diseño Musyn con PID estructurado
% Dado que \mu-síntesis es una metodología que requiere optimización no convexa (contrariamente a hinfsyn, que sí era optimización convexa), también su código puede incorporar plantas generalizadas + controlador con estructura prefijada

w_objetivo=0.82;
Werr=makeweight(40,w_objetivo,0.5);
GPW=blkdiag(Werr,Wu,1)*GPNW*blkdiag(1,1);
tic
TunableCL2=lft(GPW,C0); %la misma que para el ejemplo hinfstruc, y musyn
opt = musynOptions('MixedMu','on');
[CL,WcGAM2]=musyn(TunableCL2,opt)
toc
wcgain(CL)
regefinal=pid(CL.Blocks.ReguladorPID)

%

w_objetivo=0.6;
Werr=makeweight(40,w_objetivo,0.5);
GPW=blkdiag(Werr,Wu,1)*GPNWr*blkdiag(1,1); %con incertidumbre reducida
tic
TunableCL2=lft(GPW,C0); %la misma que para el ejemplo hinfstruc
opt = musynOptions('MixedMu','on');
[CLr,WcGAM2]=musyn(TunableCL2,opt)
toc

%

regefinal_r=pid(CLr.Blocks.ReguladorPID)
wcgain(CLr)
bcms=lft(GPNW,regefinal);
step(CLincierto3.Nominalvalue,1-bcms.NominalValue(1),8), grid on, legend('musyn','musynPID')
step(CLincierto3,[1;0]-bcms,8), grid on, legend('musyn','musynPID')
bodemag(1-CLincierto3,bcms,logspace(-2,2,200)), grid on

%% Tabla resumen de resultados con diferentes opciones
% Resumen ancho banda para garantizar prestaciones robustas (wcgain ponderado < 1, incertidumbre no simplificada)
% Detuned hinf: 0.43 [en video rcml3, Matlab 2018a]      0.61 [Matlab R2022b]
% Detuned PID hinfstruct: 0.66 [en video rcml4, Matlab 2018a]    0.69 [Matlab R2022b]
% Musyn (orden 107, incert. completa): 0.68
% Musyn (orden 24, incert. simplificada):  0.65
% Musyn PID (incert. completa): *0.82*
% Musyn PID (incert. simplificada): 0.6
%
% ¿Diferencias entre versiones? Por corrección de errores o que haya tocado en planta o planta ponderada yo algo que no me acuerde.
% Como musyn no es "optimización convexa" (mínimo único, eficiente de calcular), pues dependiendo de opciones, inicializaciones, etc. se llega a unos mínimos locales u otros.
% Lo mejor ha acabado siendo el musyn PID (0.82). De todas formas, no hay ninguna diferencia "espectacular" en respuesta temporal. Subir mucho orden de multiplicadores parece que lía/baja condicionamiento numérico, todo es opciones por defecto, no hay cambios en orden de multiplicadores, tolerancias, etc.