lmi2sedumistruct.m

国外专家做的求解LMI鲁棒控制的工具箱,可以相对高效的解决LMI问题

function [F_struc,K,KCut] = lmi2sedumistruct(F,gradientHessian)
%lmi2sedumistruct   Internal function: Converts LMI to format needed in SeDuMi
% 
% % Author Johan L鰂berg
% % $Id: lmi2sedumistruct.m,v 1.13 2005/06/08 17:53:08 joloef Exp $

nvars = yalmip('nvars'); %Needed lot'sa times...

% We first browse to see what we have got and the
% dimension of F_struc (massive speed improvement)
top = 0;
type_of_constraint = zeros(size(F.clauses,2),1);
for i = 1:size(F.clauses,2)
    type_of_constraint(i) = F.clauses{i}.type;
    [n,m] = size(getbasematrixwithoutcheck(F.clauses{i}.data,0));
    top = top+n*m;
    % Is it a complex linear cone
    if (type_of_constraint(i)==2) & (~isreal(F.clauses{i}.data))
        top = top+n*m; % We will have constraints on Re and Im
    end
end

F_struc = [];

sdp_con = find(type_of_constraint == 1 | type_of_constraint == 9);
lin_con = find(type_of_constraint == 2 | type_of_constraint == 12);
equ_con = find(type_of_constraint == 3);
qdr_con = find(type_of_constraint == 4);
rlo_con = find(type_of_constraint == 5);

% SeDuMi struct
K.f = 0;
K.l = 0;
K.q = 0;
K.r = 0;
K.s = 0;
K.rank = [];
K.dualrank = [];
K.scomplex = [];
K.xcomplex = [];

KCut.f = [];
KCut.l = [];
KCut.q = [];
KCut.r = [];
KCut.s = [];

