GainMatrixUpdater.hpp

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

// This file is part of the Acts project.
//
// Copyright (C) 2016-2018 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/Measurement.hpp"
#include "Acts/EventData/MeasurementHelpers.hpp"
#include "Acts/EventData/MultiTrajectory.hpp"
#include "Acts/EventData/TrackParameters.hpp"
#include "Acts/TrackFitting/KalmanFitterError.hpp"
#include "Acts/TrackFitting/detail/VoidKalmanComponents.hpp"
#include "Acts/Utilities/Definitions.hpp"
#include "Acts/Utilities/Logger.hpp"
#include "Acts/Utilities/Result.hpp"

#include <memory>
#include <variant>

namespace Acts {

class GainMatrixUpdater {
 public:
  template <typename track_state_t>
  Result<void> operator()(const GeometryContext& /*gctx*/,
                          track_state_t trackState,
                          const NavigationDirection& direction = forward,
                          LoggerWrapper logger = getDummyLogger()) const {
    ACTS_VERBOSE("Invoked GainMatrixUpdater");
    // let's make sure the types are consistent
    using SourceLink = typename track_state_t::SourceLink;
    using TrackStateProxy =
        typename MultiTrajectory<SourceLink>::TrackStateProxy;
    static_assert(std::is_same_v<track_state_t, TrackStateProxy>,
                  "Given track state type is not a track state proxy");

    // we should definitely have an uncalibrated measurement here
    assert(trackState.hasUncalibrated());
    // there should be a calibrated measurement
    assert(trackState.hasCalibrated());
    // we should have predicted state set
    assert(trackState.hasPredicted());
    // filtering should not have happened yet, but is allocated, therefore set
    assert(trackState.hasFiltered());

    // read-only handles. Types are eigen maps to backing storage
    const auto predicted = trackState.predicted();
    const auto predicted_covariance = trackState.predictedCovariance();

    ACTS_VERBOSE("Predicted parameters: " << predicted.transpose());
    ACTS_VERBOSE("Predicted covariance:\n" << predicted_covariance);

    // read-write handles. Types are eigen maps into backing storage.
    // This writes directly into the trajectory storage
    auto filtered = trackState.filtered();
    auto filtered_covariance = trackState.filteredCovariance();

    std::optional<std::error_code> error{std::nullopt};  // assume ok
    visit_measurement(
        trackState.calibrated(), trackState.calibratedCovariance(),
        trackState.calibratedSize(),
        [&](const auto calibrated, const auto calibrated_covariance) {
          constexpr size_t measdim = decltype(calibrated)::RowsAtCompileTime;
          using cov_t = ActsSymMatrixD<measdim>;
          using par_t = ActsVectorD<measdim>;

          ACTS_VERBOSE("Measurement dimension: " << measdim);
          ACTS_VERBOSE("Calibrated measurement: " << calibrated.transpose());
          ACTS_VERBOSE("Calibrated measurement covariance:\n"
                       << calibrated_covariance);

          const ActsMatrixD<measdim, eBoundSize> H =
              trackState.projector()
                  .template topLeftCorner<measdim, eBoundSize>();

          ACTS_VERBOSE("Measurement projector H:\n" << H);

          const ActsMatrixD<eBoundSize, measdim> K =
              predicted_covariance * H.transpose() *
              (H * predicted_covariance * H.transpose() + calibrated_covariance)
                  .inverse();

          ACTS_VERBOSE("Gain Matrix K:\n" << K);

          if (K.hasNaN()) {
            error =
                (direction == forward)
                    ? KalmanFitterError::ForwardUpdateFailed
                    : KalmanFitterError::BackwardUpdateFailed;  // set to error
            return false;  // abort execution
          }

          filtered = predicted + K * (calibrated - H * predicted);
          filtered_covariance =
              (BoundSymMatrix::Identity() - K * H) * predicted_covariance;
          ACTS_VERBOSE("Filtered parameters: " << filtered.transpose());
          ACTS_VERBOSE("Filtered covariance:\n" << filtered_covariance);

          // calculate filtered residual
          //
          // FIXME: Without separate residual construction and assignment, we
          //        currently take a +0.7GB build memory consumption hit in the
          //        EventDataView unit tests. Revisit this once Measurement
          //        overhead problems (Acts issue #350) are sorted out.
          //
          par_t residual;
          residual = calibrated - H * filtered;
          ACTS_VERBOSE("Residual: " << residual.transpose());

          trackState.chi2() =
              (residual.transpose() *
               ((cov_t::Identity() - H * K) * calibrated_covariance).inverse() *
               residual)
                  .value();

          ACTS_VERBOSE("Chi2: " << trackState.chi2());
          return true;  // continue execution
        });

    if (error) {
      // error is set, return result
      return *error;
    }

    // always succeed, no outlier logic yet
    return Result<void>::success();
  }
};

}  // namespace Acts