miqp.m

由matlab开发的hybrid系统的描述语言

end  % while

% final results                                               
xmin   = xstar;
fmin   = zstar;

Extendedflag.QPiter = QPiter;
Extendedflag.optQP  = optQP;
Extendedflag.time   = toc;  

if exist('justcreated')
   if justcreated & deletefile
      delete('qphess.m')
   end   
end   

% ------------------------------------------------------------------------------
% Subroutines                                                           
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%
% push: puts a subproblem onto the STACK and increases the STACKSIZE    
%       input:  element = record containing the subproblem   
%               cost    = cost of the subproblem, according to this value the
%                         problem will be put on the appropriate place on the
%                         stack 
%       output: none                                                    
%       modifies global variables                                       

function push(element,cost)
   global STACK
   global STACKSIZE
   global STACKCOST
   
   % Determine position in STACK where problem is inserted, according to a best
   % first strategy
   
   ii = find(STACKCOST>=cost);  % EX: STACKCOST=[100 80 33 22 ^ 5 3 2], cost=10
   if ~isempty(ii),
      i=ii(end);
   else
      i=1;
   end
   
   for j = STACKSIZE:-1:i
      STACK(j+1)     = STACK(j);
      STACKCOST(j+1) = cost;
   end
   STACK(i)     = element;
   STACKSIZE    = STACKSIZE+1;
   STACKCOST(i) = cost;
      
% ------------------------------------------------------------------------------
%
% pop: returns top element of the STACK and decreases the STACKSIZE,    
%      eliminating the element from the stack                           
%      input:  none                                                     
%      output: record containing the top-subproblem                     
%      modifies global variables                                        
  
function subprob = pop
   global STACK
   global STACKSIZE
   global STACKCOST
  
   subprob        = STACK(STACKSIZE);
   STACKSIZE      = STACKSIZE-1;
   STACKCOST(end) = [];
  
% ------------------------------------------------------------------------------
%
% separate: generates 2 new suproblems from a given problem by       
%           branching on an arbitrary variable                       
%           input:  prob=problem to separate                                 
%                   branchvar=branching variable index. The variable 
%                   is x(vartype(branchvar)) in the coordinates of subproblem
%           output: the 2 subproblems in record format        
%                 zeroOK,    if set to one, this flags denote that setting 
%                 oneOK:     the current branching variable to zero (to one)
%                            is compatible with the box constraints lb and ub
%                            of the current relaxed QP. If set to zero, these 
%                            flags denote that the correponding problem should
%                            not be pushed onto the stack, since it is
%                            infeasible in terms of the original constraints

function [p0, p1, zeroOK, oneOK] = separate(prob, branchvar)
   if (length(prob.vartype) >= 1)
      nx    = size(prob.H,1);
      this  = prob.vartype(branchvar);
      others= [1:this-1, this+1:nx];

      % extract the values of the box bounds for the binary branching variable
      % this is used, to check, whether there are box bounds that do not allow 
      % to set one variable to a particular value
      
      lbbranch = prob.lb(this);
      ubbranch = prob.ub(this);
      
      if (lbbranch <= 0) & (ubbranch >= 0)
         zeroOK = 1;
      else
         zeroOK = 0;
      end
      
      if (lbbranch <= 1) & (ubbranch >= 1)
         oneOK = 1;
      else
         oneOK = 0;
      end   
      
      if (zeroOK == 0) & (oneOK == 0)
         error('box constraints on the binary variables are infeasible')
      end    
         
      % Generate new H                                                  
      % Partition old H into 4 blocks, some of which are possibly empty 
      % Note that the old H itself is not empty                         
      
      H11 = prob.H(others, others);
      H12 = prob.H(others, this);
      H22 = prob.H(this, this);
      
      p0.H = H11;
      p1.H = H11;
      
      % Generate new f                                                  
      % Partition old f into 2 blocks, some of which are possibly empty 
      % Note that a contribution from the partitioning of H is present  
      
      b1  = prob.f(others);
      b2  = prob.f(this);
      
      p0.f = b1;
      p1.f = b1(:)+H12(:);
      
      % Generate new A                                                  
      
      A = prob.A(:,others);

      p0.A = A;
      p1.A = A;
      
      % Generate new b                                                  
      % The only modification is a contribution from the matrix A       
      
      p0.b = prob.b;
      p1.b = prob.b - prob.A(:,this);
      
      % Generate new Aeq                                                  
      
      if size(prob.Aeq,2) > 0      
         Aeq    = prob.Aeq(:,others);
         p0.Aeq = Aeq;
         p1.Aeq = Aeq;
      else
         p0.Aeq	= prob.Aeq;
	 p1.Aeq = prob.Aeq;
      end	 
      
      % Generate new beq                                                 
      % The only modification is a contribution from the matrix A       
      
      if ~isempty(prob.beq)
         p0.beq = prob.beq;
         p1.beq = prob.beq - prob.Aeq(:,this);
      else
         p0.beq = prob.beq;
	 p1.beq = prob.beq;	 
      end
         
      % Generate new e                                                  
      % The only modification is a contribution from H22, b2            
      
      p0.e = prob.e;
      p1.e = prob.e+ .5*H22 + b2;
      
      % Generate new lb,ub,x0                                         
      
      if ~isempty(prob.lb),
          lb = prob.lb(others);
      else
          lb = [];
      end
      p0.lb = lb;
      p1.lb = lb;
      
      if ~isempty(prob.ub),
          ub = prob.ub(others);
      else
          ub = [];
      end
      p0.ub = ub;
      p1.ub = ub;

      if ~isempty(prob.x0),
          x0 = prob.x0(others);
      else
          x0 = [];
      end
      p0.x0 = x0;
      p1.x0 = x0;
      
      % Generate new vartype
      
      %EX:              1 2 3 4 5 6 7 8 9 
      %       old_vartype=[    3 4 5     8]'
      %       branchvar=5
      %       newvartype= [    3 4     7]'
  
      vartype = [prob.vartype(1:branchvar-1); ...
                 prob.vartype(branchvar+1:length(prob.vartype))-1];
       
      % Collect the terms for the new subproblems                     

      p0.vartype = vartype;
      p1.vartype = vartype;
      
      % Find the absolute index of the branching variable
      ifree   = find(prob.ivalues==-1); % Collect free integer variables
      ibranch = ifree(branchvar);       % Pick up the branch variable 

      aux         = prob.ivalues;
      aux(ibranch)= 0;
      p0.ivalues  = aux;   
      aux(ibranch)= 1;
      p1.ivalues  = aux;
      
      p0.level = prob.level+1;
      p1.level = prob.level+1;
      
      if ~isempty(prob.xxmax),
          xxmax = prob.xxmax(others);
      else
          xxmax = [];
      end
      p0.xxmax = xxmax;
      p1.xxmax = xxmax;      
      
   else
      error('no more integer variables to branch on')
   end
   
