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   /// @return Success or failure of the MBF smoothing procedure
0046   template <typename traj_t>
0047   Result<void> operator()(const GeometryContext& gctx, traj_t& trajectory,
0048                           std::size_t entryIndex,
0049                           const Logger& logger = getDummyLogger()) const {
0050     (void)gctx;
0051     (void)logger;
0052
0053     using TrackStateProxy = typename traj_t::TrackStateProxy;
0054
0055     TrackStateProxy startTs = trajectory.getTrackState(entryIndex);
0056
0057     // Notation consistent with the Wikipedia article
0058     // https://en.wikipedia.org/wiki/Kalman_filter
0059     BoundMatrix bigLambdaHat = BoundMatrix::Zero();
0060     BoundVector smallLambdaHat = BoundVector::Zero();
0061
0062     trajectory.applyBackwards(startTs.index(), [&](TrackStateProxy ts) {
0063       // ensure the track state has a smoothed component
0064       ts.addComponents(TrackStatePropMask::Smoothed);
0065
0066       InternalTrackState internalTrackState(ts);
0067
0068       // Smoothe the current state
0069       calculateSmoothed(internalTrackState, bigLambdaHat, smallLambdaHat);
0070
0071       // We smoothed the last state - no need to update the lambdas
0072       if (!ts.hasPrevious()) {
0073         return;
0074       }
0075
0076       // Update the lambdas depending on the type of track state
0077       if (ts.typeFlags().test(TrackStateFlag::MeasurementFlag)) {
0078         visitMeasurement(internalTrackState, bigLambdaHat, smallLambdaHat);
0079       } else {
0080         visitNonMeasurement(internalTrackState, bigLambdaHat, smallLambdaHat);
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 Jacobian =
0092         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0093                                   false>::Covariance;
0094     using Parameters =
0095         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0096                                   false>::Parameters;
0097     using Covariance =
0098         typename TrackStateTraits<MultiTrajectoryTraits::MeasurementSizeMax,
0099                                   false>::Covariance;
0100
0101     struct Measurement final {
0102       unsigned int calibratedSize{0};
0103       // This is used to build a covariance matrix view in the .cpp file
0104       const double* calibrated{nullptr};
0105       const double* calibratedCovariance{nullptr};
0106       BoundSubspaceIndices projector;
0107
0108       template <typename TrackStateProxy>
0109       explicit Measurement(TrackStateProxy ts)
0110           : calibratedSize(ts.calibratedSize()),
0111             calibrated(ts.effectiveCalibrated().data()),
0112             calibratedCovariance(ts.effectiveCalibratedCovariance().data()),
0113             projector(ts.projectorSubspaceIndices()) {}
0114     };
0115
0116     Jacobian jacobian;
0117
0118     Parameters predicted;
0119     Covariance predictedCovariance;
0120     Parameters filtered;
0121     Covariance filteredCovariance;
0122     Parameters smoothed;
0123     Covariance smoothedCovariance;
0124
0125     std::optional<Measurement> measurement;
0126
0127     template <typename TrackStateProxy>
0128     explicit InternalTrackState(TrackStateProxy ts)
0129         : jacobian(ts.jacobian()),
0130           predicted(ts.predicted()),
0131           predictedCovariance(ts.predictedCovariance()),
0132           filtered(ts.filtered()),
0133           filteredCovariance(ts.filteredCovariance()),
0134           smoothed(ts.smoothed()),
0135           smoothedCovariance(ts.smoothedCovariance()),
0136           measurement(ts.typeFlags().test(TrackStateFlag::MeasurementFlag)
0137                           ? std::optional<Measurement>(ts)
0138                           : std::nullopt) {}
0139   };
0140
0141   /// Calculate the smoothed parameters and covariance.
0142   void calculateSmoothed(InternalTrackState& ts,
0143                          const BoundMatrix& bigLambdaHat,
0144                          const BoundVector& smallLambdaHat) const;
0145
0146   /// Visit a non-measurement track state and update the lambdas.
0147   void visitNonMeasurement(const InternalTrackState& ts,
0148                            BoundMatrix& bigLambdaHat,
0149                            BoundVector& smallLambdaHat) const;
0150
0151   /// Visit a measurement track state and update the lambdas.
0152   void visitMeasurement(const InternalTrackState& ts, BoundMatrix& bigLambdaHat,
0153                         BoundVector& smallLambdaHat) const;
0154 };
0155
0156 }  // namespace Acts