KalmanFitter.hpp

Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file KalmanFitter.hpp

// This file is part of the Acts project.
//
// Copyright (C) 2016-2019 CERN for the benefit of the Acts project
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#pragma once

// Workaround for building on clang+libstdc++
#include "Acts/Utilities/detail/ReferenceWrapperAnyCompat.hpp"

#include "Acts/EventData/Measurement.hpp"
#include "Acts/EventData/MeasurementHelpers.hpp"
#include "Acts/EventData/MultiTrajectory.hpp"
#include "Acts/EventData/MultiTrajectoryHelpers.hpp"
#include "Acts/EventData/TrackParameters.hpp"
#include "Acts/Geometry/GeometryContext.hpp"
#include "Acts/MagneticField/MagneticFieldContext.hpp"
#include "Acts/Material/MaterialSlab.hpp"
#include "Acts/Propagator/AbortList.hpp"
#include "Acts/Propagator/ActionList.hpp"
#include "Acts/Propagator/ConstrainedStep.hpp"
#include "Acts/Propagator/DirectNavigator.hpp"
#include "Acts/Propagator/Navigator.hpp"
#include "Acts/Propagator/Propagator.hpp"
#include "Acts/Propagator/StandardAborters.hpp"
#include "Acts/Propagator/detail/PointwiseMaterialInteraction.hpp"
#include "Acts/TrackFitting/KalmanFitterError.hpp"
#include "Acts/TrackFitting/detail/VoidKalmanComponents.hpp"
#include "Acts/Utilities/CalibrationContext.hpp"
#include "Acts/Utilities/Definitions.hpp"
#include "Acts/Utilities/Logger.hpp"
#include "Acts/Utilities/Result.hpp"

#include <functional>
#include <map>
#include <memory>

namespace Acts {

template <typename outlier_finder_t = VoidOutlierFinder>
struct KalmanFitterOptions {
  // Broadcast the outlier finder type
  using OutlierFinder = outlier_finder_t;

  KalmanFitterOptions() = delete;

  KalmanFitterOptions(std::reference_wrapper<const GeometryContext> gctx,
                      std::reference_wrapper<const MagneticFieldContext> mctx,
                      std::reference_wrapper<const CalibrationContext> cctx,
                      const OutlierFinder& outlierFinder_,
                      LoggerWrapper logger_,
                      const PropagatorPlainOptions& pOptions,
                      const Surface* rSurface = nullptr,
                      bool mScattering = true, bool eLoss = true,
                      bool bwdFiltering = false)
      : geoContext(gctx),
        magFieldContext(mctx),
        calibrationContext(cctx),
        outlierFinder(outlierFinder_),
        propagatorPlainOptions(pOptions),
        referenceSurface(rSurface),
        multipleScattering(mScattering),
        energyLoss(eLoss),
        backwardFiltering(bwdFiltering),
        logger(logger_) {}

  std::reference_wrapper<const GeometryContext> geoContext;
  std::reference_wrapper<const MagneticFieldContext> magFieldContext;
  std::reference_wrapper<const CalibrationContext> calibrationContext;

  OutlierFinder outlierFinder;

  PropagatorPlainOptions propagatorPlainOptions;

  const Surface* referenceSurface = nullptr;

  bool multipleScattering = true;

  bool energyLoss = true;

  bool backwardFiltering = false;

  LoggerWrapper logger;
};

template <typename source_link_t>
struct KalmanFitterResult {
  // Fitted states that the actor has handled.
  MultiTrajectory<source_link_t> fittedStates;

  // This is the index of the 'tip' of the track stored in multitrajectory.
  // Since this KF only stores one trajectory, it is unambiguous.
  // SIZE_MAX is the start of a trajectory.
  size_t trackTip = SIZE_MAX;

  // The optional Parameters at the provided surface
  std::optional<BoundTrackParameters> fittedParameters;

  // Counter for states with measurements
  size_t measurementStates = 0;

  // Counter for handled states
  size_t processedStates = 0;

  // Indicator if smoothing has been done.
  bool smoothed = false;

  // Indicator if the propagation state has been reset
  bool reset = false;

  // Indicator if track fitting has been done
  bool finished = false;

  // Measurement surfaces without hits
  std::vector<const Surface*> missedActiveSurfaces;

  // Indicator if forward filtering has been done
  bool forwardFiltered = false;

