# Implementing a square root Kalman filter

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?

• Why dont you use the mathworks.com/company/newsletters/articles/… embedded matlab? – morbo Aug 17 '19 at 8:37
• @morbo thanks for the suggestion. I’ve tried using the Matlab coder but i’ve found it doesn’t really give any additional info on the algorithms it generates (i.e methods and algorithms), which makes it hard to adapt as you don’t know what various bits of the generated code are doing. Also from a personal point of view I’d like to better understand the underlying maths. – Joe Aug 17 '19 at 8:54
• Mmm yes thats a general pain when trying to write the code...however, since you’ve already implemented the code once in matlab, are you winning anymore knowledge of the process rewriting it in C? Or are trying to understand more of how to write c code...if its the former, having it translated automagically isnt helping you any i would think and you’re saving yourself some time. If the latter...then yeah auto generated code isnt very helpful. Good luck regardless unfortunately, i dont know enough c code to help – morbo Aug 17 '19 at 8:59
• Are you aiming for c or c++? The eigen library is advisable for c++ work as it has all of the linear algebra algorithms needed. – holmeski Aug 17 '19 at 14:01
• I’m aiming for c++ I’ll have a look at the eigen library. I was hoping to confirm if the underlying maths - methodology and factorisation method was correct. – Joe Aug 18 '19 at 17:52