KalmanUpdatorSMatrix.h

Go to the documentation of this file.

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

// KalmanUpdatorSMatrix.h
//   Header file for Kalman Fitter Updator
// (c) ATLAS Detector software
// Markus.Elsing@cern.ch, Wolfgang.Liebig@cern.ch
 
#ifndef TRK_KALMANUPDATORSMATRIX_H
#define TRK_KALMANUPDATORSMATRIX_H
 
 
#include "AthenaBaseComps/AthAlgTool.h"
#include "TrkEventPrimitives/ParamDefs.h"
#include "TrkParameters/TrackParameters.h"
#include "TrkToolInterfaces/IUpdator.h"
// Eigen/Amg
#include "EventPrimitives/EventPrimitives.h"
// ROOT SMatrix lin alg: for internal calculations
#include "Math/SMatrix.h"
#include "Math/SVector.h"
#include <cmath>
#include <string_view>
 
typedef ROOT::Math::SMatrix<double, 1, 1, ROOT::Math::MatRepSym<double, 1>>
  SCovMatrix1;
typedef ROOT::Math::SMatrix<double, 2, 2, ROOT::Math::MatRepSym<double, 2>>
  SCovMatrix2;
typedef ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepSym<double, 3>>
  SCovMatrix3;
typedef ROOT::Math::SMatrix<double, 4, 4, ROOT::Math::MatRepSym<double, 4>>
  SCovMatrix4;
typedef ROOT::Math::SMatrix<double, 5, 5, ROOT::Math::MatRepSym<double, 5>>
  SCovMatrix5;
typedef ROOT::Math::SMatrix<double, 2, 2, ROOT::Math::MatRepStd<double, 2, 2>>
  SGenMatrix2;
typedef ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepStd<double, 3, 3>>
  SGenMatrix3;
typedef ROOT::Math::SMatrix<double, 4, 4, ROOT::Math::MatRepStd<double, 4, 4>>
  SGenMatrix4;
typedef ROOT::Math::SMatrix<double, 5, 5, ROOT::Math::MatRepStd<double, 5, 5>>
  SGenMatrix5;
typedef ROOT::Math::SVector<double, 2> SParVector2;
typedef ROOT::Math::SVector<double, 3> SParVector3;
typedef ROOT::Math::SVector<double, 4> SParVector4;
typedef ROOT::Math::SVector<double, 5> SParVector5;
 
namespace Trk {
 
enum RangeCheckDef
{
  absoluteCheck = 0,
  differentialCheck = 1
};

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

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

