src/TrackFitting/GainMatrixUpdater.cpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2021 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 #include "Acts/TrackFitting/GainMatrixUpdater.hpp"
0010
0011 #include "Acts/Definitions/Algebra.hpp"
0012 #include "Acts/Definitions/TrackParametrization.hpp"
0013 #include "Acts/EventData/MeasurementHelpers.hpp"
0014 #include "Acts/TrackFitting/KalmanFitterError.hpp"
0015
0016 #include <algorithm>
0017 #include <cstddef>
0018 #include <utility>
0019
0020 #include <Eigen/src/Core/MatrixBase.h>
0021
0022 namespace Acts {
0023
0024 std::tuple<double, std::error_code> GainMatrixUpdater::visitMeasurement(
0025     InternalTrackState trackState, Direction direction,
0026     const Logger& logger) const {
0027   // default-constructed error represents success, i.e. an invalid error code
0028   std::error_code error;
0029   double chi2 = 0;
0030
0031   visit_measurement(trackState.calibratedSize, [&](auto N) -> bool {
0032     constexpr std::size_t kMeasurementSize = decltype(N)::value;
0033     using ParametersVector = ActsVector<kMeasurementSize>;
0034     using CovarianceMatrix = ActsSquareMatrix<kMeasurementSize>;
0035
0036     typename TrackStateTraits<kMeasurementSize, true>::Measurement calibrated{
0037         trackState.calibrated};
0038     typename TrackStateTraits<kMeasurementSize, true>::MeasurementCovariance
0039         calibratedCovariance{trackState.calibratedCovariance};
0040
0041     ACTS_VERBOSE("Measurement dimension: " << kMeasurementSize);
0042     ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose());
0043     ACTS_VERBOSE("Calibrated measurement covariance:\n"
0044                  << calibratedCovariance);
0045
0046     const auto H = trackState.projector
0047                        .template topLeftCorner<kMeasurementSize, eBoundSize>()
0048                        .eval();
0049
0050     ACTS_VERBOSE("Measurement projector H:\n" << H);
0051
0052     const auto K = (trackState.predictedCovariance * H.transpose() *
0053                     (H * trackState.predictedCovariance * H.transpose() +
0054                      calibratedCovariance)
0055                         .inverse())
0056                        .eval();
0057
0058     ACTS_VERBOSE("Gain Matrix K:\n" << K);
0059
0060     if (K.hasNaN()) {
0061       error = (direction == Direction::Forward)
0062                   ? KalmanFitterError::ForwardUpdateFailed
0063                   : KalmanFitterError::BackwardUpdateFailed;  // set to error
0064       return false;                                           // abort execution
0065     }
0066
0067     trackState.filtered =
0068         trackState.predicted + K * (calibrated - H * trackState.predicted);
0069     trackState.filteredCovariance = (BoundSquareMatrix::Identity() - K * H) *
0070                                     trackState.predictedCovariance;
0071     ACTS_VERBOSE("Filtered parameters: " << trackState.filtered.transpose());
0072     ACTS_VERBOSE("Filtered covariance:\n" << trackState.filteredCovariance);
0073
0074     // calculate filtered residual
0075     //
0076     // FIXME: Without separate residual construction and assignment, we
0077     //        currently take a +0.7GB build memory consumption hit in the
0078     //        EventDataView unit tests. Revisit this once Measurement
0079     //        overhead problems (Acts issue #350) are sorted out.
0080     //
0081     ParametersVector residual;
0082     residual = calibrated - H * trackState.filtered;
0083     ACTS_VERBOSE("Residual: " << residual.transpose());
0084
0085     CovarianceMatrix m =
0086         ((CovarianceMatrix::Identity() - H * K) * calibratedCovariance).eval();
0087
0088     chi2 = (residual.transpose() * m.inverse() * residual).value();
0089
0090     ACTS_VERBOSE("Chi2: " << chi2);
0091     return true;  // continue execution
0092   });
0093
0094   return {chi2, error};
0095 }
0096
0097 }  // namespace Acts