  // Measurement surfaces handled in both forward and backward filtering
  std::vector<const Surface*> passedAgainSurfaces;

  Result<void> result{Result<void>::success()};
};

template <typename propagator_t, typename updater_t = VoidKalmanUpdater,
          typename smoother_t = VoidKalmanSmoother,
          typename outlier_finder_t = VoidOutlierFinder,
          typename calibrator_t = VoidMeasurementCalibrator>
class KalmanFitter {
 public:
  using MeasurementSurfaces = std::multimap<const Layer*, const Surface*>;
  using KalmanNavigator = typename propagator_t::Navigator;

  static constexpr bool isDirectNavigator =
      std::is_same<KalmanNavigator, DirectNavigator>::value;

  KalmanFitter() = delete;

  KalmanFitter(propagator_t pPropagator)
      : m_propagator(std::move(pPropagator)) {}

 private:
  propagator_t m_propagator;

  template <typename source_link_t, typename parameters_t>
  class Actor {
   public:
    using result_type = KalmanFitterResult<source_link_t>;

    const Surface* targetSurface = nullptr;

    std::map<const Surface*, source_link_t> inputMeasurements;

    bool multipleScattering = true;

    bool energyLoss = true;

    bool backwardFiltering = false;

    template <typename propagator_state_t, typename stepper_t>
    void operator()(propagator_state_t& state, const stepper_t& stepper,
                    result_type& result) const {
      const auto& logger = state.options.logger;

      if (result.finished) {
        return;
      }

      ACTS_VERBOSE("KalmanFitter step");

      // This following is added due to the fact that the navigation
      // reinitialization in reverse call cannot guarantee the navigator to
      // target for extra layers in the backward-propagation starting volume.
      // Currently, manually set navigation stage to allow for targeting layers
      // after all the surfaces on the backward-propagation starting layer has
      // been processed. Otherwise, the navigation stage will be
      // Stage::boundaryTarget after navigator status call which means the extra
      // layers on the backward-propagation starting volume won't be targeted.
      // @Todo: Let the navigator do all the re-initialization
      if constexpr (not isDirectNavigator) {
    if (result.reset and state.navigation.navSurfaceIter ==
        state.navigation.navSurfaces.end()) {
      // So the navigator target call will target layers
      
      state.navigation.navigationStage = 
        KalmanNavigator::Stage::layerTarget;
      // We only do this after the backward-propagation 
      // starting layer has been processed
      result.reset = false;
    }
  }
      // Update:
      // - Waiting for a current surface
      auto surface = state.navigation.currentSurface;
      if (surface != nullptr) {
        // Check if the surface is in the measurement map
        // -> Get the measurement / calibrate
        // -> Create the predicted state
        // -> Perform the kalman update
        // -> Check outlier behavior
        // -> Fill strack state information & update stepper information if
        // non-outlier
        if (state.stepping.navDir == forward and not result.smoothed and
            not result.forwardFiltered) {
          ACTS_VERBOSE("Perform forward filter step with surface ");
    ACTS_VERBOSE(surface->geometryId());
          auto res = filter(surface, state, stepper, result);
          if (!res.ok()) {
            ACTS_ERROR("Error in forward filter: " << res.error());
            result.result = res.error();
          }
        } else if (state.stepping.navDir == backward and
                   result.forwardFiltered) {
          ACTS_VERBOSE("Perform backward filter step");
          auto res = backwardFilter(surface, state, stepper, result);
          if (!res.ok()) {
            ACTS_ERROR("Error in backward filter: " << res.error());
            result.result = res.error();
          }
        }
      }

      // Finalization:
      // when all track states have been handled or the navigation is breaked,
      // reset navigation&stepping before run backward filtering or
      // proceed to run smoothing
      if (state.stepping.navDir == forward) {
        if (result.measurementStates == inputMeasurements.size() or
            (result.measurementStates > 0 and
             state.navigation.navigationBreak)) {
          if (backwardFiltering and not result.forwardFiltered) {
            ACTS_VERBOSE("Forward filtering done");
            result.forwardFiltered = true;
            // Start to run backward filtering:
            // Reverse navigation direction and reset navigation and stepping
            // state to last measurement
            ACTS_VERBOSE("Reverse navigation direction.");
            reverse(state, stepper, result);
          } else if (not result.smoothed) {
            // --> Search the starting state to run the smoothing
            // --> Call the smoothing
            // --> Set a stop condition when all track states have been
            // handled
            ACTS_VERBOSE("Finalize/run smoothing");
            auto res = finalize(state, stepper, result);
            if (!res.ok()) {
              ACTS_ERROR("Error in finalize: " << res.error());
              result.result = res.error();
            }
          }
        }
      }

      // Post-finalization:
      // - Progress to target/reference surface and built the final track
      // parameters
      if (result.smoothed or state.stepping.navDir == backward) {
        if (targetSurface == nullptr) {
          // If no target surface provided:
          // -> Return an error when using backward filtering mode
          // -> Fitting is finished here
          if (backwardFiltering) {
            ACTS_ERROR(
                "The target surface needed for aborting backward propagation "
                "is not provided");
            result.result =
                Result<void>(KalmanFitterError::BackwardUpdateFailed);
          } else {
            ACTS_VERBOSE(
                "No target surface set. Completing without fitted track "
                "parameter");
            // Remember the track fitting is done
            result.finished = true;
          }
        } else if (targetReached(state, stepper, *targetSurface)) {
          ACTS_VERBOSE("Completing with fitted track parameter");
          // Transport & bind the parameter to the final surface
          auto fittedState = stepper.boundState(state.stepping, *targetSurface);
          // Assign the fitted parameters
          result.fittedParameters = std::get<BoundTrackParameters>(fittedState);

          // Reset smoothed status of states missed in backward filtering
          if (backwardFiltering) {
            result.fittedStates.applyBackwards(
                result.trackTip, [&](auto trackState) {
                  auto fSurface = &trackState.referenceSurface();
                  auto surface_it = std::find_if(
                      result.passedAgainSurfaces.begin(),
                      result.passedAgainSurfaces.end(),
                      [=](const Surface* s) { return s == fSurface; });
                  if (surface_it == result.passedAgainSurfaces.end()) {
                    // If backward filtering missed this surface, then there is
                    // no smoothed parameter
                    trackState.data().ismoothed =
                        detail_lt::IndexData::kInvalid;
                  }
                });
          }
          // Remember the track fitting is done
          result.finished = true;
        }
      }
    }

