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.

ldlex.m
% LDLEX : Examples and tests of all LDL toolbox functions through
%         comparison of LDL 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 for rank deficiency
tolp=1e-12; toln=-tolp;

% Check LDL Factorization and recovery
[ld,r]=sym2ld(m,tolp,toln); p=ld2sym(ld); [l,d]=ld2ld(ld);
% Check zero differences
zero=[zero,norm([l*d*l'-m, p-m])];

% Check inverse using LDL factorization
[ld,r]=sym2ld(m,tolp,toln); ldi=ldinv(ld); pi=ldt2sym(ldi);
% Check zero differences if matrix invertible = full rank
zero=[zero,norm(eye(nx)-m*pi)];
 
% Check LL' Factorization and recovery
[lc,rc]=sym2lt(m,tolp,toln); pc=lt2sym(lc);
% Check zero differences
zero=[zero,norm([lc*lc'-m, pc-m])];

% Check inverse using LL' factorization
lic=ltinv(lc); pic=ltt2sym(lic);
% Check zero differences if matrix invertible = full rank
zero=[zero,norm(eye(nx)-p*pic)];

% Check conversions
lt1=ld2lt(ld); ld1=lt2ld(lc);
% Check zero differences
zero=[zero,norm([lt2sym(lt1)-m, ld2sym(ld1)-m])];

% Check reduced rank LDL & LL' conversion
% Tolerances for positiveness and nonnegativeness
tolp=1e-12; toln=-1e-12;
m1=psdr(nx,nr1);
[ld1]=sym2ld(m1,tolp,toln); [l1,d1]=ld2ld(ld1);
[lt1]=sym2lt(m1,tolp,toln);
% Check zero differences
zero=[zero,norm([m1-l1*d1*l1',m1-lt1*lt1'])];

% Check addition LDL factored matrices
[ld2]=ldladd(ld,ld1,1.2,2.1);
% Check zero differences
zero=[zero,norm(ld2sym(ld2)-1.2*m-2.1*m1)];

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

% LDL Kalman filter time update
ldp=sym2ld(po,tolp,toln); ldv=sym2ld(v,tolp,toln);
[ldt,r,xln]=ktldl(ldp,ldv,pd,x,tolp,toln);
ptl=ld2sym(ldt);

% Ordinary Kalman filter time update
[ptn,xn]=kmftu(pd,v,po,x);
% Check zero differences
zero=[zero,norm([ptn-ptl,xn-xln])];

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

% LDL Kalman filter time update
ldp=sym2ld(po,tolp,toln); ldv=sym2ld(v,tolp,toln);
[ldt,r,xln]=ktldl(ldp,ldv,pd,x,tolp,toln);
ptl=ld2sym(ldt);
zero=[zero,norm([ptn-ptl,xn-xln])];

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

% LDL Kalman filter time update
ldp=sym2ld(po,tolp,toln); ldv=sym2ld(v,tolp,toln);
[ldt,r,xln]=ktldl(ldp,ldv,pd,x,tolp,toln);
ptl=ld2sym(ldt);
zero=[zero,norm([ptn-ptl,xn-xln])];

% Test ldladd
p1=psdr(nx,nr1); p2=psdr(nx);
p1c=rand; p2c=rand; pa=p1c*p1+p2c*p2;
ld1=sym2ld(p1,tolp,toln); ld2=sym2ld(p2,tolp,toln);
[la]=ldladd(ld1,ld2,p1c,p2c,tolp,toln);
zero=[zero,norm(pa-ld2sym(la))];

% Test vectorized LDL factorization
% Create index vector
ind=[]; jj=nx*(nx-1);
for j=nx:-1:1
  ind=[[j:nx]+jj,ind]; jj=jj-nx;
end

% Compare
[lv2]=sym2ldv(p2,tolp,toln); l2v=ld2(:);
[lv1]=sym2ldv(p1,tolp,toln); l1v=ld1(:);
zero=[zero,norm([lv2-l2v(ind),lv1-l1v(ind)])];

% Check Kalman measurement update
c=randn(nr1,nx); w=psdr(nr1); y=randn(nr1,1);
% LDL measurement update, note that the Kalman gain ks for scalar
% and kv for vectorized updates are different
[ldm,ks,r,xml,kv]=kmldl(ldp,c,w,x,y,tolp,toln); pml=ld2sym(ldm);
% Ordinary Kalman filter time update
[pm,k,xm]=kmfmu(c,w,po,x,y);
% Check zero differences
zero=[zero,norm([pm-pml,xm-xml,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