function x=LinearlyConstrainedLSQ(A,C,b,d)
% LINEARLYCONSTRAINEDLSQ solves linearly constrained least squares problem
%   x=LinearlyConstrainedLSQ(A,C,b,d); solves ||Ax-b||= min s.t. Cx=d
%   by direct Gaussian elimination. The reduced least squares problem
%   is solved using the standard Matlab \ operator.

[p,n]=size(C); [m,n]=size(A);
CA=[C,d;A,b];                                 % augmented matrix
pp=[1:n];                                     % permutation vector
for i=1:p,                                    % eliminate p unknowns 
  [h jmax]=max(abs(CA(i,i:n)));               % with Gaussian elimination 
  jmax=i-1+jmax;     
  if h==0, error('Matrix C is rank deficient'); end
  if jmax~=i                                  % exchange columns
    h=CA(:,i); CA(:,i)=CA(:,jmax); CA(:,jmax)=h;
    zz=pp(i); pp(i)=pp(jmax); pp(jmax)=zz;
  end;
  CA(i+1:p+m,i)=CA(i+1:p+m,i)/CA(i,i);        % elimination
  CA(i+1:p+m,i+1:n+1)=CA(i+1:p+m,i+1:n+1) ...
                     -CA(i+1:p+m,i)*CA(i,i+1:n+1);
end;
y2=CA(p+1:m+p,p+1:n)\CA(p+1:m+p,n+1);         % solve lsq.-problem
y1=triu(CA(1:p,1:p))\(CA(1:p,n+1)-CA(1:p,p+1:n)*y2);
x(pp)=[y1;y2];                                % permute solution
x=x(:);
