%% Planta Generalizada: ejemplo análisis bucle cerrado
% © 2021, Antonio Sala Piqueras. Universitat Politècnica de València. Todos los derechos reservados.
% Presentación en vídeo: http://personales.upv.es/asala/YT/V/pgdydu3.html .
% Este código ejecutó sin errores en Matlab R2021b
% Objetivo: Construir planta generalizada rechazo perturbaciones entrada y salida. Simular un regulador tipo PID para un sistema con retardo y analizar sus prestaciones en tiempo y frecuencia.
% Tabla de Contenidos
% Planteamiento del problema de control como planta generalizada
%     Planta Generalizada en forma matriz (sin multiincidencia)
% Ejemplo análisis respuesta en bucle cerrado (lft)
%
%% Planteamiento del problema de control como planta generalizada
%
% Las entradas generalizadas son ( d_y , d_u , u); las salidas generalizadas ( y , u  , y_m ). No vamos a "cerrar" el bucle como hemos hecho con el symbolic: la planta generalizada es en "bucle abierto"... se cerraría con lft.
%% Planta Generalizada en forma matriz (sin multiincidencia)
% A partir de las ecuaciones y=d_y+G\cdot (u+d_u), esto es y=1\cdot d_y+G\cdot d_u+G\cdot u, e integrando y_m=-y, codificamos en forma de matriz:
% \left(\begin{array}{c}y\\ u \\y_m\end{array}\right)=\left(\begin{array}{c}1&G&G\\ 0&0&1 \\-1&-G&-G\end{array}\right)\cdot \left(\begin{array}{c}d_y\\ d_u \\u\end{array}\right)
% Debemos evitar escribir el proceso muchas veces, porque cada vez que se escribe el toolbox hace una "copia" y aumenta el número de estados.

PGbuildMatrix=@(G)   [1 0 0; 0 0 1; -1 0 0] + [1; 0; -1]*G*[0 1 1];
syms G K
PGsym=PGbuildMatrix(G) %con G "simbólico" sale lo que debe salir
s=tf('s');
Proceso=ss(1/s^2)*exp(-0.4*s); %Aquí vamos a poner retardo, por cambiar...
PG=PGbuildMatrix(Proceso);
PG.InputName={'dy','du','u'}; %para que step, bode, etc. salgan etiquetados
PG.OutputName={'y','u','ym'}; %para que step, bode, etc. salgan etiquetados
size(PG)
%PG

% Ecuaciones en bucle cerrado:
% Si hubiera un controlador " K" y consideráramos una planta "normal y corriente G" y un bucle "de toda la vida, tendríamos que hacer, por ejemplo, "feedback":
%
% BC_dy_y=feedback(1,G*K); %Sensibilidad S
% BC_dy_u=feedback(K,G); % K\cdot S (bueno, en este caso -KS. sale +KS de ref. a u )
% BC_du_y=feedback(G,K); % G\cdot S
% BC_du_u=feedback(G*K,1); % T=1-S, Sensibilidad complementaria (bueno, en este caso -T, sale +T de ref. a salida y_limpia)
% Pero podemos hacer las cuatro con una línea de código:

simplify(lft(PGsym,K))

% Y, obviamente, con la Planta Generalizada adecuada, podemos hacer cualquier interconexión asociada a cualquier problema de control lineal multivariable, en la gran mayoría de casos.
%
%% Ejemplo análisis respuesta en bucle cerrado (lft)
% Vamos a ver la respuesta en bucle cerrado ante ambas perturbaciones de un PID, probando dos sintonías diferentes.

Kp=1.5; Kd=1.3; Ki=0.3; tau_filtroruido=0.13;
K1=Kp + Kd*s/(tau_filtroruido*s+1) + Ki/s;
BC=lft(PG,K1);
size(BC)

Kp=1; Kd=1.05; Ki=0.2; tau_filtroruido=0.08;
K2=Kp + Kd*s/(tau_filtroruido*s+1) + Ki/s;
BC2=lft(PG,K2);
size(BC2)

% Al tener retardos, tiene "infinitos" polos... aproximamos con Padé para ver una aproximación a unos poquitos:

popo=eig(pade(BC)) %1er orden exp(-ds) approx. (1-ds/2)/(1+ds/2)
popo=eig(pade(BC,2)) 
popo=eig(pade(BC,3)) 
t_est_98=4/min(-real(popo))
popo2=eig(pade(BC2,3)) %al tener retardos, tiene "infinitos" polos... aproximamos con Padé.
t2_est_98=4/min(-real(popo2))
step(BC,BC2,round(t_est_98*1.5)), grid on
step(BC(2,1),BC2(2,1)), grid on
bodemag(BC,BC2,logspace(-1.5,1.5,200)), grid on