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.

[ldp,r,x]=ktldl(ldp,ldv,pd,xi,tolp,toln)
```% KTLDL :  Kalman time update based on LDL factorizations of symmetric p and v
%
%          time update:
%          p=pd*p*pd'+v, x=pd*x;
%
%          [ldp,r,x]=ktudu(ldp,ldv,pd,xi,tolp,toln)
%
%          ldp : ld of p
%          ldv : ld of v
%          pd  : may not be square but then x will be empty
%          xi  : initial state estimate
%        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
%
%          ldp: ld of pn
%          r  : rank of d
%          x  : time updated state estimate
%
%
%          References: Factorization methods for discrete sequential estimation
%                      1977, Gerald J. Bierman
%
% L.G. van Willigenburg, W.L. de Koning, Update August 2011

function [ldp,r,x]=ktldl(ldp,ldv,pd,xi,tolp,toln)

if nargin<3; error(' 3-6 input arguments'); end
if nargin<=4; tolp=1e-12; toln=tolp;
elseif nargin==5; toln=tolp; end; tolp=max(0,tolp);
if (toln>tolp); error('  toln > tolp'); end

[n1,n2]=size(ldp); [m1,m2]=size(ldv); [m,n]=size(pd);
if n1~=n || n2~=n; error('  ldp must be square and compatible with pd'); end;
if m1~=m || m2~=m; error('  ldv must be square and compatible with pd'); end;

if nargin>3; [nx,mx]=size(xi);
if nx~=0 && mx~=0; xflag=1; else xflag=0; end
else xflag=0; end

if xflag==1
if n~=nx;  error(' size pd and x do not match'); end
%if n~=m; error(' ldp, pd, and x should have equal n'); end
end

if n==0 || m==0; error(' Compatible but empty inputs'); end

if n>m; ld=[ldp [ldv; zeros(n-m,m)]];
else ld=[[ldp; zeros(m-n,n)] ldv];
end

nud=n+m; a=zeros(1,nud); d=a; v=a;

% Compute ld=pd*ld
for j=1:n-1
for i=n:-1:j; d(i)=ld(i,j); end %5
for i=m:-1:1
ld(i,j)=pd(i,j);
for k=n:-1:j+1
ld(i,j)=ld(i,j)+pd(i,k)*d(k);
%        disp([i j k]); disp([ld(i,j) pd(i,k) d(k)]);
end
end %10
end; %20
d(n)=ld(n,n);

% Compute x=pd*x
x=zeros(m,1);
for j=m:-1:1
ld(j,n)=pd(j,n);
if xflag==1;
v(j)=0; for k=1:n; v(j)=v(j)+pd(j,k)*xi(k); end %15
x(j)=v(j);
end
end %30

% x=pd*x, ld=pd*ld completed

%  keyboard;
for i=1:nud-n; d(i+n)=ld(i,n+i); ld(i,n+i)=1; end;

r=m;
for j=1:m
sig=0;
for k=nud:-1:1
v(k)=ld(j,k); a(k)=d(k)*v(k); sig=sig+v(k)*a(k);
end %40
ld(j,j)=sig;
if sig<toln; error(' toln violated');
elseif sig<tolp; dinv=0; r=r-1;
else dinv=1/sig; end;
if j~=m
jp1=j+1;
for k=m:-1:jp1
sig=0;
for i=1:nud
sig=sig+ld(k,i)*a(i);
end %50
sig=sig*dinv;
for i=nud:-1:1
ld(k,i)=ld(k,i)-sig*v(i);
ld(j,k)=sig;
end %60
end %70
end %if
end %100

for j=m-1:-1:1;
for i=m:-1:j+1;
ld(i,j)=ld(j,i); ld(j,i)=0;
end;
end;
ldp=ld(1:m,1:m);```