TrackFitting/detail/GainMatrixUpdaterImpl.hpp

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