Switch Kálmán filters to use Joseph form

This commit switches the Kálmán filters to use the Joseph form
covariance update which is known to be usable for any gain matrix $K$
even if the gain matrix is distorted by numerical imprecision.
Empirically, this seems to resolve quite a few issues with our filtering
algorithms.

Author: stephenswat

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

@@ -121,7 +121,10 @@ struct gain_matrix_updater {
         // Calculate the filtered track parameters
         const matrix_type<6, 1> filtered_vec =
             predicted_vec + K * (meas_local - H * predicted_vec);
-        const matrix_type<6, 6> filtered_cov = (I66 - K * H) * predicted_cov;
+        const matrix_type<6, 6> i_minus_kh = I66 - K * H;
+        const matrix_type<6, 6> filtered_cov =
+            i_minus_kh * predicted_cov * matrix::transpose(i_minus_kh) +
+            K * V * matrix::transpose(K);
 
         // Return false if track is parallel to z-axis or phi is not finite
         if (!std::isfinite(getter::element(filtered_vec, e_bound_theta, 0))) {
@@ -141,7 +144,13 @@ struct gain_matrix_updater {
         const matrix_type<D, 1> residual = meas_local - H * filtered_vec;
 
         // Calculate the chi square
-        const matrix_type<D, D> R = (I_m - H * K) * V;
+        const matrix_type<D, D> i_minus_hk = I_m - H * K;
+        // See
+        // https://indico.cern.ch/event/1564924/contributions/6629447/attachments/3113201/5519076/asami_250731_acts.pdf
+        const matrix_type<D, D> R =
+            i_minus_hk * V * matrix::transpose(i_minus_hk) +
+            H * i_minus_kh * predicted_cov * matrix::transpose(i_minus_kh) *
+                matrix::transpose(H);
         const matrix_type<1, 1> chi2 =
             algebra::matrix::transposed_product<true, false>(
                 residual, matrix::inverse(R)) *

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

@@ -197,7 +197,10 @@ struct two_filters_smoother {
         // Calculate the filtered track parameters
         const matrix_type<6, 1> filtered_vec =
             predicted_vec + K * (meas_local - H * predicted_vec);
-        const matrix_type<6, 6> filtered_cov = (I66 - K * H) * predicted_cov;
+        const matrix_type<6, 6> i_minus_kh = I66 - K * H;
+        const matrix_type<6, 6> filtered_cov =
+            i_minus_kh * predicted_cov * matrix::transpose(i_minus_kh) +
+            K * V * matrix::transpose(K);
 
         // Update the bound track parameters
         bound_params.set_vector(filtered_vec);
@@ -221,8 +224,14 @@ struct two_filters_smoother {
         const matrix_type<D, 1> residual = meas_local - H * filtered_vec;
 
         // Calculate backward chi2
-        const matrix_type<D, D> R = (I_m - H * K) * V;
-        // assert(matrix::determinant(R) != 0.f); // @TODO: This fails
+        const matrix_type<D, D> i_minus_hk = I_m - H * K;
+        // See
+        // https://indico.cern.ch/event/1564924/contributions/6629447/attachments/3113201/5519076/asami_250731_acts.pdf
+        const matrix_type<D, D> R =
+            i_minus_hk * V * matrix::transpose(i_minus_hk) +
+            H * i_minus_kh * predicted_cov * matrix::transpose(i_minus_kh) *
+                matrix::transpose(H);
+        // assert(matrix::determinant(R) != 0.f);
         assert(std::isfinite(matrix::determinant(R)));
         const matrix_type<1, 1> chi2 =
             algebra::matrix::transposed_product<true, false>(