MpdKfV0Fitter.cxx 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578
  1. /// \class MpdKfV0Fitter
  2. ///
  3. /// Kalman filter V0 fitter for the MPD detector
  4. /// \author Alexander Zinchenko (LHEP, JINR, Dubna)
  5. #include "MpdKfV0Fitter.h"
  6. #include "MpdKalmanFilter.h"
  7. #include "MpdKalmanGeoScheme.h"
  8. #include "MpdKalmanTrack.h"
  9. #include "MpdKalmanHit.h"
  10. #include "MpdHelix.h"
  11. #include "MpdTpcKalmanTrack.h"
  12. #include "FairField.h"
  13. #include "MpdMCTrack.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. MpdKfV0Fitter* MpdKfV0Fitter::fgV0 = 0x0;
  23. //__________________________________________________________________________
  24. MpdKfV0Fitter::MpdKfV0Fitter()
  25. : FairTask()
  26. {
  27. /// Default constructor
  28. }
  29. //__________________________________________________________________________
  30. MpdKfV0Fitter::MpdKfV0Fitter(const char *name, const char *title)
  31. : FairTask(name)
  32. {
  33. /// Constructor
  34. fgV0 = this;
  35. }
  36. //__________________________________________________________________________
  37. MpdKfV0Fitter* MpdKfV0Fitter::Instance()
  38. {
  39. /// Get pointer to the V0 fitter singleton object
  40. if (!fgV0){
  41. fgV0 = new MpdKfV0Fitter;
  42. fgV0->Init();
  43. // Automatic destruction is forced
  44. std::atexit(DestroyInstance);
  45. }
  46. return fgV0;
  47. }
  48. //__________________________________________________________________________
  49. MpdKfV0Fitter* MpdKfV0Fitter::Instance(const char *name, const char *title)
  50. {
  51. /// Get pointer to the V0 fitter singleton object
  52. if (!fgV0){
  53. fgV0 = new MpdKfV0Fitter(name, title);
  54. fgV0->Init();
  55. // Automatic destruction is forced
  56. std::atexit(DestroyInstance);
  57. }
  58. return fgV0;
  59. }
  60. //__________________________________________________________________________
  61. MpdKfV0Fitter::~MpdKfV0Fitter()
  62. {
  63. /// Destructor
  64. //FairRootManager *manager = FairRootManager::Instance();
  65. //manager->Write();
  66. }
  67. //__________________________________________________________________________
  68. InitStatus MpdKfV0Fitter::Init() {
  69. cout << "InitStatus MpdKfV0Fitter::Init\n\n";
  70. Double_t bZ = 5.;
  71. FairField * magField = FairRunAna::Instance()->GetField();
  72. if (!magField || TMath::Abs(magField->GetBz(0,0,0)) < 0.01) {
  73. cout << " -I- MpdKfV0Fitter::Init - Using the default constant magnetic field Bz = 5 kG " << endl;
  74. } else bZ = TMath::Abs(magField->GetBz(0,0,0));
  75. fieldConst = 0.3 * 0.01 * bZ / 10;
  76. return kSUCCESS;
  77. }
  78. //__________________________________________________________________________
  79. InitStatus MpdKfV0Fitter::ReInit()
  80. {
  81. //fMagField = FairRunAna::Instance()->GetField(); // !!! interim solution !!!
  82. return kSUCCESS;
  83. }
  84. //__________________________________________________________________________
  85. void MpdKfV0Fitter::Reset()
  86. {
  87. ///
  88. //cout << " MpdKfV0Fitter::Reset " << endl;
  89. }
  90. //__________________________________________________________________________
  91. void MpdKfV0Fitter::Register()
  92. {
  93. ///
  94. //FairRootManager::Instance()->
  95. //Register("TpcLheKalmanTrack","TpcLheKalmanTrack", fTracks, kFALSE);
  96. }
  97. //__________________________________________________________________________
  98. void MpdKfV0Fitter::Finish()
  99. {
  100. ///
  101. }
  102. //__________________________________________________________________________
  103. void MpdKfV0Fitter::Exec(Option_t * option)
  104. {
  105. ///
  106. }
  107. //__________________________________________________________________________
  108. void MpdKfV0Fitter::EvalVertex(MpdKalmanTrack* tr[2])
  109. {
  110. /// Evaluate V0 position as PCA of 2 helices
  111. // Create MpdHelix's
  112. MpdHelix* helix[2];
  113. TVector3 sum;
  114. for (Int_t i = 0; i < 2; ++i) {
  115. Double_t r = tr[i]->GetPosNew();
  116. Double_t phi = tr[i]->GetParam(0) / r;
  117. Double_t x = r * TMath::Cos(phi);
  118. Double_t y = r * TMath::Sin(phi);
  119. Double_t dip = tr[i]->GetParam(3);
  120. Double_t cur = TMath::Abs (tr[i]->GetParam(4)) * fieldConst;
  121. TVector3 o(x, y, tr[i]->GetParam(1));
  122. Int_t h = (Int_t) TMath::Sign(1.1,tr[i]->GetParam(4));
  123. //helix[itr++] = MpdHelix(cur, dip, phi, o, h);
  124. helix[i] = new MpdHelix(cur, dip, tr[i]->GetParam(2)-TMath::PiOver2()*h, o, h);
  125. //cout << helix[i]->xcenter() << " " << helix[i]->ycenter() << endl;
  126. sum += o;
  127. }
  128. pair<Double_t,Double_t> paths = helix[0]->pathLengths(*helix[1]);
  129. //cout << " Intersection: " << paths.first << " " << paths.second << endl;
  130. TVector3 p1 = helix[0]->at(paths.first);
  131. TVector3 p2 = helix[1]->at(paths.second);
  132. p1 += p2;
  133. p1 *= 0.5;
  134. if (TMath::Abs(paths.first) > 50) p1.SetXYZ(0,0,0);
  135. p1.GetXYZ(fVtx);
  136. for (Int_t i = 0; i < 2; ++i) delete helix[i];
  137. sum *= 0.5;
  138. //sum.GetXYZ(fVtx); // half-sum
  139. }
  140. //__________________________________________________________________________
  141. Double_t MpdKfV0Fitter::FindVertex(MpdKalmanTrack *tr1, MpdKalmanTrack *tr2, TVector3 &vtx)
  142. {
  143. /// Kalman filter based secondary vertex finder (fitter)
  144. const Int_t nPass = 3; // number of iterations
  145. const Double_t cutChi2 = 1000.; // chi2-cut
  146. MpdKalmanTrack* tracks[2] = {tr1, tr2};
  147. EvalVertex(tracks);
  148. Int_t nPrim = 0;
  149. TMatrixD c(3,3), cov(3,3), xk0(3,1), xk(3,1), ck0(5,1);
  150. TMatrixD a(5,3), b(5,3);
  151. MpdKalmanHit hit;
  152. Double_t chi2 = 0;//, chi2o = 0;
  153. xk.SetMatrixArray(fVtx);
  154. c(0,0) = c(1,1) = c(2,2) = 1;
  155. for (Int_t ipass = 0; ipass < nPass; ++ipass) {
  156. //chi2o = chi2;
  157. chi2 = 0.;
  158. if (ipass == 0) cov = TMatrixD(TMatrixD::kInverted,c);
  159. for (Int_t itr = 0; itr < 2; ++itr) {
  160. xk0 = xk; // xk0 stands for x(k-1)
  161. cov = TMatrixD(TMatrixD::kInverted,c);
  162. MpdKalmanTrack *track = tracks[itr];
  163. // Select primaries
  164. //MpdMCTrack *mcTr = (MpdMCTrack*) fMCTracks->UncheckedAt(track->GetTrackID());
  165. //if (mcTr->GetMotherId() >= 0) continue; // secondary
  166. //Double_t th = TMath::PiOver2() - track->GetParam(3);
  167. //if (TMath::Abs(TMath::Log(TMath::Tan(th/2))) > 1.) continue; // eta-cut
  168. //if (1./TMath::Abs(track->GetParam(4)) < 0.2) continue; // pt-cut
  169. ++nPrim;
  170. MpdKalmanTrack track1 = *track;
  171. track1.SetParamNew(*track1.GetParam());
  172. track1.SetPos(track1.GetPosNew());
  173. track1.ReSetWeight();
  174. TMatrixD g = *track1.GetWeight(); // track weight matrix
  175. //track1.GetWeight()->Print();
  176. if (track->GetNode() == "") {
  177. hit.SetType(MpdKalmanHit::kFixedR);
  178. hit.SetPos(track->GetPos());
  179. } else {
  180. hit.SetType(MpdKalmanHit::kFixedP);
  181. TString detName = track->GetNode();
  182. if (track->GetUniqueID()) {
  183. // ITS
  184. detName = detName(16,detName.Length());
  185. detName += "#0";
  186. }
  187. MpdKalmanGeoScheme *geo = MpdKalmanFilter::Instance()->GetGeo();
  188. hit.SetDetectorID(geo->DetId(detName));
  189. // Find distance from the current track position to the last point (plane) -
  190. // to define direction (mainly for ITS)
  191. TVector3 pos = geo->GlobalPos(&hit);
  192. TVector3 norm = geo->Normal(&hit);
  193. Double_t v7[7] = {0.0};
  194. TString node = track1.GetNode();
  195. track1.SetNode("");
  196. MpdKalmanFilter::Instance()->SetGeantParamB(&track1, v7, 1);
  197. Double_t d = -(pos * norm); // Ax+By+Cz+D=0, A=nx, B=ny, C=nz
  198. TVector3 v3(v7[0], v7[1], v7[2]);
  199. d += v3 * norm;
  200. if (d < 0) track1.SetDirection(MpdKalmanTrack::kOutward);
  201. }
  202. MpdKalmanFilter::Instance()->PropagateToHit(&track1,&hit,kFALSE,kTRUE);
  203. //track1.GetParamNew()->Print();
  204. //cout << " Meas. radius " << track1.GetPosNew() << endl;
  205. ComputeAandB(xk0,track,track1,a,b,ck0); // compute matrices of derivatives
  206. // W = (Bt*G*B)'
  207. TMatrixD tmp(g,TMatrixD::kMult,b);
  208. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  209. w.Invert();
  210. // Gb = G - G*B*W*Bt*G
  211. TMatrixD tmp1(b,TMatrixD::kTransposeMult,g);
  212. TMatrixD tmp2(w,TMatrixD::kMult,tmp1);
  213. TMatrixD tmp3(b,TMatrixD::kMult,tmp2);
  214. TMatrixD tmp4(g,TMatrixD::kMult,tmp3);
  215. TMatrixD gb = g;
  216. gb -= tmp4;
  217. // Ck = ((Ck-1)' + At*Gb*A)'
  218. TMatrixD tmp5(gb,TMatrixD::kMult,a);
  219. c = TMatrixD(a,TMatrixD::kTransposeMult,tmp5);
  220. c += cov;
  221. c.Invert();
  222. // xk = Ck*((Ck-1)'x(k-1)+At*Gb*(m-ck0))
  223. TMatrixD m = *track1.GetParamNew();
  224. m -= ck0; // m-ck0
  225. //cout << " m-ck0: " << endl;
  226. //ck0.Print();
  227. //m.Print();
  228. TMatrixD tmp11(gb,TMatrixD::kMult,m);
  229. TMatrixD tmp12(a,TMatrixD::kTransposeMult,tmp11);
  230. TMatrixD tmp13(cov,TMatrixD::kMult,xk0);
  231. tmp13 += tmp12;
  232. xk = TMatrixD(c,TMatrixD::kMult,tmp13);
  233. //cout << " Vertex: " << itr << " " << track->GetTrackID() << " " << xk(0,0) << " " << xk(1,0) << " " << xk(2,0) << endl;
  234. // qk = W*Bt*G*(m-ck0-A*xk)
  235. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  236. tmp21 *= -1;
  237. tmp21 += m; // m-ck0-A*xk
  238. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  239. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  240. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  241. // Residual m-ck0-A*xk-B*qk
  242. TMatrixD r = tmp21;
  243. TMatrixD tmp31(b,TMatrixD::kMult,qk);
  244. r -= tmp31;
  245. //cout << " Residual matrix: " << endl;
  246. //r.Print();
  247. //qk.Print();
  248. // Chi2 = rt*G*r + (xk-x(k-1))t*(Ck-1)'*(xk-x(k-1))
  249. TMatrixD tmp41(g,TMatrixD::kMult,r);
  250. TMatrixD chi21(r,TMatrixD::kTransposeMult,tmp41);
  251. //chi21.Print();
  252. TMatrixD dx = xk;
  253. dx -= xk0;
  254. TMatrixD tmp42(cov,TMatrixD::kMult,dx);
  255. TMatrixD chi22(dx,TMatrixD::kTransposeMult,tmp42);
  256. //chi22.Print();
  257. if (chi21(0,0) < 0 || chi22(0,0) < 0) {
  258. cout << " !!! Chi2 < 0 " << chi21(0,0) << " " << chi22(0,0) << " " << ipass << " " << itr << " " << track1.GetParamNew(4) << endl;
  259. //exit(0);
  260. }
  261. chi21 += chi22;
  262. chi2 += chi21(0,0);
  263. //if (chi2 > cutChi2) {
  264. if (chi2 > cutChi2 || chi21(0,0) < 0 || chi22(0,0) < 0) {
  265. for (Int_t i = 0; i < 3; ++i) vtx[i] = fVtx[i];
  266. for (Int_t i = 0; i < 2; ++i) {
  267. //MpdTpcKalmanTrack trTmp = *tracks[i];
  268. MpdKalmanTrack trTmp = *tracks[i];
  269. trTmp.SetPos(trTmp.GetPosNew());
  270. trTmp.SetParamNew(*trTmp.GetParam());
  271. if (trTmp.GetNode() != "") trTmp.SetNode(""); // 25-jul-2012
  272. MpdKalmanFilter::Instance()->FindPca(&trTmp,fVtx);
  273. tracks[i]->SetParam(*trTmp.GetParamNew());
  274. tracks[i]->SetPosNew(trTmp.GetPosNew());
  275. }
  276. // AZ-Debug
  277. //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;
  278. return cutChi2;
  279. }
  280. } // for (Int_t itr = 0; itr < 2;
  281. } // for (Int_t ipass = 0; ipass < nPass;
  282. for (Int_t i = 0; i < 3; ++i) vtx[i] = fVtx[i] = xk(i,0);
  283. // Covariance matrix
  284. fCovar.ResizeTo(3,3);
  285. fCovar = c;
  286. Smooth(tracks);
  287. return chi2;
  288. }
  289. //__________________________________________________________________________
  290. void MpdKfV0Fitter::Smooth(MpdKalmanTrack* tr[2])
  291. {
  292. /// Smooth tracks from primary vertex (update momentum and track length -
  293. /// covariance matrix is not updated !!!)
  294. MpdKalmanHit hit;
  295. TMatrixD c(3,3), xk(3,1), ck0(5,1);
  296. TMatrixD a(5,3), b(5,3);
  297. Double_t rad = 0.;
  298. for (Int_t i = 0; i < 3; ++i) {
  299. xk(i,0) = fVtx[i];
  300. if (i < 2) rad += fVtx[i] * fVtx[i];
  301. }
  302. rad = TMath::Sqrt(rad);
  303. Double_t phi = TMath::ATan2 (fVtx[1],fVtx[0]);
  304. for (Int_t itr = 0; itr < 2; ++itr) {
  305. MpdKalmanTrack *track = tr[itr];
  306. MpdKalmanTrack track1 = *track;
  307. track1.SetParamNew(*track1.GetParam());
  308. track1.SetPos(track1.GetPosNew());
  309. track1.ReSetWeight();
  310. track1.SetLength(0.);
  311. TMatrixD g = *track1.GetWeight(); // track weight matrix
  312. //track1.GetWeight()->Print();
  313. if (track->GetNode() == "") {
  314. hit.SetType(MpdKalmanHit::kFixedR);
  315. hit.SetPos(track->GetPos());
  316. } else {
  317. hit.SetType(MpdKalmanHit::kFixedP);
  318. TString detName = track->GetNode();
  319. if (track->GetUniqueID()) {
  320. // ITS
  321. detName = detName(16,detName.Length());
  322. detName += "#0";
  323. }
  324. MpdKalmanGeoScheme *geo = MpdKalmanFilter::Instance()->GetGeo();
  325. hit.SetDetectorID(geo->DetId(detName));
  326. // Find distance from the current track position to the last point (plane) -
  327. // to define direction (mainly for ITS)
  328. TVector3 pos = geo->GlobalPos(&hit);
  329. TVector3 norm = geo->Normal(&hit);
  330. Double_t v7[7] = {0.0};
  331. track1.SetNode("");
  332. MpdKalmanFilter::Instance()->SetGeantParamB(&track1, v7, 1);
  333. Double_t d = -(pos * norm); // Ax+By+Cz+D=0, A=nx, B=ny, C=nz
  334. TVector3 v3(v7[0], v7[1], v7[2]);
  335. d += v3 * norm;
  336. if (d < 0) track1.SetDirection(MpdKalmanTrack::kOutward);
  337. }
  338. MpdKalmanFilter::Instance()->PropagateToHit(&track1,&hit,kTRUE,kTRUE);
  339. //track1.GetParamNew()->Print();
  340. //cout << " Meas. radius " << track1.GetPosNew() << endl;
  341. ComputeAandB(xk,track,track1,a,b,ck0); // compute matrices of derivatives
  342. // W = (Bt*G*B)'
  343. TMatrixD tmp(g,TMatrixD::kMult,b);
  344. TMatrixD w(b,TMatrixD::kTransposeMult,tmp);
  345. w.Invert();
  346. TMatrixD m = *track1.GetParamNew();
  347. m -= ck0; // m-ck0
  348. // qk = W*Bt*G*(m-ck0-A*xk)
  349. TMatrixD tmp21(a,TMatrixD::kMult,xk);
  350. tmp21 *= -1;
  351. tmp21 += m; // m-ck0-A*xk
  352. TMatrixD tmp22(g,TMatrixD::kMult,tmp21);
  353. TMatrixD tmp23(b,TMatrixD::kTransposeMult,tmp22);
  354. TMatrixD qk(w,TMatrixD::kMult,tmp23);
  355. // Update momentum and last coordinates
  356. TMatrixD par = *track->GetParam();
  357. for (Int_t i = 0; i < 3; ++i) par(i+2,0) = qk(i,0);
  358. par(0,0) = rad * phi;
  359. par(1,0) = fVtx[2];
  360. track->SetParam(par);
  361. track->SetPosNew(rad);
  362. // Update track length
  363. Double_t dLeng = track1.GetLength(); // track length from DCA to last saved position
  364. track1.SetParam(par);
  365. track1.SetPos(rad);
  366. track1.SetLength(0.);
  367. if (track->GetNode() == "") MpdKalmanFilter::Instance()->PropagateParamR(&track1,&hit,kTRUE);
  368. else MpdKalmanFilter::Instance()->PropagateParamP(&track1,&hit,kTRUE,kTRUE);
  369. track->SetLength (track->GetLength() - dLeng + track1.GetLength());
  370. } // for (Int_t itr = 0; itr < 2;
  371. }
  372. //__________________________________________________________________________
  373. void MpdKfV0Fitter::ComputeAandB(TMatrixD &xk0, const MpdKalmanTrack *track,
  374. const MpdKalmanTrack &trackM,
  375. TMatrixD &a, TMatrixD &b, TMatrixD &ck0)
  376. {
  377. /// Compute matrices of derivatives w.r.t. vertex coordinates and track momentum
  378. Double_t vert0[3], *vert = xk0.GetMatrixArray(); //zero[3] = {0},
  379. for (Int_t i = 0; i < 3; ++i) vert0[i] = vert[i];
  380. MpdKalmanTrack trackk = *track;
  381. trackk.SetPos(trackk.GetPosNew());
  382. //trackk.GetParam()->Print();
  383. // Propagate track to PCA w.r.t. point xk0
  384. MpdKalmanFilter::Instance()->FindPca(&trackk,vert0);
  385. //MpdKalmanFilter::Instance()->FindPca(&trackk,zero); // just for test
  386. //cout << trackk.GetPosNew() << endl;
  387. trackk.SetParam(*trackk.GetParamNew());
  388. //trackk.GetParam()->Print();
  389. // Put track at xk0
  390. Double_t r = TMath::Sqrt (vert0[0] * vert0[0] + vert0[1] * vert0[1]);
  391. Double_t phi = trackk.GetParamNew(2); // track Phi
  392. if (r > 1.e-7) phi = TMath::ATan2 (vert0[1], vert0[0]);
  393. trackk.SetPos(r);
  394. trackk.SetParam(0,r*phi);
  395. trackk.SetParam(1,vert0[2]);
  396. trackk.SetNode("");
  397. MpdKalmanTrack track0 = trackk;
  398. // Propagate track to chosen radius
  399. MpdKalmanHit hit;
  400. if (track->GetNode() == "") {
  401. hit.SetType(MpdKalmanHit::kFixedR);
  402. //hit.SetR(35.);
  403. //hit = *(MpdKalmanHitR*)track->GetTrHits()->Last();
  404. hit.SetPos(track->GetPos());
  405. MpdKalmanFilter::Instance()->PropagateParamR(&trackk,&hit,kFALSE);
  406. //trackk.GetParamNew()->Print();
  407. Proxim(trackM,trackk);
  408. } else {
  409. hit.SetType(MpdKalmanHit::kFixedP);
  410. TString detName = track->GetNode();
  411. if (track->GetUniqueID()) {
  412. // ITS
  413. detName = detName(16,detName.Length());
  414. detName += "#0";
  415. }
  416. MpdKalmanGeoScheme *geo = MpdKalmanFilter::Instance()->GetGeo();
  417. hit.SetDetectorID(geo->DetId(detName));
  418. // Find distance from the current track position to the last point (plane) -
  419. // to define direction (mainly for ITS)
  420. TVector3 pos = geo->GlobalPos(&hit);
  421. TVector3 norm = geo->Normal(&hit);
  422. Double_t v7[7] = {0.0};
  423. MpdKalmanFilter::Instance()->SetGeantParamB(&trackk, v7, 1);
  424. Double_t d = -(pos * norm); // Ax+By+Cz+D=0, A=nx, B=ny, C=nz
  425. TVector3 v3(v7[0], v7[1], v7[2]);
  426. d += v3 * norm;
  427. if (d < 0) trackk.SetDirection(MpdKalmanTrack::kOutward);
  428. MpdKalmanFilter::Instance()->PropagateParamP(&trackk,&hit,kFALSE,kTRUE);
  429. track0.SetDirection(trackk.GetDirection());
  430. }
  431. //Double_t shift = 0.01; // 100 um coordinate shift
  432. Double_t shift = 0.1; // 1 mm coordinate shift
  433. for (Int_t i = 0; i < 3; ++i) {
  434. MpdKalmanTrack track1 = track0;
  435. vert0[i] += shift;
  436. if (i > 0) vert0[i-1] -= shift;
  437. r = TMath::Sqrt (vert0[0] * vert0[0] + vert0[1] * vert0[1]);
  438. if (r > 1.e-7) phi = TMath::ATan2 (vert0[1], vert0[0]);
  439. else phi = track0.GetParamNew(2); // track Phi
  440. track1.SetPos(r);
  441. track1.SetParam(0,r*phi);
  442. track1.SetParam(1,vert0[2]);
  443. if (track->GetNode() == "") {
  444. MpdKalmanFilter::Instance()->PropagateParamR(&track1,&hit,kFALSE);
  445. Proxim(trackk,track1);
  446. //Proxim(track1,trackk);
  447. } else MpdKalmanFilter::Instance()->PropagateParamP(&track1,&hit,kFALSE,kTRUE);
  448. // Derivatives
  449. for (Int_t j = 0; j < 5; ++j) {
  450. a(j,i) = (track1.GetParamNew(j) - trackk.GetParamNew(j)) / shift;
  451. }
  452. }
  453. for (Int_t i = 0; i < 3; ++i) {
  454. MpdKalmanTrack track1 = track0;
  455. Int_t j = i + 2;
  456. shift = (*track->GetCovariance())(j,j);
  457. shift = TMath::Sqrt(shift);
  458. if (j == 4) shift *= TMath::Sign(1.,-track0.GetParamNew(j)); // 1/p
  459. //if (j == 4) shift *= TMath::Sign(1.,track0.GetParamNew(j)); // 1/p
  460. track1.SetParam(j,track0.GetParamNew(j)+shift);
  461. //if (j == 2 && track1.GetParamNew(j)*TMath::Sign(1.,track1.GetParamNew(j)) > TMath::Pi())
  462. //track1.SetParam(j,track0.GetParamNew(j)-shift);
  463. if (track->GetNode() == "") {
  464. MpdKalmanFilter::Instance()->PropagateParamR(&track1,&hit,kFALSE);
  465. Proxim(trackk,track1);
  466. //Proxim(track1,trackk);
  467. } else MpdKalmanFilter::Instance()->PropagateParamP(&track1,&hit,kFALSE,kTRUE);
  468. // Derivatives
  469. for (Int_t k = 0; k < 5; ++k) {
  470. b(k,i) = (track1.GetParamNew(k) - trackk.GetParamNew(k)) / shift;
  471. }
  472. }
  473. TMatrixD qk0(3,1);
  474. for (Int_t i = 0; i < 3; ++i) qk0(i,0) = track0.GetParamNew(i+2);
  475. //qk0.Print();
  476. ck0 = *trackk.GetParamNew();
  477. ck0 -= TMatrixD(a,TMatrixD::kMult,xk0);
  478. ck0 -= TMatrixD(b,TMatrixD::kMult,qk0);
  479. //cout << " Derivatives: " << endl;
  480. //a.Print();
  481. //b.Print();
  482. //ck0.Print();
  483. //TMatrixD(b,TMatrixD::kMult,qk0).Print();
  484. //TMatrixD(a,TMatrixD::kMult,xk0).Print();
  485. }
  486. //__________________________________________________________________________
  487. void MpdKfV0Fitter::Proxim(const MpdKalmanTrack &track0, MpdKalmanTrack &track)
  488. {
  489. /// Adjust track parameters
  490. if (track0.GetType() != MpdKalmanTrack::kBarrel) {
  491. cout << " !!! Implemented only for kBarrel tracks !!!" << endl;
  492. exit(0);
  493. }
  494. //Double_t tmp = track.GetParamNew(0);
  495. Double_t phi0 = track0.GetParamNew(0) / track0.GetPosNew();
  496. Double_t phi = track.GetParamNew(0) / track.GetPosNew();
  497. phi = MpdKalmanFilter::Instance()->Proxim(phi0,phi);
  498. TMatrixD *par = track.GetParamNew();
  499. (*par)(0,0) = phi * track.GetPosNew();
  500. phi0 = track0.GetParamNew(2);
  501. phi = track.GetParamNew(2);
  502. phi = MpdKalmanFilter::Instance()->Proxim(phi0,phi);
  503. (*par)(2,0) = phi;
  504. track.SetParamNew(*par);
  505. //cout << " Proxim: " << track0.GetParamNew(0) << " " << track.GetParamNew(0) << " " << tmp << endl;
  506. }
  507. ClassImp(MpdKfV0Fitter)