nlconst.m

数值类综合算法 常用数值计算工具包（龙贝格算法、改进欧拉法、龙格库塔方法、复合辛普森）

         else
            how=' Hessian modified';
         end
      end
      
      %----------Perform BFGS Update If YL'S Is Positive---------
      if YL'*sdiff>eps
         HESS=HESS ...
            +(YL*YL')/(YL'*sdiff)-((HESS*sdiff)*(sdiff'*HESS'))/(sdiff'*HESS*sdiff);
         % BFGS Update using Cholesky factorization  of Gill, Murray and Wright.
         % In practice this was less robust than above method and slower. 
         %   R=chol(HESS); 
         %   s2=R*S; y=R'\YL; 
         %   W=eye(numberOfVariables,numberOfVariables)-(s2'*s2)\(s2*s2') + (y'*s2)\(y*y');
         %   HESS=R'*W*R;
      else
         how=' Hessian not updated';
      end
      
   else % First call
      OLDLAMBDA=(eps+gf'*gf)*ones(ncstr,1)./(sum(AN'.*AN')'+eps) ;
   end % if numGradEvals>1
   numGradEvals=numGradEvals+1;
   
   LOLD=LAMBDA;
   OLDAN=AN;
   OLDgf=gf;
   OLDC=c;
   OLDF=f;
   OLDX=XOUT;
   XN=zeros(numberOfVariables,1);
   if (meritFunctionType>0 & meritFunctionType<5)
      % Minimax and attgoal problems have special Hessian:
      HESS(numberOfVariables,1:numberOfVariables)=zeros(1,numberOfVariables);
      HESS(1:numberOfVariables,numberOfVariables)=zeros(numberOfVariables,1);
      HESS(numberOfVariables,numberOfVariables)=1e-8*norm(HESS,'inf');
      XN(numberOfVariables)=max(c); % Make a feasible solution for qp
   end
   
   GT =c;
   
   HESS = (HESS + HESS')*0.5;
   [SD,lambda,exitflagqp,outputqp,howqp] ...
      = qpsub(HESS,gf,AN,-GT,[],[],XN,eq,-1, ...
      Nlconst,size(AN,1),numberOfVariables); 
   
   lambda((1:eq)') = abs(lambda( (1:eq)' ));
   ga=[abs(c( (1:eq)' )) ; c( (eq+1:ncstr)' ) ];
   if ~isempty(c)
      mg=max(ga);
   else
      mg = 0;
   end
   
   if verbosity>1
      if strncmp(howqp,'ok',2); 
         howqp =''; 
      end
      if ~isempty(how) & ~isempty(howqp) 
         how = [how,'; '];
      end
      if meritFunctionType==1,
         gamma = mg+f;
         CurrOutput = sprintf(formatstr,iter,numFunEvals,gamma,stepsize,gf'*SD,how,howqp); 
         disp(CurrOutput)

      else
         CurrOutput = sprintf(formatstr,iter,numFunEvals,f,mg,stepsize,gf'*SD,how,howqp); 
         disp(CurrOutput)
        % disp([sprintf('%5.0f %12.6g %12.6g ',numFunEvals,f,mg), ...
        %       sprintf('%12.3g  ',stepsize),how, ' ',howqp]);
      end
   end
   LAMBDA=lambda((1:ncstr)');
   OLDLAMBDA=max([LAMBDA';0.5*(LAMBDA+OLDLAMBDA)'])' ;
   
   %---------------LINESEARCH--------------------
   MATX=XOUT;
   MATL = f+sum(OLDLAMBDA.*(ga>0).*ga) + 1e-30;
   infeas = strncmp(howqp,'i',1);
   if meritFunctionType==0 | meritFunctionType == 5
      % This merit function looks for improvement in either the constraint
      % or the objective function unless the sub-problem is infeasible in which
      % case only a reduction in the maximum constraint is tolerated.
      % This less "stringent" merit function has produced faster convergence in
      % a large number of problems.
      if mg > 0
         MATL2 = mg;
      elseif f >=0 
         MATL2 = -1/(f+1);
      else 
         MATL2 = 0;
      end
      if ~infeas & f < 0
         MATL2 = MATL2 + f - 1;
      end
   else
      % Merit function used for MINIMAX or ATTGOAL problems.
      MATL2=mg+f;
   end
   if mg < eps & f < bestf
      bestf = f;
      bestx = XOUT;
      bestHess = HESS;
      bestgrad = gf;
      bestlambda = lambda;
   end
   MERIT = MATL + 1;
   MERIT2 = MATL2 + 1; 
   stepsize=2;
   while  (MERIT2 > MATL2) & (MERIT > MATL) ...
         & numFunEvals < maxFunEvals & ~OPT_STOP
      stepsize=stepsize/2;
      if stepsize < 1e-4,  
         stepsize = -stepsize; 
         
         % Semi-infinite may have changing sampling interval
         % so avoid too stringent check for improvement
         if meritFunctionType == 5, 
            stepsize = -stepsize; 
            MATL2 = MATL2 + 10; 
         end
      end
      XOUT = MATX + stepsize*SD;
      x(:)=XOUT; 
      
      if strcmp(funfcn{2},'fseminf')
         f= feval(funfcn{3},x,varargin{3:end});
         
         [nctmp,nceqtmp,NPOINT,NEWLAMBDA,OLDLAMBDA,LOLD,s] = ...
            semicon(x,LAMBDA,NEWLAMBDA,OLDLAMBDA,POINT,FLAG,s,varargin{:});
         nctmp = nctmp(:); nceqtmp = nceqtmp(:);
         non_ineq = length(nctmp);  % the length of nctmp can change
         ineq = non_ineq + lin_ineq;
         ncstr = ineq + eq;
         
      else
         f = feval(funfcn{3},x,varargin{:});
         if constflag
            [nctmp,nceqtmp] = feval(confcn{3},x,varargin{:});
            nctmp = nctmp(:); nceqtmp = nceqtmp(:);
         else
            nctmp = []; nceqtmp=[];
         end
      end
            
      nc = [nceqtmp(:); nctmp(:)];
      c = [Aeq*XOUT-Beq; nceqtmp(:); A*XOUT-B; nctmp(:)];  
      
      if OPT_STOP
         break;
      end
      
      numFunEvals = numFunEvals + 1;
      ga=[abs(c( (1:eq)' )) ; c( (eq+1:length(c))' )];
      if ~isempty(c)
         mg=max(ga);
      else
         mg = 0;
      end
      
      MERIT = f+sum(OLDLAMBDA.*(ga>0).*ga);
      if meritFunctionType==0 | meritFunctionType == 5
         if mg > 0
            MERIT2 = mg;
         elseif f >=0 
            MERIT2 = -1/(f+1);
         else 
            MERIT2 = 0;
         end
         if ~infeas & f < 0
            MERIT2 = MERIT2 + f - 1;
         end
      else
         MERIT2=mg+f;
      end
   end  % line search loop
   %------------Finished Line Search-------------
   
   if meritFunctionType~=5
      mf=abs(stepsize);
      LAMBDA=mf*LAMBDA+(1-mf)*LOLD;
   end
   % Test stopping conditions (convergence)
   if (max(abs(SD)) < 2*tolX | abs(gf'*SD) < 2*tolFun ) & ...
         (mg < tolCon | (strncmp(howqp,'i',1) & mg > 0 ) )
      if verbosity>0
         if ~strncmp(howqp, 'i', 1) 
            disp('Optimization terminated successfully:')
            if max(abs(SD)) < 2*tolX 
               disp(' Search direction less than 2*options.TolX and')
               disp('  maximum constraint violation is less than options.TolCon')
            else
               disp(' Magnitude of directional derivative in search direction ')
               disp('  less than 2*options.TolFun and maximum constraint violation ')
               disp('  is less than options.TolCon')     
            end
            
            active_const = find(LAMBDA>0);
            if active_const 
               disp('Active Constraints:'), 
               disp(active_const) 
            else % active_const == 0
               disp(' No Active Constraints');
            end 
         end
         
         if (strncmp(howqp, 'i',1) & mg > 0)
            disp('Optimization terminated: No feasible solution found.')
            if max(abs(SD)) < 2*tolX 
               disp(' Search direction less than 2*options.TolX but constraints are not satisfied.')
            else
               disp(' Magnitude of directional derivative in search direction ')
               disp('  less than 2*options.TolFun but constraints are not satisfied.')    
            end
            EXITFLAG = -1;   
         end
      end
      status=1;
   else % continue
      % NEED=[LAMBDA>0] | G>0
      if numFunEvals > maxFunEvals  | OPT_STOP
         XOUT = MATX;
         f = OLDF;
         if ~OPT_STOP
            if verbosity > 0
               disp('Maximum number of function evaluations exceeded;')
               disp('increase OPTIONS.MaxFunEvals')
            end
         end
         EXITFLAG = 0;
         status=1;
      end
      if iter > maxIter
         XOUT = MATX;
         f = OLDF;
         if verbosity > 0
            disp('Maximum number of function evaluations exceeded;')
            disp('increase OPTIONS.MaxIter')
         end
         EXITFLAG = 0;
         status=1;
      end
   end 
   
   x(:) = XOUT;
   switch funfcn{1} % evaluate function gradients
   case 'fun'
      ;  % do nothing...will use finite difference.
   case 'fungrad'
      [f,gf_user] = feval(funfcn{3},x,varargin{:});
      gf_user = gf_user(:);
      numGradEvals=numGradEvals+1;
   case 'fun_then_grad'
      gf_user = feval(funfcn{4},x,varargin{:});
      gf_user = gf_user(:);
      numGradEvals=numGradEvals+1;
   otherwise
      error('Undefined calltype in FMINCON');
   end
   numFunEvals=numFunEvals+1;
   
   
   % Evaluate constraint gradients
   switch confcn{1}
   case 'fun'
      gnceq=[]; gncineq=[];
   case 'fungrad'
      [nctmp,nceqtmp,gncineq,gnceq] = feval(confcn{3},x,varargin{:});
      nctmp = nctmp(:); nceqtmp = nceqtmp(:);
      numGradEvals=numGradEvals+1;
   case 'fun_then_grad'
      [gncineq,gnceq] = feval(confcn{4},x,varargin{:});
      numGradEvals=numGradEvals+1;
   case ''
      nctmp=[]; nceqtmp =[];
      gncineq = zeros(numberOfVariables,length(nctmp));
      gnceq = zeros(numberOfVariables,length(nceqtmp));
      
   otherwise
      error('Undefined calltype in FMINCON');
   end
   gnc_user = [gnceq, gncineq];
   gc = [Aeq', gnceq, A', gncineq];
   
   
end % while status ~= 1

% Update 
numConstrEvals = numGradEvals;

% Gradient is in the variable gf
GRADIENT = gf;

% If a better unconstrained solution was found earlier, use it:
if f > bestf 
   XOUT = bestx;
   f = bestf;
   HESS = bestHess;
   GRADIENT = bestgrad;
   lambda = bestlambda;
end

FVAL = f;
x(:) = XOUT;
if (OPT_STOP)
   if verbosity > 0
      disp('Optimization terminated prematurely by user')
   end
end


OUTPUT.iterations = iter;
OUTPUT.funcCount = numFunEvals;
OUTPUT.stepsize = stepsize;
OUTPUT.algorithm = 'medium-scale: SQP, Quasi-Newton, line-search';
OUTPUT.firstorderopt = [];
OUTPUT.cgiterations = [];

[lin_ineq,Acol] = size(Ain);  % excludes upper and lower

lambda_out.lower=zeros(lenlb,1);
lambda_out.upper=zeros(lenub,1);

lambda_out.eqlin = lambda(1:lin_eq);
ii = lin_eq ;
lambda_out.eqnonlin = lambda(ii+1: ii+ non_eq);
ii = ii+non_eq;
lambda_out.lower(arglb) = lambda(ii+1 :ii+nnz(arglb));
ii = ii + nnz(arglb) ;
lambda_out.upper(argub) = lambda(ii+1 :ii+nnz(argub));
ii = ii + nnz(argub);
lambda_out.ineqlin = lambda(ii+1: ii + lin_ineq);
ii = ii + lin_ineq ;
lambda_out.ineqnonlin = lambda(ii+1 : end);