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