    template <typename propagator_state_t, typename stepper_t>
    void reverse(propagator_state_t& state, stepper_t& stepper,
                 result_type& result) const {
      // Remember the navigation direciton has been reserved
      result.reset = true;

      // Reset propagator options
      state.options.maxStepSize = -1.0 * state.options.maxStepSize;
      // Not sure if reset of pathLimit during propagation makes any sense
      state.options.pathLimit = -1.0 * state.options.pathLimit;

      // Reset stepping&navigation state using last measurement track state on
      // sensitive surface
      state.navigation = typename propagator_t::NavigatorState();
      result.fittedStates.applyBackwards(result.trackTip, [&](auto st) {
        if (st.typeFlags().test(Acts::TrackStateFlag::MeasurementFlag)) {
          // Set the navigation state
          state.navigation.startSurface = &st.referenceSurface();
          if (state.navigation.startSurface->associatedLayer() != nullptr) {
      state.navigation.startLayer =
        state.navigation.startSurface->associatedLayer();
         
          }
          state.navigation.startVolume =
              state.navigation.startLayer->trackingVolume();
          state.navigation.targetSurface = targetSurface;
          state.navigation.currentSurface = state.navigation.startSurface;
          state.navigation.currentVolume = state.navigation.startVolume;

          // Update the stepping state
          stepper.resetState(state.stepping, st.filtered(),
                             st.filteredCovariance(), st.referenceSurface(),
                             backward, state.options.maxStepSize);

          // For the last measurement state, smoothed is filtered
          st.smoothed() = st.filtered();
          st.smoothedCovariance() = st.filteredCovariance();
          result.passedAgainSurfaces.push_back(&st.referenceSurface());

          // Update material effects for last measurement state in backward
          // direction
          materialInteractor(state.navigation.currentSurface, state, stepper);

          return false;  // abort execution
        }
        return true;  // continue execution
      });
    }

