MpdKinFitter.cxx 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332
  1. /// \class MpdKinFitter
  2. ///
  3. /// MPD kinematic fitter.
  4. ///
  5. /// \author Alexander Zinchenko (LHEP, JINR, Dubna)
  6. /// (original version by N.Geraksiev, formalism by A.Belyaev)
  7. #include "MpdKinFitter.h"
  8. #include "MpdKalmanFilter.h"
  9. #include "MpdKalmanTrack.h"
  10. //#include "MpdMotherFitterPart.h"
  11. #include "MpdParticle.h"
  12. #include "FairMCTrack.h"
  13. #include <TRandom3.h>
  14. #include <TVector3.h>
  15. #include <TDatabasePDG.h>
  16. //__________________________________________________________________________
  17. MpdKinFitter::MpdKinFitter(vector<TObject*> &daughts)
  18. : TObject(), fChi2(0.0), fieldConst(0.0)
  19. {
  20. /// Constructor
  21. fDaughts = daughts;
  22. Init();
  23. }
  24. //__________________________________________________________________________
  25. MpdKinFitter::MpdKinFitter(vector<MpdParticle*> &daughts)
  26. : TObject(), fChi2(0.0), fieldConst(0.0)
  27. {
  28. /// Constructor
  29. Int_t nDau = daughts.size();
  30. fDaughts.clear();
  31. for (Int_t j = 0; j < nDau; ++j) fDaughts.push_back(daughts[j]);
  32. Init();
  33. }
  34. //__________________________________________________________________________
  35. void MpdKinFitter::Init()
  36. {
  37. /// Initialization
  38. Double_t pos[3] = {0.0}, magf[3] = {0.0};
  39. MpdKalmanFilter::Instance()->GetField(pos,magf);
  40. fieldConst = 0.3 * 0.01 * magf[2] / 10;
  41. Int_t nDau = fDaughts.size();
  42. Int_t npar = nDau * 3;
  43. fx.ResizeTo(npar,1);
  44. fF.ResizeTo(1,1);
  45. fdFdx.ResizeTo(npar,1);
  46. fdydx.ResizeTo(npar,3);
  47. fVm1.ResizeTo(npar,npar);
  48. fUm1.ResizeTo(3,3);
  49. }
  50. //__________________________________________________________________________
  51. MpdKinFitter::~MpdKinFitter()
  52. {
  53. /// Destructor
  54. }
  55. //__________________________________________________________________________
  56. Double_t MpdKinFitter::DoKinFit(Int_t pdg0)
  57. {
  58. // Kinematic fit with energy conservation constrain (1C fit), minimization with lagrangian multipliers
  59. // pdg0 - mother PDG code
  60. Double_t m0 = TDatabasePDG::Instance()->GetParticle(pdg0)->Mass();
  61. return DoKinFit(m0);
  62. }
  63. //__________________________________________________________________________
  64. Double_t MpdKinFitter::DoKinFit(Double_t m0)
  65. {
  66. // Kinematic fit with energy conservation constrain (1C fit), minimization with lagrangian multipliers
  67. // m0 - mother mass
  68. //
  69. //parameters: pt, theta, phi
  70. //TODO only diagonal covariance elements used
  71. //TODO parameters:phi, theta, fieldConst/pt
  72. //TODO kalman track doesnt have a pdg code can be set as input parameters
  73. //TODO setters getters for parameters
  74. /*
  75. if (vDaught.size() != 2) {
  76. cout << "DoKinFit requires two daughters" << endl;
  77. return -1;
  78. }
  79. */
  80. //constants
  81. const Int_t iter_limiter = 10; //limit iterations, for pt about 10 should be enough
  82. const Double_t epsLa = 1e-13; //1e-13; convergence limiters, different values can be used
  83. const Double_t epsX = 1e-14; //1e-14; TODO use them as input parameters of function or setters?
  84. Int_t pdg[9];
  85. Int_t defpdg[2] = {2212, 211}; //proton, pion//default values for kalman tracks
  86. Double_t mD[9];
  87. //TMatrixD xm(6,1);
  88. //TMatrixD Gm1(6,6);
  89. TMatrixD xm(fx);
  90. TMatrixD Gm1(fVm1);
  91. //for mc tracks and smearing
  92. TVector3 mom;
  93. Double_t gamma = 0.;
  94. //TMatrixD sigma(6,1);
  95. TMatrixD sigma(fx);
  96. TRandom3 r(0);
  97. Double_t c1, c2, c3, cc1, cc2, cc3; //TODO different coefficients for parameters?
  98. c1 = c2 = c3 = 0.005;
  99. cc1 = cc2 = cc3 = 0.1;
  100. // Setting parameters based on input classes: MpdParticle, MpdTpcKalmanTrack or FairMCTrack
  101. for (Int_t i = 0; i < fDaughts.size(); i++) {
  102. Int_t j = 3 * i; // 3 params per daughter use j for first daughter j = 0,1,2, for second j = 3,4,5
  103. TObject *od = fDaughts[i]; //object daughter
  104. if (od->InheritsFrom("MpdParticle")) {
  105. MpdParticle *d1 = (MpdParticle*) od;
  106. pdg[i] = d1->GetPdg();
  107. xm(j,0) = fieldConst/TMath::Abs(d1->Getq()(2,0)); //pt
  108. xm(j+1,0) = d1->Getq()(1,0); //theta
  109. xm(j+2,0) = d1->Getq()(0,0); //phi
  110. TMatrixD D = d1->GetD();
  111. //AZ Gm1(j,j) = D(2,2)*xm(0,0)*xm(0,0)/fieldConst/fieldConst;
  112. //Gm1(j,j) = D(2,2)*xm(j,0)*xm(j,0)/fieldConst/fieldConst; //AZ ????
  113. Gm1(j,j) = D(2,2) * xm(j,0) * xm(j,0) / d1->Getq()(2,0) / d1->Getq()(2,0); //AZ
  114. Gm1(j+1,j+1) = D(1,1);
  115. Gm1(j+2,j+2) = D(0,0);
  116. }
  117. else if (od->InheritsFrom("MpdKalmanTrack")) {
  118. MpdKalmanTrack *d1 = (MpdKalmanTrack*) od;
  119. pdg[i] = defpdg[i]; // FIXME default pdg code
  120. xm(j,0) = TMath::Abs(d1->Pt()); // pt
  121. xm(j+1,0) = d1->Theta();
  122. xm(j+2,0) = d1->Phi();
  123. TMatrixD D = *d1->GetCovariance();
  124. //AZ Gm1(j,j) = D(4,4)*xm(0,0)*xm(0,0);
  125. Gm1(j,j) = D(4,4)*xm(j,0)*xm(j,0);
  126. Gm1(j+1,j+1) = D(3,3);
  127. Gm1(j+2,j+2) = D(2,2);
  128. }
  129. else if (od->InheritsFrom("FairMCTrack")) { // mc tracks with smearing
  130. FairMCTrack *d1 = (FairMCTrack*) od;
  131. pdg[i] = d1->GetPdgCode();
  132. d1->GetMomentum(mom);
  133. //smearing // get a new random value for each param
  134. gamma = r.Gaus(0,1); sigma(j,0) = c1*mom.Perp()*(1+cc1*gamma);
  135. gamma = r.Gaus(0,1); sigma(j+1,0) = c2*(1+cc2*gamma);
  136. gamma = r.Gaus(0,1); sigma(j+2,0) = c3*(1+cc3*gamma);
  137. gamma = r.Gaus(0,1); xm(j,0) = mom.Perp() + sigma(j,0)*gamma;
  138. gamma = r.Gaus(0,1); xm(j+1,0) = mom.Theta() + sigma(j+1,0)*gamma;
  139. gamma = r.Gaus(0,1); xm(j+2,0) = mom.Phi() + sigma(j+2,0)*gamma;
  140. Gm1(j,j) = sigma(j,0)*sigma(j,0);
  141. Gm1(j+1,j+1) = sigma(j+1,0)*sigma(j+1,0);;
  142. Gm1(j+2,j+2) = sigma(j+2,0)*sigma(j+2,0);;
  143. }
  144. mD[i] = TDatabasePDG::Instance()->GetParticle(pdg[i])->Mass();
  145. } //for n daughters
  146. if (Gm1(0,0) == 0) return -2; // vertex fit has failed
  147. fx = xm; // initial value x0
  148. CalculateFdFdx(fx, m0, mD); // Calc F dFdx with new parameters
  149. Int_t niter = 0;
  150. Bool_t twice = kFALSE;
  151. Bool_t convergence = kFALSE;
  152. Bool_t loopPossible = kTRUE;
  153. TMatrixD H(1,1);
  154. TMatrixD Hm1(1,1); // inverted H
  155. TMatrixD la(1,1);
  156. while (loopPossible){
  157. niter++;
  158. if (niter >= iter_limiter) { loopPossible = kFALSE; return -1; }
  159. TMatrixD dFdx0 = fdFdx; // save old value of dFdx for psi calculation
  160. //E = Gm1 * dFdx;
  161. TMatrixD E(Gm1, TMatrixD::kMult, fdFdx);
  162. //H = Et*dFdx;
  163. H = TMatrixD(E, TMatrixD::kTransposeMult, fdFdx);
  164. Double_t detH = H.Determinant();
  165. if (detH == 0) { // has inverse if DetA != 0
  166. loopPossible = kFALSE;
  167. return -1;
  168. }
  169. Hm1 = TMatrixD(TMatrixD::kInverted, H);
  170. //b = F + (xm - x).T()*dFdx;
  171. TMatrixD difx = xm;
  172. difx -= fx;
  173. TMatrixD tmp0(difx, TMatrixD::kTransposeMult, fdFdx);
  174. TMatrixD b = fF;
  175. b += tmp0;
  176. TMatrixD bt(TMatrixD::kTransposed, b);
  177. //cout<<"bt2";bt.Print();
  178. //la = Hm1*bt;
  179. la = TMatrixD(Hm1, TMatrixD::kMult, bt);
  180. //x = xm - E*la;
  181. TMatrixD tmp1(E, TMatrixD::kMult, la);
  182. fx = xm;
  183. fx -= tmp1;
  184. //Calc F dFdx with new parameters
  185. CalculateFdFdx(fx, m0, mD);
  186. //dLla = F*Hm1*Ft;
  187. TMatrixD Ft(TMatrixD::kTransposed, fF);
  188. TMatrixD tmp2(Hm1, TMatrixD::kMult, Ft);
  189. TMatrixD dLla(fF, TMatrixD::kMult, tmp2);
  190. if (dLla(0,0) < epsLa) {
  191. TMatrixD Psi = fdFdx;
  192. Psi -= dFdx0;
  193. Psi *= la;
  194. TMatrixD tmp3(Gm1, TMatrixD::kMult, Psi);
  195. TMatrixD dLx(Psi, TMatrixD::kTransposeMult, tmp3);
  196. if (dLx(0,0) >= epsX) {
  197. twice = kFALSE;
  198. } else {
  199. if (twice) {
  200. loopPossible = kFALSE;
  201. convergence = kTRUE;
  202. break;
  203. } else {
  204. twice = kTRUE;
  205. }
  206. }
  207. } else {
  208. twice = kFALSE;
  209. } // epsLa
  210. } // while loop
  211. //chi2 = lat*H*la;
  212. TMatrixD tmp4(H, TMatrixD::kMult, la);
  213. TMatrixD kinchi2(la, TMatrixD::kTransposeMult, tmp4);
  214. if (kinchi2(0,0) == 0.) return -1;
  215. //E = Gm1*dFdx;
  216. TMatrixD E(Gm1, TMatrixD::kMult, fdFdx); // E with last dFdx, recalculate H, Hm1 or use previous?
  217. //Vm1 = Gm1 - E*Hm1*Et; //updated covariance matrix of daughter parameters
  218. TMatrixD tmp5(TMatrixD::kTransposed, E);
  219. TMatrixD tmp6(Hm1, TMatrixD::kMult, tmp5);
  220. TMatrixD tmp7(E, TMatrixD::kMult, tmp6);
  221. fVm1 = Gm1;
  222. fVm1 -= tmp7;
  223. //Um1 = dydxt*Vm1*dydx; //updated covariance matrix of mother parameters
  224. TMatrixD tmp8(fVm1, TMatrixD::kMult, fdydx);
  225. fUm1 = TMatrixD(fdydx, TMatrixD::kTransposeMult, tmp8);
  226. return kinchi2(0,0);
  227. //TODO non-diagonal elements ? convert C/pt to pt?
  228. /*
  229. Gm1(0,1) = part1D(2,1); //Cov(C/pt and theta) multiply by xm/fieldConst ?
  230. Gm1(0,2) = part1D(2,0); //Cov(C/pt phi)
  231. Gm1(1,0) = part1D(1,2); //Cov(theta C/pt)
  232. Gm1(1,2) = part1D(1,0); //Cov(theta phi)
  233. Gm1(2,0) = part1D(0,2); //Cov(phi C/pt)
  234. Gm1(2,1) = part1D(0,1); //Cov(phi, theta)
  235. Gm1(3,4) = part2D(2,1);
  236. Gm1(3,5) = part2D(2,0);
  237. Gm1(4,3) = part2D(1,2);
  238. Gm1(4,5) = part2D(1,0);
  239. Gm1(5,4) = part2D(0,2);
  240. Gm1(5,4) = part2D(0,1);
  241. */
  242. }
  243. //__________________________________________________________________________
  244. void MpdKinFitter::CalculateFdFdx(TMatrixD mtr, Double_t m0, Double_t* mD)
  245. {
  246. // Calculate F and derivatives, 1C - fit
  247. Int_t nDau = fDaughts.size(), ndim = nDau * 3;;
  248. Double_t px0 = 0, py0 = 0, pz0 = 0, esum = 0;
  249. //Matrix containing partial derivatives of momentum x,y,z x dxi = 6x3
  250. TMatrixD pDp(ndim,3), deidx(ndim,1);
  251. for (Int_t i = 0; i < nDau; ++i) {
  252. Int_t j = i * 3;
  253. Double_t pt = mtr(j,0);
  254. Double_t th = mtr(j+1,0);
  255. Double_t phi = mtr(j+2,0);
  256. Double_t costh = TMath::Cos(th);
  257. Double_t cosphi = TMath::Cos(phi);
  258. Double_t sinth = TMath::Sin(th);
  259. Double_t sinphi = TMath::Sin(phi);
  260. Double_t cotth = costh / sinth;
  261. //Double_t cosphi1Mphi2 = TMath::Cos(phi1 - phi2);
  262. Double_t p = pt / sinth;
  263. Double_t p2 = p * p;
  264. //Double_t pt2sin2th = pt * pt / (sinth * sinth);
  265. Double_t e = TMath::Sqrt (mD[i] * mD[i] + p2);
  266. esum += e;
  267. Double_t px = pt * cosphi;
  268. Double_t py = pt * sinphi;
  269. Double_t pz = pt * cotth;
  270. px0 += px;
  271. py0 += py;
  272. pz0 += pz;
  273. pDp(j+0,0) = cosphi; pDp(j+0,1) = sinphi; pDp(j+0,2) = cotth;
  274. //pDp(j+1,0) = 0; pDp(j+1,1) = 0; pDp(j+1,2) = - pt12sin2th1/pt1;
  275. pDp(j+1,0) = 0; pDp(j+1,1) = 0; pDp(j+1,2) = -p / sinth;
  276. pDp(j+2,0) = -py; pDp(j+2,1) = px; pDp(j+2,2) = 0;
  277. //Matrix containing partial derivatives of e
  278. //deidx(j+0,0) = pt12sin2th1/(pt1*e1);
  279. //deidx(j+1,0) = - pt12sin2th1*cotth1/e1;
  280. deidx(j+0,0) = p / (sinth * e);
  281. deidx(j+1,0) = - p2 * cotth / e;
  282. deidx(j+2,0) = 0;
  283. }
  284. Double_t e0 = TMath::Sqrt (m0*m0 + px0*px0 + py0*py0 + pz0*pz0);
  285. fF(0,0) = e0 - esum;
  286. fdydx = pDp;
  287. for (Int_t i = 0; i < ndim; i++)
  288. fdFdx(i,0) = (px0 * pDp(i,0) + py0 * pDp(i,1) + pz0 * pDp(i,2)) / e0 - deidx(i,0);
  289. }
  290. //__________________________________________________________________________
  291. ClassImp(MpdKinFitter)