%
% Síntese mu 
%
% SEM 5928 - Sistemas de Controle 
% Escola de Engenharia de São Carlos - USP
%
% Adriano Siqueira - 2016
%
%
 Uses the Mu toolbox

clear all
close all
clc;

G0=[87.8 -86.4; 108.2 -109.6];
dyn = nd2sys(1,[75 1]);
Dyn=daug(dyn,dyn);
G=mmult(Dyn,G0);

% Weights

wp=nd2sys([10 1],[10 1.e-5],0.5);Wp=daug(wp,wp);
wi=nd2sys([1 0.2],[0.5 1]);Wi=daug(wi,wi);

% Generalized plant P
 
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;

% Initialize

omega = logspace(-3,3,61);
blk = [1 1; 1 1; 2 2];
nmeas=2;nu=2;gmin=0.9;gamma=2;tol=0.01;d0=1;
dsysl = daug(d0,d0,eye(2),eye(2));
dsysr = dsysl;

%%
%START ITERATION1

% STEP 1 Find Hinfinity optimal controller
% with given scalings

DPD = mmult(dsysl,P,minv(dsysr));gmax=1.05*gamma;
[K,Nsc,gamma] = hinfsyn(DPD,nmeas,nu,gmin,gmax,tol);
Nf=frsp(Nsc,omega);%Remark Without scaling N=starp(P,K).


%STEP 2 Compute mu using upper bound
[mubnds,rowd,sens,rowp,rowg] = mu(Nf,blk,'c');
vplot('liv,m',mubnds);
murp=pkvnorm(mubnds,inf)

% STEP 3 Fit resulting Dscales

[dsysl,dsysr] = musynflp(dsysl,rowd,sens,blk,nmeas,nu);% choose 4th order
% entrar com 4 enter -1 enter 4 enter -1 enter
%

%%
%START ITERATION2

% STEP 1 Find Hinfinity optimal controller
% with given scalings

DPD = mmult(dsysl,P,minv(dsysr));gmax=1.05*gamma;
[K,Nsc,gamma] = hinfsyn(DPD,nmeas,nu,gmin,gmax,tol);
Nf=frsp(Nsc,omega);%Remark Without scaling N=starp(P,K).

%STEP 2 Compute mu using upper bound
[mubnds,rowd,sens,rowp,rowg] = mu(Nf,blk,'c');
vplot('liv,m',mubnds);
murp=pkvnorm(mubnds,inf)

% STEP 3 Fit resulting Dscales

[dsysl,dsysr] = musynflp(dsysl,rowd,sens,blk,nmeas,nu);% choose 4th order
% entrar com 4 enter -1 enter 4 enter -1 enter
%


%%
%START ITERATION3

% STEP 1 Find Hinfinity optimal controller
% with given scalings

DPD = mmult(dsysl,P,minv(dsysr));gmax=1.05*gamma;
[K,Nsc,gamma] = hinfsyn(DPD,nmeas,nu,gmin,gmax,tol);
Nf=frsp(Nsc,omega);%Remark Without scaling N=starp(P,K).

save controller.mat K

%STEP 2 Compute mu using upper bound
[mubnds,rowd,sens,rowp,rowg] = mu(Nf,blk,'c');
vplot('liv,m',mubnds);
murp=pkvnorm(mubnds,inf)

% STEP 3 Fit resulting Dscales

[dsysl,dsysr] = musynflp(dsysl,rowd,sens,blk,nmeas,nu);% choose 4th order
% entrar com 4 enter -1 enter 4 enter -1 enter
%

%%

N = starp(P,K); omega=logspace(-3,3,61);
Nf = frsp(N,omega);

% mu for RP

blk =[1 1;1 1;2 2];   
[mubnds,rowd,sens,rowp,rowg] = mu(Nf,blk,'c');
muRP = sel(mubnds,':',1);
pkvnorm(muRP) %

% Worstcase weighted sensitivity

[delworst,muslow,musup] = wcperf(Nf,blk,1); 
musup%

% mu for RS

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) %

% mu for NP (= max singular value of Nnp)

Nnp=sel(Nf,3:4,3:4);
[mubnds,rowd,sens,rowp,rowg] = mu(Nnp,[2 2],'c');
muNP= sel(mubnds,':',1);
pkvnorm(muNP)  % ans =0.5
figure 
vplot('liv,m',muRP,muRS,muNP);
legend('RP','RS','NP')