Code covered by the BSD License

UD Factorization & Kalman Filtering

Gerard Van Willigenburg (view profile)

15 Aug 2011 (Updated )

UD and LD factorization of nonnegative matrices and associated Kalman filter implementations.

[ld,r,inz]=sym2ld(p,tolp,toln)
```% SYM2LD: LDL factorization P=LDL'
%         L unit lower triangular, D diagonal, P nonnegative symmetric
%
%         [ld,r,inz]=sym2ld(p,tolp,toln);
%
%         p     : square symmetric matrix >= 0 (see toln)
%         tolp  : tolerance for positiveness
%         toln  : error generation if
%     	          to be square rooted part < toln
%                 default: tolp
%                 if toln < 0 all the to be square rooted
%                 parts < tolp are set to zero
%
%         ld    : factorization matrix, u unit lower triangular
%                 d diagonal, both stored in ld
%         r     : rank of u and d
%         inz   : row with indices non zero columns u,d
%
%
%         References: Factorization methods for discrete sequential estimation
%                     1977, Gerald J. Bierman
%
% L.G. van Willigenburg, W.L. de Koning, Update August 2011

function [ld,r,inz]=sym2ld(p,tolp,toln)

if nargin > 3; error('  one, two or three input arguments required'); end;
if nargin==1; tolp=1e-12; toln=tolp;
elseif nargin==2; toln=tolp; end; tolp=max(0,tolp);
if (toln>tolp); error('  toln > tolp'); end;
%toln,tolp
[n,m]=size(p);
if n~=m; error('  p must be square'); end;
if n==0; error('  Compatible but empty inputs'); end;

ld=zeros(n,n); r=n; inz=[];

% copy lower triangular part p to ld
for j=n:-1:1; for i=n:-1:j; ld(i,j)=p(i,j); end; end;

for j=1:n-1
if ld(j,j)<toln; error('  toln or symmetry violated');
elseif ld(j,j)<=tolp; ld(j,j)=0; a=0; r=r-1; zf=1;
else a=1./ld(j,j); inz=[j,inz]; zf=0; end
if ~zf
for k=n:-1:j+1;
b=ld(k,j); ld(k,j)=a*b;
for i=n:-1:k
ld(i,k)=ld(i,k)-b*ld(i,j);
end
end
else
for k=n:-1:j+1;
ld(k,j)=0;
end
end
end
if ld(n,n)<toln; error('  toln or symmetry violated');
elseif ld(n,n)<=tolp; ld(n,n)=0; r=r-1;
end```