[C,d]=eliminateConstraints(A,b)

function [C,d]=eliminateConstraints(A,b)
% function [C,d]=eliminateConstraints(A,b) eliminates variables from
% problem with linear equality constraints to give an unconstrained
% problem. This is useful e.g. when solving a problem with linear
% constraints and a nonlinear objective or further nonlinear constraints;
% eliminating the linear constraints makes the problem easier.
%
% Where the original constrained problem has:
% - variable vector x (length n).
% - linear constraint A*x=b, where the matrix A and vector b are
%     eliminateConstraints's arguments.
% 
% And the final unconstrained problem has:
% - variable vector y (length p).
% - an x satisfying the original constraint can be generated by x=C*y+d,
%     where the matrix C and vector d are returned by eliminateConstraints.
% 
% Inputs:
% - A (an m*n matrix of real numbers)
% - b (a  m*1 vector of real numbers)
%
% - C (an n*p matrix of real numbers)
% - d (a  n*1 vector of real numbers)
%
% Hopefully, p<n, so the unconstrained problem is smaller than the
% constrained problem.
%
% The functionality provided by eliminateConstraints is trivial for small
% matricies.  For larger matrices, it can be done using MATLAB's various
% matrix decompositions.  However, as far as the author is aware, there is
% no trivial MATLAB solution for the case where A is very large and sparse,
% so we require a C which is also sparse without using any very large full
% matrices.
%
% The case for which this was developed automatically creates a
% well-defined problem, so only minimal validation and error checking has
% been implemented at this stage.

%% validate input
if nargin~=2
    error('Must specify A & b')
end
if size(A,1)~=size(b,1)
    error('A & b must have the same number of rows')
end

%% set up
if issparse(A)
    C=sparse(1:size(A,2),1:size(A,2),1,size(A,2),size(A,2));
else
    C=eye(size(A,2));
end
if issparse(b)
    d=sparse(size(A,2),1);
else
    d=zeros(size(A,2),1);
end

independent=true(size(A,2),1);

curA=A;
curb=b;

%% process
for curRow=1:size(A,1)
    newDependent=find(curA(curRow,:) & independent');
    if isempty(newDependent)
        %check that this row is consistent with the others
        if nnz(curA(curRow,:)*C)~=0 || curA(curRow,:)*d~=curb(curRow)
            error(['Row ',num2str(curRow),' is inconsistent with some previous row; perhaps the problem was ill-defined?'])
        end
    else
        [temp,I]=max(curA(curRow,newDependent));
        newDependent=newDependent(I);
        %create a new dependent variable
        newC=-curA(curRow,:)/curA(curRow,newDependent);
        newC(newDependent)=0;
        newd=curb(curRow)/curA(curRow,newDependent);
        
        d=d+C(:,newDependent)*newd;
        C1=C;
        C1(:,newDependent)=0;
        C=C1+C(:,newDependent)*newC;
        
        curb=curb-curA(:,newDependent)*newd;
        A1=curA;
        A1(:,newDependent)=0;
        curA=A1+curA(:,newDependent)*newC;
        
        independent(newDependent)=false;
    end
end
C=C(:,independent);

%% check
if max(max(A*C))>1e-6 || min(min(A*C))<-1e-6
    error('The output is not valid (A*C~=0).  Perhaps the original problem was ill-defined?')
end
if max(A*d-b)>1e-6 || min(A*d-b)<-1e-6
    error('The output is not valid (A*d~=b).  Perhaps the original problem was ill-defined?')
end