%_________________________________________
% Este código sintetiza controladores
% empregando iteração DK
% módulo: dk_sintese3.m
%__________________________AHMK 10/01/04__
clear;
omega = logspace(-3,3,61);
%
% Ponderações
%
G0 = [87.8 -86.4; 108.2 -109.69];
dyn = nd2sys(1,[75 1]); Dyn = daug(dyn,dyn);
G = mmult(Dyn,G0);
wp = nd2sys([10 1],[10 1.e-5],0.5);
wi = nd2sys([1 0.2],[0.5 1]);
Wp = daug(wp,wp); Wi = daug(wi,wi);
%
% Geração da planta com incerteza multiplicativa
% (Wi) na entrada e performance (Wp) na saída.
%
systemnames = 'G Wp Wi';
inputvar = '[ydel(2); w(2); u(2)]';
outputvar = '[Wi; Wp; -G-w]';
input_to_G = '[u+ydel]';
input_to_Wp = '[G+w]';
input_to_Wi = '[u]';
sysoutname = 'P';
cleanupsysic = 'yes';
sysic;
%
% Inicialização
%
% blk = [1 1; 1 1; 2 2];
blk = [2 0; 2 2]; % diag(Wi(I2), Wp(cheio))
nmeas = 2; nu = 2; gmin = .4; gamma=3.; tol=0.01; do=1;
dsysl = daug(daug(do,do),eye(2),eye(2)); dsysr = dsysl;
iteração = 0;
murp = 2;
%
% Início da iteração
%
% Passo 1: Encontra o controlador Hinf com a planta P escalonada.
%
while murp > 0.61;
  iteração = iteração + 1;
  DPD = mmult(dsysl,P,minv(dsysr));
  gmax = 1.05*gamma;
  K = hinfsyn(DPD,nmeas,nu,gmin,gmax,tol);
  Nsc = starp(P,K);
  Nf = frsp(Nsc,omega);
  %
  % Passo 2: Calcula 'mu' empregando limite superior ('c').
  %
  [mubnds,rowd,sens,rowp,rowg] = mu(Nf,blk,'c');
  murp = pkvnorm(mubnds,inf)
  if iteração == 1;
   historia = murp;
  else;
   historia = [historia murp];
  end;
  %
  % Passo 3: Ajusta o módulo de cada elemento de D(w)
  % a uma função D(s) estável e de fase mínima.
  %
  [dsysL,dsysR] = msfbatch(Nf,mubnds,rowd,sens,blk,2);
  dsysl = daug(dsysL,eye(2));
  dsysr = daug(dsysR,eye(2));
  save dscalings.mat dsysl dsysr iteração;
  [Ak,Bk,Ck,Dk] = unpck(K);
  iteração
end;
omega = logspace(-5,5,61);
N = starp(P,K);
Nf = frsp(N,omega);
[mubnds,rowd,sens,rowp,rowg] = mu(Nf,blk,'c');
close all;
%
% Determinação de RP (Robust Performance)
%
muRP = sel(mubnds,':',1); pkvnorm(muRP);
%
% Determinação de RS (Robust Stability)
%
Nrs = sel(Nf,1:2,1:2);
[mubnds,rowd,sens,rowp,rowg] = mu(Nrs,[1 1; 1 1],'c');
muRS = sel(mubnds,':',1); pkvnorm(muRS)
%
% Determinação de NP (Nominal Performance)
%
Nnp = sel(Nf,3:4,3:4);
[mubnds,rowd,sens,rowp,rowg] = mu(Nnp,[2 2],'c');
muNP = sel(mubnds,':',1); pkvnorm(muNP)
figure(2),subplot(2,2,1),plot([1:iteração],historia,'k-o'),grid;
title('desemp. robusto - iterações'),ylabel('mu');
subplot(2,2,2),vplot('liv,m',muRP),grid;
title('Desempenho robusto (RP)'),ylabel('Magnitude');
subplot(2,2,3),vplot('liv,m',muRS),grid;
title('Estabilidade robusta (RS)'),xlabel('Frequência em 
rad/s'),ylabel('Magnitude');
subplot(2,2,4),vplot('liv,m',muNP),grid;
title('Desempenho Nominal (NP)'),xlabel('Frequência em 
rad/s'),ylabel('Magnitude');

save cntrlmdl Ak Bk Ck Dk;
[n1,d1]=ss2tf(Ak,Bk,Ck,Dk,1);
[n2,d2]=ss2tf(Ak,Bk,Ck,Dk,2);
[nn1,dd1]=c2dm(n1,d1,1,'zoh');
[nn2,dd2]=c2dm(n2,d2,1,'zoh');
printsys(nn1,dd1,'z');
printsys(nn2,dd2,'z');