% ------------------------------------------------------------------------------
%
% decision: when a problem has to be separated, this function decides   
%           which will be the next branching variable                   
%           input: x       = present value of the solution of the qp            
%                  vartype = indices of the free integer variables relative to x
%                  br      = parameter denoting the branching rule that has to  
%                            be adopted     
%          output: branchvar = next branching variable position within vartype
  
function branchvar = decision(x,vartype,br);
   switch br
   case 'first'
       % first free variable is chosen as branching variable        
       branchvar = 1;
   case 'max'
       % integer free variable with maximal frac part is 
       % chosen as branching variable          
       xi          = x(vartype);
       [aux1,aux2] = max(abs(xi-round(xi)));
       branchvar   = aux2(1); % pick the first of the variables with max value
   case 'min'
       % integer free variable with minimal frac part is 
       % chosen as branching variable          
       xi          = x(vartype);
       [aux1,aux2] = min(abs(xi-round(xi)));
       branchvar   = aux2(1); % pick the first of the variables with max value
   otherwise
       % decision not implemented                                   
       warning('decision not implemented: switch to first free');
       branchvar = 1;
   end

% ------------------------------------------------------------------------------
%
% isymm: checks symmetry of matrices  
%       input:  Matrix M     
%       output: 1 if symmetric
%               0 otherwise

function [b] = isymm(M)

if size(M,1)~=size(M,2),
   b = 0; 
   return
else
   if max(max(abs(M-M'))) <= eps*max(max(abs(M)))
      b = 1;
   else
      b = 0;   
   end
end   

% ------------------------------------------------------------------------------
%
% QP_DANTZ Quadratic programming 
%         [X,how]=QP(H,f,A,b,xmax) solves the quadratic programming problem:
% 
%                 min 0.5*x'Hx + f'x   subject to:  Ax <= b, -xmax<=x<=xmax
%                  x    
%
%         by using DANTZGMP.M routine of the MPC Toolbox
%
% (C) 1998 by Alberto Bemporad, Z\"urich, 11/2/1998

function [xopt,how] = qp_dantz(H,f,A,b,xmax)

mnu=length(f);
nc =length(b);

% H must be symmetric. Otherwise set H=(H+H')/2
if norm(H-H') > eps
   warning('Your Hessian is not symmetric.  Resetting H=(H+H'')/2')
   H = (H+H')*0.5;
end

a=H*xmax;    % This is a constant term that adds to the initial basis
              % in each QP.
H=H\eye(mnu);

rhsc=b+A*xmax;
rhsa=a-f;
TAB=[-H H*A';A*H -A*H*A'];
basisi=[H*rhsa;
       rhsc-A*H*rhsa];
ibi=-[1:mnu+nc]';
ili=-ibi;
[basis,ib,il,iter]=dantzgmp(TAB,basisi,ibi,ili);
if iter < 0
   how='infeasible';
   warning('The constraints are overly stringent. No feasible solution')
else
   how='feasible';
end

xopt=zeros(mnu,1);
for j=1:mnu
   if il(j) <= 0
      xopt(j)=-xmax(j);
   else
      xopt(j)=basis(il(j))-xmax(j);
   end
end