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