TrackFitting/detail/GainMatrixUpdaterImpl.hpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2024 CERN for the benefit of the Acts project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "Acts/TrackFitting/GainMatrixUpdater.hpp"
0012 #include "Acts/Utilities/Logger.hpp"
0013
0014 #include <cstddef>
0015 #include <tuple>
0016
0017 namespace Acts {
0018
0019 template <std::size_t N>
0020 std::tuple<double, std::error_code> GainMatrixUpdater::visitMeasurementImpl(
0021     InternalTrackState trackState, const Logger& logger) const {
0022   double chi2 = 0;
0023
0024   constexpr std::size_t kMeasurementSize = N;
0025   using ParametersVector = ActsVector<kMeasurementSize>;
0026   using CovarianceMatrix = ActsSquareMatrix<kMeasurementSize>;
0027
0028   typename TrackStateTraits<kMeasurementSize, true>::Calibrated calibrated{
0029       trackState.calibrated};
0030   typename TrackStateTraits<kMeasurementSize, true>::CalibratedCovariance
0031       calibratedCovariance{trackState.calibratedCovariance};
0032
0033   ACTS_VERBOSE("Measurement dimension: " << kMeasurementSize);
0034   ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose());
0035   ACTS_VERBOSE("Calibrated measurement covariance:\n" << calibratedCovariance);
0036
0037   std::span<std::uint8_t, kMeasurementSize> validSubspaceIndices(
0038       trackState.projector.begin(),
0039       trackState.projector.begin() + kMeasurementSize);
0040   FixedBoundSubspaceHelper<kMeasurementSize> subspaceHelper(
0041       validSubspaceIndices);
0042
0043   // TODO use subspace helper for projection instead
0044   const auto H = subspaceHelper.projector();
0045
0046   ACTS_VERBOSE("Measurement projector H:\n" << H);
0047
0048   const auto K = (trackState.predictedCovariance * H.transpose() *
0049                   (H * trackState.predictedCovariance * H.transpose() +
0050                    calibratedCovariance)
0051                       .inverse())
0052                      .eval();
0053
0054   ACTS_VERBOSE("Gain Matrix K:\n" << K);
0055
0056   if (K.hasNaN()) {
0057     // set to error abort execution
0058     return {0, KalmanFitterError::UpdateFailed};
0059   }
0060
0061   trackState.filtered =
0062       trackState.predicted + K * (calibrated - H * trackState.predicted);
0063   trackState.filteredCovariance =
0064       (BoundSquareMatrix::Identity() - K * H) * trackState.predictedCovariance;
0065   ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose());
0066   ACTS_VERBOSE("Filtered covariance:\n" << trackState.filteredCovariance);
0067
0068   ParametersVector residual;
0069   residual = calibrated - H * trackState.filtered;
0070   ACTS_VERBOSE("Residual: " << residual.transpose());
0071
0072   CovarianceMatrix m =
0073       ((CovarianceMatrix::Identity() - H * K) * calibratedCovariance).eval();
0074
0075   chi2 = (residual.transpose() * m.inverse() * residual).value();
0076
0077   ACTS_VERBOSE("Chi2: " << chi2);
0078
0079   return {chi2, {}};
0080 }
0081
0082 // Ensure thet the compiler does not implicitly instantiate the template
0083
0084 #define _EXTERN(N)                                                          \
0085   extern template std::tuple<double, std::error_code>                       \
0086   GainMatrixUpdater::visitMeasurementImpl<N>(InternalTrackState trackState, \
0087                                              const Logger& logger) const
0088
0089 _EXTERN(1);
0090 _EXTERN(2);
0091 _EXTERN(3);
0092 _EXTERN(4);
0093 _EXTERN(5);
0094 _EXTERN(6);
0095
0096 #undef _EXTERN
0097
0098 }  // namespace Acts