solvesos.m

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

        if ~isempty(exponent_c)
            hash = randn(size(exponent_m{1},2),1);
            for i = 1:length(exponent_m)
                hash = randn(size(exponent_m{i},2),1);
                keep = find(ismember(exponent_m{i}*hash,exponent_c*hash));
                if length(keep)>0
                    exponent_m{i} = exponent_m{i}(keep,:);
                else
                    exponent_m{i}=[];
                end
            end
        end
        
        % *********************************************
        %% REDUCE #of MONOMIALS
        % *********************************************
        exponent_m = monomialreduction(exponent_m,exponent_p_monoms,options,csclasses);        
                
        % *********************************************
        %% BLOCK PARTITION THE MONOMIALS BY CONGRUENCE
        % *********************************************
        N = congruenceblocks(exponent_m,exponent_p_monoms,options,csclasses);
                
        % *********************************************
        %% REDUCE FURTHER BY EXPLOITING BLOCK-STRUCTURE
        % *********************************************
        N = blockmonomialreduction(exponent_p_monoms,N,options);
        
        % *********************************************
        %% PREPARE FOR SDP FORMULATION BY CALCULATING ALL
        % POSSIBLE MONOMIAL PRODUCS IN EACH BLOCK
        % *********************************************
        [exponent_m2,N_unique] = monomialproducts(N);
        
        % *********************************************
        %% CHECK FOR BUG/IDIOT PROBLEMS IN FIXED PROBLEM
        % *********************************************
        if isempty(ParametricIndicies)
            if ~isempty(setdiff(exponent_p_monoms,N_unique(:,3:end),'rows'))
                for i = 1:length(p)
                    B{i}=[];
                end
                residuals = [];
                sol.problem = 2;
                sol.info = yalmiperror(1,'YALMIP');
                warning('Problem is trivially infeasible (odd highest power?)');
                return
            end
        end
    end
    
    previous_exponent_p_monoms = exponent_p_monoms;
    
    % *********************************************
    %% GENERATE DATA FOR SDP FORMULATIONS
    % *********************************************
    p_base_parametric = parameterizedbase(p{constraint},z,params,ParametricIndicies,exponent_p,p_base);
    [BlockedA{constraint},Blockedb{constraint}] = generate_kernel_representation_data(N,N_unique,exponent_m2,exponent_p,p{constraint},options,p_base_parametric,ParametricIndicies,MonomIndicies,constraint==1);
    
    % SAVE FOR LATER
    BlockedN{constraint} = N;    
    Blockedx{constraint} = x;
    Blockedvarchange{constraint}=varchange;
end

% *********************************************
%% And now get the SDP formulations
%
% The code above has generated matrices A and b
% in AQ == b(parametric)
%
% We use these to generate kernel or image models
% *********************************************
sol.problem = 0;
switch options.sos.model
case 1
    % Kernel model
    [F,obj,BlockedQ,Primal_matrices,Free_variables] = create_kernelmodel(BlockedA,Blockedb,F_parametric,parobj,options,[]);
    
case 2
    % Image model
    [F,obj,BlockedQ,sol] = create_imagemodel(BlockedA,Blockedb,F_parametric,parobj,options);
    
case 3
    % Un-official model to solve bilinearly parameterized SOS using SDPLR
    [F,obj,options] = create_lrmodel(BlockedA,Blockedb,F_parametric,parobj,options,ParametricVariables);
        
otherwise
end

% % **********************************************
% %% SOLVE SDP
% % **********************************************
everything.BlockedA = BlockedA;
everything.Blockedb = Blockedb;
everything.F = F;
everything.obj = obj;
if sol.problem == 0   
    
    if all(isinf(ranks))
        % Standard solve
        sol =  solvesdp(F,obj,options);
    else
        % % **********************************************
        % %% SOLVE SDP with rank constraints?
        % % **********************************************
        Frank = set([]);
        for i = 1:length(ranks)
            if ~isinf(ranks(i))
                Frank = Frank + set(rank(BlockedQ{i}{1}) <= ranks(i));
            end
        end
        % rank adds the pos.def constraints again!!, so we remove them
        check = ones(length(F),1);
        keep  = ones(length(F),1);
        for i = 1:length(BlockedQ)
            for j = 1:length(BlockedQ{i})
                Qij = BlockedQ{i}{j};
                for k = find(check)'
                    if isequal(Qij,sdpvar(F(k)))
                        keep(k)  = 0;
                        check(k) = 0;
                    end
                end
            end
        end                
        % Let's hope LMIRANK is there
        sol =  solvesdp(F(find(keep)) + Frank,[],sdpsettings('solver',''));
    end
    
