KalmanUpdateHelpers.hpp

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

// This file is part of the Acts project.
//
// Copyright (C) 2021-2023 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

#include "Acts/EventData/MultiTrajectory.hpp"
#include "Acts/EventData/SourceLink.hpp"
#include "Acts/EventData/detail/CorrectedTransformationFreeToBound.hpp"
#include "Acts/Surfaces/Surface.hpp"
#include "Acts/Utilities/CalibrationContext.hpp"
#include "Acts/Utilities/Result.hpp"

namespace Acts {
namespace detail {

template <typename propagator_state_t, typename stepper_t,
          typename extensions_t, typename traj_t>
auto kalmanHandleMeasurement(
    const CalibrationContext &calibrationContext, propagator_state_t &state,
    const stepper_t &stepper, const extensions_t &extensions,
    const Surface &surface, const SourceLink &source_link, traj_t &fittedStates,
    const size_t lastTrackIndex, bool doCovTransport, const Logger &logger,
    const FreeToBoundCorrection &freeToBoundCorrection = FreeToBoundCorrection(
        false)) -> Result<typename traj_t::TrackStateProxy> {
  // Add a <mask> TrackState entry multi trajectory. This allocates storage for
  // all components, which we will set later.
  TrackStatePropMask mask = TrackStatePropMask::All;
  const size_t currentTrackIndex =
      fittedStates.addTrackState(mask, lastTrackIndex);

  // now get track state proxy back
  typename traj_t::TrackStateProxy trackStateProxy =
      fittedStates.getTrackState(currentTrackIndex);

  // Set the trackStateProxy components with the state from the ongoing
  // propagation
  {
    trackStateProxy.setReferenceSurface(surface.getSharedPtr());
    // Bind the transported state to the current surface
    auto res = stepper.boundState(state.stepping, surface, doCovTransport,
                                  freeToBoundCorrection);
    if (!res.ok()) {
      ACTS_ERROR("Propagate to surface " << surface.geometryId()
                                         << " failed: " << res.error());
      return res.error();
    }
    auto &[boundParams, jacobian, pathLength] = *res;

    // Fill the track state
    trackStateProxy.predicted() = std::move(boundParams.parameters());
    if (boundParams.covariance().has_value()) {
      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
  // measurement
  extensions.calibrator(state.geoContext, calibrationContext, source_link,
                        trackStateProxy);

  // Get and set the type flags
  {
    auto typeFlags = trackStateProxy.typeFlags();
    typeFlags.set(TrackStateFlag::ParameterFlag);
    if (surface.surfaceMaterial() != nullptr) {
      typeFlags.set(TrackStateFlag::MaterialFlag);
    }

    // Check if the state is an outlier.
    // If not:
    // - run Kalman update
    // - tag it as a measurement
    // - update the stepping state.
    // Else, just tag it as an outlier
    if (not extensions.outlierFinder(trackStateProxy)) {
      // Run Kalman update
      auto updateRes = extensions.updater(state.geoContext, trackStateProxy,
                                          state.options.direction, logger);
      if (!updateRes.ok()) {
        ACTS_ERROR("Update step failed: " << updateRes.error());
        return updateRes.error();
      }
      // Set the measurement type flag
      typeFlags.set(TrackStateFlag::MeasurementFlag);
    } else {
      ACTS_VERBOSE(
          "Filtering step successful. But measurement is determined "
          "to be an outlier. Stepping state is not updated.")
      // Set the outlier type flag
      typeFlags.set(TrackStateFlag::OutlierFlag);
      trackStateProxy.shareFrom(trackStateProxy, TrackStatePropMask::Predicted,
                                TrackStatePropMask::Filtered);
    }
  }

  return trackStateProxy;
}

template <typename propagator_state_t, typename stepper_t, typename traj_t>
auto kalmanHandleNoMeasurement(
    propagator_state_t &state, const stepper_t &stepper, const Surface &surface,
    traj_t &fittedStates, const size_t lastTrackIndex, bool doCovTransport,
    const Logger &logger,
    const FreeToBoundCorrection &freeToBoundCorrection = FreeToBoundCorrection(
        false)) -> Result<typename traj_t::TrackStateProxy> {
  // Add a <mask> TrackState entry multi trajectory. This allocates storage for
  // all components, which we will set later.
  TrackStatePropMask mask =
      ~(TrackStatePropMask::Calibrated | TrackStatePropMask::Filtered);
  const size_t currentTrackIndex =
      fittedStates.addTrackState(mask, lastTrackIndex);

  // now get track state proxy back
  typename traj_t::TrackStateProxy trackStateProxy =
      fittedStates.getTrackState(currentTrackIndex);

  // Set the trackStateProxy components with the state from the ongoing
  // propagation
  {
    trackStateProxy.setReferenceSurface(surface.getSharedPtr());
    // Bind the transported state to the current surface
    auto res = stepper.boundState(state.stepping, surface, doCovTransport,
                                  freeToBoundCorrection);
    if (!res.ok()) {
      return res.error();
    }
    auto &[boundParams, jacobian, pathLength] = *res;

    // Fill the track state
    trackStateProxy.predicted() = std::move(boundParams.parameters());
    if (boundParams.covariance().has_value()) {
      trackStateProxy.predictedCovariance() =
          std::move(*boundParams.covariance());
    }

    trackStateProxy.jacobian() = std::move(jacobian);
    trackStateProxy.pathLength() = std::move(pathLength);

    // Set the filtered parameter index to be the same with predicted
    // parameter
    trackStateProxy.shareFrom(trackStateProxy, TrackStatePropMask::Predicted,
                              TrackStatePropMask::Filtered);
  }

  // Set the track state flags
  auto typeFlags = trackStateProxy.typeFlags();
  typeFlags.set(TrackStateFlag::ParameterFlag);
  if (surface.surfaceMaterial() != nullptr) {
    typeFlags.set(TrackStateFlag::MaterialFlag);
  }
  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);
  } else if (surface.surfaceMaterial() != nullptr) {
    ACTS_VERBOSE("Detected in-sensitive surface " << surface.geometryId());
  }

  return trackStateProxy;
}

}  // namespace detail
}  // namespace Acts