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,ks,r,x,kv]=kmudu(ud,c,w,x,y,tolp,toln,wbf)
% KMUDU :  Kalman measurement update based on UDU factorizations
%          of symmetric p and consecutive scalar measurement updates
%
%          [ud,ks,r,x,kv]=kmudu(ud,c,w,x,y,tolp,toln,wbf)
%
%          ud  : udu factorization of p
%          c   : observation matrix
%          w   : observation noise covariance matrix. A column vector
%                w=diag(w) indicates uncorrelated noise
%          x   : state
%          y   : vector with observations
%          tolp,toln : tolerances for semi-positiveness
%          wbf : Waitbar flag; no-zero then waitbar display
%
%          ud   : ud of pn
%          ks   : Kalman gain matrix for scalar updates
%                 x(i)=ks(:,i)*(y(i)-c(i,:)*x(i-1),i=1,2,..,ny
%                 x(0): Initial state estimate
%                 x(ny): Updated state estimate
%          r    : rank of d
%          x    : state
%          kv   : Kalman gain for vector updates
%                 x(ny)=kv*(y-c*x(0))
%
%          See also kmsudu, ud2sym, sym2ud, ktudu
%
%          References: Factorization methods for discrete sequential estimation
%                      1977, Gerald J. Bierman
%
% L.G. van Willigenburg, W.L. de Koning, Update January 2012

  function [ud,ks,r,x,kv]=kmudu(ud,c,w,x,y,tolp,toln,wbf)

  if nargin<=4; xflag=0; x=[]; tolp=1e-12; toln=tolp; wbf=0;
  elseif nargin<=5; tolp=1e-12; toln=tolp; xflag=1; wbf=0;
  elseif nargin<=6; toln=1e-12; xflag=1; wbf=0;
  elseif nargin<=7; wbf=0;
  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;

  [nud,mud]=size(ud);
  if mud~=nud; error('  ud should be square'); end;

  [ny,n]=size(c); 
  if n~=nud; error('  size c and ud do not match'); end

  [nw,mw]=size(w); 
  if nw~=ny; error('  w incompatibel with c'); end
  if mw==1; uc=1;
  elseif mw~=ny; error('  w incompatibel with c');
  else uc=0; end
  
  if nargin>3; [nx,mx]=size(x);
    if nx~=0 && mx~=0; xflag=1; else xflag=0; end;
  else if nargout==4; x=[]; end; xflag=0; end;

  if xflag==1 && nargin<4;
    error('  if x is specified y must also be specified');
  end
  if xflag==1
    [ny1,my]=size(y); if ny1~=ny || my~=1; error('  y and c incompatible'); end;
  end

  if xflag==1
    if n~=nx;  error(' size ud and x do not match'); end;
  end; ksf=[]; % Initialize scalar Kalman gains after Cholesky factorization

  if wbf;
    wbh=waitbar(0,'Recursive least squares computation ...');
  end
  
  % Cholesky factorization w to get uncorrelated measurements with unit variance
  % and associated adaptation y,c,w
  if ~uc
    [ut,r]=sym2ut(w,tolp,toln); if (r~=nw); error(' w must be positive'); end
    uti=utinv(ut,tolp,toln); cf=uti*c; wf=ones(nw,1); if xflag==1;  y=uti*y; end
  else
    cf=c; wf=w;
  end  %  y, cf, wf computed: y, c, w after Cholesky factorization
  
% loop of scalar updates
  for q=1:ny
    if xflag==1
      [ud,kg,r,x]=kmsudu(ud,cf(q,:),wf(q),x,y(q),tolp,toln);
    else
      [ud,kg,r,x]=kmsudu(ud,cf(q,:),wf(q),[],[],tolp,toln);
    end
%   Collect scalar Kalman gains after Cholesky factorization
    ksf=[ksf kg];
    if wbf; waitbar(q/ny,wbh); end
  end
  if wbf; close(wbh); end
  if ~uc; ks=ksf*uti; else ks=ksf; end % Scalar Kalman gains
  
% Compute Kalman gain kv for vector update if required
% Uses eye(n)-kv*c = 
% (eye(n)-ksf(:,1)*cf(1,:))*(eye(n)-ksf(:,2)*cf(2,:))*...*(eye(n)-ksf(:,ny)*cf(ny,:))
% cf  : c obtained after Cholesky factorization,
% ksf : scalar Kalman gains obtained after Cholesky factorization
% kv  : Kalman gain for vectorized update
  if nargout>4
    for q=1:ny
      kg=ksf(:,q);
      if q==1;
        ikh=eye(n)-kg*cf(q,:); ikvc=ikh;
      else
        ikh=eye(n)-kg*cf(q,:); ikvc=ikh*ikvc;
      end
    end
    kvc=eye(n)-ikvc; % convert ikvc=eye(nx)-kv*c to kv*c
    kv=kvc/c; % compute kv=(kv*c)/c
  end

Contact us