function [vals,dims,sdims]=RobSFandOBS(uncert,mud,beta)
% [vals,dims]=RobSFandOBS(uncert,mud,beta)
%
% Code used for the Automatica paper
% https://doi.org/10.1016/j.automatica.2016.10.003
% author: Dimitri Peaucelle
%
% Needs YAMLIP https://yalmip.github.io/
%
% uncert=[0.1,0.2] 
% means 10% variation on first uncertainty and 20% on the other
%
% mud=10 in the paper
%
% beta=[beta2,betap]
% most tested values are beta=[1 1] in the paper

beta=beta/norm(beta);
mud2=mud^2;
sdpopts=sdpsettings('verbose',0);

%% define the data
% u contains all information on uncertainties
u.v{1}=[1-uncert(1) 1+uncert(1)];% vertices of first uncertainty
u.v{2}=[1-uncert(2) 1+uncert(2)];
bp=2;%number of parameters
% Build all indexes of vertices to be tested
[I]=gen_vtx_indexes(u);
bbv=size(I,1);%global number of vertices
% Build the vertices of the DMAR
for ii=1:bbv
    for p=1:bp
        v{p}=u.v{p}(I(ii,p));
    end
    Ex(:,:,ii)=[v{2} 0;0 1];
    Ez(:,:,ii)=[0 1];           pz=1;
    Ep(:,:,ii)=[v{1} 0;0 1];    np=2;
    A(:,:,ii)=[-v{2} -v{1};1 0];n=2;
    B(:,:,ii)=[0;v{1}];         mu=1;
    Bw(:,:,ii)=[v{1};0];        mw=1;
