11 #include <boost/range/adaptors.hpp>
26 template <
typename parameters_t>
35 std::shared_ptr<const Logger>
logger = std::shared_ptr<const Logger>(
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;
47 using CovMatrix_t =
typename parameters_t::CovMatrix_t;
48 using gain_matrix_t = CovMatrix_t;
54 prev_ts.smoothed() = prev_ts.filtered();
55 prev_ts.smoothedCovariance() = prev_ts.filteredCovariance();
61 std::optional<std::error_code>
error{std::nullopt};
63 ACTS_VERBOSE(
"Only one track state given, smoothing terminates early");
65 ACTS_VERBOSE(
"Start smoothing from previous track state at index: "
66 << prev_ts.previous());
72 assert(ts.hasFiltered());
73 assert(ts.hasPredicted());
74 assert(ts.hasJacobian());
77 assert(prev_ts.hasSmoothed());
78 assert(prev_ts.hasPredicted());
81 ACTS_VERBOSE(
"Filtered covariance:\n" << ts.filteredCovariance());
84 << prev_ts.predictedCovariance() <<
"\n, inverse: \n"
85 << prev_ts.predictedCovariance().inverse());
88 G = ts.filteredCovariance() * ts.jacobian().transpose() *
89 prev_ts.predictedCovariance().inverse();
92 error = KalmanFitterError::SmoothFailed;
99 ACTS_VERBOSE(
"Filtered parameters: " << ts.filtered().transpose());
101 "Prev. smoothed parameters: " << prev_ts.smoothed().transpose());
103 "Prev. predicted parameters: " << prev_ts.predicted().transpose());
107 ts.filtered() + G * (prev_ts.smoothed() - prev_ts.predicted());
109 ACTS_VERBOSE(
"Smoothed parameters are: " << ts.smoothed().transpose());
113 << prev_ts.smoothedCovariance());
116 ts.smoothedCovariance() =
117 ts.filteredCovariance() -
118 G * (prev_ts.predictedCovariance() - prev_ts.smoothedCovariance()) *
125 CovMatrix_t smoothedCov = ts.smoothedCovariance();
128 "Smoothed covariance is not positive definite. Could result in "
129 "negative covariance!");
132 ts.smoothedCovariance() = smoothedCov;
133 ACTS_VERBOSE(
"Smoothed covariance is: \n" << ts.smoothedCovariance());
145 return prev_ts.smoothedParameters(gctx);