function [x_opt,f_opt,g_opt,HESS,lambda,cost_rec,maxg_rec,parm_rec]=old_sqp(func,x_init,x_min,x_max,options,gradfunc) % [p_opt,f_opt,g_opt,Hess,lambda,cost_rec,maxg_rec,parm_rec]=old_sqp(func,p_init,p_min,p_max,options) % % OLD_SQP: Nonlinear optimization with inequality constraints using % Sequential Quadratid Programming % % minimizes f(p) such that g(p)<0 and p_min <= p_opt <= p_max. % f is a scalar cost function, p is a vector of parameters, and % g is a vector of constraints. % % INPUT % ====== % func : the name of the matlab function to be optimized in the form % [cost,constraints] = func(p) % p_init : the vector of initial parameter values ... a column vector % p_min : minimum permissible values of the parameters, p % p_max : maximum permissible values of the parameters, p % options : options(1) = 1 means display intermediate results % options(2) = tol_p tolerance on convergence of parameters % options(3) = tol_f tolerance on convergence of objective % options(4) = tol_g tolerance on constraint functions % options(5) = max_count limit on function evaluations % % OUTPUT % ====== % p_opt : a set of parameters at or near the optimal value % f_opt : the cost associated with the optimal parameters % g_opt : the constraints associated with the optimal parameters % Hess : the Hessian of the cost function at the optimal point % lambda : the set of Lagrange multipliers at the active constraints % cost_rec : record of the cost function at each iteration % maxg_rec : record of the maximum constraint value at each iteration % parm_rec : record of the parameters at each iteration % % X=OLD_SQP('FUN',X,p_min,p_max,OPTIONS,'gradfunc') allows a function % 'gradfunc' to be entered which returns the partial derivatives of the % function and the constraints at X: [grad_f,grad_g] = gradfunc(X). % Copyright (c) 1990 by the MathWorks, Inc. % Andy Grace 7-9-90. % --------------------- READ INPUTS ----------------------------- if nargin<2 % need at least 'func' and x_init to get started help old_sqp; return end x_init = x_init(:); % Set up parameters as a column vector % Handle missing arguments and do error checking if nargin<3 % default values for parameter ranges x_min = -1.0e6*abs(x_init); x_max = 1.0e6*abs(x_init); else x_min = x_min(:); x_max = x_max(:); end % Put the initial X within the specified parameter ranges. if any(x_min>x_max) error('Parameter bounds are infeasible ... x_min > x_max') else x_init = min(max(x_min,x_init),x_max); % apply constraints end if nargin<5, options = []; end % use default option values options = optim_options(options); dsply = options(1); % 1: display intermediate values tol_x = options(2); % tolerance for convergence in x tol_f = options(3); % tolerance for convergence in f tol_g = options(4); % tolerence for active constraints g max_count= options(5); % maximum number of function evaluations penalty = options(6); % penalty factor for unsatisfied constraints q = options(7); % penalty exponent for unsatisfied constraints grad_chk = options(9); % gradient step size del_min = options(11); % minimum param. change for finite difference del_max = options(12); % maximum param. change for finite difference neqcstr = options(13); % number of equality constraints if nargin < 6, gradfunc=[]; end % --------------- INITIALIZE VECTORS AND MATRICES ------------------------- XOUT = x_init; x = x_init ; f_opt = Inf; % best value of cost function starts at infinity [f,g] = feval(func,x); % the first evaluation of the function and constraints nvars = length(x); % the number of variable parameters. ncstr = length(g); % the current number of constraints; OLDX = x_init; OLDG = g; gradf = zeros(1,nvars); OLDgradf = zeros(1,nvars); gradg = zeros(ncstr,nvars); OLDgradg = zeros(ncstr,nvars); LAMBDA = zeros(ncstr,1); % initialize Lagrange multipliers for constraints HESS = eye(nvars,nvars); % initialize Hessian matrix SD = zeros(nvars,1); % search direction CHG = 1e-7 * ( ones(nvars,1) + abs(x_init) ); % fractional paramter changes GNEW = 1e8*CHG; %if length(GRADfunc) % gradients supplied % if ~any(gradfunc<48) % Check alphanumeric % gtype = 1; % evalstr2 = [gradfunc,'(x']; % for i=1:nargin - 6 % gtype = 2; % evalstr2 = [evalstr2,',P',int2str(i)]; % end % evalstr2 = [evalstr2, ')']; % else % gtype = 3; % evalstr2=[gradfunc,';']; % end %end if dsply more off end if nvars == 2 & dsply == 2 % generate data for a surface plot a1 = [x_min(1):(x_max(1)-x_min(1))/45:x_max(1)]; a2 = [x_min(2):(x_max(1)-x_min(1))/50:x_max(2)]; n1 = length(a1); n2 = length(a2); fex = zeros(n1,n2); for i=1:n1 for j=1:n2 a = [ a1(i) ; a2(j) ]; [J,g] = feval(func,a); fex(i,j) = J + penalty*sum(g.*(g>tol_g))^q; end end end %---------------------------- MAIN LOOP ---------------------------- function_count = 1; gradient_count = 0; iteration = 0; StepLength = 1; end_iterations = 0; while ~end_iterations % ----------- COMPUTE GRADIENTS OF COST AND CONSTRAINTS ------------- % if ~length(gradfunc) | grad_chk % Finite Difference gradients oldf = f; oldg = g; ncstr = length(g); gradg = zeros(ncstr,nvars); % Try to make the finite differences equal to 1.0e-8. CHG = -1.0e-8./(GNEW+eps); CHG = sign(CHG+eps).*min(max(abs(CHG),del_min),del_max); for gcnt=1:nvars temp = XOUT(gcnt); XOUT(gcnt)= temp + CHG(gcnt); x(:) =XOUT; [f, g] = feval(func,x); % Next line is to be used for problems with varying number of constraints % if ncstr~=length(g), diff=length(g); g=v2sort(oldg,g); end gradf(1,gcnt) = (f - oldf) / CHG(gcnt); gradg(:,gcnt) = (g - oldg) / CHG(gcnt); XOUT(gcnt) = temp; end function_count = function_count + nvars; f = oldf; g = oldg; % if grad_chk == 1 % Gradient check % gradfFD = gradf; % gradgFD = gradg; % x(:)=XOUT; % if gtype == 1 % [gradf, gradg] = feval(gradfunc, x); % elseif gtype == 2 % [gradf, gradg] = eval(evalstr2); % else % eval(evalstr2); % end % disp('Function derivative') % graderr(gradfFD, gradf, evalstr2); % disp('Constraint derivative') % graderr(gradgFD, gradg, evalstr2); % grad_chk = 0; % end % % else % User-supplied gradients % % if gtype == 1 % [gradf, gradg] = feval(gradfunc, x); % elseif gtype == 2 % [gradf, gradg] = eval(evalstr2); % else % eval(evalstr2); % end % end gradient_count = gradient_count + 1; if iteration == 0 % this is within the first iteration PENALTY = (eps+gradf*gradf')*ones(ncstr,1)./(sum(gradg'.^2)'+eps); end % Change constraint gradients to account for equality constraints ... % Make the gradient of the equality constraints face in % the opposite direction to the function gradient. % % for i=1:neqstr % schg=gradg(i,:)*gradf; % if schg>0 % gradg(:,i)=-gradg(:,i); % g(i)=-g(i); % end % end GOLD = OLDgradf + LAMBDA'*OLDgradg; % old gradient of augmented cost GNEW = gradf + LAMBDA'*gradg; % new gradient of augmented cost q = GNEW - GOLD; % change in the augmented gradients p = XOUT - OLDX; % change in the parameters % ------------ Finished Gradients --------------- %------------- UPDATE THE HESSIAN --------------- % Ensure the Hessian is positive definite in the update. if q*p < StepLength^2*1e-3 % if (qp) is small or negative how='modify gradients to ensure Hessian > 0,'; while q*p < -1e-5 [qp_min,qp_idx]=min(q'.*p); % find most negative qp q(qp_idx)=q(qp_idx)/2; % cut corresponding q in half end if q*p < (eps*norm(HESS,'fro')); % qp still small or negative FACTOR = gradg' * g - OLDgradg' * OLDG; FACTOR = FACTOR.*(p.*FACTOR>0).*(q'.*p<=eps); WT=1e-2; if max(abs(FACTOR))==0; FACTOR = 1e-5*sign(p); how = 'small gradients,'; end while q*p < (eps*norm(HESS,'fro')) & WT < 1/eps q = q + WT*FACTOR'; WT = WT*2; end end else how='regular'; end %--------- perform BFGS Hessian update only if q*p is positive -------- if q*p > eps HESS = HESS + (q'*q)/(q*p) - (HESS*p*p'*HESS')/(p'*HESS*p); how=[how,' Hessian update.']; % 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'\q; % W=eye(nvars,nvars)-(s2'*s2)\(s2*s2') + (y'*s2)\(y*y'); % HESS=R'*W*R; else how=[how,' no Hessian update.']; end % --------- Finished Hessian Update ------------- % --------- DETERMINE THE SEARCH DIRECTION ------------------ % ----------- set up the quadratic programming problem -------- OLDX = XOUT; OLDF = f; OLDG = g; OLDgradf = gradf; OLDgradg = gradg; OLDLAMBDA = LAMBDA; SDi = zeros(nvars,1); % initial guess for search direction % append parameter constraints to nonlinear constraints and gradg GT = [ g ; -XOUT+x_min ; XOUT-x_max ]; gradg = [ gradg ; -eye(nvars) ; eye(nvars) ]; % solve the quadratic programming problem to get search direction, SD. % min 0.5* SD' * HESS * SD + gradf * SD subject to gradg * SD < -GT [SD, lambda, howqp] = old_qp(HESS,gradf',gradg,-GT,[],[],SD,neqcstr); % lambda(1:neqcstr) = abs(lambda(1:neqcstr)); % equality cnstrnts % ga=[ abs(g(1:neqcstr))' ; g(neqcstr+1:ncstr)' ]; ga = g; g_max = max(ga); LAMBDA=lambda(1:ncstr); % exclude parameter constraints from LAMBDA % don't change PENALTY too quickly ... PENALTY = max([ LAMBDA ; 0.5*(LAMBDA+PENALTY) ]) % PENALTY is used only as a scalar penalty factors for % unsatisfied constraints in the merit function for the line search % --------- Finished Search Direction ------------- % ---------------- LINE SEARCH TO REDUCE f OR max(g) ---------------- % This line search looks for improvement in either the maximum 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. infeas = (howqp(1) == 'i'); GOAL_1 = f + sum(PENALTY*(ga>0).*ga ) + 1e-30; % penalized cost if g_max > tol_g % constraints are being violated GOAL_2 = g_max; % GOAL_2 is max constraint elseif f >=0 GOAL_2 = -1/(f+1); % GOAL_2 is -1/(f+1) (negative) else GOAL_2 = 0; % GOAL_2 is zero end if ~infeas & f < 0 GOAL_2 = GOAL_2 + f - 1; end if g_max < eps & f < f_opt % update the optimal solution f_opt = f; g_opt = g; x_opt = XOUT; end COST_1 = GOAL_1 + 1; % initialize penalized cost COST_2 = GOAL_2 + 1; % initialize max constraint (if it is > 0) if dsply == 2 disp(' alpha max{g} COST_1 COST_2'); end StepLength = 2; % initialize step length along search direction while (COST_1>GOAL_1) & (COST_2>GOAL_2) & (function_count0).*ga); % penalized cost if g_max > tol_g % constraints are being violated COST_2 = g_max; % COST_2 is max constraint elseif f >=0 COST_2 = -1/(f+1); % COST_2 is -1/(f+1) (negative) else COST_2 = 0; % COST_2 is zero end if ~infeas & f < 0 COST_2 = COST_2 + f - 1; end if dsply == 2 fprintf(' %9.2e %9.2e %9.2e %6.3f %9.2e %6.3f\n', ... StepLength,g_max,COST_1,COST_1/GOAL_1,COST_2,COST_2/GOAL_2); end end %pause(5.0); %------------ Finished Line Search ------------- absSL = abs(StepLength); % absolute value of step length LAMBDA = absSL*LAMBDA + (1-absSL)*OLDLAMBDA; % new Lagrange multipliers iteration = iteration + 1; % ----------- DISPLAY RESULTS ------------- if dsply home clc fprintf(1,' ************************* OLD_SQP *******************\n'); fprintf(1,'iteration = %5d', iteration ); if max(g) > tol_g fprintf(1,' !!! infeasible !!!\n'); else fprintf(1,' *** feasible ***\n'); end fprintf(1,'function evaluations = %5d \n', function_count ); fprintf(1,'cost = %11.3e\n', f ); fprintf(1,'parameters = '); fprintf(1,'%11.3e', x ); fprintf(1,'\n'); fprintf(1,'max constraint = %11.3e\n', max(g) ); fprintf(1,'Step Size = %11.3e\n', StepLength ); fprintf(1,'BFGS method : %12s\n', how ); fprintf(1,'QP method : %12s\n', howqp ); fprintf(1,' ************************* OLD_SQP *******************\n'); end cost_rec(iteration) = f; maxg_rec(iteration) = max(g); parm_rec(:,iteration) = x(:); if nvars == 2 & dsply == 2 % plot surface and convergence point figure(1) clf mesh(a1,a2,fex') hold on plot3(x(1),x(2),f,'r*') hold off xlabel('p_1') ylabel('p_2') zlabel('f') drawnow pause(1) end % ----------- CHECK CONVERGENCE CRITERIA ------------ if (max(abs(absSL*SD./x)) < tol_x) & ... % param converged (abs(absSL*gradf*SD/f) < tol_f) & ... % cost converged (g_max < tol_g | (howqp(1) == 'i' & g_max > 0 ) ) % constraints ok end_iterations = 1; if howqp(1) ~= 'i' fprintf(' Woo-Hoo! Convergence in X, F, and G \n') fprintf(' Active Constraints:') disp(find(LAMBDA>0)') end if (howqp(1) == 'i' & g_max > 0) disp(' Boo-Hoo! No feasible solution found.') end elseif function_count >= max_count % NOT converged but give up x_opt = OLDX; % iterations exeeded during line search, reset XOUT f_opt = OLDF; % iterations exeeded during line search, reset f disp(' Drats! Maximum number of iterations exceeded ...') disp(' increase options(5) and try, try, try again!') end_iterations = 1; end %------------ Finished Convergence Criteria ------------- end %------------------------- Finished Main Loop ---------------------------- if f_opt < f % if a better feasible solution was found, then use it. XOUT = x_opt; f = f_opt; end x_opt = XOUT; f_opt = f; g_opt = g; % ------------------ PLOT CONVERGENCE HISTORY ---------------------- if dsply == 2 itr = [1:iteration]; figure(2) subplot(311); semilogy(itr,abs(cost_rec)); xlabel('iteration') ylabel('cost') subplot(312) plot(itr,maxg_rec); xlabel('iteration') ylabel('max(constraints)') subplot(313) plot(itr,parm_rec); xlabel('iteration') ylabel('parameters') end % endfunction # --------------------------------------------------- OLD_SQP function [X,lambda,how]=old_qp(H,f,A,B,x_min,x_max,X,neqcstr,verbosity,negdef,normalize) % [X,lambda,how]=old_qp(H,f,A,B,x_min,x_max,X0,neqcstr,verbosity,negdef,normalize) % % [x,lambda] = OLD_QP(H,f,A,b,x_min,x_max,X0,neqcstr,verbosity)) % % solves the quadratic programming problem: % % min 0.5*x'Hx + f'x subject to: Ax <= b % x % % lambda is the set of Lagrangian multipliers at the optimal point % % x_min and x_max define a set of lower and upper bounds on the % design variables, X, so that the solution % is always in the range x_min < X < x_max. % % X0 is the initial starting point % % neqcstr indicates that the first neqcstr constraints are equality % constraints defined by A and b are equality constraints. % % verbosity indicates the level of warning messages displayed during % the solution. A value of -1 results in no warning messages. % % Copyright (c) 1990-94 by The MathWorks, Inc. % Andy Grace 7-9-90. % Handle missing arguments if nargin < 11, normalize = 1; if nargin < 10, negdef = 0; if nargin< 9, verbosity = []; if nargin< 8, neqcstr=[]; if nargin < 7, X=[]; if nargin<6, x_max=[]; if nargin<5, x_min=[]; end, end, end, end, end, end, end [ncstr,nvars]=size(A); nvars = length(f); % In case A is empty if ~length(verbosity), verbosity = 0; end if ~length(neqcstr), neqcstr = 0; end if ~length(X), X=zeros(nvars,1); end f=f(:); B=B(:); simplex_iter = 0; if ( norm(H,'inf')==0 | prod(size(H))==0 ) H=0; is_qp=0; else is_qp=~negdef; end how = 'ok'; normf = 1; if normalize > 0 if ~is_qp normf = norm(f); f = f./normf; end end % Handle the parameter bounds as linear constraints lenXmin=length(x_min); if lenXmin > 0 A=[A;-eye(lenXmin,nvars)]; B=[B;-x_min(:)]; end lenXmax=length(x_max); if lenXmax>0 A=[A;eye(lenXmax,nvars)]; B=[B;x_max(:)]; end ncstr=ncstr+lenXmin+lenXmax; errcstr = 100*sqrt(eps)*norm(A); % Used for determining threshold for whether a direction will violate % a constraint. normA = ones(ncstr,1); if normalize > 0 for i=1:ncstr n = norm(A(i,:)); if (n ~= 0) A(i,:) = A(i,:)/n; B(i) = B(i)/n; normA(i,1) = n; end end else normA = ones(ncstr,1); end errnorm = 0.01*sqrt(eps); lambda=zeros(ncstr,1); aix=lambda; ACTCNT=0; % number of active constraints ACTSET=[]; % the set of active constraints ACTIND=0; % the indices of the active constraints CIND=1; % a constraint index eqix = 1:neqcstr; %------------EQUALITY CONSTRAINTS--------------------------- Q = zeros(nvars,nvars); R = []; if neqcstr>0 aix(eqix)=ones(neqcstr,1); ACTSET=A(eqix,:); ACTIND=eqix; ACTCNT=neqcstr; if ACTCNT >= nvars - 1, simplex_iter = 1; end CIND=neqcstr+1; [Q,R] = qr(ACTSET'); if max(abs(A(eqix,:)*X-B(eqix)))>1e-10 X = ACTSET\B(eqix); % X2 = Q*(R'\B(eqix)); does not work here ! end % Z=null(ACTSET); [m,n]=size(ACTSET); Z = Q(:,m+1:n); err = 0; if neqcstr > nvars err = max(abs(A(eqix,:)*X-B(eqix))); if (err > 1e-8) how='infeasible quadratic program'; if verbosity > -1 disp('Warning: The equality constraints are overly stringent;') disp(' there is no feasible solution.') end end actlambda = -R\(Q'*(H*X+f)); lambda(eqix) = normf * (actlambda ./normA(eqix)); return end if ~length(Z) actlambda = -R\(Q'*(H*X+f)); lambda(eqix) = normf * (actlambda./normA(eqix)); if (max(A*X-B) > 1e-8) how = 'infeasible quadratic program'; disp('Warning: The constraints or bounds are overly stringent;') disp(' there is no feasible solution.') disp(' Equality constraints have been met.') end return end % Check whether in Phase 1 of feasibility point finding. if (verbosity == -2) cstr = A*X-B; mc=max(cstr(neqcstr+1:ncstr)); if (mc > 0) X(nvars) = mc + 1; end end else Z=1; end % === Find Initial Feasible Solution ==== cstr = A*X-B; mc=max(cstr(neqcstr+1:ncstr)); if mc>eps A2=[[A;zeros(1,nvars)],[zeros(neqcstr,1);-ones(ncstr+1-neqcstr,1)]]; [XS,lambdas]=old_qp([],[zeros(nvars,1);1],A2,[B;1e-5],[],[],[X;mc+1],neqcstr,-2,0,-1); X=XS(1:nvars); cstr=A*X-B; if XS(nvars+1)>eps if XS(nvars+1)>1e-8 how='infeasible quadratic program'; if verbosity > -1 disp('Warning: The constraints are overly stringent;') disp(' there is no feasible solution.') end else how = 'overly constrained quadratic program'; end lambda = normf * (lambdas(1:ncstr)./normA); return end end if (is_qp & prod(size(H)) ) gf=H*X+f; SD=-Z*((Z'*H*Z)\(Z'*gf)); % Check for -ve definite problems: % if SD'*gf>0, is_qp = 0; SD=-SD; end else gf = f; SD=-Z*Z'*gf; if norm(SD) < 1e-10 & neqcstr % This happens when equality constraint is perpendicular % to objective function f(x). actlambda = -R\(Q'*(H*X+f)); lambda(eqix) = normf * (actlambda ./ normA(eqix)); return; end end % Sometimes the search direction goes to zero in negative % definite problems when the initial feasible point rests on % the top of the quadratic function. In this case we can move in % any direction to get an improvement in the function so try % a random direction. if negdef if norm(SD) < sqrt(eps) SD = -Z*Z'*(rand(nvars,1) - 0.5); end end oldind = 0; t=zeros(10,2); tt = zeros(10,1); % The maximum number of iterations for a simplex type method is: % maxiters = prod(1:ncstr)/(prod(1:nvars)*prod(1:max(1,ncstr-nvars))); % --------- START MAIN QUADRATIC PROGRAMMING ROUTINE ---------- while 1 % Find distance we can move in search direction SD before a % constraint is violated. % Gradient with respect to search direction. GSD=A*SD; % Note: we consider only constraints whose gradients are greater % than some threshold. If we considered all gradients greater than % zero then it might be possible to add a constraint which would lead to % a singular (rank deficient) working set. The gradient (GSD) of such % a constraint in the direction of search would be very close to zero. indf = find((GSD > errnorm * norm(SD)) & ~aix); if ~length(indf) STEPMIN=1e16; else dist = abs(cstr(indf)./GSD(indf)); [STEPMIN,ind2] = min(dist); ind2 = find(dist == STEPMIN); % Bland's rule for anti-cycling: if there is more than one blocking constraint % then add the one with the smallest index. ind=indf(min(ind2)); % Non-cycling rule: % ind = indf(ind2(1)); end %------------------QP------------- if (is_qp & prod(size(H)) > 0 ) % If STEPMIN is 1 then this is the exact distance to the solution. if STEPMIN>=1 X=X+SD; if ACTCNT>0 if ACTCNT>=nvars-1 if size(ACTSET,1) >= CIND ACTSET(CIND,:)=[]; end if length(ACTIND) >= CIND ACTIND(CIND)=[]; end end rlambda = -R\(Q'*(H*X+f)); actlambda = rlambda; actlambda(eqix) = abs(rlambda(eqix)); indlam = find(actlambda < 0); if (~length(indlam)) lambda(ACTIND) = normf * (rlambda./normA(ACTIND)); return end % Remove constraint lind = find(ACTIND == min(ACTIND(indlam))); lind=lind(1); ACTSET(lind,:) = []; aix(ACTIND(lind)) = 0; [Q,R]=qrdelete(Q,R,lind); ACTIND(lind) = []; ACTCNT = ACTCNT - 2; simplex_iter = 0; ind = 0; else return end else X=X+STEPMIN*SD; end % Calculate gradient w.r.t objective at this point gf=H*X+f; else % Unbounded Solution if ~length(indf) | ~finite(STEPMIN) if norm(SD) > errnorm if normalize < 0 STEPMIN=abs((X(nvars)+1e-5)/(SD(nvars)+eps)); else STEPMIN = 1e16; end X=X+STEPMIN*SD; how='unbounded quadratic program'; else how = 'ill posed quadratic program'; end if verbosity > -1 if norm(SD) > errnorm disp('Warning: The solution is unbounded and at infinity;') disp(' the constraints are not restrictive enough.') else disp('Warning: The search direction is close to zero; the problem is ill posed.') disp(' The gradient of the objective function may be zero') disp(' or the problem may be badly conditioned.') end end return else X=X+STEPMIN*SD; end end %if (qp) % Update X and calculate constraints cstr = A*X-B; cstr(eqix) = abs(cstr(eqix)); % Check no constraint is violated if normalize < 0 if X(nvars,1) < eps return; end end if max(cstr) > 1e5 * errnorm if max(cstr) > norm(X) * errnorm if verbosity > -1 disp('Warning: The problem is badly conditioned;') disp(' the solution is not reliable') verbosity = -1; end how='unreliable quadratic program'; if 0 X=X-STEPMIN*SD; return end end end % Sometimes the search direction goes to zero in negative % definite problems when the current point rests on % the top of the quadratic function. In this case we can move in % any direction to get an improvement in the function so % foil search direction by giving a random gradient. if negdef if norm(gf) < sqrt(eps) gf = randn(nvars,1); end end if ind aix(ind)=1; ACTSET(CIND,:)=A(ind,:); ACTIND(CIND)=ind; [m,n]=size(ACTSET); [Q,R] = qrinsert(Q,R,CIND,A(ind,:)'); end if oldind aix(oldind) = 0; end if ~simplex_iter % Z = null(ACTSET); [m,n]=size(ACTSET); Z = Q(:,m+1:n); ACTCNT=ACTCNT+1; if ACTCNT == nvars - 1, simplex_iter = 1; end CIND=ACTCNT+1; oldind = 0; else rlambda = -R\(Q'*gf); if rlambda(1) == -Inf fprintf(' Working set is singular; results may still be reliable.\n'); [m,n] = size(ACTSET); rlambda = -(ACTSET + sqrt(eps)*randn(m,n))'\gf; end actlambda = rlambda; actlambda(eqix)=abs(actlambda(eqix)); indlam = find(actlambda<0); if length(indlam) if STEPMIN > errnorm % If there is no chance of cycling then pick the constraint which causes % the biggest reduction in the cost function. i.e the constraint with % the most negative Lagrangian multiplier. Since the constraints % are normalized this may result in less iterations. [minl,CIND] = min(actlambda); else % Bland's rule for anti-cycling: if there is more than one % negative Lagrangian multiplier then delete the constraint % with the smallest index in the active set. CIND = find(ACTIND == min(ACTIND(indlam))); end [Q,R]=qrdelete(Q,R,CIND); Z = Q(:,nvars); oldind = ACTIND(CIND); else lambda(ACTIND)= normf * (rlambda./normA(ACTIND)); return end end %if ACTCNT 0 ) Zgf = Z'*gf; if (norm(Zgf) < 1e-15) SD = zeros(nvars,1); elseif ~length(Zgf) % Only happens in -ve semi-definite problems disp('Warning: QP problem is -ve semi-definite.') SD = zeros(nvars,1); else SD = -Z * ( (Z'*H*Z)\(Zgf) ); end % Check for -ve definite problems % if SD'*gf>0, is_qp = 0; SD=-SD; end else if ~simplex_iter SD = -Z*Z'*gf; gradsd = norm(SD); else gradsd = Z'*gf; if gradsd > 0 SD = -Z; else SD = Z; end end if abs(gradsd) < 1e-10 % Search direction null % Check whether any constraints can be deleted from active set. % rlambda = -ACTSET'\gf; if ~oldind rlambda = -R\(Q'*gf); end actlambda = rlambda; actlambda(1:neqcstr) = abs(actlambda(1:neqcstr)); indlam = find(actlambda < errnorm); lambda(ACTIND) = normf * (rlambda./normA(ACTIND)); if ~length(indlam) return end cindmax = length(indlam); cindcnt = 0; newactcnt = 0; while (abs(gradsd) < 1e-10) & (cindcnt < cindmax) cindcnt = cindcnt + 1; if oldind % Put back constraint which we deleted [Q,R] = qrinsert(Q,R,CIND,A(oldind,:)'); else simplex_iter = 0; if ~newactcnt newactcnt = ACTCNT - 1; end end CIND = indlam(cindcnt); oldind = ACTIND(CIND); [Q,R]=qrdelete(Q,R,CIND); [m,n]=size(ACTSET); Z = Q(:,m:n); if m ~= nvars SD = -Z*Z'*gf; gradsd = norm(SD); else gradsd = Z'*gf; if gradsd > 0 SD = -Z; else SD = Z; end end end if abs(gradsd) < 1e-10 % Search direction still null return; end lambda = zeros(ncstr,1); if newactcnt ACTCNT = newactcnt; end end end if simplex_iter & oldind ACTIND(CIND)=[]; ACTSET(CIND,:)=[]; CIND = nvars; end end % while 1 % endfunction # ------------------------------------------------------- OLD_QP