TrackFitting/detail/KalmanGlobalCovariance.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/Definitions/Algebra.hpp"
0012 #include "Acts/Definitions/TrackParametrization.hpp"
0013 #include "Acts/EventData/BoundTrackParameters.hpp"
0014 #include "Acts/EventData/Types.hpp"
0015
0016 #include <unordered_map>
0017
0018 namespace Acts::detail {
0019
0020 /// Calculate the global track parameters covariance for a smoothed trajectory
0021 /// stored in MultiTrajecty based on formulas at Journal of Physics: Conference
0022 /// Series 219 (2010) 032028.
0023 ///
0024 /// @tparam source_link_t The source link type of the trajectory
0025 /// @tparam parameters_t The track parameters type
0026 ///
0027 /// @param multiTraj The MultiTrajectory containing the trajectory to be
0028 /// investigated
0029 /// @param entryIndex The trajectory entry index
0030 ///
0031 /// @return The global track parameters covariance matrix and the starting
0032 /// row/column for smoothed states
0033 template <typename traj_t, typename parameters_t = BoundTrackParameters>
0034 std::pair<DynamicMatrix, std::unordered_map<std::size_t, std::size_t>>
0035 globalTrackParametersCovariance(const traj_t& multiTraj,
0036                                 const std::size_t& entryIndex) {
0037   using CovMatrix = BoundMatrix;
0038   using GainMatrix = BoundMatrix;
0039
0040   // The last smoothed state index
0041   std::size_t lastSmoothedIndex = kTrackIndexInvalid;
0042   // The total number of smoothed states
0043   std::size_t nSmoothedStates = 0;
0044   // Visit all the states
0045   multiTraj.visitBackwards(entryIndex, [&](const auto& ts) {
0046     if (ts.hasSmoothed()) {
0047       if (lastSmoothedIndex == kTrackIndexInvalid) {
0048         lastSmoothedIndex = ts.index();
0049       }
0050       nSmoothedStates++;
0051     }
0052   });
0053
0054   // Set the size of global track parameters covariance for all smoothed states
0055   DynamicMatrix fullGlobalTrackParamsCov(nSmoothedStates * eBoundSize,
0056                                          nSmoothedStates * eBoundSize);
0057   fullGlobalTrackParamsCov.setZero();
0058   // The index of state within the trajectory and the starting row/column for
0059   // this state in the global covariance matrix
0060   std::unordered_map<std::size_t, std::size_t> stateRowIndices;
0061   // Visit the smoothed states to calculate the full global track parameters
0062   // covariance
0063   std::size_t nProcessed = 0;
0064   auto prev_ts = multiTraj.getTrackState(lastSmoothedIndex);
0065   multiTraj.visitBackwards(lastSmoothedIndex, [&](const auto& ts) {
0066     const std::size_t iRow =
0067         fullGlobalTrackParamsCov.rows() - eBoundSize * (nProcessed + 1);
0068     // Fill the covariance of this state
0069     fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(iRow, iRow) =
0070         ts.smoothedCovariance();
0071     // Fill the correlation between this state (indexed by i-1) and
0072     // beforehand smoothed states (indexed by j): C^n_{i-1, j}= G_{i-1} *
0073     // C^n_{i, j} for i <= j
0074     if (nProcessed > 0) {
0075       // Calculate the gain matrix
0076       GainMatrix G = ts.filteredCovariance() * prev_ts.jacobian().transpose() *
0077                      prev_ts.predictedCovariance().inverse();
0078       // Loop over the beforehand smoothed states
0079       for (std::size_t iProcessed = 1; iProcessed <= nProcessed; iProcessed++) {
0080         const std::size_t iCol = iRow + eBoundSize * iProcessed;
0081         CovMatrix prev_correlation =
0082             fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(
0083                 iRow + eBoundSize, iCol);
0084         CovMatrix correlation = G * prev_correlation;
0085         fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(iRow, iCol) =
0086             correlation;
0087         fullGlobalTrackParamsCov.block<eBoundSize, eBoundSize>(iCol, iRow) =
0088             correlation.transpose();
0089       }
0090     }
0091     stateRowIndices.emplace(ts.index(), iRow);
0092     nProcessed++;
0093     prev_ts = ts;
0094   });
0095
0096   return {fullGlobalTrackParamsCov, stateRowIndices};
0097 }
0098
0099 }  // namespace Acts::detail