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.

[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
%                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 < tolp are set to zero
%
%          ldp: ld of pn
%          r  : rank of d
%          x  : time updated state estimate
%
%          See also ld2sym, sym2ld, ldladd
%
%          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);

Contact us