Acts/TrackFitting/MbfSmoother.hpp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2024 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 #pragma once
0010
0011 #include "Acts/Definitions/TrackParametrization.hpp"
0012 #include "Acts/EventData/MultiTrajectory.hpp"
0013 #include "Acts/Geometry/GeometryContext.hpp"
0014 #include "Acts/Utilities/Logger.hpp"
0015 #include "Acts/Utilities/Result.hpp"
0016
0017 #include <cstddef>
0018 #include <optional>
0019
0020 namespace Acts {
0021
0022 /// Kalman trajectory smoother based on the Modified Bryson–Frazier (mBF)
0023 /// smoother.
0024 ///
0025 /// The benefit of the mBF smoother is that it does not require the inverse of
0026 /// the full covariance matrix, but only the inverse of the residual covariance
0027 /// matrix which can be cached by the filter step. The same holds for the
0028 /// Kalman gain matrix.
0029 ///
0030 /// This implements not a single smoothing step, but the full backwards
0031 /// smoothing procedure for a filtered, forward trajectory using the stored
0032 /// linearization.
0033 ///
0034 /// See
0035 /// [Wikipedia](https://en.wikipedia.org/wiki/Kalman_filter#Modified_Bryson%E2%80%93Frazier_smoother)
0036 /// for more information.
0037 class MbfSmoother {
0038  public:
0039   /// Run the Kalman smoothing for one trajectory.
0040   ///
0041   /// @param[in] gctx The geometry context to be used
0042   /// @param[in,out] trajectory The trajectory to be smoothed
0043   /// @param[in] entryIndex The index of state to start the smoothing
0044   /// @param[in] logger Where to write logging information to
0045   template <typename traj_t>
0046   Result<void> operator()(const GeometryContext& gctx, traj_t& trajectory,
0047                           std::size_t entryIndex,
0048                           const Logger& logger = getDummyLogger()) const {
0049     (void)gctx;
0050     (void)logger;
0051
0052     using TrackStateProxy = typename traj_t::TrackStateProxy;
0053
0054     TrackStateProxy start_ts = trajectory.getTrackState(entryIndex);
0055
0056     // Notation consistent with the Wikipedia article
0057     // https://en.wikipedia.org/wiki/Kalman_filter
0058     BoundMatrix big_lambda_hat = BoundMatrix::Zero();
0059     BoundVector small_lambda_hat = BoundVector::Zero();
0060
0061     trajectory.applyBackwards(start_ts.index(), [&](TrackStateProxy ts) {
0062       // ensure the track state has a smoothed component
0063       ts.addComponents(TrackStatePropMask::Smoothed);
0064
0065       InternalTrackState internalTrackState(ts);
0066
0067       // Smoothe the current state
0068       calculateSmoothed(internalTrackState, big_lambda_hat, small_lambda_hat);
0069
0070       // We smoothed the last state - no need to update the lambdas
0071       if (!ts.hasPrevious()) {
0072         return;
0073       }
0074
0075       // Update the lambdas depending on the type of track state
0076       if (ts.typeFlags().test(TrackStateFlag::MeasurementFlag)) {
0077         visitMeasurement(internalTrackState, big_lambda_hat, small_lambda_hat);
0078       } else {
0079         visitNonMeasurement(internalTrackState, big_lambda_hat,
0080                             small_lambda_hat);
0081       }
0082     });
0083
0084     return Result<void>::success();
0085   }
0086
0087  private:
0088   /// Internal track state representation for the smoother.
0089   /// @note This allows us to move parts of the implementation into the .cpp
0090   struct InternalTrackState final {
0091     using Projector =
0092         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0093                                   false>::Projector;
0094     using Jacobian =
0095         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0096                                   false>::Covariance;
0097     using Parameters =
0098         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0099                                   false>::Parameters;
0100     using Covariance =
0101         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0102                                   false>::Covariance;
0103
0104     struct Measurement final {
0105       unsigned int calibratedSize{0};
0106       // This is used to build a covariance matrix view in the .cpp file
0107       const double* calibrated{nullptr};
0108       const double* calibratedCovariance{nullptr};
0109       Projector projector;
0110
0111       template <typename TrackStateProxy>
0112       explicit Measurement(TrackStateProxy ts)
0113           : calibratedSize(ts.calibratedSize()),
0114             calibrated(ts.effectiveCalibrated().data()),
0115             calibratedCovariance(ts.effectiveCalibratedCovariance().data()),
0116             projector(ts.projector()) {}
0117     };
0118
0119     Jacobian jacobian;
0120
0121     Parameters predicted;
0122     Covariance predictedCovariance;
0123     Parameters filtered;
0124     Covariance filteredCovariance;
0125     Parameters smoothed;
0126     Covariance smoothedCovariance;
0127
0128     std::optional<Measurement> measurement;
0129
0130     template <typename TrackStateProxy>
0131     explicit InternalTrackState(TrackStateProxy ts)
0132         : jacobian(ts.jacobian()),
0133           predicted(ts.predicted()),
0134           predictedCovariance(ts.predictedCovariance()),
0135           filtered(ts.filtered()),
0136           filteredCovariance(ts.filteredCovariance()),
0137           smoothed(ts.smoothed()),
0138           smoothedCovariance(ts.smoothedCovariance()),
0139           measurement(ts.typeFlags().test(TrackStateFlag::MeasurementFlag)
0140                           ? std::optional<Measurement>(ts)
0141                           : std::nullopt) {}
0142   };
0143
0144   /// Calculate the smoothed parameters and covariance.
0145   void calculateSmoothed(InternalTrackState& ts,
0146                          const BoundMatrix& big_lambda_hat,
0147                          const BoundVector& small_lambda_hat) const;
0148
0149   /// Visit a non-measurement track state and update the lambdas.
0150   void visitNonMeasurement(const InternalTrackState& ts,
0151                            BoundMatrix& big_lambda_hat,
0152                            BoundVector& small_lambda_hat) const;
0153
0154   /// Visit a measurement track state and update the lambdas.
0155   void visitMeasurement(const InternalTrackState& ts,
0156                         BoundMatrix& big_lambda_hat,
0157                         BoundVector& small_lambda_hat) const;
0158 };
0159
0160 }  // namespace Acts