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/AnyTrackStateProxy.hpp"
0012 #include "Acts/EventData/TrackParameterHelpers.hpp"
0013 #include "Acts/TrackFitting/GainMatrixUpdater.hpp"
0014 #include "Acts/TrackFitting/KalmanFitterError.hpp"
0015 #include "Acts/Utilities/Logger.hpp"
0016
0017 #include <cstddef>
0018 #include <tuple>
0019
0020 namespace Acts {
0021
0022 template <std::size_t N>
0023 std::tuple<double, std::error_code> GainMatrixUpdater::visitMeasurementImpl(
0024     AnyMutableTrackStateProxy trackState, const Logger& logger) const {
0025   constexpr std::size_t kMeasurementSize = N;
0026   using ProjectedVector = ActsVector<kMeasurementSize>;
0027   using ProjectedMatrix = ActsSquareMatrix<kMeasurementSize>;
0028
0029   const auto calibrated = trackState.calibrated<kMeasurementSize>();
0030   const auto calibratedCovariance =
0031       trackState.calibratedCovariance<kMeasurementSize>();
0032
0033   ACTS_VERBOSE("Measurement dimension: " << kMeasurementSize);
0034   ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose());
0035   ACTS_VERBOSE("Calibrated measurement covariance:\n" << calibratedCovariance);
0036
0037   const auto validSubspaceIndices =
0038       trackState.template projectorSubspaceIndices<kMeasurementSize>();
0039
0040   const 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   auto filtered = trackState.filtered();
0049   auto filteredCovariance = trackState.filteredCovariance();
0050   const auto predicted = trackState.predicted();
0051   const auto predictedCovariance = trackState.predictedCovariance();
0052
0053   const auto K =
0054       (predictedCovariance * H.transpose() *
0055        (H * predictedCovariance * H.transpose() + calibratedCovariance)
0056            .inverse())
0057           .eval();
0058
0059   ACTS_VERBOSE("Gain Matrix K:\n" << K);
0060
0061   if (K.hasNaN()) {
0062     // set to error abort execution
0063     return {0, KalmanFitterError::UpdateFailed};
0064   }
0065
0066   filtered = predicted + K * (calibrated - H * predicted);
0067   // Normalize phi and theta
0068   filtered = normalizeBoundParameters(filtered);
0069   filteredCovariance =
0070       (BoundSquareMatrix::Identity() - K * H) * predictedCovariance;
0071   ACTS_VERBOSE("Filtered parameters: " << filtered.transpose());
0072   ACTS_VERBOSE("Filtered covariance:\n" << filteredCovariance);
0073
0074   const ProjectedVector residual = calibrated - H * filtered;
0075   ACTS_VERBOSE("Residual: " << residual.transpose());
0076
0077   const ProjectedMatrix m =
0078       ((ProjectedMatrix::Identity() - H * K) * calibratedCovariance);
0079   const double chi2 = (residual.transpose() * m.inverse() * residual).value();
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>(         \
0090       AnyMutableTrackStateProxy trackState, 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