dualize.m

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

        Fi = sdpvar(F(i));
        n = size(Fi,1);
        S = sdpvar(n,1);
        slack = Fi-S;
        F_AXb =  F_AXb + set(slack==0);
        F_CONE = F_CONE + set(cone(S(2:end),S(1)));
        shiftMatrix{end+1} = spalloc(n,1,0);
        X{end+1}=S;
        keep(i)=0;
    end
end

% Now, remove all mixed cones...
F = F(find(keep));

% Get the equalities
AXbset = is(F,'equality');
if any(AXbset)
    % Get the constraints
    F_AXb = F_AXb + F(find(AXbset));
    F = F(find(~AXbset));
end

% Is thee something we missed in our tests?
if length(F)>0
    error('DUALIZE can only treat standard SDPs (and LPs) at the moment.')
end


% Sort the SDP cone variables X according to YALMIP
% This is just to simplify some indexing later
ns = [];
first_var = [];
for i = 1:length(F_CONE)    
    ns = [ns length(X{i})];
    first_var = [first_var min(getvariables(X{i}))];    
end
[sorted,index] = sort(first_var);
X={X{index}};
shiftMatrix={shiftMatrix{index}};

shift = [];
for i = 1:length(F_CONE)     
    ns(i) = length(X{i});
    if size(X{i},2)==1
        % SOCP
        shift = [shift;shiftMatrix{i}(:)];
    else
        % SDP
        ind =  find(tril(reshape(1:ns(i)^2,ns(i),ns(i))));
        shift = [shift;shiftMatrix{i}(ind)];
    end
end

% Free variables (here called t) is everything except the cone variables
X_variables = getvariables(F_CONE);
x_variables = getvariables(x);
Xx_variables = [X_variables x_variables]; 
other_variables = [getvariables(obj) getvariables(F_AXb)];
all_variables = uniquestripped([other_variables Xx_variables]);
t_variables = setdiff(all_variables,Xx_variables);

t = recover(t_variables);

ind = ismembc(all_variables,t_variables);
t_in_all = find(ind);
ind = ismembc(all_variables,x_variables);
x_in_all = find(ind);
ind = ismembc(all_variables,X_variables);
X_in_all = find(ind);

