Acts/TrackFitting/MbfSmoother.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/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 startTs = trajectory.getTrackState(entryIndex);
0055
0056     // Notation consistent with the Wikipedia article
0057     // https://en.wikipedia.org/wiki/Kalman_filter
0058     BoundMatrix bigLambdaHat = BoundMatrix::Zero();
0059     BoundVector smallLambdaHat = BoundVector::Zero();
0060
0061     trajectory.applyBackwards(startTs.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, bigLambdaHat, smallLambdaHat);
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, bigLambdaHat, smallLambdaHat);
0078       } else {
0079         visitNonMeasurement(internalTrackState, bigLambdaHat, smallLambdaHat);
0080       }
0081     });
0082
0083     return Result<void>::success();
0084   }
0085
0086  private:
0087   /// Internal track state representation for the smoother.
0088   /// @note This allows us to move parts of the implementation into the .cpp
0089   struct InternalTrackState final {
0090     using Jacobian =
0091         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0092                                   false>::Covariance;
0093     using Parameters =
0094         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0095                                   false>::Parameters;
0096     using Covariance =
0097         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0098                                   false>::Covariance;
0099
0100     struct Measurement final {
0101       unsigned int calibratedSize{0};
0102       // This is used to build a covariance matrix view in the .cpp file
0103       const double* calibrated{nullptr};
0104       const double* calibratedCovariance{nullptr};
0105       BoundSubspaceIndices projector;
0106
0107       template <typename TrackStateProxy>
0108       explicit Measurement(TrackStateProxy ts)
0109           : calibratedSize(ts.calibratedSize()),
0110             calibrated(ts.effectiveCalibrated().data()),
0111             calibratedCovariance(ts.effectiveCalibratedCovariance().data()),
0112             projector(ts.projectorSubspaceIndices()) {}
0113     };
0114
0115     Jacobian jacobian;
0116
0117     Parameters predicted;
0118     Covariance predictedCovariance;
0119     Parameters filtered;
0120     Covariance filteredCovariance;
0121     Parameters smoothed;
0122     Covariance smoothedCovariance;
0123
0124     std::optional<Measurement> measurement;
0125
0126     template <typename TrackStateProxy>
0127     explicit InternalTrackState(TrackStateProxy ts)
0128         : jacobian(ts.jacobian()),
0129           predicted(ts.predicted()),
0130           predictedCovariance(ts.predictedCovariance()),
0131           filtered(ts.filtered()),
0132           filteredCovariance(ts.filteredCovariance()),
0133           smoothed(ts.smoothed()),
0134           smoothedCovariance(ts.smoothedCovariance()),
0135           measurement(ts.typeFlags().test(TrackStateFlag::MeasurementFlag)
0136                           ? std::optional<Measurement>(ts)
0137                           : std::nullopt) {}
0138   };
0139
0140   /// Calculate the smoothed parameters and covariance.
0141   void calculateSmoothed(InternalTrackState& ts,
0142                          const BoundMatrix& bigLambdaHat,
0143                          const BoundVector& smallLambdaHat) const;
0144
0145   /// Visit a non-measurement track state and update the lambdas.
0146   void visitNonMeasurement(const InternalTrackState& ts,
0147                            BoundMatrix& bigLambdaHat,
0148                            BoundVector& smallLambdaHat) const;
0149
0150   /// Visit a measurement track state and update the lambdas.
0151   void visitMeasurement(const InternalTrackState& ts, BoundMatrix& bigLambdaHat,
0152                         BoundVector& smallLambdaHat) const;
0153 };
0154
0155 }  // namespace Acts