    template <typename propagator_state_t, typename stepper_t>
    Result<void> filter(const Surface* surface, propagator_state_t& state,
                        const stepper_t& stepper, result_type& result) const {
      const auto& logger = state.options.logger;
      // Try to find the surface in the measurement surfaces
      auto sourcelink_it = inputMeasurements.find(surface);
      if(sourcelink_it != inputMeasurements.end()){
      }
      else
  ACTS_VERBOSE("Couldn't find source link in measurement list");

      if (sourcelink_it != inputMeasurements.end()) {
        // Screen output message
        ACTS_VERBOSE("Measurement surface " << surface->geometryId()
                                            << " detected.");

        // Transport & bind the state to the current surface
        auto [boundParams, jacobian, pathLength] =
            stepper.boundState(state.stepping, *surface);

        // Update state and stepper with pre material effects
        materialInteractor(surface, state, stepper, preUpdate);

        // add a full TrackState entry multi trajectory
        // (this allocates storage for all components, we will set them later)
        result.trackTip = result.fittedStates.addTrackState(
            TrackStatePropMask::All, result.trackTip);

        // now get track state proxy back
        auto trackStateProxy =
            result.fittedStates.getTrackState(result.trackTip);

        // assign the source link to the track state
        trackStateProxy.uncalibrated() = sourcelink_it->second;

        // Fill the track state
        trackStateProxy.predicted() = std::move(boundParams.parameters());
        trackStateProxy.predictedCovariance() =
            std::move(*boundParams.covariance());
        trackStateProxy.jacobian() = std::move(jacobian);
        trackStateProxy.pathLength() = std::move(pathLength);

        // We have predicted parameters, so calibrate the uncalibrated input
        // measuerement
        std::visit(
            [&](const auto& calibrated) {
              trackStateProxy.setCalibrated(calibrated);
            },
            m_calibrator(trackStateProxy.uncalibrated(),
                         trackStateProxy.predicted()));

        // Get and set the type flags
        auto& typeFlags = trackStateProxy.typeFlags();
        typeFlags.set(TrackStateFlag::MaterialFlag);
        typeFlags.set(TrackStateFlag::ParameterFlag);

        // Check if the state is an outlier.
        // If not, run Kalman update, tag it as a
        // measurement and update the stepping state. Otherwise, just tag it as
        // an outlier
        if (not m_outlierFinder(trackStateProxy)) {
          // Run Kalman update
          auto updateRes =
              m_updater(state.geoContext, trackStateProxy, forward);
          if (!updateRes.ok()) {
            ACTS_ERROR("Update step failed: " << updateRes.error());
            return updateRes.error();
          }
          // Set the measurement type flag
          typeFlags.set(TrackStateFlag::MeasurementFlag);
          // Update the stepping state with filtered parameters
          ACTS_VERBOSE("Filtering step successful, updated parameters are : \n"
                       << trackStateProxy.filtered().transpose());
          // update stepping state using filtered parameters after kalman
          stepper.update(state.stepping,
                         MultiTrajectoryHelpers::freeFiltered(
                             state.options.geoContext, trackStateProxy),
                         trackStateProxy.filteredCovariance());
          // We count the state with measurement
          ++result.measurementStates;
        } else {
          ACTS_VERBOSE(
              "Filtering step successful. But measurement is deterimined "
              "to "
              "be an outlier. Stepping state is not updated.")
          // Set the outlier type flag
          typeFlags.set(TrackStateFlag::OutlierFlag);
        }

        // Update state and stepper with post material effects
        materialInteractor(surface, state, stepper, postUpdate);
        // We count the processed state
        ++result.processedStates;
      } else if (surface->surfaceMaterial() != nullptr) {
        // We only create track states here if there is already measurement
        // detected
        if (result.measurementStates > 0) {
          // No source links on surface, add either hole or passive material
          // TrackState entry multi trajectory. No storage allocation for
          // uncalibrated/calibrated measurement and filtered parameter
          result.trackTip = result.fittedStates.addTrackState(
              ~(TrackStatePropMask::Uncalibrated |
                TrackStatePropMask::Calibrated | TrackStatePropMask::Filtered),
              result.trackTip);

          // now get track state proxy back
          auto trackStateProxy =
              result.fittedStates.getTrackState(result.trackTip);

          // Set the surface
          trackStateProxy.setReferenceSurface(surface->getSharedPtr());

          // Set the track state flags
          auto& typeFlags = trackStateProxy.typeFlags();
          typeFlags.set(TrackStateFlag::MaterialFlag);
          typeFlags.set(TrackStateFlag::ParameterFlag);

          if (surface->associatedDetectorElement() != nullptr) {
            ACTS_VERBOSE("Detected hole on " << surface->geometryId());
            // If the surface is sensitive, set the hole type flag
            typeFlags.set(TrackStateFlag::HoleFlag);

            // Count the missed surface
            result.missedActiveSurfaces.push_back(surface);

            // Transport & bind the state to the current surface
            auto [boundParams, jacobian, pathLength] =
                stepper.boundState(state.stepping, *surface);

            // Fill the track state
            trackStateProxy.predicted() = std::move(boundParams.parameters());
            trackStateProxy.predictedCovariance() =
                std::move(*boundParams.covariance());
            trackStateProxy.jacobian() = std::move(jacobian);
            trackStateProxy.pathLength() = std::move(pathLength);
          } else {
            ACTS_VERBOSE("Detected in-sensitive surface "
                         << surface->geometryId());

            // Transport & get curvilinear state instead of bound state
            auto [curvilinearParams, jacobian, pathLength] =
                stepper.curvilinearState(state.stepping);

            // Fill the track state
            trackStateProxy.predicted() =
                std::move(curvilinearParams.parameters());
            trackStateProxy.predictedCovariance() =
                std::move(*curvilinearParams.covariance());
            trackStateProxy.jacobian() = std::move(jacobian);
            trackStateProxy.pathLength() = std::move(pathLength);
          }

          // Set the filtered parameter index to be the same with predicted
          // parameter
          trackStateProxy.data().ifiltered = trackStateProxy.data().ipredicted;

          // We count the processed state
          ++result.processedStates;
        }

        // Update state and stepper with material effects
        materialInteractor(surface, state, stepper, fullUpdate);
      }
      return Result<void>::success();
    }

