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]=sym2udv(p,tolp,toln)
```% SYM2UDV : vectorized UDU factorization P=UDU'
%           U unit upper triangular, D diagonal, P nonnegative symmetric
%
%           [ud,r]=sym2udv(p,tolp,toln);
%
%           p     : square symmetric matrix >= 0 (see toln)
%           tolp  : tolerance for positiveness and symmetry
%           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
%
%           ud    : vectorized ud matrix p=udu': v = vec(no-zero elements ud)
%           r     : rank of u and 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]=sym2udv(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 upper triangular part p into ud
lud=floor(n*(n+1)/2); ud=zeros(lud,1); ii=1;
for j=1:n; for i=1:j; ud(ii)=p(i,j); ii=ii+1; end; end

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