fix: Guarantee angle periodicity after KF filtering and smoothing

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

@@ -8,8 +8,10 @@
 
 #pragma once
 
+#include "Acts/Definitions/TrackParametrization.hpp"
 #include "Acts/TrackFitting/GainMatrixUpdater.hpp"
 #include "Acts/Utilities/Logger.hpp"
+#include "Acts/Utilities/detail/periodic.hpp"
 
 #include <cstddef>
 #include <tuple>
@@ -60,6 +62,9 @@ std::tuple<double, std::error_code> GainMatrixUpdater::visitMeasurementImpl(
 
   trackState.filtered =
       trackState.predicted + K * (calibrated - H * trackState.predicted);
+  // Normalize phi and theta
+  detail::normalizePhiThetaInplace(trackState.filtered[eBoundPhi],
+                                   trackState.filtered[eBoundTheta]);
   trackState.filteredCovariance =
       (BoundSquareMatrix::Identity() - K * H) * trackState.predictedCovariance;
   ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose());

Core/include/Acts/Utilities/detail/periodic.hpp

@@ -9,6 +9,7 @@
 #pragma once
 
 #include <cmath>
+#include <tuple>
 
 namespace Acts::detail {
 
@@ -90,4 +91,15 @@ inline std::pair<T, T> normalizePhiTheta(T phi, T theta) {
   return {radian_sym(phi), theta};
 }
 
+/// Ensure both phi and theta direction angles are within the allowed range.
+///
+/// See `normalizePhiTheta` for details.
+///
+/// @param[in,out] phi Transverse direction angle
+/// @param[in,out] theta Longitudinal direction angle
+template <typename T>
+inline void normalizePhiThetaInplace(T& phi, T& theta) {
+  std::tie(phi, theta) = normalizePhiTheta(phi, theta);
+}
+
 }  // namespace Acts::detail

Core/src/TrackFitting/GainMatrixSmoother.cpp

@@ -11,6 +11,7 @@
 #include "Acts/Definitions/TrackParametrization.hpp"
 #include "Acts/EventData/detail/CovarianceHelper.hpp"
 #include "Acts/TrackFitting/KalmanFitterError.hpp"
+#include "Acts/Utilities/detail/periodic.hpp"
 
 #include <algorithm>
 #include <ostream>
@@ -56,8 +57,17 @@ Result<void> GainMatrixSmoother::calculate(
   ACTS_VERBOSE(
       "Prev. predicted parameters: " << predicted(prev_ts).transpose());
 
+  BoundVector smoothedMinusPredicted = smoothed(prev_ts) - predicted(prev_ts);
+  smoothedMinusPredicted[eBoundPhi] = detail::difference_periodic(
+      smoothed(prev_ts)[eBoundPhi], predicted(prev_ts)[eBoundPhi], 2 * M_PI);
+  smoothedMinusPredicted[eBoundTheta] = detail::difference_periodic(
+      smoothed(prev_ts)[eBoundTheta], predicted(prev_ts)[eBoundTheta], M_PI);
+
   // Calculate the smoothed parameters
-  smoothed(ts) = filtered(ts) + G * (smoothed(prev_ts) - predicted(prev_ts));
+  smoothed(ts) = filtered(ts) + G * smoothedMinusPredicted;
+  // Normalize phi and theta
+  detail::normalizePhiThetaInplace(smoothed(ts)[eBoundPhi],
+                                   smoothed(ts)[eBoundTheta]);
 
   ACTS_VERBOSE("Smoothed parameters are: " << smoothed(ts).transpose());
   ACTS_VERBOSE("Calculate smoothed covariance:");

Core/src/TrackFitting/MbfSmoother.cpp

@@ -8,6 +8,9 @@
 
 #include "Acts/TrackFitting/MbfSmoother.hpp"
 
+#include "Acts/Definitions/TrackParametrization.hpp"
+#include "Acts/Utilities/detail/periodic.hpp"
+
 namespace Acts {
 
 void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
@@ -17,6 +20,9 @@ void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
                                                       bigLambdaHat *
                                                       ts.filteredCovariance;
   ts.smoothed = ts.filtered - ts.filteredCovariance * smallLambdaHat;
+  // Normalize phi and theta
+  detail::normalizePhiThetaInplace(ts.smoothed[eBoundPhi],
+                                   ts.smoothed[eBoundTheta]);
 }
 
 void MbfSmoother::visitNonMeasurement(const InternalTrackState& ts,