  // fails: @copydoc Trk::IUpdator::addToState(const TrackParameters&, const
  // Amg::Vector2D&, const Amg::MatrixX&)
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&,
    FitQualityOnSurface*&) const override final;
  virtual std::unique_ptr<TrackParameters> addToState(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&,
    FitQualityOnSurface*&) const override final;

  virtual std::unique_ptr<TrackParameters> removeFromState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;
  virtual std::unique_ptr<TrackParameters> removeFromState(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  virtual std::unique_ptr<TrackParameters> removeFromState(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&,
    FitQualityOnSurface*&) const override final;
  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;

  virtual FitQualityOnSurface fullStateFitQuality(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;
  virtual FitQualityOnSurface fullStateFitQuality(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  virtual FitQualityOnSurface predictedStateFitQuality(
    const TrackParameters&,
    const Amg::Vector2D&,
    const Amg::MatrixX&) const override final;
  virtual FitQualityOnSurface predictedStateFitQuality(
    const TrackParameters&,
    const LocalParameters&,
    const Amg::MatrixX&) const override final;
  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> prepareFilterStep(const TrackParameters&,
                                                     const LocalParameters&,
                                                     const Amg::MatrixX&,
                                                     const int,
                                                     FitQualityOnSurface*&,
                                                     bool) const;
  std::unique_ptr<TrackParameters> calculateFilterStep_1D(
    const TrackParameters&,
    const SParVector5&,
    const SCovMatrix5&,
    double,
    int,
    const Amg::MatrixX&,
    const int,
    FitQualityOnSurface*&,
    bool) const;
  std::unique_ptr<TrackParameters> calculateFilterStep_2D(
    const TrackParameters&,
    const SParVector5&,
    const SCovMatrix5&,
    const SParVector2&,
    int,
    const Amg::MatrixX&,
    const int,
    FitQualityOnSurface*&,
    bool) const;
  std::unique_ptr<TrackParameters> calculateFilterStep_3D(
    const TrackParameters&,
    const SParVector5&,
    const SCovMatrix5&,
    const LocalParameters&,
    const Amg::MatrixX&,
    const int,
    FitQualityOnSurface*&,
    bool) const;
  std::unique_ptr<TrackParameters> calculateFilterStep_4D(
    const TrackParameters&,
    const SParVector5&,
    const SCovMatrix5&,
    const LocalParameters&,
    const Amg::MatrixX&,
    const int,
    FitQualityOnSurface*&,
    bool) const;

  std::unique_ptr<TrackParameters> calculateFilterStep_5D(
    const TrackParameters&,
    const SParVector5&,
    const SCovMatrix5&,
    const SParVector5&,
    const Amg::MatrixX&,
    const int,
    FitQualityOnSurface*&,
    bool) const;

  bool getStartCov(SCovMatrix5&, const TrackParameters&, const int) const;
  std::unique_ptr<TrackParameters> convertToClonedTrackPars(
    const TrackParameters&,
    const SParVector5&,
    const SCovMatrix5&,
    int,
    bool,
    std::string_view) const;
  FitQualityOnSurface  makeChi2_1D(const SParVector5&,
                                   const AmgSymMatrix(5)&,
                                   double,
                                   double,
                                   int,
                                   int) const;
  FitQualityOnSurface  makeChi2_2D(const SParVector5&,
                                   const AmgSymMatrix(5)&,
                                   const SParVector2&,
                                   const SCovMatrix2&,
                                   int,
                                   int) const;
  FitQualityOnSurface  makeChi2_5D(const SParVector5&,
                                   const AmgSymMatrix(5)&,
                                   const SParVector5&,
                                   const AmgSymMatrix(5)&,
                                   int) const;
  FitQualityOnSurface  makeChi2Object(const Amg::VectorX&,
                                      const AmgSymMatrix(5)&,
                                      const Amg::MatrixX&,
                                      const Amg::MatrixX&,
                                      int) const;

  static SCovMatrix2 projection_2D(const SCovMatrix5&, int) ;
  static SCovMatrix2 projection_2D(const Amg::MatrixX&, int) ;
  static SCovMatrix3 projection_3D(const SCovMatrix5&, int) ;
  static SCovMatrix4 projection_4D(const SCovMatrix5&, int) ;
 
  // === note: any of the following log... method is only called if
  // the msgstream level has been set appropriately.
  void logStart(const std::string&, const TrackParameters&) const;
  void logInputCov(const SCovMatrix5&,
                   const Amg::VectorX&,
                   const Amg::MatrixX&) const;
  void logGainForm(int,
                   const SParVector5&,
                   const SCovMatrix5&,
                   const SGenMatrix5&) const;
  void logResult(const std::string&,
                 const AmgVector(5) &,
                 const AmgSymMatrix(5) &) const;

  bool consistentParamDimensions(const LocalParameters&, int) const;


  bool thetaPhiWithinRange_5D(const SParVector5& V,
                              const RangeCheckDef rcd) const;

  bool thetaWithinRange_5D(const SParVector5& V) const;


  bool correctThetaPhiRange_5D(SParVector5&,
                               SCovMatrix5&,
                               const RangeCheckDef) const;
 
  std::vector<double> m_cov_stdvec; 
  SParVector5 m_cov0;               
  bool m_useFruehwirth8a; 
  float m_thetaGainDampingValue;
 
  SCovMatrix5 m_unitMatrix; 
  static const ParamDefsAccessor s_enumAccessor;
};
inline bool
KalmanUpdatorSMatrix::thetaPhiWithinRange_5D(const SParVector5& V,
                                             const RangeCheckDef rcd) const
{
  static const SParVector2 thetaMin(0.0, -M_PI);
  return ((std::abs(V(Trk::phi)) <= M_PI) &&
          (V(Trk::theta) >= thetaMin((int)rcd)) && (V(Trk::theta) <= M_PI));
}
 
inline bool
KalmanUpdatorSMatrix::thetaWithinRange_5D(const SParVector5& V) const
{
  return (V(Trk::theta) >= 0.0 && (V(Trk::theta) <= M_PI));
}
 
} // end of namespace
 
#endif // TRK_KALMANUPDATOR_H