%% Equivalencia dLQG (dLQR+filtro Kalman) con control óptimo H2
% (c) 2018, Antonio Sala Piqueras, Universitat Politécnica de Valencia. Todos los derechos reservados.
% Presentación en vídeo en: http://personales.upv.es/asala/YT/V/h2lqrml.html
%
% Nota: los resultados de h2syn con versiones anteriores a R2017b son "peores" que R2017b en sistemas discretos, según consta en https://es.mathworks.com/help/robust/release-notes-R2017b.html
% Este código, por tanto, debe ejecutarse en esa versión de Matlab (robust control toolbox) o posterior para que los resultados sean coincidentes con los aquí mostrados.
% Tabla de Contenidos
% Modelos
% 1 Control con regulador óptimo-cuadrático + filtro de Kalman (observador óptimo)
% 2 Control por planta generalizada ponderada
% 3 A la inversa, dada la planta generalizada, resolver h2syn mediante dlqe, dlqr
% Comparación con diseños anteriores (LQG inicial, h2syn)
%

%matrices representación interna discreta
A=[.9 .5 -.4;0 -.2 -.8;-.5 -.4 -.3]; 
eig(A)%es inestable
B_u=[1;1;.9]; %entradas manipuladas "u"
B_pert=[-1;0;1]; %entradas ruido de proceso
C1=[1 .5 0;0 0 .8]; %variables controladas "z"
C2=[0 .5 .5]; %variables medidas "y"
Ts=0.1; %período de muestreo

%% Modelos

planta_small=ss(A,B_u,C2,0,Ts); %lo que "ve" el controlador.
planta_completa=ss(A,[B_pert B_u],[C1; C2],0,Ts); %todas las entradas y salidas...
%como falta el "ruidos de medida", aún no es la planta generalizada
%"buena".

% Ninguna de estas dos se gastan para nada... son sólo por ilustración...
%% 1 Control con regulador óptimo-cuadrático + filtro de Kalman (observador óptimo)

%objetivo de control dLQR: minimizar J=x'*C1'*Qy*C1*x+u'*R*u
Qy=diag([9 1]); R=2;
Qx=C1'*Qy*C1
K=dlqr(A,B_u,Qx,R)
%para Filtro de Kalman
DesvTRmed=0.05;
DesvTRproc=0.1;
L=dlqe(A,B_pert,C2,DesvTRproc^2,DesvTRmed^2)
%formemos el controlador por realimentación de la salida, controlador + observador
[Ac,Bc,Cc,Dc]=dreg(A,B_u,C2,0,K,L);
todojunto=ss(Ac,Bc,Cc,Dc,Ts);
zpk(todojunto)

%% 2 Control por planta generalizada ponderada

%Planta generalizada NO ponderada como antes + ruido de medida
sysg=ss(A,[B_pert zeros(3,1) B_u],[C1; zeros(1,3);C2],[zeros(2,3);zeros(1,2) 1;0 1 0],Ts); 
sysg.InputName={'ruido_proc','ruido_med','u'}; sysg.OutputName={'z1','z2','u','medida'};
size(sysg)
%Pesos de salidas grupo 1 (para LQR)
raizQ=chol(Qy);raizR=chol(R);%como son diag, es raíz cuadrada
Wout=blkdiag(raizQ,raizR)
size(Wout)
%Pesos de entrada grupo 1 (para Kalman)
Win=blkdiag(DesvTRproc,DesvTRmed)
size(Win)
plantagenPond=blkdiag(Wout,1)*sysg*blkdiag(Win,1);%ahora ya ponderada.
%diseñemos control optimo H2:
[controlador,CL,GAM,~]=h2syn(plantagenPond,1,1);
zpk(controlador)
%polos de bucle cerrado
eig(CL)' %son los seis de la dinámica de la planta+controlador, dado que los pesos son constantes.
%desv típica total
GAM
%desv típica de las SALIDAS CONTROLADAS (C1)
dtz=norm(CL(1:2,:),2)
%desvtípica de la ACCIÓN DE CONTROL
dtu=norm(CL(3,:),2)

%% 3 A la inversa, dada la planta generalizada, resolver h2syn mediante dlqe, dlqr

% Esto sería una implementación alternativa a la "nativa" de Matlab de h2syn
% datos, plantagenPond, nsens, nact
PG=plantagenPond;%por abreviar
nsens=1; nact=1;
[nsalidas,nentradas]=size(PG)
%aqui me quedo...
nsalidasgrupo1=nsalidas-nsens
indice_salidas1=1:nsalidasgrupo1
indice_salidas2=(nsalidasgrupo1+1):nsalidas
nentradasgrupo1=nentradas-nact
indice_entradas1=1:nentradasgrupo1
indice_entradas2=(nentradasgrupo1+1):nentradas

% Paso 1: recuperamos el problema LQR de PG, con ponderación identidad:

trozoC=PG.c(indice_salidas1,:)
trozoD=PG.d(indice_salidas1,indice_entradas2)
Q2=trozoC'*trozoC
%que es igual que Qx, recuperada de PG...
R2=trozoD'*trozoD
%que es igual que R
N2=trozoC'*trozoD
%que en este caso es cero, por la particular forma del problema, pero vale en caso general...
K2=dlqr(PG.a,PG.b(:,indice_entradas2),Q2,R2,N2)

% Paso 2: recuperamos el problema de observación de PG, con varianza ruidos identidad...

W=eye(nentradasgrupo1); %la desviación típica ya está en Bpert...
%recuperamos ruido de medida
trozoD1=PG.d(indice_salidas2,indice_entradas1)
%como a priori una misma perturbación podría afectar estado y medidas, gastamos una 
% versión de dlqe que lo permite, ver ayuda.
L2=dlqew(PG.a,PG.b(:,indice_entradas1),PG.c(indice_salidas2,:),trozoD1,W,zeros(nsens))

% Paso 3: formemos el regulador completo

[Ar2,Br2,Cr2,Dr2]=dreg(PG.a,PG.b(:,indice_entradas2),PG.c(indice_salidas2,:),PG.d(indice_salidas2,indice_entradas2),K2,L2);
controlH2=ss(Ar2,Br2,Cr2,Dr2,Ts); %H2syn terminado.
zpk(controlH2)

%% Comparación con diseños anteriores (LQG inicial, h2syn)

%que es el mismo que había sacado h2syn
zpk(todojunto)

zpk(controlador)