more periodic treatment

acts-project · Oct 2, 2024 · a96d7d9 · a96d7d9
1 parent fa8ac48
commit a96d7d9
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/Core/src/TrackFitting/GainMatrixSmoother.cpp b/Core/src/TrackFitting/GainMatrixSmoother.cpp
@@ -57,8 +57,14 @@ 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]);