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.

[ld,kg,ra,x]=kmsldl(ld,c,w,x,y,tolp,toln)
%   KMSLDL: Scalar Kalman LDL measurement update
%
%   [ld,kg,ra,x]=kmsldl(ld,c,w,x,y,tolp,toln)
%
%             ld   : ldl factorization of the covariance
%             c    : Output row vector
%             w    : scalar measurement covariance
%             y    : scalar measurement
%             x    : state estimate before the measurement update
%             tolp : tolerance for positiveness
%             toln : tolerance for negativeness
%
%             ld   : ldl factorization of the covariance
%             kg   : Kalman gain column vector
%             ra   : rank(ld)
%             x    : State estimate after the measurement update
%
%             References: Factorization methods for discrete sequential estimation
%                         1977, Gerald J. Bierman
%
% L.G. van Willigenburg, W.L. de Koning, Update August 2011

function [ld,kg,ra,x]=kmsldl(ld,c,w,x,y,tolp,toln)

if nargin<3 || nargin>7; error('  KMSUDU requires 3-7 inputs'); end;

if nargin<=4; xflag=0; x=[]; tolp=1e-12; toln=tolp;
elseif nargin<=5; tolp=1e-12; toln=tolp; xflag=1;
elseif nargin<=6; toln=1e-12; xflag=1;
else xflag=1;
end

if isempty(x); xflag=0; end
if isempty(tolp); tolp=1e-12; end
if isempty(toln); toln=1e-12; end
tolp=max(0,tolp); 
if (toln>tolp); error('  toln > tolp'); end

ra=sum(diag(ld)>1e-6);
[n,m]=size(ld); [nc,mc]=size(c); [nw,mw]=size(w);
[nx,mx]=size(x);
    
if n~=m; error('  ld must be a square matrix'); end
if nc~=1 || mc~=n; error('  c must be a row vector with length n'); end
if nw~=1 || mw~=1; error('  w must be a scalar'); end
if xflag;
  if nx~=n || mx~=1; error('  x must be a column vector of length n'); end;
  [ny,my]=size(y); if ny~=1 || my~=1; error('  y must be a scalar'); end;
end

b=zeros(n,1);
a=c; r=w; if xflag==1; z=y; end
if xflag==1;
  for j=1:n; z=z-a(j)*x(j); end
end
for j=1:n-1
  for k=n:-1:j+1; a(j)=a(j)+ld(k,j)*a(k); end;
  b(j)=ld(j,j)*a(j);
end
b(n)=ld(n,n)*a(n);

alp=r+b(n)*a(n);
if alp<toln; error('  toln violated');
elseif alp<=tolp; alp=0; gam=0;
else gam=1/alp;
end
ld(n,n)=r*gam*ld(n,n);

for j=n-1:-1:1
  beta=alp; alp=alp+b(j)*a(j); lab=-a(j)*gam;
  if alp<toln; error('  toln violated');
  elseif alp<=tolp; alp=0; gam=0;
  else gam=1/alp;
  end;
  ld(j,j)=beta*gam*ld(j,j);
  for i=n:-1:j+1;
    beta=ld(i,j); ld(i,j)=beta+b(i)*lab; b(i)=b(i)+b(j)*beta;
  end
end
if alp~=0; kg=b/alp; else kg=zeros(size(b)); end

%   Compute new state estimate if necessary
if xflag==1;
  z=z*gam; for j=1:n; x(j)=x(j)+b(j)*z; end
end

Contact us