source: git/external/TrackCovariance/ObsTrk.cc@ a47edcc

#include <TMath.h>
#include <TVector3.h>
#include <TMatrixD.h>
#include <TMatrixDSym.h>
#include <TDecompChol.h>
#include <TRandom.h>
#include <iostream>
#include "SolGridCov.h"
#include "ObsTrk.h"
//
// Constructors
//
// x(3) track origin, p(3) track momentum at origin, Q charge, B magnetic field in Tesla
ObsTrk::ObsTrk(TVector3 x, TVector3 p, Double_t Q, Double_t B, SolGridCov *GC)
{
        SetB(B);
        fGC = GC;
        fGenX = x;
        fGenP = p;
        fGenQ = Q;
        fB = B;
        fGenPar.ResizeTo(5);
        fGenParMm.ResizeTo(5);
        fGenParACTS.ResizeTo(6);
        fGenParILC.ResizeTo(5);
        fObsPar.ResizeTo(5);
        fObsParMm.ResizeTo(5);
        fObsParACTS.ResizeTo(6);
        fObsParILC.ResizeTo(5);
        fCov.ResizeTo(5, 5);
        fCovMm.ResizeTo(5, 5);
        fCovACTS.ResizeTo(6, 6);
        fCovILC.ResizeTo(5, 5);
        fGenPar = XPtoPar(x,p,Q);
        fGenParMm = ParToMm(fGenPar);
        fGenParACTS = ParToACTS(fGenPar);
        fGenParILC = ParToILC(fGenPar);
        /*
        cout << "ObsTrk::ObsTrk: fGenPar";
        for (Int_t i = 0; i < 5; i++)cout << fGenPar(i) << ", ";
        cout << endl;
        */
        fObsPar = GenToObsPar(fGenPar, fGC);
        fObsParMm = ParToMm(fObsPar);
        fObsParACTS = ParToACTS(fObsPar);
        fObsParILC = ParToILC(fObsPar);
        fObsX = ParToX(fObsPar);
        fObsP = ParToP(fObsPar);
        fObsQ = ParToQ(fObsPar);
        fCovMm = CovToMm(fCov);
        fCovACTS = CovToACTS(fObsPar, fCov);
        fCovILC = CovToILC(fCov);
}
//
// x[3] track origin, p[3] track momentum at origin, Q charge, B magnetic field in Tesla
ObsTrk::ObsTrk(Double_t *x, Double_t *p, Double_t Q, Double_t B, SolGridCov* GC)
{
        SetB(B);
        fGC = GC;
        fGenX.SetXYZ(x[0],x[1],x[2]);
        fGenP.SetXYZ(p[0],p[1],p[2]);
        fGenQ = Q;
        fB = B;
        fGenPar.ResizeTo(5);
        fGenParMm.ResizeTo(5);
        fGenParACTS.ResizeTo(6);
        fGenParILC.ResizeTo(5);
        fObsPar.ResizeTo(5);
        fObsParMm.ResizeTo(5);
        fObsParACTS.ResizeTo(6);
        fObsParILC.ResizeTo(5);
        fCov.ResizeTo(5, 5);
        fCovMm.ResizeTo(5, 5);
        fCovACTS.ResizeTo(6, 6);
        fCovILC.ResizeTo(5, 5);
        fGenPar = XPtoPar(fGenX, fGenP, Q);
        fGenParACTS = ParToACTS(fGenPar);
        fGenParILC = ParToILC(fGenPar);
        /*
        cout << "ObsTrk::ObsTrk: fGenPar";
        for (Int_t i = 0; i < 5; i++)cout << fGenPar(i) << ", ";
        cout << endl;
        */
        fObsPar = GenToObsPar(fGenPar, fGC);
        fObsParACTS = ParToACTS(fObsPar);
        fObsParILC = ParToILC(fObsPar);
        fObsX = ParToX(fObsPar);
        fObsP = ParToP(fObsPar);
        fObsQ = ParToQ(fObsPar);
        fCovACTS = CovToACTS(fObsPar, fCov);
        fCovILC = CovToILC(fCov);
}
//
// Destructor
ObsTrk::~ObsTrk()
{
        fGenX.Clear();
        fGenP.Clear();
        fGenPar.Clear();
        fGenParMm.Clear();
        fGenParACTS.Clear();
        fGenParILC.Clear();
        fObsX.Clear();
        fObsP.Clear();
        fObsPar.Clear();
        fObsParMm.Clear();
        fObsParACTS.Clear();
        fObsParILC.Clear();
        fCov.Clear();
        fCovMm.Clear();
        fCovACTS.Clear();
        fCovILC.Clear();
}
//
TVectorD ObsTrk::GenToObsPar(TVectorD gPar, SolGridCov *GC)
{
        TVector3 p = ParToP(gPar);
        Double_t pt = p.Pt();
        Double_t tanTh = 1.0 / TMath::Abs(gPar(4));
        Double_t angd = TMath::ATan(tanTh)*180. / TMath::Pi();
        //
        // Check ranges
        Double_t minPt = GC->GetMinPt ();
        //if (pt < minPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is below grid range of " << minPt << endl;
        Double_t maxPt = GC->GetMaxPt();
        //if (pt > maxPt) cout << "Warning ObsTrk::GenToObsPar: pt " << pt << " is above grid range of " << maxPt << endl;
        Double_t minAn = GC->GetMinAng();
        //if (angd < minAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd 
        //      << " is below grid range of " << minAn << endl;
        Double_t maxAn = GC->GetMaxAng();
        //if (angd > maxAn) cout << "Warning ObsTrk::GenToObsPar: angle " << angd
        //      << " is above grid range of " << maxAn << endl;
        //
        TMatrixDSym Cov = GC->GetCov(pt, angd);
        fCov = Cov;
        //
        // Now do Choleski decomposition and random number extraction, with appropriate stabilization
        //
        TMatrixDSym CvN = Cov;
        TMatrixDSym DCv(5); DCv.Zero();
        TMatrixDSym DCvInv(5); DCvInv.Zero();
        for (Int_t id = 0; id < 5; id++)
        {
                Double_t dVal = TMath::Sqrt(Cov(id, id));
                DCv   (id, id) = dVal;
                DCvInv(id, id) = 1.0 / dVal;
        }
        CvN.Similarity(DCvInv);                 // Normalize diagonal to 1
        TDecompChol Chl(CvN);
        Bool_t OK = Chl.Decompose();            // Choleski decomposition of normalized matrix
        TMatrixD U = Chl.GetU();                        // Get Upper triangular matrix
        TMatrixD Ut(TMatrixD::kTransposed, U); // Transposed of U (lower triangular)
        TVectorD r(5);
        for (Int_t i = 0; i < 5; i++)r(i) = gRandom->Gaus(0.0, 1.0);            // Array of normal random numbers
        TVectorD oPar = gPar + DCv*(Ut*r);      // Observed parameter vector
        //
        return oPar;
}