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.

uduex.m
% UDUEX : Examples and tests of all UDU toolbox functions through
%         comparison of UDU factored with conventional computations
%
% L.G. van Willigenburg, W.L. de Koning, Updated August 2011

% Dimensions and ranks
nx=5; nr=nx; nr1=max(nx-2,1); m=psdr(nx,nr); zero=[];

% Tolerances rank deficiency
tolp=1e-12; toln=-tolp;

% Check UDU Factorization and recovery
[ud,r]=sym2ud(m,tolp,toln); p=ud2sym(ud); [u,d]=ud2ud(ud);
% Check zero differences
zero=[zero,norm([u*d*u'-m, p-m])];

% Check inverse using UDU factorization
udi=udinv(ud,tolp,toln); pi=udt2sym(udi);
% Check zero differences if matrix invertible = full rank
zero=[zero,norm(eye(nx)-p*pi)];

% Check UU' Factorization and recovery
[uc,rc]=sym2ut(m,tolp,toln); pc=ut2sym(uc);
% Check zero differences
zero=[zero,norm([uc*uc'-m, pc-m])];

% Check inverse using UU' factorization
uic=utinv(uc); pic=utt2sym(uic);
% Check zero differences if matrix invertible = full rank
zero=[zero,norm(eye(nx)-p*pic)];

% Check conversions
ut1=ud2ut(ud); ud1=ut2ud(uc);
% Check zero differences
zero=[zero,norm([ut2sym(ut1)-m, ud2sym(ud1)-m])];

% Check reduced rank UDU & UU' conversion
% Tolerances for positiveness and nonnegativeness
tolp=1e-12; toln=-1e-12;
m1=psdr(nx,nr1);
[ud1]=sym2ud(m1,tolp,toln); [u1,d1]=ud2ud(ud1);
[ut1]=sym2ut(m1,tolp,toln);
% Check zero differences
zero=[zero,norm([m1-u1*d1*u1',m1-ut1*ut1'])];

% Check addition UDU factored matrices
[ud2]=uduadd(ud,ud1,1.2,2.1);
% Check zero differences
zero=[zero,norm(ud2sym(ud2)-1.2*m-2.1*m1)];

% Check Kalman time update
v=psdr(nx); udv=sym2ud(v,tolp,toln);
po=psdr(nx); udp=sym2ud(po,tolp,toln);
pd=randn(nx); x=randn(nx,1);

% UDU Kalman filter time update
[udt,r,xun]=ktudu(udp,udv,pd,x,tolp,toln);
ptu=ud2sym(udt); gd=zeros(nx,1); uo=0;
% Ordinary Kalman filter time update
[ptn,xn]=kmftu(pd,v,po,x);
% Check zero differences
zero=[zero,norm([ptn-ptu,xn-xun])];

% Kalman filter time-updates with variable state dimension
pd=randn(nr1,nx); v=psdr(nr1); udv=sym2ud(v,tolp,toln);
[ptn,xn]=kmftu(pd,v,po,x); % conventional time update

% UDU Kalman filter time update
[udt,r,xnu]=ktudu(udp,udv,pd,x,tolp,toln); % udu time update not for x
ptu=ud2sym(udt);
zero=[zero,norm([ptn-ptu,xn-xnu])];

pd=randn(nx+2,nx); v=psdr(nx+2); udv=sym2ud(v);
[ptn,xn]=kmftu(pd,v,po,x); % conventional time update

% UDU Kalman filter time update
[udt,r,xnu]=ktudu(udp,udv,pd,x,tolp,toln); % udu time update not for x
ptu=ud2sym(udt);
zero=[zero,norm([ptn-ptu,xn-xnu])];

% Test uduadd
p1=psdr(nx,nr1); p2=psdr(nx);
p1c=rand; p2c=rand; pa=p1c*p1+p2c*p2;
ud1=sym2ud(p1,tolp,toln); ud2=sym2ud(p2,tolp,toln);
[ua]=uduadd(ud1,ud2,p1c,p2c,tolp,toln);
zero=[zero,norm(pa-ud2sym(ua))];

% Test vectorized UDU factorization
% Create index vector
ind=[]; jj=0;
for j=1:nx
  ind=[ind,[1:j]+jj]; jj=jj+nx;
end
% Compare
[uv2]=sym2udv(p2); u2v=ud2(:);
[uv1]=sym2udv(p1,tolp,toln); u1v=ud1(:);
zero=[zero,norm([uv2-u2v(ind),uv1-u1v(ind)])];

% Check Kalman measurement update
c=randn(nr1,nx); w=psdr(nr1); y=randn(nr1,1);
% UDU measurement update, note that the Kalman gain ks for scalar
% and kv for vectorized updates are different
[udm,ks,r,xmu,kv]=kmudu(udp,c,w,x,y,tolp,toln); pmu=ud2sym(udm);
% Ordinary Kalman filter time update
[pm,k,xm]=kmfmu(c,w,po,x,y);
% Check zero differences
zero=[zero,norm([pm-pmu,xm-xmu,kv-k])];

% Display vector that should be approximately zero
disp('  All elements of the vector zero below should be approximately zero');
zero

% Display message to confirm or deconfirm
if max(abs(zero))<1e-8;
  disp('  OK: all elements are approximately zero')
else
  disp('  Failure: some elements deviate significantly from zero');
end

Contact us