KfTrackKalmanFilter.h

Go to the documentation of this file.

/* Copyright (C) 2017-2021 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
   SPDX-License-Identifier: GPL-3.0-only
   Authors: Sergey Gorbunov [committer], Maksym Zyzak */
 
 
 
#pragma once  // include this header only once per compilation unit
 
#include "KfFieldRegion.h"
#include "KfMeasurementTime.h"
#include "KfMeasurementU.h"
#include "KfMeasurementXy.h"
#include "KfSimd.h"
#include "KfTrackParam.h"
#include "KfUtils.h"
 
#include <type_traits>
 
namespace cbm::algo::kf
{
  class Hit;
 
  enum class FitDirection
  {
    kUpstream,
    kDownstream
  };
 
  inline FitDirection operator!(FitDirection d)
  {
    return d == FitDirection::kUpstream ? FitDirection::kDownstream : FitDirection::kUpstream;
  }
 
  template<typename DataT>
  class TrackKalmanFilter {
 
   public:
    using DataTscal = kf::utils::scaltype<DataT>;
    using DataTmask = kf::utils::masktype<DataT>;
 
    TrackKalmanFilter() = default;
 
    TrackKalmanFilter(const kf::TrackParam<DataT>& t) { SetTrack(t); }
 
    TrackKalmanFilter(const DataTmask& m, bool fitV) : fMask(m), fDoFitVelocity(fitV) {}
 
    template<typename T>
    TrackKalmanFilter(const kf::TrackParam<T>& t)
    {
      SetTrack(t);
    }
 
    void SetMask(const DataTmask& m) { fMask = m; }
 
    template<typename T>
    void SetTrack(const kf::TrackParam<T>& t)
    {
      fTr.Set(t);
      fQp0 = fTr.GetQp();
    }
 
    void SetQp0(DataT qp0) { fQp0 = qp0; }
 
    kf::TrackParam<DataT>& Tr() { return fTr; }
 
    DataT& Qp0() { return fQp0; }
 
    void SetDoFitVelocity(bool v) { fDoFitVelocity = v; }
 
    void SetOneEntry(const int i0, const TrackKalmanFilter& T1, const int i1);
 
    std::string ToString(int i = -1);
 
 
    void SetParticleMass(DataT mass)
    {
      fMass  = mass;
      fMass2 = mass * mass;
    }
 
    DataT GetParticleMass() const { return fMass; }
 
    DataT GetParticleMass2() const { return fMass2; }
 
    void SetMaxExtrapolationStep(double step) { fMaxExtraplationStep = DataT(step); }
 
    DataT GetMaxExtrapolationStep() const { return fMaxExtraplationStep; }
 
    void Filter1d(const kf::MeasurementU<DataT>& m);
 
    void FilterXY(const kf::MeasurementXy<DataT>& m, bool skipUnmeasuredCoordinates = false);
 
    void FilterTime(DataT t, DataT dt2, const DataTmask& m);
 
    void FilterTime(kf::MeasurementTime<DataT> mt) { FilterTime(mt.T(), mt.Dt2(), DataTmask(mt.NdfT() > DataT(0.))); }
 
    void FilterVi(DataT vi);
 
    void MeasureVelocityWithQp();
 
    void Extrapolate(DataT z, const kf::FieldRegion<DataT>& F);
 
    void ExtrapolateStep(DataT z_out, const kf::FieldRegion<DataT>& F);
 
    void ExtrapolateLine(DataT z_out, const kf::FieldRegion<DataT>& F);
 
    void ExtrapolateLineNoField(DataT z_out);
 
    void EnergyLossCorrection(DataT radThick, FitDirection direction);
 
    void EnergyLossCorrection(int atomicZ, DataTscal atomicA, DataTscal rho, DataTscal radLen, DataT radThick,
                              FitDirection direction);
 
 
    void MultipleScattering(DataT radThick, DataT tx0, DataT ty0, DataT qp0);
 
    void MultipleScattering(DataT radThick) { MultipleScattering(radThick, fTr.GetTx(), fTr.GetTy(), fQp0); }
 
