CbmKfUtil.cxx

Go to the documentation of this file.

/* Copyright (C) 2023 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
   SPDX-License-Identifier: GPL-3.0-only
   Authors: Sergey Gorbunov [committer] */
 
#include "CbmKfUtil.h"
 
#include "FairTrackParam.h"
#include "KFParticle.h"
#include "KfDefs.h"
#include "KfFieldRegion.h"
#include "KfTrackKalmanFilter.h"
 
 
namespace cbm::kf
{
  using namespace cbm::algo;
 
  cbm::algo::kf::TrackParamD ConvertTrackParam(const FairTrackParam& par)
  {
    cbm::algo::kf::TrackParamD t;
    t.X()    = par.GetX();
    t.Y()    = par.GetY();
    t.Tx()   = par.GetTx();
    t.Ty()   = par.GetTy();
    t.Qp()   = par.GetQp();
    t.Z()    = par.GetZ();
    t.Time() = 0.;
    t.Vi()   = cbm::algo::kf::defs::SpeedOfLightInv<>;
 
    t.CovMatrix().fill(0.);
 
    t.ChiSq()     = 0.;
    t.Ndf()       = 0.;
    t.ChiSqTime() = 0.;
    t.NdfTime()   = 0.;
 
    for (int i = 0; i < 5; i++) {
      for (int j = 0; j <= i; j++) {
        t.C(i, j) = par.GetCovariance(i, j);
      }
    }
 
    t.C55() = 1.;
    t.C66() = 1.;
 
    return t;
  }
 
  FairTrackParam ConvertTrackParam(const cbm::algo::kf::TrackParamD& t)
  {
    FairTrackParam par;
    par.SetX(t.GetX());
    par.SetY(t.GetY());
    par.SetZ(t.GetZ());
    par.SetTx(t.GetTx());
    par.SetTy(t.GetTy());
    par.SetQp(t.GetQp());
    for (int i = 0; i < 5; i++) {
      for (int j = 0; j <= i; j++) {
        par.SetCovariance(i, j, t.C(i, j));
      }
    }
    return par;
  }
 
  std::optional<KFParticle> CreateKfParticle(const cbm::algo::kf::TrackParamD& trackParam, double mass, int absCharge)
  {
 
    if (!trackParam.IsConsistent(false, 1)) {
      return std::nullopt;
    }
 
    double par[6] = {0.};
 
    double tx = trackParam.GetTx();
    double ty = trackParam.GetTy();
    double qp = trackParam.GetQp();
 
    int q = 0;
    if (qp > 0.) {
      q = absCharge;
    }
    if (qp < 0.) {
      q = -absCharge;
    }
 
    double t2i = 1. / (1. + tx * tx + ty * ty);
    double t1i = sqrt(t2i);
    double t3i = t1i * t2i;
 
    double p     = 1.e4;  // limit the momentum to 10,000 GeV/c
    double DpDqp = 0.;
 
    if ((q != 0) && fabs(qp / q) >= 1e-4) {  // define the momentum from the fitted value of q/p
      p     = fabs(q / qp);
      DpDqp = -p / qp;
    }
 
    double pz = p * t1i;
    double px = tx * pz;
    double py = ty * pz;
 
    par[0] = trackParam.GetX();
    par[1] = trackParam.GetY();
    par[2] = trackParam.GetZ();
    par[3] = px;
    par[4] = py;
    par[5] = pz;
 
    //calculate covariance matrix
 
    double DpzDtx = -px * t2i;
    double DpzDty = -py * t2i;
    double DpzDqp = DpDqp * t1i;
 
    double DpxDtx = (1. + ty * ty) * t3i * p;
    double DpxDty = -tx * ty * t3i * p;
    double DpxDqp = tx * DpzDqp;
 
    double DpyDtx = DpxDty;
    double DpyDty = (1. + tx * tx) * t3i * p;
    double DpyDqp = ty * DpzDqp;
 
    double J[6][5] = {{1., 0., 0., 0., 0.},
                      {0., 1., 0., 0., 0.},
                      {0., 0., 0., 0., 0.},
                      {0., 0., DpxDtx, DpxDty, DpxDqp},
                      {0., 0., DpyDtx, DpyDty, DpyDqp},
                      {0., 0., DpzDtx, DpzDty, DpzDqp}};
 
    double CJt[5][6];
    for (int i = 0; i < 5; i++) {
      for (int j = 0; j < 6; j++) {
        CJt[i][j] = 0;
        for (int k = 0; k < 5; k++) {
          CJt[i][j] += trackParam.GetCovariance(i, k) * J[j][k];
        }
      }
    }
 
    double cov[21];
    for (int i = 0, l = 0; i < 6; i++) {
      for (int j = 0; j <= i; j++, l++) {
        cov[l] = 0;
        for (int k = 0; k < 5; k++) {
          cov[l] += J[i][k] * CJt[k][j];
        }
      }
    }
 
    KFParticle kfParticle;
    kfParticle.Create(par, cov, q, mass);
 
    // Set field to the particle
 
    algo::kf::FieldRegion<double> fieldOrig(algo::kf::GlobalField::fgOriginalFieldType,
                                            algo::kf::GlobalField::fgOriginalField);
 
    cbm::algo::kf::TrackKalmanFilter<double> fit;
    fit.SetTrack(trackParam);
    fit.SetParticleMass(mass);
    auto& tr = fit.Tr();
 
    double z0 = trackParam.GetZ();
    double z1 = z0 - 10.;
    double z2 = z0 - 20.;
 
    algo::kf::FieldValue<double> B0 = fieldOrig.Get(tr.GetX(), tr.GetY(), tr.GetZ());
 
    fit.Extrapolate(z1, fieldOrig);
 
    algo::kf::FieldValue<double> B1 = fieldOrig.Get(tr.GetX(), tr.GetY(), tr.GetZ());
 
    fit.Extrapolate(z2, fieldOrig);
 
    algo::kf::FieldValue<double> B2 = fieldOrig.Get(tr.GetX(), tr.GetY(), tr.GetZ());
 
    algo::kf::detail::ConcreteFieldRegion<double, algo::kf::EFieldMode::Interpolated> fieldInterpolated;
 
    fieldInterpolated.Set(B0, B1, B2);
 
    for (int i = 0, j = 0; j < 3; j++) {
      for (int k = 0; k < 3; k++, i++) {
        kfParticle.SetFieldCoeff(fieldInterpolated.GetCoefficients()[j][k], i);
      }
    }
 
    kfParticle.SetFieldCoeff(fieldInterpolated.GetZfirst(), 9);
 
    return kfParticle;
  }
 
}  // namespace cbm::kf