solvesos.m

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

        end
    end
    p_base_parametric = stackcell(sdpvar(1,1),xx)';
end



function [A,b] = generate_kernel_representation_data(N,N_unique,exponent_m2,exponent_p,p,options,p_base_parametric,ParametricIndicies,MonomIndicies,FirstRun)

persistent saveData

exponent_p_parametric = exponent_p(:,ParametricIndicies);
exponent_p_monoms = exponent_p(:,MonomIndicies);
pcoeffs = getbase(p);
if any(exponent_p_monoms(1,:))
    pcoeffs=pcoeffs(2:end); % No constant term in p
end
b = [];

parametric = full((~isempty(ParametricIndicies) & any(any(exponent_p_parametric))));

% For problems with a lot of similar cones, this saves some time
reuse = 0;
if ~isempty(saveData) & isequal(saveData.N,N) & ~FirstRun
    n = saveData.n;
    ind = saveData.ind;
    if  isequal(saveData.N_unique,N_unique) & isequal(saveData.exponent_m2,exponent_m2)% & isequal(saveData.epm,exponent_p_monoms)
        reuse = 1;
    end
else
    % Congruence partition sizes
    for k = 1:size(N,1)
        n(k) = size(N{k},1);
    end
    % Save old SOS definition
    saveData.N = N;
    saveData.n = n;
    saveData.N_unique = N_unique;
    saveData.exponent_m2 = exponent_m2;
    saveData.N_unique = N_unique;
end

if reuse & options.sos.reuse
    % Get old stuff
    if size(exponent_m2{1},2)==2 % Stupid set(sos(parametric)) case
        ind = spalloc(1,1,0);
        ind(1)=1;
        allj = 1:size(exponent_p_monoms,1);
        used_in_p = ones(size(exponent_p_monoms,1),1);
    else
        allj = [];
        used_in_p = zeros(size(exponent_p_monoms,1),1);
        hash = randn(size(exponent_p_monoms,2),1);
        exponent_p_monoms_hash = exponent_p_monoms*hash;
        for i = 1:size(N_unique,1)
            monom = sparse(N_unique(i,3:end));
            j = find(exponent_p_monoms_hash == (monom*hash));
           % if ~isequal(j,find(exponent_p_monoms_hash == (full(monom)*hash)))
           %     j
           % end
            %j = findrows(exponent_p_monoms,monom);
            
            if isempty(j)
                b = [b 0];
                allj(end+1,1) = 0;
            else
                used_in_p(j) = 1;
                allj(end+1,1:length(j)) = j(:)';
            end
        end
        ind = saveData.ind;
    end
else
    allj = [];
    used_in_p = zeros(size(exponent_p_monoms,1),1);
    if size(exponent_m2{1},2)==2 % Stupid set(sos(parametric)) case
        ind = spalloc(1,1,0);
        ind(1)=1;
        allj = 1:size(exponent_p_monoms,1);
        used_in_p = ones(size(exponent_p_monoms,1),1);
    else
        % To speed up some searching, we random-hash data
        hash = randn(size(exponent_p_monoms,2),1);
        for k = 1:length(exponent_m2)
            if isempty(exponent_m2{k})
                exp_hash{k}=[];
            else
                exp_hash{k} = sparse((exponent_m2{k}(:,3:end)))*hash; % SPARSE NEEDED DUE TO STRANGE NUMERICS IN MATLAB ON 0s (the stuff will differ on last bit in hex format)
            end
        end
        
        p_hash = exponent_p_monoms*hash;
        
        ind = spalloc(size(N_unique,1),sum(n.^2),0);
        for i = 1:size(N_unique,1)
            monom = N_unique(i,3:end);
            monom_hash = sparse(monom)*hash;
            LHS = 0;
            start = 0;
            for k = 1:size(N,1)
                j = find(exp_hash{k} == monom_hash);
                if ~isempty(j)
                    pos=exponent_m2{k}(j,1:2);
                    ns = pos(:,1);
                    ms = pos(:,2);
                    indicies = ns+(ms-1)*n(k);
                    ind(i,indicies+start) = ind(i,indicies+start) + 1;
                end
                start = start + n(k)^2;
            end
            
            j = find(p_hash == monom_hash);
            
            if isempty(j)
                allj(end+1,1) = 0;
            else
                used_in_p(j) = 1;
                allj(end+1,1:length(j)) = j(:)';
            end
        end
    end
end
saveData.ind = ind;

