feat: Helpers to calculate residuals, chi2 and unbiased states (#3877)

This came up a couple of times and it really should be in core so I added helpers to
- calculate residuals and their covariance for predicted, filtered and smoothed states
- calculate chi2 for these
- calculate unbiased parameters and covariance

For now I added them to `TrackHelpers.hpp` but potentially this could also live in the `TrackStateProxy`.

<!-- This is an auto-generated comment: release notes by coderabbit.ai -->

## Summary by CodeRabbit

- **New Features**
	- Introduced new helper functions for track state calculations, including methods for computing predicted, filtered, and smoothed residuals and chi-squared values.
	- Added functionality to calculate unbiased track parameters and their covariance.

- **Bug Fixes**
	- Simplified the logic for calculating unbiased track parameters, improving maintainability and reducing complexity.

- **Documentation**
	- Updated method signatures and descriptions to reflect changes in functionality.

<!-- end of auto-generated comment: release notes by coderabbit.ai -->

Core/include/Acts/Utilities/TrackHelpers.hpp

@@ -8,11 +8,15 @@
 
 #pragma once
 
+#include "Acts/Definitions/Algebra.hpp"
 #include "Acts/Definitions/Tolerance.hpp"
+#include "Acts/Definitions/TrackParametrization.hpp"
+#include "Acts/EventData/MeasurementHelpers.hpp"
 #include "Acts/EventData/MultiTrajectoryHelpers.hpp"
 #include "Acts/EventData/TrackContainerFrontendConcept.hpp"
 #include "Acts/EventData/TrackParameters.hpp"
 #include "Acts/EventData/TrackProxyConcept.hpp"
+#include "Acts/EventData/TrackStateProxyConcept.hpp"
 #include "Acts/EventData/TrackStateType.hpp"
 #include "Acts/Geometry/GeometryContext.hpp"
 #include "Acts/Propagator/StandardAborters.hpp"
@@ -493,6 +497,216 @@ void trimTrack(track_proxy_t track, bool trimHoles, bool trimOutliers,
   trimTrackBack(track, trimHoles, trimOutliers, trimMaterial);
 }
 
+/// Helper function to calculate the predicted residual and its covariance
+/// @tparam nMeasurementDim the dimension of the measurement
+/// @tparam track_state_proxy_t the track state proxy type
+/// @param trackState the track state to calculate the residual from
+/// @return a pair of the residual and its covariance
+template <std::size_t nMeasurementDim,
+          TrackStateProxyConcept track_state_proxy_t>
+std::pair<ActsVector<nMeasurementDim>, ActsSquareMatrix<nMeasurementDim>>
+calculatePredictedResidual(track_state_proxy_t trackState) {
+  using MeasurementVector = ActsVector<nMeasurementDim>;
+  using MeasurementMatrix = ActsSquareMatrix<nMeasurementDim>;
+
+  if (!trackState.hasPredicted()) {
+    throw std::invalid_argument("track state has no predicted parameters");
+  }
+  if (!trackState.hasCalibrated()) {
+    throw std::invalid_argument("track state has no calibrated parameters");
+  }
+
+  auto subspaceHelper =
+      trackState.template fixedBoundSubspaceHelper<nMeasurementDim>();
+
+  auto measurement = trackState.calibrated();
+  auto measurementCovariance = trackState.calibratedCovariance();
+  MeasurementVector predicted =
+      subspaceHelper.projectVector(trackState.predicted());
+  MeasurementMatrix predictedCovariance =
+      subspaceHelper.projectMatrix(trackState.predictedCovariance());
+
+  MeasurementVector residual = measurement - predicted;
+  MeasurementMatrix residualCovariance =
+      measurementCovariance + predictedCovariance;
+
+  return std::pair(residual, residualCovariance);
+}
+
+/// Helper function to calculate the filtered residual and its covariance
+/// @tparam nMeasurementDim the dimension of the measurement
+/// @tparam track_state_proxy_t the track state proxy type
+/// @param trackState the track state to calculate the residual from
+/// @return a pair of the residual and its covariance
+template <std::size_t nMeasurementDim,
+          TrackStateProxyConcept track_state_proxy_t>
+std::pair<ActsVector<nMeasurementDim>, ActsSquareMatrix<nMeasurementDim>>
+calculateFilteredResidual(track_state_proxy_t trackState) {
+  using MeasurementVector = ActsVector<nMeasurementDim>;
+  using MeasurementMatrix = ActsSquareMatrix<nMeasurementDim>;
+
+  if (!trackState.hasFiltered()) {
+    throw std::invalid_argument("track state has no filtered parameters");
+  }
+  if (!trackState.hasCalibrated()) {
+    throw std::invalid_argument("track state has no calibrated parameters");
+  }
+
+  auto subspaceHelper =
+      trackState.template fixedBoundSubspaceHelper<nMeasurementDim>();
+
+  auto measurement = trackState.calibrated();
+  auto measurementCovariance = trackState.calibratedCovariance();
+  MeasurementVector filtered =
+      subspaceHelper.projectVector(trackState.filtered());
+  MeasurementMatrix filteredCovariance =
+      subspaceHelper.projectMatrix(trackState.filteredCovariance());
+
+  MeasurementVector residual = measurement - filtered;
+  MeasurementMatrix residualCovariance =
+      measurementCovariance + filteredCovariance;
+
+  return std::pair(residual, residualCovariance);
+}
+
+/// Helper function to calculate the smoothed residual and its covariance
+/// @tparam nMeasurementDim the dimension of the measurement
+/// @tparam track_state_proxy_t the track state proxy type
+/// @param trackState the track state to calculate the residual from
+/// @return a pair of the residual and its covariance
+template <std::size_t nMeasurementDim,
+          TrackStateProxyConcept track_state_proxy_t>
+std::pair<ActsVector<nMeasurementDim>, ActsSquareMatrix<nMeasurementDim>>
+calculateSmoothedResidual(track_state_proxy_t trackState) {
+  using MeasurementVector = ActsVector<nMeasurementDim>;
+  using MeasurementMatrix = ActsSquareMatrix<nMeasurementDim>;
+
+  if (!trackState.hasSmoothed()) {
+    throw std::invalid_argument("track state has no smoothed parameters");
+  }
+  if (!trackState.hasCalibrated()) {
+    throw std::invalid_argument("track state has no calibrated parameters");
+  }
+
+  auto subspaceHelper =
+      trackState.template fixedBoundSubspaceHelper<nMeasurementDim>();
+
+  auto measurement = trackState.calibrated();
+  auto measurementCovariance = trackState.calibratedCovariance();
+  MeasurementVector smoothed =
+      subspaceHelper.projectVector(trackState.smoothed());
+  MeasurementMatrix smoothedCovariance =
+      subspaceHelper.projectMatrix(trackState.smoothedCovariance());
+
+  MeasurementVector residual = measurement - smoothed;
+  MeasurementMatrix residualCovariance =
+      measurementCovariance + smoothedCovariance;
+
+  return std::pair(residual, residualCovariance);
+}
+
+/// Helper function to calculate the predicted chi2
+/// @tparam track_state_proxy_t the track state proxy type
+/// @param trackState the track state to calculate the chi2 from
+/// @return the chi2
+template <TrackStateProxyConcept track_state_proxy_t>
+double calculatePredictedChi2(track_state_proxy_t trackState) {
+  if (!trackState.hasPredicted()) {
+    throw std::invalid_argument("track state has no predicted parameters");
+  }
+  if (!trackState.hasCalibrated()) {
+    throw std::invalid_argument("track state has no calibrated parameters");
+  }
+
+  return visit_measurement(
+      trackState.calibratedSize(),
+      [&]<std::size_t measdim>(std::integral_constant<std::size_t, measdim>) {
+        auto [residual, residualCovariance] =
+            calculatePredictedResidual<measdim>(trackState);
+
+        return residual.transpose() * residualCovariance.inverse() * residual;
+      });
+}
+
+/// Helper function to calculate the filtered chi2
+/// @tparam track_state_proxy_t the track state proxy type
+/// @param trackState the track state to calculate the chi2 from
+/// @return the chi2
+template <TrackStateProxyConcept track_state_proxy_t>
+double calculateFilteredChi2(track_state_proxy_t trackState) {
+  if (!trackState.hasFiltered()) {
+    throw std::invalid_argument("track state has no filtered parameters");
+  }
+  if (!trackState.hasCalibrated()) {
+    throw std::invalid_argument("track state has no calibrated parameters");
+  }
+
+  return visit_measurement(
+      trackState.calibratedSize(),
+      [&]<std::size_t measdim>(std::integral_constant<std::size_t, measdim>) {
+        auto [residual, residualCovariance] =
+            calculateFilteredResidual<measdim>(trackState);
+
+        return residual.transpose() * residualCovariance.inverse() * residual;
+      });
+}
+
+/// Helper function to calculate the smoothed chi2
+/// @tparam track_state_proxy_t the track state proxy type
+/// @param trackState the track state to calculate the chi2 from
+/// @return the chi2
+template <TrackStateProxyConcept track_state_proxy_t>
+double calculateSmoothedChi2(track_state_proxy_t trackState) {
+  if (!trackState.hasSmoothed()) {
+    throw std::invalid_argument("track state has no smoothed parameters");
+  }
+  if (!trackState.hasCalibrated()) {
+    throw std::invalid_argument("track state has no calibrated parameters");
+  }
+
+  return visit_measurement(
+      trackState.calibratedSize(),
+      [&]<std::size_t measdim>(std::integral_constant<std::size_t, measdim>) {
+        auto [residual, residualCovariance] =
+            calculateSmoothedResidual<measdim>(trackState);
+
+        return residual.transpose() * residualCovariance.inverse() * residual;
+      });
+}
+
+/// Helper function to calculate the unbiased track parameters and their
+/// covariance (i.e. fitted track parameters with this measurement removed)
+/// using Eq.(12a)-Eq.(12c) of NIMA 262, 444 (1987)
+/// @tparam track_state_proxy_t the track state proxy type
+/// @param trackState the track state to calculate the unbiased parameters from
+/// @return a pair of the unbiased parameters and their covariance
+template <TrackStateProxyConcept track_state_proxy_t>
+std::pair<BoundVector, BoundMatrix> calculateUnbiasedParametersCovariance(
+    track_state_proxy_t trackState) {
+  if (!trackState.hasSmoothed()) {
+    throw std::invalid_argument("track state has no smoothed parameters");
+  }
+  if (!trackState.hasCalibrated()) {
+    throw std::invalid_argument("track state has no calibrated parameters");
+  }
+
+  return visit_measurement(
+      trackState.calibratedSize(),
+      [&]<std::size_t measdim>(std::integral_constant<std::size_t, measdim>) {
+        auto H = trackState.projector()
+                     .template topLeftCorner<measdim, eBoundSize>();
+        auto s = trackState.smoothed();
+        auto C = trackState.smoothedCovariance();
+        auto m = trackState.template calibrated<measdim>();
+        auto V = trackState.template calibratedCovariance<measdim>();
+        auto K =
+            (C * H.transpose() * (H * C * H.transpose() - V).inverse()).eval();
+        BoundVector unbiasedParamsVec = s + K * (m - H * s);
+        BoundMatrix unbiasedParamsCov = C - K * H * C;
+        return std::make_pair(unbiasedParamsVec, unbiasedParamsCov);
+      });
+}
+
 }  // namespace Acts
 
 namespace std {

Examples/Io/Root/src/RootTrackStatesWriter.cpp

@@ -12,28 +12,22 @@
 #include "Acts/Definitions/Common.hpp"
 #include "Acts/Definitions/TrackParametrization.hpp"
 #include "Acts/EventData/MultiTrajectory.hpp"
-#include "Acts/EventData/MultiTrajectoryHelpers.hpp"
-#include "Acts/EventData/TrackParameters.hpp"
 #include "Acts/EventData/TransformationHelpers.hpp"
 #include "Acts/EventData/VectorMultiTrajectory.hpp"
-#include "Acts/Geometry/GeometryContext.hpp"
 #include "Acts/Utilities/Helpers.hpp"
 #include "Acts/Utilities/MultiIndex.hpp"
+#include "Acts/Utilities/TrackHelpers.hpp"
 #include "Acts/Utilities/detail/periodic.hpp"
 #include "ActsExamples/EventData/AverageSimHits.hpp"
 #include "ActsExamples/EventData/IndexSourceLink.hpp"
 #include "ActsExamples/EventData/Track.hpp"
 #include "ActsExamples/Framework/AlgorithmContext.hpp"
 #include "ActsExamples/Utilities/Range.hpp"
-#include "ActsExamples/Validation/TrackClassification.hpp"
 #include "ActsFatras/EventData/Barcode.hpp"
-#include "ActsFatras/EventData/Particle.hpp"
 
 #include <cmath>
-#include <cstddef>
 #include <ios>
 #include <limits>
-#include <memory>
 #include <numbers>
 #include <optional>
 #include <ostream>
@@ -487,21 +481,7 @@ ProcessCode RootTrackStatesWriter::writeT(const AlgorithmContext& ctx,
         }
         if (ipar == eUnbiased && state.hasSmoothed() && state.hasProjector() &&
             state.hasCalibrated()) {
-          // calculate the unbiased track parameters (i.e. fitted track
-          // parameters with this measurement removed) using Eq.(12a)-Eq.(12c)
-          // of NIMA 262, 444 (1987)
-          auto m = state.effectiveCalibrated();
-          auto H = state.effectiveProjector();
-          auto V = state.effectiveCalibratedCovariance();
-          auto K =
-              (state.smoothedCovariance() * H.transpose() *
-               (H * state.smoothedCovariance() * H.transpose() - V).inverse())
-                  .eval();
-          auto unbiasedParamsVec =
-              state.smoothed() + K * (m - H * state.smoothed());
-          auto unbiasedParamsCov =
-              state.smoothedCovariance() - K * H * state.smoothedCovariance();
-          return std::make_pair(unbiasedParamsVec, unbiasedParamsCov);
+          return Acts::calculateUnbiasedParametersCovariance(state);
         }
         return std::nullopt;
       };