18 #include <unordered_map>
36 template <
typename traj_t,
typename parameters_t = BoundTrackParameters>
37 std::pair<ActsDynamicMatrix, std::unordered_map<size_t, size_t>>
39 const size_t& entryIndex) {
40 using CovMatrix =
typename parameters_t::CovarianceMatrix;
41 using GainMatrix = CovMatrix;
44 size_t lastSmoothedIndex = SIZE_MAX;
46 size_t nSmoothedStates = 0;
48 multiTraj.visitBackwards(entryIndex, [&](
const auto& ts) {
49 if (ts.hasSmoothed()) {
50 if (lastSmoothedIndex == SIZE_MAX) {
51 lastSmoothedIndex = ts.index();
59 nSmoothedStates * eBoundSize);
60 fullGlobalTrackParamsCov.setZero();
63 std::unordered_map<size_t, size_t> stateRowIndices;
66 size_t nProcessed = 0;
67 auto prev_ts = multiTraj.getTrackState(lastSmoothedIndex);
68 multiTraj.visitBackwards(lastSmoothedIndex, [&](
const auto& ts) {
70 fullGlobalTrackParamsCov.rows() - eBoundSize * (nProcessed + 1);
72 fullGlobalTrackParamsCov.block<
eBoundSize, eBoundSize>(iRow, iRow) =
73 ts.smoothedCovariance();
79 GainMatrix
G = ts.filteredCovariance() * prev_ts.jacobian().transpose() *
80 prev_ts.predictedCovariance().inverse();
82 for (
size_t iProcessed = 1; iProcessed <= nProcessed; iProcessed++) {
83 const size_t iCol = iRow + eBoundSize * iProcessed;
84 CovMatrix prev_correlation =
85 fullGlobalTrackParamsCov.block<
eBoundSize, eBoundSize>(
87 CovMatrix correlation = G * prev_correlation;
88 fullGlobalTrackParamsCov.block<
eBoundSize, eBoundSize>(iRow, iCol) =
90 fullGlobalTrackParamsCov.block<
eBoundSize, eBoundSize>(iCol, iRow) =
91 correlation.transpose();
94 stateRowIndices.emplace(ts.index(), iRow);
99 return std::make_pair(fullGlobalTrackParamsCov, stateRowIndices);