    template <typename propagator_state_t, typename stepper_t>
    Result<void> backwardFilter(const Surface* surface,
                                propagator_state_t& state,
                                const stepper_t& stepper,
                                result_type& result) const {
      const auto& logger = state.options.logger;
      // Try to find the surface in the measurement surfaces
      auto sourcelink_it = inputMeasurements.find(surface);
      if (sourcelink_it != inputMeasurements.end()) {
        // Screen output message
        ACTS_VERBOSE("Measurement surface "
                     << surface->geometryId()
                     << " detected in backward propagation.");

        // No backward filtering for last measurement state, but still update
        // with material effects
        if (state.stepping.navDir == backward and
            surface == state.navigation.startSurface) {
          materialInteractor(surface, state, stepper);
          return Result<void>::success();
        }

        // Transport & bind the state to the current surface
        auto [boundParams, jacobian, pathLength] =
            stepper.boundState(state.stepping, *surface);

        // Update state and stepper with pre material effects
        materialInteractor(surface, state, stepper, preUpdate);

        // Create a detached track state proxy
        auto tempTrackTip =
            result.fittedStates.addTrackState(TrackStatePropMask::All);

        // Get the detached track state proxy back
        auto trackStateProxy = result.fittedStates.getTrackState(tempTrackTip);

        // Assign the source link to the detached track state
        trackStateProxy.uncalibrated() = sourcelink_it->second;

        // Fill the track state
        trackStateProxy.predicted() = std::move(boundParams.parameters());
        trackStateProxy.predictedCovariance() =
            std::move(*boundParams.covariance());
        trackStateProxy.jacobian() = std::move(jacobian);
        trackStateProxy.pathLength() = std::move(pathLength);

        // We have predicted parameters, so calibrate the uncalibrated input
        // measuerement
        std::visit(
            [&](const auto& calibrated) {
              trackStateProxy.setCalibrated(calibrated);
            },
            m_calibrator(trackStateProxy.uncalibrated(),
                         trackStateProxy.predicted()));

        // If the update is successful, set covariance and
        auto updateRes =
            m_updater(state.geoContext, trackStateProxy, backward, logger);
        if (!updateRes.ok()) {
          ACTS_ERROR("Backward update step failed: " << updateRes.error());
          return updateRes.error();
        } else {
          // Update the stepping state with filtered parameters
          ACTS_VERBOSE(
              "Backward Filtering step successful, updated parameters are : "
              "\n"
              << trackStateProxy.filtered().transpose());

          // Fill the smoothed parameter for the existing track state
          result.fittedStates.applyBackwards(
              result.trackTip, [&](auto trackState) {
                auto fSurface = &trackState.referenceSurface();
                if (fSurface == surface) {
                  result.passedAgainSurfaces.push_back(surface);
                  trackState.smoothed() = trackStateProxy.filtered();
                  trackState.smoothedCovariance() =
                      trackStateProxy.filteredCovariance();
                  return false;
                }
                return true;
              });

          // update stepping state using filtered parameters after kalman
          // update We need to (re-)construct a BoundTrackParameters instance
          // here, which is a bit awkward.
          stepper.update(state.stepping,
                         MultiTrajectoryHelpers::freeFiltered(
                             state.options.geoContext, trackStateProxy),
                         trackStateProxy.filteredCovariance());

          // Update state and stepper with post material effects
          materialInteractor(surface, state, stepper, postUpdate);
        }
      } else if (surface->surfaceMaterial() != nullptr) {
        // Transport covariance
        if (surface->associatedDetectorElement() != nullptr) {
          ACTS_VERBOSE("Detected hole on " << surface->geometryId()
                                           << " in backward filtering");
          if (state.stepping.covTransport) {
            stepper.covarianceTransport(state.stepping, *surface);
          }
        } else {
          ACTS_VERBOSE("Detected in-sensitive surface "
                       << surface->geometryId() << " in backward filtering");
          if (state.stepping.covTransport) {
            stepper.covarianceTransport(state.stepping);
          }
        }

        // Not creating bound state here, so need manually reinitialize
        // jacobian
        state.stepping.jacobian = BoundMatrix::Identity();

        // Update state and stepper with material effects
        materialInteractor(surface, state, stepper);
      }

      return Result<void>::success();
    }

