CbmRoot
CbmKFTrackInterface.cxx
Go to the documentation of this file.
1 
16 #include "CbmKFTrackInterface.h"
17 
18 #include "CbmKF.h"
19 #include "CbmKFFieldMath.h"
20 #include "CbmKFHit.h"
21 #include "CbmKFMath.h"
22 #include "CbmKFVertexInterface.h"
23 
24 #include <algorithm>
25 
26 using std::vector;
27 
28 ClassImp(CbmKFTrackInterface)
29 
30  static Double_t gTempD[100];
31 static Int_t gTempI[10];
32 
33 Double_t* CbmKFTrackInterface::GetTrack() { return gTempD; }
34 Double_t* CbmKFTrackInterface::GetCovMatrix() { return gTempD + 6; }
35 Double_t& CbmKFTrackInterface::GetRefChi2() { return gTempD[21]; }
36 Int_t& CbmKFTrackInterface::GetRefNDF() { return gTempI[0]; }
37 
38 
39 Int_t CbmKFTrackInterface::Extrapolate(Double_t z_out, Double_t* QP0) {
40 
41  Bool_t err = 0;
42  CbmKF* KF = CbmKF::Instance();
43  const Double_t z_in = GetTrack()[5];
44  Double_t qp0 = (QP0) ? *QP0 : GetTrack()[4];
45  Double_t z, Z;
46  Bool_t downstream_direction = (z_out > z_in);
47 
48  if (downstream_direction) {
49  z = z_in;
50  Z = z_out;
51  } else {
52  z = z_out;
53  Z = z_in;
54  }
55 
56  vector<CbmKFMaterial*>::iterator i, ibeg, iend;
57  ibeg = lower_bound(
58  KF->vMaterial.begin(), KF->vMaterial.end(), z, CbmKFMaterial::compareP_z);
59  iend = upper_bound(
60  KF->vMaterial.begin(), KF->vMaterial.end(), Z, CbmKFMaterial::compareP_Z);
61  if (iend != KF->vMaterial.end()
62  && (*iend)->ZReference - (*iend)->ZThickness / 2 < Z)
63  iend++;
64  if (downstream_direction) {
65  } else {
66  i = ibeg;
67  ibeg = iend;
68  iend = i;
69  ibeg--;
70  iend--;
71  }
72 
73  for (i = ibeg; i != iend; downstream_direction ? ++i : --i) {
74  Double_t zthick = (*i)->ZThickness, zcross = (*i)->ZReference;
75  if (CbmKFMath::GetThickness(z, Z, zcross, zthick, &zcross, &zthick))
76  continue;
77 
78  double z0 =
79  (downstream_direction) ? zcross - zthick / 2. : zcross + zthick / 2.;
80  double d = (downstream_direction) ? 1 : -1;
81  double dz = 1.E-1 * (*i)->RadLength;
82  double z_ = 0;
83  while (z_ + dz < zthick) {
84  err = err
85  || (*i)->Pass(
86  z0 + d * (z_ + dz / 2.), dz, *this, downstream_direction, qp0);
87  z_ += dz;
88  }
89  dz = zthick - z_;
90  err = err
91  || (*i)->Pass(
92  z0 + d * (z_ + dz / 2.), dz, *this, downstream_direction, qp0);
93  //(*i)->Pass( zcross, zthick, *this, downstream_direction, qp0 );
94  }
95  err = err || Propagate(z_out, qp0);
96  if (QP0) *QP0 = qp0;
97  return err;
98 }
99 
100 
101 Int_t CbmKFTrackInterface::Fit(Bool_t downstream) {
102  CbmKF* KF = CbmKF::Instance();
103  Double_t* T = GetTrack();
104  Double_t* C = GetCovMatrix();
105  Int_t NHits = GetNOfHits();
106 
107  Bool_t err = 0;
108  if (NHits == 0) return 1;
109 
110  // use initial momentum
111  // this fixed value will be used during fit instead of T[4]
112 
113  Double_t qp0 = T[4];
114 
115  const Double_t INF = 10000.;
116 
117  GetRefChi2() = 0;
118  GetRefNDF() = 0;
119 
120  // initialize covariance matrix
121 
122  C[0] = INF;
123  C[1] = 0.;
124  C[2] = INF;
125  C[3] = 0.;
126  C[4] = 0.;
127  C[5] = INF;
128  C[6] = 0.;
129  C[7] = 0.;
130  C[8] = 0.;
131  C[9] = INF;
132  C[10] = 0.;
133  C[11] = 0.;
134  C[12] = 0.;
135  C[13] = 0.;
136  C[14] = INF;
137 
138  try {
139 
140  if (downstream) {
141  CbmKFHit* h = GetHit(0);
142  err = h->Filter(*this, downstream, qp0);
143  Int_t istold = h->MaterialIndex;
144  for (Int_t i = 1; i < NHits; i++) {
145  h = GetHit(i);
146  Int_t ist = h->MaterialIndex;
147  for (Int_t j = istold + 1; j < ist; j++)
148  err = err || KF->vMaterial[j]->Pass(*this, downstream, qp0);
149  err = err || h->Filter(*this, downstream, qp0);
150  istold = ist;
151  }
152  } else {
153  CbmKFHit* h = GetHit(NHits - 1);
154  err = h->Filter(*this, downstream, qp0);
155  Int_t istold = h->MaterialIndex;
156  for (Int_t i = NHits - 2; i >= 0; i--) {
157  h = GetHit(i);
158  Int_t ist = h->MaterialIndex;
159  for (Int_t j = istold - 1; j > ist; j--)
160  err = err || KF->vMaterial[j]->Pass(*this, downstream, qp0);
161  err = err || h->Filter(*this, downstream, qp0);
162  istold = ist;
163  }
164  }
165 
166  // correct NDF value to number of fitted track parameters (straight line(4) or helix(5) )
167 
168  GetRefNDF() -= (KF->GetMethod() == 0) ? 4 : 5;
169  } catch (...) {
170  GetRefChi2() = 0;
171  GetRefNDF() = 0;
172  C[0] = INF;
173  C[1] = 0.;
174  C[2] = INF;
175  C[3] = 0.;
176  C[4] = 0.;
177  C[5] = INF;
178  C[6] = 0.;
179  C[7] = 0.;
180  C[8] = 0.;
181  C[9] = INF;
182  C[10] = 0.;
183  C[11] = 0.;
184  C[12] = 0.;
185  C[13] = 0.;
186  C[14] = INF;
187  T[0] = T[1] = T[2] = T[3] = T[4] = T[5] = 0;
188  return 1;
189  }
190  if (err) {
191  GetRefChi2() = 0;
192  GetRefNDF() = 0;
193  C[0] = INF;
194  C[1] = 0.;
195  C[2] = INF;
196  C[3] = 0.;
197  C[4] = 0.;
198  C[5] = INF;
199  C[6] = 0.;
200  C[7] = 0.;
201  C[8] = 0.;
202  C[9] = INF;
203  C[10] = 0.;
204  C[11] = 0.;
205  C[12] = 0.;
206  C[13] = 0.;
207  C[14] = INF;
208  T[0] = T[1] = T[2] = T[3] = T[4] = T[5] = 0;
209  }
210  return err;
211 }
212 
213 
214 void CbmKFTrackInterface::Smooth(Double_t Z) {
215  CbmKF* KF = CbmKF::Instance();
216 
217  Double_t* T = GetTrack();
218  Double_t* C = GetCovMatrix();
219  Int_t NHits = GetNOfHits();
220 
221  if (NHits == 0) return;
222 
223  // use initial momentum
224  // this fixed value will be used during fit instead of T[4]
225 
226  Double_t qp0 = T[4];
227 
228  const Double_t INF = 100.;
229 
230  GetRefChi2() = 0;
231  GetRefNDF() = 0;
232 
233  // initialize covariance matrix
234 
235  C[0] = INF;
236  C[1] = 0.;
237  C[2] = INF;
238  C[3] = 0.;
239  C[4] = 0.;
240  C[5] = INF;
241  C[6] = 0.;
242  C[7] = 0.;
243  C[8] = 0.;
244  C[9] = INF;
245  C[10] = 0.;
246  C[11] = 0.;
247  C[12] = 0.;
248  C[13] = 0.;
249  C[14] = INF;
250 
251  // fit downstream
252  {
253  CbmKFHit* h = GetHit(0);
254  if (KF->vMaterial[h->MaterialIndex]->ZReference <= Z) {
255  h->Filter(*this, 1, qp0);
256  Int_t istold = h->MaterialIndex;
257  for (Int_t i = 1; i < NHits; i++) {
258  h = GetHit(i);
259  Int_t ist = h->MaterialIndex;
260  Int_t j = istold + 1;
261  for (; j < ist; j++) {
262  if (KF->vMaterial[j]->ZReference > Z) break;
263  KF->vMaterial[j]->Pass(*this, 1, qp0);
264  }
265  if (j < ist || KF->vMaterial[h->MaterialIndex]->ZReference > Z) break;
266  h->Filter(*this, 1, qp0);
267  istold = ist;
268  }
269  }
270  }
271 
272  KF->Propagate(T, C, Z, qp0);
273  double Ts[6], Cs[15];
274  for (int k = 0; k < 6; k++)
275  Ts[k] = T[k];
276  for (int k = 0; k < 15; k++)
277  Cs[k] = C[k];
278  C[0] = INF;
279  C[1] = 0.;
280  C[2] = INF;
281  C[3] = 0.;
282  C[4] = 0.;
283  C[5] = INF;
284  C[6] = 0.;
285  C[7] = 0.;
286  C[8] = 0.;
287  C[9] = INF;
288  C[10] = 0.;
289  C[11] = 0.;
290  C[12] = 0.;
291  C[13] = 0.;
292  C[14] = INF;
293 
294  { // fit upstream
295  CbmKFHit* h = GetHit(NHits - 1);
296  if (KF->vMaterial[h->MaterialIndex]->ZReference > Z) {
297  h->Filter(*this, 0, qp0);
298  Int_t istold = h->MaterialIndex;
299  for (Int_t i = NHits - 2; i >= 0; i--) {
300  h = GetHit(i);
301  Int_t ist = h->MaterialIndex;
302  Int_t j = istold - 1;
303  for (; j > ist; j--) {
304  if (KF->vMaterial[j]->ZReference <= Z) break;
305  KF->vMaterial[j]->Pass(*this, 0, qp0);
306  }
307  if (j > ist || KF->vMaterial[h->MaterialIndex]->ZReference <= Z) break;
308  h->Filter(*this, 0, qp0);
309  istold = ist;
310  }
311  }
312  }
313  KF->Propagate(T, C, Z, qp0);
314 
315  { // smooth
316 
317  double I[15];
318  for (int k = 0; k < 15; k++)
319  I[k] = C[k] + Cs[k];
320  CbmKFMath::invS(I, 5);
321  double K[25];
322  CbmKFMath::multSSQ(C, I, K, 5);
323  double r[5];
324  for (int k = 0; k < 5; k++)
325  r[k] = T[k] - Ts[k];
326  for (int k = 0; k < 5; k++)
327  for (int l = 0; l < 5; l++)
328  T[k] -= K[k * 5 + l] * r[l];
329 
330  double A[15];
331 
332  for (int l = 0; l < 5; l++)
333  K[(5 + 1) * l] -= 1;
334  for (int l = 0; l < 5; ++l)
335  for (int j = 0; j <= l; ++j) {
336  int ind = CbmKFMath::indexS(l, j);
337  A[ind] = 0;
338  for (int k = 0; k < 5; ++k)
339  A[ind] -= K[l * 5 + k] * C[CbmKFMath::indexS(k, j)];
340  }
341  for (int l = 0; l < 15; l++)
342  C[l] = A[l];
343  }
344 
345  // correct NDF value to number of fitted track parameters (straight line(4) or helix(5) )
346 
347  GetRefNDF() -= (KF->GetMethod() == 0) ? 4 : 5;
348 }
349 
350 void CbmKFTrackInterface::Fit2Vertex(CbmKFVertexInterface& vtx) {
351  Double_t* T = GetTrack();
352  Double_t* C = GetCovMatrix();
353  Double_t* Cv = vtx.GetCovMatrix();
354 
355 
356  Double_t& x = vtx.GetRefX();
357  Double_t& y = vtx.GetRefY();
358  Double_t& z = vtx.GetRefZ();
359 
360  Extrapolate(z);
361  Int_t MaxIter = 2;
362 
363  Double_t a = T[2], b = T[3];
364 
365  //H = {{1 0 0 0 0 }
366  // {0 1 0 0 0}};
367 
368  Double_t zeta[2] = {x - T[0], y - T[1]};
369 
370  Double_t CHt[5][2] = {
371  {C[0], C[1]}, {C[1], C[2]}, {C[3], C[4]}, {C[6], C[7]}, {C[10], C[11]}};
372 
373  for (Int_t iter = 0; iter < MaxIter; iter++) {
374 
375  Double_t Cv0 = Cv[0] - a * (2 * Cv[3] - a * Cv[5]);
376  Double_t Cv1 = Cv[1] - b * Cv[3] - a * (Cv[4] - b * Cv[5]);
377  Double_t Cv2 = Cv[2] - b * (2 * Cv[4] - b * Cv[5]);
378 
379  Double_t S[3] = {C[0] + Cv0, C[1] + Cv1, C[2] + Cv2};
380 
381  //* Invert S
382  {
383  Double_t s = S[0] * S[2] - S[1] * S[1];
384  if (s < 1.E-20) return;
385  s = 1. / s;
386  Double_t S0 = S[0];
387  S[0] = s * S[2];
388  S[1] = -s * S[1];
389  S[2] = s * S0;
390  }
391 
392  //* Kalman gain K = CH'*S
393 
394  Double_t K[5][2];
395 
396  if (iter < MaxIter - 1) {
397 
398  for (Int_t i = 2; i < 4; ++i) {
399  K[i][0] = CHt[i][0] * S[0] + CHt[i][1] * S[1];
400  K[i][1] = CHt[i][0] * S[1] + CHt[i][1] * S[2];
401  }
402 
403  //* New estimation of the track slopes
404 
405  a = T[2] + K[2][0] * zeta[0] + K[2][1] * zeta[1];
406  b = T[3] + K[3][0] * zeta[0] + K[3][1] * zeta[1];
407 
408  continue;
409  }
410 
411  for (Int_t i = 0; i < 5; ++i) {
412  K[i][0] = CHt[i][0] * S[0] + CHt[i][1] * S[1];
413  K[i][1] = CHt[i][0] * S[1] + CHt[i][1] * S[2];
414  }
415 
416  //* New estimation of the track parameters r += K*zeta
417 
418  T[0] = x;
419  T[1] = y;
420  T[2] += K[2][0] * zeta[0] + K[2][1] * zeta[1];
421  T[3] += K[3][0] * zeta[0] + K[3][1] * zeta[1];
422  T[4] += K[4][0] * zeta[0] + K[4][1] * zeta[1];
423  T[5] = z;
424 
425  GetRefNDF() += 2;
426  GetRefChi2() += zeta[0] * zeta[0] * S[0] + 2 * zeta[0] * zeta[1] * S[1]
427  + zeta[1] * zeta[1] * S[2];
428 
429  //* New covariance matrix C -= K*(CH')'
430 
431  C[0] -= K[0][0] * CHt[0][0] + K[0][1] * CHt[0][1];
432  C[1] -= K[1][0] * CHt[0][0] + K[1][1] * CHt[0][1];
433  C[2] -= K[1][0] * CHt[1][0] + K[1][1] * CHt[1][1];
434  C[3] -= K[2][0] * CHt[0][0] + K[2][1] * CHt[0][1];
435  C[4] -= K[2][0] * CHt[1][0] + K[2][1] * CHt[1][1];
436  C[5] -= K[2][0] * CHt[2][0] + K[2][1] * CHt[2][1];
437  C[6] -= K[3][0] * CHt[0][0] + K[3][1] * CHt[0][1];
438  C[7] -= K[3][0] * CHt[1][0] + K[3][1] * CHt[1][1];
439  C[8] -= K[3][0] * CHt[2][0] + K[3][1] * CHt[2][1];
440  C[9] -= K[3][0] * CHt[3][0] + K[3][1] * CHt[3][1];
441  C[10] -= K[4][0] * CHt[0][0] + K[4][1] * CHt[0][1];
442  C[11] -= K[4][0] * CHt[1][0] + K[4][1] * CHt[1][1];
443  C[12] -= K[4][0] * CHt[2][0] + K[4][1] * CHt[2][1];
444  C[13] -= K[4][0] * CHt[3][0] + K[4][1] * CHt[3][1];
445  C[14] -= K[4][0] * CHt[4][0] + K[4][1] * CHt[4][1];
446  }
447 }
448 
449 Int_t CbmKFTrackInterface::Propagate(Double_t z_out, Double_t QP0) {
450  return CbmKF::Instance()->Propagate(GetTrack(), GetCovMatrix(), z_out, QP0);
451 }
452 
453 Int_t CbmKFTrackInterface::Propagate(Double_t z_out) {
454  return Propagate(z_out, GetTrack()[4]);
455 }
CbmKFVertexInterface.h
CbmKF.h
CbmKFTrackInterface::GetHit
virtual CbmKFHit * GetHit(Int_t)
Number of hits.
Definition: CbmKFTrackInterface.h:55
CbmKFMaterial::compareP_Z
static Bool_t compareP_Z(Double_t z, const CbmKFMaterial *a)
Definition: CbmKFMaterial.h:62
CbmKFTrackInterface::GetTrack
virtual Double_t * GetTrack()
Is it electron.
Definition: CbmKFTrackInterface.cxx:33
CbmKF::Propagate
Int_t Propagate(Double_t *T, Double_t *C, Double_t z_out, Double_t QP0)
Definition: CbmKF.cxx:554
ClassImp
ClassImp(CbmKFTrackInterface) static Double_t gTempD[100]
CbmKFMath::indexS
static Int_t indexS(Int_t i, Int_t j)
Definition: CbmKFMath.h:39
CbmKFMath::multSSQ
static void multSSQ(const Double_t *A, const Double_t *B, Double_t *C, Int_t n)
Definition: CbmKFMath.cxx:94
CbmKFHit
Definition: CbmKFHit.h:16
i
int i
Definition: L1/vectors/P4_F32vec4.h:25
CbmKF::GetMethod
Int_t GetMethod()
Definition: CbmKF.h:96
CbmKFTrackInterface::Fit2Vertex
void Fit2Vertex(CbmKFVertexInterface &vtx)
Definition: CbmKFTrackInterface.cxx:350
CbmKFFieldMath.h
CbmKF
Definition: CbmKF.h:33
CbmKFTrackInterface.h
CbmKFMath.h
CbmKF::Instance
static CbmKF * Instance()
Definition: CbmKF.h:39
CbmKFVertexInterface::GetRefZ
virtual Double_t & GetRefZ()
Definition: CbmKFVertexInterface.cxx:30
h
Data class with information on a STS local track.
gTempI
static Int_t gTempI[10]
Definition: CbmKFTrackInterface.cxx:31
CbmKFMath::invS
static Bool_t invS(Double_t A[], Int_t N)
Definition: CbmKFMath.cxx:232
d
double d
Definition: P4_F64vec2.h:24
CbmKFTrackInterface::GetNOfHits
virtual Int_t GetNOfHits()
Number of Degrees of Freedom after fit.
Definition: CbmKFTrackInterface.h:54
CbmKFVertexInterface
Definition: CbmKFVertexInterface.h:24
CbmKF::vMaterial
std::vector< CbmKFMaterial * > vMaterial
Definition: CbmKF.h:71
CbmKFTrackInterface::GetRefNDF
virtual Int_t & GetRefNDF()
Chi^2 after fit.
Definition: CbmKFTrackInterface.cxx:36
CbmKFVertexInterface::GetRefX
virtual Double_t & GetRefX()
Definition: CbmKFVertexInterface.cxx:28
CbmKFTrackInterface
Definition: CbmKFTrackInterface.h:26
CbmKFTrackInterface::Smooth
void Smooth(Double_t Z)
Definition: CbmKFTrackInterface.cxx:214
CbmKFVertexInterface::GetRefY
virtual Double_t & GetRefY()
Definition: CbmKFVertexInterface.cxx:29
CbmKFTrackInterface::GetCovMatrix
virtual Double_t * GetCovMatrix()
array[6] of track parameters(x,y,tx,ty,qp,z)
Definition: CbmKFTrackInterface.cxx:34
x
Double_t x
Definition: CbmMvdSensorDigiToHitTask.cxx:68
CbmKFMaterial::compareP_z
static Bool_t compareP_z(const CbmKFMaterial *a, Double_t z)
Definition: CbmKFMaterial.h:58
y
Double_t y
Definition: CbmMvdSensorDigiToHitTask.cxx:68
CbmKFHit.h
CbmKFTrackInterface::Propagate
Int_t Propagate(Double_t z_out, Double_t QP0)
Definition: CbmKFTrackInterface.cxx:449
CbmKFTrackInterface::Fit
Int_t Fit(Bool_t downstream=1)
Definition: CbmKFTrackInterface.cxx:101
CbmKFTrackInterface::GetRefChi2
virtual Double_t & GetRefChi2()
array[15] of covariance matrix
Definition: CbmKFTrackInterface.cxx:35
CbmKFVertexInterface::GetCovMatrix
virtual Double_t * GetCovMatrix()
Definition: CbmKFVertexInterface.cxx:31
CbmKFMath::GetThickness
static Bool_t GetThickness(Double_t z1, Double_t z2, Double_t mz, Double_t mthick, Double_t *mz_out, Double_t *mthick_out)
Definition: CbmKFMath.cxx:717
CbmKFTrackInterface::Extrapolate
Int_t Extrapolate(Double_t z, Double_t *QP0=0)
Access to i-th hit.
Definition: CbmKFTrackInterface.cxx:39