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.

```% LDLADD : Add (scaled) LDL factored matrices P,V: P=pc*P+vc*V, pc,vc scalars
%          P,V nonnegative symmetric
%
%
%          ldp : ld of p
%          ldv : ld of v
%          pc  : scale factor P, default 1
%          vc  : scale factor V, default 1
%
%          ldp: ld of p
%          r  : rank of d
%
%
%          References: Factorization methods for discrete sequential estimation
%                      1977, Gerald J. Bierman
%
% L.G. van Willigenburg, W.L. de Koning, Update August 2011

if nargin<2; error(' 2-6 input arguments'); end;
if nargin<3; pc=1; vc=1; tolp=1e-12; toln=tolp;
elseif nargin<4
[n,m]=size(pc);
if n>1 || m>1; error(' pc must empty or scalar'); end;
if n==0 || m==0; pc=1; end;
vc=1; tolp=1e-12; toln=tolp;
elseif nargin<5;
[n,m]=size(vc);
if n>1 || m>1; error(' vc must empty or scalar'); end;
if n==0 || m==0; vc=1; end;
tolp=1e-12; toln=tolp;
elseif nargin<6;
toln=tolp;
elseif nargin>6;
error(' 2-6 input arguments');
end
if isempty(pc); pc=1; end
if isempty(vc); vc=1; end

tolp=max(0,tolp);
if (toln>tolp); error('  toln > tolp'); end;

[n,m]=size(ldp); [n1,m1]=size(ldv);
if n~=m || n1~=n || m1~=m; error('  ldp, ldv must be square of equal size'); end;
if n==0 || m==0; error('  Compatible but empty inputs'); end;

ld=[ldp ldv];
nud=n+n; a=zeros(1,nud); v=zeros(1,nud); d=zeros(1,nud);

for i=1:n; d(i)=pc*ld(i,i); ld(i,i)=1; end;

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

r=n;
for j=1:n
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; sig=0; 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);```