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/AnyTrackStateProxy.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 <utility>
0019
0020 namespace Acts {
0021
0022 /// Kalman trajectory smoother based on the Modified Bryson–Frazier (mBF)
0023 /// smoother.
0024 ///
0025 /// @ingroup track_fitting
0026 ///
0027 /// The benefit of the mBF smoother is that it does not require the inverse of
0028 /// the full covariance matrix, but only the inverse of the residual covariance
0029 /// matrix which can be cached by the filter step. The same holds for the
0030 /// Kalman gain matrix.
0031 ///
0032 /// This implements not a single smoothing step, but the full backwards
0033 /// smoothing procedure for a filtered, forward trajectory using the stored
0034 /// linearization.
0035 ///
0036 /// See
0037 /// [Wikipedia](https://en.wikipedia.org/wiki/Kalman_filter#Modified_Bryson%E2%80%93Frazier_smoother)
0038 /// for more information.
0039 class MbfSmoother {
0040  public:
0041   /// Run the Kalman smoothing for one trajectory.
0042   ///
0043   /// @param[in] gctx The geometry context to be used
0044   /// @param[in,out] trajectory The trajectory to be smoothed
0045   /// @param[in] entryIndex The index of state to start the smoothing
0046   /// @param[in] logger Where to write logging information to
0047   /// @return Success or failure of the MBF smoothing procedure
0048   template <typename traj_t>
0049   Result<void> operator()(const GeometryContext& gctx, traj_t& trajectory,
0050                           std::size_t entryIndex,
0051                           const Logger& logger = getDummyLogger()) const {
0052     static_cast<void>(gctx);
0053     static_cast<void>(logger);
0054
0055     using TrackStateProxy = typename traj_t::TrackStateProxy;
0056
0057     TrackStateProxy startTs = trajectory.getTrackState(entryIndex);
0058
0059     // Notation consistent with the Wikipedia article
0060     // https://en.wikipedia.org/wiki/Kalman_filter
0061     BoundMatrix bigLambdaHat = BoundMatrix::Zero();
0062     BoundVector smallLambdaHat = BoundVector::Zero();
0063
0064     trajectory.applyBackwards(startTs.index(), [&](TrackStateProxy ts) {
0065       // ensure the track state has a smoothed component
0066       ts.addComponents(TrackStatePropMask::Smoothed);
0067
0068       AnyMutableTrackStateProxy internalTrackState(ts);
0069
0070       // Smoothe the current state
0071       calculateSmoothed(internalTrackState, bigLambdaHat, smallLambdaHat);
0072
0073       // We smoothed the last state - no need to update the lambdas
0074       if (!ts.hasPrevious()) {
0075         return;
0076       }
0077
0078       // Update the lambdas depending on the type of track state
0079       if (ts.typeFlags().isMeasurement()) {
0080         visitMeasurement(AnyConstTrackStateProxy{ts}, bigLambdaHat,
0081                          smallLambdaHat);
0082       } else {
0083         visitNonMeasurement(std::as_const(ts).jacobian(), bigLambdaHat,
0084                             smallLambdaHat);
0085       }
0086     });
0087
0088     return Result<void>::success();
0089   }
0090
0091  private:
0092   /// Calculate the smoothed parameters and covariance.
0093   void calculateSmoothed(AnyMutableTrackStateProxy& ts,
0094                          const BoundMatrix& bigLambdaHat,
0095                          const BoundVector& smallLambdaHat) const;
0096
0097   /// Visit a non-measurement track state and update the lambdas.
0098   void visitNonMeasurement(
0099       const AnyConstTrackStateProxy::ConstCovarianceMap& jacobian,
0100       BoundMatrix& bigLambdaHat, BoundVector& smallLambdaHat) const;
0101
0102   /// Visit a measurement track state and update the lambdas.
0103   void visitMeasurement(const AnyConstTrackStateProxy& ts,
0104                         BoundMatrix& bigLambdaHat,
0105                         BoundVector& smallLambdaHat) const;
0106
0107   template <std::size_t N>
0108   void visitMeasurementImpl(const AnyConstTrackStateProxy& ts,
0109                             BoundMatrix& bigLambdaHat,
0110                             BoundVector& smallLambdaHat) const;
0111 };
0112
0113 }  // namespace Acts