end

% **********************************************
%% Recover solution
% **********************************************
for constraint = 1:length(p)
    for i = 1:length(BlockedQ{constraint})
        doubleQ{constraint}{i} = normp(constraint)*double(BlockedQ{constraint}{i});
    end       
    doubleb{constraint}=normp(constraint)*double(Blockedb{constraint});
end

% **********************************************
%% Post-process
% **********************************************
[doubleQ,residuals] = postprocesssos(BlockedA,doubleb,doubleQ,[],options);

% **********************************************
%% De-block
% **********************************************
for constraint = 1:length(p)
    Qtemp = [];
    for i = 1:length(BlockedQ{constraint})
        Qtemp = blkdiag(Qtemp,doubleQ{constraint}{i});
    end
    Q{constraint} = full(Qtemp);
end

% **********************************************
%% Experimental code for declaring sparsity
% **********************************************
if options.sos.impsparse == 1
    somesmall = 0;
    for i = 1:length(BlockedQ)
        for j =  1:length(BlockedQ{i})
            small = find(abs(double(BlockedQ{i}{j}))<options.sos.sparsetol);
            nullThese{i}{j} = small;
            if ~isempty(small)
                somesmall = 1;
            end
        end
    end
    if somesmall
        [F,obj,BlockedQ,Primal_matrices,Free_variables] = create_kernelmodel(BlockedA,Blockedb,F_parametric,parobj,options,nullThese);
        sol =  solvesdp(F,obj,options);
        for constraint = 1:length(p)
            for i = 1:length(BlockedQ{constraint})
                doubleQ{constraint}{i} = normp(constraint)*double(BlockedQ{constraint}{i});
            end
            doubleb{constraint}=normp(constraint)*double(Blockedb{constraint});
        end
        % for i = 1:length(doubleQ)
        %     for j =  1:length(doubleQ{i})
        %         doubleQ{i}{j}(nullThese{i}{j}) = 0;
        %     end
        % end
        % **********************************************
        %% Post-process
        % **********************************************
        [doubleQ,residuals] = postprocesssos(BlockedA,doubleb,doubleQ,nullThese,options);
        for constraint = 1:length(p)
            Qtemp = [];
            for i = 1:length(BlockedQ{constraint})
                Qtemp = blkdiag(Qtemp,doubleQ{constraint}{i});
            end
            Q{constraint} = Qtemp;
        end
    end
end

% *********************************************
%% EXTRACT DECOMPOSITION
% *********************************************
switch sol.problem
case {0,1,2,3,4,5} % Well, it didn't f**k up completely at-least
                 