    void MultipleScatteringInThickMaterial(DataT radThick, DataT thickness, bool fDownstream);
 
 
    void GetExtrapolatedXYline(DataT z, const kf::FieldRegion<DataT>& F, DataT& extrX, DataT& extrY,
                               std::array<DataT, kf::TrackParam<DataT>::kNtrackParam>& Jx,
                               std::array<DataT, kf::TrackParam<DataT>::kNtrackParam>& Jy) const;
 
    void FilterExtrapolatedXY(const kf::MeasurementXy<DataT>& m, DataT extrX, DataT extrY,
                              const std::array<DataT, kf::TrackParam<DataT>::kNtrackParam>& Jx,
                              const std::array<DataT, kf::TrackParam<DataT>::kNtrackParam>& Jy);
 
    std::pair<DataT, DataT> ExtrapolateLineXdX2(DataT z_out) const;
 
    std::pair<DataT, DataT> ExtrapolateLineYdY2(DataT z_out) const;
 
    DataT ExtrapolateLineDxy(DataT z_out) const;
 
    void FilterWithTargetAtLine(DataT targZ, const kf::MeasurementXy<DataT>& targXYInfo,
                                const kf::FieldRegion<DataT>& F);
 
    static DataT ApproximateBetheBloch(DataT bg2);
 
    static DataT ApproximateBetheBloch(DataT bg2, DataT kp0, DataT kp1, DataT kp2, DataT kp3, DataT kp4);
 
    static std::tuple<DataT, DataT> GetChi2XChi2U(kf::MeasurementXy<DataT> m, DataT x, DataT y, DataT C00, DataT C10,
                                                  DataT C11);
 
    void GuessTrack(const DataT& trackZ, const DataT hitX[], const DataT hitY[], const DataT hitZ[], const DataT hitT[],
                    const DataT By[], const DataTmask hitW[], const DataTmask hitWtime[], int NHits);
 
   private:
    typedef const DataT cnst;
 
 
    DataTmask fMask{true};  
 
    kf::TrackParam<DataT> fTr{};  
    DataT fQp0{0.};
 
    DataT fMass{0.10565800};      
    DataT fMass2{fMass * fMass};  
 
    DataT fMaxExtraplationStep{50.};  
 
    bool fDoFitVelocity{0};  // should the track velocity be fitted as an independent parameter
 
  } _fvecalignment;
 
  // =============================================================================================
 
  template<typename DataT>
  inline std::string TrackKalmanFilter<DataT>::ToString(int i)
  {
    return fTr.ToString(i);
  }
 
 
  template<typename DataT>
  inline void TrackKalmanFilter<DataT>::SetOneEntry(const int i0, const TrackKalmanFilter& T1, const int i1)
  {
    fTr.SetOneEntry(i0, T1.fTr, i1);
    kf::utils::VecCopy<DataT, DataT, false, false>::CopyEntries(fQp0, i0, T1.fQp0, i1);
  }
 
  template<typename DataT>
  inline std::pair<DataT, DataT> TrackKalmanFilter<DataT>::ExtrapolateLineXdX2(DataT z_out) const
  {
    DataT dz = (z_out - fTr.GetZ());
    return std::pair(fTr.GetX() + fTr.GetTx() * dz, fTr.C00() + dz * (2 * fTr.C20() + dz * fTr.C22()));
  }
 
  template<typename DataT>
  inline std::pair<DataT, DataT> TrackKalmanFilter<DataT>::ExtrapolateLineYdY2(DataT z_out) const
  {
    DataT dz = (z_out - fTr.GetZ());
    return std::pair(fTr.GetY() + fTr.GetTy() * dz, fTr.C11() + dz * (DataT(2.) * fTr.C31() + dz * fTr.C33()));
  }
 
  template<typename DataT>
  inline DataT TrackKalmanFilter<DataT>::ExtrapolateLineDxy(DataT z_out) const
  {
    DataT dz = (z_out - fTr.GetZ());
    return fTr.C10() + dz * (fTr.C21() + fTr.C30() + dz * fTr.C32());
  }
 
}  // namespace cbm::algo::kf