Implement Joseph stabilised form Kálmán updating

[stephenswat]

The filtered covariance computation for the Kálmán filter in traccc is found here:

traccc/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp

Line 118 in f7a25f7

const matrix_type<6, 6> filtered_cov = (I66 - K * H) * predicted_cov;

This uses the traditional update step, described in "Pattern Recognition, Tracking and Vertex Reconstruction in Particle Detectors" by Frühwirth and Standlie in Eq. 3.29:

$$C_k = (I - K_kH_k)C_{k | k-1}$$

However, this form is known to be numerically unstable, and it can lead to asymmetries in the covariance matrix. An alternative form is the Joseph stabilised form (see "Filtering for Stochastic Processes With Applications to Guidance" by Bucy and Joseph) which is as follows (where $V$ is the measurement covariance:

$$C_k = (I - K_kH_k)C_{k | k-1}(I - K_kH_k)^\intercal + KVK^\intercal$$

Proof that this formulation remains symmetric is trivial; indeed, $ABA^\intercal$ is symmetric if $ABA^\intercal = (ABA^\intercal)^\intercal$. Expansion gives $(ABA^\intercal)^\intercal = (A^\intercal)^\intercal B^\intercal A^\intercal = AB^\intercal A^\intercal$; since $C$ is symmetric by definition this yields $ABA^\intercal$. $\blacksquare$

Obviously, the Joseph form requires more computation. Both forms require the computation of $I - K_kH_k$; the traditional update step then requires one $6\times 6$ matrix multiplication while the Joseph form requires two $6\times 6$ matrix multiplications, two $6 \times 2$ matrix multiplications, and a $6 \times 6$ matrix addition. However, the increased numerical stability might well be worth it.

This option should be implemented as a configurable option.

Pinging @yasami-24 as this might be interesting to them.

[beomki-yeo]

If I understand correctly, the second equation is multiplying $(1-K_kH_k)^T$ to the first equation. Is that correct? Two equations will have different result for $C_k$

[beomki-yeo]

Maybe you meant this formula:

https://en.wikipedia.org/wiki/Kalman_filter

In any case, I am down for it as long as it actually improves the precision

[stephenswat]

Yes indeed, I stupidly dropped the $KRK^\intercal$ part (where we use V for $R$). I'll correct the post.