% Some parametric terms in p(x,t) do not appear in v'Qv
% So these have to be added 0*Q = b
not_dealt_with  = find(used_in_p==0);
while ~isempty(not_dealt_with)
    j = findrows(exponent_p_monoms,exponent_p_monoms(not_dealt_with(1),:));
    allj(end+1,1:length(j)) = j(:)';
    used_in_p(j) = 1;
    not_dealt_with  = find(used_in_p==0);
    ind(end+1,1)=0;
end

if parametric
    % Inconsistent behaviour in MATLAB
    if size(allj,1)==1
        uu = [0;p_base_parametric];
        b = sum(uu(allj+1))';
    else
        uu = [0;p_base_parametric];
        b = sum(uu(allj+1),2)';
    end
else
    uu = [0;pcoeffs(:)];
    b = sum(uu(allj+1),2)';
end

b = b';
dualbase = ind;
j = 1;
A = cell(size(N,1),1);
for k = 1:size(N,1)
    A{k} = dualbase(:,j:j+n(k)^2-1);
    j = j + n(k)^2;
end

b_base = getbase(b);
if any(sum(abs(b_base(:,2:end)),2)==1)
    b;
end


function [Z,Q1,R] = sparsenull(A)

[Q,R] = qr(A');
n = max(find(sum(abs(R),2)));
Q1 = Q(:,1:n);
R = R(1:n,:);
Z = Q(:,n+1:end); % New basis


function [F,obj,BlockedQ,Primal_matrices,Free_variables] = create_kernelmodel(BlockedA,Blockedb,F_parametric,parobj,options,sparsityPattern);

% To get the primal kernel representation, we simply use
% the built-in dualization module!
% First, write the problem in primal kernel format
traceobj = 0;
dotraceobj = options.sos.traceobj;
F = F_parametric;
for i = 1:length(Blockedb)
    res = 0;
    for j = 1:length(BlockedA{i})
        n = sqrt(size(BlockedA{i}{j},2));
        BlockedQ{i}{j} = sdpvar(n,n);
        F = F + set(BlockedQ{i}{j});
        res = res + BlockedA{i}{j}*reshape(BlockedQ{i}{j},n^2,1);
        if dotraceobj
            traceobj = traceobj + trace(BlockedQ{i}{j});
        end
    end
    F = F + set(res == Blockedb{i});
end

% % Constrain elements according to a desired sparsity
if ~isempty(sparsityPattern)
    res = [];
    for i = 1:length(BlockedQ)
        for j = 1:length(BlockedQ{i})
            if ~isempty(sparsityPattern{i}{j})
                H = spalloc(length(BlockedQ{i}{j}),length(BlockedQ{i}{j}),length(sparsityPattern{i}{j}));
                H(sparsityPattern{i}{j}) = 1;
                k = find(triu(H));
                res = [res;BlockedQ{i}{j}(k)];
            end
        end
    end
    F = F + set(0 == res);
end

% And get the primal model of this
if isempty(parobj)
    if options.sos.traceobj
        [F,obj,Primal_matrices,Free_variables] = dualize(F,traceobj,1,options.sos.extlp);
    else
        [F,obj,Primal_matrices,Free_variables] = dualize(F,[],1,options.sos.extlp);
    end
else
    [F,obj,Primal_matrices,Free_variables] = dualize(F,parobj,1,options.sos.extlp);
end
% In dual mode, we maximize
obj = -obj;


function      [F,obj,BlockedQ,sol] = create_imagemodel(BlockedA,Blockedb,F_parametric,parobj,options);


% *********************************
% IMAGE REPRESENTATION
% Needed for nonlinearly parameterized problems
% More efficient in some cases
% *********************************
g = [];
vecQ = [];
sol.problem = 0;
for i = 1:length(BlockedA)
    q = [];
    A = [];
    for j = 1:length(BlockedA{i})
        n = sqrt(size(BlockedA{i}{j},2));
        BlockedQ{i}{j} = sdpvar(n,n);
        q = [q;reshape(BlockedQ{i}{j},n^2,1)];
        A = [A BlockedA{i}{j}];
    end
    vecQ = [vecQ;q];
    g = [g;Blockedb{i}-A*q];
end

g_vars = getvariables(g);
q_vars = getvariables(vecQ);
x_vars = setdiff(g_vars,q_vars);

base = getbase(g);
if isempty(x_vars)
    A = base(:,1);base = base(:,2:end);
    B = (base(:,ismember(g_vars,q_vars)));
    Bnull = sparsenull(B);
    t = sdpvar(size(Bnull,2),1);
    imQ = -B\A+Bnull*t;
else
    A = base(:,1);base = base(:,2:end);
    C = base(:,ismember(g_vars,x_vars));
    B = (base(:,ismember(g_vars,q_vars)));
    [Bnull,Q1,R1] = sparsenull(B);Bnull(abs(Bnull) < 1e-12) = 0;
    t = sdpvar(size(Bnull,2),1);
    imQ = -Q1*(R1'\(A+C*recover(x_vars)))+Bnull*t;
end
notUsed = find(sum(abs(B),2)==0);
if ~isempty(notUsed)
    ff=g(notUsed);
    if isa(ff,'double')
        if nnz(ff)>0
            error('You seem to have a strange model. Have you forgotten to define some parametric variable?');
        end
    else
        F_parametric = F_parametric + set(g(notUsed)==0);
    end
end
F_sos = set([]);
obj = 0;
for i = 1:length(BlockedQ)
    for j = 1:size(BlockedQ{i},2)
        Q_old = BlockedQ{i}{j};
        Q_old_vars = getvariables(Q_old);
        Q_old_base = getbase(Q_old);
        in_this = [];
        for k = 1:length(Q_old_vars)
            in_this = [in_this;find(Q_old_vars(k)==q_vars)];
        end
        Q_new = Q_old_base(:,1) + Q_old_base(:,2:end)*imQ(in_this);
        Q_new = reshape(Q_new,length(BlockedQ{i}{j}),length(BlockedQ{i}{j}));
        obj = obj+trace(Q_new);
        if ~isa(Q_new,'double')                   
            F_sos = F_sos + set(Q_new);
        elseif min(eig(Q_new))<-1e-8
            sol.yalmiptime = 0; % FIX
            sol.solvertime = 0;
            sol.problem = 2;
            sol.info = yalmiperror(1,'YALMIP');
            F = [];
            obj = [];            
            error('Problem is trivially infeasible. After block-diagonalizing, I found constant negative definite blocks!');
            return
        end
        BlockedQ{i}{j} = Q_new;
    end
end

F = F_parametric + F_sos;

if isa(obj,'double') | (options.sos.traceobj == 0)
    obj = [];
end

if ~isempty(parobj)
    obj = parobj;
end


function   [F,obj,options] = create_lrmodel(BlockedA,Blockedb,F_parametric,parobj,options,ParametricVariables)
% Some special code for ther low-rank model in SDPLR
% Experimental code, not official yet
allb = [];
allA = [];
K.s = [];
for i = 1:length(Blockedb)
    allb = [allb;Blockedb{i}];
    Ai = [];
    for j = 1:size(BlockedA{i},2)
        Ai = [Ai BlockedA{i}{j}];
        K.s = [K.s sqrt(size(BlockedA{i}{j},2))];
    end
    %blkdiag bug in 7.0...
    [n1,m1] = size(allA);
    [n2,m2] = size(Ai);
    allA = [allA spalloc(n1,m2,0);spalloc(n2,m1,0) Ai];
end
options.solver = 'sdplr';
z = recover(ParametricVariables)
start = size(BlockedA,2)+1;
Mi = [];
for i = 1:length(allb)
    if isa(allb(i),'sdpvar')
        [Qi,ci,fi,xi,infoi] = quaddecomp(allb(i),z);
    else
        Qi = zeros(length(z));
        ci = zeros(length(z),1);
        fi = allb(i);
    end
    Z = -[fi ci'/2;ci/2 Qi];
    Mi = [Mi;Z(:)'];
end
K.s = [K.s length(z)+1];
zeroRow = zeros(1,size(allA,2));
allA = [allA Mi;zeroRow 1 zeros(1,K.s(end)^2-1)];
b = zeros(size(allA,1),1);b(end) = 1;
y = sdpvar(length(b),1);
CminusAy = -allA'*y;
start = 1;

% Get the cost, expressed in Z
[Qi,ci,fi,xi,infoi] = quaddecomp(parobj,z);
C = [fi ci'/2;ci/2 Qi];
F = set([]);
for i = 1:length(K.s)
    if i<length(K.s)
        F = F + set(reshape(CminusAy(start:start+K.s(i)^2-1),K.s(i),K.s(i)));
    else
        F = F + set(reshape(C(:) + CminusAy(start:start+K.s(i)^2-1),K.s(i),K.s(i)));
    end
    start = start + K.s(i)^2;
end
obj = -b'*y;

options.sdplr.forcerank = ceil(K.s/2);
options.sdplr.forcerank(end) = 1;
options.sdplr.feastol = 1e-7;