top = 1;
localtop = 1;
% Linear constraints
for i = 1:length(equ_con)
    constraints = equ_con(i);
    data = getbase(F.clauses{constraints}.data);
    [n,m] = size(F.clauses{constraints}.data);
    % Which variables are needed in this constraint
    lmi_variables = getvariables(F.clauses{constraints}.data);
    if isreal(data)
        ntimesm = n*m; %Just as well pre-calc			
    else
        % Complex constraint, Expand to real and Imag
        ntimesm = 2*n*m; %Just as well pre-calc
        data = [real(data);imag(data)];			
    end
    F_structemp  = spalloc(ntimesm,1+nvars,0);
    F_structemp(:,[1 1+lmi_variables(:)'])= data;
    % ...and add them together (efficient for large structures)
    %	F_struc(top:top+ntimesm-1,:) = F_structemp;	
    F_struc = [F_struc;F_structemp];
    
    if F.clauses{constraints}.cut
        KCut.f = [KCut.f localtop:localtop+ntimesm-1];
    end
    
    localtop = localtop+ntimesm;	    
    top = top+ntimesm;	
    K.f = K.f+ntimesm;
end

% Linear constraints
localtop = 1;
F_struc = F_struc';
for i = 1:length(lin_con)
    constraints = lin_con(i);
    data = getbase(F.clauses{constraints}.data);
    [n,m] = size(F.clauses{constraints}.data);
    
    % Which variables are needed in this constraint
    lmi_variables = getvariables(F.clauses{constraints}.data);
    
    % Convert to real problem
    if isreal(data)
        ntimesm = n*m; %Just as well pre-calc			
    else
        % Complex constraint, Expand to real and Imag
        ntimesm = 2*n*m; %Just as well pre-calc
        data = [real(data);imag(data)];			
    end
    
    % Add numerical data to complete problem setup
    mapX = [1 1+lmi_variables];    
    [ix,jx,sx] = find(data);   
    F_structemp = sparse(mapX(jx),ix,sx,1+nvars,ntimesm);
    F_struc = [F_struc F_structemp];
    
    if F.clauses{constraints}.cut
        KCut.l = [KCut.l localtop:localtop+ntimesm-1];
    end

    localtop = localtop+ntimesm;	
    top = top+ntimesm;	
    K.l = K.l+ntimesm;
end
F_struc = F_struc';

% second order cone constraints
for i = 1:length(qdr_con)
    constraints = qdr_con(i);
    
    [n,m] = size(F.clauses{constraints}.data);
    ntimesm = n*m; %Just as well pre-calc
    
    % Which variables are needed in this constraint
    lmi_variables = getvariables(F.clauses{constraints}.data);
    
    data = getbase(F.clauses{constraints}.data);
    if isreal(data)
        F_structemp  = spalloc(ntimesm,1+nvars,0);
        F_structemp(:,[1 1+lmi_variables(:)'])= data;
    else
        n = n+(n-1);
        ntimesm = n*m;
        F_structemp  = spalloc(ntimesm,1+nvars,0);
        data = [data(1,:);real(data(2:end,:));imag(data(2:end,:))];
        F_structemp(:,[1 1+lmi_variables(:)'])= data;
    end
    % ...and add them together (efficient for large structures)
    F_struc = [F_struc;F_structemp];  
    top = top+ntimesm;
    % Check for a complex structure
    %    if ~isreal(F_structemp)
    %        complex_ind = 1+sum(K.f)+sum(K.l)+sum(K.q):sum(K.f)+sum(K.l)+sum(K.q)+n;
    %        K.xcomplex = [K.xcomplex(:)' complex_ind(:)'];
    %    end
    K.q(i) = n;	
end

% Rotated Lorentz cone constraints
for i = 1:length(rlo_con)
    constraints = rlo_con(i);
    
    [n,m] = size(F.clauses{constraints}.data);
    ntimesm = n*m; %Just as well pre-calc
    
    % Which variables are needed in this constraint
    lmi_variables = getvariables(F.clauses{constraints}.data);
    
    % We allocate the structure blockwise...
    F_structemp  = spalloc(ntimesm,1+nvars,0);
    % Add these rows only 
    F_structemp(:,[1 1+lmi_variables(:)'])= getbase(F.clauses{constraints}.data);
    % ...and add them together (efficient for large structures)
    %	F_struc(top:top+ntimesm-1,:) = F_structemp;
    F_struc = [F_struc;F_structemp];
    
    top = top+ntimesm;
    K.r(i) = n;	
end

% Wemidefinite  constraints
% We append the recursively in order to speed up construction
% of problems with a lot of medium size SDPs
[F_struc,K,KCut] = recursivefix(F,F_struc',K,KCut,sdp_con,nvars,8,1);
F_struc = F_struc';

% F_struc = F_struc';
% for i = 1:length(sdp_con)
%     constraints = sdp_con(i);    
%     Fi = F.clauses{constraints}.data;
%     Fibase = getbase(Fi);
%     [n,m] = size(Fi);
%     ntimesm = n*m; %Just as well pre-calc
%     
%     % Which variables are needed in this constraint
%     lmi_variables = getvariables(Fi);
%         
%     if 1       
%         [ix,jx,sx] = find(Fibase);    
%        % in_X = find(ismembc(1:nvars,lmi_variables));
%         mapX = [1 1+lmi_variables];        
% %        F_structemp = sparse(ix,mapX(jx),sx,ntimesm,1+nvars);    
%         F_structemp = sparse(ix,mapX(jx),sx,ntimesm,1+nvars)';            
%     else
%         % This code does not work for huge problems
%         % We allocate the structure blockwise...
%         % F_structemp  = spalloc(ntimesm,1+nvars,nnz(Fibase));
%         % Add these rows only     
%         % F_structemp(:,[1 1+lmi_variables(:)'])= Fibase;
%     end
%     % ...and add them together (efficient for large structures)
%     %	F_struc(top:top+ntimesm-1,:) = F_structemp;
% %    F_struc = [F_struc;F_structemp];
%     F_struc = [F_struc F_structemp];
% %    iF = [iF;ix+top-1];
% %    jF = [jF;mapX(jx)'];
% %    sF = [sF;sx];
%     
%     if F.clauses{constraints}.cut
%         KCut.s = [KCut.s i];
%     end
%     K.s(i) = n;
%     K.rank(i) = n;
%     K.dualrank(i) = n;
%     top = top+ntimesm;
%     % Check for a complex structure
%     if ~isreal(F_structemp)
%         K.scomplex = [K.scomplex i];
%     end
% end
% F_struc = F_struc';


%F_struc = sparse(iF,jF,sF);

% Now fix things for the rank constraint
% This is currently a hack...
% Should not be in this file
rank_variables = yalmip('rankvariables');
if ~isempty(rank_variables)    
    used_in = find(sum(abs(F_struc(:,1+rank_variables)),2));
    if ~isempty(used_in)
        if used_in >=1+K.f & used_in < 1+K.l+K.f
            for i = 1:length(used_in)
                [ii,jj,kk] = find(F_struc(used_in(i),:));
                if length(ii)==2 & kk(2)<1
                    r = floor(kk(1));
                    var = jj(2)-1;
                    extstruct = yalmip('extstruct',var);
                    X = extstruct.arg{1};
                    if issymmetric(X)
                        F_structemp = sedumize(X,nvars);
                    else
                        error('Only symmetric matrices can be rank constrained.')
                    end
                    F_struc = [F_struc;F_structemp];
                    if isequal(K.s,0)
                        K.s(1,1) = size(extstruct.arg{1},1);
                    else
                        K.s(1,end+1) = size(extstruct.arg{1},1);
                    end
                    K.rank(1,end+1) = min(r,K.s(end));
                else
                    error('This rank constraint is not supported (only supports rank(X) < r)')
                end
            end
            % Remove the nonlinear operator constraints
            
            F_struc(used_in,:) = [];
            K.l = K.l - length(used_in);
        else
            error('You have added a rank constraint on an equality constraint, or a scalar expression?!')
        end
    end
end
rank_variables = yalmip('dualrankvariables');
if ~isempty(rank_variables)
    used_in = find(sum(abs(F_struc(:,1+rank_variables)),2));
    if ~isempty(used_in)
        if used_in >=1+K.f & used_in < 1+K.l+K.f
            for i = 1:length(used_in)
                [ii,jj,kk] = find(F_struc(used_in(i),:));
                if length(ii)==2 & kk(2)<1
                    r = floor(kk(1));
                    var = jj(2)-1;
                    extstruct = yalmip('extstruct',var);
                    X = extstruct.arg{1};
                    id = getlmiid(X);
                    inlist=getlmiid(F);
                    index=find(id==inlist);
                    if ~isempty(index)                                            
                        K.rank(1,index) = min(r,K.s(index));
                    end
                else
                    error('This rank constraint is not supported (only supports rank(X) < r)')
                end
            end
            % Remove the nonlinear operator constraints
            F_struc(used_in,:) = [];
            K.l = K.l - length(used_in);
        else
            error('You have added a rank constraint on an equality constraint, or a scalar expression?!')
        end
    end
end


if ~isempty(gradientHessian)
    data = getbase(gradientHessian);
    [n,m] = size(gradientHessian);
    lmi_variables = getvariables(gradientHessian);
    ntimesm = n*m; %Just as well pre-calc
    F_structemp  = spalloc(ntimesm,1+nvars,0);
    F_structemp(:,[1 1+lmi_variables(:)'])= data;
    F_struc = [F_struc;F_structemp];
    K.gh = n;
end

function F_structemp = sedumize(Fi,nvars)
Fibase = getbase(Fi);
[n,m] = size(Fi);
ntimesm = n*m;
lmi_variables = getvariables(Fi);
[ix,jx,sx] = find(Fibase);
mapX = [1 1+lmi_variables];
F_structemp = sparse(ix,mapX(jx),sx,ntimesm,1+nvars);





function [F_struc,K,KCut] = recursivefix(F,F_struc,K,KCut,sdp_con,nvars,maxnsdp,startindex)

% Check if we should recurse
if length(sdp_con)>2*maxnsdp
    % recursing costs, so do 4 in one step
    ind = 1+ceil(length(sdp_con)*(0:0.25:1));
    [F_struc1,K,KCut] = recursivefix(F,[],K,KCut,sdp_con(ind(1):ind(2)-1),nvars,maxnsdp,startindex+ind(1)-1);
    [F_struc2,K,KCut] = recursivefix(F,[],K,KCut,sdp_con(ind(2):ind(3)-1),nvars,maxnsdp,startindex+ind(2)-1);
    [F_struc3,K,KCut] = recursivefix(F,[],K,KCut,sdp_con(ind(3):ind(4)-1),nvars,maxnsdp,startindex+ind(3)-1);
    [F_struc4,K,KCut] = recursivefix(F,[],K,KCut,sdp_con(ind(4):ind(5)-1),nvars,maxnsdp,startindex+ind(4)-1);
    F_struc = [F_struc F_struc1 F_struc2 F_struc3 F_struc4];
    return
elseif length(sdp_con)>maxnsdp
    mid = ceil(length(sdp_con)/2);
    [F_struc1,K,KCut] = recursivefix(F,[],K,KCut,sdp_con(1:mid),nvars,maxnsdp,startindex);
    [F_struc2,K,KCut] = recursivefix(F,[],K,KCut,sdp_con(mid+1:end),nvars,maxnsdp,startindex+mid);
    F_struc = [F_struc F_struc1 F_struc2];
    return
end

% Working with transpose is faster
%F_struc = F_struc';
oldF_struc = F_struc;
F_struc = [];
for i = 1:length(sdp_con)
    constraints = sdp_con(i);
    Fi = F.clauses{constraints}.data;
    Fibase = getbase(Fi);
    [n,m] = size(Fi);
    ntimesm = n*m; %Just as well pre-calc

    % Which variables are needed in this constraint
    lmi_variables = getvariables(Fi);
    mapX = [1 1+lmi_variables];
    
    [ix,jx,sx] = find(Fibase);
    
    F_structemp = sparse(mapX(jx),ix,sx,1+nvars,ntimesm);
    F_struc = [F_struc F_structemp];

    if F.clauses{constraints}.cut
        KCut.s = [KCut.s i+startindex-1];
    end
    K.s(i+startindex-1) = n;
    K.rank(i+startindex-1) = n;
    K.dualrank(i+startindex-1) = n;
    % Check for a complex structure
    if ~isreal(F_structemp)
        K.scomplex = [K.scomplex i+startindex-1];
    end
end
F_struc = [oldF_struc F_struc];
%F_struc = F_struc';