%% Control en cascada con sensor extra (H infinito)
% (c) 2018, Antonio Sala Piqueras. Universitat Politècnica de València. Todos los derechos reservados.
% Presentación en vídeo en http://personales.upv.es/asala/YT/V/cashi.html
%
% Consideremos el diagrama de bloques:
%
% Objetivo: comparar las prestaciones alcanzables con 1 sensor (e) o 2 sensores (y1,e).
% Sólo existen requerimientos de control sobre la variable "e". También se desea limitar en lo posible la actividad de u.
% Planta a controlar:

s=tf('s');
G1=10/(s+10); %subsistema rapido para "cascada" antes 6
G2=6/((s+1)*(0.25*s+1)); %planta "lenta"

%% Planta Generalizada:
% Suponiendo el uso de ambos sensores, añadiendo "u" a las salidas generalizadas, tenemos:
%

GenP=[G2 tf(1) G2*G1;... %salida "e"
      tf([0  0  1]);...   %Control para ser pesado
      tf([-1  0])   G1;...  %y1
      G2 tf(1) G2*G1];  %copia de e
size(ss(GenP))
GenP=minreal(ss(GenP)); %eliminar apariciones múltiples de G1,G2.
size(GenP) %sigue siendo orden 3, como G1 y G2.
GenP.inputname={'delta', 'p','u'};
GenP.outputname={'e','u','y1','copiadee'};

% Si sólo tuviéramos el sensor de "e", quitaríamos la tercera fila:

GenP1s=GenP([1 2 4],:);
GenP1s.Outputname

%
%% Planta Generalizada Ponderada, diseño control óptimo Hinfinito:
% Conseguir ancho de banda máximo sobre error, sin saturar (a 1) acción de control

% requisitos de control
errpos=0.01;
anchobanda=2.007; %relacionado con "tiempo de subida", corte por 0dB. antes 1.833
pico_res=2; %relacionado con sobreoscilación.
We=makeweight(1/errpos,anchobanda,1/pico_res);Wu=tf(1);
sigma(1/We,logspace(-3,2)), grid on %plantilla del peso error

% cuando las entradas son senoidales con amplitud por debajo de:

Wp=1;
Wdelta=0.35/(s/10+1)+.04;

Wout=blkdiag(We,Wu,1,1);%con 2 sensores
Win=blkdiag(Wdelta,Wp,1); %peso generalizado entradas
GenPW=minreal(Wout*GenP*Win);
[K2sens,~,Gam2,~]=hinfsyn(GenPW,2,1);
Gam2
anchobanda1s=0.993; %relacionado con "tiempo de subida", corte por 0dB.
We1s=makeweight(1/errpos,anchobanda1s,1/pico_res);

Wout1s=blkdiag(We1s,Wu,1);% con 1 sensor
GenPW1s=minreal(Wout1s*GenP1s*Win);
[K1sens,~,Gam1,~]=hinfsyn(GenPW1s,1,1);
Gam1
mejora2s=anchobanda/anchobanda1s %porcent. rapidez con sensor extra.
%la respuesta temporal "exactamente optimizada" sería con entradas senoidales
%pero a "ingenieros" y "electricistas" nos gusta ver la respuesta ante escalón.
CLSinPeso1s=lft(GenP1s,K1sens);
CLSinPeso2s=lft(GenP,K2sens);
step(CLSinPeso2s,CLSinPeso1s,4), grid on
%resp. escalón pasado "delta" por filtro paso bajo... se supone que no varía "rápido"
% en la optim: si varía "rápido" entonces saturará "u".
CL1b=CLSinPeso2s*blkdiag(Wdelta,Wp);
CL2b=CLSinPeso1s*blkdiag(Wdelta,Wp);
step(CL1b,CL2b,4), grid on
legend('2 sensores','1 sensor')
%y los espectros en frecuencia esperables en error y acción de control...
sigma(CL1b(1,:),CL2b(1,:),1/We,1/We1s,logspace(-3,1.6)), grid on
legend('2 sensores','1 sensor','filt.2 sens','fil.1 sens','Location','best')
axis([2e-3 4e1 -42 7]) 
sigma(CL1b(2,:),CL2b(2,:)), grid on
legend('2 sensores','1 sensor')
axis([2e-3 4e1 -9 0]) 