    template <typename propagator_state_t, typename stepper_t>
    void materialInteractor(
        const Surface* surface, propagator_state_t& state, stepper_t& stepper,
        const MaterialUpdateStage& updateStage = fullUpdate) const {
      const auto& logger = state.options.logger;
      // Indicator if having material
      bool hasMaterial = false;

      if (surface and surface->surfaceMaterial()) {
        // Prepare relevant input particle properties
        detail::PointwiseMaterialInteraction interaction(surface, state,
                                                         stepper);
        // Evaluate the material properties
        if (interaction.evaluateMaterialSlab(state, updateStage)) {
          // Surface has material at this stage
          hasMaterial = true;

          // Evaluate the material effects
          interaction.evaluatePointwiseMaterialInteraction(multipleScattering,
                                                           energyLoss);

          // Screen out material effects info
          ACTS_VERBOSE("Material effects on surface: "
                       << surface->geometryId()
                       << " at update stage: " << updateStage << " are :");
          ACTS_VERBOSE("eLoss = "
                       << interaction.Eloss << ", "
                       << "variancePhi = " << interaction.variancePhi << ", "
                       << "varianceTheta = " << interaction.varianceTheta
                       << ", "
                       << "varianceQoverP = " << interaction.varianceQoverP);

          // Update the state and stepper with material effects
          interaction.updateState(state, stepper);
        }
      }

      if (not hasMaterial) {
        // Screen out message
        ACTS_VERBOSE("No material effects on surface: " << surface->geometryId()
                                                        << " at update stage: "
                                                        << updateStage);
      }
    }

