function [L,D]=modldlt(A,tol)
%MODLDLT Perform modifed positive definite LDLT factorization.
%
%[L,D]=modldlt(A,tol)
%A   - symmetric matrix.
%tol - tolerance for the factorization. D(i,i) will always be >= tol.
%L   - unitary lower triangular matrix.
%D   - diagonal matrix.
%
%MODLDLT performs a modified LDL' factorization of a symmetric matrix A.
%The factorization is modified to make the product LDL' positive definite,
%corresponding to making an unmodified LDL' factorization of A+E, where E
%is a diagonal matrix.
%
%The tolerance tol controls the replacement of values in D. A negative
%value in D(i,i) is replaced by max(abs(D(i,i)),tol). A positive value in
%D(i,i) less than tol is replaced by tol.

% v1.0  97-11-15. Niclas Borlin, niclas@cs.umu.se.

[m,n]=size(A);

error(nargchk(2,2,nargin));

if (m~=n)
	error('MODLDLT: A must be square');
end

if (any(any(A~=A')))
	error('MODLDLT: A must be symmetric');
end

if (tol<=0)
	error ('MODLDLT: Tolerance must be positive');
end

% Start with identity matrix.
L=eye(n);
% Store only diagonal elements.
D=zeros(1,n);

if (n>0)
	D(1)=A(1,1);
	if (D(1)<tol)
		v=max(abs(D(1)),tol);
		% disp(sprintf('%g replaced by %g',D(1),v));
		D(1)=v;
	end
	L(2:n,1)=A(2:n,1)/D(1);
	
	for i=2:n
		D(i)=A(i,i)-sum(L(i,1:i-1).^2.*D(1:i-1));
		if (D(i)<tol)
			v=max(abs(D(i)),tol);
			% disp(sprintf('%g replaced by %g',D(i),v));
			D(i)=v;
		end

		LiDi=L(i,1:i-1).*D(1:i-1);
		L(i+1:n,i)=(A(i+1:n,i)-L(i+1:n,1:i-1)*LiDi')/D(i);
	end
end

% Convert to diagonal matrix.
D=diag(D);

