tcsub.m

这是在网上下的一个东东

function [x,lagrange,status] = tcsub(A,b,G,d,x,nEqCstr,caller,nCstr,nVars)
%TCSUB Active set algorithm used by mtsvdcstr and tikhcstr
%
% [x,lagrange,status] = tcsub(A,b,G,d,x,nEqCstr,caller,nCstr,nVars);
%
% Solves the constrained least squares problem:
%   min || A x - b ||   s.t.   G x >= d
% using an active set algorithm.
%
% The first nEqCstr constraints of G are equality constraints.

% Modified and simplified version of function ??? from Optimization Toolbox.
% Ann-Charlotte Berglund, IMM & UNI-C, June 28, 1999.

phase2 = 0;
status = 'feasible'; 
errNorm = 0.01*sqrt(eps); 
tolDep = 1000*nVars*eps;      
lagrange = zeros(nCstr,1);
showActInact = zeros(nCstr,1);
% The number of constraints in the active set.
actCnt = 0;
% The active set matrix.
actSet = [];
%Index vector.
actInd = 0;
% Vector to keep track of the equality constraints.
eqVec = 1:nEqCstr; 
Q = zeros(nVars,nVars);
R = []; 
indepInd = 1:nCstr;
if strcmp(caller, 'mtsvdcstr') | strcmp(caller, 'tikhcstr')
   phase2 = 1;
   [mA,n] = size(A);
end

simplex = 0;

