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

CA = [C d; A b];      % augmented matrix
[p,n] = size(C); [m,n] = size(A);
pp = [1:n];           % permutation vector
% elimination of p unknowns with Gaussian elimination
for i = 1:p,
   [h jmax]=max(abs(CA(i,i:n))); jmax = i-1+jmax;
   if  h==0 ; error('Matrix C has not full rank')
   end
   % interchange columns
   if jmax~=i ,
      h=CA(:,i); CA(:,i)= CA(:,jmax); CA(:,jmax)=h;
      zz=pp(i); pp(i)=pp(jmax); pp(jmax)=zz;
   end;
   % elimination
   CA(i+1 : p+m,i) = CA(i+1 : p+m,i)/CA(i,i);
   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;
% solution of the least squares problem
y2 = CA(p+1:m+p,p+1:n)\CA(p+1:m+p,n+1);
% back-substitution for the rest of unknowns
y1 = triu(CA(1:p,1:p))\(CA(1:p,n+1)-CA(1:p,p+1:n)*y2);
% permute the solution
x(pp) = [y1;y2];
x = x(:);

