callmpcvx.m

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

                    if x==y
                        p.lb(p.nonlins(i,1)) = max(0,min(bounds));
                        p.ub(p.nonlins(i,1)) = max(bounds);
                    else
                        p.lb(p.nonlins(i,1)) = min(bounds);
                        p.ub(p.nonlins(i,1)) = max(bounds);    
                    end
                end
            end
        end
    else    
        for i = 1:size(p.nonlins,1)
            z = p.nonlins(i,1);
            x = p.nonlins(i,2);
            y = p.nonlins(i,3);
            bound_x1 = [p.lb(p.nonlins(i,2));p.ub(p.nonlins(i,2))];
            bound_x2 = [p.lb(p.nonlins(i,3));p.ub(p.nonlins(i,3))];
            bounds = [bound_x1(1)*bound_x2(1) bound_x1(1)*bound_x2(2) bound_x1(2)*bound_x2(1) bound_x1(2)*bound_x2(2)];
            if x==y
                p.lb(p.nonlins(i,1)) = max( p.lb(p.nonlins(i,1)) ,max(0,min(bounds)));
                p.ub(p.nonlins(i,1)) = min( p.ub(p.nonlins(i,1)) ,max(bounds));
            else
                p.lb(p.nonlins(i,1)) = max(p.lb(p.nonlins(i,1)),min(bounds));
                p.ub(p.nonlins(i,1)) = min(p.ub(p.nonlins(i,1)),max(bounds));    
            end
        end
    end
    
    % *************************************
    % DERIVE LINEAR CUTS FROM SDPs
    % THESE ARE ONLY USED IN BOXREDUCE
    % *************************************
    function p = addsdpcut(p,x)
    if p.K.s > 0
        top = p.K.f+p.K.l+1;
        newcuts = 1;
        newF = [];
        for i = 1:length(p.K.s)
            n = p.K.s(i);
            X = p.F_struc(top:top+n^2-1,:)*[1;x];
            X = reshape(X,n,n);
            [d,v] = eig(X);
            for m = 1:length(v)
                if v(m,m)<0
                    for j = 1:length(x)+1;
                        newF(newcuts,j)= d(:,m)'*reshape(p.F_struc(top:top+n^2-1,j),n,n)*d(:,m);
                    end
                    % max(abs(newF(:,2:end)),[],2)
                    newF(newcuts,1)=newF(newcuts,1)+1e-6;
                    newcuts = newcuts + 1;
                    if size(p.lpcuts,1)>0
                        dist = p.lpcuts*newF(newcuts-1,:)'/(newF(newcuts-1,:)*newF(newcuts-1,:)');
                        if any(abs(dist-1)<1e-3)
                            newF = newF(1:end-1,:);
                            newcuts = newcuts - 1;
                        end
                    end
                end
            end
            top = top+n^2;
        end
        
        if ~isempty(newF)
            % Don't keep all
            m = size(newF,2);
            %  size(p.lpcuts)
            p.lpcuts = [newF;p.lpcuts];
            violations = p.lpcuts*[1;x];
            p.lpcuts = p.lpcuts(violations<0.1,:);
            
            if size(p.lpcuts,1)>15*m
                violations = p.lpcuts*[1;x];
                [i,j] = sort(violations);
                p.lpcuts = p.lpcuts(j(1:15*m),:);
                p.lpcuts = p.lpcuts(end-15*m+1:end,:);
            end
        end
    end
    
    
    function spliton = branchvariable(p,options,x)
    
    % Split if box is to narrow
    width = abs(p.ub(p.branch_variables)-p.lb(p.branch_variables));
    if min(width)/max(width) < 0.1
        [i,j] = max(width);
        spliton = p.branch_variables(j);
    else
        %     res = zeros(length(p.lb),1);
        %     for i = 1:size(p.nonlins,1)
        %         res(p.nonlins(i,2)) = res(p.nonlins(i,2)) + abs( x(p.nonlins(i,1))-x(p.nonlins(i,2)).*x(p.nonlins(i,3)));
        %         res(p.nonlins(i,3)) = res(p.nonlins(i,3)) + abs( x(p.nonlins(i,1))-x(p.nonlins(i,2)).*x(p.nonlins(i,3)));
        %     end
        %     
        %      [ii,jj] = sort(abs(res));               
        %      spliton = jj(end);    
        
        res = x(p.nonlins(:,1))-x(p.nonlins(:,2)).*x(p.nonlins(:,3));
        [ii,jj] = sort(abs(res));               
        v1 = p.nonlins(jj(end),2);
        v2 = p.nonlins(jj(end),3);
        
        acc_res1 = sum(abs(res(find((p.nonlins(:,2)==v1) |  p.nonlins(:,3)==v1))));
        acc_res2 = sum(abs(res(find((p.nonlins(:,2)==v2) |  p.nonlins(:,3)==v2))));
        
        if (acc_res1>acc_res2) & ismember(v1,p.branch_variables)
            spliton = v1;
        else
            spliton = v2;
        end         
    end
    
    function bounds = partition(p,options,spliton,x_min)
    
    switch options.bmibnb.branchrule
        case 'omega'
            if ~isempty(x_min)
                bounds = [p.lb(spliton) 0.5*max(p.lb(spliton),min(x_min(spliton),p.ub(spliton)))+0.5*(p.lb(spliton)+p.ub(spliton))/2 p.ub(spliton)];                                          
            else
                bounds = [p.lb(spliton) (p.lb(spliton)+p.ub(spliton))/2 p.ub(spliton)];                                          
            end
        case 'bisect'
            bounds = [p.lb(spliton) (p.lb(spliton)+p.ub(spliton))/2 p.ub(spliton)];                                          
        otherwise
            bounds = [p.lb(spliton) (p.lb(spliton)+p.ub(spliton))/2 p.ub(spliton)]; 
    end
    
    
    
    function [p,feasible,vol_reduction] = boxreduce(p,upper,lower,lpsolver,options);
    
    if options.bmibnb.lpreduce
        
        vol_start    = prod(p.ub(p.branch_variables)-p.lb(p.branch_variables));
        diag_before  =  sum(p.ub(p.branch_variables)-p.lb(p.branch_variables));
        diag_before0 = diag_before;
        
        [pcut,feasible,lower] = lpbmitighten(p,lower,upper,lpsolver);
        diag_after = sum(pcut.ub(p.branch_variables)-pcut.lb(p.branch_variables));      
        iterations = 0;
        while (diag_after/(1e-18+diag_before) < 0.75    ) & feasible & iterations<4
            [pcut,feasible,lower] = lpbmitighten(pcut,lower,upper,lpsolver);
            diag_before = diag_after;
            diag_after = sum(pcut.ub(p.branch_variables)-pcut.lb(p.branch_variables));        
            iterations = iterations + 1;
        end        
        
        % Clean up...
        for i = 1:length(pcut.lb)
            if (pcut.lb(i)>pcut.ub(i)) & (pcut.lb-pcut.ub < 1e-3)
                pcut.lb(i)=pcut.ub(i);
                pcut = updatenonlinearbounds(pcut,i);            
            end
        end
        p.lb = pcut.lb;
        p.ub = pcut.ub;    
        
        % Metric = (V0/V)^(1/n)
        vol_reduction = max(0,min(1,(prod(p.ub(p.branch_variables)-p.lb(p.branch_variables))/(1e-12+vol_start))^(1/length(p.branch_variables))));
        p.lb(p.lb<-1e12) = -inf;
        p.ub(p.ub>1e12) = inf;
    else
        vol_reduction = 1;
        feasible = 1;
    end
    
    function output = solvelower(p,options,lowersolver)
    
    % ********************************************
    % Convex envelope
    % ********************************************
    %p.binary_variables = [];
    p_with_bilinear_cuts = p;
    p_with_bilinear_cuts.F_struc(1:p.K.f,:)=[];
    p_with_bilinear_cuts = addmcgormick(p_with_bilinear_cuts);   
    p_with_bilinear_cuts.F_struc = [p.F_struc(1:p.K.f,:);p_with_bilinear_cuts.F_struc];    
    
    % **************************************
    % SOLVE NODE PROBLEM 
    % **************************************
    if any(p_with_bilinear_cuts.ub<p_with_bilinear_cuts.lb) 
        output.problem=1;
    else
        % We are solving relaxed problem (penbmi might be local solver)
        p_with_bilinear_cuts.monomtable = eye(length(p_with_bilinear_cuts.c));
        
        % fix implied  from mccormick
        % p_with_bilinear_cuts.lb(p.linears) = -inf;
        % p_with_bilinear_cuts.ub(p.linears) = inf;
        % p_with_bilinear_cuts.lb(p.nonlins(:,1)) = -inf;
        % p_with_bilinear_cuts.ub(p.nonlins(:,1)) = inf;
        
        output = feval(lowersolver,p_with_bilinear_cuts); 
        
        relaxed_residual = resids(p_with_bilinear_cuts,output.Primal);
        % Minor clean-up
        output.Primal(output.Primal<p.lb) = p.lb(output.Primal<p.lb);
        output.Primal(output.Primal>p.ub) = p.ub(output.Primal>p.ub);    
    end 
    
    function [p,stack] = selectbranch(p,options,stack,x_min,upper);
    switch options.bmibnb.branchmethod
        case 'maxvol'
            [node,stack] = pull(stack,'maxvol',x_min,upper); 
        case 'best'
            [node,stack] = pull(stack,'best',x_min,upper); 
        otherwise
            [node,stack] = pull(stack,'best',x_min,upper); 
    end
    % Copy node data to p
    if isempty(node)
        p = [];
    else
        p.depth = node.depth;
        p.dpos = node.dpos;
        p.lb = node.lb;
        p.ub = node.ub;
        p.lower = node.lower;
        p.lpcuts = node.lpcuts;
        p.x0 = node.x0;
    end
    
    
    
    function output = solveupper(p,p_original,x,options,uppersolver)
    
    p_upper = p_original;
    
    % Pick an initial point (this can be a bit tricky...)
    % Use relaxed point, shifted towards center of box
    if all(x<=p.ub) & all(x>=p.lb)
        p_upper.x0 = 0.1*x + 0.9*(p.lb+p.ub)/2;
    else
        p_upper.x0 = (p.lb+p.ub)/2;
    end
    % Shift towards interior for variables with unbounded lower or upper
    lbinfbounds = find(isinf(p.lb));
    ubinfbounds = find(isinf(p.ub));
    p_upper.x0(ubinfbounds) = x(ubinfbounds)+0.01;
    p_upper.x0(lbinfbounds) = x(lbinfbounds)-0.01;
    ublbinfbounds = find(isinf(p.lb) & isinf(p.ub));
    p_upper.x0(ublbinfbounds) = x(ublbinfbounds);
    % ...expand the current node just slightly
    p_upper.lb = p.lb;
    p_upper.ub = p.ub;
    p_upper.lb(~isinf(p_original.lb)) = 0.99*p.lb(~isinf(p_original.lb))+p_original.lb(~isinf(p_original.lb))*0.01;
    p_upper.ub(~isinf(p_original.ub)) = 0.99*p.ub(~isinf(p_original.ub))+p_original.ub(~isinf(p_original.ub))*0.01;
    p_upper.lb(isinf(p_original.lb)) = p_upper.lb(isinf(p_original.lb)) - 0.001;
    p_upper.ub(isinf(p_original.ub)) = p_upper.ub(isinf(p_original.ub)) + 0.001;
    p_upper.options.saveduals = 0;
    
    % Solve upper bounding problem
    p_upper.options.usex0 = 1;
    output = feval(uppersolver,p_upper);
    % Project into the box (numerical issue)
    output.Primal(output.Primal<p_upper.lb) = p_upper.lb(output.Primal<p_upper.lb);
    output.Primal(output.Primal>p_upper.ub) = p_upper.ub(output.Primal>p_upper.ub);
    
    
    % This one needs a lot of work
    function p = nonlinear_constraint_propagation(p)
    
    for i = 1:size(p.nonlins,1)
        x = p.nonlins(i,2);
        y = p.nonlins(i,3);
        z = p.nonlins(i,1);
        
        if y==x & p.ub(z)>0
            p.ub(x) = min(p.ub(x),sqrt(p.ub(z)));
            p.lb(x) = max(p.lb(x),-sqrt(p.ub(z)));
        end
        
        if p.lb(y)>0 & p.ub(z)>0 & p.ub(x)>0
            p.ub(x) = min(p.ub(x),p.ub(z)/p.lb(y));
        end
        if p.lb(x)>0 & p.ub(z)>0 & p.ub(y)>0
            p.ub(y) = min(p.ub(y),p.ub(z)/p.lb(x));
        end
        
        if p.ub(y)>0 & p.lb(y)>0 & p.lb(z)>0 
            p.lb(x) = max(p.lb(x),p.lb(z)/p.ub(y));
        end
        if p.ub(x)>0 & p.lb(x)>0 & p.lb(z)>0 
            p.lb(y) = max(p.lb(y),p.lb(z)/p.ub(x));
        end    
    end
    
    
    function vars = decide_branch_variables(p)
    
    if p.options.bmibnb.lowrank==0
        nonlinear = find(~(sum(p.monomtable~=0,2)==1 & sum(p.monomtable,2)==1));
        vars      =  find(sum(abs(full(p.monomtable(nonlinear,:))),1));
    else
        pool1 = p.nonlins(1,2);
        pool2 = p.nonlins(1,3);
        
        for i = 2:size(p.nonlins,1)
            v1 = p.nonlins(i,2);
            v2 = p.nonlins(i,3);
            if v1==v2
                % We are fucked
                pool1 = [pool1 v1];
                pool2 = [pool2 v2];
            else
                if ismember(v1,pool1)
                    pool2 = [pool2 v2];
                elseif ismember(v1,pool2)
                    pool1 = [pool1 v2];
                elseif ismember(v2,pool1)
                    pool2 = [pool2 v1];
                elseif ismember(v2,pool2)
                    pool1 = [pool1 v1];
                else
                    % No member yet
                    pool1 = [pool1 v1];
                    pool2 = [pool2 v2];
                end
            end
        end
        pool1 = unique(pool1);
        pool2 = unique(pool2);
        if isempty(intersect(pool1,pool2))
            if length(pool1)<=length(pool2)
                vars = pool1;
            else
                vars = pool2;
            end
        else
            nonlinear          = find(~(sum(p.monomtable~=0,2)==1 & sum(p.monomtable,2)==1));
            vars =  find(sum(abs(full(p.monomtable(nonlinear,:))),1));  
        end
    end
    
    
    function x = evaluate_nonlinear(p,x);
    x(p.nonlins(:,1)) = x(p.nonlins(:,2)).*x(p.nonlins(:,3));
    
    function p = clean_bounds(p)
    % Fix to improve numerica with integer bounds
    %close = find(1e-6>abs(p.ub - round(p.ub)));
    %p.ub(close) = round(p.ub(close));
    close = 1e-6>abs(p.ub - round(p.ub));
    p.ub(close) = round(p.ub(close));
    
    close = 1e-6>abs(p.lb - round(p.lb));
    p.lb(close) = round(p.lb(close));
    
    p.ub(p.binary_variables) = floor(p.ub(p.binary_variables) + 1e-2);
    %p.lb(p.binary_variables) =  ceil(p.lb(p.binary_variables) - 1e-2);
    %p = updatenonlinearbounds(p);
    
    % Nothing coded to do non-linear propagation
    %p = nonlinear_constraint_propagation(p);
    p.lb(p.lb<-1e12) = -inf;
    p.ub(p.ub>1e12) = inf;
    
    
    
    function node = savetonode(p,spliton,bounds1,bounds2,direction,x,cost);
    
    node.lb = p.lb;
    node.ub = p.ub;
    node.lb(spliton) = bounds1;
    node.ub(spliton) = bounds2;
    if direction == -1
        node.dpos = p.dpos-1/(2^sqrt(p.depth));
    else
        node.dpos = p.dpos+1/(2^sqrt(p.depth));
    end
    node.depth = p.depth+1;
    node.x0 = x;
    node.lpcuts = p.lpcuts;
    node.lower = cost;