Code covered by the BSD License  

Highlights from
UD Factorization & Kalman Filtering

UD Factorization & Kalman Filtering

by

 

15 Aug 2011 (Updated )

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

[ld,r]=sym2ldv(p,tolp,toln)
% SYM2LDV : vectorized LDL factorization P=LDL'
%           L unit lower triangular, D diagonal, P nonnegative symmetric
%
%           [ld,r]=sym2ldv(p,tolp,toln);
%
%           p     : square symmetric matrix >= 0 (see toln)
%           tolp  : tolerance for positiveness and symmetry
%                   default 1e-12, see also toln
%           toln  : error generation if to be square rooted part < toln
%                   default: tolp
%                   if toln < 0 all the to be square rooted
%                   parts < max(0,tolp) are set to zero
%
%           ld    : vectorized ld matrix p=ldl': v = vec(no-zero elements ld)
%           r     : rank of l and d
%
%           See also ld2sym, ldt2sym, sym2ld, ldinv
%
%           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]=sym2ldv(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); r=n;
if n~=m; error('  p must be square'); end;
if n==0; error('  Compatible but empty inputs'); end;

% Copy lower triangular part p into ld
lld=floor(n*(n+1)/2); ld=zeros(lld,1); ii=lld;
for j=n:-1:1; for i=n:-1:j; ld(ii)=p(i,j); ii=ii-1; end; end

% Vectorized ld conversion
jj=0;
for j=1:n-1
  if ld(jj+j)<toln; error('  toln or symmetry violated');
  elseif ld(jj+j)<=tolp; ld(jj+j)=0; a=0; r=r-1; zf=1;
  else a=1./ld(jj+j); zf=0; end;
  kk=lld-n;
  if ~zf
    for k=n:-1:j+1;
      b=ld(jj+k); ld(jj+k)=a*b;
      for i=n:-1:k
        ld(kk+i)=ld(kk+i)-b*ld(jj+i);
      end
      kk=kk-n+k-1;
    end
  else
    for k=n:-1:j+1;
      ld(jj+k)=0;
    end
  end
  jj=jj+n-j;
end
if ld(lld)<toln; error('  toln or symmetry violated');
elseif ld(lld)<=tolp; ld(lld)=0; r=r-1;
end

Contact us