function [f,e,r,itn] = stls(K,g,lambda,tol)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  function [f,e,r,itn] = stls(K,g,lambda,tol)
%
%  This function computes a solution to a structured
%  total least squares problem.
%  Given a matrix K, a positive scalar lambda, and 
%  a vector g, we minimize the function
%    || E ||^2 + || r ||^2 + lambda^2 || f ||^2
%  over all choices of E and f
%  subject to the constraints that
%      (K+E) f = g + r and
%       E is a Toeplitz matrix defined by parameters e.
%  We measure matrix norms using the Frobenius norm
%  (the square root of the sum of the elements squared)
%  and we measure vector norms by the Euclidean norm.
%  The algorithm uses Newton's method with steplength = 1
%  and stops when the norm of the Newton direction is
%  less than tol.  The scalar itn records the number
%  of iterations.
%  The number of rows in K must be greater than or equal 
%  to the number of columns.
%
%  stls.m   Dianne P. O'Leary 11/2004
%  changed 12/2007 to include the case that nrows = ncols
%                  (thanks to Scott Miller)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization: 
%          f = solution to least squares problem
%              min || K f - g ||
%          D = diagonal matrix of weights
%              so that || E || can be computed
%              from the elements e that
%              define it.
%          e = 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

f = K \ g;
[nrows,ncols]=size(K);
r = g - K*f;

length_e = nrows + ncols - 1;

% change 12/2007 to include the case of nrows = ncols

if (nrows > ncols)
   d = sqrt([1:ncols,ncols*ones(1,nrows-ncols-1),ncols:-1:1]');
elseif (nrows == ncols)
   d = sqrt([1:ncols,ncols-1:-1:1]');
else
   error('The number of rows in K must be greater than or equal to the number of columns.')
end

% end change

D = diag(d);

e = zeros(length_e,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We solve the problem using Newton's method.
% The Newton direction p is the solution to the least
% squares problem || M p - s ||
% where
%   M = [ F     K+E   ]        s = [ -r       ]
%       [ D      0    ]   ,        [ D e   ] .
%       [ 0  lambda I ]            [ lambda f ]
%
% and F is a Toeplitz matrix formed from the entries of f. 
% The matrix part2 containes the constant part of M.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

part2 = [D,zeros(length_e,ncols);
         zeros(ncols,length_e), lambda*eye(ncols)];
p = f;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This loop performs the Newton iteration, computing
% p and then updating e and f until convergence.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

itn = 0;

while norm(p) > tol,
   f_row = [f(ncols:-1:1);zeros(nrows-1,1)];
   f_col = [f(ncols);zeros(nrows-1,1)];
   F = toeplitz(f_col,f_row);
   E = toeplitz(e(ncols:length_e),e(ncols:-1:1));
   p = -[F,K+E;part2] \ [-r;d.*e;lambda*f];
   e = e + p(1:length_e);
   f    = f    + p(length_e+1:end);
   r = g - K*f - E*f;
   itn = itn + 1;
end