%% Planta caso 3x3, análisis propiedades
% (c) 2021, Antonio Sala, Universitat Politécnica de Valencia. Todos los derechos reservados.
% Presentación en vídeo: http://personales.upv.es/asala/YT/V/props3x3.html
% Este código funcionó con Matlab R2020b
% Objetivo: analizar propiedades de una planta 3x3, estimar límites de prestaciones alcanzables, estimar posibilidad de reducir orden de representación interna para tener controladores menos complejos posteriormente.
% Tabla de Contenidos
% Planta a controlar
% Polos
% Análisis de controlabilidad/observabilidad del estado
% Análisis de controlabilidad entrada/salida
% Ceros
%% Planta a controlar
% Modelo:

s=tf('s');
G=[ 4/(s+1)^2    3/(s+2)   3.1/(s+2);
    4/(s+1.8)   4.5/(s+1.5) 4.0/(s+1.5);
    0.6/(s+1.8)   0.75/(s+1.5)  1.2/(s+1.5)];

% Asumiremos que G ya está escalada entradas y salidas a +/- 1, con lo que no necesitamos contemplar unidades "físicas" originales. La matriz G ha sido obtenida por identificación experimental, y no suponemos ningún significado físico a los estados asociados a los polos.
% Hay 10 polos en los denominadores de la MdT, pero algunos repetidos: hagamos realización mínima y trabajemos en espacio de estados (sin significado físico de los mismos, claro).

Gss=minreal(ss(G));n=size(Gss.A,1)

%% Polos

eig(Gss)

% Coinciden con los de la MdT, pero sin repeticiones innecesarias.
%% Análisis de controlabilidad/observabilidad del estado
% Como el estado de Gss no tiene significado físico, tampoco lo tiene analizar controlabilidad u observabilidad por separado. El "estado" es un "artefacto" para explicar las entradas y salidas experimentales.
% En realización equilibrada controlabilidad = observabilidad, al menos eso sí tiene cierto sentido.

[Gbal,hank]=balreal(Gss);
bar(hank), grid on

% ¿Podría ser suficiente con manejar una planta de orden 4 para diseño de controladores?

Gred=balred(Gss,4); 
step(G,Gred), grid on
sigma(G,Gred,G-Gred), grid on, ylim([-70 20]), legend("G","Gred","error modelado aditivo")

%% Análisis de controlabilidad entrada/salida
% Objetivo: conseguir \|y\|_2\geq 1 en todas direcciones con acciones de control que verifiquen \|u\|_2\leq 1

sigma(Gss,tf(1),'r:'),grid on, legend("svd G(j\omega)","0 dB")
axis([1e-2 1e2 -40 20])
semiperiodo_masrapido_maniobras=[pi/8.1 pi/1.8] %y otra que NO se puede controlar mucho...
diferencia_rapidez_maniobras=8.1/1.8

% Atención: tiempo de establecimiento ( \approxsemiperído) es un estimado muy "aproximado" ( \bullet quizás en vez de tiempo de establecimiento sea tiempo de subida,  \bulletsenoidal vs. escalón \bullet sin tener en cuenta retrasos por observador con mala relación señal/ruido,  \bullet retardos en el proceso --fase no se ve en "sigma"--   \bullet ceros de fase no mínima --ver abajo--): un controlador causal puede acabar dando unas prestaciones mucho menores.
% Estudiemos en detalle la ganancia estática:

[U,S,V]=svd(dcgain(G))

% Con este cambio de variable, la planta "parece" desacoplada al menos en ganancia (esto dará lugar al desacoplamiento SVD como metodología de diseño, válido incluso para plantas no cuadradas):  G(0)=USV^T

G_desacsvd=U'*Gss*V;
dcgain(G_desacsvd) %es "S"
step(G_desacsvd), grid on
title("Step Response (componentes principales ganancia)")
step(G_desacsvd(1,1),G_desacsvd(2,2),G_desacsvd(3,3)), grid on,legend("C.p. 1","C.p. 2","C.p. 3")
title("Step Response (componentes principales, diagonal)")
step(G_desacsvd(1,1),G_desacsvd(2,2),G_desacsvd(3,3)), grid on, yline(1,'-.r')
legend("C.p. 1","C.p. 2","C.p. 3","objetivo amplitud 1","location","best")
title("Ampliación resp. comp. principales"), axis([0 2.25 -.25 1.2])

% *Provee de otro estimado de rapidez (lo más rápido saturando a norma 1 actuadores), optimista porque "hay que frenar". Podría dar un tiempo de subida orientativo de estas maniobras.
%
% Conclusiones del análisis sigma/svd
% No se pueden controlar tres "cosas", pero dos sí. Aparte de revisar especificaciones (rangos de y) o potencias de actuadores (rangos de u) en el proyecto de control (cambiar escalados), podremos considerar varias opciones:
% Controlar sólo dos salidas (ventaja: mantenemos significado "simple" en vez de combinaciones de entradas y salidas; quizás podamos hacer un control sencillo tipo PID descentralizado, cascada, desacoplamiento; desventaja: nos olvidamos del resto de cosas que podrían desviarse, y somos subóptimos porque no utilizamos el subespacio de "mayor ganancia" de G); QUITAR UNA FILA de G.
% Basarse en el SVD para diseñar el control, esto es, controlar G_desacsvd (desacoplamiento SVD, ventaja: usamos PIDs y igual funciona). Sólo habría que controlar DOS bucles (las dos maniobras >0 dB ganancia).
% Enfoques parecidos al desacoplamiento SVD, con control óptimo \mathcal H_2 o \mathcal H_\infty incorporando las matrices resultado del SVD en los índices a optimizar.
%% Ceros
% La planta tiene un cero de fase no mínima, que limitará las prestaciones alcanzables:

zz=tzero(Gss)
H1=evalfr(Gss,zz(1))
[Uz,Sz,Vz]=svd(H1)
step(-Uz(:,3)'*Gss*Vz(:,3)), grid on

% Controlar en "menos de 2 segundos" cambios de referencia en esa dirección no sería aconsejable.