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.

[ud,kg,ra,x]=kmsudu(ud,c,w,x,y,tolp,toln)
%   KMSUDU: Scalar Kalman UDU measurement update
%
%   [ud,kg,ra,x]=kmsudu(ud,c,w,x,y,tolp,toln)
%
%             ud   : udu 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
%
%             ud   : udu factorization of the covariance
%             kg   : Kalman gain column vector
%             ra   : rank(ud)
%             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 [ud,kg,ra,x]=kmsudu(ud,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(ud)>1e-6);
[n,m]=size(ud); [nc,mc]=size(c); [nw,mw]=size(w);
[nx,mx]=size(x);
    
if n~=m; error('  ud 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=n:-1:2
  for k=1:j-1; a(j)=a(j)+ud(k,j)*a(k); end;
  b(j)=ud(j,j)*a(j);
end
b(1)=ud(1,1)*a(1);

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

for j=2:n
  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;
  ud(j,j)=beta*gam*ud(j,j);
  for i=1:j-1;
    beta=ud(i,j); ud(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