From 50776d58b2d6983b399a80d5cf10d0a8db124b2e Mon Sep 17 00:00:00 2001 From: Stephen Nicholas Swatman Date: Tue, 1 Jul 2025 10:25:21 +0200 Subject: [PATCH] =?UTF-8?q?Switch=20K=C3=A1lm=C3=A1n=20filters=20to=20use?= =?UTF-8?q?=20Joseph=20form?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- .../fitting/kalman_filter/gain_matrix_updater.hpp | 13 +++++++++++-- .../kalman_filter/two_filters_smoother.hpp | 15 ++++++++++++--- 2 files changed, 23 insertions(+), 5 deletions(-) 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(