    template <typename propagator_state_t, typename stepper_t>
    Result<void> finalize(propagator_state_t& state, const stepper_t& stepper,
                          result_type& result) const {
      const auto& logger = state.options.logger;
      // Remember you smoothed the track states
      result.smoothed = true;

      // Get the indices of measurement states;
      std::vector<size_t> measurementIndices;
      measurementIndices.reserve(result.measurementStates);
      // Count track states to be smoothed
      size_t nStates = 0;
      result.fittedStates.applyBackwards(result.trackTip, [&](auto st) {
        bool isMeasurement =
            st.typeFlags().test(TrackStateFlag::MeasurementFlag);
        if (isMeasurement) {
          measurementIndices.emplace_back(st.index());
        } else if (measurementIndices.empty()) {
          // No smoothed parameters if the last measurement state has not been
          // found yet
          st.data().ismoothed = detail_lt::IndexData::kInvalid;
        }
        // Start count when the last measurement state is found
        if (not measurementIndices.empty()) {
          nStates++;
        }
      });
      // Return error if the track has no measurement states (but this should
      // not happen)
      if (measurementIndices.empty()) {
        ACTS_ERROR("Smoothing for a track without measurements.");
        return KalmanFitterError::SmoothFailed;
      }
      // Screen output for debugging
      if (logger().doPrint(Logging::VERBOSE)) {
        ACTS_VERBOSE("Apply smoothing on " << nStates
                                           << " filtered track states.");
      }

      // Smooth the track states
      auto smoothRes = m_smoother(state.geoContext, result.fittedStates,
                                  measurementIndices.front(), logger);
      if (!smoothRes.ok()) {
        ACTS_ERROR("Smoothing step failed: " << smoothRes.error());
        return smoothRes.error();
      }
      // Obtain the smoothed parameters at first measurement state
      auto firstMeasurement =
          result.fittedStates.getTrackState(measurementIndices.back());

      // Update the stepping parameters - in order to progress to destination
      ACTS_VERBOSE(
          "Smoothing successful, updating stepping state, "
          "set target surface.");
      stepper.update(state.stepping,
                     MultiTrajectoryHelpers::freeSmoothed(
                         state.options.geoContext, firstMeasurement),
                     firstMeasurement.smoothedCovariance());
      // Reverse the propagation direction
      state.stepping.stepSize =
          ConstrainedStep(-1. * state.options.maxStepSize);
      state.stepping.navDir = backward;
      // Set accumulatd path to zero before targeting surface
      state.stepping.pathAccumulated = 0.;

      return Result<void>::success();
    }

    updater_t m_updater;

    smoother_t m_smoother;

    outlier_finder_t m_outlierFinder;

    calibrator_t m_calibrator;

    SurfaceReached targetReached;
  };

  template <typename source_link_t, typename parameters_t>
  class Aborter {
   public:
    using action_type = Actor<source_link_t, parameters_t>;

    template <typename propagator_state_t, typename stepper_t,
              typename result_t>
    bool operator()(propagator_state_t& /*state*/, const stepper_t& /*stepper*/,
                    const result_t& result) const {
      if (!result.result.ok() or result.finished) {
        return true;
      }
      return false;
    }
  };

 public:
  template <typename source_link_t, typename start_parameters_t,
            typename parameters_t = BoundTrackParameters>
  auto fit(const std::vector<source_link_t>& sourcelinks,
           const start_parameters_t& sParameters,
           const KalmanFitterOptions<outlier_finder_t>& kfOptions) const
      -> std::enable_if_t<!isDirectNavigator,
                          Result<KalmanFitterResult<source_link_t>>> {
    const auto& logger = kfOptions.logger;

    static_assert(SourceLinkConcept<source_link_t>,
                  "Source link does not fulfill SourceLinkConcept");

    // To be able to find measurements later, we put them into a map
    // We need to copy input SourceLinks anyways, so the map can own them.
    ACTS_VERBOSE("Preparing " << sourcelinks.size() << " input measurements");
    std::map<const Surface*, source_link_t> inputMeasurements;
    for (const auto& sl : sourcelinks) {
      const Surface* srf = &sl.referenceSurface();
      ACTS_VERBOSE("Adding surface with geoID ");
      ACTS_VERBOSE(srf->geometryId());
      inputMeasurements.emplace(srf, sl);
    }

    // Create the ActionList and AbortList
    using KalmanAborter = Aborter<source_link_t, parameters_t>;
    using KalmanActor = Actor<source_link_t, parameters_t>;
    using KalmanResult = typename KalmanActor::result_type;
    using Actors = ActionList<KalmanActor>;
    using Aborters = AbortList<KalmanAborter>;

    // Create relevant options for the propagation options
    PropagatorOptions<Actors, Aborters> kalmanOptions(
        kfOptions.geoContext, kfOptions.magFieldContext, logger);

    // Set the trivial propagator options
    kalmanOptions.setPlainOptions(kfOptions.propagatorPlainOptions);

    // Catch the actor and set the measurements
    auto& kalmanActor = kalmanOptions.actionList.template get<KalmanActor>();
    kalmanActor.inputMeasurements = std::move(inputMeasurements);
    kalmanActor.targetSurface = kfOptions.referenceSurface;
    kalmanActor.multipleScattering = kfOptions.multipleScattering;
    kalmanActor.energyLoss = kfOptions.energyLoss;
    kalmanActor.backwardFiltering = kfOptions.backwardFiltering;

    // Set config for outlier finder
    kalmanActor.m_outlierFinder = kfOptions.outlierFinder;

    // Run the fitter
    auto result = m_propagator.template propagate(sParameters, kalmanOptions);

    if (!result.ok()) {
      ACTS_VERBOSE("Propapation failed: " << result.error());
      return result.error();
    }

    const auto& propRes = *result;

    auto kalmanResult = propRes.template get<KalmanResult>();

    if (kalmanResult.result.ok() and not kalmanResult.processedStates) {
      kalmanResult.result = Result<void>(KalmanFitterError::NoMeasurementFound);
    }

    if (!kalmanResult.result.ok()) {
      ACTS_VERBOSE("KalmanFilter failed: " << kalmanResult.result.error());
      return kalmanResult.result.error();
    }

    // Return the converted Track
    return kalmanResult;
  }

