%
% x=irls(A,b,tolr,tolx,p,maxiter)
%
% This function uses the iteratively reweight least squares strategy
% to find an approximate L_p solution to Ax=b.  
%
% Inputs;
%   A             Matrix of the system of equations.
%   b             right hand side of the system of equations.
%   tol           Tolerance below which residuals are ignored.
%   p             Specifies which p-norm to use.
%   maxiter       Limit on number of iterations of IRLS
%
% Outputs:
%   x             Approximate L_p solution.  
%
function x=irls(A,b,tolr,tolx,p,maxiter)
%
% Find the size of the matrix A.
%
[m,n]=size(A);
%
% Start the first iteration with R=I, and x=A\b (the lsq solution)
%
R=eye(m);
x=A\b;
%
% Now loop up to maxiter iterations
%
iter=1;
while (iter <= maxiter)
  iter=iter+1;
  r=A*x-b;
  for i=1:m
    if (abs(r(i)) < tolr)
      r(i)=abs(tolr)^(p-2);
    else
      r(i)=abs(r(i))^(p-2);
    end
  end
  R=diag(r);
  newx=(A'*R*A)\(A'*R*b);
  if (norm(newx-x)/(1+norm(x)) < tolx)
    return;
  else
    x=newx;
  end
end