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.

[ud,r,inz]=sym2ud(p,tolp,toln)
% SYM2UD: UDU factorization P=UDU'
%         U unit upper triangular, D diagonal, P nonnegative symmetric
%
%         [ud,r,inz]=sym2ud(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
%
%         ud    : factorization matrix, u unit upper triangular
%                 d diagonal, both stored in ud
%         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 [ud,r,inz]=sym2ud(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;

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

% copy upper triangular part p to ud
for j=1:n; for i=1:j; ud(i,j)=p(i,j); end; end;

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