%% Análisis de Robustez de un bucle de control de dos entradas y dos salidas ante incertidumbre aditiva "estructurada" LTI, ideas preliminares
% © 2020, Antonio Sala Piqueras, Universitat Politècnica de Valencia. Todos los derechos reservados.
% Presentación en vídeo: http://personales.upv.es/asala/YT/V/IncAd2s.html
% Este código funcionó correctamente con Matlab R2020a
% Objetivo: comprender cómo usar las fórmulas de análisis de robustez ante incertidumbre aditiva en un caso multivariable.
% Tabla de Contenidos
% 1. Modelado y estimación de error de modelado
% 2. Diseño Nominal (PID multibucle)
% 3. Análisis de robustez por pequeña ganancia (inc. aditiva no estructurada)
% 4. Reducción de conservadurismo (MIMO -- estructurado)
% 4. Respuesta temporal
% Conclusiones
%
%% 1. Modelado y estimación de error de modelado
% Consideremos el proceso:

s=tf('s');
G=[6.7/(s+1)^2  4/(0.8*s+1)^2; 
   -13/(s+1)^2 -2.7/(0.5*s+1)^2];

% Y consideremos la posibilidad de que el proceso real esté en determinado intervalo de incertidumbre alrededor de él:

rng(12323)
randinter=@(low,up) low+rand()*(up-low);
Ntests=80;
for i=1:Ntests
   tau1=randinter(0.9,1.1);
Greal{i}=[randinter(6.5,6.9)/(tau1*s+1)^2  randinter(3.8,4.2)/(randinter(0.75,0.85)*s+1)^2; 
         -randinter(12,14)/(tau1*s+1)^2 -randinter(2.5,2.9)/(randinter(0.45,0.55)*s+1)^2] ...
         +randn(2)*.09*s/(s+10);
      errormod{i}=G-Greal{i};
      normerr{i}=norm(errormod{i},inf); %estimamos el tamaño del error
end
cotaerrmod=max([normerr{:}])
sigma(Greal{:}), grid on
sigma(G,errormod{:}), grid on

%% 2. Diseño Nominal (PID multibucle)
% Comprobamos condicionamiento y diseñamos PIDs por multibucle (RGA), como es 2x2 no hace falta Niederlinski

gan=dcgain(G)
svd(gan)
cond(gan)
%sigma(G), grid on
rga=gan.*inv(gan')
K=[0 -(.9+.2/s); .9+.3/s 0]; 
bucley=feedback(minreal(ss(G*K)),eye(2));
step(bucley,15), grid on

%% 3. Análisis de robustez por pequeña ganancia (inc. aditiva no estructurada)
%

bucleu=feedback(K,G); %ref->u %INCERT ADITIVA
nunu=norm(bucleu,inf) 

% Cota "pequeña ganancia":

tamanyomaximodeltaaditiva=1/nunu %estimado "pesimista", ante cualquier no-linealidad (distorsión armónica) o sistmea variante en el tiempo
sigma(inv(bucleu),tf(tamanyomaximodeltaaditiva)),grid on %el proceso puede ser cualquier cosa por debajo de ese "suelo" a altas frecuencias.

% Cota "pequeña ganancia" ante incertidumbre LTI (vale frecuencia a frecuencia):
% Dibujemos las cotas de error de modelado aditivo (LTI y no-lineal o variante en el tiempo):

w=logspace(-2,2,80);

% La cota del teorema de pequeña ganancia se refiere a la inversa de la ganancia máxima (= ganancia mínima de la inversa). Resaltemos sólo ese valor singular:

[sv]=sigma(inv(bucleu),w);
minsv=sv(2,:)';
hold on
plot(w,20*log10(minsv),'r','LineWidth',4)
hold off

% Verifiquemos si el error de modelado de la familia de procesos lo cumple o no:

sigma(errormod{:},w),grid on 
hold on
plot(w,20*log10(minsv),'r','LineWidth',4)
yline(20*log10(tamanyomaximodeltaaditiva),'m','LineWidth',3)
hold off

%
%% 4. Reducción de conservadurismo (MIMO -- estructurado)
% El error de modelado "estructurado" puede tener más incertidumbre en ciertas direcciones que en otras, no es sólo una "bola", sino una especie de "elipse"... del mismo modo, la tolerancia a error de modelado es mayor en unas direcciones que en otras (inv(bucleu) tiene ganancia "mínima" -- la única hasta ahora considerada-- y "máxima").
% Si es LTI, pequeña ganancia puede reformularse a: estabilidad si \|\Delta(jw)\cdot K(I+GK)^{-1}\|_\infty\leq 1 (consecuencia crit. Nyquist det(I+\Delta\cdot K(I+GK)^{-1})\neq 0 para toda w). Obviamente, eso se cumple si \|\Delta(jw)\|_\infty\cdot \|K(I+GK)^{-1}\|_\infty\leq 1, que es lo que hemos comprobado arriba, suponiendo \Delta no estructurada...
% En nuestro caso, que es "estructurada" (paramétrica), podemos hacer el producto completo:

peorcaso=0;
for i=1:Ntests
    cosa{i}=errormod{i}*bucleu;
    peorcaso=max(peorcaso,norm(cosa{i},inf));
end
sigma(cosa{:},tf(1)),grid on
peorcaso

% Vemos que está todo por debajo de los 0dB: garantizamos estabilidad, porque por donde se "salía" era en una dirección en la que se toleraba más error.
% La estabilidad está garantizada para cada una de las plantas simuladas... ¿Para plantas "entre medio"? ¿Para resto de plantas no producidas por el dado?
% Comprobar que para todas las frecuencias, y para todo \Delta\in\mathbf{\Delta}, siendo \mathbf{\Delta} un conjunto de plantas (LTI) posibles (infinito, y no "finito" como hemos explorado aquí) se verifica det(I+\Delta\cdot K(I+GK)^{-1})\neq 0 dio origen a la teoría del "valor singular estructurado \mu", fuera de los objetivos de este material.
%
%% 4. Respuesta temporal

[U,S,V]=svd(gan);
for i=1:Ntests
   bcreal_y{i}=feedback(minreal(ss(Greal{i})*ss(K),[],false),eye(2));
end
step(bcreal_y{:},15), grid on

%% Conclusiones
% Hemos comprobado cómo estimar el error de modelado ante una familia de sistemas de 2 entradas y 2 salidas y cómo obtener márgen de robustez antes incertidumbre aditiva (cualquiera o LTI no estructurada). De todos modos, las fórmulas son conservativas ante incertidumbre estructurada (como la incertidumbre "paramétrica" que hemos insertado), y superar los umbrales no implica que "seguro" haya inestabilidad: el teorema de pequaña ganancia sólo asegura que si NO se superan los umbrales entonces el bucle cerrado será ESTABLE.
% En este material, en particular, se observa que en el caso concreto bajo estudio, fórmulas de pequeña ganancia \|\Delta(jw)\cdot K(I+GK)^{-1}\|_\infty\leq 1 permiten asegurar estabilidad en bucle cerrado ante esta familia de plantas LTI. Relajar el teorema de pequeña ganancia (peq. ganancia escalado, pasividad, IQC, ...) fue, por tanto, un importante objeto de estudio del control robusto.
%
%
%
%