%% Compromiso Especificaciones-Robustez (Matlab): márgenes, comparación mixsyn, ncfsyn
% © 2019 Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Presentacion en video en http://personales.upv.es/asala/YT/V/c3pidmx.html y http://personales.upv.es/asala/YT/V/c3pidncf.html
%
% Este código ejecutó correctamente en Matlab R2018b
% Tabla de Contenidos
% 1.- Modelado
% 2.-Control PID, como punto de partida
%     Márgenes de robustez (tma pequeña ganancia, incert. no estructurada) de los PIDs
% 3- Comparativa con diseño MIXED SENSITIVITY (mixsyn)
% 4- Comparativa con diseño Glover-McFarlane loop shaping (ncfsyn)
%%
%% 1.- Modelado
% Modelo Nominal

k=1; c=1; M=1; s=tf('s');
G=1/(M*s^2+c*s+k);

%
%% 2.-Control PID, como punto de partida
% K(s)=K_P+K_D\cdot \frac{s}{\tau_f s+1}+K_I\cdot \frac{1}{s}

%PID de "medias prestaciones" t_est=1.5 s
K{1}=1.6+1.6*s/(0.025*s+1)+1.6/s;
%PID "bueno nominalmente" t_est=0.3 s
K{2}=20+20*s/(0.015*s+1)+20/s;
%PID más lento pero bastante más robusto (y menos amplif. ruido alta frec)
K{3}=0.9+0.8*s/(0.3*s+1)+1.1/s;


for i=1:3
    S{i}=minreal(1/(1+G*K{i}));
    T{i}=1-S{i};
    KS{i}=K{i}*S{i};
end
step(T{1},T{2},T{3},G/dcgain(G)), grid on
legend('k1','k2','k3','bucle ab.','Location','SouthEast')

% Tenemos que analizar el compromiso prestaciones-robustez...
%
%% Márgenes de robustez (tma pequeña ganancia, incert. no estructurada) de los PIDs
% Multiplicativa G_r=G(1+\Delta)

[1/norm(T{1},'inf') 1/norm(T{2},'inf') 1/norm(T{3},'inf')]

% Aditiva G_r=G+\Delta

[1/norm(KS{1},'inf') 1/norm(KS{2},'inf') 1/norm(KS{3},'inf')]

% Incert. coprima normalizada (equivalentemente mixta, un poco de todo por todas partes: mult., ad., directa e inversa...)

[ncfmargin(G,K{1}) ncfmargin(G,K{2}) ncfmargin(G,K{3})]

%
%% 3- Comparativa con diseño MIXED SENSITIVITY (mixsyn)

%disenyohinf='lento';compararcon=3; %ejemplo 1, comparar con el PID K{3} más lento pero más robusto
disenyohinf='intermedio';compararcon=1; %ejemplo 2: buscar algo más de rapidez, sin sacrificar robustez como el PID K{1}
if(strcmp(disenyohinf,'lento'))
    bw=5.46;
    W1=0.2*(s+bw)/(s+bw/300);%el diseño más rápido para u=3 saturación...
    W2=tf(0.333);%KS<1/W2
else
    bw=7.76; %comparable ancho de banda a K{1} original... pero más robusto.
    W1=0.2*(s+bw)/(s+bw/300);%diseño sqrt(2) más rapido, con u=3*2 saturación...
    W2=tf(0.167);%KS<1/W2    
end
W3=tf(0.2);%T<1/W3, igual en ambos diseños
[Kms,~,GAM,~]=mixsyn(G,W1,W2,W3,hinfsynOptions('RelTol',1e-3));
GAM
Sms=minreal(1/(1+Kms*G));
Tms=1-Sms;
step(Tms,T{compararcon},8), grid on, legend('mixsyn',['PID ' int2str(compararcon)])
KSms=minreal(Kms*Sms); 
step(KSms,KS{compararcon},3), grid on, legend('mixsyn',['PID ' int2str(compararcon)]), axis([0 3 -2 10]), title('Step referencia a control')
sigma(Sms,S{compararcon},1/W1), grid on, title('Sensibilidad (ref->error)'), legend('mixsyn',['PID ' int2str(compararcon)],'1/W1')
sigma(Tms,T{compararcon},1/W3), grid on, title('Sensibilidad compl. (ref->salida)'), legend('mixsyn',['PID ' int2str(compararcon)],'1/W3')
sigma(KSms,KS{compararcon},1/W2), grid on, title('Sensibilidad control (ref->control)'), legend('mixsyn',['PID ' int2str(compararcon)],'1/W2')
[ ncfmargin(G,Kms) ncfmargin(G,K{compararcon})]
loscuatro=@(G,K) minreal([1;K]*(1/(1+G*K))*[G 1]);
sigma(loscuatro(G,Kms),loscuatro(G,K{compararcon}))
axis([0.1 500 -10 40]), grid on, grid on, title('Sensibilidad incert. coprima normalizada'), legend('mixsyn',['PID ' int2str(compararcon)])
zpk(Kms)

% Lo expresamos, en el caso del diseño 1, en forma PID (se puede porque el regulador es de orden 3 con un polo muuuy rápido y otro muy lento que se eliminan y aproximan por integrador, respectivamente; en otros casos habría que "aproximar" a un PID con, por ejemplo, reducción modal):

%para el caso diseño lento, expresar como PID:
if(strcmp(disenyohinf,'lento'))
    Kmsaprox=(2793.1/949.5)*(s^2+s+1)/(s+2.527)/s
    disp('Margen de robustez:'), ncfmargin(G,Kmsaprox)
    %Sfinal=minreal(1/(1+G*Kmsaprox));
    %figure(), step(1-Sfinal), grid on
    %figure(), step(minreal(Kmsaprox*Sfinal)), grid on
    [R,P,D] = residue(Kmsaprox.num{:},Kmsaprox.den{:});
    K_proporcional=D-R(1)/P(1), K_integral=R(2), K_deriv=(-D+K_proporcional)/P(1)
    PID=K_proporcional + K_deriv*s/(s/(-P(1))+1) + K_integral/s    
end

%
%% 4- Comparativa con diseño Glover-McFarlane loop shaping (ncfsyn)

if(strcmp(disenyohinf,'lento')) %vamos a intentar un diseño "comparable" a los mixsyn
    bwn=1.1;
    Wnf=1.6*(bwn+s)/s;
else
    bwn=0.6;
    Wnf=3.4*(bwn+s)/s;
end
%Wnf=tf(1)
bodemag(Wnf), grid on 
[Knf,~,GAM,~]=ncfsyn(G,Wnf,1/(0.05*s+1));
sigma(loscuatro(G,-Knf),loscuatro(G,Kms),loscuatro(G,K{compararcon})), grid on, axis([0.1 100 0 30])
[ncfmargin(G,-Knf) ncfmargin(G,Kms) ncfmargin(G,K{compararcon})]
Snf=minreal(1/(1-G*Knf));
Tnf=1-Snf;
zpk(Knf)
minreal(zpk(Knf),1e-2)
step(Tnf,T{1},T{3},Tms,6), grid on, legend('ncfsyn','K1','K3','mixsyn')