vecF1 = [];
nvars = length(all_variables);
for i = 1:length(F_AXb)
    AXb = sdpvar(F_AXb(i));
    mapper = find(ismembc(all_variables,getvariables(AXb)));

    [n,m] = size(AXb);
    data = getbase(AXb);
    F_structemp  = spalloc(n*m,1+nvars,nnz(data));
    F_structemp(:,[1 1+mapper(:)'])= data;
    vecF1 = [vecF1;F_structemp];
end
if isempty(obj)
    vecF1(end+1,1) = 0;
else
    mapper = find(ismembc(all_variables,getvariables(obj)));
    [n,m] = size(obj);
    data = getbase(obj);
    F_structemp  = spalloc(n*m,1+nvars,nnz(data));
    F_structemp(:,[1 1+mapper(:)'])= data;
    vecF1 = [vecF1;F_structemp];
end
vecF1(end+1,1) = 0;
Fbase = vecF1;


b = Fbase(1:end-2,1);

Fbase = -Fbase(1:end-1,2:end);
vecA = [];
Fbase_t = Fbase(:,t_in_all);
Fbase_x = Fbase(:,x_in_all);
Fbase_X = Fbase;
%Fbase_X(:,unionstripped(t_in_all,x_in_all)) = [];
Fbase_X(:,[t_in_all x_in_all]) = [];

% Shift due to translated dual cones X = Z+shift
if length(shift > 0)
    b = b + Fbase_X(1:end-1,:)*shift;
end
if length(x)>0
    % Arrgh
    base = getbase(x);
    constant = base(:,1);
    base = base(:,2:end);[i,j,k] = find(base);
    b = b + Fbase_x(1:end-1,:)*constant(i);
end

start = 0;
n_cones = length(ns);
% All LPs in one cone
if length(x)>0
    n_lp = 1;
else
    n_lp = 0;
end
n_free = length(t_variables);

% SDP cones
for j = 1:1:n_cones

    if size(X{j},1)==size(X{j},2)
        % SDP cone
        ind = reshape(1:ns(j)^2,ns(j),ns(j));
        ind = find(tril(ind));

        % Get non-symmetric constraint AiX=b
        Fi = Fbase_X(1:length(b),start+(1:length(ind)))'/2;
        Ai = spalloc(ns(j)^2,length(b),nnz(Fi));
        Ai(ind,:) = Fi;
        % Symmetrize
        [rowcols,varindicies,vals]=find(Ai);
        the_col = 1+floor((rowcols-1)/ns(j));
        the_row = rowcols-(the_col-1)*ns(j);
        these_must_be_transposed = find(the_row > the_col);
        if ~isempty(these_must_be_transposed)
            new_rowcols = the_col(these_must_be_transposed) + (the_row(these_must_be_transposed)-1)*ns(j);
            Ai(sub2ind(size(Ai),new_rowcols,varindicies(these_must_be_transposed))) = vals(these_must_be_transposed);
        end
        % Fix diagonal term
        diags = find(diag(1:ns(j)));
        Ai(diags,:) = 2*Ai(diags,:);
        vecA{j} = Ai;

        start = start + length(ind);
    else
        % Second order cone
        ind = 1:ns(j);

        % Get constraint AiX=b
        Fi = Fbase_X(1:length(b),start+(1:length(ind)))';
        Ai = spalloc(ns(j),length(b),nnz(Fi));
        Ai(ind,:) = Fi;
        vecA{j} = Ai;
        start = start + length(ind);
    end

end
% LP Cone
if n_lp>0  
    Alp=Fbase_x(1:length(b),:)';
end
% FREE VARIABLES
start = 0;
if n_free>0    
    Afree=Fbase_t(1:length(b),:)';
end

% COST MATRIX
% SDP CONE
start = 0;
for j = 1:1:n_cones
    if size(X{j},1)==size(X{j},2)
        ind = reshape(1:ns(j)^2,ns(j),ns(j));
        ind = find(tril(ind));
        C{j} = spalloc(ns(j),ns(j),0);
        C{j}(ind) = -Fbase_X(end,start+(1:length(ind)));
        C{j} = (C{j}+ C{j}')/2;
        start = start + length(ind);
    else
        ind = 1:ns(j);
        C{j} = spalloc(ns(j),1,0);
        C{j}(ind) = -Fbase_X(end,start+(1:length(ind)));
        start = start + length(ind);
    end
end
% LP CONE
for j = 1:1:n_lp
    Clp = -Fbase_x(end,:)';
end
% FREE CONE
if n_free>0
    Cfree = -Fbase_t(end,1:end)';
end

% Create dual
y = sdpvar(length(b),1);
yvars = getvariables(y);
cf = [];
Af = [];
Fdual = set([]);
for j = 1:n_cones
    if size(X{j},1)==size(X{j},2)
        % Yep, this is optimized...
        S = sdpvar(ns(j),ns(j),[],yvars,[C{j}(:) -vecA{j}]);        
        Fdual = Fdual + lmi(S);
    else
        Ay = reshape(vecA{j}*y,ns(j),1);
        S = C{j}-Ay;
        Fdual = Fdual + lmi(cone(S(2:end),S(1)));
    end
end
if n_lp > 0
    keep = any(Alp,2);
    if ~all(keep)
        % Fix for unused primal cones
        preset=find(~keep);
        xfix = x(preset);        
        assign(xfix(:),Clp(preset(:)));       
    end
    keep = find(keep);
    if ~isempty(keep)
        z = Clp(keep)-Alp(keep,:)*y;
        if isa(z,'double')
            assign(x(:),z(:));
        else
            Fdual = Fdual + set(z);
            x =x(keep);
            X{end+1} = x(:); % We have worked with a row for performance reasons
        end
    end
end
if n_free > 0
    CfreeAfreey = Cfree-Afree*y;
    if isa(CfreeAfreey,'double')
        if nnz(CfreeAfreey)>0        
            error('Trvially unbounded!');
        end
    else
        Fdual = Fdual + set(0 == CfreeAfreey);
    end
end

objdual = b'*y;
if auto
    for i = 1:length(X)
        yalmip('associatedual',getlmiid(Fdual(i)),X{i});
    end
    if n_free>0
        yalmip('associatedual',getlmiid(Fdual(end)),t);
    end
end