MpdMotherFitterPart.cxx 29 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910
  1. /// \class MpdMotherFitterPart
  2. ///
  3. /// Kalman filter mother particle fitter for the MPD detector (using MpdParticle)
  4. /// \author Alexander Zinchenko (LHEP, JINR, Dubna)
  5. #include "MpdMotherFitterPart.h"
  6. #include "MpdKalmanFilter.h"
  7. #include "MpdKalmanGeoScheme.h"
  8. #include "MpdKalmanTrack.h"
  9. #include "MpdKalmanHit.h"
  10. #include "MpdHelix.h"
  11. #include "MpdVertex.h"
  12. #include "MpdMCTrack.h"
  13. #include "FairField.h"
  14. #include "FairRunAna.h"
  15. #include "FairTask.h"
  16. #include <TMath.h>
  17. #include <TMatrixD.h>
  18. #include <TVector3.h>
  19. #include <iostream>
  20. using std::cout;
  21. using std::endl;
  22. MpdMotherFitterPart* MpdMotherFitterPart::fgMF = 0x0;
  23. //__________________________________________________________________________
  24. MpdMotherFitterPart::MpdMotherFitterPart()
  25. : FairTask()
  26. {
  27. /// Default constructor
  28. fCovar.ResizeTo(3,3);
  29. }
  30. //__________________________________________________________________________
  31. MpdMotherFitterPart::MpdMotherFitterPart(const char *name, const char *title)
  32. : FairTask(name)
  33. {
  34. /// Constructor
  35. fgMF = this;
  36. fCovar.ResizeTo(3,3);
  37. }
  38. //__________________________________________________________________________
  39. MpdMotherFitterPart* MpdMotherFitterPart::Instance()
  40. {
  41. /// Get pointer to the mother fitter singleton object
  42. if (!fgMF){
  43. fgMF = new MpdMotherFitterPart;
  44. fgMF->Init();
  45. // automatic destruction is forced
  46. std::atexit(DestroyInstance);
  47. }
  48. return fgMF;
  49. }
  50. //__________________________________________________________________________
  51. MpdMotherFitterPart* MpdMotherFitterPart::Instance(const char *name, const char *title)
  52. {
  53. /// Get pointer to the mother fitter singleton object
  54. if (!fgMF){
  55. fgMF = new MpdMotherFitterPart(name, title);
  56. fgMF->Init();
  57. // automatic destruction is forced
  58. std::atexit(DestroyInstance);
  59. }
  60. return fgMF;
  61. }
  62. //__________________________________________________________________________
  63. MpdMotherFitterPart::~MpdMotherFitterPart()
  64. {
  65. /// Destructor
  66. //FairRootManager *manager = FairRootManager::Instance();
  67. //manager->Write();
  68. }
  69. //__________________________________________________________________________
  70. InitStatus MpdMotherFitterPart::Init() {
  71. cout << "InitStatus MpdMotherFitterPart::Init\n\n";
  72. Double_t bZ = 5.;
  73. FairField * magField = FairRunAna::Instance()->GetField();
  74. if (!magField || TMath::Abs(magField->GetBz(0,0,0)) < 0.01) {
  75. cout << " -I- MpdMotherFitterPart::Init - Using the default constant magnetic field Bz = 5 kG " << endl;
  76. } else bZ = TMath::Abs(magField->GetBz(0,0,0));
  77. fieldConst = 0.3 * 0.01 * bZ / 10;
  78. return kSUCCESS;
  79. }
  80. //__________________________________________________________________________
  81. InitStatus MpdMotherFitterPart::ReInit()
  82. {
  83. //fMagField = FairRunAna::Instance()->GetField(); // !!! interim solution !!!
  84. return kSUCCESS;
  85. }
  86. //__________________________________________________________________________
  87. void MpdMotherFitterPart::Reset()
  88. {
  89. ///
  90. //cout << " MpdMotherFitterPart::Reset " << endl;
  91. }
  92. //__________________________________________________________________________
  93. void MpdMotherFitterPart::Register()
  94. {
  95. ///
  96. //FairRootManager::Instance()->
  97. //Register("TpcLheKalmanTrack","TpcLheKalmanTrack", fTracks, kFALSE);
  98. }
  99. //__________________________________________________________________________
  100. void MpdMotherFitterPart::Finish()
  101. {
  102. ///
  103. }
  104. //__________________________________________________________________________
  105. void MpdMotherFitterPart::Exec(Option_t * option)
  106. {
  107. ///
  108. }
  109. //__________________________________________________________________________
  110. Double_t MpdMotherFitterPart::BuildMother(MpdParticle *mother, vector<MpdParticle*> &vDaught)
  111. {
  112. /// Build mother particle from daughters which were smoothed
  113. /// according to the decay vertex constraint (after FindVertex)
  114. TVector3 vtx;
  115. Double_t chi2 = MpdMotherFitterPart::Instance()->FindVertex(vDaught,vtx);
  116. if (chi2 < -1.0) return chi2; // failed to find decay vertex (too high chi2)
  117. Int_t nDaught = vDaught.size();
  118. // Check for sanity
  119. for (Int_t id = 0; id < nDaught; ++id) {
  120. Double_t theta = vDaught[id]->Theta();
  121. if (theta < 0 || theta > TMath::Pi()) return -chi2; // weird track
  122. }
  123. mother->SetChi2(chi2);
  124. TVector3 mom3;
  125. Int_t charge = 0;
  126. Double_t energy = 0.0;
  127. for (Int_t i = 0; i < nDaught; ++i) {
  128. MpdParticle* part = vDaught[i];
  129. charge += part->GetCharge();
  130. mom3 += part->Momentum3();
  131. part->FillJ();
  132. Double_t ptot = part->Momentum3().Mag();
  133. energy += TMath::Sqrt (part->GetMass() * part->GetMass() + ptot * ptot);
  134. mother->AddDaughter(part->GetIndx());
  135. }
  136. mother->SetCharge(charge);
  137. mother->SetMass(TMath::Sqrt (energy*energy - mom3.X()*mom3.X() - mom3.Y()*mom3.Y() - mom3.Z()*mom3.Z()));
  138. mother->Setx(vDaught[0]->Getx());
  139. TMatrixD qm(3,1);
  140. qm(0,0) = mom3.Phi();
  141. qm(1,0) = mom3.Theta();
  142. if (charge == 0) qm(2,0) = mom3.Mag();
  143. else qm(2,0) = -fieldConst / mom3.Pt() * TMath::Abs(charge);
  144. mother->Setq(qm);
  145. ParamsAtDca(mother); // compute params at DCA
  146. //return chi2; //
  147. mother->FillJinv(mom3);
  148. // Compute covariance matrix
  149. TMatrixD en(3,3);
  150. TMatrixD ec(3,3);
  151. for (Int_t i = 0; i < nDaught; ++i) {
  152. MpdParticle* part = vDaught[i];
  153. // E += E*Jt
  154. TMatrixD jt = TMatrixD(TMatrixD::kTransposed,part->GetJ());
  155. TMatrixD tmp = TMatrixD(part->GetE(),TMatrixD::kMult,jt);
  156. if (part->GetCharge()) {
  157. // Charged track
  158. ec += tmp;
  159. } else {
  160. // Neutral
  161. en += tmp;
  162. }
  163. }
  164. TMatrixD etot = ec;
  165. etot += en;
  166. TMatrixD c = GetCovariance();
  167. TMatrixD qtot = ComputeQmatr(vDaught);
  168. TMatrixD ck0(5,1);
  169. //ComputeAandB(mother->Getx(), *mother, fA, fB, ck0);
  170. ComputeAandB(mother->Getx(), *mother, mother->GetA(), mother->GetB(), ck0);
  171. // Covar. matrix
  172. TMatrixD at(TMatrixD::kTransposed,mother->GetA());
  173. TMatrixD tmp11(c,TMatrixD::kMult,at);
  174. TMatrixD tmp12(mother->GetA(),TMatrixD::kMult,tmp11);
  175. TMatrixD bt(TMatrixD::kTransposed,mother->GetB());
  176. TMatrixD tmp21(mother->GetJinv(),TMatrixD::kTransposeMult,bt);
  177. TMatrixD tmp22(etot,TMatrixD::kMult,tmp21);
  178. TMatrixD tmp23(mother->GetA(),TMatrixD::kMult,tmp22);
  179. TMatrixD tmp31(etot,TMatrixD::kTransposeMult,at);
  180. TMatrixD tmp32(mother->GetJinv(),TMatrixD::kMult,tmp31);
  181. TMatrixD tmp33(mother->GetB(),TMatrixD::kMult,tmp32);
  182. TMatrixD tmp41(mother->GetJinv(),TMatrixD::kTransposeMult,bt);
  183. TMatrixD tmp42(qtot,TMatrixD::kMult,tmp41);
  184. TMatrixD tmp43(mother->GetJinv(),TMatrixD::kMult,tmp42);
  185. TMatrixD tmp44(mother->GetB(),TMatrixD::kMult,tmp43);
  186. TMatrixD gm(5,5);
  187. gm = tmp12;
  188. gm += tmp23;
  189. gm += tmp33;
  190. gm += tmp44;
  191. //cout << " Mother covariance " << endl;
  192. //fG.Print();
  193. gm.Invert(); // mother weight
  194. mother->SetG(gm);
  195. return chi2;
  196. }
  197. //__________________________________________________________________________
  198. Double_t MpdMotherFitterPart::BuildMother(MpdParticle* mother, vector<MpdKalmanTrack*> &vTracks, vector<MpdParticle*> &vDaught)
  199. {
  200. /// Build mother particle from daughters which were smoothed
  201. /// according to the decay vertex constraint (after FindVertex).
  202. /// Daughters are built from tracks and parametrized at their
  203. /// intersection point.
  204. vDaught.clear();
  205. vector<MpdHelix> vhel;
  206. // Create 2 helices and cross them
  207. for (Int_t itr = 0; itr < 2; ++itr) {
  208. Double_t rad = vTracks[itr]->GetPosNew();
  209. Double_t phi = 0.0;
  210. if (rad > 1.e-6) phi = (*vTracks[itr]->GetParam())(0,0) / rad;
  211. vhel.push_back (MpdHelix (vTracks[itr]->Momentum3(), TVector3(rad*TMath::Cos(phi),rad*TMath::Sin(phi),(*vTracks[itr]->GetParam())(1,0)),
  212. vTracks[itr]->Charge()));
  213. }
  214. pair<Double_t,Double_t> paths = vhel[0].pathLengths(vhel[1]);
  215. TVector3 cross;
  216. Double_t xyz[3] = {0};
  217. Int_t ntr = vTracks.size();
  218. if (paths.first < 100.0 || paths.second < 100.0) {
  219. //MpdKalmanHit hit;
  220. //hit.SetType(MpdKalmanHit::kFixedR);
  221. cross = vhel[0].at(paths.first);
  222. cross += vhel[1].at(paths.second);
  223. cross *= 0.5;
  224. cross.GetXYZ(xyz);
  225. /*
  226. for (Int_t itr = 0; itr < ntr; ++itr) {
  227. //Double_t s = vhel[itr].pathLength(cross);
  228. //TVector3 pca = vhel[itr].at(s);
  229. //hit.SetPos(pca.Pt());
  230. //MpdKalmanFilter::Instance()->PropagateToHit(vTacks[itr],&hit,kFALSE);
  231. }
  232. */
  233. }
  234. //xyz[0] = xyz[1] = xyz[2] = 0;
  235. for (Int_t itr = 0; itr < ntr; ++itr) {
  236. MpdKalmanFilter::Instance()->FindPca(vTracks[itr],xyz);
  237. vTracks[itr]->SetParam(*vTracks[itr]->GetParamNew());
  238. vTracks[itr]->Weight2Cov(); //AZ
  239. vDaught.push_back(new MpdParticle(*vTracks[itr],-1,0.1396,xyz));
  240. }
  241. Double_t chi2 = BuildMother (mother, vDaught);
  242. // Bring back to master frame in transverse plane
  243. for (Int_t j = 0; j < 2; ++j) mother->Getx()(j,0) += xyz[j];
  244. return chi2;
  245. }
  246. //__________________________________________________________________________
  247. void MpdMotherFitterPart::EvalVertex(vector<MpdParticle*> vDaught)
  248. {
  249. /// Evaluate decay vertex position as PCA of helices
  250. // Create MpdHelix's
  251. Int_t nDaught = vDaught.size(), i0 = 0;
  252. MpdHelix** helix = new MpdHelix* [nDaught];
  253. TVector3 sum, p1, p2, p0;
  254. for (Int_t i = 0; i < nDaught; ++i) {
  255. Double_t dip = TMath::PiOver2() - vDaught[i]->Theta();
  256. Double_t cur = TMath::Abs (vDaught[i]->GetMeas(4));
  257. if (vDaught[i]->GetCharge() == 0) cur = numeric_limits<double>::epsilon();
  258. Int_t h = (Int_t) TMath::Sign(1.1,vDaught[i]->GetMeas(4));
  259. //Double_t dca = vDaught[i]->Dca();
  260. Double_t phase = vDaught[i]->GetMeas(2) - TMath::PiOver2() * h;
  261. //Double_t x = dca * TMath::Sin(vDaught[i]->Phi());
  262. //Double_t y = -dca * TMath::Cos(vDaught[i]->Phi());
  263. Double_t x = vDaught[i]->GetXY(0);
  264. Double_t y = vDaught[i]->GetXY(1);
  265. TVector3 o(x, y, vDaught[i]->GetMeas(1));
  266. helix[i] = new MpdHelix(cur, dip, phase, o, h);
  267. //o.Print();
  268. //*
  269. if (vDaught[i]->GetCharge() == 0) {
  270. p0.SetXYZ(vDaught[i]->Getx()(0,0),vDaught[i]->Getx()(1,0),vDaught[i]->Getx()(2,0));
  271. i0 = 1;
  272. //p0.SetXYZ(0,0,0);
  273. //*
  274. TVector3 dcaV(x, y, vDaught[i]->GetMeas(1));
  275. //p0.Print();
  276. //dcaV.Print();
  277. p0 += dcaV;
  278. p0 *= 0.5;
  279. //*/
  280. }
  281. //*/
  282. }
  283. Int_t ncombs = 0, nD1 = nDaught - 1;
  284. Double_t pathMax = 0.0;
  285. for (Int_t i = 0; i < nD1; ++i) {
  286. for (Int_t j = i + 1; j < nDaught; ++j) {
  287. //if (TMath::Abs(helix[j]->curvature()) <= numeric_limits<double>::epsilon()) {
  288. // straight line
  289. //Double_t dx = helix[j]
  290. pair<Double_t,Double_t> paths = helix[i]->pathLengths(*helix[j]);
  291. p1 = helix[i]->at(paths.first);
  292. p2 = helix[j]->at(paths.second);
  293. //cout << " Intersection: " << helix[i]->period() << " " << paths.first << " " << helix[j]->period() << " " << paths.second << " " << (p1-p2).Mag() << endl;
  294. sum += (p1+p2);
  295. ncombs += 2;
  296. pathMax = TMath::Max (pathMax, TMath::Abs(paths.first));
  297. pathMax = TMath::Max (pathMax, TMath::Abs(paths.second));
  298. }
  299. }
  300. if (pathMax > 80.) p1.SetXYZ(0,0,0);
  301. else p1 = sum * (1./ncombs);
  302. if (i0) p0.GetXYZ(fVtx);
  303. else p1.GetXYZ(fVtx);
  304. for (Int_t i = 0; i < nDaught; ++i) delete helix[i];
  305. //sum *= 0.5;
  306. //sum.GetXYZ(fVtx); // half-sum
  307. delete [] helix;
  308. }
  309. //__________________________________________________________________________
  310. Double_t MpdMotherFitterPart::FindVertex(vector<MpdParticle*> vDaught, TVector3 &vtx)
  311. {
  312. /// Kalman filter based secondary vertex fitter
  313. const Int_t nPass = 3; // number of iterations
  314. const Double_t cutChi2 = 1000.; // chi2-cut
  315. EvalVertex(vDaught);
  316. Int_t nDaught = vDaught.size();
  317. TMatrixD c(3,3), cov(3,3), xk0(3,1), xk(3,1), ck0(5,1);
  318. TMatrixD a(5,3), b(5,3);
  319. MpdKalmanHit hit;
  320. Double_t chi2 = 0;//, chi2o = 0;
  321. xk.SetMatrixArray(fVtx);
  322. //xk += 1.0; // test
  323. c(0,0) = c(1,1) = c(2,2) = 1;
  324. for (Int_t ipass = 0; ipass < nPass; ++ipass) {
  325. //chi2o = chi2;
  326. chi2 = 0.;
  327. //c.Zero(); // new 05.06.17
  328. c(0,0) = c(1,1) = c(2,2) = 100; // new 05.06.17
  329. if (ipass == 0) cov = TMatrixD(TMatrixD::kInverted,c);
  330. Int_t ibeg = 0, iend = nDaught, istep = 1;
  331. if (ipass % 2 > 0) { ibeg = nDaught-1; iend = -1; istep = -1; }
  332. for (Int_t itr = ibeg; itr != iend; itr+=istep) {
  333. xk0 = xk; // xk0 stands for x(k-1)
  334. cov = TMatrixD(TMatrixD::kInverted,c);
  335. MpdParticle *part = vDaught[itr];
  336. ComputeAandB(xk0,*part,a,b,ck0); // compute matrices of derivatives
  337. TMatrixD g = part->GetG();
  338. // W = (Bt*G*B)'
  339. TMatrixD tmp(g,TMatrixD::kMult,b);
  340. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  341. w.Invert();
  342. // Gb = G - G*B*W*Bt*G
  343. TMatrixD tmp1(b,TMatrixD::kTransposeMult,g);
  344. TMatrixD tmp2(w,TMatrixD::kMult,tmp1);
  345. TMatrixD tmp3(b,TMatrixD::kMult,tmp2);
  346. TMatrixD tmp4(g,TMatrixD::kMult,tmp3);
  347. TMatrixD gb = g;
  348. gb -= tmp4;
  349. // Ck = ((Ck-1)' + At*Gb*A)'
  350. TMatrixD tmp5(gb,TMatrixD::kMult,a);
  351. c = TMatrixD(a,TMatrixD::kTransposeMult,tmp5);
  352. c += cov;
  353. c.Invert();
  354. // xk = Ck*((Ck-1)'x(k-1)+At*Gb*(m-ck0))
  355. TMatrixD m = part->GetMeas();
  356. //m.Print();
  357. m -= ck0; // m-ck0
  358. //cout << " m-ck0: " << endl;
  359. //ck0.Print();
  360. //m.Print();
  361. TMatrixD tmp11(gb,TMatrixD::kMult,m);
  362. TMatrixD tmp12(a,TMatrixD::kTransposeMult,tmp11);
  363. TMatrixD tmp13(cov,TMatrixD::kMult,xk0);
  364. tmp13 += tmp12;
  365. xk = TMatrixD(c,TMatrixD::kMult,tmp13);
  366. //cout << " Vertex: " << itr << " " << track->GetTrackID() << " " << xk(0,0) << " " << xk(1,0) << " " << xk(2,0) << endl;
  367. // qk = W*Bt*G*(m-ck0-A*xk)
  368. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  369. tmp21 *= -1;
  370. tmp21 += m; // m-ck0-A*xk
  371. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  372. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  373. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  374. // Residual m-ck0-A*xk-B*qk
  375. TMatrixD r = tmp21;
  376. TMatrixD tmp31(b,TMatrixD::kMult,qk);
  377. r -= tmp31;
  378. //cout << " Residual matrix: " << endl;
  379. //r.Print();
  380. //qk.Print();
  381. // Chi2 = rt*G*r + (xk-x(k-1))t*(Ck-1)'*(xk-x(k-1))
  382. TMatrixD tmp41(g,TMatrixD::kMult,r);
  383. TMatrixD chi21(r,TMatrixD::kTransposeMult,tmp41);
  384. //chi21.Print();
  385. TMatrixD dx = xk;
  386. dx -= xk0;
  387. //Double_t dxMax = dx.Max();
  388. //if (dxMax > 7*TMath::Sqrt(cov.Max())) return -cutChi2; //AZ-050121
  389. TMatrixD tmp42(cov,TMatrixD::kMult,dx);
  390. TMatrixD chi22(dx,TMatrixD::kTransposeMult,tmp42);
  391. //chi22.Print();
  392. if (chi21(0,0) < 0 || chi22(0,0) < 0) {
  393. cout << " !!! Chi2 < 0 " << chi21(0,0) << " " << chi22(0,0) << " " << ipass << " " << itr << " " << part->GetMeas(4) << endl;
  394. //exit(0);
  395. }
  396. chi21 += chi22;
  397. chi2 += chi21(0,0);
  398. //if (chi2 > cutChi2) {
  399. if (chi2 > cutChi2 || chi21(0,0) < 0 || chi22(0,0) < 0) {
  400. for (Int_t i = 0; i < 3; ++i) vtx[i] = fVtx[i];
  401. /*
  402. for (Int_t i = 0; i < nDaught; ++i) {
  403. MpdKalmanTrack trTmp = *vDaught[i];
  404. trTmp.SetPos(trTmp.GetPosNew());
  405. trTmp.SetParamNew(*trTmp.GetParam());
  406. if (trTmp.GetNode() != "") trTmp.SetNode(""); // 25-jul-2012
  407. MpdKalmanFilter::Instance()->FindPca(&trTmp,fVtx);
  408. vDaught[i]->SetParam(*trTmp.GetParamNew());
  409. vDaught[i]->SetPosNew(trTmp.GetPosNew());
  410. }
  411. */
  412. // AZ-Debug
  413. //cout << " !!! Too high chi2: " << ipass << " " << itr << " " << chi2 << " " << chi22(0,0) << " " << chi21(0,0) << " " << vtx[0] << " " << vtx[1] << " " << vtx[2] << " " << fVtx[0] << " " << fVtx[1] << " " << fVtx[2] << " " << tracks[0]->GetNode() << " " << tracks[1]->GetNode() << " ids: " << tracks[0]->GetTrackID() << " " << tracks[1]->GetTrackID() << " " << xk0(0,0) << " " << xk0(1,0) << " " << xk0(2,0) << " " << xk(0,0) << " " << xk(1,0) << " " << xk(2,0) << endl;
  414. return -cutChi2;
  415. }
  416. //cout << ipass << " " << itr << " " << chi2 << endl;
  417. } // for (Int_t itr = 0; itr < nDaught;
  418. } // for (Int_t ipass = 0; ipass < nPass;
  419. for (Int_t i = 0; i < 3; ++i) vtx[i] = fVtx[i] = xk(i,0);
  420. fCovar = c;
  421. Smooth(vDaught);
  422. return chi2;
  423. }
  424. //__________________________________________________________________________
  425. void MpdMotherFitterPart::Smooth(vector<MpdParticle*> vDaught)
  426. {
  427. /// Smooth particle momentum and corresponding covariance matrices
  428. TMatrixD c(3,3), xk(3,1), ck0(5,1);
  429. TMatrixD a(5,3), b(5,3);
  430. xk.SetMatrixArray(fVtx);
  431. Int_t nDaught = vDaught.size();
  432. for (Int_t itr = 0; itr < nDaught; ++itr) {
  433. MpdParticle *part = vDaught[itr];
  434. TMatrixD g = part->GetG(); // track weight matrix
  435. ComputeAandB(xk,*part,a,b,ck0); // compute matrices of derivatives
  436. // W = (Bt*G*B)'
  437. TMatrixD tmp(g,TMatrixD::kMult,b);
  438. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  439. w.Invert();
  440. TMatrixD m = part->GetMeas();
  441. m -= ck0; // m-ck0
  442. // qk = W*Bt*G*(m-ck0-A*xk)
  443. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  444. tmp21 *= -1;
  445. tmp21 += m; // m-ck0-A*xk
  446. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  447. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  448. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  449. // Update momentum and last coordinates
  450. /*
  451. TMatrixD par = *track->GetParam();
  452. for (Int_t i = 0; i < 3; ++i) par(i+2,0) = qk(i,0);
  453. par(0,0) = rad * phi;
  454. par(1,0) = fVtx[2];
  455. track->SetParam(par);
  456. track->SetPosNew(rad);
  457. // Update track length
  458. Double_t dLeng = track1.GetLength(); // track length from DCA to last saved position
  459. track1.SetParam(par);
  460. track1.SetPos(rad);
  461. track1.SetLength(0.);
  462. if (track->GetNode() == "") MpdKalmanFilter::Instance()->PropagateParamR(&track1,&hit,kTRUE);
  463. else MpdKalmanFilter::Instance()->PropagateParamP(&track1,&hit,kTRUE,kTRUE);
  464. track->SetLength (track->GetLength() - dLeng + track1.GetLength());
  465. */
  466. // Save all matrices for further usage
  467. part->Setq(qk);
  468. part->Setx(xk);
  469. part->SetA(a);
  470. part->SetB(b);
  471. part->SetC(fCovar);
  472. part->SetW(w);
  473. // E = -C*At*G*B*W
  474. TMatrixD tmp31(b,TMatrixD::kMult,w);
  475. TMatrixD tmp32(g,TMatrixD::kMult,tmp31);
  476. TMatrixD tmp33(a,TMatrixD::kTransposeMult,tmp32);
  477. TMatrixD e(fCovar,TMatrixD::kMult,tmp33);
  478. // D = W + W*Bt*G*A*C*At*G*B*W = W + W*Bt*G*A*(-E)
  479. TMatrixD tmp41(a,TMatrixD::kMult,e);
  480. e *= -1.0;
  481. part->SetCovE(e);
  482. TMatrixD tmp42(g,TMatrixD::kMult,tmp41);
  483. TMatrixD tmp43(b,TMatrixD::kTransposeMult,tmp42);
  484. TMatrixD d(w,TMatrixD::kMult,tmp43);
  485. d += w;
  486. part->SetCovD(d);
  487. } // for (Int_t itr = 0; itr < nDaught;
  488. }
  489. //__________________________________________________________________________
  490. void MpdMotherFitterPart::ComputeAandB(const TMatrixD &xk0, const MpdParticle &part,
  491. TMatrixD &a, TMatrixD &b, TMatrixD &ck0, Bool_t flag)
  492. {
  493. /// Compute matrices of derivatives w.r.t. vertex coordinates and particle momentum
  494. if (part.GetCharge() == 0) {
  495. // Neutral particle
  496. ComputeAB0(xk0, part, a, b, ck0, flag);
  497. return;
  498. }
  499. // Put track at xk0 - find rotation angle from xk0 to DCA (i.e. gamma)
  500. // Center of original circle:
  501. Double_t mx = part.GetXY(0) - TMath::Sin(part.GetMeas(2)) / part.GetMeas(4);
  502. Double_t my = part.GetXY(1) + TMath::Cos(part.GetMeas(2)) / part.GetMeas(4);
  503. Double_t x0 = xk0(0,0);
  504. Double_t y0 = xk0(1,0);
  505. Double_t phDca = TMath::ATan2 (part.GetXY(1)-my, part.GetXY(0)-mx);
  506. Double_t phXk = TMath::ATan2 (y0-my, x0-mx);
  507. Double_t gam = phDca - phXk;
  508. Double_t ph0 = part.GetMeas(2) - gam; // phi at xk0
  509. // Center of shifted circle
  510. Double_t cosph = TMath::Cos(ph0);
  511. Double_t sinph = TMath::Sin(ph0);
  512. mx = x0 - sinph / part.GetMeas(4);
  513. my = y0 + cosph / part.GetMeas(4);
  514. // Recompute gamma angle
  515. Double_t kinv = 1. / part.GetMeas(4);
  516. //Double_t gam1 = TMath::ATan ((x0 * cosph + y0 * sinph) / (x0 * sinph - y0 * cosph - kinv));
  517. // Gamma derivatives
  518. Double_t mxmy2 = mx * mx + my * my;
  519. Double_t dgdx = -my / mxmy2;
  520. Double_t dgdy = mx / mxmy2;
  521. Double_t dgdk = -(x0*cosph + y0*sinph) / mxmy2 * kinv * kinv;
  522. Double_t dgdph = y0 * dgdx - x0 * dgdy;
  523. // A = dHc / dx
  524. a = 0.0;
  525. Double_t cosgam = TMath::Cos(gam);
  526. Double_t singam = TMath::Sin(gam);
  527. Double_t val = kinv - x0 * sinph + y0 * cosph;
  528. Double_t costh = TMath::Cos(part.GetMeas(3));
  529. Double_t sinth = TMath::Sin(part.GetMeas(3));
  530. a(0,0) = (cosgam * sinph - singam * dgdx * val) / cosgam / cosgam; // d(dca) / dx0
  531. a(0,1) = -(cosgam * cosph + singam * dgdy * val) / cosgam / cosgam; // d(dca) / dy0
  532. a(1,0) = dgdx * costh / sinth * kinv; // dz / dx
  533. a(1,1) = dgdy * costh / sinth * kinv; // dz / dy
  534. a(1,2) = 1.0; // dz / dz
  535. a(2,0) = dgdx; // d(phi) / dx
  536. a(2,1) = dgdy; // d(phi) / dy
  537. // B = dHc / dq
  538. b = 0.0;
  539. b(0,0) = ((x0 * cosph + y0 * sinph) * cosgam - singam * dgdph * val) / cosgam / cosgam; // d(dca) / dphi
  540. b(0,2) = -kinv * kinv + (kinv * kinv * cosgam - singam * dgdk * val) / cosgam / cosgam; // d(dca) / dk
  541. b(1,0) = dgdph * costh / sinth * kinv; // dz / dphi
  542. b(1,1) = -gam / sinth / sinth * kinv; // dz / dtheta
  543. b(1,2) = kinv * cosph / sinph * (dgdk - kinv * gam); // dz / dk
  544. b(2,0) = 1 + dgdph; // dphi / dphi
  545. b(2,2) = dgdk; // dphi / dk
  546. b(3,1) = 1.0; // d(theta) / d(theta)
  547. b(4,2) = 1.0; // dk / dk
  548. if (!flag) return;
  549. // ck0
  550. ck0(0,0) = kinv - val / cosgam;
  551. ck0(1,0) = xk0(2,0) + gam * costh / sinth * kinv;
  552. //ck0(2,0) = ph0 + gam1;
  553. ck0(2,0) = part.GetMeas(2);
  554. ck0(3,0) = part.GetMeas(3);
  555. ck0(4,0) = part.GetMeas(4);
  556. TMatrixD qk0(3,1);
  557. qk0(0,0) = ph0;
  558. qk0(1,0) = part.GetMeas(3);
  559. qk0(2,0) = part.GetMeas(4);
  560. ck0 -= TMatrixD(a,TMatrixD::kMult,xk0);
  561. ck0 -= TMatrixD(b,TMatrixD::kMult,qk0);
  562. }
  563. //__________________________________________________________________________
  564. void MpdMotherFitterPart::ComputeAB0(const TMatrixD &xk0, const MpdParticle &part,
  565. TMatrixD &a, TMatrixD &b, TMatrixD &ck0, Bool_t flag)
  566. {
  567. /// Compute matrices of derivatives w.r.t. vertex coordinates and particle momentum
  568. /// (for neutrals)
  569. // A = dHn / dx
  570. a = 0.0;
  571. Double_t cosph = TMath::Cos(part.Phi());
  572. Double_t sinph = TMath::Sin(part.Phi());
  573. Double_t sinth = TMath::Sin(part.Theta());
  574. //Double_t tanth = TMath::Tan(part.Theta());
  575. Double_t costh = TMath::Cos(part.Theta());
  576. Double_t coTan = costh / sinth;
  577. a(0,0) = sinph; // d(dca) / dx0
  578. a(0,1) = -cosph; // d(dca) / dy0
  579. //a(1,0) = -cosph / tanth; // dz / dx0
  580. //a(1,1) = -sinph / tanth; // dz / dy0
  581. a(1,0) = -cosph * coTan; // dz / dx0
  582. a(1,1) = -sinph * coTan; // dz / dy0
  583. a(1,2) = 1.0; // dz / dz0
  584. // B = dHn / dq
  585. Double_t x0 = xk0(0,0);
  586. Double_t y0 = xk0(1,0);
  587. Double_t r = TMath::Sqrt (x0*x0 + y0*y0);
  588. //Double_t phi1 = TMath::ACos (x0 / r);
  589. Double_t phi1 = TMath::ATan2 (y0, x0);
  590. Double_t ksi = part.Phi() - phi1;
  591. Double_t cosksi = TMath::Cos(ksi);
  592. Double_t sinksi = TMath::Sin(ksi);
  593. b = 0.0;
  594. b(0,0) = r * cosksi; // d(dca) / dphi
  595. //b(1,0) = r * sinksi / tanth; // dz / dphi
  596. b(1,0) = r * sinksi * coTan; // dz / dphi
  597. //b(1,1) = r * cosph / sinth / sinth; // dz / d(theta)
  598. b(1,1) = r * cosksi / sinth / sinth; // dz / d(theta)
  599. b(2,0) = 1.0; // dphi / dphi
  600. b(3,1) = 1.0; // d(theta) / d(theta)
  601. b(4,2) = 1.0; // dp / dp
  602. if (!flag) return;
  603. // ck0
  604. ck0(0,0) = r * sinksi;
  605. //ck0(1,0) = xk0(2,0) - r * cosksi / tanth;
  606. ck0(1,0) = xk0(2,0) - r * cosksi * coTan;
  607. ck0(2,0) = part.GetMeas(2);
  608. ck0(3,0) = part.GetMeas(3);
  609. ck0(4,0) = part.GetMeas(4);
  610. TMatrixD qk0(3,1);
  611. qk0(0,0) = part.GetMeas(2);
  612. qk0(1,0) = part.GetMeas(3);
  613. qk0(2,0) = part.GetMeas(4);
  614. ck0 -= TMatrixD(a,TMatrixD::kMult,xk0);
  615. ck0 -= TMatrixD(b,TMatrixD::kMult,qk0);
  616. }
  617. //__________________________________________________________________________
  618. void MpdMotherFitterPart::ParamsAtDca(MpdParticle *part)
  619. {
  620. /// Compute particle parameters at DCA
  621. TMatrixD meas(5,1);
  622. if (part->GetCharge() == 0) {
  623. // Neutral
  624. meas(2,0) = part->Phi();
  625. meas(3,0) = part->Theta();
  626. meas(4,0) = part->Momentum();
  627. Double_t x0 = part->Getx()(0,0);
  628. Double_t y0 = part->Getx()(1,0);
  629. Double_t r = TMath::Sqrt (x0*x0 + y0*y0);
  630. //Double_t phi1 = TMath::ACos (x0 / r);
  631. Double_t phi1 = TMath::ATan2 (y0, x0);
  632. Double_t ksi = meas(2,0) - phi1;
  633. meas(0,0) = r * TMath::Sin(ksi);
  634. //meas(1,0) = part->Getx()(2,0) - r * TMath::Cos(ksi) / TMath::Tan(meas(3,0));
  635. meas(1,0) = part->Getx()(2,0) - r * TMath::Cos(ksi) * TMath::Cos(meas(3,0)) / TMath::Sin(meas(3,0));
  636. part->SetMeas(meas);
  637. part->SetXY (meas(0,0)*TMath::Sin(meas(2,0)), -meas(0,0)*TMath::Cos(meas(2,0)));
  638. return;
  639. }
  640. // Charged
  641. meas(3,0) = part->Theta();
  642. meas(4,0) = part->Getq()(2,0);
  643. Double_t cosph = TMath::Cos(part->Phi());
  644. Double_t sinph = TMath::Sin(part->Phi());
  645. Double_t kinv = 1. / meas(4,0);
  646. Double_t x0 = part->Getx()(0,0), y0 = part->Getx()(1,0);
  647. Double_t expr = x0 * sinph - y0 * cosph - kinv;
  648. Double_t gam = TMath::ATan ((x0 * cosph + y0 * sinph) / expr);
  649. meas(2,0) = part->Phi() + gam;
  650. meas(0,0) = kinv + expr / TMath::Cos(gam);
  651. meas(1,0) = part->Getx()(2,0) + gam * kinv * TMath::Cos(meas(3,0)) / TMath::Sin(meas(3,0));
  652. part->SetMeas(meas);
  653. part->SetXY (-meas(0,0)*TMath::Cos(meas(2,0)+TMath::PiOver2()),
  654. -meas(0,0)*TMath::Sin(meas(2,0)+TMath::PiOver2()));
  655. }
  656. //__________________________________________________________________________
  657. void MpdMotherFitterPart::Proxim(const MpdKalmanTrack &track0, MpdKalmanTrack &track)
  658. {
  659. /// Adjust track parameters
  660. if (track0.GetType() != MpdKalmanTrack::kBarrel) {
  661. cout << " !!! Implemented only for kBarrel tracks !!!" << endl;
  662. exit(0);
  663. }
  664. //Double_t tmp = track.GetParamNew(0);
  665. Double_t phi0 = track0.GetParamNew(0) / track0.GetPosNew();
  666. Double_t phi = track.GetParamNew(0) / track.GetPosNew();
  667. phi = MpdKalmanFilter::Instance()->Proxim(phi0,phi);
  668. TMatrixD *par = track.GetParamNew();
  669. (*par)(0,0) = phi * track.GetPosNew();
  670. phi0 = track0.GetParamNew(2);
  671. phi = track.GetParamNew(2);
  672. phi = MpdKalmanFilter::Instance()->Proxim(phi0,phi);
  673. (*par)(2,0) = phi;
  674. track.SetParamNew(*par);
  675. //cout << " Proxim: " << track0.GetParamNew(0) << " " << track.GetParamNew(0) << " " << tmp << endl;
  676. }
  677. //__________________________________________________________________________
  678. TMatrixD MpdMotherFitterPart::ComputeQmatr(vector<MpdParticle*> vDaught)
  679. {
  680. /// Compute matrix Q = covariance cov(qk,qj)
  681. /// Qkj = Wk*Bkt*Gk*Ak*C*Ajt*Gj*Bj*Wj = Wk*Bkt*Gk*Ak*(-E), k!=j
  682. /// Qii = D
  683. TMatrixD qc(3,3), qn(3,3), qnc(3,3);
  684. Int_t nDaught = vDaught.size();
  685. for (Int_t i = 0; i < nDaught; ++i) {
  686. MpdParticle* part = vDaught[i];
  687. for (Int_t j = 0; j < nDaught; ++j) {
  688. MpdParticle* part1 = vDaught[j];
  689. TMatrixD q(3,3);
  690. if (j == i) {
  691. q = part->GetD();
  692. } else {
  693. TMatrixD tmp = part1->GetE();
  694. tmp *= -1.0;
  695. TMatrixD tmp1 = TMatrixD(part->GetA(),TMatrixD::kMult,tmp);
  696. TMatrixD tmp2 = TMatrixD(part->GetG(),TMatrixD::kMult,tmp1);
  697. TMatrixD tmp3 = TMatrixD(part->GetB(),TMatrixD::kTransposeMult,tmp2);
  698. q = TMatrixD(part->GetW(),TMatrixD::kMult,tmp3);
  699. }
  700. TMatrixD jt = TMatrixD(TMatrixD::kTransposed,part1->GetJ());
  701. TMatrixD tmp11 = TMatrixD(q,TMatrixD::kMult,jt);
  702. TMatrixD tmp12 = TMatrixD(part->GetJ(),TMatrixD::kMult,tmp11);
  703. if (part->GetCharge() && part1->GetCharge()) qc += tmp12;
  704. else if (part->GetCharge() == 0 && part1->GetCharge() == 0) qn += tmp12;
  705. else qnc += tmp12;
  706. }
  707. }
  708. qc += qn;
  709. qc += qnc;
  710. return qc;
  711. }
  712. //__________________________________________________________________________
  713. Double_t MpdMotherFitterPart::Chi2Vertex(MpdParticle* part, const MpdVertex *vtx)
  714. {
  715. /// Compute Chi2 w.r.t. vertex
  716. Double_t vpos[3] = {vtx->GetX(), vtx->GetY(), vtx->GetZ()};
  717. TMatrixD xk(3,1), xk0(3,1), ck0(5,1), a(5,3), b(5,3), c(3,3);
  718. TMatrixFSym cov(3);
  719. cov.SetTol(1.e-10); //AZ-311220
  720. xk0.SetMatrixArray(vpos);
  721. vtx->CovMatrix(cov);
  722. cov.Invert();
  723. ComputeAandB(xk0, *part, a, b, ck0, kTRUE); // compute matrices of derivatives
  724. TMatrixD g = part->GetG();
  725. // W = (Bt*G*B)'
  726. TMatrixD tmp(g,TMatrixD::kMult,b);
  727. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  728. w.Invert();
  729. // Gb = G - G*B*W*Bt*G
  730. TMatrixD tmp1(b,TMatrixD::kTransposeMult,g);
  731. TMatrixD tmp2(w,TMatrixD::kMult,tmp1);
  732. TMatrixD tmp3(b,TMatrixD::kMult,tmp2);
  733. TMatrixD tmp4(g,TMatrixD::kMult,tmp3);
  734. TMatrixD gb = g;
  735. gb -= tmp4;
  736. // Ck = ((Ck-1)' + At*Gb*A)'
  737. TMatrixD tmp5(gb,TMatrixD::kMult,a);
  738. c = TMatrixD(a,TMatrixD::kTransposeMult,tmp5);
  739. c += cov;
  740. c.Invert();
  741. // xk = Ck*((Ck-1)'x(k-1)+At*Gb*(m-ck0))
  742. TMatrixD m = part->GetMeas();
  743. m -= ck0; // m-ck0
  744. TMatrixD tmp11(gb,TMatrixD::kMult,m);
  745. TMatrixD tmp12(a,TMatrixD::kTransposeMult,tmp11);
  746. TMatrixD tmp13(cov,TMatrixD::kMult,xk0);
  747. tmp13 += tmp12;
  748. xk = TMatrixD(c,TMatrixD::kMult,tmp13);
  749. // qk = W*Bt*G*(m-ck0-A*xk)
  750. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  751. tmp21 *= -1;
  752. tmp21 += m; // m-ck0-A*xk
  753. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  754. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  755. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  756. // Residual m-ck0-A*xk-B*qk
  757. TMatrixD r = tmp21;
  758. TMatrixD tmp31(b,TMatrixD::kMult,qk);
  759. r -= tmp31;
  760. // Chi2 = rt*G*r + (xk-x(k-1))t*(Ck-1)'*(xk-x(k-1))
  761. TMatrixD tmp41(g,TMatrixD::kMult,r);
  762. TMatrixD chi21(r,TMatrixD::kTransposeMult,tmp41);
  763. TMatrixD dx = xk;
  764. dx -= xk0;
  765. TMatrixD tmp42(cov,TMatrixD::kMult,dx);
  766. TMatrixD chi22(dx,TMatrixD::kTransposeMult,tmp42);
  767. chi21 += chi22;
  768. return chi21(0,0);
  769. }
  770. ClassImp(MpdMotherFitterPart)