TrackFitting/detail/KalmanUpdateHelpers.hpp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "Acts/EventData/MultiTrajectory.hpp"
0012 #include "Acts/EventData/SourceLink.hpp"
0013 #include "Acts/EventData/detail/CorrectedTransformationFreeToBound.hpp"
0014 #include "Acts/Surfaces/Surface.hpp"
0015 #include "Acts/Utilities/CalibrationContext.hpp"
0016 #include "Acts/Utilities/Result.hpp"
0017
0018 namespace Acts::detail {
0019
0020 /// This function encapsulates the Kalman update performed on a MultiTrajectory
0021 /// for a single source link.
0022 /// @tparam propagator_state_t The propagator state type
0023 /// @tparam stepper_t The stepper type
0024 /// @tparam extensions_t The type of the extensions used for the update
0025 /// @param state The propagator state
0026 /// @param stepper The stepper
0027 /// @param extensions The extension used for the update
0028 /// @param surface The current surface
0029 /// @param sourceLink The source link used for the update
0030 /// @param fittedStates The Multitrajectory to that we add the state
0031 /// @param lastTrackIndex The parent index for the new state in the MT
0032 /// @param doCovTransport Whether to perform a covariance transport when
0033 /// computing the bound state or not
0034 /// @param freeToBoundCorrection Correction for non-linearity effect during transform from free to bound (only corrected when performing CovTransport)
0035 template <typename propagator_state_t, typename stepper_t,
0036           typename extensions_t, typename traj_t>
0037 auto kalmanHandleMeasurement(
0038     const CalibrationContext &calibrationContext, propagator_state_t &state,
0039     const stepper_t &stepper, const extensions_t &extensions,
0040     const Surface &surface, const SourceLink &sourceLink, traj_t &fittedStates,
0041     const std::size_t lastTrackIndex, bool doCovTransport, const Logger &logger,
0042     const FreeToBoundCorrection &freeToBoundCorrection = FreeToBoundCorrection(
0043         false)) -> Result<typename traj_t::TrackStateProxy> {
0044   // Add a <mask> TrackState entry multi trajectory. This allocates storage for
0045   // all components, which we will set later.
0046   TrackStatePropMask mask =
0047       TrackStatePropMask::Predicted | TrackStatePropMask::Filtered |
0048       TrackStatePropMask::Smoothed | TrackStatePropMask::Jacobian |
0049       TrackStatePropMask::Calibrated;
0050   typename traj_t::TrackStateProxy trackStateProxy =
0051       fittedStates.makeTrackState(mask, lastTrackIndex);
0052
0053   typename traj_t::ConstTrackStateProxy trackStateProxyConst{trackStateProxy};
0054
0055   // Set the trackStateProxy components with the state from the ongoing
0056   // propagation
0057   {
0058     trackStateProxy.setReferenceSurface(surface.getSharedPtr());
0059     // Bind the transported state to the current surface
0060     auto res = stepper.boundState(state.stepping, surface, doCovTransport,
0061                                   freeToBoundCorrection);
0062     if (!res.ok()) {
0063       ACTS_ERROR("Propagate to surface " << surface.geometryId()
0064                                          << " failed: " << res.error());
0065       return res.error();
0066     }
0067     const auto &[boundParams, jacobian, pathLength] = *res;
0068
0069     // Fill the track state
0070     trackStateProxy.predicted() = boundParams.parameters();
0071     trackStateProxy.predictedCovariance() = state.stepping.cov;
0072
0073     trackStateProxy.jacobian() = jacobian;
0074     trackStateProxy.pathLength() = pathLength;
0075   }
0076
0077   // We have predicted parameters, so calibrate the uncalibrated input
0078   // measurement
0079   extensions.calibrator(state.geoContext, calibrationContext, sourceLink,
0080                         trackStateProxy);
0081
0082   // Get and set the type flags
0083   {
0084     auto typeFlags = trackStateProxy.typeFlags();
0085     typeFlags.set(TrackStateFlag::ParameterFlag);
0086     if (surface.surfaceMaterial() != nullptr) {
0087       typeFlags.set(TrackStateFlag::MaterialFlag);
0088     }
0089
0090     // Check if the state is an outlier.
0091     // If not:
0092     // - run Kalman update
0093     // - tag it as a measurement
0094     // - update the stepping state.
0095     // Else, just tag it as an outlier
0096     if (!extensions.outlierFinder(trackStateProxyConst)) {
0097       // Run Kalman update
0098       auto updateRes =
0099           extensions.updater(state.geoContext, trackStateProxy, logger);
0100       if (!updateRes.ok()) {
0101         ACTS_ERROR("Update step failed: " << updateRes.error());
0102         return updateRes.error();
0103       }
0104       // Set the measurement type flag
0105       typeFlags.set(TrackStateFlag::MeasurementFlag);
0106     } else {
0107       ACTS_VERBOSE(
0108           "Filtering step successful. But measurement is determined "
0109           "to be an outlier. Stepping state is not updated.");
0110       // Set the outlier type flag
0111       typeFlags.set(TrackStateFlag::OutlierFlag);
0112       trackStateProxy.shareFrom(trackStateProxy, TrackStatePropMask::Predicted,
0113                                 TrackStatePropMask::Filtered);
0114     }
0115   }
0116
0117   return trackStateProxy;
0118 }
0119
0120 /// This function encapsulates what actions should be performed on a
0121 /// MultiTrajectory when we have no measurement.
0122 /// If there are no source links on surface, add either a hole or passive
0123 /// material TrackState entry multi trajectory. No storage allocation for
0124 /// uncalibrated/calibrated measurement and filtered parameter
0125 /// @tparam stepper_t The stepper type
0126 /// @param state The propagator state
0127 /// @param stepper The stepper
0128 /// @param surface The current surface
0129 /// @param fittedStates The Multitrajectory to that we add the state
0130 /// @param lastTrackIndex The parent index for the new state in the MT
0131 /// @param doCovTransport Whether to perform a covariance transport when
0132 /// computing the bound state or not
0133 /// @param freeToBoundCorrection Correction for non-linearity effect during transform from free to bound (only corrected when performing CovTransport)
0134 template <typename stepper_t, typename traj_t>
0135 auto kalmanHandleNoMeasurement(
0136     typename stepper_t::State &state, const stepper_t &stepper,
0137     const Surface &surface, traj_t &fittedStates,
0138     const std::size_t lastTrackIndex, bool doCovTransport, const Logger &logger,
0139     const bool precedingMeasurementExists,
0140     const FreeToBoundCorrection &freeToBoundCorrection = FreeToBoundCorrection(
0141         false)) -> Result<typename traj_t::TrackStateProxy> {
0142   // Add a <mask> TrackState entry multi trajectory. This allocates storage for
0143   // all components, which we will set later.
0144   TrackStatePropMask mask = TrackStatePropMask::Predicted |
0145                             TrackStatePropMask::Smoothed |
0146                             TrackStatePropMask::Jacobian;
0147   typename traj_t::TrackStateProxy trackStateProxy =
0148       fittedStates.makeTrackState(mask, lastTrackIndex);
0149
0150   // Set the trackStateProxy components with the state from the ongoing
0151   // propagation
0152   {
0153     trackStateProxy.setReferenceSurface(surface.getSharedPtr());
0154     // Bind the transported state to the current surface
0155     auto res = stepper.boundState(state, surface, doCovTransport,
0156                                   freeToBoundCorrection);
0157     if (!res.ok()) {
0158       return res.error();
0159     }
0160     const auto &[boundParams, jacobian, pathLength] = *res;
0161
0162     // Fill the track state
0163     trackStateProxy.predicted() = boundParams.parameters();
0164     trackStateProxy.predictedCovariance() = state.cov;
0165
0166     trackStateProxy.jacobian() = jacobian;
0167     trackStateProxy.pathLength() = pathLength;
0168
0169     // Set the filtered parameter index to be the same with predicted
0170     // parameter
0171     trackStateProxy.shareFrom(trackStateProxy, TrackStatePropMask::Predicted,
0172                               TrackStatePropMask::Filtered);
0173   }
0174
0175   // Set the track state flags
0176   const bool surfaceHasMaterial = surface.surfaceMaterial() != nullptr;
0177   const bool surfaceIsSensitive =
0178       surface.associatedDetectorElement() != nullptr;
0179   auto typeFlags = trackStateProxy.typeFlags();
0180   typeFlags.set(TrackStateFlag::ParameterFlag);
0181
0182   if (surfaceHasMaterial) {
0183     typeFlags.set(TrackStateFlag::MaterialFlag);
0184   }
0185
0186   if (surfaceIsSensitive && precedingMeasurementExists) {
0187     ACTS_VERBOSE("Detected hole on " << surface.geometryId());
0188     // If the surface is sensitive, set the hole type flag
0189     typeFlags.set(TrackStateFlag::HoleFlag);
0190   } else if (surfaceIsSensitive) {
0191     ACTS_VERBOSE("Skip hole (no preceding measurements) on surface "
0192                  << surface.geometryId());
0193   } else if (surfaceHasMaterial) {
0194     ACTS_VERBOSE("Detected in-sensitive surface " << surface.geometryId());
0195   }
0196
0197   return trackStateProxy;
0198 }
0199
0200 }  // namespace Acts::detail