Back to home page

EIC code displayed by LXR

 
 

    


File indexing completed on 2025-01-18 09:11:07

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 // Workaround for building on clang+libstdc++
0012 #include "Acts/Utilities/detail/ReferenceWrapperAnyCompat.hpp"
0013 
0014 #include "Acts/EventData/MultiTrajectory.hpp"
0015 #include "Acts/EventData/MultiTrajectoryHelpers.hpp"
0016 #include "Acts/EventData/SourceLink.hpp"
0017 #include "Acts/EventData/TrackContainerFrontendConcept.hpp"
0018 #include "Acts/EventData/TrackParameters.hpp"
0019 #include "Acts/EventData/VectorMultiTrajectory.hpp"
0020 #include "Acts/Geometry/GeometryContext.hpp"
0021 #include "Acts/MagneticField/MagneticFieldContext.hpp"
0022 #include "Acts/Propagator/ActorList.hpp"
0023 #include "Acts/Propagator/DirectNavigator.hpp"
0024 #include "Acts/Propagator/Propagator.hpp"
0025 #include "Acts/Propagator/StandardAborters.hpp"
0026 #include "Acts/Propagator/detail/PointwiseMaterialInteraction.hpp"
0027 #include "Acts/TrackFitting/KalmanFitterError.hpp"
0028 #include "Acts/TrackFitting/detail/KalmanUpdateHelpers.hpp"
0029 #include "Acts/TrackFitting/detail/VoidFitterComponents.hpp"
0030 #include "Acts/Utilities/CalibrationContext.hpp"
0031 #include "Acts/Utilities/Delegate.hpp"
0032 #include "Acts/Utilities/Logger.hpp"
0033 #include "Acts/Utilities/Result.hpp"
0034 #include "Acts/Utilities/TrackHelpers.hpp"
0035 
0036 #include <functional>
0037 #include <limits>
0038 #include <map>
0039 #include <memory>
0040 #include <type_traits>
0041 
0042 namespace Acts {
0043 
0044 enum class KalmanFitterTargetSurfaceStrategy {
0045   /// Use the first trackstate to reach target surface
0046   first,
0047   /// Use the last trackstate to reach target surface
0048   last,
0049   /// Use the first or last trackstate to reach target surface depending on the
0050   /// distance
0051   firstOrLast,
0052 };
0053 
0054 /// Extension struct which holds delegates to customise the KF behavior
0055 template <typename traj_t>
0056 struct KalmanFitterExtensions {
0057   using TrackStateProxy = typename traj_t::TrackStateProxy;
0058   using ConstTrackStateProxy = typename traj_t::ConstTrackStateProxy;
0059   using Parameters = typename TrackStateProxy::Parameters;
0060 
0061   using Calibrator =
0062       Delegate<void(const GeometryContext&, const CalibrationContext&,
0063                     const SourceLink&, TrackStateProxy)>;
0064 
0065   using Smoother = Delegate<Result<void>(const GeometryContext&, traj_t&,
0066                                          std::size_t, const Logger&)>;
0067 
0068   using Updater = Delegate<Result<void>(const GeometryContext&, TrackStateProxy,
0069                                         const Logger&)>;
0070 
0071   using OutlierFinder = Delegate<bool(ConstTrackStateProxy)>;
0072 
0073   using ReverseFilteringLogic = Delegate<bool(ConstTrackStateProxy)>;
0074 
0075   /// The Calibrator is a dedicated calibration algorithm that allows
0076   /// to calibrate measurements using track information, this could be
0077   /// e.g. sagging for wires, module deformations, etc.
0078   Calibrator calibrator;
0079 
0080   /// The updater incorporates measurement information into the track parameters
0081   Updater updater;
0082 
0083   /// The smoother back-propagates measurement information along the track
0084   Smoother smoother;
0085 
0086   /// Determines whether a measurement is supposed to be considered as an
0087   /// outlier
0088   OutlierFinder outlierFinder;
0089 
0090   /// Decides whether the smoothing stage uses linearized transport or full
0091   /// reverse propagation
0092   ReverseFilteringLogic reverseFilteringLogic;
0093 
0094   /// Retrieves the associated surface from a source link
0095   SourceLinkSurfaceAccessor surfaceAccessor;
0096 
0097   /// Default constructor which connects the default void components
0098   KalmanFitterExtensions() {
0099     calibrator.template connect<&detail::voidFitterCalibrator<traj_t>>();
0100     updater.template connect<&detail::voidFitterUpdater<traj_t>>();
0101     smoother.template connect<&detail::voidFitterSmoother<traj_t>>();
0102     outlierFinder.template connect<&detail::voidOutlierFinder<traj_t>>();
0103     reverseFilteringLogic
0104         .template connect<&detail::voidReverseFilteringLogic<traj_t>>();
0105     surfaceAccessor.connect<&detail::voidSurfaceAccessor>();
0106   }
0107 };
0108 
0109 /// Combined options for the Kalman fitter.
0110 ///
0111 /// @tparam traj_t The trajectory type
0112 template <typename traj_t>
0113 struct KalmanFitterOptions {
0114   /// PropagatorOptions with context.
0115   ///
0116   /// @param gctx The geometry context for this fit
0117   /// @param mctx The magnetic context for this fit
0118   /// @param cctx The calibration context for this fit
0119   /// @param extensions_ The KF extensions
0120   /// @param pOptions The plain propagator options
0121   /// @param rSurface The reference surface for the fit to be expressed at
0122   /// @param mScattering Whether to include multiple scattering
0123   /// @param eLoss Whether to include energy loss
0124   /// @param rFiltering Whether to run filtering in reversed direction as smoothing
0125   /// @param rfScaling Scale factor for the covariance matrix before the backward filtering
0126   /// @param freeToBoundCorrection_ Correction for non-linearity effect during transform from free to bound
0127   KalmanFitterOptions(const GeometryContext& gctx,
0128                       const MagneticFieldContext& mctx,
0129                       std::reference_wrapper<const CalibrationContext> cctx,
0130                       KalmanFitterExtensions<traj_t> extensions_,
0131                       const PropagatorPlainOptions& pOptions,
0132                       const Surface* rSurface = nullptr,
0133                       bool mScattering = true, bool eLoss = true,
0134                       bool rFiltering = false, double rfScaling = 1.0,
0135                       const FreeToBoundCorrection& freeToBoundCorrection_ =
0136                           FreeToBoundCorrection(false))
0137       : geoContext(gctx),
0138         magFieldContext(mctx),
0139         calibrationContext(cctx),
0140         extensions(extensions_),
0141         propagatorPlainOptions(pOptions),
0142         referenceSurface(rSurface),
0143         multipleScattering(mScattering),
0144         energyLoss(eLoss),
0145         reversedFiltering(rFiltering),
0146         reversedFilteringCovarianceScaling(rfScaling),
0147         freeToBoundCorrection(freeToBoundCorrection_) {}
0148   /// Contexts are required and the options must not be default-constructible.
0149   KalmanFitterOptions() = delete;
0150 
0151   /// Context object for the geometry
0152   std::reference_wrapper<const GeometryContext> geoContext;
0153   /// Context object for the magnetic field
0154   std::reference_wrapper<const MagneticFieldContext> magFieldContext;
0155   /// context object for the calibration
0156   std::reference_wrapper<const CalibrationContext> calibrationContext;
0157 
0158   KalmanFitterExtensions<traj_t> extensions;
0159 
0160   /// The trivial propagator options
0161   PropagatorPlainOptions propagatorPlainOptions;
0162 
0163   /// The reference Surface
0164   const Surface* referenceSurface = nullptr;
0165 
0166   /// Strategy to propagate to reference surface
0167   KalmanFitterTargetSurfaceStrategy referenceSurfaceStrategy =
0168       KalmanFitterTargetSurfaceStrategy::firstOrLast;
0169 
0170   /// Whether to consider multiple scattering
0171   bool multipleScattering = true;
0172 
0173   /// Whether to consider energy loss
0174   bool energyLoss = true;
0175 
0176   /// Whether to run filtering in reversed direction overwrite the
0177   /// ReverseFilteringLogic
0178   bool reversedFiltering = false;
0179 
0180   /// Factor by which the covariance of the input of the reversed filtering is
0181   /// scaled. This is only used in the backwardfiltering (if reversedFiltering
0182   /// is true or if the ReverseFilteringLogic return true for the track of
0183   /// interest)
0184   double reversedFilteringCovarianceScaling = 1.0;
0185 
0186   /// Whether to include non-linear correction during global to local
0187   /// transformation
0188   FreeToBoundCorrection freeToBoundCorrection;
0189 };
0190 
0191 template <typename traj_t>
0192 struct KalmanFitterResult {
0193   /// Fitted states that the actor has handled.
0194   traj_t* fittedStates{nullptr};
0195 
0196   /// This is the index of the 'tip' of the track stored in multitrajectory.
0197   /// This corresponds to the last measurement state in the multitrajectory.
0198   /// Since this KF only stores one trajectory, it is unambiguous.
0199   /// Acts::MultiTrajectoryTraits::kInvalid is the start of a trajectory.
0200   std::size_t lastMeasurementIndex = Acts::MultiTrajectoryTraits::kInvalid;
0201 
0202   /// This is the index of the 'tip' of the states stored in multitrajectory.
0203   /// This corresponds to the last state in the multitrajectory.
0204   /// Since this KF only stores one trajectory, it is unambiguous.
0205   /// Acts::MultiTrajectoryTraits::kInvalid is the start of a trajectory.
0206   std::size_t lastTrackIndex = Acts::MultiTrajectoryTraits::kInvalid;
0207 
0208   /// The optional Parameters at the provided surface
0209   std::optional<BoundTrackParameters> fittedParameters;
0210 
0211   /// Counter for states with non-outlier measurements
0212   std::size_t measurementStates = 0;
0213 
0214   /// Counter for measurements holes
0215   /// A hole correspond to a surface with an associated detector element with no
0216   /// associated measurement. Holes are only taken into account if they are
0217   /// between the first and last measurements.
0218   std::size_t measurementHoles = 0;
0219 
0220   /// Counter for handled states
0221   std::size_t processedStates = 0;
0222 
0223   /// Indicator if smoothing has been done.
0224   bool smoothed = false;
0225 
0226   /// Indicator if navigation direction has been reversed
0227   bool reversed = false;
0228 
0229   /// Indicator if track fitting has been done
0230   bool finished = false;
0231 
0232   /// Measurement surfaces without hits
0233   std::vector<const Surface*> missedActiveSurfaces;
0234 
0235   /// Measurement surfaces handled in both forward and backward filtering
0236   std::vector<const Surface*> passedAgainSurfaces;
0237 
0238   Result<void> result{Result<void>::success()};
0239 };
0240 
0241 /// Kalman fitter implementation.
0242 ///
0243 /// @tparam propagator_t Type of the propagation class
0244 ///
0245 /// The Kalman filter contains an Actor and a Sequencer sub-class.
0246 /// The Sequencer has to be part of the Navigator of the Propagator
0247 /// in order to initialize and provide the measurement surfaces.
0248 ///
0249 /// The Actor is part of the Propagation call and does the Kalman update
0250 /// and eventually the smoothing.  Updater, Smoother and Calibrator are
0251 /// given to the Actor for further use:
0252 /// - The Updater is the implemented kalman updater formalism, it
0253 ///   runs via a visitor pattern through the measurements.
0254 /// - The Smoother is called at the end of the filtering by the Actor.
0255 ///
0256 /// Measurements are not required to be ordered for the KalmanFilter,
0257 /// measurement ordering needs to be figured out by the navigation of
0258 /// the propagator.
0259 ///
0260 /// The void components are provided mainly for unit testing.
0261 template <typename propagator_t, typename traj_t>
0262 class KalmanFitter {
0263   /// The navigator type
0264   using KalmanNavigator = typename propagator_t::Navigator;
0265 
0266   /// The navigator has DirectNavigator type or not
0267   static constexpr bool isDirectNavigator =
0268       std::is_same_v<KalmanNavigator, DirectNavigator>;
0269 
0270  public:
0271   KalmanFitter(propagator_t pPropagator,
0272                std::unique_ptr<const Logger> _logger =
0273                    getDefaultLogger("KalmanFitter", Logging::INFO))
0274       : m_propagator(std::move(pPropagator)),
0275         m_logger{std::move(_logger)},
0276         m_actorLogger{m_logger->cloneWithSuffix("Actor")} {}
0277 
0278  private:
0279   /// The propagator for the transport and material update
0280   propagator_t m_propagator;
0281 
0282   /// The logger instance
0283   std::unique_ptr<const Logger> m_logger;
0284   std::unique_ptr<const Logger> m_actorLogger;
0285 
0286   const Logger& logger() const { return *m_logger; }
0287 
0288   /// @brief Propagator Actor plugin for the KalmanFilter
0289   ///
0290   /// @tparam parameters_t The type of parameters used for "local" parameters.
0291   /// @tparam calibrator_t The type of calibrator
0292   /// @tparam outlier_finder_t Type of the outlier finder class
0293   ///
0294   /// The KalmanActor does not rely on the measurements to be
0295   /// sorted along the track.
0296   template <typename parameters_t>
0297   class Actor {
0298    public:
0299     /// Broadcast the result_type
0300     using result_type = KalmanFitterResult<traj_t>;
0301 
0302     /// The target surface aboter
0303     SurfaceReached targetReached{std::numeric_limits<double>::lowest()};
0304 
0305     /// Strategy to propagate to target surface
0306     KalmanFitterTargetSurfaceStrategy targetSurfaceStrategy =
0307         KalmanFitterTargetSurfaceStrategy::firstOrLast;
0308 
0309     /// Allows retrieving measurements for a surface
0310     const std::map<GeometryIdentifier, SourceLink>* inputMeasurements = nullptr;
0311 
0312     /// Whether to consider multiple scattering.
0313     bool multipleScattering = true;
0314 
0315     /// Whether to consider energy loss.
0316     bool energyLoss = true;
0317 
0318     /// Whether run reversed filtering
0319     bool reversedFiltering = false;
0320 
0321     /// Scale the covariance before the reversed filtering
0322     double reversedFilteringCovarianceScaling = 1.0;
0323 
0324     /// Whether to include non-linear correction during global to local
0325     /// transformation
0326     FreeToBoundCorrection freeToBoundCorrection;
0327 
0328     /// Input MultiTrajectory
0329     std::shared_ptr<traj_t> outputStates;
0330 
0331     /// The logger instance
0332     const Logger* actorLogger{nullptr};
0333 
0334     /// Logger helper
0335     const Logger& logger() const { return *actorLogger; }
0336 
0337     KalmanFitterExtensions<traj_t> extensions;
0338 
0339     /// Calibration context for the fit
0340     const CalibrationContext* calibrationContext{nullptr};
0341 
0342     /// @brief Kalman actor operation
0343     ///
0344     /// @tparam propagator_state_t is the type of Propagator state
0345     /// @tparam stepper_t Type of the stepper
0346     /// @tparam navigator_t Type of the navigator
0347     ///
0348     /// @param state is the mutable propagator state object
0349     /// @param stepper The stepper in use
0350     /// @param navigator The navigator in use
0351     /// @param result is the mutable result state object
0352     template <typename propagator_state_t, typename stepper_t,
0353               typename navigator_t>
0354     void act(propagator_state_t& state, const stepper_t& stepper,
0355              const navigator_t& navigator, result_type& result,
0356              const Logger& /*logger*/) const {
0357       assert(result.fittedStates && "No MultiTrajectory set");
0358 
0359       if (result.finished) {
0360         return;
0361       }
0362 
0363       ACTS_VERBOSE("KalmanFitter step at pos: "
0364                    << stepper.position(state.stepping).transpose()
0365                    << " dir: " << stepper.direction(state.stepping).transpose()
0366                    << " momentum: "
0367                    << stepper.absoluteMomentum(state.stepping));
0368 
0369       // Update:
0370       // - Waiting for a current surface
0371       auto surface = navigator.currentSurface(state.navigation);
0372       std::string direction = state.options.direction.toString();
0373       if (surface != nullptr) {
0374         // Check if the surface is in the measurement map
0375         // -> Get the measurement / calibrate
0376         // -> Create the predicted state
0377         // -> Check outlier behavior, if non-outlier:
0378         // -> Perform the kalman update
0379         // -> Fill track state information & update stepper information
0380 
0381         if (!result.smoothed && !result.reversed) {
0382           ACTS_VERBOSE("Perform " << direction << " filter step");
0383           auto res = filter(surface, state, stepper, navigator, result);
0384           if (!res.ok()) {
0385             ACTS_ERROR("Error in " << direction << " filter: " << res.error());
0386             result.result = res.error();
0387           }
0388         }
0389         if (result.reversed) {
0390           ACTS_VERBOSE("Perform " << direction << " filter step");
0391           auto res = reversedFilter(surface, state, stepper, navigator, result);
0392           if (!res.ok()) {
0393             ACTS_ERROR("Error in " << direction << " filter: " << res.error());
0394             result.result = res.error();
0395           }
0396         }
0397       }
0398 
0399       // Finalization:
0400       // when all track states have been handled or the navigation is breaked,
0401       // reset navigation&stepping before run reversed filtering or
0402       // proceed to run smoothing
0403       if (!result.smoothed && !result.reversed) {
0404         if (result.measurementStates == inputMeasurements->size() ||
0405             (result.measurementStates > 0 &&
0406              navigator.navigationBreak(state.navigation))) {
0407           // Remove the missing surfaces that occur after the last measurement
0408           result.missedActiveSurfaces.resize(result.measurementHoles);
0409           // now get track state proxy for the smoothing logic
0410           auto trackStateProxy =
0411               result.fittedStates->getTrackState(result.lastMeasurementIndex);
0412           if (reversedFiltering ||
0413               extensions.reverseFilteringLogic(trackStateProxy)) {
0414             // Start to run reversed filtering:
0415             // Reverse navigation direction and reset navigation and stepping
0416             // state to last measurement
0417             ACTS_VERBOSE("Reverse navigation direction.");
0418             auto res = reverse(state, stepper, navigator, result);
0419             if (!res.ok()) {
0420               ACTS_ERROR("Error in reversing navigation: " << res.error());
0421               result.result = res.error();
0422             }
0423           } else {
0424             // --> Search the starting state to run the smoothing
0425             // --> Call the smoothing
0426             // --> Set a stop condition when all track states have been
0427             // handled
0428             ACTS_VERBOSE("Finalize/run smoothing");
0429             auto res = finalize(state, stepper, navigator, result);
0430             if (!res.ok()) {
0431               ACTS_ERROR("Error in finalize: " << res.error());
0432               result.result = res.error();
0433             }
0434           }
0435         }
0436       }
0437 
0438       // Post-finalization:
0439       // - Progress to target/reference surface and built the final track
0440       // parameters
0441       if (result.smoothed || result.reversed) {
0442         if (result.smoothed) {
0443           // Update state and stepper with material effects
0444           // Only for smoothed as reverse filtering will handle this separately
0445           materialInteractor(navigator.currentSurface(state.navigation), state,
0446                              stepper, navigator,
0447                              MaterialUpdateStage::FullUpdate);
0448         }
0449 
0450         if (targetReached.surface == nullptr) {
0451           // If no target surface provided:
0452           // -> Return an error when using reversed filtering mode
0453           // -> Fitting is finished here
0454           if (result.reversed) {
0455             ACTS_ERROR(
0456                 "The target surface needed for aborting reversed propagation "
0457                 "is not provided");
0458             result.result =
0459                 Result<void>(KalmanFitterError::ReversePropagationFailed);
0460           } else {
0461             ACTS_VERBOSE(
0462                 "No target surface set. Completing without fitted track "
0463                 "parameter");
0464             // Remember the track fitting is done
0465             result.finished = true;
0466           }
0467         } else if (targetReached.checkAbort(state, stepper, navigator,
0468                                             logger())) {
0469           ACTS_VERBOSE("Completing with fitted track parameter");
0470           // Transport & bind the parameter to the final surface
0471           auto res = stepper.boundState(state.stepping, *targetReached.surface,
0472                                         true, freeToBoundCorrection);
0473           if (!res.ok()) {
0474             ACTS_ERROR("Error in " << direction << " filter: " << res.error());
0475             result.result = res.error();
0476             return;
0477           }
0478           auto& fittedState = *res;
0479           // Assign the fitted parameters
0480           result.fittedParameters = std::get<BoundTrackParameters>(fittedState);
0481 
0482           // Reset smoothed status of states missed in reversed filtering
0483           if (result.reversed) {
0484             result.fittedStates->applyBackwards(
0485                 result.lastMeasurementIndex, [&](auto trackState) {
0486                   auto fSurface = &trackState.referenceSurface();
0487                   if (!rangeContainsValue(result.passedAgainSurfaces,
0488                                           fSurface)) {
0489                     // If reversed filtering missed this surface, then there is
0490                     // no smoothed parameter
0491                     trackState.unset(TrackStatePropMask::Smoothed);
0492                     trackState.typeFlags().set(TrackStateFlag::OutlierFlag);
0493                   }
0494                 });
0495           }
0496           // Remember the track fitting is done
0497           result.finished = true;
0498         }
0499       }
0500     }
0501 
0502     template <typename propagator_state_t, typename stepper_t,
0503               typename navigator_t>
0504     bool checkAbort(propagator_state_t& /*state*/, const stepper_t& /*stepper*/,
0505                     const navigator_t& /*navigator*/, const result_type& result,
0506                     const Logger& /*logger*/) const {
0507       return (!result.result.ok() || result.finished);
0508     }
0509 
0510     /// @brief Kalman actor operation: reverse direction
0511     ///
0512     /// @tparam propagator_state_t is the type of Propagator state
0513     /// @tparam stepper_t Type of the stepper
0514     /// @tparam navigator_t Type of the navigator
0515     ///
0516     /// @param state is the mutable propagator state object
0517     /// @param stepper The stepper in use
0518     /// @param navigator The navigator in use
0519     /// @param result is the mutable result state object
0520     template <typename propagator_state_t, typename stepper_t,
0521               typename navigator_t>
0522     Result<void> reverse(propagator_state_t& state, const stepper_t& stepper,
0523                          const navigator_t& navigator,
0524                          result_type& result) const {
0525       // Check if there is a measurement on track
0526       if (result.lastMeasurementIndex ==
0527           Acts::MultiTrajectoryTraits::kInvalid) {
0528         ACTS_ERROR("No point to reverse for a track without measurements.");
0529         return KalmanFitterError::ReversePropagationFailed;
0530       }
0531 
0532       // Remember the navigation direction has been reversed
0533       result.reversed = true;
0534 
0535       // Reverse navigation direction
0536       state.options.direction = state.options.direction.invert();
0537 
0538       // Reset propagator options
0539       // TODO Not sure if reset of pathLimit during propagation makes any sense
0540       state.options.pathLimit =
0541           state.options.direction * std::abs(state.options.pathLimit);
0542 
0543       // Get the last measurement state and reset navigation&stepping state
0544       // based on information on this state
0545       auto st = result.fittedStates->getTrackState(result.lastMeasurementIndex);
0546 
0547       // Update the stepping state
0548       stepper.resetState(
0549           state.stepping, st.filtered(),
0550           reversedFilteringCovarianceScaling * st.filteredCovariance(),
0551           st.referenceSurface(), state.options.stepping.maxStepSize);
0552 
0553       // For the last measurement state, smoothed is filtered
0554       st.smoothed() = st.filtered();
0555       st.smoothedCovariance() = st.filteredCovariance();
0556       result.passedAgainSurfaces.push_back(&st.referenceSurface());
0557 
0558       // Reset navigation state
0559       // We do not need to specify a target here since this will be handled
0560       // separately in the KF actor
0561       auto navigationOptions = state.navigation.options;
0562       navigationOptions.startSurface = &st.referenceSurface();
0563       navigationOptions.targetSurface = nullptr;
0564       state.navigation = navigator.makeState(navigationOptions);
0565       navigator.initialize(state.navigation, stepper.position(state.stepping),
0566                            stepper.direction(state.stepping),
0567                            state.options.direction);
0568 
0569       // Update material effects for last measurement state in reversed
0570       // direction
0571       materialInteractor(navigator.currentSurface(state.navigation), state,
0572                          stepper, navigator, MaterialUpdateStage::FullUpdate);
0573 
0574       return Result<void>::success();
0575     }
0576 
0577     /// @brief Kalman actor operation: update
0578     ///
0579     /// @tparam propagator_state_t is the type of Propagator state
0580     /// @tparam stepper_t Type of the stepper
0581     /// @tparam navigator_t Type of the navigator
0582     ///
0583     /// @param surface The surface where the update happens
0584     /// @param state The mutable propagator state object
0585     /// @param stepper The stepper in use
0586     /// @param navigator The navigator in use
0587     /// @param result The mutable result state object
0588     template <typename propagator_state_t, typename stepper_t,
0589               typename navigator_t>
0590     Result<void> filter(const Surface* surface, propagator_state_t& state,
0591                         const stepper_t& stepper, const navigator_t& navigator,
0592                         result_type& result) const {
0593       const bool precedingMeasurementExists = result.measurementStates > 0;
0594       const bool surfaceIsSensitive =
0595           surface->associatedDetectorElement() != nullptr;
0596       const bool surfaceHasMaterial = surface->surfaceMaterial() != nullptr;
0597 
0598       // Try to find the surface in the measurement surfaces
0599       auto sourceLinkIt = inputMeasurements->find(surface->geometryId());
0600       if (sourceLinkIt != inputMeasurements->end()) {
0601         // Screen output message
0602         ACTS_VERBOSE("Measurement surface " << surface->geometryId()
0603                                             << " detected.");
0604         // Transport the covariance to the surface
0605         stepper.transportCovarianceToBound(state.stepping, *surface,
0606                                            freeToBoundCorrection);
0607 
0608         // Update state and stepper with pre material effects
0609         materialInteractor(surface, state, stepper, navigator,
0610                            MaterialUpdateStage::PreUpdate);
0611 
0612         // do the kalman update (no need to perform covTransport here, hence no
0613         // point in performing globalToLocal correction)
0614         auto trackStateProxyRes = detail::kalmanHandleMeasurement(
0615             *calibrationContext, state, stepper, extensions, *surface,
0616             sourceLinkIt->second, *result.fittedStates, result.lastTrackIndex,
0617             false, logger());
0618 
0619         if (!trackStateProxyRes.ok()) {
0620           return trackStateProxyRes.error();
0621         }
0622 
0623         const auto& trackStateProxy = *trackStateProxyRes;
0624         result.lastTrackIndex = trackStateProxy.index();
0625 
0626         // Update the stepper if it is not an outlier
0627         if (trackStateProxy.typeFlags().test(
0628                 Acts::TrackStateFlag::MeasurementFlag)) {
0629           // Update the stepping state with filtered parameters
0630           ACTS_VERBOSE("Filtering step successful, updated parameters are:\n"
0631                        << trackStateProxy.filtered().transpose());
0632           // update stepping state using filtered parameters after kalman
0633           stepper.update(state.stepping,
0634                          MultiTrajectoryHelpers::freeFiltered(
0635                              state.options.geoContext, trackStateProxy),
0636                          trackStateProxy.filtered(),
0637                          trackStateProxy.filteredCovariance(), *surface);
0638           // We count the state with measurement
0639           ++result.measurementStates;
0640         }
0641 
0642         // Update state and stepper with post material effects
0643         materialInteractor(surface, state, stepper, navigator,
0644                            MaterialUpdateStage::PostUpdate);
0645         // We count the processed state
0646         ++result.processedStates;
0647         // Update the number of holes count only when encountering a
0648         // measurement
0649         result.measurementHoles = result.missedActiveSurfaces.size();
0650         // Since we encountered a measurement update the lastMeasurementIndex to
0651         // the lastTrackIndex.
0652         result.lastMeasurementIndex = result.lastTrackIndex;
0653 
0654       } else if ((precedingMeasurementExists && surfaceIsSensitive) ||
0655                  surfaceHasMaterial) {
0656         // We only create track states here if there is already measurement
0657         // detected or if the surface has material (no holes before the first
0658         // measurement)
0659         auto trackStateProxyRes = detail::kalmanHandleNoMeasurement(
0660             state, stepper, *surface, *result.fittedStates,
0661             result.lastTrackIndex, true, logger(), precedingMeasurementExists,
0662             freeToBoundCorrection);
0663 
0664         if (!trackStateProxyRes.ok()) {
0665           return trackStateProxyRes.error();
0666         }
0667 
0668         const auto& trackStateProxy = *trackStateProxyRes;
0669         result.lastTrackIndex = trackStateProxy.index();
0670 
0671         if (trackStateProxy.typeFlags().test(TrackStateFlag::HoleFlag)) {
0672           // Count the missed surface
0673           result.missedActiveSurfaces.push_back(surface);
0674         }
0675 
0676         ++result.processedStates;
0677 
0678         // Update state and stepper with (possible) material effects
0679         materialInteractor(surface, state, stepper, navigator,
0680                            MaterialUpdateStage::FullUpdate);
0681       }
0682       return Result<void>::success();
0683     }
0684 
0685     /// @brief Kalman actor operation: update in reversed direction
0686     ///
0687     /// @tparam propagator_state_t is the type of Propagator state
0688     /// @tparam stepper_t Type of the stepper
0689     /// @tparam navigator_t Type of the navigator
0690     ///
0691     /// @param surface The surface where the update happens
0692     /// @param state The mutable propagator state object
0693     /// @param stepper The stepper in use
0694     /// @param navigator The navigator in use
0695     /// @param result The mutable result state object
0696     template <typename propagator_state_t, typename stepper_t,
0697               typename navigator_t>
0698     Result<void> reversedFilter(const Surface* surface,
0699                                 propagator_state_t& state,
0700                                 const stepper_t& stepper,
0701                                 const navigator_t& navigator,
0702                                 result_type& result) const {
0703       // Try to find the surface in the measurement surfaces
0704       auto sourceLinkIt = inputMeasurements->find(surface->geometryId());
0705       if (sourceLinkIt != inputMeasurements->end()) {
0706         // Screen output message
0707         ACTS_VERBOSE("Measurement surface "
0708                      << surface->geometryId()
0709                      << " detected in reversed propagation.");
0710 
0711         // No reversed filtering for last measurement state, but still update
0712         // with material effects
0713         if (result.reversed &&
0714             surface == navigator.startSurface(state.navigation)) {
0715           materialInteractor(surface, state, stepper, navigator,
0716                              MaterialUpdateStage::FullUpdate);
0717           return Result<void>::success();
0718         }
0719 
0720         // Transport the covariance to the surface
0721         stepper.transportCovarianceToBound(state.stepping, *surface,
0722                                            freeToBoundCorrection);
0723 
0724         // Update state and stepper with pre material effects
0725         materialInteractor(surface, state, stepper, navigator,
0726                            MaterialUpdateStage::PreUpdate);
0727 
0728         auto fittedStates = *result.fittedStates;
0729 
0730         // Add a <mask> TrackState entry multi trajectory. This allocates
0731         // storage for all components, which we will set later.
0732         TrackStatePropMask mask =
0733             TrackStatePropMask::Predicted | TrackStatePropMask::Filtered |
0734             TrackStatePropMask::Smoothed | TrackStatePropMask::Jacobian |
0735             TrackStatePropMask::Calibrated;
0736         const std::size_t currentTrackIndex = fittedStates.addTrackState(
0737             mask, Acts::MultiTrajectoryTraits::kInvalid);
0738 
0739         // now get track state proxy back
0740         typename traj_t::TrackStateProxy trackStateProxy =
0741             fittedStates.getTrackState(currentTrackIndex);
0742 
0743         // Set the trackStateProxy components with the state from the ongoing
0744         // propagation
0745         {
0746           trackStateProxy.setReferenceSurface(surface->getSharedPtr());
0747           // Bind the transported state to the current surface
0748           auto res = stepper.boundState(state.stepping, *surface, false,
0749                                         freeToBoundCorrection);
0750           if (!res.ok()) {
0751             return res.error();
0752           }
0753           const auto& [boundParams, jacobian, pathLength] = *res;
0754 
0755           // Fill the track state
0756           trackStateProxy.predicted() = boundParams.parameters();
0757           trackStateProxy.predictedCovariance() = state.stepping.cov;
0758 
0759           trackStateProxy.jacobian() = jacobian;
0760           trackStateProxy.pathLength() = pathLength;
0761         }
0762 
0763         // We have predicted parameters, so calibrate the uncalibrated input
0764         // measurement
0765         extensions.calibrator(state.geoContext, *calibrationContext,
0766                               sourceLinkIt->second, trackStateProxy);
0767 
0768         // If the update is successful, set covariance and
0769         auto updateRes =
0770             extensions.updater(state.geoContext, trackStateProxy, logger());
0771         if (!updateRes.ok()) {
0772           ACTS_ERROR("Backward update step failed: " << updateRes.error());
0773           return updateRes.error();
0774         } else {
0775           // Update the stepping state with filtered parameters
0776           ACTS_VERBOSE(
0777               "Backward filtering step successful, updated parameters are:\n"
0778               << trackStateProxy.filtered().transpose());
0779 
0780           // Fill the smoothed parameter for the existing track state
0781           result.fittedStates->applyBackwards(
0782               result.lastMeasurementIndex, [&](auto trackState) {
0783                 auto fSurface = &trackState.referenceSurface();
0784                 if (fSurface == surface) {
0785                   result.passedAgainSurfaces.push_back(surface);
0786                   trackState.smoothed() = trackStateProxy.filtered();
0787                   trackState.smoothedCovariance() =
0788                       trackStateProxy.filteredCovariance();
0789                   return false;
0790                 }
0791                 return true;
0792               });
0793 
0794           // update stepping state using filtered parameters after kalman
0795           // update We need to (re-)construct a BoundTrackParameters instance
0796           // here, which is a bit awkward.
0797           stepper.update(state.stepping,
0798                          MultiTrajectoryHelpers::freeFiltered(
0799                              state.options.geoContext, trackStateProxy),
0800                          trackStateProxy.filtered(),
0801                          trackStateProxy.filteredCovariance(), *surface);
0802 
0803           // Update state and stepper with post material effects
0804           materialInteractor(surface, state, stepper, navigator,
0805                              MaterialUpdateStage::PostUpdate);
0806         }
0807       } else if (surface->associatedDetectorElement() != nullptr ||
0808                  surface->surfaceMaterial() != nullptr) {
0809         // Transport covariance
0810         if (surface->associatedDetectorElement() != nullptr) {
0811           ACTS_VERBOSE("Detected hole on " << surface->geometryId()
0812                                            << " in reversed filtering");
0813           if (state.stepping.covTransport) {
0814             stepper.transportCovarianceToBound(state.stepping, *surface);
0815           }
0816         } else if (surface->surfaceMaterial() != nullptr) {
0817           ACTS_VERBOSE("Detected in-sensitive surface "
0818                        << surface->geometryId() << " in reversed filtering");
0819           if (state.stepping.covTransport) {
0820             stepper.transportCovarianceToCurvilinear(state.stepping);
0821           }
0822         }
0823         // Not creating bound state here, so need manually reinitialize
0824         // jacobian
0825         stepper.setIdentityJacobian(state.stepping);
0826         if (surface->surfaceMaterial() != nullptr) {
0827           // Update state and stepper with material effects
0828           materialInteractor(surface, state, stepper, navigator,
0829                              MaterialUpdateStage::FullUpdate);
0830         }
0831       }
0832 
0833       return Result<void>::success();
0834     }
0835 
0836     /// @brief Kalman actor operation: material interaction
0837     ///
0838     /// @tparam propagator_state_t is the type of Propagator state
0839     /// @tparam stepper_t Type of the stepper
0840     /// @tparam navigator_t Type of the navigator
0841     ///
0842     /// @param surface The surface where the material interaction happens
0843     /// @param state The mutable propagator state object
0844     /// @param stepper The stepper in use
0845     /// @param navigator The navigator in use
0846     /// @param updateStage The material update stage
0847     ///
0848     template <typename propagator_state_t, typename stepper_t,
0849               typename navigator_t>
0850     void materialInteractor(const Surface* surface, propagator_state_t& state,
0851                             const stepper_t& stepper,
0852                             const navigator_t& navigator,
0853                             const MaterialUpdateStage& updateStage) const {
0854       // Protect against null surface
0855       if (!surface) {
0856         ACTS_VERBOSE(
0857             "Surface is nullptr. Cannot be used for material interaction");
0858         return;
0859       }
0860 
0861       // Indicator if having material
0862       bool hasMaterial = false;
0863 
0864       if (surface && surface->surfaceMaterial()) {
0865         // Prepare relevant input particle properties
0866         detail::PointwiseMaterialInteraction interaction(surface, state,
0867                                                          stepper);
0868         // Evaluate the material properties
0869         if (interaction.evaluateMaterialSlab(state, navigator, updateStage)) {
0870           // Surface has material at this stage
0871           hasMaterial = true;
0872 
0873           // Evaluate the material effects
0874           interaction.evaluatePointwiseMaterialInteraction(multipleScattering,
0875                                                            energyLoss);
0876 
0877           // Screen out material effects info
0878           ACTS_VERBOSE("Material effects on surface: "
0879                        << surface->geometryId()
0880                        << " at update stage: " << updateStage << " are :");
0881           ACTS_VERBOSE("eLoss = "
0882                        << interaction.Eloss << ", "
0883                        << "variancePhi = " << interaction.variancePhi << ", "
0884                        << "varianceTheta = " << interaction.varianceTheta
0885                        << ", "
0886                        << "varianceQoverP = " << interaction.varianceQoverP);
0887 
0888           // Update the state and stepper with material effects
0889           interaction.updateState(state, stepper, addNoise);
0890         }
0891       }
0892 
0893       if (!hasMaterial) {
0894         // Screen out message
0895         ACTS_VERBOSE("No material effects on surface: " << surface->geometryId()
0896                                                         << " at update stage: "
0897                                                         << updateStage);
0898       }
0899     }
0900 
0901     /// @brief Kalman actor operation: finalize
0902     ///
0903     /// @tparam propagator_state_t is the type of Propagator state
0904     /// @tparam stepper_t Type of the stepper
0905     /// @tparam navigator_t Type of the navigator
0906     ///
0907     /// @param state is the mutable propagator state object
0908     /// @param stepper The stepper in use
0909     /// @param navigator The navigator in use
0910     /// @param result is the mutable result state object
0911     template <typename propagator_state_t, typename stepper_t,
0912               typename navigator_t>
0913     Result<void> finalize(propagator_state_t& state, const stepper_t& stepper,
0914                           const navigator_t& navigator,
0915                           result_type& result) const {
0916       // Remember you smoothed the track states
0917       result.smoothed = true;
0918 
0919       // Get the indices of the first states (can be either a measurement or
0920       // material);
0921       std::size_t firstStateIndex = result.lastMeasurementIndex;
0922       // Count track states to be smoothed
0923       std::size_t nStates = 0;
0924       result.fittedStates->applyBackwards(
0925           result.lastMeasurementIndex, [&](auto st) {
0926             bool isMeasurement =
0927                 st.typeFlags().test(TrackStateFlag::MeasurementFlag);
0928             bool isOutlier = st.typeFlags().test(TrackStateFlag::OutlierFlag);
0929             // We are excluding non measurement states and outlier here. Those
0930             // can decrease resolution because only the smoothing corrected the
0931             // very first prediction as filtering is not possible.
0932             if (isMeasurement && !isOutlier) {
0933               firstStateIndex = st.index();
0934             }
0935             nStates++;
0936           });
0937       // Return error if the track has no measurement states (but this should
0938       // not happen)
0939       if (nStates == 0) {
0940         ACTS_ERROR("Smoothing for a track without measurements.");
0941         return KalmanFitterError::SmoothFailed;
0942       }
0943       // Screen output for debugging
0944       ACTS_VERBOSE("Apply smoothing on " << nStates
0945                                          << " filtered track states.");
0946 
0947       // Smooth the track states
0948       auto smoothRes =
0949           extensions.smoother(state.geoContext, *result.fittedStates,
0950                               result.lastMeasurementIndex, logger());
0951       if (!smoothRes.ok()) {
0952         ACTS_ERROR("Smoothing step failed: " << smoothRes.error());
0953         return smoothRes.error();
0954       }
0955 
0956       // Return in case no target surface
0957       if (targetReached.surface == nullptr) {
0958         return Result<void>::success();
0959       }
0960 
0961       // Obtain the smoothed parameters at first/last measurement state
0962       auto firstCreatedState =
0963           result.fittedStates->getTrackState(firstStateIndex);
0964       auto lastCreatedMeasurement =
0965           result.fittedStates->getTrackState(result.lastMeasurementIndex);
0966 
0967       // Lambda to get the intersection of the free params on the target surface
0968       auto target = [&](const FreeVector& freeVector) -> SurfaceIntersection {
0969         return targetReached.surface
0970             ->intersect(
0971                 state.geoContext, freeVector.segment<3>(eFreePos0),
0972                 state.options.direction * freeVector.segment<3>(eFreeDir0),
0973                 BoundaryTolerance::None(), state.options.surfaceTolerance)
0974             .closest();
0975       };
0976 
0977       // The smoothed free params at the first/last measurement state.
0978       // (the first state can also be a material state)
0979       auto firstParams = MultiTrajectoryHelpers::freeSmoothed(
0980           state.options.geoContext, firstCreatedState);
0981       auto lastParams = MultiTrajectoryHelpers::freeSmoothed(
0982           state.options.geoContext, lastCreatedMeasurement);
0983       // Get the intersections of the smoothed free parameters with the target
0984       // surface
0985       const auto firstIntersection = target(firstParams);
0986       const auto lastIntersection = target(lastParams);
0987 
0988       // Update the stepping parameters - in order to progress to destination.
0989       // At the same time, reverse navigation direction for further stepping if
0990       // necessary.
0991       // @note The stepping parameters is updated to the smoothed parameters at
0992       // either the first measurement state or the last measurement state. It
0993       // assumes the target surface is not within the first and the last
0994       // smoothed measurement state. Also, whether the intersection is on
0995       // surface is not checked here.
0996       bool useFirstTrackState = true;
0997       switch (targetSurfaceStrategy) {
0998         case KalmanFitterTargetSurfaceStrategy::first:
0999           useFirstTrackState = true;
1000           break;
1001         case KalmanFitterTargetSurfaceStrategy::last:
1002           useFirstTrackState = false;
1003           break;
1004         case KalmanFitterTargetSurfaceStrategy::firstOrLast:
1005           useFirstTrackState = std::abs(firstIntersection.pathLength()) <=
1006                                std::abs(lastIntersection.pathLength());
1007           break;
1008         default:
1009           ACTS_ERROR("Unknown target surface strategy");
1010           return KalmanFitterError::SmoothFailed;
1011       }
1012       bool reverseDirection = false;
1013       if (useFirstTrackState) {
1014         stepper.resetState(state.stepping, firstCreatedState.smoothed(),
1015                            firstCreatedState.smoothedCovariance(),
1016                            firstCreatedState.referenceSurface(),
1017                            state.options.stepping.maxStepSize);
1018         reverseDirection = firstIntersection.pathLength() < 0;
1019       } else {
1020         stepper.resetState(state.stepping, lastCreatedMeasurement.smoothed(),
1021                            lastCreatedMeasurement.smoothedCovariance(),
1022                            lastCreatedMeasurement.referenceSurface(),
1023                            state.options.stepping.maxStepSize);
1024         reverseDirection = lastIntersection.pathLength() < 0;
1025       }
1026       // Reverse the navigation direction if necessary
1027       if (reverseDirection) {
1028         ACTS_VERBOSE(
1029             "Reverse navigation direction after smoothing for reaching the "
1030             "target surface");
1031         state.options.direction = state.options.direction.invert();
1032       }
1033       const auto& surface = useFirstTrackState
1034                                 ? firstCreatedState.referenceSurface()
1035                                 : lastCreatedMeasurement.referenceSurface();
1036 
1037       ACTS_VERBOSE(
1038           "Smoothing successful, updating stepping state to smoothed "
1039           "parameters at surface "
1040           << surface.geometryId() << ". Prepared to reach the target surface.");
1041 
1042       // Reset the navigation state to enable propagation towards the target
1043       // surface
1044       // Set targetSurface to nullptr as it is handled manually in the actor
1045       auto navigationOptions = state.navigation.options;
1046       navigationOptions.startSurface = &surface;
1047       navigationOptions.targetSurface = nullptr;
1048       state.navigation = navigator.makeState(navigationOptions);
1049       navigator.initialize(state.navigation, stepper.position(state.stepping),
1050                            stepper.direction(state.stepping),
1051                            state.options.direction);
1052 
1053       return Result<void>::success();
1054     }
1055   };
1056 
1057  public:
1058   /// Fit implementation of the forward filter, calls the
1059   /// the filter and smoother/reversed filter
1060   ///
1061   /// @tparam source_link_iterator_t Iterator type used to pass source links
1062   /// @tparam start_parameters_t Type of the initial parameters
1063   /// @tparam parameters_t Type of parameters used for local parameters
1064   /// @tparam track_container_t Type of the track container
1065   ///
1066   /// @param it Begin iterator for the fittable uncalibrated measurements
1067   /// @param end End iterator for the fittable uncalibrated measurements
1068   /// @param sParameters The initial track parameters
1069   /// @param kfOptions KalmanOptions steering the fit
1070   /// @param trackContainer Input track container storage to append into
1071   /// @note The input measurements are given in the form of @c SourceLink s.
1072   /// It's the calibrators job to turn them into calibrated measurements used in
1073   /// the fit.
1074   ///
1075   /// @return the output as an output track
1076   template <typename source_link_iterator_t, typename start_parameters_t,
1077             typename parameters_t = BoundTrackParameters,
1078             TrackContainerFrontend track_container_t>
1079   Result<typename track_container_t::TrackProxy> fit(
1080       source_link_iterator_t it, source_link_iterator_t end,
1081       const start_parameters_t& sParameters,
1082       const KalmanFitterOptions<traj_t>& kfOptions,
1083       track_container_t& trackContainer) const
1084     requires(!isDirectNavigator)
1085   {
1086     // To be able to find measurements later, we put them into a map
1087     // We need to copy input SourceLinks anyway, so the map can own them.
1088     ACTS_VERBOSE("Preparing " << std::distance(it, end)
1089                               << " input measurements");
1090     std::map<GeometryIdentifier, SourceLink> inputMeasurements;
1091     // for (const auto& sl : sourceLinks) {
1092     for (; it != end; ++it) {
1093       SourceLink sl = *it;
1094       const Surface* surface = kfOptions.extensions.surfaceAccessor(sl);
1095       // @TODO: This can probably change over to surface pointers as keys
1096       auto geoId = surface->geometryId();
1097       inputMeasurements.emplace(geoId, std::move(sl));
1098     }
1099 
1100     // Create the ActorList
1101     using KalmanActor = Actor<parameters_t>;
1102 
1103     using KalmanResult = typename KalmanActor::result_type;
1104     using Actors = ActorList<KalmanActor>;
1105     using PropagatorOptions = typename propagator_t::template Options<Actors>;
1106 
1107     // Create relevant options for the propagation options
1108     PropagatorOptions propagatorOptions(kfOptions.geoContext,
1109                                         kfOptions.magFieldContext);
1110 
1111     // Set the trivial propagator options
1112     propagatorOptions.setPlainOptions(kfOptions.propagatorPlainOptions);
1113 
1114     // Add the measurement surface as external surface to navigator.
1115     // We will try to hit those surface by ignoring boundary checks.
1116     for (const auto& [surfaceId, _] : inputMeasurements) {
1117       propagatorOptions.navigation.insertExternalSurface(surfaceId);
1118     }
1119 
1120     // Catch the actor and set the measurements
1121     auto& kalmanActor = propagatorOptions.actorList.template get<KalmanActor>();
1122     kalmanActor.inputMeasurements = &inputMeasurements;
1123     kalmanActor.targetReached.surface = kfOptions.referenceSurface;
1124     kalmanActor.targetSurfaceStrategy = kfOptions.referenceSurfaceStrategy;
1125     kalmanActor.multipleScattering = kfOptions.multipleScattering;
1126     kalmanActor.energyLoss = kfOptions.energyLoss;
1127     kalmanActor.reversedFiltering = kfOptions.reversedFiltering;
1128     kalmanActor.reversedFilteringCovarianceScaling =
1129         kfOptions.reversedFilteringCovarianceScaling;
1130     kalmanActor.freeToBoundCorrection = kfOptions.freeToBoundCorrection;
1131     kalmanActor.calibrationContext = &kfOptions.calibrationContext.get();
1132     kalmanActor.extensions = kfOptions.extensions;
1133     kalmanActor.actorLogger = m_actorLogger.get();
1134 
1135     return fit_impl<start_parameters_t, PropagatorOptions, KalmanResult,
1136                     track_container_t>(sParameters, propagatorOptions,
1137                                        trackContainer);
1138   }
1139 
1140   /// Fit implementation of the forward filter, calls the
1141   /// the filter and smoother/reversed filter
1142   ///
1143   /// @tparam source_link_iterator_t Iterator type used to pass source links
1144   /// @tparam start_parameters_t Type of the initial parameters
1145   /// @tparam parameters_t Type of parameters used for local parameters
1146   /// @tparam track_container_t Type of the track container
1147   ///
1148   /// @param it Begin iterator for the fittable uncalibrated measurements
1149   /// @param end End iterator for the fittable uncalibrated measurements
1150   /// @param sParameters The initial track parameters
1151   /// @param kfOptions KalmanOptions steering the fit
1152   /// @param sSequence surface sequence used to initialize a DirectNavigator
1153   /// @param trackContainer Input track container storage to append into
1154   /// @note The input measurements are given in the form of @c SourceLinks.
1155   /// It's
1156   /// @c calibrator_t's job to turn them into calibrated measurements used in
1157   /// the fit.
1158   ///
1159   /// @return the output as an output track
1160   template <typename source_link_iterator_t, typename start_parameters_t,
1161             typename parameters_t = BoundTrackParameters,
1162             TrackContainerFrontend track_container_t>
1163   Result<typename track_container_t::TrackProxy> fit(
1164       source_link_iterator_t it, source_link_iterator_t end,
1165       const start_parameters_t& sParameters,
1166       const KalmanFitterOptions<traj_t>& kfOptions,
1167       const std::vector<const Surface*>& sSequence,
1168       track_container_t& trackContainer) const
1169     requires(isDirectNavigator)
1170   {
1171     // To be able to find measurements later, we put them into a map
1172     // We need to copy input SourceLinks anyway, so the map can own them.
1173     ACTS_VERBOSE("Preparing " << std::distance(it, end)
1174                               << " input measurements");
1175     std::map<GeometryIdentifier, SourceLink> inputMeasurements;
1176     for (; it != end; ++it) {
1177       SourceLink sl = *it;
1178       const Surface* surface = kfOptions.extensions.surfaceAccessor(sl);
1179       // @TODO: This can probably change over to surface pointers as keys
1180       auto geoId = surface->geometryId();
1181       inputMeasurements.emplace(geoId, std::move(sl));
1182     }
1183 
1184     // Create the ActorList
1185     using KalmanActor = Actor<parameters_t>;
1186 
1187     using KalmanResult = typename KalmanActor::result_type;
1188     using Actors = ActorList<KalmanActor>;
1189     using PropagatorOptions = typename propagator_t::template Options<Actors>;
1190 
1191     // Create relevant options for the propagation options
1192     PropagatorOptions propagatorOptions(kfOptions.geoContext,
1193                                         kfOptions.magFieldContext);
1194 
1195     // Set the trivial propagator options
1196     propagatorOptions.setPlainOptions(kfOptions.propagatorPlainOptions);
1197 
1198     // Catch the actor and set the measurements
1199     auto& kalmanActor = propagatorOptions.actorList.template get<KalmanActor>();
1200     kalmanActor.inputMeasurements = &inputMeasurements;
1201     kalmanActor.targetReached.surface = kfOptions.referenceSurface;
1202     kalmanActor.targetSurfaceStrategy = kfOptions.referenceSurfaceStrategy;
1203     kalmanActor.multipleScattering = kfOptions.multipleScattering;
1204     kalmanActor.energyLoss = kfOptions.energyLoss;
1205     kalmanActor.reversedFiltering = kfOptions.reversedFiltering;
1206     kalmanActor.reversedFilteringCovarianceScaling =
1207         kfOptions.reversedFilteringCovarianceScaling;
1208     kalmanActor.extensions = kfOptions.extensions;
1209     kalmanActor.actorLogger = m_actorLogger.get();
1210 
1211     // Set the surface sequence
1212     propagatorOptions.navigation.surfaces = sSequence;
1213 
1214     return fit_impl<start_parameters_t, PropagatorOptions, KalmanResult,
1215                     track_container_t>(sParameters, propagatorOptions,
1216                                        trackContainer);
1217   }
1218 
1219  private:
1220   /// Common fit implementation
1221   ///
1222   /// @tparam start_parameters_t Type of the initial parameters
1223   /// @tparam actor_list_t Type of the actor list
1224   /// @tparam aborter_list_t Type of the abort list
1225   /// @tparam kalman_result_t Type of the KF result
1226   /// @tparam track_container_t Type of the track container
1227   ///
1228   /// @param sParameters The initial track parameters
1229   /// @param propagatorOptions The Propagator Options
1230   /// @param trackContainer Input track container storage to append into
1231   ///
1232   /// @return the output as an output track
1233   template <typename start_parameters_t, typename propagator_options_t,
1234             typename kalman_result_t, TrackContainerFrontend track_container_t>
1235   auto fit_impl(const start_parameters_t& sParameters,
1236                 const propagator_options_t& propagatorOptions,
1237                 track_container_t& trackContainer) const
1238       -> Result<typename track_container_t::TrackProxy> {
1239     auto propagatorState =
1240         m_propagator.makeState(sParameters, propagatorOptions);
1241 
1242     auto& kalmanResult =
1243         propagatorState.template get<KalmanFitterResult<traj_t>>();
1244     kalmanResult.fittedStates = &trackContainer.trackStateContainer();
1245 
1246     // Run the fitter
1247     auto result = m_propagator.propagate(propagatorState);
1248 
1249     if (!result.ok()) {
1250       ACTS_ERROR("Propagation failed: " << result.error());
1251       return result.error();
1252     }
1253 
1254     /// It could happen that the fit ends in zero measurement states.
1255     /// The result gets meaningless so such case is regarded as fit failure.
1256     if (kalmanResult.result.ok() && !kalmanResult.measurementStates) {
1257       kalmanResult.result = Result<void>(KalmanFitterError::NoMeasurementFound);
1258     }
1259 
1260     if (!kalmanResult.result.ok()) {
1261       ACTS_ERROR("KalmanFilter failed: "
1262                  << kalmanResult.result.error() << ", "
1263                  << kalmanResult.result.error().message());
1264       return kalmanResult.result.error();
1265     }
1266 
1267     auto track = trackContainer.makeTrack();
1268     track.tipIndex() = kalmanResult.lastMeasurementIndex;
1269     if (kalmanResult.fittedParameters) {
1270       const auto& params = kalmanResult.fittedParameters.value();
1271       track.parameters() = params.parameters();
1272       track.covariance() = params.covariance().value();
1273       track.setReferenceSurface(params.referenceSurface().getSharedPtr());
1274     }
1275 
1276     calculateTrackQuantities(track);
1277 
1278     if (trackContainer.hasColumn(hashString("smoothed"))) {
1279       track.template component<bool, hashString("smoothed")>() =
1280           kalmanResult.smoothed;
1281     }
1282 
1283     if (trackContainer.hasColumn(hashString("reversed"))) {
1284       track.template component<bool, hashString("reversed")>() =
1285           kalmanResult.reversed;
1286     }
1287 
1288     // Return the converted Track
1289     return track;
1290   }
1291 };
1292 
1293 }  // namespace Acts