callmpcvx.m

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

                z = evaluate_nonlinear(p,x);
                p = addsdpcut(p,z);        
                
                % Maybe the relaxed solution is feasible
                relaxed_residual = resids(p_upper,z);
                relaxed_feasible = all(relaxed_residual(1:p.K.f)>=-options.bmibnb.eqtol) & all(relaxed_residual(1+p.K.f:end)>=options.bmibnb.pdtol);
                if relaxed_feasible
                    this_upper = f+c'*z+z'*Q*z;
                    if (this_upper < (1-1e-5)*upper) & (this_upper < upper - 1e-5)
                        x_min = z;
                        upper = this_upper;
                        info_text = 'Improved upper bound';
                        cost = cost-1e-10; % Otherwise we'll fathome!
                    end        
                end
                
                % UPDATE THE LOWER BOUND                
                if isnan(lower)
                    lower = cost;
                end
                if ~isempty(stack)
                    lower =min(cost,min([stack.lower]));
                else
                    lower = min(lower,cost);             
                end
                
                if cost<upper 
                    output = solveupper(p,p_upper,x,options,uppersolver);
                    xu = evaluate_nonlinear(p,output.Primal);
                    upper_residual = resids(p_upper,xu);                   
                    if output.problem == 0 | (all(upper_residual(1:p_upper.K.f)>=-options.bmibnb.eqtol) & all(upper_residual(1+p_upper.K.f:end)>=options.bmibnb.pdtol))
                        this_upper = f+c'*xu+xu'*Q*xu;
                        if (this_upper < (1-1e-5)*upper) & (this_upper < upper - 1e-5)
                            x_min = xu;
                            upper = this_upper;
                            info_text = 'Improved upper bound';
                        end                    
                    end
                else
                    if doplot;ellipplot(diag([200 25]),1,'b',[p.dpos;-p.depth]);drawnow;end
                    info_text = 'Poor lower bound';
                    keep_digging = 0;
                end
            otherwise
            end
        else
            if doplot;ellipplot(diag([200 25]),1,'r',[p.dpos;-p.depth]);drawnow;end
            info_text = 'Infeasible during box-reduction';
            keep_digging = 0;
            cost = inf;
            feasible = 0;          
        end
        solved_nodes = solved_nodes+1;
        
        if ~isempty(stack)
            [stack,lower] = prune(stack,upper,options,solved_nodes); 
        end            
        lower = min(lower,cost); 
        
        % **********************************
        % CONTINUE SPLITTING?
        % **********************************   
        if keep_digging & max(p.ub(p.branch_variables)-p.lb(p.branch_variables))>options.bmibnb.vartol
            spliton = branchvariable(p,options,x);
            bounds  = partition(p,options,spliton,x_min); 
            
            node_1 = savetonode(p,spliton,bounds(1),bounds(2),-1,x,cost);
            node_2 = savetonode(p,spliton,bounds(2),bounds(3),1,x,cost);
            stack = push(stack,node_1);
            stack = push(stack,node_2);        
        end
        
        % Pick and create a suitable node to continue on
        [p,stack] = selectbranch(p,options,stack,x_min,upper);
        
        if isempty(p)
            if ~isinf(upper)
                relgap = 0;
            end
            depth = 0;          
        else      
            relgap = 100*(upper-lower)/(1+abs(upper));
            depth = p.depth;
        end    
        if options.bmibnb.verbose>0
            ws = whos; %Work-space
            Mb = sum([ws(:).bytes])/1024^2; %Megs
            showprogress(sprintf(['%3d ' info_text repmat(' ',1,35-length(info_text)) ' %8.2f%%  %8.4f  %8.4f  %8.4f  %3d  %3d    %5.2fMB    %4.1f%%  '],solved_nodes,relgap,upper,cost,lower,depth,length(stack),Mb,100-vol_reduction*100),options.bmibnb.verbose)
        end                            
        
        absgap = upper-lower;
        % **************************************
        % Continue?
        % **************************************    
        time_ok = cputime-t_start < options.bmibnb.maxtime;
        iter_ok = solved_nodes < options.bmibnb.maxiter;
        any_nodes = ~isempty(p);   
        relgap_too_big = (isinf(lower) | isnan(relgap) | relgap>100*options.bmibnb.relgaptol);
        absgap_too_big = (isinf(lower) | isnan(absgap) | absgap>options.bmibnb.absgaptol);
        taget_not_met = upper>options.bmibnb.target;
        go_on = taget_not_met & time_ok & any_nodes & iter_ok & relgap_too_big & absgap_too_big ;
    end
    if options.bmibnb.verbose>0
        fprintf('******************************************************************************************************************\n')
        if options.bmibnb.verbose;showprogress([num2str2(solved_nodes,3)  ' Finishing.  Cost: ' num2str(upper) ' Gap: ' num2str(relgap) '%'],options.bnb.verbose);end
        fprintf('******************************************************************************************************************\n')
    end
    
    
    
    function stack = push(stackin,p)
    if ~isempty(stackin)
        stack = [p;stackin];
    else
        stack(1)=p;
    end
    
    function [p,stack] = pull(stack,method,x_min,upper);
    if ~isempty(stack)
        switch method
            case 'maxvol'
                for i = 1:length(stack)
                    vol(i) = sum(stack(i).ub(stack(i).branch_variables)-stack(i).lb(stack(i).branch_variables));
                end
                [i,j] = max(vol);
                p=stack(j);  
                stack = stack([1:1:j-1 j+1:1:end]); 
                
            case 'best'
                [i,j]=min([stack.lower]);
                p=stack(j);  
                stack = stack([1:1:j-1 j+1:1:end]);  
                
            otherwise        
        end
    else
        p = [];
    end
    
    
    function s = num2str2(x,d,c);
    if nargin==3
        s = num2str(x,c);
    else
        s = num2str(x);
    end
    s = [repmat(' ',1,d-length(s)) s];
    
    
    function res = resids(p,x)
    res= [];
    if p.K.f>0
        res = -abs(p.F_struc(1:p.K.f,:)*[1;x]); 
    end
    if p.K.l>0
        res = [res;p.F_struc(p.K.f+1:p.K.f+p.K.l,:)*[1;x]];
    end
    if (length(p.K.s)>1) | p.K.s>0
        top = 1+p.K.f+p.K.l;
        for i = 1:length(p.K.s)
            n = p.K.s(i);
            X = p.F_struc(top:top+n^2-1,:)*[1;x];top = top+n^2;
            X = reshape(X,n,n);
            res = [res;min(eig(X))];
        end
    end
    
    res = [res;min([p.ub-x;x-p.lb])];
    
    
    function [stack,lower] = prune(stack,upper,options,solved_nodes)
    % *********************************
    % PRUNE STACK W.R.T NEW UPPER BOUND
    % *********************************
    if ~isempty(stack)
        toolarge = find([stack.lower]>upper*(1-1e-4));
        if ~isempty(toolarge)
            if options.bnb.verbose;showprogress([num2str2(solved_nodes,3) ' Pruned ' num2str(length(toolarge))  '  nodes'],options.bnb.verbose-1);end
            stack(toolarge)=[];
        end
    end
    if ~isempty(stack)
        lower = min([stack.lower]);
    else
        lower = upper;
    end
    
    function pcut = addmcgormick(p)
    
    pcut = p;
    top = 0;
    row = [];
    col = [];
    val = [];
    F_temp = [];
    for i = 1:size(p.nonlins,1)
        z = p.nonlins(i,1);
        x = p.nonlins(i,2);
        y = p.nonlins(i,3);
        x_lb = p.lb(x);
        x_ub = p.ub(x);
        y_lb = p.lb(y);
        y_ub = p.ub(y);
        top = 0;
        row = [];
        col = [];
        val = [];
        
        if x~=y
            row = [1;1;1;1;2;2;2;2;3;3;3;3;4;4;4;4];
            col = [1 ; z+1 ; x+1 ; y+1 ; 1 ; z+1 ; x+1 ; y+1 ; 1 ; z+1 ; x+1 ; y+1 ; 1 ; z+1 ; x+1 ; y+1];
            val = [x_lb*y_lb;1;-y_lb;-x_lb;x_ub*y_ub;1;-y_ub;-x_ub;-x_ub*y_lb;-1;y_lb;x_ub;-x_lb*y_ub;-1;y_ub;x_lb];
            F_temp = [F_temp;sparse(row,col,val,4,size(pcut.F_struc,2))];
        else
            
            nr = 3;
            row = [1;1;1;2;2 ;2; 3; 3; 3];
            col = [1 ;z+1 ;x+1 ;1 ;z+1 ;x+1 ;1 ;z+1 ;x+1];
            val = [-x_ub*x_lb;-1;x_lb+x_ub;x_lb*y_lb;1;-y_lb-x_lb;x_ub*y_ub;1;-y_ub-x_ub];
            
            F_temp = [F_temp;sparse(row,col,val,nr,1+length(p.c))];
        end
        bounds = [x_lb*y_lb x_lb*y_ub x_ub*y_lb x_ub*y_ub];
        if x==y
            pcut.lb(pcut.nonlins(i,1)) = max(pcut.lb(pcut.nonlins(i,1)),max(0,min(bounds)));
        else
            pcut.lb(pcut.nonlins(i,1)) = max(pcut.lb(pcut.nonlins(i,1)),min(bounds));
        end
        pcut.ub(pcut.nonlins(i,1)) = min(pcut.ub(pcut.nonlins(i,1)),max(bounds));
    end
    
    keep = find(~isinf(F_temp(:,1)));
    F_temp = F_temp(keep,:);
    pcut.F_struc = [F_temp;pcut.F_struc];
    pcut.K.l = pcut.K.l+size(F_temp,1);  
    
    function [p,feasible,lower] = lpbmitighten(p,lower,upper,lpsolver)
    
    % Construct problem with only linear terms
    % and add cuts from lower/ upper bounds
    c = p.c;
    p_test = p;
    p_test.K.s = 0;
    p_test.F_struc = p_test.F_struc(1+p_test.K.f:1:p_test.K.l+p_test.K.f,:);
    
    if ~isnan(lower)
        p_test.F_struc = [-(p.lower-abs(p.lower)*0.01) p_test.c';p_test.F_struc];
    end
    if upper < inf
        p_test.F_struc = [upper+abs(upper)*0.01 -p_test.c';p_test.F_struc];
    end
    
    p_test.F_struc = [p_test.lpcuts;p_test.F_struc];
    p_test.K.l = size(p_test.F_struc,1);
    
    % Add cuts for nonlinear terms
    p_test = addmcgormick(p_test);
    
    p_test.F_struc = [p.F_struc(1:1:p.K.f,:);p_test.F_struc];
    
    
    feasible = 1;
    i = 1;
    
    p_test = clean_bounds(p_test);
    
    j = 1;
    n = ceil(max(p.options.bmibnb.lpreduce*length(p_test.linears),1));
    res = zeros(length(p.lb),1);
    for i = 1:size(p.nonlins,1)
        res(p.nonlins(i,2)) = res(p.nonlins(i,2)) + abs( p.x0(p.nonlins(i,1))-p.x0(p.nonlins(i,2)).*p.x0(p.nonlins(i,3)));
        res(p.nonlins(i,3)) = res(p.nonlins(i,3)) + abs( p.x0(p.nonlins(i,1))-p.x0(p.nonlins(i,2)).*p.x0(p.nonlins(i,3)));
    end
    res = res(p.linears);
    [ii,jj] = sort(abs(res));               
    jj = jj(end-n+1:end);    
    
    while feasible & j<=length(jj)
        i = p_test.linears(jj(j));
        if abs(p.ub(i)-p.lb(i)>0.1)
            p_test.c = eyev(length(p_test.c),i); 
            
            output = feval(lpsolver,p_test);
            if output.problem == 0
                if p_test.lb(i) < output.Primal(i)-1e-5
                    p_test.lb(i) = output.Primal(i);  
                    p_test = updateonenonlinearbound(p_test,i);
                end     
                p_test.c = -eyev(length(p_test.c),i);
                output = feval(lpsolver,p_test);
                if output.problem == 0
                    if p_test.ub(i) > output.Primal(i)+1e-5
                        p_test.ub(i) = output.Primal(i);
                        p_test = updateonenonlinearbound(p_test,i);        
                    end
                end
                if output.problem == 1
                    feasible = 0;
                end   
            end
            if output.problem == 1
                feasible = 0;
            end  
            p_test = clean_bounds(p_test);
        end
        j = j + 1;
    end
    p.lb = p_test.lb;
    p.ub = p_test.ub;
    
    
    function p = updateonenonlinearbound(p,changed_var);
    for i = 1:size(p.nonlins,1)
        x = p.nonlins(i,2);
        y = p.nonlins(i,3); 
        if (x==changed_var) | (y==changed_var)
            z = p.nonlins(i,1);        
            x_lb = p.lb(x);
            x_ub = p.ub(x);
            y_lb = p.lb(y);
            y_ub = p.ub(y);
            bounds = [x_lb*y_lb x_lb*y_ub x_ub*y_lb x_ub*y_ub];    
            if x==y        
                p.lb(p.nonlins(i,1)) = max([p.lb(z) 0 min(bounds)]);
                p.ub(p.nonlins(i,1)) = min(p.ub(z),max(bounds));
            else
                p.lb(p.nonlins(i,1)) = max(p.lb(z),min(bounds));
                p.ub(p.nonlins(i,1)) = min(p.ub(z),max(bounds));    
            end 
        end
    end
    
    
    function p = updatenonlinearbounds(p,changed_var,keepbest);
    % if nargin>1
    %     changed_var
    % else
    %     i = 1:size(p.nonlins,1);
    % end
    for i = 1:size(p.nonlins,1)
        z = p.nonlins(i,1);
        x = p.nonlins(i,2);
        y = p.nonlins(i,3);    
        x_lb = p.lb(x);
        x_ub = p.ub(x);
        y_lb = p.lb(y);
        y_ub = p.ub(y);
        bounds = [x_lb*y_lb x_lb*y_ub x_ub*y_lb x_ub*y_ub];    
        if x==y        
            p.lb(p.nonlins(i,1)) = max([p.lb(z) 0 min(bounds)]);
            p.ub(p.nonlins(i,1)) = min(p.ub(z),max(bounds));
        else
            p.lb(p.nonlins(i,1)) = max(p.lb(z),min(bounds));
            p.ub(p.nonlins(i,1)) = min(p.ub(z),max(bounds));    
        end 
    end
    return
    
    if nargin > 1
        
        for i = 1:size(p.nonlins,1)
            z = p.nonlins(i,1);
            x = p.nonlins(i,2);
            y = p.nonlins(i,3);
            if isempty(changed_var) | (x==changed_var) | (y == changed_var) | nargin==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 nargin==3
                    if x==y
                        p.lb(p.nonlins(i,1)) = max([p.lb(p.nonlins(i,1)) 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 
                else