end
C=[0 1];                        py=1;
% and of its dual DMAR
Edx=permute(A,[2,1,3]);
Edy=permute(B,[2,1,3]);
Edz=permute(Bw,[2,1,3]);
Edp=permute(Ep,[2,1,3]);
Ad=permute(Ex,[2,1,3]);
Bdw=permute(Ez,[2,1,3]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Find an H-infty state-feedback - Theorem 5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Sdx=sdpvar(n,n,'full');
Sdy=sdpvar(n,mu,'full');
N=zeros(mw+np,n+mw+np+n+pz,bbv);
for ii=1:bbv
    N(:,:,ii)=[...
        zeros(mw,n),eye(mw),Edz(:,:,ii),zeros(mw,n+pz);...
        zeros(np,n+mw),Edp(:,:,ii),Ad(:,:,ii),Bdw(:,:,ii)];
end
[N,M]=apply_constant_rows(N);
Sdp=sdpvar(size(N,2),size(N,1),'full');
Pd=sdpvar(n,n,bbv,'sym');
quiz=Pd(:,:,1)>=0;
if isempty(mud)
    mud2=sdpvar;
end
for ii=1:bbv
    separator=blkvar;
    separator(1,1)=Pd(:,:,ii);
    separator(2,2)=eye(mw);
    separator(1,2)=zeros(n,mw);
    separator(3,3)=zeros(np,np);
    separator(1,3)=zeros(n,np);
    separator(4,4)=-Pd(:,:,ii);
    separator(1,4)=zeros(n,n);
    separator(5,5)=-mud2*eye(pz);  
    separator(1,5)=zeros(n,pz);
    separator=sdpvar(separator);
    Ns=[Sdx,zeros(n,mw),Sdx*Edx(:,:,ii)+Sdy*Edy(:,:,ii),zeros(n,n+pz)];
    Svarpart = M'*[eye(n);zeros(size(M,1)-n,n)]*Ns*M+Sdp*N(:,:,ii);
    quiz=quiz +[ M'*separator*M <= Svarpart+Svarpart' ];
end
solvesdp(quiz,mud2,sdpopts);
dims.sfdesign=get_size_LMI(quiz);
sdims.sfdesign=size(Sdp);
[pres,~] = checkset(quiz);
if all(pres>0)
    vals.K=double(Sdy')/double(Sdx');
else
    vals.K=[];
    return
end
K=vals.K;
if isempty(mud)
    vals.mud=sqrt(double(mud2));
end
yalmip('clear')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Find an H-infty state-feedback - Theorem 12
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Sdx=sdpvar(n,n,'full');
Sdy=sdpvar(n,mu,'full');
N=zeros(np,n+np+n+pz,bbv);
for ii=1:bbv
    N(:,:,ii)=[...
        zeros(np,n),Edp(:,:,ii),Ad(:,:,ii),Bdw(:,:,ii)];
end
[N,M]=apply_constant_rows(N);
Sdp=sdpvar(size(N,2),size(N,1),'full');
Pd=sdpvar(n,n,bbv,'sym');
if isempty(mud)
    mud2=sdpvar;
end
quiz=Pd(:,:,1)>=0;
for ii=1:bbv
    separator=blkvar;
    separator(1,1)=Pd(:,:,ii);
    separator(2,2)=Edz(:,:,ii)'*Edz(:,:,ii);
    separator(1,2)=zeros(n,np);
    separator(3,3)=-Pd(:,:,ii);
    separator(1,3)=zeros(n,n);
    separator(4,4)=-mud2*eye(pz);  
    separator(1,4)=zeros(n,pz);
    separator=sdpvar(separator);
    Ns=[Sdx,Sdx*Edx(:,:,ii)+Sdy*Edy(:,:,ii),zeros(n,n+pz)];
    Svarpart= M'*[eye(n);zeros(size(M,1)-n,n)]*Ns*M+Sdp*N(:,:,ii);
    quiz=quiz +[ M'*separator*M <= Svarpart+Svarpart' ];
end
solvesdp(quiz,mud2,sdpopts);
dims.sfdesignbis=get_size_LMI(quiz);
sdims.sfdesignbis=size(Sdp);
[pres,~] = checkset(quiz);
if all(pres>0)
    vals.Kbis=double(Sdy')/double(Sdx');
else
    vals.Kbis=[];
    return
end
if isempty(mud)
    vals.mudbis=sqrt(double(mud2));
end
yalmip('clear')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Analyze the state-feedback - Theorem 6
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=zeros(n+np,n+np+n+mu,bbv);
for ii=1:bbv
    N(:,:,ii)=[...
        eye(n),Ex(:,:,ii),zeros(n,n+mu);...
        zeros(np,n),Ep(:,:,ii),A(:,:,ii)+B(:,:,ii)*K,B(:,:,ii)];
end
[N,M]=apply_constant_rows(N);
S=sdpvar(size(N,2),size(N,1),'full');
Q=sdpvar(n,n,'sym');
tQ=sdpvar;
P=sdpvar(n,n,bbv,'sym');
quiz=[P(:,:,1)>=0,Q>=tQ*eye(n),tQ>=0];
for ii=1:bbv
    separator=blkvar;
    separator(1,1)=P(:,:,ii);
    separator(2,2)=zeros(np,np);
    separator(1,2)=zeros(n,np);
    separator(3,3)=Q-P(:,:,ii);
    separator(1,3)=zeros(n,n);
    separator(4,4)=-eye(mu);  
    separator(1,4)=zeros(n,mu);
    separator=sdpvar(separator);
    Svarpart=S*N(:,:,ii);
    quiz=quiz+[ M'*separator*M <= Svarpart+Svarpart' ];
end
solvesdp(quiz,-1*trace(Q)-10*tQ,sdpopts);
dims.sfanalysis=get_size_LMI(quiz);
sdims.sfanalysis=size(S);
[pres,] = checkset(quiz);
if all(pres>0)
    vals.Q=(double(Q));
else
    vals.Q=[];
    return
end
Q=vals.Q;
yalmip('clear')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Design observer - Theorem 7
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Sx=sdpvar(n,n,'full');
Sa=sdpvar(n,n,'full');
Sb=sdpvar(n,mu,'full');
Sl=sdpvar(n,py,'full');
N=zeros(np,n+np+n+n,bbv);
for ii=1:bbv
    N(:,:,ii)=[...
        zeros(np,n),Ep(:,:,ii),-B(:,:,ii)*K,A(:,:,ii)+B(:,:,ii)*K];
end
[N,M]=apply_constant_rows(N);
Sp2=sdpvar(size(N,2),size(N,1),'full');
Spp=sdpvar(size(N,2),size(N,1),'full');
g2=sdpvar;
gp=sdpvar;
P2=sdpvar(n,n,bbv,'sym');
Pp=sdpvar(n,n,bbv,'sym');
quiz=[P2(:,:,1)>=0];
for ii=1:size(I,1)
    separator2=blkvar;
    separator2(1,1)=P2(:,:,ii);
    separator2(2,2)=zeros(np,np);
    separator2(1,2)=zeros(n,np);
    separator2(3,3)=K'*K-P2(:,:,ii);
    separator2(1,3)=zeros(n,n);
    separator2(4,4)=-g2*Q;  
    separator2(1,4)=zeros(n,n);
    separator2=sdpvar(separator2);
    Ns=[Sx,Sx*Ex(:,:,ii),-Sa-Sb*K-Sl*C, Sa+Sb*K];
    Svarpart2= M'*[eye(n);zeros(size(M,1)-n,n)]*Ns*M+Sp2*N(:,:,ii);
    quiz=quiz+[ M'*separator2*M <= Svarpart2+Svarpart2' ];
    separatorp=blkvar;
    separatorp(1,1)=Pp(:,:,ii);
    separatorp(2,2)=zeros(np,np);
    separatorp(1,2)=zeros(n,np);
    separatorp(3,3)=-Pp(:,:,ii);
    separatorp(1,3)=zeros(n,n);
    separatorp(4,4)=-gp*Q;  
    separatorp(1,4)=zeros(n,n);
    separatorp=sdpvar(separatorp);
    Svarpartp=M'*[eye(n);zeros(size(M,1)-n,n)]*Ns*M+Spp*N(:,:,ii);
    quiz=quiz+[ M'*separatorp*M <= Svarpartp+Svarpartp' ] + [Pp(:,:,ii)>=K'*K];
end
solvesdp(quiz,beta(1)*g2+beta(2)*gp,sdpopts);
dims.obsdesign=get_size_LMI(quiz);
sdims.obsdesign2=size(Sp2);
sdims.obsdesignp=size(Spp);
[pres] = checkset(quiz);
if all(pres>0)
    vals.Ao=(double(Sx))\double(Sa);
    vals.Bo=(double(Sx))\double(Sb);
    vals.L=(double(Sx))\double(Sl);
    vals.g=sqrt(double([g2,gp]));
else
    vals.Ao=[];
    return
end
Ao=vals.Ao;
Bo=vals.Bo;
L=vals.L;
yalmip('clear')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Analyze the closed-loop - Theorem 9
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=zeros(n+n+pz+np,n+n+pz+np+n+n+mw,bbv);
for ii=1:bbv
    N(:,:,ii)=[...
        eye(n),zeros(n,n+pz+np),-Ao-Bo*K-L*C,L*C,zeros(n,mw);...
        zeros(n,n),eye(n),zeros(n,pz),Ex(:,:,ii),zeros(n,n+n+mw);...
        zeros(pz,n+n),eye(pz),Ez(:,:,ii),zeros(pz,n+n+mw);...
        zeros(np,n+n+pz),Ep(:,:,ii),B(:,:,ii)*K,A(:,:,ii),Bw(:,:,ii)];
end
[N,M]=apply_constant_rows(N);
S=sdpvar(size(N,2),size(N,1),'full');
muc=sdpvar;
P=sdpvar(2*n,2*n,bbv,'sym');
quiz=[P(:,:,1)>=0];
for ii=1:bbv
    separator=blkvar;
    separator(1,1)=P(:,:,ii);
    separator(2,2)=eye(pz);
    separator(1,2)=zeros(2*n,pz);
    separator(3,3)=zeros(np,np);
    separator(1,3)=zeros(2*n,np);
    separator(4,4)=-P(:,:,ii);
    separator(1,4)=zeros(2*n,2*n);
    separator(5,5)=-muc*eye(mu);  
    separator(1,5)=zeros(2*n,mu);
    separator=sdpvar(separator);
    Svarpart=S*N(:,:,ii);
    quiz=quiz+[ M'*separator*M <= Svarpart+Svarpart' ];
end
solvesdp(quiz,muc,sdpopts);
dims.oanalysis=get_size_LMI(quiz);
sdims.oanalysis=size(S);
[pres,] = checkset(quiz);
if all(pres>0)
    vals.muc=sqrt(double(muc));
else
    vals.muc=[];
end
yalmip('clear')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Analyze the closed-loop - Theorem 10
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=zeros(n+n+mw+np,n+n+mw+np+n+n+pz,bbv);
for ii=1:bbv
    N(:,:,ii)=[...
        eye(n),zeros(n,n+mw),K'*Edy(:,:,ii),-Ao'-K'*Bo'-C'*L',zeros(n,n+pz);...
        zeros(n,n),eye(n),zeros(n,mw),Edx(:,:,ii),C'*L',zeros(n,n+pz);...
        zeros(mw,n+n),eye(mw),Edz(:,:,ii),zeros(mw,n+n+pz);...
        zeros(np,n+n+mw),Edp(:,:,ii),zeros(np,n),Ad(:,:,ii),Bdw(:,:,ii)];
end
[N,M]=apply_constant_rows(N);
S=sdpvar(size(N,2),size(N,1),'full');
mudc=sdpvar;
Pd=sdpvar(2*n,2*n,bbv,'sym');
quiz=[Pd(:,:,1)>=0];
for ii=1:bbv
    separator=blkvar;
    separator(1,1)=Pd(:,:,ii);
    separator(2,2)=eye(mw);
    separator(1,2)=zeros(2*n,mw);
    separator(3,3)=zeros(np,np);
    separator(1,3)=zeros(2*n,np);
    separator(4,4)=-Pd(:,:,ii);
    separator(1,4)=zeros(2*n,2*n);
    separator(5,5)=-mudc*eye(pz);  
    separator(1,5)=zeros(2*n,pz);
    separator=sdpvar(separator);
    Svarpart=S*N(:,:,ii);
    quiz=quiz+[ M'*separator*M <= Svarpart+Svarpart' ];
end
solvesdp(quiz,mudc,sdpopts);
dims.oanalysisdual=get_size_LMI(quiz);
sdims.oanalysisdual=size(S);
[pres] = checkset(quiz);
if all(pres>0)
    vals.mudc=sqrt(double(mudc));
else
    vals.mudc=[];
end
yalmip('clear')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% State-feedback imporvement - Theorem 11
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Sdx=sdpvar(n,n,'full');
Sdy=sdpvar(n,mu,'full');
Sda=sdpvar(n,n,'full');
N=zeros(n+mw+np,n+n+mw+np+n+n+pz,bbv);
for ii=1:bbv
    N(:,:,ii)=[...
        zeros(n,n),eye(n),zeros(n,mw),Edx(:,:,ii),C'*L',zeros(n,n+pz);...
        zeros(mw,n+n),eye(mw),Edz(:,:,ii),zeros(mw,n+n+pz);...
        zeros(np,n+n+mw),Edp(:,:,ii),zeros(np,n),Ad(:,:,ii),Bdw(:,:,ii)];
end
[N,M]=apply_constant_rows(N);
Sdp=sdpvar(size(N,2),size(N,1),'full');
Pd=sdpvar(2*n,2*n,bbv,'sym');
muds2=sdpvar;
quiz=[Pd(:,:,1)>=0];
for ii=1:bbv
    separator=blkvar;
    separator(1,1)=Pd(:,:,ii);
    separator(2,2)=eye(mw);
    separator(1,2)=zeros(2*n,mw);
    separator(3,3)=zeros(np,np);
    separator(1,3)=zeros(2*n,np);
    separator(4,4)=-Pd(:,:,ii);
    separator(1,4)=zeros(2*n,2*n);
    separator(5,5)=-muds2*eye(pz);  
    separator(1,5)=zeros(2*n,pz);
    separator=sdpvar(separator);
    Ns=[Sdx,zeros(n,n+mw),Sdy*Edy(:,:,ii),-Sda-Sdy*Bo'-Sdx*C'*L',zeros(n,n+pz)];
    Svarpart = M'*[eye(n);zeros(size(M,1)-n,n)]*Ns*M+Sdp*N(:,:,ii);
    quiz=quiz +[ M'*separator*M <= Svarpart+Svarpart' ];
end
solvesdp(quiz,muds2,sdpopts);
dims.sfimporvement=get_size_LMI(quiz);
[pres,~] = checkset(quiz);
if all(pres>0)
    vals.Kimp=double(Sdy')/double(Sdx');
    vals.Aoimp=double(Sda')/double(Sdx');
    vals.muds=sqrt(double(muds2));
else
    vals.Kimp=[];
end
yalmip('clear')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Impulse responses
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate a sample
figure
for ii=1:20
    zeta=rand;
    theta{1}=u.v{1}*[zeta;1-zeta];
    zeta=rand;
    theta{2}=u.v{2}*[zeta;1-zeta];
    Ext=[theta{2} 0;0 1];
    Ezt=[0 1];
    Ept=[theta{1} 0;0 1];
    At=[-theta{2} -theta{1};1 0];
    Bt=[0;theta{1}];
    Bwt=[theta{1};0];
    C=[0 1];
    sys=ssmodel;
    sys.a=Ext/Ept*At;
    sys.bu=Ext/Ept*Bt;
    sys.bw=Ext/Ept*Bwt;
    sys.cz=Ezt/Ept*At;
    sys.dzu=Ezt/Ept*Bt;
    sys.dzw=Ezt/Ept*Bwt;
    sys.cy=C;
    ctr=ssmodel;
    ctr.a=Ao+Bo*K+L*C;
    ctr.bu=L;
    ctr.cy=K;
    sbf=feedback(sys,ctr);
    sbf.cz=[sbf.cz;eye(2) -eye(2)];
    sbf.ts=1;
    impulse(sbf,20), hold on, grid on
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [I]=gen_vtx_indexes(u,varargin)
if nargin<2
    Io=ones(1,0);
else
    Io=varargin{1};
end
In=kron(ones(size(Io,1),1),(1:size(u.v{1},2)).');
Io=kron(Io,ones(size(u.v{1},2),1));
I=[Io,In];
if length(u.v)>1
    un.v={u.v{2:end}};
    [I]=gen_vtx_indexes(un,I);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Nn,M]=apply_constant_rows(N)
Ntest=repmat(N(:,:,1),[1,1,size(N,3)])-N;
cr=find(~any(any(Ntest,3),2));
ncr=find(any(any(Ntest,3),2));
M=null(N(cr,:,1));
for ii=1:size(N,3)
    Nn(:,:,ii)=N(ncr,:,ii)*M;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [dims]=get_size_LMI(quiz)
nbv=length(getvariables(quiz));
info=lmiinfo(quiz);
nbr=sum(info.sdp(:,1));
dims=[nbv,nbr];
end

