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