I am attempting to increase the performance of a legacy Kalman Filter implementation. The state covariance is factored in terms of UDU, i.e. $\mathbf{P} = \mathbf{U}\mathbf{D}\mathbf{U}^T$.
Many years ago, Bierman showed how scalar measurements may be used to update the covariance. Helpfully, it includes an implementation in FORTRAN that I have reproduced in MATLAB (though it combines certain operations in a loop and thus the algorithm is not particularly transparent).
Ultimately, I would like to replace the routine to take advantage of modern matrix libraries (BLAS, Eigen, etc) to increase performance.
Ultimately, the core of the operation is to perform a rank-1 update (downdate) of the UD-factored covariance in the form of:
$\mathbf{U_+}\mathbf{D_+}\mathbf{U_+}^T = \mathbf{U_-}\mathbf{D_-}\mathbf{U_-}^T - c\mathbf{v}\mathbf{v}^T$
where $\mathbf{U_-}$ and $\mathbf{D_-}$ is my prior known factorisation, c is a non-negative scalar and $\mathbf{v}$ is (known) a column vector. The goal is to determine $\mathbf{U_+}$ and $\mathbf{D_+}$.
So, my questions are:
- For the modified Cholesky update, what is the cost in terms of FLOPS - I understand it is $O(n^2$)?
- Is the a known efficient implementation in a commonly available library, or so I need to try to roll my own?
(For interest, $n$ is about 40 , but is being implemented on an embedded platform).