diff --git a/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp b/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp index ca9014781f..bc6af308f3 100644 --- a/core/include/traccc/fitting/kalman_filter/gain_matrix_updater.hpp +++ b/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 residual = meas_local - H * filtered_vec; // Calculate the chi square - const matrix_type R = (I_m - H * K) * V; + const matrix_type 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 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( residual, matrix::inverse(R)) * diff --git a/core/include/traccc/fitting/kalman_filter/two_filters_smoother.hpp b/core/include/traccc/fitting/kalman_filter/two_filters_smoother.hpp index b0a45e9250..679bb7becc 100644 --- a/core/include/traccc/fitting/kalman_filter/two_filters_smoother.hpp +++ b/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 residual = meas_local - H * filtered_vec; // Calculate backward chi2 - const matrix_type R = (I_m - H * K) * V; - // assert(matrix::determinant(R) != 0.f); // @TODO: This fails + const matrix_type 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 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(