KalmanUpdator.h

Go to the documentation of this file.

/*
  Copyright (C) 2002-2021 CERN for the benefit of the ATLAS collaboration
*/

// KalmanUpdator.h
//   Header file for Kalman Fitter Updator
// (c) ATLAS Detector software
// Markus.Elsing@cern.ch
 
#ifndef TRK_KALMANUPDATOR_H
#define TRK_KALMANUPDATOR_H
 
#include "AthenaBaseComps/AthAlgTool.h"
#include "EventPrimitives/EventPrimitivesCovarianceHelpers.h"
#include "EventPrimitives/EventPrimitives.h"
#include "GaudiKernel/MsgStream.h"
#include "GeoPrimitives/GeoPrimitives.h"
#include "TrkEventPrimitives/FitQualityOnSurface.h"
#include "TrkEventPrimitives/ParamDefs.h"
#include "TrkEventPrimitives/ProjectionMatricesSet.h"
#include "TrkToolInterfaces/IUpdator.h"
#include <cmath>
 
namespace Trk {

class KalmanUpdator final
  : virtual public IUpdator
  , public AthAlgTool
{
public:
  KalmanUpdator(const std::string&, const std::string&, const IInterface*);
  ~KalmanUpdator();

  virtual StatusCode initialize() override final;
  virtual StatusCode finalize() override final;

  // from Amg::Vector2D (use eg with PRD)
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;
  // from LocalParameters (use eg with MeasurementBase, ROT)
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  // of the state at the same time (Amg::Vector2D-version)
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&,
    FitQualityOnSurface*&) const override final;
  // of the state at the same time (LocalParameters-version)
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&,
    FitQualityOnSurface*&) const override final;

  // getting the measurement coordinates from the Amg::Vector2D class.
  virtual std::unique_ptr<TrackParameters> removeFromState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;
  // getting the measurement coordinates from the LocalParameters class.
  virtual std::unique_ptr<TrackParameters> removeFromState(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  // interface with Amg::Vector2D and FitQualityOnSurface.
  virtual std::unique_ptr<TrackParameters> removeFromState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&,
    FitQualityOnSurface*&) const override final;
  // the interface with LocalParameters and FitQualityOnSurface.
  virtual std::unique_ptr<TrackParameters> removeFromState(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&,
    FitQualityOnSurface*&) const override final;

  virtual std::unique_ptr<TrackParameters> combineStates(
    const TrackParameters&,
    const TrackParameters&) const override final;
  virtual std::unique_ptr<TrackParameters> combineStates(
    const TrackParameters&,
    const TrackParameters&,
    FitQualityOnSurface*&) const override final;

  // which contains the current hit (expressed as Amg::Vector2D).
  virtual FitQualityOnSurface fullStateFitQuality(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;

  // a state which contains the current hit (expressed as LocalParameters).
  virtual FitQualityOnSurface fullStateFitQuality(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  // which contains the current hit (expressed as Amg::Vector2D).
  virtual FitQualityOnSurface predictedStateFitQuality(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;
  // which contains the current hit (expressed as LocalParameters).
  virtual FitQualityOnSurface predictedStateFitQuality(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  // the parameters of another trajectory part extrapolated to the common
  // surface.
  virtual FitQualityOnSurface predictedStateFitQuality(
    const TrackParameters&,
    const TrackParameters&) const override final;
  virtual std::pair<AmgVector(5), AmgSymMatrix(5)>* updateParameterDifference(
    const AmgVector(5) &,
    const AmgSymMatrix(5) &,
    const Amg::VectorX&,
    const Amg::MatrixX&,
    int,
    Trk::FitQualityOnSurface*&,
    bool) const override final
  {
    return nullptr;
  }

  virtual std::vector<double> initialErrors() const override final;
 
private:
  std::unique_ptr<TrackParameters> calculateFilterStep(const TrackParameters&,
                                                       const Amg::Vector2D&,
                                                       const Amg::MatrixX&,
                                                       const int,
                                                       FitQualityOnSurface*&,
                                                       bool) const;
  std::unique_ptr<TrackParameters> calculateFilterStep(const TrackParameters&,
                                                       const LocalParameters&,
                                                       const Amg::MatrixX&,
                                                       const int,
                                                       FitQualityOnSurface*&,
                                                       bool) const;
  FitQualityOnSurface  makeChi2Object(const Amg::VectorX&,
                                      const Amg::MatrixX&,
                                      const Amg::MatrixX&,
                                      const int,
                                      const int) const;

  Amg::MatrixX projection(const Amg::MatrixX&, const int) const;

  void logStart(const std::string&, const TrackParameters&) const;
  void logInputCov(const Amg::MatrixX&,
                   const Amg::VectorX&,
                   const Amg::MatrixX&) const;
  void logGainForm(int,
                   const Amg::VectorX&,
                   const Amg::MatrixX&,
                   const Amg::MatrixX&,
                   const Amg::MatrixX&) const;
  void logResult(const std::string&,
                 const Amg::VectorX&,
                 const Amg::MatrixX&) const;

  bool consistentParamDimensions(const LocalParameters&, int) const;
  bool thetaPhiWithinRange(const Amg::VectorX&, const int key = 31) const;
  bool diffThetaPhiWithinRange(const Amg::VectorX&, const int key = 31) const;
  bool correctThetaPhiRange(Amg::VectorX&,
                            AmgSymMatrix(5) &,
                            const bool isDifference = false,
                            const int key = 31) const;
  bool correctThetaPhiRange(Amg::VectorX&,
                            Amg::MatrixX&,
                            const bool isDifference = false,
                            const int key = 31) const;
 
  std::vector<double> m_cov0; 
  ProjectionMatricesSet
    m_projectionMatrices; 
  bool m_useFruehwirth8a; 
  int m_outputlevel;      
};
 
inline bool
KalmanUpdator::thetaPhiWithinRange(const Amg::VectorX& V, const int key) const
{
  if (!(key & 4 || key & 8))
    return true; // in case no angles measured.
  if (key == 31)
    return ((std::abs(V[Trk::phi]) <= M_PI) && (V[Trk::theta] >= 0.0) &&
            (V[Trk::theta] <= M_PI));
  else { // if vector is compressed (i.e. localParameters) need to extract
         // phi,theta first.
    bool okay = true;
    if (key & 4) { // phi is being measured
      int jphi = 0;
      for (int itag = 0, ipos = 1; itag < Trk::phi; ++itag, ipos *= 2)
        if (key & ipos)
          ++jphi;
      okay = okay && (std::abs(V[jphi]) <= M_PI);
    }
    if (key & 8) { // theta is being measured
      int jtheta = 0;
      for (int itag = 0, ipos = 1; itag <= Trk::theta; ++itag, ipos *= 2)
        if (key & ipos)
          ++jtheta;
      okay = okay && (std::abs(V[jtheta] - M_PI_2) <= M_PI_2);
    }
    return okay;
  }
}
 
// for speed reasons make two separate inline functions
// phi   differences should be smaller than pi (else go other way round) => same
// as absolute phi value. theta differences should be smaller than pi but can be
// negative => other constraint than absolute theta.
inline bool
KalmanUpdator::diffThetaPhiWithinRange(const Amg::VectorX& V,
                                       const int key) const
{
  if (!(key & 4 || key & 8))
    return true; // in case no angles measured.
  if (key == 31)
    return ((std::abs(V[Trk::phi]) <= M_PI) && (V[Trk::theta] >= -M_PI) &&
            (V[Trk::theta] <= M_PI));
 
  else { // if vector is compressed (i.e. localParameters) need to extract
         // phi,theta first.
    bool okay = true;
    if (key & 4) { // phi is being measured
      int jphi = 0;
      for (int itag = 0, ipos = 1; itag < Trk::phi; ++itag, ipos *= 2)
        if (key & ipos)
          ++jphi;
      okay = okay && (std::abs(V[jphi]) <= M_PI);
    }
    if (key & 8) { // theta is being measured
      int jtheta = 0;
      for (int itag = 0, ipos = 1; itag <= Trk::theta; ++itag, ipos *= 2)
        if (key & ipos)
          ++jtheta;
      okay = okay && (std::abs(V[jtheta]) <= M_PI);
    }
    return okay;
  }
}
 
inline FitQualityOnSurface
KalmanUpdator::makeChi2Object(const Amg::VectorX& residual,
                              const Amg::MatrixX& covTrk,
                              const Amg::MatrixX& covRio,
                              const int sign,
                              const int key) const
{ // sign: -1 = updated, +1 = predicted parameters.
  Amg::MatrixX R = covRio + sign * projection(covTrk, key); // .similarity(H);
  if (R.determinant() == 0) {
    ATH_MSG_DEBUG("matrix inversion failed");
    return FitQualityOnSurface(0.0, (int)covRio.cols());
  }
  // get chi2 = r.T() * R^-1 * r
  const double chiSquared = Amg::chi2(R.inverse(), residual);
  ATH_MSG_VERBOSE("-U- fitQuality of " << (sign > 0 ? "predicted" : "updated")
                                       << " state, chi2 :" << chiSquared
                                       << " / ndof= " << covRio.cols());
  // return the FitQualityOnSurface object
  return  FitQualityOnSurface(chiSquared, int(covRio.cols()));
}
 
} // end of namespace
 
#endif // TRK_KALMANUPDATOR_CLHEP_H