%         % If SDPLR was sucessful, the last dual should have rank 1
%         Z = dual(F(end));
%         i = 1e-16;
%         [R,fail] = chol(Z);
%         while fail & i<10
%             i = i*100;
%             [R,fail] = chol(Z+i*eye(length(Z)));
%         end
%         Z = R(1,2:end)';
%         assign(recover(ParametricVariables),Z);
%         start = 1;
%         for constraint = 1:length(p)
%             Qi = [];
%             for j = 1:length(BlockedA{constraint})
%                 Qi = blkdiag(Qi,dual(F(start)));
%                 start = start + 1;
%             end
%             Q{constraint} = Qi;
%         end
    
    % *********************************************
    %% GENERATE MONOMIALS IN SOS-DECOMPOSITION
    % *********************************************
    for constraint = 1:length(p)

        if constraint > 1 & isequal(BlockedN{constraint},BlockedN{constraint-1}) & isequal(Blockedx{constraint},Blockedx{constraint-1}) & isequal(Blockedvarchange{constraint},Blockedvarchange{constraint-1})
            monoms{constraint} = monoms{constraint-1};
        else
            monoms{constraint} = [];
            totalN{constraint} = [];
            N = BlockedN{constraint};
            x = Blockedx{constraint};
            for i = 1:length(N)
                % Original variable
                for j = 1:size(N{i},1)
                    N{i}(j,:)=N{i}(j,:).*Blockedvarchange{constraint};
                end
                if isempty(N{i})
                    monoms{constraint} = [monoms{constraint};[]];
                else
                    monoms{constraint} = [monoms{constraint};recovermonoms(N{i},x)];
                end
            end
            if isempty(monoms{constraint})
                monoms{constraint}=1;
            end
        end

        % For small negative eigenvalues
        % this is a good quick'n'dirty approximation
        % Improve...use shifted eigenvalues and chol or what ever...
        if ~any(any(isnan(Q{constraint})))
            if isempty(Q{constraint})
                Q{constraint}=0;
                h{constraint}=0;
            else
                usedVariables = find(any(Q{constraint},2));
                if length(usedVariables)<length(Q{constraint})
                    Qpart = Q{constraint}(usedVariables,usedVariables);
                    [U,S,V]=svd(Qpart);
                    R = sqrt(S)*V';
                    h0 = R*monoms{constraint}(usedVariables);
                    if isa(h0,'sdpvar')
                        h{constraint} = clean(R*monoms{constraint}(usedVariables),options.sos.clean);
                        h{constraint} = h{constraint}(find(h{constraint}));
                    else
                        h{constraint} = h0;
                    end
                else
                    [U,S,V]=svd(Q{constraint});
                    R = sqrt(S)*V';
                    h0 = R*monoms{constraint};
                    
                    if isa(h0,'sdpvar')
                        h{constraint} = clean(R*monoms{constraint},0);%options.sos.clean);
                        h{constraint} = h{constraint}(find(h{constraint}));
                    else
                        h{constraint} = h0;
                    end
                end
            end
            if isempty(ParametricVariables)
                ParametricVariables = [];
            end
            setsos(p{constraint},h{constraint},ParametricVariables,Q{constraint},monoms{constraint});
        else
            if options.verbose>0;disp('FAILURE : SOS decomposition not available.');end
            if options.verbose>0;disp(' ');end;
            if options.verbose>0;disp('The reason is probably that you are using a solver that does not deliver a dual (LMILAB)');end;
            if options.verbose>0;disp('Use sdsettings(''sos.model'',2) to circumvent this, or use another solver (SDPT3, SEDUMI,...)');end;
            if options.verbose>0;disp('');end;
            if options.verbose>0;disp('An alternative reason is that YALMIP detected infeasibility during the compilation phase.');end;            
        end
    end

    m = monoms;
    
otherwise
    Q = [];
    m = [];
end

% Don't need these outside
yalmip('cleardual')

% Done with YALMIP, this is the time it took, minus solver
if ~isfield(sol,'solvertime')
    sol.solvertime = 0;
end

sol.yalmiptime = etime(clock,yalmip_time)-sol.solvertime;


function p_base_parametric = parameterizedbase(p,z, params,ParametricIndicies,exponent_p,p_base)

% Check for linear parameterization
parametric_basis = exponent_p(:,ParametricIndicies);
if all(sum(parametric_basis,2)==0)
    p_base_parametric = full(p_base(:));
    return
end
if all(sum(parametric_basis,2)<=1)
    p_base_parametric = full(p_base(:));
    n = length(p_base_parametric);
 
 
    if 1
        %         [ii,vars] = find(parametric_basis(:,indicies));
        %         vars = indicies(vars);
        %         ii = ii(:)';
        %         vars = vars(:)';
        [ii,vars] = find(parametric_basis);
        %        vars = indicies(vars);
        ii = ii(:)';
        vars = vars(:)';
    else
        ii = [];
        vars = [];
        js = sum(parametric_basis,1);
        indicies = find(js);
        for i = indicies
            if js(i)
                j = find(parametric_basis(:,i));
                ii = [ii j(:)'];
                vars = [vars repmat(i,1,js(i))];
            end
        end
    end

    k = setdiff1D(1:n,ii);
    if isempty(k)
        p_base_parametric = p_base_parametric.*sparse(ii,repmat(1,1,n),params(vars));
    else
        pp = params(vars); % Must do this, bug in ML 6.1 (x=sparse(1);x([1 1]) gives different result in 6.1 and 7.0!)
        p_base_parametric = p_base_parametric.*sparse([ii k(:)'],repmat(1,1,n),[pp(:)' ones(1,1,length(k))]);
    end
else
    % Bummer, nonlinear parameterization sucks...
    for i = 1:length(p_base)
        j = find(exponent_p(i,ParametricIndicies));
        if ~isempty(j)
            temp = p_base(i);
            
            for k = 1:length(j)
                if exponent_p(i,ParametricIndicies(j(k)))==1
                    temp = temp*params(j(k));
                else
                    temp = temp*params(j(k))^exponent_p(i,ParametricIndicies(j(k)));
                end
            end
            xx{i} = temp;
        else
            xx{i} = p_base(i);