ECCE @ EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
GainMatrixSmoother.hpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file GainMatrixSmoother.hpp
1 // This file is part of the Acts project.
2 //
3 // Copyright (C) 2018 CERN for the benefit of the Acts project
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
8 
9 #pragma once
10 
11 #include <boost/range/adaptors.hpp>
12 #include <memory>
19 
20 namespace Acts {
21 
26 template <typename parameters_t>
28  public:
31 
35  std::shared_ptr<const Logger> logger = std::shared_ptr<const Logger>(
36  getDefaultLogger("GainMatrixSmoother", Logging::INFO).release()))
37  : m_logger(std::move(logger)) {}
38 
39  template <typename source_link_t>
42  size_t entryIndex) const {
43  ACTS_VERBOSE("Invoked GainMatrixSmoother on entry index: " << entryIndex);
44  using namespace boost::adaptors;
45 
46  // using ParVector_t = typename parameters_t::ParVector_t;
47  using CovMatrix_t = typename parameters_t::CovMatrix_t;
48  using gain_matrix_t = CovMatrix_t;
49 
50  // For the last state: smoothed is filtered - also: switch to next
51  ACTS_VERBOSE("Getting previous track state");
52  auto prev_ts = trajectory.getTrackState(entryIndex);
53 
54  prev_ts.smoothed() = prev_ts.filtered();
55  prev_ts.smoothedCovariance() = prev_ts.filteredCovariance();
56 
57  // Smoothing gain matrix
58  gain_matrix_t G;
59 
60  // make sure there is more than one track state
61  std::optional<std::error_code> error{std::nullopt}; // assume ok
62  if (prev_ts.previous() == Acts::detail_lt::IndexData::kInvalid) {
63  ACTS_VERBOSE("Only one track state given, smoothing terminates early");
64  } else {
65  ACTS_VERBOSE("Start smoothing from previous track state at index: "
66  << prev_ts.previous());
67 
68  trajectory.applyBackwards(prev_ts.previous(), [&prev_ts, &G, &error,
69  this](auto ts) {
70  // should have filtered and predicted, this should also include the
71  // covariances.
72  assert(ts.hasFiltered());
73  assert(ts.hasPredicted());
74  assert(ts.hasJacobian());
75 
76  // previous trackstate should have smoothed and predicted
77  assert(prev_ts.hasSmoothed());
78  assert(prev_ts.hasPredicted());
79 
80  ACTS_VERBOSE("Calculate smoothing matrix:");
81  ACTS_VERBOSE("Filtered covariance:\n" << ts.filteredCovariance());
82  ACTS_VERBOSE("Jacobian:\n" << ts.jacobian());
83  ACTS_VERBOSE("Prev. predicted covariance\n"
84  << prev_ts.predictedCovariance() << "\n, inverse: \n"
85  << prev_ts.predictedCovariance().inverse());
86 
87  // Gain smoothing matrix
88  G = ts.filteredCovariance() * ts.jacobian().transpose() *
89  prev_ts.predictedCovariance().inverse();
90 
91  if (G.hasNaN()) {
92  error = KalmanFitterError::SmoothFailed; // set to error
93  return false; // abort execution
94  }
95 
96  ACTS_VERBOSE("Gain smoothing matrix G:\n" << G);
97 
98  ACTS_VERBOSE("Calculate smoothed parameters:");
99  ACTS_VERBOSE("Filtered parameters: " << ts.filtered().transpose());
100  ACTS_VERBOSE(
101  "Prev. smoothed parameters: " << prev_ts.smoothed().transpose());
102  ACTS_VERBOSE(
103  "Prev. predicted parameters: " << prev_ts.predicted().transpose());
104 
105  // Calculate the smoothed parameters
106  ts.smoothed() =
107  ts.filtered() + G * (prev_ts.smoothed() - prev_ts.predicted());
108 
109  ACTS_VERBOSE("Smoothed parameters are: " << ts.smoothed().transpose());
110 
111  ACTS_VERBOSE("Calculate smoothed covariance:");
112  ACTS_VERBOSE("Prev. smoothed covariance:\n"
113  << prev_ts.smoothedCovariance());
114 
115  // And the smoothed covariance
116  ts.smoothedCovariance() =
117  ts.filteredCovariance() -
118  G * (prev_ts.predictedCovariance() - prev_ts.smoothedCovariance()) *
119  G.transpose();
120 
121  // Check if the covariance matrix is semi-positive definite.
122  // If not, make one (could do more) attempt to replace it with the
123  // nearest semi-positive def matrix,
124  // but it could still be non semi-positive
125  CovMatrix_t smoothedCov = ts.smoothedCovariance();
127  ACTS_DEBUG(
128  "Smoothed covariance is not positive definite. Could result in "
129  "negative covariance!");
130  }
131  // Reset smoothed covariance
132  ts.smoothedCovariance() = smoothedCov;
133  ACTS_VERBOSE("Smoothed covariance is: \n" << ts.smoothedCovariance());
134 
135  prev_ts = ts;
136  return true; // continue execution
137  });
138  }
139  if (error) {
140  // error is set, return result
141  return *error;
142  }
143 
144  // construct parameters from last track state
145  return prev_ts.smoothedParameters(gctx);
146  }
147 
149  std::shared_ptr<const Logger> m_logger{nullptr};
150 
152  const Logger& logger() const {
153  assert(m_logger);
154  return *m_logger;
155  }
156 };
157 } // namespace Acts