Code covered by the BSD License

# UD Factorization & Kalman Filtering

### Gerard Van Willigenburg (view profile)

15 Aug 2011 (Updated )

UD and LD factorization of nonnegative matrices and associated Kalman filter implementations.

[ld,ks,r,x,kv]=kmldl(ld,c,w,x,y,tolp,toln,wbf)
```% KMLDL :  Kalman measurement update based on LDL factorizations
%          of symmetric p and consecutive scalar measurement updates
%
%          [ld,ks,r,x,kv]=kmldl(ld,c,w,x,y,tolp,toln,wbf)
%
%          ld  : ldl 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
%
%          ld : ld 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))
%
%
%          References: Factorization methods for discrete sequential estimation
%                      1977, Gerald J. Bierman
%
% L.G. van Willigenburg, W.L. de Koning, Update January 2012
function [ld,ks,r,x,kv]=kmldl(ld,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(ld);
if mud~=nud; error('  ld should be square'); end;

[ny,n]=size(c);
if n~=nud; error('  size c and ld 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 ld 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
if ~uc
[lt,r]=sym2lt(w,tolp,toln); if (r~=nw); error(' w must be positive'); end
lti=ltinv(lt); cf=lti*c; wf=ones(nw,1); if xflag==1; y=lti*y; end
else
cf=c; wf=w;
end  %  y, cf, wf computed: y, c, w after Cholesky factorization

for q=1:ny
if xflag==1
[ld,kg,r,x]=kmsldl(ld,cf(q,:),wf(q),x,y(q),tolp,toln);
else
[ld,kg,r,x]=kmsldl(ld,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*lti; else ks=ksf; end % Scalar Kalman gain

% 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```