fix: Guarantee angle periodicity after KF filtering and smoothing (ac…

…ts-project#3684)

I noticed a few cases where the angles escape their valid range after a KF update / smoothing. We can avoid that by normalizing the angles back to their range after update.

In principle other params like local position and momentum are also constrained and might need checking+fixing but for those I am not sure how to do it right now.

Core/include/Acts/EventData/TrackParameterHelpers.hpp

@@ -0,0 +1,44 @@
+// This file is part of the ACTS project.
+//
+// Copyright (C) 2016 CERN for the benefit of the ACTS project
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#pragma once
+
+#include "Acts/Definitions/Algebra.hpp"
+#include "Acts/Definitions/TrackParametrization.hpp"
+#include "Acts/Utilities/detail/periodic.hpp"
+
+namespace Acts {
+
+/// Normalize the bound parameter angles
+///
+/// @param boundParams The bound parameters to normalize
+///
+/// @return The normalized bound parameters
+inline BoundVector normalizeBoundParameters(const BoundVector& boundParams) {
+  BoundVector result = boundParams;
+  std::tie(result[eBoundPhi], result[eBoundTheta]) =
+      detail::normalizePhiTheta(result[eBoundPhi], result[eBoundTheta]);
+  return result;
+}
+
+/// Subtract bound parameters and take care of angle periodicity for phi and
+/// theta.
+///
+/// @param lhs The left hand side bound parameters
+/// @param rhs The right hand side bound parameters
+///
+/// @return The difference of the bound parameters
+inline BoundVector subtractBoundParameters(const BoundVector& lhs,
+                                           const BoundVector& rhs) {
+  BoundVector result = lhs - rhs;
+  result[eBoundPhi] =
+      detail::difference_periodic(lhs[eBoundPhi], rhs[eBoundPhi], 2 * M_PI);
+  return result;
+}
+
+}  // namespace Acts

Core/include/Acts/TrackFitting/detail/GainMatrixUpdaterImpl.hpp

@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include "Acts/EventData/TrackParameterHelpers.hpp"
 #include "Acts/TrackFitting/GainMatrixUpdater.hpp"
 #include "Acts/Utilities/Logger.hpp"
 
@@ -60,6 +61,8 @@ std::tuple<double, std::error_code> GainMatrixUpdater::visitMeasurementImpl(
 
   trackState.filtered =
       trackState.predicted + K * (calibrated - H * trackState.predicted);
+  // Normalize phi and theta
+  trackState.filtered = normalizeBoundParameters(trackState.filtered);
   trackState.filteredCovariance =
       (BoundSquareMatrix::Identity() - K * H) * trackState.predictedCovariance;
   ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose());

Core/src/TrackFitting/GainMatrixSmoother.cpp

@@ -9,6 +9,7 @@
 #include "Acts/TrackFitting/GainMatrixSmoother.hpp"
 
 #include "Acts/Definitions/TrackParametrization.hpp"
+#include "Acts/EventData/TrackParameterHelpers.hpp"
 #include "Acts/EventData/detail/CovarianceHelper.hpp"
 #include "Acts/TrackFitting/KalmanFitterError.hpp"
 
@@ -57,7 +58,10 @@ Result<void> GainMatrixSmoother::calculate(
       "Prev. predicted parameters: " << predicted(prev_ts).transpose());
 
   // Calculate the smoothed parameters
-  smoothed(ts) = filtered(ts) + G * (smoothed(prev_ts) - predicted(prev_ts));
+  smoothed(ts) = filtered(ts) + G * subtractBoundParameters(smoothed(prev_ts),
+                                                            predicted(prev_ts));
+  // Normalize phi and theta
+  smoothed(ts) = normalizeBoundParameters(smoothed(ts));
 
   ACTS_VERBOSE("Smoothed parameters are: " << smoothed(ts).transpose());
   ACTS_VERBOSE("Calculate smoothed covariance:");

Core/src/TrackFitting/MbfSmoother.cpp

@@ -8,6 +8,8 @@
 
 #include "Acts/TrackFitting/MbfSmoother.hpp"
 
+#include "Acts/EventData/TrackParameterHelpers.hpp"
+
 namespace Acts {
 
 void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
@@ -17,6 +19,8 @@ void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
                                                       bigLambdaHat *
                                                       ts.filteredCovariance;
   ts.smoothed = ts.filtered - ts.filteredCovariance * smallLambdaHat;
+  // Normalize phi and theta
+  ts.smoothed = normalizeBoundParameters(ts.smoothed);
 }
 
 void MbfSmoother::visitNonMeasurement(const InternalTrackState& ts,

Examples/Python/tests/root_file_hashes.txt

@@ -20,7 +20,7 @@ test_propagation__propagation_summary.root: 280c1a6fcfe71974ac39587b4afad27a3164
 test_material_recording__geant4_material_tracks.root: c022b9362249b29f57a07926b20644e3ab4ab8ebcf03f773fbf46c446fc1a0a1
 test_truth_tracking_gsf[generic]__trackstates_gsf.root: 4df2c69d5dd7d5446a547651e4e962daf17924f5c8617165a93a3223c8ba18fd
 test_truth_tracking_gsf[generic]__tracksummary_gsf.root: 8c01d139cb865afa1959c62dbca76f3a1fb8b684c57ea4c2968baa6ffedadb6f
-test_truth_tracking_gsf[odd]__trackstates_gsf.root: 8e6559aaec4fada8b82cfcad5801f3a609c6c905c3172fc044473cef7de77870
+test_truth_tracking_gsf[odd]__trackstates_gsf.root: c7397e53ea093f2432943ae263fc99bc9aa774504ea6152c6907066a06d21caf
 test_truth_tracking_gsf[odd]__tracksummary_gsf.root: 4562341f12a61ea0d5e25872b6bf466b79a73781dc95fc18ef9c6515f0a47916
 test_particle_gun__particles.root: 5fe7dda2933ee6b9615b064d192322fe07831133cd998e5ed99a3b992b713a10
 test_material_mapping__material-map_tracks.root: 938b1a855369e9304401cb10d2751df3fd7acf32a2573f2057eb1691cd94edf3
@@ -33,21 +33,21 @@ test_digitization_example_input[smeared]__particles.root: 5fe7dda2933ee6b9615b06
 test_digitization_example_input[smeared]__measurements.root: 243c2f69b7b0db9dbeaa7494d4ea0f3dd1691dc90f16e10df6c0491ff4dc7d62
 test_digitization_example_input[geometric]__particles.root: 5fe7dda2933ee6b9615b064d192322fe07831133cd998e5ed99a3b992b713a10
 test_digitization_example_input[geometric]__measurements.root: 63ec81635979058fb8976f94455bf490cf92b7b142c4a05cc39de6225f5de2fb
-test_ckf_tracks_example[generic-full_seeding]__trackstates_ckf.root: 35249a79237804bce337d797986e1082c6987f0700e30877f6f217f9ac91d36a
+test_ckf_tracks_example[generic-full_seeding]__trackstates_ckf.root: 7c48ec32a2cb1723416a9791a8067ef09825fcf71a6cf561c1f6d2ab9dc1c1ad
 test_ckf_tracks_example[generic-full_seeding]__tracksummary_ckf.root: e6b9e539998ba007e9b7d2c8d9d022c47726a39e8ab9b1724c52b1d78234be03
 test_ckf_tracks_example[generic-full_seeding]__performance_seeding_trees.root: 0e0676ffafdb27112fbda50d1cf627859fa745760f98073261dcf6db3f2f991e
-test_ckf_tracks_example[generic-truth_estimated]__trackstates_ckf.root: a8c5c6f6c1e6303b887d47b509b7f71a2ffa5f38638fe46ce5bce76fd20d64ca
+test_ckf_tracks_example[generic-truth_estimated]__trackstates_ckf.root: df730fd00a7e6a0941f5f94c07ea9cffdb763853272d284d25bec0eb2072bb2e
 test_ckf_tracks_example[generic-truth_estimated]__tracksummary_ckf.root: 417f7326e1e1bb4519f1378145ac733bdda6653eb9871fd69e455e0269d996a6
 test_ckf_tracks_example[generic-truth_estimated]__performance_seeding.root: 1facb05c066221f6361b61f015cdf0918e94d9f3fce2269ec7b6a4dffeb2bc7e
-test_ckf_tracks_example[generic-truth_smeared]__trackstates_ckf.root: edf0b06ce9ee0e4fcb153e41859af7b5153271de18f49a6842a23ad2d66b7e09
+test_ckf_tracks_example[generic-truth_smeared]__trackstates_ckf.root: 82a6744980553e6274df78eea15f0dec22676b1c04e14afc3828bff9bbf5e1b1
 test_ckf_tracks_example[generic-truth_smeared]__tracksummary_ckf.root: 06d6ae1d05cb611b19df3c59531997c9b0108f5ef6027d76c4827bd2d9edb921
-test_ckf_tracks_example[odd-full_seeding]__trackstates_ckf.root: 6411378b31c1612120318773f8b807ce83a76e07c90a5f308ea86c6b34d02661
+test_ckf_tracks_example[odd-full_seeding]__trackstates_ckf.root: 0fb43661cc3a7973c28940a283dc168ceb13bc60badf1f520096edaa5982a039
 test_ckf_tracks_example[odd-full_seeding]__tracksummary_ckf.root: c2e029e462d4ca77df2c7f8963093da43be66c8279ca2cc9aee8c0bc35259eec
 test_ckf_tracks_example[odd-full_seeding]__performance_seeding_trees.root: 43c58577aafe07645e5660c4f43904efadf91d8cda45c5c04c248bbe0f59814f
-test_ckf_tracks_example[odd-truth_estimated]__trackstates_ckf.root: 465bb9e982982eb2e79fc97cae9f513ff5937041da546081281f1f959d8173ea
+test_ckf_tracks_example[odd-truth_estimated]__trackstates_ckf.root: 39ac67c47f371c576d7094bca987a04e0315bd286dc79503a63a5f568b58ac97
 test_ckf_tracks_example[odd-truth_estimated]__tracksummary_ckf.root: 59e2c75e9524653a80a9fd62fe99e958f73f80aa09240dcbb4ea469372e4811d
 test_ckf_tracks_example[odd-truth_estimated]__performance_seeding.root: 1a36b7017e59f1c08602ef3c2cb0483c51df248f112e3780c66594110719c575
-test_ckf_tracks_example[odd-truth_smeared]__trackstates_ckf.root: 9cbf99353d71ee4e6779bffb231043b41db6219c9544a80fe5172f4a4fe60cbe
+test_ckf_tracks_example[odd-truth_smeared]__trackstates_ckf.root: 35a65e15a6f479f628a96f56ee78e1ac371d71a686ee0c974944d681499fe6bd
 test_ckf_tracks_example[odd-truth_smeared]__tracksummary_ckf.root: 3e257de624674fa9a19dcc72598c78c29a52633821acaa56dc2aa39a1395f1b5
 test_vertex_fitting_reading[Truth-False-100]__performance_vertexing.root: 76ef6084d758dfdfc0151ddec2170e12d73394424e3dac4ffe46f0f339ec8293
 test_vertex_fitting_reading[Iterative-False-100]__performance_vertexing.root: 60372210c830a04f95ceb78c6c68a9b0de217746ff59e8e73053750c837b57eb