I'm attempting to implement a square root filter in C, translating it from Matlab in order to run the filter on an embedded target. I've implemented the Matlab square root filter as detailed in these lecture notes. I can run the filter in Matlab, but have several queries regarding implementation on an embedded target.
Firstly, are there any issues with implementing a square root filter in the manner outlined - using cholesky and QR decomposition to factor covariance matrices and then a cholesky downdate to update the covariance matrix? I've come across various methods for factorising matrices for square root filtering (such as LU decomposition) and I'm not sure what the benefits/trade offs are of the different approaches. Can anyone point me to some free access resources that detail these other implementations and the advantages/disadvantages of the form?
Secondly, I'm trying to implement several linear algebra functions in C to translate the Matlab. I've tried to implement QR decomposition (see the code and algorithm references below) but the generated cholesky factors can result in negative or a mix of positive and negative values along the cholesky diagonal (I believe a negative definite or indefinite matrix). Will this potentially cause numerical instabilities and if so what would be the best way of handling this?
function R = qrDecomposition(X) %function [U,R] = qr(X) % Householder triangularization. [U,R] = hqrd(X); % Generators of Householder reflections stored in U. % H_k = I - U(:,k)*U(:,k)'. % prod(H_m ... H_1)X = [R; 0] % where m = min(size(X)) % G. W. Stewart, "Matrix Algorithms, Volume 1", SIAM, 1998. [n,p] = size(X); U = zeros(size(X)); m = min(n,p); R = zeros(m,m); for k = 1:m [U(k:n,k),R(k,k)] = housegen(X(k:n,k)); v = U(k:n,k)'*X(k:n,k+1:p); X(k:n,k+1:p) = X(k:n,k+1:p) - U(k:n,k)*v; R(k,k+1:p) = X(k,k+1:p); end end function [u,nu] = housegen(x) % [u,nu] = housegen(x) % Generate Householder reflection. % G. W. Stewart, "Matrix Algorithms, Volume 1", SIAM, 1998. % [u,nu] = housegen(x). % H = I - uu' with Hx = -+ nu e_1 % returns nu = norm(x). u = x; nu = norm(x); if nu == 0 u(1) = sqrt(2); return end u = x/nu; if u(1) >= 0 u(1) = u(1) + 1; nu = -nu; else u(1) = u(1) - 1; end u = u/sqrt(abs(u(1))); end
For example if Matrix A is:
A = [0.0511407714010260 0 0 0 0 0 0 0.0511407714010260 0 0 0 0 0 0 0.0511407714010260 0 0 0 1.02396836644569e-10 1.37135681886295e-10 1.12478797159422e-12 3.42643762452408e-08 0 0 -1.37132429558910e-10 1.02402553782641e-10 -9.93121985781834e-13 0 3.42643762452408e-08 0 -1.46872654731679e-12 -3.07053071785980e-13 1.71144153011299e-10 0 0 3.42643762452408e-08 1.13193271701665e-07 0 0 0 0 0 0 1.13193271701665e-07 0 0 0 0 0 0 1.13193271701665e-07 0 0 0 0 0 0 5.43386723518420e-11 0 0 0 0 0 0 5.43386723518420e-11 0 0 0 0 0 0 5.43386723518420e-11];
Calling qrDecomposition(A) I get:
-0.0511407714011513 2.38321671658982e-34 -1.86188805983580e-36 -6.86059994987285e-17 9.18788871011182e-17 9.84048492813760e-19 0 -0.0511407714011513 1.25012484017546e-35 -9.18810661642442e-17 -6.86098299878779e-17 2.05725914777557e-19 0 0 -0.0511407714011513 -7.53609247551514e-19 6.65392884110532e-19 -1.14666781323134e-16 0 0 0 -3.42644193321109e-08 -4.48644757368387e-40 -1.61429380095321e-41 0 0 0 0 -3.42644193321109e-08 7.35400509323176e-42 0 0 0 0 0 -3.42644193321109e-08
Finally, are there any additional checks or actions I should take to ensure the cholesky factors (and therefore covariance matrices) remain numerically stable?