if nEqCstr>0
  tolCons = 1e-10;
  showActInact(eqVec) = ones(nEqCstr,1);
  actSet = G(eqVec,:);
  actInd = eqVec;
  actCnt = nEqCstr;
  Z = [];  
  % Find the linearly dependent equality constraints if any.
  [QG,RG,EG] = qr(G(eqVec,:));
  if min(size(RG)) == 1
    depInd = find( abs(RG(1,1)) < tolDep);
  else
    depInd = find( abs(diag(RG)) < tolDep );
  end

  if ~isempty(depInd)
    if nEqCstr > nVars
      depInd = [depInd; ((nVars+1):nEqCstr)'];
    end      
    bdepInd =  abs(QG(:,depInd)'*d(eqVec)) >= tolDep ;
    
    if any( bdepInd ) % Not consistent
      status = 'incons';
      return
    % The equality constraints are consistent. Delete the dependent
    % constraint/-s from the active set.
    end
    [QAct,RAct,EAct] = qr(actSet');        
    [i,j] = find(EAct);
    % Find the dependent equality constraint/-s.
    remove = i(depInd);
    G(eqVec(remove),:) = [];
    d(eqVec(remove)) = [];
    indepInd(remove) = [];
    nEqCstr = nEqCstr-nnz(remove);
    nCstr = nCstr-nnz(remove);
    eqVec = 1:nEqCstr;
    showActInact = [ones(nEqCstr,1); zeros(nCstr-nEqCstr,1)];
    actInd = eqVec;
    actSet = G(eqVec,:);
    actCnt = nEqCstr;
  end % if ~isempty(depInd)
  [Q,R] = qr(actSet');
  Z = Q(:,nEqCstr+1:nVars);
  Y = Q(:,1:nEqCstr);
  err = 0;
  % Overdetermind active set matix. The equality constrints are
  % linearly dependent, but we have not been able to identify the
  % independent equality constraints.
  if nEqCstr > nVars
    status = 'overdet';
    return
  elseif nEqCstr == nVars
    x = actSet\d(eqVec);
      [mG,nG] = size(G)
      [md,nd] = size(d)
    cstr = G*x-d(nEqCstr+1:nCstr);
    if min(cstr) > -eps
      return
    end
  end  % end overdet. active set matrix. 
else
  Z = 1;
  Y = Q;
end  

cstr = G*x-d;

if phase2
  res = b-A*x;
else
  gb = b;
  dirType = 'StD';
  simplex = 1;
end

minInact = 0;
delCstr  = 0;
%-------------------------------------------------
%                   Main part.
%------------------------------------------------
while 1    
  %Find search direction.
  if phase2
    AZ = A*Z;
    [mm,nn] = size(AZ);
    if mm >= nn
      [QAZ, RAZ] =  qr(AZ);
      Pd = QAZ'*res;
      c = Pd(1:nn,1);
      if min(size(RAZ))==1
	depInd = find( abs(RAZ(1,1)) < tolDep);
      else
	depInd = find( abs(diag(RAZ)) < tolDep );
      end  
    end
    if mm >= nn & isempty(depInd)
      p = Z*(RAZ(1:nn, 1:nn)\c);
      dirType = 'NS';
    end
  else
    % Phase 1
    if strcmp(dirType,'StD')
      p = -Z*(Z'*gb);
    end    
  end %End If phase2
  
  % Find the steplength.
  Gp = G*p;
  indNew = find((Gp < -errNorm*norm(p))  &  ~showActInact);

  if isempty(indNew)
    % Did not hit a constraint.
    alpha = inf;
    dist = []; 
    indVec = []; 
    indMin = 0;
  else
    % Did hit a constraint.  
    dist = abs(cstr(indNew)./Gp(indNew));
    [alpha,indVec] =  min(dist);
    indVec = find(dist == alpha);
    indMin = indNew(min(indVec));
  end

  % Update x.
  if ~isempty(indNew) & isfinite(alpha)
    % Did hit a constraint and steplength < inf. 
    if strcmp(dirType, 'NS')
      if alpha > 1 
	alpha = 1;
	delCstr = 1;
	indMin = 0;
      end
    end
    a = alpha;
    x = x+alpha*p; 
  else
    % Did not hit a constraint or steplength = inf.
    if strcmp(dirType,'NS')
      % If a Newton step, reset steplength.
      alpha = 1;
      x = x + p;
      delCstr = 1;
      indMin = 0;
    else
      %Unbounded problem.
      status = 'unbounded'; 
      disp('unbounded')
      return
    end % if strcmp(dirType, 'NS')
  end % if ~isempty(indnew)& isfinite(alpha)
  
  if phase2
    res = b-A*x;
  end

  cstr = G*x-d;
  cstr(eqVec) = abs(cstr(eqVec));
  % If in Phase 1, searching for initial feasible point.
  % For finding a feasible point, the artificial variable
  % must tend to zero.
  if ~phase2
    if x(nVars,1) < eps
      return
    end
  end

  % Check for optimality if the active set is not equal to the
  % empty set.
  % If optimum => stop.
  % else       => a constraint in the active set is now inactive. 
  if delCstr
    if actCnt > 0
      % Test for optimality.
      if phase2
	AY = A*Y;
	U = QAZ'*AY;
	[mU,nU] = size(U);
	U = U(nn+1:mU,:);
	cn = Pd(nn+1:mU,1);
	rlagrange = -(R(1:actCnt,1:actCnt))\(U'*cn);
	actlagrange = rlagrange;
	actlagrange(eqVec) = abs(rlagrange(eqVec));
	indInact = find(actlagrange < 0);
	if (~length(indInact)) 
	  lagrange(indepInd(actInd)) = rlagrange;
	  return
	end
	% Remove constraint.
	minInact = find(actInd == min(actInd(indInact)));
	minInact = minInact(1);
      end
      actSet(minInact,:) = [];
      showActInact(actInd(minInact)) = 0;
      [Q,R] = qrdelete(Q,R,minInact);
      actCnt = actCnt-1;
      Y = Q(:,1:actCnt);
      Z = Q(:,1+actCnt:nVars);
      actInd(minInact) = [];
    else
      % Exact Newton step.
      lagrange = zeros(size(d));
      return
    end
    delCstr = 0;
  end
  
  % If hit a constraint, add the new constraint to the active set.
  if indMin
    showActInact(indMin) = 1;
    actSet(actCnt+1,:) = G(indMin,:);
    actInd(actCnt+1) = indMin;
    % Insert constraint to the active set.
    [Q,R] = qrinsert(Q,R,actCnt+1,G(indMin,:)');
    actCnt = actCnt+1;
    Y = Q(:,1:actCnt);
    Z = Q(:,1+actCnt:nVars);
  end
  
  if simplex
    % Test for optimality.
    if actCnt > nEqCstr
      if norm(Z'*b) < eps
	Qb = (Q(:,1:actCnt))'*b;
	rlagrange = R(1:actCnt,1:actCnt)\Qb;
	actlagrange = rlagrange;
	actlagrange(eqVec) = abs(rlagrange(eqVec));
	indInact = find(actlagrange < 0);
	if (~length(indInact)) 
	  lagrange(indepInd(actInd)) = rlagrange;
	  return
	end
	% Remove constraint.
	minInact = find(actInd == min(actInd(indInact)));
	minInact = minInact(1);
	if actCnt == nVars
	  es = zeros(size(actSet,1),1);
	  es(minInact,1) = 1;
	  p = actSet\es;
	  dirType = 'inActStep';
	else
	  dirType = 'StD';
	end
	delCstr = 1;
      end
    else
      minInact = 0;
      delCstr = 0;
    end
  end

end % while 1
%-------------------------------------------------
%      END OF TCSUB
%-------------------------------------------------