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]=ldladd(ldp,ldv,pc,vc,tolp,toln)
% LDLADD : Add (scaled) LDL factored matrices P,V: P=pc*P+vc*V, pc,vc scalars
%          P,V nonnegative symmetric
%
%          [ldp,r]=ldladd(ldp,ldv,pc,vc,tolp,toln)
%
%          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
%
%          See also ld2sym, sym2ld, ktldl
%
%          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]=ldladd(ldp,ldv,pc,vc,tolp,toln)

  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);

Contact us