  template <typename source_link_t, typename start_parameters_t,
            typename parameters_t = BoundTrackParameters>
  auto fit(const std::vector<source_link_t>& sourcelinks,
           const start_parameters_t& sParameters,
           const KalmanFitterOptions<outlier_finder_t>& kfOptions,
           const std::vector<const Surface*>& sSequence) const
      -> std::enable_if_t<isDirectNavigator,
                          Result<KalmanFitterResult<source_link_t>>> {
    const auto& logger = kfOptions.logger;
    static_assert(SourceLinkConcept<source_link_t>,
                  "Source link does not fulfill SourceLinkConcept");

    // To be able to find measurements later, we put them into a map
    // We need to copy input SourceLinks anyways, so the map can own them.
    ACTS_VERBOSE("Preparing " << sourcelinks.size() << " input measurements");
    std::map<const Surface*, source_link_t> inputMeasurements;
    for (const auto& sl : sourcelinks) {
      const Surface* srf = &sl.referenceSurface();
      inputMeasurements.emplace(srf, sl);
    }

    // Create the ActionList and AbortList
    using KalmanAborter = Aborter<source_link_t, parameters_t>;
    using KalmanActor = Actor<source_link_t, parameters_t>;
    using KalmanResult = typename KalmanActor::result_type;
    using Actors = ActionList<DirectNavigator::Initializer, KalmanActor>;
    using Aborters = AbortList<KalmanAborter>;

    // Create relevant options for the propagation options
    PropagatorOptions<Actors, Aborters> kalmanOptions(
    kfOptions.geoContext, kfOptions.magFieldContext, logger);

    // Set the trivial propagator options
    kalmanOptions.setPlainOptions(kfOptions.propagatorPlainOptions);

    // Catch the actor and set the measurements
    auto& kalmanActor = kalmanOptions.actionList.template get<KalmanActor>();
    kalmanActor.inputMeasurements = std::move(inputMeasurements);
    kalmanActor.targetSurface = kfOptions.referenceSurface;
    kalmanActor.multipleScattering = kfOptions.multipleScattering;
    kalmanActor.energyLoss = kfOptions.energyLoss;
    kalmanActor.backwardFiltering = kfOptions.backwardFiltering;
    // Set config for outlier finder
    kalmanActor.m_outlierFinder = kfOptions.outlierFinder;

    // Set the surface sequence
    auto& dInitializer =
        kalmanOptions.actionList.template get<DirectNavigator::Initializer>();
    dInitializer.navSurfaces = sSequence;

    // Run the fitter
    auto result = m_propagator.template propagate(sParameters, kalmanOptions);

    if (!result.ok()) {
      ACTS_ERROR("Propapation failed: " << result.error());
      return result.error();
    }

    const auto& propRes = *result;

    auto kalmanResult = propRes.template get<KalmanResult>();

    if (kalmanResult.result.ok() and not kalmanResult.processedStates) {
      kalmanResult.result = Result<void>(KalmanFitterError::NoMeasurementFound);
    }

    if (!kalmanResult.result.ok()) {
      ACTS_ERROR("KalmanFilter failed: " << kalmanResult.result.error());
      return kalmanResult.result.error();
    }

    // Return the converted Track
    return kalmanResult;
  }
};

}  // namespace Acts