BOSS 8.0.0
BESIII Offline Software System
Loading...
Searching...
No Matches
KalFitTrack.cxx
Go to the documentation of this file.
1
2// #include "TrackUtil/Helix.h"
3#include "KalFitAlg/KalFitTrack.h"
4#include "CLHEP/Geometry/Transform3D.h"
5#include "CLHEP/Geometry/Vector3D.h"
6#include "CLHEP/Matrix/Matrix.h"
7#include "CLHEP/Matrix/SymMatrix.h"
8#include "CLHEP/Matrix/Vector.h"
9#include "CLHEP/Units/PhysicalConstants.h"
10#include "CLHEP/Vector/ThreeVector.h"
11#include "GaudiKernel/Bootstrap.h"
12#include "KalFitAlg/KalFitCylinder.h"
13#include "KalFitAlg/KalFitElement.h"
14#include "KalFitAlg/KalFitMaterial.h"
15#include "KalFitAlg/KalFitWire.h"
16#include "KalFitAlg/helix/Helix.h"
17#include "Math/Point3D.h"
18#include "Math/Point3Dfwd.h"
19#include "Math/Vector3D.h"
20#include "MdcGeomSvc/IMdcGeomSvc.h"
21
22using namespace ROOT::Math;
23
24using namespace KalmanFit;
25
26using namespace CLHEP;
27typedef HepGeom::Transform3D HepTransform3D;
28typedef HepGeom::Vector3D<double> HepVector3D;
29
30// for debug purpose .
31int KalFitTrack::drifttime_choice_ = 0;
32int KalFitTrack::debug_ = 0;
33int KalFitTrack::numf_ = 0;
34int KalFitTrack::numfcor_ = 1;
35double KalFitTrack::Bznom_ = 10;
36int KalFitTrack::steplev_ = 0;
37int KalFitTrack::inner_steps_ = 3;
38int KalFitTrack::outer_steps_ = 3;
39// Mdc factors :
40int KalFitTrack::Tof_correc_ = 1;
41int KalFitTrack::strag_ = 1;
42double KalFitTrack::chi2_hitf_ = 1000;
43double KalFitTrack::chi2_hits_ = 1000;
44double KalFitTrack::factor_strag_ = 0.4;
45// double KalFitTrack::chi2_hitf_ = DBL_MAX;
46int KalFitTrack::lead_ = 2;
47int KalFitTrack::back_ = 1;
48// attention resolflag !!!
49int KalFitTrack::resolflag_ = 0;
50int KalFitTrack::tofall_ = 1;
51// chi2 cut set :
52int KalFitTrack::nmdc_hit2_ = 500;
53// int KalFitTrack::LR_ = 0;
54int KalFitTrack::LR_ = 1;
55
56double KalFitTrack::dchi2cutf_anal[43] = { 0. };
57double KalFitTrack::dchi2cuts_anal[43] = { 0. };
58double KalFitTrack::dchi2cutf_calib[43] = { 0. };
59double KalFitTrack::dchi2cuts_calib[43] = { 0. };
60
61///
62double KalFitTrack::mdcGasRadlen_ = 0.;
63
64const double KalFitTrack::MASS[NMASS] = { 0.000510999, 0.105658, 0.139568, 0.493677,
65 0.938272 };
66const double M_PI2 = 2. * M_PI;
67
68const double M_PI4 = 4. * M_PI;
69
70const double M_PI8 = 8. * M_PI;
71
72// constructor
73KalFitTrack::KalFitTrack( const HepPoint3D& pivot, const HepVector& a, const HepSymMatrix& Ea,
74 unsigned int m, double chisq, unsigned int nhits )
75 : Helix( pivot, a, Ea )
76 , type_( 0 )
77 , insist_( 0 )
78 , chiSq_( 0 )
79 , nchits_( 0 )
80 , nster_( 0 )
81 , ncath_( 0 )
82 , ndf_back_( 0 )
83 , chiSq_back_( 0 )
84 , pathip_( 0 )
85 , path_rd_( 0 )
86 , path_ab_( 0 )
87 , tof_( 0 )
88 , dchi2_max_( 0 )
89 , r_max_( 0 )
90 , tof_kaon_( 0 )
91 , tof_proton_( 0 )
92 , pat1_( 0 )
93 , pat2_( 0 )
94 , layer_prec_( 0 )
95 , trasan_id_( 0 )
96 , nhit_r_( 0 )
97 , nhit_z_( 0 )
98 , pathSM_( 0 )
99 , tof2_( 0 ) {
100 memset( PathL_, 0, sizeof( PathL_ ) );
101 l_mass_ = m;
102 if ( m < NMASS ) mass_ = MASS[m];
103 else mass_ = MASS[2];
104 r0_ = fabs( center().perp() - fabs( radius() ) );
105 // bFieldZ(Bznom_);
106 Bznom_ = bFieldZ(); // 2012-09-13 wangll
107 update_last();
108}
109
110// destructor
111KalFitTrack::~KalFitTrack( void ) {
112 // delete all objects
113}
114
115void KalFitTrack::update_last( void ) {
116 pivot_last_ = pivot();
117 a_last_ = a();
118 Ea_last_ = Ea();
119}
120
121void KalFitTrack::update_forMdc( void ) {
122 pivot_forMdc_ = pivot();
123 a_forMdc_ = a();
124 Ea_forMdc_ = Ea();
125}
126
127double KalFitTrack::intersect_cylinder( double r ) const {
128 double m_rad = radius();
129 double l = center().perp();
130
131 double cosPhi = ( m_rad * m_rad + l * l - r * r ) / ( 2 * m_rad * l );
132
133 if ( cosPhi < -1 || cosPhi > 1 ) return 0;
134
135 double dPhi = center().phi() - acos( cosPhi ) - phi0();
136
137 if ( dPhi < -M_PI ) dPhi += 2 * M_PI;
138
139 return dPhi;
140}
141
142double KalFitTrack::intersect_zx_plane( const HepTransform3D& plane, double y ) const {
143 HepPoint3D xc = plane * center();
144 double r = radius();
145 double d = r * r - ( y - xc.y() ) * ( y - xc.y() );
146 if ( d < 0 ) return 0;
147
148 double xx = xc.x();
149 if ( xx > 0 ) xx -= sqrt( d );
150 else xx += sqrt( d );
151
152 double l = ( plane.inverse() * HepPoint3D( xx, y, 0 ) ).perp();
153
154 return intersect_cylinder( l );
155}
156
157double KalFitTrack::intersect_yz_plane( const HepTransform3D& plane, double x ) const {
158 HepPoint3D xc = plane * center();
159 double r = radius();
160 double d = r * r - ( x - xc.x() ) * ( x - xc.x() );
161 if ( d < 0 ) return 0;
162
163 double yy = xc.y();
164 if ( yy > 0 ) yy -= sqrt( d );
165 else yy += sqrt( d );
166
167 double l = ( plane.inverse() * HepPoint3D( x, yy, 0 ) ).perp();
168
169 return intersect_cylinder( l );
170}
171
172double KalFitTrack::intersect_xy_plane( double z ) const {
173 if ( tanl() != 0 && radius() != 0 )
174 return ( pivot().z() + dz() - z ) / ( radius() * tanl() );
175 else return 0;
176}
177
178void KalFitTrack::ms( double path, const KalFitMaterial& material, int index ) {
179 HepSymMatrix ea = Ea();
180 // cout<<"ms():path "<<path<<endl;
181 // cout<<"ms():ea before: "<<ea<<endl;
182 double k = kappa();
183 double t = tanl();
184 double t2 = t * t;
185 double pt2 = 1 + t2;
186 double k2 = k * k;
187
188 double pmag = 1 / fabs( k ) * sqrt( pt2 );
189 double dth = material.mcs_angle( mass_, path, pmag );
190 double dth2 = dth * dth;
191 double pt2dth2 = pt2 * dth2;
192
193 ea[1][1] += pt2dth2;
194 ea[2][2] += k2 * t2 * dth2;
195 ea[2][4] += k * t * pt2dth2;
196 ea[4][4] += pt2 * pt2dth2;
197
198 ea[3][3] += dth2 * path * path / 3 / ( 1 + t2 );
199 ea[3][4] += dth2 * path / 2 * sqrt( 1 + t2 );
200 ea[3][2] += dth2 * t / sqrt( 1 + t2 ) * k * path / 2;
201 ea[0][0] += dth2 * path * path / 3;
202 ea[0][1] += dth2 * sqrt( 1 + t2 ) * path / 2;
203
204 Ea( ea );
205 // cout<<"ms():ms angle in this: "<<dth<<endl;
206 // cout<<"ms():ea after: "<<Ea()<<endl;
207 if ( index < 0 )
208 {
209 double x0 = material.X0();
210 if ( x0 ) path_rd_ += path / x0;
211 }
212}
213
214void KalFitTrack::msgasmdc( double path, int index ) {
215 double k = kappa();
216 double t = tanl();
217 double t2 = t * t;
218 double k2 = k * k;
219
220 double pmag = ( 1 / fabs( k ) ) * sqrt( 1 + t2 );
221 double psq = pmag * pmag;
222 /*
223 double Zprims = 3/2*0.076 + 0.580/9.79*4.99*(4.99+1) +
224 0.041/183.85*74*(74+1) + 0.302/26.98 * 13 * (13+1);
225 double chicc = 0.00039612 * sqrt(Zprims * 0.001168);
226 double dth = 2.557 * chicc * sqrt(path * (mass_*mass_ + psq)) / psq;
227 */
228
229 // std::cout<<" mdcGasRadlen: "<<mdcGasRadlen_<<std::endl;
230 double pathx = path / mdcGasRadlen_;
231 double dth =
232 0.0136 * sqrt( pathx * ( mass_ * mass_ + psq ) ) / psq * ( 1 + 0.038 * log( pathx ) );
233 ;
234 HepSymMatrix ea = Ea();
235#ifdef YDEBUG
236 cout << "msgasmdc():path " << path << " pathx " << pathx << endl;
237 cout << "msgasmdc():dth " << dth << endl;
238 cout << "msgasmdc():ea before: " << ea << endl;
239#endif
240 double dth2 = dth * dth;
241
242 ea[1][1] += ( 1 + t2 ) * dth2;
243 ea[2][2] += k2 * t2 * dth2;
244 ea[2][4] += k * t * ( 1 + t2 ) * dth2;
245 ea[4][4] += ( 1 + t2 ) * ( 1 + t2 ) * dth2;
246
247 // additionnal terms (terms proportional to l and l^2)
248
249 ea[3][3] += dth2 * path * path / 3 / ( 1 + t2 );
250 ea[3][4] += dth2 * path / 2 * sqrt( 1 + t2 );
251 ea[3][2] += dth2 * t / sqrt( 1 + t2 ) * k * path / 2;
252 ea[0][0] += dth2 * path * path / 3;
253 ea[0][1] += dth2 * sqrt( 1 + t2 ) * path / 2;
254
255 Ea( ea );
256#ifdef YDEBUG
257 cout << "msgasmdc():ea after: " << Ea() << endl;
258#endif
259 if ( index < 0 )
260 {
261 pathip_ += path;
262 // RMK : put by hand, take care !!
263 double x0 = mdcGasRadlen_; // for the Mdc gas
264 path_rd_ += path / x0;
265 tof( path );
266#ifdef YDEBUG
267 cout << "ms...pathip..." << pathip_ << endl;
268#endif
269 }
270}
271
272void KalFitTrack::eloss( double path, const KalFitMaterial& material, int index ) {
273#ifdef YDEBUG
274 cout << "eloss():ea before: " << Ea() << endl;
275#endif
276 HepVector v_a = a();
277 double v_a_2_2 = v_a[2] * v_a[2];
278 double v_a_4_2 = v_a[4] * v_a[4];
279 double pmag = 1 / fabs( v_a[2] ) * sqrt( 1 + v_a_4_2 );
280 double psq = pmag * pmag;
281 double E = sqrt( mass_ * mass_ + psq );
282 double dE = material.dE( mass_, path, pmag );
283 // std::cout<<" eloss(): dE: "<<dE<<std::endl;//wangll
284
285 if ( index > 0 ) psq += dE * ( dE + 2 * sqrt( mass_ * mass_ + psq ) );
286 else
287 {
288 double dE_max = E - mass_;
289 if ( dE < dE_max ) psq += dE * ( dE - 2 * sqrt( mass_ * mass_ + psq ) );
290 else psq = -1.0;
291 }
292
293 if ( tofall_ && index < 0 )
294 {
295 // Kaon case :
296 if ( p_kaon_ > 0 )
297 {
298 double psq_kaon = p_kaon_ * p_kaon_;
299 double dE_kaon = material.dE( MASS[3], path, p_kaon_ );
300 psq_kaon += dE_kaon * ( dE_kaon - 2 * sqrt( MASS[3] * MASS[3] + psq_kaon ) );
301 if ( psq_kaon < 0 ) psq_kaon = 0;
302 p_kaon_ = sqrt( psq_kaon );
303 }
304 // Proton case :
305 if ( p_proton_ > 0 )
306 {
307 double psq_proton = p_proton_ * p_proton_;
308 double dE_proton = material.dE( MASS[4], path, p_proton_ );
309 psq_proton += dE_proton * ( dE_proton - 2 * sqrt( MASS[4] * MASS[4] + psq_proton ) );
310 if ( psq_proton < 0 ) psq_proton = 0;
311 p_proton_ = sqrt( psq_proton );
312 }
313 }
314
315 double dpt;
316 // cout<<"eloss(): psq = "<<psq<<endl;//wangll
317 if ( psq < 0 ) dpt = 9999;
318 else dpt = v_a[2] * pmag / sqrt( psq );
319 // cout<<"eloss():k: "<<v_a[2]<<" k' "<<dpt<<endl;//wangll
320#ifdef YDEBUG
321 cout << "eloss():k: " << v_a[2] << " k' " << dpt << endl;
322#endif
323 // attempt to take account of energy loss for error matrix
324
325 HepSymMatrix ea = Ea();
326 double r_E_prim = ( E + dE ) / E;
327
328 // 1/ Straggling in the energy loss :
329 if ( strag_ )
330 {
331 double del_E( 0 );
332 if ( l_mass_ == 0 ) { del_E = dE * factor_strag_; }
333 else { del_E = material.del_E( mass_, path, pmag ); }
334
335 ea[2][2] += ( v_a[2] * v_a[2] ) * E * E * del_E * del_E / ( psq * psq );
336 }
337
338 // Effect of the change of curvature (just variables change):
339 double dpt2 = dpt * dpt;
340 double A = dpt * dpt2 / ( v_a_2_2 * v_a[2] ) * r_E_prim;
341 double B = v_a[4] / ( 1 + v_a_4_2 ) * dpt * ( 1 - dpt2 / v_a_2_2 * r_E_prim );
342
343 double ea_2_0 = A * ea[2][0] + B * ea[4][0];
344 double ea_2_1 = A * ea[2][1] + B * ea[4][1];
345 double ea_2_2 = A * A * ea[2][2] + 2 * A * B * ea[2][4] + B * B * ea[4][4];
346 double ea_2_3 = A * ea[2][3] + B * ea[4][3];
347 double ea_2_4 = A * ea[2][4] + B * ea[4][4];
348
349 v_a[2] = dpt;
350 a( v_a );
351
352 ea[2][0] = ea_2_0;
353 // std::cout<<"ea[2][0] is "<<ea[2][0]<<" ea(3,1) is "<<ea(3,1)<<std::endl;
354 ea[2][1] = ea_2_1;
355 // std::cout<<"ea[2][2] is "<<ea[2][2]<<" ea(3,3) is "<<ea(3,3)<<std::endl;
356 ea[2][2] = ea_2_2;
357 ea[2][3] = ea_2_3;
358 ea[2][4] = ea_2_4;
359
360 Ea( ea );
361 // cout<<"eloss():dE: "<<dE<<endl;
362 // cout<<"eloss():A: "<<A<<" B: "<<B<<endl;
363 // cout<<"eloss():ea after: "<<Ea()<<endl;
364 r0_ = fabs( center().perp() - fabs( radius() ) );
365}
366
367void KalFitTrack::order_wirhit( int index ) {
368 unsigned int nhit = HitsMdc_.size();
369 Helix tracktest = *(Helix*)this;
370 int ind = 0;
371 double* Rad = new double[nhit];
372 double* Ypos = new double[nhit];
373 for ( unsigned i = 0; i < nhit; i++ )
374 {
375
376 HepPoint3D fwd( HitsMdc_[i].wire().fwd() );
377 HepPoint3D bck( HitsMdc_[i].wire().bck() );
378 Hep3Vector wire = (CLHEP::Hep3Vector)fwd - (CLHEP::Hep3Vector)bck;
379
380 // Modification for stereo wires :
381 Helix work = tracktest;
382 work.ignoreErrorMatrix();
383 work.pivot( ( fwd + bck ) * .5 );
384 HepPoint3D x0 = ( work.x( 0 ).z() - bck.z() ) / wire.z() * wire + bck;
385
386 tracktest.pivot( x0 );
387 Rad[ind] = tracktest.x( 0 ).perp();
388 Ypos[ind] = x0.y();
389 ind++;
390 // cout<<"Ypos: "<<Ypos[ind-1]<<endl;
391 }
392
393 // Reorder...
394 if ( index < 0 )
395 for ( int j, k = nhit - 1; k >= 0; k = j )
396 {
397 j = -1;
398 for ( int i = 1; i <= k; i++ )
399 if ( Rad[i - 1] > Rad[i] )
400 {
401 j = i - 1;
402 std::swap( Rad[i], Rad[j] );
403 std::swap( HitsMdc_[i], HitsMdc_[j] );
404 }
405 }
406 if ( index > 0 )
407 for ( int j, k = nhit - 1; k >= 0; k = j )
408 {
409 j = -1;
410 for ( int i = 1; i <= k; i++ )
411 if ( Rad[i - 1] < Rad[i] )
412 {
413 j = i - 1;
414 std::swap( Rad[i], Rad[j] );
415 std::swap( HitsMdc_[i], HitsMdc_[j] );
416 }
417 }
418 if ( index == 0 )
419 for ( int j, k = nhit - 1; k >= 0; k = j )
420 {
421 j = -1;
422 for ( int i = 1; i <= k; i++ )
423 if ( Ypos[i - 1] > Ypos[i] )
424 {
425 j = i - 1;
426 std::swap( Ypos[i], Ypos[j] );
427 std::swap( HitsMdc_[i], HitsMdc_[j] );
428 }
429 }
430 delete[] Rad;
431 delete[] Ypos;
432}
433
434void KalFitTrack::order_hits( void ) {
435 for ( int it = 0; it < HitsMdc().size() - 1; it++ )
436 {
437 if ( HitsMdc_[it].wire().layer().layerId() == HitsMdc_[it + 1].wire().layer().layerId() )
438 {
439 if ( ( kappa() < 0 ) &&
440 ( HitsMdc_[it].wire().localId() > HitsMdc_[it + 1].wire().localId() ) )
441 { std::swap( HitsMdc_[it], HitsMdc_[it + 1] ); }
442 if ( ( kappa() > 0 ) &&
443 ( HitsMdc_[it].wire().localId() < HitsMdc_[it + 1].wire().localId() ) )
444 { std::swap( HitsMdc_[it], HitsMdc_[it + 1] ); }
445 }
446 }
447}
448
449void KalFitTrack::number_wirhit( void ) {
450 unsigned int nhit = HitsMdc_.size();
451 int Num[50] = { 0 };
452 for ( unsigned i = 0; i < nhit; i++ ) Num[HitsMdc_[i].wire().layer().layerId()]++;
453 for ( unsigned i = 0; i < nhit; i++ )
454 if ( Num[HitsMdc_[i].wire().layer().layerId()] > 2 ) HitsMdc_[i].chi2( -2 );
455}
456
457double KalFitTrack::smoother_Mdc( KalFitHitMdc& HitMdc, Hep3Vector& meas, KalFitHelixSeg& seg,
458 double& dchi2, int csmflag ) {
459 //
460 double lr = HitMdc.LR();
461 // For taking the propagation delay into account :
462 int wire_ID = HitMdc.wire().geoID(); // from 0 to 6796
463 int layerid = HitMdc.wire().layer().layerId();
464 double entrangle = HitMdc.rechitptr()->getEntra();
465 if ( wire_ID < 0 || wire_ID > 6796 )
466 { // bes
467 std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
468 return 0;
469 }
470
471 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
472 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
473
474 double tofest( 0 );
475 double dd( 0. );
476 double phi = fmod( phi0() + M_PI4, M_PI2 );
477 double csf0 = cos( phi );
478 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
479 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
480 if ( phi > M_PI ) snf0 = -snf0;
481
482 if ( Tof_correc_ )
483 {
484 Hep3Vector ip( 0, 0, 0 );
485 Helix work = *(Helix*)this;
486 work.ignoreErrorMatrix();
487 work.pivot( ip );
488 double phi_ip = work.phi0();
489 if ( fabs( phi - phi_ip ) > M_PI )
490 {
491 if ( phi > phi_ip ) phi -= 2 * M_PI;
492 else phi_ip -= 2 * M_PI;
493 }
494 double t = tanl();
495 double l = fabs( radius() * ( phi - phi_ip ) * sqrt( 1 + t * t ) );
496 double pmag( sqrt( 1.0 + t * t ) / kappa() );
497 double mass_over_p( mass_ / pmag );
498 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
499 tofest = l / ( 29.9792458 * beta );
500 if ( csmflag == 1 && HitMdc.wire().y() > 0. ) tofest = -1. * tofest;
501 }
502
503 const HepSymMatrix& ea = Ea();
504 const HepVector& v_a = a();
505
506 HepPoint3D pivot_work = pivot();
507
508 double dchi2R( 99999999 ), dchi2L( 99999999 );
509
510 HepVector v_H( 5, 0 );
511 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
512 v_H[3] = -meas.z();
513 HepMatrix v_HT = v_H.T();
514
515 double estim = ( v_HT * v_a )[0];
516 HepVector ea_v_H = ea * v_H;
517 HepMatrix ea_v_HT = ( ea_v_H ).T();
518 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
519
520 HepSymMatrix eaNew( 5 );
521 HepVector aNew( 5 );
522
523 // double time = HitMdc.tdc();
524 // if (Tof_correc_)
525 // time -= tofest;
526
527 double drifttime = getDriftTime( HitMdc, tofest );
528 // cout<<"drifttime = "<<drifttime<<endl;
529 seg.dt( drifttime );
530 double ddl = getDriftDist( HitMdc, drifttime, 0 );
531 double ddr = getDriftDist( HitMdc, drifttime, 1 );
532 /*cout<<endl<<" $$$ smoother_Mdc():: "<<endl;//wangll
533 cout<<"pivot = "<<pivot()<<endl;//wangll
534 cout<<"helix = "<<a()<<endl;//wangll
535 cout<<"drifttime = "<<drifttime<<endl;//wangll
536 cout<<"ddl, ddr = "<<ddl<<", "<<ddr<<endl;//wangll
537 */
538 double erddl = getSigma( HitMdc, a()[4], 0, ddl );
539 double erddr = getSigma( HitMdc, a()[4], 1, ddr );
540
541 // double dd = 1.0e-4 * 40.0 * time;
542 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr }; // millimeter to centimeter
543 double er_dmeas2[2] = { 0., 0. };
544 if ( resolflag_ == 1 )
545 {
546 er_dmeas2[0] = 0.1 * erddl;
547 er_dmeas2[1] = 0.1 * erddr;
548 }
549 else if ( resolflag_ == 0 )
550 {
551 // int layid = HitMdc.wire().layer().layerId();
552 // double sigma = getSigma(layid, dd);
553 // er_dmeas2[0] = er_dmeas2[1] = sigma;
554 }
555
556 // Left hypothesis :
557 if ( lr == -1 )
558 {
559 double er_dmeasL, dmeasL;
560 if ( Tof_correc_ )
561 {
562 dmeasL = ( -1.0 ) * dmeas2[0]; // the dmeas&erdmeas calculated by myself
563 er_dmeasL = er_dmeas2[0];
564 }
565 else
566 {
567 dmeasL = ( -1.0 ) * fabs( HitMdc.dist()[0] ); // the dmeas&erdmeas calculated by
568 // track-finding algorithm
569 er_dmeasL = HitMdc.erdist()[0];
570 }
571
572 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
573 eaNew.assign( ea - ea_v_H * AL * ea_v_HT );
574 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
575 if ( 0. == RkL ) RkL = 1.e-4;
576
577 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
578 aNew = v_a + diffL;
579 double sigL = ( dmeasL - ( v_HT * aNew )[0] );
580 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
581 }
582 else if ( lr == 1 )
583 {
584 // Right hypothesis :
585
586 double er_dmeasR, dmeasR;
587 if ( Tof_correc_ )
588 {
589 dmeasR = dmeas2[1];
590 er_dmeasR = er_dmeas2[1];
591 }
592 else
593 {
594 dmeasR = fabs( HitMdc.dist()[1] );
595 er_dmeasR = HitMdc.erdist()[1];
596 }
597
598 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
599 eaNew.assign( ea - ea_v_H * AR * ea_v_HT );
600 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
601 if ( 0. == RkR ) RkR = 1.e-4;
602
603 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
604 aNew = v_a + diffR;
605 double sigR = ( dmeasR - ( v_HT * aNew )[0] );
606 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
607 }
608
609 // Update Kalman result :
610#ifdef YDEBUG
611 cout << "in smoother_Mdc: lr= " << lr << " a: " << v_a << " a_NEW: " << aNew << endl;
612#endif
613 double dchi2_loc( 0 );
614 if ( ( dchi2R < dchi2cuts_anal[layerid] && lr == 1 ) ||
615 ( dchi2L < dchi2cuts_anal[layerid] && lr == -1 ) )
616 {
617 ndf_back_++;
618 int chge( 1 );
619 if ( !( aNew[2] && fabs( aNew[2] - a()[2] ) < 1.0 ) ) chge = 0;
620 if ( lr == 1 )
621 {
622 chiSq_back_ += dchi2R;
623 dchi2_loc = dchi2R;
624 dd = 0.1 * ddr;
625 }
626 else if ( lr == -1 )
627 {
628 chiSq_back_ += dchi2L;
629 dchi2_loc = dchi2L;
630 dd = -0.1 * ddl;
631 }
632 if ( chge )
633 {
634 a( aNew );
635 Ea( eaNew );
636 }
637 }
638 dchi2 = dchi2_loc;
639
640 seg.doca_include( fabs( ( v_HT * a() )[0] ) );
641 seg.doca_exclude( fabs( estim ) );
642 /// move the pivot of the helixseg to IP(0,0,0)
643 Hep3Vector ip( 0, 0, 0 );
644 KalFitTrack helixNew1( pivot_work, a(), Ea(), 0, 0, 0 );
645 helixNew1.pivot( ip );
646 HepVector a_temp1 = helixNew1.a();
647 HepSymMatrix ea_temp1 = helixNew1.Ea();
648 seg.Ea( ea_temp1 );
649 seg.a( a_temp1 );
650 seg.Ea_include( ea_temp1 );
651 seg.a_include( a_temp1 );
652
653 KalFitTrack helixNew2( pivot_work, v_a, ea, 0, 0, 0 );
654 helixNew2.pivot( ip );
655 HepVector a_temp2 = helixNew2.a();
656 HepSymMatrix ea_temp2 = helixNew2.Ea();
657 seg.Ea_exclude( ea_temp2 );
658 seg.a_exclude( a_temp2 );
659
660 seg.tof( tofest );
661 seg.dd( dd );
662
663 return chiSq_back_;
664}
665
666// Smoothing procedure in Mdc cosmic align
667// RMK attention for the case chi2R = chi2L during forward filter !
668double KalFitTrack::smoother_Mdc_csmalign( KalFitHelixSeg& seg, Hep3Vector& meas, int& flg,
669 int csmflag ) {
670
671 HepPoint3D ip( 0., 0., 0. );
672 // attention! we should not to decide the left&right in the smoother process,
673 // because we choose the left&right of hits from the filter process.
674
675 KalFitHitMdc* HitMdc = seg.HitMdc();
676 double lr = HitMdc->LR();
677
678 // For taking the propagation delay into account :
679 int layerid = ( *HitMdc ).wire().layer().layerId();
680 int wire_ID = HitMdc->wire().geoID();
681 double entrangle = HitMdc->rechitptr()->getEntra();
682
683 if ( wire_ID < 0 || wire_ID > 6796 )
684 { // bes
685 std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
686 return 0;
687 }
688
689 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
690 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
691 double dd( 0. );
692 double tofest( 0 );
693 double phi = fmod( phi0() + M_PI4, M_PI2 );
694 double csf0 = cos( phi );
695 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
696 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
697 if ( phi > M_PI ) snf0 = -snf0;
698
699 if ( Tof_correc_ )
700 {
701 Hep3Vector ip( 0, 0, 0 );
702 Helix work = *(Helix*)this;
703 work.ignoreErrorMatrix();
704 work.pivot( ip );
705 double phi_ip = work.phi0();
706 if ( fabs( phi - phi_ip ) > M_PI )
707 {
708 if ( phi > phi_ip ) phi -= 2 * M_PI;
709 else phi_ip -= 2 * M_PI;
710 }
711 double t = tanl();
712 double l = fabs( radius() * ( phi - phi_ip ) * sqrt( 1 + t * t ) );
713 double pmag( sqrt( 1.0 + t * t ) / kappa() );
714 double mass_over_p( mass_ / pmag );
715 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
716 tofest = l / ( 29.9792458 * beta );
717 if ( csmflag == 1 && ( *HitMdc ).wire().y() > 0. ) tofest = -1. * tofest;
718 }
719
720 const HepSymMatrix& ea = Ea();
721 const HepVector& v_a = a();
722
723 HepPoint3D pivot_work = pivot();
724
725 /*
726 HepVector a_work = a();
727 HepSymMatrix ea_work = Ea();
728
729 KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
730 helix_work.pivot(ip);
731
732 HepVector a_temp = helix_work.a();
733 HepSymMatrix ea_temp = helix_work.Ea();
734
735 seg.Ea_pre_bwd(ea_temp);
736 seg.a_pre_bwd(a_temp);
737
738 */
739
740 seg.a_pre_bwd( a() );
741 seg.Ea_pre_bwd( Ea() );
742
743 double dchi2R( 99999999 ), dchi2L( 99999999 );
744 HepVector v_H( 5, 0 );
745 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
746 v_H[3] = -meas.z();
747 HepMatrix v_HT = v_H.T();
748
749 double estim = ( v_HT * v_a )[0];
750 HepVector ea_v_H = ea * v_H;
751 HepMatrix ea_v_HT = ( ea_v_H ).T();
752 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
753 HepSymMatrix eaNew( 5 );
754 HepVector aNew( 5 );
755 double time = ( *HitMdc ).tdc();
756
757 // seg.dt(time);
758 // if (Tof_correc_)
759 // {
760 // time -= tofest;
761 // seg.dt(time);
762 // }
763 // double dd = 1.0e-4 * 40.0 * time;
764 // seg.dd(dd);
765
766 double drifttime = getDriftTime( *HitMdc, tofest );
767 seg.dt( drifttime );
768 double ddl = getDriftDist( *HitMdc, drifttime, 0 );
769 double ddr = getDriftDist( *HitMdc, drifttime, 1 );
770 double erddl = getSigma( *HitMdc, a()[4], 0, ddl );
771 double erddr = getSigma( *HitMdc, a()[4], 1, ddr );
772
773 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr }; // millimeter to centimeter
774 double er_dmeas2[2] = { 0., 0. };
775 if ( resolflag_ == 1 )
776 {
777 er_dmeas2[0] = 0.1 * erddl;
778 er_dmeas2[1] = 0.1 * erddr;
779 }
780 else if ( resolflag_ == 0 ) {}
781
782 // Left hypothesis :
783 if ( lr == -1 )
784 {
785 double er_dmeasL, dmeasL;
786 if ( Tof_correc_ )
787 {
788 dmeasL = ( -1.0 ) * dmeas2[0];
789 er_dmeasL = er_dmeas2[0];
790 }
791 else
792 {
793 dmeasL = ( -1.0 ) * fabs( ( *HitMdc ).dist()[0] );
794 er_dmeasL = ( *HitMdc ).erdist()[0];
795 }
796
797 // if(layerid < 4) er_dmeasL*=2.0;
798
799 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
800 eaNew.assign( ea - ea_v_H * AL * ea_v_HT );
801 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
802 if ( 0. == RkL ) RkL = 1.e-4;
803
804 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
805 aNew = v_a + diffL;
806 double sigL = ( dmeasL - ( v_HT * aNew )[0] );
807 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
808 }
809 else if ( lr == 1 )
810 {
811 // Right hypothesis :
812
813 double er_dmeasR, dmeasR;
814 if ( Tof_correc_ )
815 {
816 dmeasR = dmeas2[1];
817 er_dmeasR = er_dmeas2[1];
818 }
819 else
820 {
821 dmeasR = fabs( ( *HitMdc ).dist()[1] );
822 er_dmeasR = ( *HitMdc ).erdist()[1];
823 }
824
825 // if(layerid < 4) er_dmeasR*=2.0;
826
827 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
828 eaNew.assign( ea - ea_v_H * AR * ea_v_HT );
829 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
830 if ( 0. == RkR ) RkR = 1.e-4;
831
832 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
833 aNew = v_a + diffR;
834 double sigR = ( dmeasR - ( v_HT * aNew )[0] );
835 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
836 }
837
838 // Update Kalman result :
839#ifdef YDEBUG
840 cout << "in smoother_Mdc: lr= " << lr << " a: " << v_a << " a_NEW: " << aNew << endl;
841#endif
842 double dchi2_loc( 0 );
843 if ( ( dchi2R < dchi2cuts_calib[layerid] && lr == 1 ) ||
844 ( dchi2L < dchi2cuts_calib[layerid] && lr == -1 ) )
845 {
846
847 ndf_back_++;
848 flg = 1;
849 int chge( 1 );
850 if ( !( aNew[2] && fabs( aNew[2] - a()[2] ) < 1.0 ) ) chge = 0;
851 if ( lr == 1 )
852 {
853 chiSq_back_ += dchi2R;
854 dchi2_loc = dchi2R;
855 dd = 0.1 * ddr;
856 // if(debug_ ==4) std::cout<<"in smoother "<<std::endl;
857 }
858 else if ( lr == -1 )
859 {
860 chiSq_back_ += dchi2L;
861 dchi2_loc = dchi2L;
862 dd = -0.1 * ddl;
863 }
864 if ( chge )
865 {
866 a( aNew );
867 Ea( eaNew );
868 }
869
870 seg.a_filt_bwd( aNew );
871 seg.Ea_filt_bwd( eaNew );
872
873 HepVector a_pre_fwd_temp = seg.a_pre_fwd();
874 if ( ( a_pre_fwd_temp[1] - seg.a_pre_bwd()[1] ) > 3. * M_PI / 2. )
875 a_pre_fwd_temp[1] -= M_PI2;
876 if ( ( a_pre_fwd_temp[1] - seg.a_pre_bwd()[1] ) < -3. * M_PI / 2. )
877 a_pre_fwd_temp[1] += M_PI2;
878
879 seg.a_pre_fwd( a_pre_fwd_temp );
880
881 HepVector a_pre_filt_temp = seg.a_filt_fwd();
882 if ( ( a_pre_filt_temp[1] - seg.a_pre_bwd()[1] ) > 3. * M_PI / 2. )
883 a_pre_filt_temp[1] -= M_PI2;
884 if ( ( a_pre_filt_temp[1] - seg.a_pre_bwd()[1] ) < -3. * M_PI / 2. )
885 a_pre_filt_temp[1] += M_PI2;
886 seg.a_filt_fwd( a_pre_filt_temp );
887
888 /*
889 KalFitTrack helixNew(pivot_work, aNew, eaNew, 0, 0, 0);
890 helixNew.pivot(ip);
891 a_temp = helixNew.a();
892 ea_temp = helixNew.Ea();
893 seg.Ea_filt_bwd(ea_temp);
894 seg.a_filt_bwd(a_temp);
895 */
896
897 int inv;
898
899 if ( debug_ == 4 )
900 {
901 std::cout << "seg.Ea_pre_bwd().inverse(inv) ..." << seg.Ea_pre_bwd().inverse( inv )
902 << std::endl;
903 std::cout << "seg.Ea_pre_fwd().inverse(inv) ..." << seg.Ea_pre_fwd().inverse( inv )
904 << std::endl;
905 }
906
907 HepSymMatrix Ea_pre_comb =
908 ( seg.Ea_pre_bwd().inverse( inv ) + seg.Ea_pre_fwd().inverse( inv ) ).inverse( inv );
909 seg.Ea_exclude( Ea_pre_comb );
910
911 if ( debug_ == 4 )
912 {
913 std::cout << "Ea_pre_comb_ ... " << Ea_pre_comb << std::endl;
914 std::cout << "seg.a_pre_bwd()..." << seg.a_pre_bwd() << std::endl;
915 std::cout << "seg.a_pre_fwd()..." << seg.a_pre_fwd() << std::endl;
916 }
917 HepVector a_pre_comb =
918 Ea_pre_comb * ( ( seg.Ea_pre_bwd().inverse( inv ) ) * seg.a_pre_bwd() +
919 ( seg.Ea_pre_fwd().inverse( inv ) ) * seg.a_pre_fwd() );
920 seg.a_exclude( a_pre_comb );
921
922 if ( debug_ == 4 )
923 {
924 std::cout << "seg.Ea_filt_fwd()..." << seg.Ea_filt_fwd() << std::endl;
925 std::cout << "seg.Ea_filt_fwd().inverse(inv)..." << seg.Ea_filt_fwd().inverse( inv )
926 << std::endl;
927 }
928 seg.Ea( ( seg.Ea_filt_fwd().inverse( inv ) + seg.Ea_pre_bwd().inverse( inv ) )
929 .inverse( inv ) );
930 seg.Ea_include( ( seg.Ea_filt_fwd().inverse( inv ) + seg.Ea_pre_bwd().inverse( inv ) )
931 .inverse( inv ) );
932
933 if ( debug_ == 4 )
934 {
935 std::cout << "seg.Ea() is ..." << seg.Ea() << std::endl;
936 std::cout << "seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd() ..."
937 << seg.Ea_filt_fwd().inverse( inv ) * seg.a_filt_fwd() << std::endl;
938 std::cout << "seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd() ... "
939 << seg.Ea_pre_bwd().inverse( inv ) * seg.a_pre_bwd() << std::endl;
940 }
941
942 seg.a( seg.Ea() * ( seg.Ea_filt_fwd().inverse( inv ) * seg.a_filt_fwd() +
943 seg.Ea_pre_bwd().inverse( inv ) * seg.a_pre_bwd() ) );
944 seg.a_include( seg.Ea() * ( seg.Ea_filt_fwd().inverse( inv ) * seg.a_filt_fwd() +
945 seg.Ea_pre_bwd().inverse( inv ) * seg.a_pre_bwd() ) );
946
947 if ( inv != 0 )
948 {
949 // std::cout<<"ERROR OCCUR WHEN INVERSE MATRIX !"<<std::endl;
950 }
951
952 seg.residual_exclude( dd - ( v_HT * a_pre_comb )[0] );
953 seg.residual_include( dd - ( v_HT * seg.a() )[0] );
954 seg.doca_exclude( fabs( ( v_HT * a_pre_comb )[0] ) );
955 seg.doca_include( fabs( ( v_HT * seg.a() )[0] ) );
956
957 if ( debug_ == 4 )
958 {
959 std::cout << "the dd in smoother_Mdc is " << dd << " the (v_HT*a_pre_comb)[0] is "
960 << ( v_HT * a_pre_comb )[0] << std::endl;
961 }
962
963 // compare the two method to calculate the include doca value :
964 if ( debug_ == 4 )
965 {
966 std::cout << "method 1 ..." << sqrt( seg.a()[0] * seg.a()[0] + seg.a()[3] * seg.a()[3] )
967 << std::endl;
968 std::cout << "method 2 ..." << fabs( ( v_HT * seg.a() )[0] ) << std::endl;
969 }
970
971 /// move the pivot of the helixseg to IP(0,0,0)
972 KalFitTrack helixNew1( pivot_work, seg.a(), seg.Ea(), 0, 0, 0 );
973 helixNew1.pivot( ip );
974 HepVector a_temp1 = helixNew1.a();
975 HepSymMatrix ea_temp1 = helixNew1.Ea();
976 seg.Ea( ea_temp1 );
977 seg.a( a_temp1 );
978 seg.Ea_include( ea_temp1 );
979 seg.a_include( a_temp1 );
980
981 KalFitTrack helixNew2( pivot_work, seg.a_exclude(), seg.Ea_exclude(), 0, 0, 0 );
982 helixNew2.pivot( ip );
983 HepVector a_temp2 = helixNew2.a();
984 HepSymMatrix ea_temp2 = helixNew2.Ea();
985 seg.Ea_exclude( ea_temp2 );
986 seg.a_exclude( a_temp2 );
987
988 seg.tof( tofest );
989 seg.dd( dd );
990 }
991 return chiSq_back_;
992}
993
994// Smoothing procedure in Mdc calib
995// RMK attention for the case chi2R = chi2L during forward filter !
996double KalFitTrack::smoother_Mdc( KalFitHelixSeg& seg, Hep3Vector& meas, int& flg,
997 int csmflag ) {
998
999 HepPoint3D ip( 0., 0., 0. );
1000 // attention! we should not to decide the left&right in the smoother process,
1001 // because we choose the left&right of hits from the filter process.
1002
1003 KalFitHitMdc* HitMdc = seg.HitMdc();
1004 double lr = HitMdc->LR();
1005
1006 // For taking the propagation delay into account :
1007 int layerid = ( *HitMdc ).wire().layer().layerId();
1008 int wire_ID = HitMdc->wire().geoID();
1009 double entrangle = HitMdc->rechitptr()->getEntra();
1010
1011 if ( wire_ID < 0 || wire_ID > 6796 )
1012 { // bes
1013 std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
1014 return 0;
1015 }
1016
1017 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
1018 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
1019 double dd( 0. );
1020 double tofest( 0 );
1021 double phi = fmod( phi0() + M_PI4, M_PI2 );
1022 double csf0 = cos( phi );
1023 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
1024 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
1025 if ( phi > M_PI ) snf0 = -snf0;
1026
1027 if ( Tof_correc_ )
1028 {
1029 Hep3Vector ip( 0, 0, 0 );
1030 Helix work = *(Helix*)this;
1031 work.ignoreErrorMatrix();
1032 work.pivot( ip );
1033 double phi_ip = work.phi0();
1034 if ( fabs( phi - phi_ip ) > M_PI )
1035 {
1036 if ( phi > phi_ip ) phi -= 2 * M_PI;
1037 else phi_ip -= 2 * M_PI;
1038 }
1039 double t = tanl();
1040 double l = fabs( radius() * ( phi - phi_ip ) * sqrt( 1 + t * t ) );
1041 double pmag( sqrt( 1.0 + t * t ) / kappa() );
1042 double mass_over_p( mass_ / pmag );
1043 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
1044 tofest = l / ( 29.9792458 * beta );
1045 if ( csmflag == 1 && ( *HitMdc ).wire().y() > 0. ) tofest = -1. * tofest;
1046 }
1047
1048 const HepSymMatrix& ea = Ea();
1049 const HepVector& v_a = a();
1050
1051 HepPoint3D pivot_work = pivot();
1052
1053 /*
1054 HepVector a_work = a();
1055 HepSymMatrix ea_work = Ea();
1056
1057 KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
1058 helix_work.pivot(ip);
1059
1060 HepVector a_temp = helix_work.a();
1061 HepSymMatrix ea_temp = helix_work.Ea();
1062
1063 seg.Ea_pre_bwd(ea_temp);
1064 seg.a_pre_bwd(a_temp);
1065
1066 */
1067
1068 seg.a_pre_bwd( a() );
1069 seg.Ea_pre_bwd( Ea() );
1070
1071 double dchi2R( 99999999 ), dchi2L( 99999999 );
1072 HepVector v_H( 5, 0 );
1073 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
1074 v_H[3] = -meas.z();
1075 HepMatrix v_HT = v_H.T();
1076
1077 double estim = ( v_HT * v_a )[0];
1078 HepVector ea_v_H = ea * v_H;
1079 HepMatrix ea_v_HT = ( ea_v_H ).T();
1080 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
1081 HepSymMatrix eaNew( 5 );
1082 HepVector aNew( 5 );
1083 double time = ( *HitMdc ).tdc();
1084
1085 // seg.dt(time);
1086 // if (Tof_correc_)
1087 // {
1088 // time -= tofest;
1089 // seg.dt(time);
1090 // }
1091 // double dd = 1.0e-4 * 40.0 * time;
1092 // seg.dd(dd);
1093
1094 double drifttime = getDriftTime( *HitMdc, tofest );
1095 seg.dt( drifttime );
1096 double ddl = getDriftDist( *HitMdc, drifttime, 0 );
1097 double ddr = getDriftDist( *HitMdc, drifttime, 1 );
1098 double erddl = getSigma( *HitMdc, a()[4], 0, ddl );
1099 double erddr = getSigma( *HitMdc, a()[4], 1, ddr );
1100
1101 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr }; // millimeter to centimeter
1102 double er_dmeas2[2] = { 0., 0. };
1103 if ( resolflag_ == 1 )
1104 {
1105 er_dmeas2[0] = 0.1 * erddl;
1106 er_dmeas2[1] = 0.1 * erddr;
1107 }
1108 else if ( resolflag_ == 0 ) {}
1109
1110 // Left hypothesis :
1111 if ( lr == -1 )
1112 {
1113 double er_dmeasL, dmeasL;
1114 if ( Tof_correc_ )
1115 {
1116 dmeasL = ( -1.0 ) * dmeas2[0];
1117 er_dmeasL = er_dmeas2[0];
1118 }
1119 else
1120 {
1121 dmeasL = ( -1.0 ) * fabs( ( *HitMdc ).dist()[0] );
1122 er_dmeasL = ( *HitMdc ).erdist()[0];
1123 }
1124
1125 // if(layerid < 4) er_dmeasL*=2.0;
1126
1127 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
1128 eaNew.assign( ea - ea_v_H * AL * ea_v_HT );
1129 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
1130 if ( 0. == RkL ) RkL = 1.e-4;
1131
1132 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
1133 aNew = v_a + diffL;
1134 double sigL = ( dmeasL - ( v_HT * aNew )[0] );
1135 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
1136 }
1137 else if ( lr == 1 )
1138 {
1139 // Right hypothesis :
1140
1141 double er_dmeasR, dmeasR;
1142 if ( Tof_correc_ )
1143 {
1144 dmeasR = dmeas2[1];
1145 er_dmeasR = er_dmeas2[1];
1146 }
1147 else
1148 {
1149 dmeasR = fabs( ( *HitMdc ).dist()[1] );
1150 er_dmeasR = ( *HitMdc ).erdist()[1];
1151 }
1152
1153 // if(layerid < 4) er_dmeasR*=2.0;
1154
1155 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
1156 eaNew.assign( ea - ea_v_H * AR * ea_v_HT );
1157 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
1158 if ( 0. == RkR ) RkR = 1.e-4;
1159
1160 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
1161 aNew = v_a + diffR;
1162 double sigR = ( dmeasR - ( v_HT * aNew )[0] );
1163 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
1164 }
1165
1166 // Update Kalman result :
1167#ifdef YDEBUG
1168 cout << "in smoother_Mdc: lr= " << lr << " a: " << v_a << " a_NEW: " << aNew << endl;
1169#endif
1170 double dchi2_loc( 0 );
1171 if ( ( dchi2R < dchi2cuts_calib[layerid] && lr == 1 ) ||
1172 ( dchi2L < dchi2cuts_calib[layerid] && lr == -1 ) )
1173 {
1174
1175 ndf_back_++;
1176 flg = 1;
1177 int chge( 1 );
1178 if ( !( aNew[2] && fabs( aNew[2] - a()[2] ) < 1.0 ) ) chge = 0;
1179 if ( lr == 1 )
1180 {
1181 chiSq_back_ += dchi2R;
1182 dchi2_loc = dchi2R;
1183 dd = 0.1 * ddr;
1184 // if(debug_ ==4) std::cout<<"in smoother "<<std::endl;
1185 }
1186 else if ( lr == -1 )
1187 {
1188 chiSq_back_ += dchi2L;
1189 dchi2_loc = dchi2L;
1190 dd = -0.1 * ddl;
1191 }
1192 if ( chge )
1193 {
1194 a( aNew );
1195 Ea( eaNew );
1196 }
1197
1198 seg.a_filt_bwd( aNew );
1199 seg.Ea_filt_bwd( eaNew );
1200
1201 HepVector a_pre_fwd_temp = seg.a_pre_fwd();
1202 if ( ( a_pre_fwd_temp[1] - seg.a_pre_bwd()[1] ) > 3. * M_PI / 2. )
1203 a_pre_fwd_temp[1] -= M_PI2;
1204 if ( ( a_pre_fwd_temp[1] - seg.a_pre_bwd()[1] ) < -3. * M_PI / 2. )
1205 a_pre_fwd_temp[1] += M_PI2;
1206
1207 seg.a_pre_fwd( a_pre_fwd_temp );
1208
1209 HepVector a_pre_filt_temp = seg.a_filt_fwd();
1210 if ( ( a_pre_filt_temp[1] - seg.a_pre_bwd()[1] ) > 3. * M_PI / 2. )
1211 a_pre_filt_temp[1] -= M_PI2;
1212 if ( ( a_pre_filt_temp[1] - seg.a_pre_bwd()[1] ) < -3. * M_PI / 2. )
1213 a_pre_filt_temp[1] += M_PI2;
1214 seg.a_filt_fwd( a_pre_filt_temp );
1215
1216 /*
1217 KalFitTrack helixNew(pivot_work, aNew, eaNew, 0, 0, 0);
1218 helixNew.pivot(ip);
1219 a_temp = helixNew.a();
1220 ea_temp = helixNew.Ea();
1221 seg.Ea_filt_bwd(ea_temp);
1222 seg.a_filt_bwd(a_temp);
1223 */
1224
1225 int inv;
1226
1227 if ( debug_ == 4 )
1228 {
1229 std::cout << "seg.Ea_pre_bwd().inverse(inv) ..." << seg.Ea_pre_bwd().inverse( inv )
1230 << std::endl;
1231 std::cout << "seg.Ea_pre_fwd().inverse(inv) ..." << seg.Ea_pre_fwd().inverse( inv )
1232 << std::endl;
1233 }
1234
1235 HepSymMatrix Ea_pre_comb =
1236 ( seg.Ea_pre_bwd().inverse( inv ) + seg.Ea_pre_fwd().inverse( inv ) ).inverse( inv );
1237 seg.Ea_exclude( Ea_pre_comb );
1238
1239 if ( debug_ == 4 )
1240 {
1241 std::cout << "Ea_pre_comb_ ... " << Ea_pre_comb << std::endl;
1242 std::cout << "seg.a_pre_bwd()..." << seg.a_pre_bwd() << std::endl;
1243 std::cout << "seg.a_pre_fwd()..." << seg.a_pre_fwd() << std::endl;
1244 }
1245 HepVector a_pre_comb =
1246 Ea_pre_comb * ( ( seg.Ea_pre_bwd().inverse( inv ) ) * seg.a_pre_bwd() +
1247 ( seg.Ea_pre_fwd().inverse( inv ) ) * seg.a_pre_fwd() );
1248 seg.a_exclude( a_pre_comb );
1249
1250 if ( debug_ == 4 )
1251 {
1252 std::cout << "seg.Ea_filt_fwd()..." << seg.Ea_filt_fwd() << std::endl;
1253 std::cout << "seg.Ea_filt_fwd().inverse(inv)..." << seg.Ea_filt_fwd().inverse( inv )
1254 << std::endl;
1255 }
1256 seg.Ea( ( seg.Ea_filt_fwd().inverse( inv ) + seg.Ea_pre_bwd().inverse( inv ) )
1257 .inverse( inv ) );
1258 seg.Ea_include( ( seg.Ea_filt_fwd().inverse( inv ) + seg.Ea_pre_bwd().inverse( inv ) )
1259 .inverse( inv ) );
1260
1261 if ( debug_ == 4 )
1262 {
1263 std::cout << "seg.Ea() is ..." << seg.Ea() << std::endl;
1264 std::cout << "seg.Ea_filt_fwd().inverse(inv)*seg.a_filt_fwd() ..."
1265 << seg.Ea_filt_fwd().inverse( inv ) * seg.a_filt_fwd() << std::endl;
1266 std::cout << "seg.Ea_pre_bwd().inverse(inv)*seg.a_pre_bwd() ... "
1267 << seg.Ea_pre_bwd().inverse( inv ) * seg.a_pre_bwd() << std::endl;
1268 }
1269
1270 seg.a( seg.Ea() * ( seg.Ea_filt_fwd().inverse( inv ) * seg.a_filt_fwd() +
1271 seg.Ea_pre_bwd().inverse( inv ) * seg.a_pre_bwd() ) );
1272 seg.a_include( seg.Ea() * ( seg.Ea_filt_fwd().inverse( inv ) * seg.a_filt_fwd() +
1273 seg.Ea_pre_bwd().inverse( inv ) * seg.a_pre_bwd() ) );
1274
1275 if ( inv != 0 )
1276 {
1277 // std::cout<<"ERROR OCCUR WHEN INVERSE MATRIX !"<<std::endl;
1278 }
1279
1280 seg.residual_exclude( dd - ( v_HT * a_pre_comb )[0] );
1281 seg.residual_include( dd - ( v_HT * seg.a() )[0] );
1282 seg.doca_exclude( fabs( ( v_HT * a_pre_comb )[0] ) );
1283 seg.doca_include( fabs( ( v_HT * seg.a() )[0] ) );
1284
1285 if ( debug_ == 4 )
1286 {
1287 std::cout << "the dd in smoother_Mdc is " << dd << " the (v_HT*a_pre_comb)[0] is "
1288 << ( v_HT * a_pre_comb )[0] << std::endl;
1289 }
1290
1291 // compare the two method to calculate the include doca value :
1292 if ( debug_ == 4 )
1293 {
1294 std::cout << "method 1 ..." << sqrt( seg.a()[0] * seg.a()[0] + seg.a()[3] * seg.a()[3] )
1295 << std::endl;
1296 std::cout << "method 2 ..." << fabs( ( v_HT * seg.a() )[0] ) << std::endl;
1297 }
1298
1299 /// move the pivot of the helixseg to IP(0,0,0)
1300 KalFitTrack helixNew1( pivot_work, seg.a(), seg.Ea(), 0, 0, 0 );
1301 helixNew1.pivot( ip );
1302 HepVector a_temp1 = helixNew1.a();
1303 HepSymMatrix ea_temp1 = helixNew1.Ea();
1304 seg.Ea( ea_temp1 );
1305 seg.a( a_temp1 );
1306 seg.Ea_include( ea_temp1 );
1307 seg.a_include( a_temp1 );
1308
1309 KalFitTrack helixNew2( pivot_work, seg.a_exclude(), seg.Ea_exclude(), 0, 0, 0 );
1310 helixNew2.pivot( ip );
1311 HepVector a_temp2 = helixNew2.a();
1312 HepSymMatrix ea_temp2 = helixNew2.Ea();
1313 seg.Ea_exclude( ea_temp2 );
1314 seg.a_exclude( a_temp2 );
1315
1316 seg.tof( tofest );
1317 seg.dd( dd );
1318 }
1319 return chiSq_back_;
1320}
1321
1322double KalFitTrack::filter( double v_m, const HepVector& m_H, double v_d, double m_V ) {
1323 HepMatrix m_HT = m_H.T();
1324 HepPoint3D x0 = x( 0 );
1325 HepVector v_x( 3 );
1326 v_x[0] = x0.x();
1327 v_x[1] = x0.y();
1328 v_x[2] = x0.z();
1329 HepMatrix m_X = delXDelA( 0 );
1330 HepMatrix m_XT = m_X.T();
1331 HepMatrix m_C = m_X * Ea() * m_XT;
1332 // int err;
1333 HepVector m_K = 1 / ( m_V + ( m_HT * m_C * m_H )[0] ) * m_H;
1334 HepVector v_a = a();
1335 HepSymMatrix ea = Ea();
1336 HepMatrix mXTmK = m_XT * m_K;
1337 double v_r = v_m - v_d - ( m_HT * v_x )[0];
1338 v_a += ea * mXTmK * v_r;
1339 a( v_a );
1340 ea.assign( ea - ea * mXTmK * m_HT * m_X * ea );
1341 Ea( ea );
1342 // Record last hit included parameters :
1343 update_last();
1344 HepMatrix mCmK = m_C * m_K;
1345 v_x += mCmK * v_r;
1346 m_C -= mCmK * m_HT * m_C;
1347 v_r = v_m - v_d - ( m_HT * v_x )[0];
1348 double m_R = m_V - ( m_HT * m_C * m_H )[0];
1349 double chiSq = v_r * v_r / m_R;
1350 chiSq_ += chiSq;
1351 return chiSq;
1352}
1353
1354void KalFitTrack::path_add( double path ) {
1355 pathip_ += path;
1356 tof( path );
1357}
1358
1359void KalFitTrack::addPathSM( double path ) { pathSM_ += path; }
1360
1361void KalFitTrack::addTofSM( double time ) { tof2_ += time; }
1362
1363void KalFitTrack::fiTerm( double fi ) { fiTerm_ = fi; }
1364
1365void KalFitTrack::tof( double path ) {
1366 double light_speed( 29.9792458 ); // light speed in cm/nsec
1367 double t = tanl();
1368 double pmag( sqrt( 1.0 + t * t ) / kappa() );
1369 if ( pmag != 0 )
1370 {
1371 double mass_over_p( mass_ / pmag );
1372 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
1373 tof_ += path / ( light_speed * beta );
1374 }
1375
1376 if ( tofall_ )
1377 {
1378 if ( p_kaon_ > 0 )
1379 {
1380 double massk_over_p( MASS[3] / p_kaon_ );
1381 double beta_kaon( 1.0 / sqrt( 1.0 + massk_over_p * massk_over_p ) );
1382 tof_kaon_ += path / ( light_speed * beta_kaon );
1383 }
1384 if ( p_proton_ > 0 )
1385 {
1386 double massp_over_p( MASS[4] / p_proton_ );
1387 double beta_proton( 1.0 / sqrt( 1.0 + massp_over_p * massp_over_p ) );
1388 tof_proton_ += path / ( light_speed * beta_proton );
1389 }
1390 }
1391}
1392
1393double KalFitTrack::radius_numf( void ) const {
1394
1395 double Bz( Bznom_ );
1396 // std::cout<<"Bz: "<<Bz<<std::endl;
1397 if ( numf_ > 10 )
1398 {
1399 double dr = a()[0];
1400 double phi0 = a()[1];
1401 double dz = a()[3];
1402 double phi = fmod( phi0 + M_PI4, M_PI2 );
1403 double csf0 = cos( phi );
1404 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
1405 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
1406 if ( phi > M_PI ) snf0 = -snf0;
1407 // XYZPoint
1408 HepPoint3D x0( ( pivot().x() + dr * csf0 ), ( pivot().y() + dr * snf0 ),
1409 ( pivot().z() + dz ) );
1410
1411 // XYZVector b;
1412 HepVector3D b;
1413
1414 // std::cout<<"b: "<<b<<std::endl;
1415
1416 MFSvc_->fieldVector( 10. * x0, b );
1417
1418 // std::cout<<"b: "<<b<<std::endl;
1419
1420 b = 10000. * b;
1421 Bz = b.z();
1422 }
1423 if ( Bz == 0 ) Bz = Bznom_;
1424 double ALPHA_loc = 10000. / 2.99792458 / Bz;
1425 return ALPHA_loc / a()[2];
1426}
1427
1428const HepPoint3D& KalFitTrack::pivot_numf( const HepPoint3D& newPivot ) {
1429
1430 int nstep( 1 );
1431 HepPoint3D delta_x( ( newPivot - pivot() ).x() / double( inner_steps_ ),
1432 ( newPivot - pivot() ).y() / double( inner_steps_ ),
1433 ( newPivot - pivot() ).z() / double( inner_steps_ ) );
1434 int i = 1;
1435
1436 while ( i <= inner_steps_ )
1437 {
1438 HepPoint3D nextPivot( pivot() + delta_x );
1439 double xnp( nextPivot.x() ), ynp( nextPivot.y() ), znp( nextPivot.z() );
1440 HepSymMatrix Ea_now = Ea();
1441 HepPoint3D piv( pivot() );
1442 double xp( piv.x() ), yp( piv.y() ), zp( piv.z() );
1443 double dr = a()[0];
1444 double phi0 = a()[1];
1445 double kappa = a()[2];
1446 double dz = a()[3];
1447 double tanl = a()[4];
1448 double m_rad( 0 );
1449 if ( numfcor_ == 1 ) m_rad = radius_numf();
1450 else m_rad = radius();
1451 double rdr = dr + m_rad;
1452 double phi = fmod( phi0 + M_PI4, M_PI2 );
1453 double csf0 = cos( phi );
1454 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
1455 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
1456 if ( phi > M_PI ) snf0 = -snf0;
1457
1458 double xc = xp + rdr * csf0;
1459 double yc = yp + rdr * snf0;
1460 double csf = ( xc - xnp ) / m_rad;
1461 double snf = ( yc - ynp ) / m_rad;
1462 double anrm = sqrt( csf * csf + snf * snf );
1463 csf /= anrm;
1464 snf /= anrm;
1465 phi = atan2( snf, csf );
1466 double phid = fmod( phi - phi0 + M_PI8, M_PI2 );
1467 if ( phid > M_PI ) phid = phid - M_PI2;
1468 double drp = ( xp + dr * csf0 + m_rad * ( csf0 - csf ) - xnp ) * csf +
1469 ( yp + dr * snf0 + m_rad * ( snf0 - snf ) - ynp ) * snf;
1470 double dzp = zp + dz - m_rad * tanl * phid - znp;
1471
1472 HepVector ap( 5 );
1473 ap[0] = drp;
1474 ap[1] = fmod( phi + M_PI4, M_PI2 );
1475 ap[2] = kappa;
1476 ap[3] = dzp;
1477 ap[4] = tanl;
1478
1479 // Modification due to non uniform magnetic field :
1480 if ( numf_ > 10 )
1481 {
1482
1483 Hep3Vector x1( xp + dr * csf0, yp + dr * snf0, zp + dz );
1484 double csf0p = cos( ap[1] );
1485 double snf0p = ( 1. - csf0p ) * ( 1. + csf0p );
1486 snf0p = sqrt( ( snf0p > 0. ) ? snf0p : 0. );
1487 if ( ap[1] > M_PI ) snf0p = -snf0p;
1488
1489 Hep3Vector x2( xnp + drp * csf0p, ynp + drp * snf0p, znp + dzp );
1490 Hep3Vector dist( ( x1 - x2 ).x() / 100.0, ( x1 - x2 ).y() / 100.0,
1491 ( x1 - x2 ).z() / 100.0 );
1492 HepPoint3D middlebis( ( x1.x() + x2.x() ) / 2, ( x1.y() + x2.y() ) / 2,
1493 ( x1.z() + x2.z() ) / 2 );
1494 HepVector3D field;
1495 MFSvc_->fieldVector( 10. * middlebis, field );
1496 field = 1000. * field;
1497 Hep3Vector dB( field.x(), field.y(), ( field.z() - 0.1 * Bznom_ ) );
1498 if ( field.z() )
1499 {
1500 double akappa( fabs( kappa ) );
1501 double sign = kappa / akappa;
1502 HepVector dp( 3 );
1503 dp = 0.299792458 * sign * dB.cross( dist );
1504 HepVector dhp( 3 );
1505 dhp[0] = -akappa * ( dp[0] * csf0p + dp[1] * snf0p );
1506 dhp[1] = kappa * akappa * ( dp[0] * snf0p - dp[1] * csf0p );
1507 dhp[2] = dp[2] * akappa + dhp[1] * tanl / kappa;
1508 if ( numfcor_ == 0 ) { ap[1] += dhp[0]; }
1509 ap[2] += dhp[1];
1510 ap[4] += dhp[2];
1511 }
1512 }
1513 HepMatrix m_del = delApDelA( ap );
1514 Ea_now.assign( m_del * Ea_now * m_del.T() );
1515 pivot( nextPivot );
1516 a( ap );
1517 Ea( Ea_now );
1518 i++;
1519 }
1520 return newPivot;
1521}
1522
1523const HepPoint3D& KalFitTrack::pivot_numf( const HepPoint3D& newPivot, double& pathl ) {
1524
1525 Helix tracktest = *(Helix*)this;
1526 tracktest.ignoreErrorMatrix();
1527 double tl = a()[4];
1528 double th = 90.0 - 180.0 * M_1_PI * atan( tl );
1529 /*
1530 int nstep(1);
1531 if (steplev_ == 1)
1532 nstep = 3;
1533 else if (steplev_ == 2 && (th > 140 || th <45))
1534 if ((pivot()-newPivot).mag()<3.)
1535 nstep = 3;
1536 else
1537 nstep = 6;
1538 */
1539 Hep3Vector delta_x( ( newPivot - pivot() ).x() / double( outer_steps_ ),
1540 ( newPivot - pivot() ).y() / double( outer_steps_ ),
1541 ( newPivot - pivot() ).z() / double( outer_steps_ ) );
1542 int i = 1;
1543
1544 while ( i <= outer_steps_ )
1545 {
1546 HepPoint3D nextPivot( pivot() + delta_x );
1547 double xnp( nextPivot.x() ), ynp( nextPivot.y() ), znp( nextPivot.z() );
1548
1549 HepSymMatrix Ea_now = Ea();
1550 HepPoint3D piv( pivot() );
1551 double xp( piv.x() ), yp( piv.y() ), zp( piv.z() );
1552
1553 double dr = a()[0];
1554 double phi0 = a()[1];
1555 double kappa = a()[2];
1556 double dz = a()[3];
1557 double tanl = a()[4];
1558
1559 double m_rad( 0 );
1560 m_rad = radius();
1561
1562 double rdr = dr + m_rad;
1563 double phi = fmod( phi0 + M_PI4, M_PI2 );
1564 double csf0 = cos( phi );
1565 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
1566 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
1567 if ( phi > M_PI ) snf0 = -snf0;
1568
1569 double xc = xp + rdr * csf0;
1570 double yc = yp + rdr * snf0;
1571 double csf = ( xc - xnp ) / m_rad;
1572 double snf = ( yc - ynp ) / m_rad;
1573 double anrm = sqrt( csf * csf + snf * snf );
1574 csf /= anrm;
1575 snf /= anrm;
1576 phi = atan2( snf, csf );
1577 double phid = fmod( phi - phi0 + M_PI8, M_PI2 );
1578 if ( phid > M_PI ) phid = phid - M_PI2;
1579 double drp = ( xp + dr * csf0 + m_rad * ( csf0 - csf ) - xnp ) * csf +
1580 ( yp + dr * snf0 + m_rad * ( snf0 - snf ) - ynp ) * snf;
1581 double dzp = zp + dz - m_rad * tanl * phid - znp;
1582
1583 HepVector ap( 5 );
1584 ap[0] = drp;
1585 ap[1] = fmod( phi + M_PI4, M_PI2 );
1586 ap[2] = kappa;
1587 ap[3] = dzp;
1588 ap[4] = tanl;
1589
1590 // std::cout<<" numf_: "<<numf_<<std::endl;
1591
1592 // Modification due to non uniform magnetic field :
1593 if ( numf_ > 10 )
1594 {
1595
1596 Hep3Vector x1( xp + dr * csf0, yp + dr * snf0, zp + dz );
1597
1598 double csf0p = cos( ap[1] );
1599 double snf0p = ( 1. - csf0p ) * ( 1. + csf0p );
1600 snf0p = sqrt( ( snf0p > 0. ) ? snf0p : 0. );
1601 if ( ap[1] > M_PI ) snf0p = -snf0p;
1602
1603 Hep3Vector x2( xnp + drp * csf0p, ynp + drp * snf0p, znp + dzp );
1604
1605 Hep3Vector dist( ( x1 - x2 ).x() / 100.0, ( x1 - x2 ).y() / 100.0,
1606 ( x1 - x2 ).z() / 100.0 );
1607
1608 HepPoint3D middlebis( ( x1.x() + x2.x() ) / 2, ( x1.y() + x2.y() ) / 2,
1609 ( x1.z() + x2.z() ) / 2 );
1610
1611 HepVector3D field;
1612 MFSvc_->fieldVector( 10. * middlebis, field );
1613 field = 1000. * field;
1614
1615 // std::cout<<"B field: "<<field<<std::endl;
1616 Hep3Vector dB( field.x(), field.y(), ( field.z() - 0.1 * Bznom_ ) );
1617
1618 // std::cout<<" dB: "<<dB<<std::endl;
1619
1620 if ( field.z() )
1621 {
1622 double akappa( fabs( kappa ) );
1623 double sign = kappa / akappa;
1624 HepVector dp( 3 );
1625 dp = 0.299792458 * sign * dB.cross( dist );
1626
1627 // std::cout<<"dp: "<<dp<<std::endl;
1628
1629 HepVector dhp( 3 );
1630 dhp[0] = -akappa * ( dp[0] * csf0p + dp[1] * snf0p );
1631 dhp[1] = kappa * akappa * ( dp[0] * snf0p - dp[1] * csf0p );
1632 dhp[2] = dp[2] * akappa + dhp[1] * tanl / kappa;
1633
1634 // std::cout<<"dhp: "<<dhp<<std::endl;
1635
1636 ap[1] += dhp[0];
1637 ap[2] += dhp[1];
1638 ap[4] += dhp[2];
1639 }
1640 }
1641 HepMatrix m_del = delApDelA( ap );
1642 Ea_now.assign( m_del * Ea_now * m_del.T() );
1643 pivot( nextPivot );
1644 a( ap );
1645 Ea( Ea_now );
1646 i++;
1647
1648 // std::cout<<" a: "<<a()<<std::endl;
1649 }
1650
1651 // Estimation of the path length :
1652 double tanl_0( tracktest.a()[4] );
1653 double phi0_0( tracktest.a()[1] );
1654 double radius_0( tracktest.radius() );
1655 tracktest.pivot( newPivot );
1656
1657 double phi0_1 = tracktest.a()[1];
1658 if ( fabs( phi0_1 - phi0_0 ) > M_PI )
1659 {
1660 if ( phi0_1 > phi0_0 ) phi0_1 -= 2 * M_PI;
1661 else phi0_0 -= 2 * M_PI;
1662 }
1663 if ( phi0_1 == phi0_0 ) phi0_1 = phi0_0 + 1.e-10;
1664 pathl = fabs( radius_0 * ( phi0_1 - phi0_0 ) * sqrt( 1 + tanl_0 * tanl_0 ) );
1665 return newPivot;
1666}
1667
1668// function to calculate the path length in the layer
1669/*
1670 double KalFitTrack::PathL(int layer){
1671 HepPoint3D piv(pivot());
1672 double phi0 = a()[1];
1673 double kappa = a()[2];
1674 double tanl = a()[4];
1675
1676#ifdef YDEBUG
1677cout<<"helix "<<a()<<endl;
1678#endif
1679// Choose the local field !!
1680double Bz(Bznom_);
1681if (numf_ > 10){
1682HepVector3D b;
1683MFSvc_->fieldVector(10.*piv, b);
1684b = 10000.*b;
1685Bz=b.z();
1686}
1687double ALPHA_loc = 10000./2.99792458/Bz;
1688int charge = ( kappa >= 0 )? 1 : -1;
1689double rho = ALPHA_loc/kappa;
1690double pt = fabs( 1.0/kappa );
1691double lambda = 180.0*M_1_PI*atan( tanl );
1692double theta = 90.0 - lambda;
1693theta = 2.0*M_PI*theta/360.;
1694double phi = fmod(phi0 + M_PI4, M_PI2);
1695double csf0 = cos(phi);
1696double snf0 = (1. - csf0) * (1. + csf0);
1697snf0 = sqrt((snf0 > 0.) ? snf0 : 0.);
1698if(phi > M_PI) snf0 = - snf0;
1699
1700double x_c = piv.x() + ( dr() + rho )*csf0;
1701double y_c = piv.y() + ( dr() + rho )*snf0;
1702double z_c = piv.z() + dz();
1703HepPoint3D ccenter(x_c, y_c, 0);
1704double m_c_perp(ccenter.perp());
1705Hep3Vector m_c_unit(((CLHEP::Hep3Vector)ccenter).unit());
1706#ifdef YDEBUG
1707cout<<"rho=..."<<rho<<endl;
1708cout<<"ccenter "<<ccenter<<" m_c_unit "<<m_c_unit<<endl;
1709#endif
1710//phi resion estimation
1711double phi_io[2];
1712HepPoint3D IO = charge*m_c_unit;
1713double dphi0 = fmod( IO.phi()+4*M_PI,2*M_PI ) - phi;
1714if( dphi0 > M_PI ) dphi0 -= 2*M_PI;
1715else if( dphi0 < -M_PI ) dphi0 += 2*M_PI;
1716phi_io[0] = -(1+charge)*M_PI_2 - charge*dphi0;
1717phi_io[1] = phi_io[0]+1.5*M_PI;
1718double phi_in(0); /// for debug
1719
1720double m_crio[2];
1721double m_zb, m_zf;
1722int m_div;
1723
1724// retrieve Mdc geometry information
1725// IMdcGeomSvc* geosvc;
1726StatusCode sc= Gaudi::svcLocator()->service("MdcGeomSvc", geosvc);
1727if(sc==StatusCode::FAILURE) cout << "GeoSvc failing!!!!!!!SC=" << sc << endl;
1728
1729MdcGeomSvc* geomsvc = dynamic_cast<MdcGeomSvc*>(iGeomSvc_);
1730if(!geomsvc){
1731std::cout<<"ERROR OCCUR when dynamic_cast in KalFitTrack.cxx !!"<<std::endl;
1732}
1733double rcsiz1 = geomsvc->Layer(layer)->RCSiz1();
1734double rcsiz2 = geomsvc->Layer(layer)->RCSiz2();
1735double rlay = geomsvc->Layer(layer)->Radius();
1736m_zb = 0.5*(geomsvc->Layer(layer)->Length());
1737m_zf = -0.5*(geomsvc->Layer(layer)->Length());
1738
1739m_crio[0] = rlay - rcsiz1;
1740m_crio[1] = rlay + rcsiz2;
1741//conversion of the units(mm=>cm)
1742m_crio[0] = 0.1*m_crio[0];
1743m_crio[1] = 0.1*m_crio[1];
1744m_zb = 0.1*m_zb;
1745m_zf = 0.1*m_zf;
1746
1747int sign = 1; ///assumed the first half circle
1748int epflag[2];
1749Hep3Vector iocand;
1750Hep3Vector cell_IO[2];
1751double dphi;
1752for( int i = 0; i < 2; i++ ){
1753 double cos_alpha = m_c_perp*m_c_perp + m_crio[i]*m_crio[i] - rho*rho;
1754 cos_alpha = 0.5*cos_alpha/( m_c_perp*m_crio[i] );
1755 double Calpha = acos( cos_alpha );
1756 epflag[i] = 0;
1757 iocand = m_c_unit;
1758 iocand.rotateZ( charge*sign*Calpha );
1759 iocand *= m_crio[i];
1760 double xx = iocand.x() - x_c;
1761 double yy = iocand.y() - y_c;
1762 dphi = atan2(yy, xx) - phi0 - M_PI_2*(1+charge);
1763 dphi = fmod( dphi + 8.0*M_PI, 2*M_PI );
1764
1765 if( dphi < phi_io[0] ){
1766 dphi += 2*M_PI;
1767 }
1768 else if( phi_io[1] < dphi ){
1769 dphi -= 2*M_PI;
1770 }
1771
1772 ///in the Local Helix case, phi must be small
1773
1774 Hep3Vector zvector( 0., 0., z_c-rho*dphi*tanl);
1775
1776 double xcio, ycio, phip;
1777 ///// z region check active volume is between zb+2. and zf-2. [cm]
1778 double delrin = 2.0;
1779 if( m_zf-delrin > zvector.z() ){
1780 phip = z_c - m_zb + delrin;
1781 phip = phip/( rho*tanl );
1782 phip = phip + phi0;
1783 xcio = x_c - rho*cos( phip );
1784 ycio = y_c - rho*sin( phip );
1785 cell_IO[i].setX( xcio );
1786 cell_IO[i].setY( ycio );
1787 cell_IO[i].setZ( m_zb+delrin );
1788 epflag[i] = 1;
1789 }
1790 else if( m_zb+delrin < zvector.z() ){
1791 phip = z_c - m_zf-delrin;
1792 phip = phip/( rho*tanl );
1793 phip = phip + phi0;
1794 xcio = x_c - rho*cos( phip );
1795 ycio = y_c - rho*sin( phip );
1796 cell_IO[i].setX( xcio );
1797 cell_IO[i].setY( ycio );
1798 cell_IO[i].setZ( m_zf-delrin );
1799 epflag[i] = 1;
1800 }
1801 else{
1802 cell_IO[i] = iocand;
1803 cell_IO[i] += zvector;
1804 }
1805 /// for debug
1806 if( i == 0 ) phi_in = dphi;
1807}
1808// path length estimation
1809// phi calculation
1810Hep3Vector cl = cell_IO[1] - cell_IO[0];
1811#ifdef YDEBUG
1812cout<<"cell_IO[0] "<<cell_IO[0]<<" cell_IO[1] "<<cell_IO[1]<<" cl "<<cl<<endl;
1813#endif
1814
1815double ch_theta;
1816double ch_dphi;
1817double ch_ltrk = 0;
1818double ch_ltrk_rp = 0;
1819ch_dphi = cl.perp()*0.5/(ALPHA_loc*pt);
1820ch_dphi = 2.0 * asin( ch_dphi );
1821ch_ltrk_rp = ch_dphi*ALPHA_loc*pt;
1822#ifdef YDEBUG
1823cout<<"ch_dphi "<<ch_dphi<<" ch_ltrk_rp "<<ch_ltrk_rp<<" cl.z "<<cl.z()<<endl;
1824#endif
1825ch_ltrk = sqrt( ch_ltrk_rp*ch_ltrk_rp + cl.z()*cl.z() );
1826ch_theta = cl.theta();
1827
1828if( ch_theta < theta*0.85 || 1.15*theta < ch_theta)
1829 ch_ltrk *= -1; /// miss calculation
1830
1831if( epflag[0] == 1 || epflag [1] == 1 )
1832 ch_ltrk *= -1; /// invalid resion for dE/dx or outside of Mdc
1833
1834 mom_[layer] = momentum( phi_in );
1835 PathL_[layer] = ch_ltrk;
1836#ifdef YDEBUG
1837 cout<<"ch_ltrk "<<ch_ltrk<<endl;
1838#endif
1839 return ch_ltrk;
1840 }
1841*/
1842
1843void KalFitTrack::update_bit( int i ) {
1844 int j( 0 );
1845 if ( i < 31 )
1846 {
1847 j = (int)pow( 2., i );
1848 if ( !( pat1_ & j ) ) pat1_ = pat1_ | j;
1849 }
1850 else if ( i < 50 )
1851 {
1852 j = (int)pow( 2., ( i - 31 ) );
1853 if ( !( pat2_ & j ) ) pat2_ = pat2_ | j;
1854 }
1855}
1856
1857int KalFitTrack::nmass( void ) { return NMASS; }
1858double KalFitTrack::mass( int i ) { return MASS[i]; }
1859void KalFitTrack::chgmass( int i ) {
1860 mass_ = MASS[i];
1861 l_mass_ = i;
1862}
1863
1864void KalFitTrack::lead( int i ) { lead_ = i; }
1865int KalFitTrack::lead( void ) { return lead_; }
1866void KalFitTrack::back( int i ) { back_ = i; }
1867int KalFitTrack::back( void ) { return back_; }
1868void KalFitTrack::resol( int i ) { resolflag_ = i; }
1869int KalFitTrack::resol( void ) { return resolflag_; }
1870void KalFitTrack::numf( int i ) { numf_ = i; }
1871int KalFitTrack::numf( void ) { return numf_; }
1872void KalFitTrack::LR( int x ) { LR_ = x; }
1873void KalFitTrack::HitsMdc( vector<KalFitHitMdc>& lh ) { HitsMdc_ = lh; }
1874void KalFitTrack::appendHitsMdc( KalFitHitMdc h ) { HitsMdc_.push_back( h ); }
1875
1876/*
1877 This member function is in charge of the forward filter,
1878 for the tof correction of the drift distance in the case of cosmic rays
1879 if way=1, it's a down track means same as track in collision data,
1880 if way=-1, it's a up track !
1881 */
1882
1883double KalFitTrack::update_hits( KalFitHitMdc& HitMdc, int inext, Hep3Vector& meas, int way,
1884 double& dchi2out, double& dtrack, double& dtracknew,
1885 double& dtdc, int csmflag ) {
1886
1887 double lr = HitMdc.LR();
1888 // std::cout<<" in update_hits: lr= " << lr <<std::endl;
1889 // For taking the propagation delay into account :
1890 const KalFitWire& Wire = HitMdc.wire();
1891 int wire_ID = Wire.geoID();
1892 int layerid = HitMdc.wire().layer().layerId();
1893 // std::cout<<" when in layer: "<<layerid<<std::endl;
1894 double entrangle = HitMdc.rechitptr()->getEntra();
1895
1896 if ( wire_ID < 0 || wire_ID > 6796 )
1897 { // bes
1898 std::cout << " KalFitTrack: wire_ID problem : " << wire_ID << std::endl;
1899 return 0;
1900 }
1901 int flag_ster = Wire.stereo();
1902 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
1903 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
1904 double tofest( 0 );
1905 double phi = fmod( phi0() + M_PI4, M_PI2 );
1906 double csf0 = cos( phi );
1907 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
1908 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
1909 if ( phi > M_PI ) snf0 = -snf0;
1910 if ( Tof_correc_ )
1911 {
1912 double t = tanl();
1913 double dphi( 0 );
1914
1915 Helix work = *(Helix*)this;
1916 work.ignoreErrorMatrix();
1917 double phi_ip( 0 );
1918 Hep3Vector ip( 0, 0, 0 );
1919 work.pivot( ip );
1920 phi_ip = work.phi0();
1921 if ( fabs( phi - phi_ip ) > M_PI )
1922 {
1923 if ( phi > phi_ip ) phi -= 2 * M_PI;
1924 else phi_ip -= 2 * M_PI;
1925 }
1926 dphi = phi - phi_ip;
1927 double l = fabs( radius() * dphi * sqrt( 1 + t * t ) );
1928 double pmag( sqrt( 1.0 + t * t ) / kappa() );
1929 double mass_over_p( mass_ / pmag );
1930 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
1931 tofest = l / ( 29.9792458 * beta );
1932 if ( csmflag == 1 && HitMdc.wire().y() > 0. ) tofest = -1. * tofest;
1933 }
1934
1935 const HepSymMatrix& ea = Ea();
1936 const HepVector& v_a = a();
1937 double dchi2R( 99999999 ), dchi2L( 99999999 );
1938
1939 HepVector v_H( 5, 0 );
1940 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
1941 v_H[3] = -meas.z();
1942 HepMatrix v_HT = v_H.T();
1943
1944 double estim = ( v_HT * v_a )[0];
1945 dtrack = estim;
1946 // std::cout<<" in update_hits estim is "<<estim<<std::endl;
1947 HepVector ea_v_H = ea * v_H;
1948 HepMatrix ea_v_HT = ( ea_v_H ).T();
1949 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
1950
1951 HepSymMatrix eaNewL( 5 ), eaNewR( 5 );
1952 HepVector aNewL( 5 ), aNewR( 5 );
1953
1954 double drifttime = getDriftTime( HitMdc, tofest );
1955 double ddl = getDriftDist( HitMdc, drifttime, 0 );
1956 double ddr = getDriftDist( HitMdc, drifttime, 1 );
1957 double erddl = getSigma( HitMdc, a()[4], 0, ddl );
1958 double erddr = getSigma( HitMdc, a()[4], 1, ddr );
1959
1960 if ( debug_ == 4 )
1961 {
1962 std::cout << "drifttime in update_hits() for ananlysis is " << drifttime << std::endl;
1963 std::cout << "ddl in update_hits() for ananlysis is " << ddl << std::endl;
1964 std::cout << "ddr in update_hits() for ananlysis is " << ddr << std::endl;
1965 std::cout << "erddl in update_hits() for ananlysis is " << erddl << std::endl;
1966 std::cout << "erddr in update_hits() for ananlysis is " << erddr << std::endl;
1967 }
1968
1969 // the following 3 line : tempory
1970 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr }; // millimeter to centimeter
1971 double er_dmeas2[2] = { 0., 0. };
1972 if ( resolflag_ == 1 )
1973 {
1974 er_dmeas2[0] = 0.1 * erddl;
1975 er_dmeas2[1] = 0.1 * erddr;
1976 }
1977 else if ( resolflag_ == 0 )
1978 {
1979 // int layid = HitMdc.wire().layer().layerId();
1980 // double sigma = getSigma(layid, dd);
1981 // er_dmeas2[0] = er_dmeas2[1] = sigma;
1982 }
1983
1984 // Left hypothesis :
1985 if ( ( LR_ == 0 && lr != 1.0 ) || ( LR_ == 1 && lr == -1.0 ) )
1986 {
1987 double er_dmeasL, dmeasL;
1988 if ( Tof_correc_ )
1989 {
1990 dmeasL = ( -1.0 ) * fabs( dmeas2[0] );
1991 er_dmeasL = er_dmeas2[0];
1992 }
1993 else
1994 {
1995 dmeasL = ( -1.0 ) * fabs( HitMdc.dist()[0] );
1996 er_dmeasL = HitMdc.erdist()[0];
1997 }
1998 dtdc = dmeasL;
1999 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
2000 eaNewL.assign( ea - ea_v_H * AL * ea_v_HT );
2001 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
2002 if ( debug_ == 4 )
2003 {
2004 std::cout << " ea_v_H * AL * ea_v_HT is: " << ea_v_H * AL * ea_v_HT << std::endl;
2005 std::cout << " v_H is: " << v_H << " Ea is: " << ea
2006 << " v_H_T_ea_v_H is: " << v_H_T_ea_v_H << std::endl;
2007 std::cout << " AL is: " << AL
2008 << " (v_H_T_ea_v_H)[0] * AL is: " << ( v_H_T_ea_v_H )[0] * AL << std::endl;
2009 }
2010
2011 if ( 0. == RkL ) RkL = 1.e-4;
2012
2013 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
2014 aNewL = v_a + diffL;
2015 double sigL = dmeasL - ( v_HT * aNewL )[0];
2016 dtracknew = ( v_HT * aNewL )[0];
2017 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
2018
2019 if ( debug_ == 4 )
2020 { std::cout << " fwd_filter : in left hypothesis dchi2L is " << dchi2L << std::endl; }
2021 }
2022
2023 if ( ( LR_ == 0 && lr != -1.0 ) || ( LR_ == 1 && lr == 1.0 ) )
2024 {
2025 double er_dmeasR, dmeasR;
2026 if ( Tof_correc_ )
2027 {
2028 dmeasR = dmeas2[1];
2029 er_dmeasR = er_dmeas2[1];
2030 }
2031 else
2032 {
2033 dmeasR = fabs( HitMdc.dist()[1] );
2034 er_dmeasR = HitMdc.erdist()[1];
2035 }
2036 dtdc = dmeasR;
2037 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
2038 eaNewR.assign( ea - ea_v_H * AR * ea_v_HT );
2039 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
2040
2041 if ( debug_ == 4 )
2042 {
2043 std::cout << " ea_v_H * AR * ea_v_HT is: " << ea_v_H * AR * ea_v_HT << std::endl;
2044 std::cout << " v_H is: " << v_H << " Ea is: " << ea
2045 << " v_H_T_ea_v_H is: " << v_H_T_ea_v_H << std::endl;
2046 std::cout << " AR is: " << AR
2047 << " (v_H_T_ea_v_H)[0] * AR is: " << ( v_H_T_ea_v_H )[0] * AR << std::endl;
2048 }
2049
2050 if ( 0. == RkR ) RkR = 1.e-4;
2051 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
2052 aNewR = v_a + diffR;
2053 double sigR = dmeasR - ( v_HT * aNewR )[0];
2054 dtracknew = ( v_HT * aNewR )[0];
2055 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
2056 if ( debug_ == 4 )
2057 { std::cout << " fwd_filter : in right hypothesis dchi2R is " << dchi2R << std::endl; }
2058 }
2059 // Update Kalman result :
2060 double dchi2_loc( 0 );
2061 if ( fabs( dchi2R - dchi2L ) < 10. && inext > 0 )
2062 {
2063 KalFitHitMdc& HitMdc_next = HitsMdc_[inext];
2064 Helix H_fromR( pivot(), aNewR, eaNewR );
2065 double chi2_fromR( chi2_next( H_fromR, HitMdc_next, csmflag ) );
2066 Helix H_fromL( pivot(), aNewL, eaNewL );
2067 double chi2_fromL( chi2_next( H_fromL, HitMdc_next, csmflag ) );
2068#ifdef YDEBUG
2069 std::cout << " chi2_fromL = " << chi2_fromL << ", chi2_fromR = " << chi2_fromR
2070 << std::endl;
2071#endif
2072 if ( fabs( chi2_fromR - chi2_fromL ) < 10. )
2073 {
2074 int inext2( -1 );
2075 if ( inext + 1 < HitsMdc_.size() )
2076 for ( int k = inext + 1; k < HitsMdc_.size(); k++ )
2077 if ( !( HitsMdc_[k].chi2() < 0 ) )
2078 {
2079 inext2 = k;
2080 break;
2081 }
2082
2083 if ( inext2 != -1 )
2084 {
2085 KalFitHitMdc& HitMdc_next2 = HitsMdc_[inext2];
2086 double chi2_fromR2( chi2_next( H_fromR, HitMdc_next2, csmflag ) );
2087 double chi2_fromL2( chi2_next( H_fromL, HitMdc_next2, csmflag ) );
2088#ifdef YDEBUG
2089 std::cout << " chi2_fromL2 = " << chi2_fromL2 << ", chi2_fromR2 = " << chi2_fromR2
2090 << std::endl;
2091#endif
2092 if ( fabs( dchi2R + chi2_fromR + chi2_fromR2 -
2093 ( dchi2L + chi2_fromL + chi2_fromL2 ) ) < 2 )
2094 {
2095 if ( chi2_fromR2 < chi2_fromL2 ) dchi2L = DBL_MAX;
2096 else dchi2R = DBL_MAX;
2097 }
2098 }
2099 }
2100
2101 if ( !( dchi2L == DBL_MAX && dchi2R == DBL_MAX ) )
2102 {
2103 if ( dchi2R + chi2_fromR < dchi2L + chi2_fromL )
2104 {
2105 dchi2L = DBL_MAX;
2106#ifdef YDEBUG
2107 std::cout << " choose right..." << std::endl;
2108#endif
2109 }
2110 else
2111 {
2112 dchi2R = DBL_MAX;
2113#ifdef YDEBUG
2114 std::cout << " choose left..." << std::endl;
2115#endif
2116 }
2117 }
2118 }
2119 if ( ( 0 < dchi2R && dchi2R < dchi2cutf_anal[layerid] ) ||
2120 ( 0 < dchi2L && dchi2L < dchi2cutf_anal[layerid] ) )
2121 {
2122
2123 if ( ( ( LR_ == 0 && dchi2R < dchi2L && lr != -1 ) || ( LR_ == 1 && lr == 1 ) ) &&
2124 fabs( aNewR[2] - a()[2] ) < 1000. && aNewR[2] )
2125 {
2126 if ( nchits_ < (unsigned)nmdc_hit2_ || dchi2R < dchi2cutf_anal[layerid] )
2127 {
2128 nchits_++;
2129 if ( flag_ster ) nster_++;
2130 Ea( eaNewR );
2131 a( aNewR );
2132 chiSq_ += dchi2R;
2133 dchi2_loc = dchi2R;
2134 if ( l_mass_ == lead_ ) { HitMdc.LR( 1 ); }
2135 update_bit( HitMdc.wire().layer().layerId() );
2136 }
2137 }
2138 else if ( ( ( LR_ == 0 && dchi2L <= dchi2R && lr != 1 ) || ( LR_ == 1 && lr == -1 ) ) &&
2139 fabs( aNewL[2] - a()[2] ) < 1000. && aNewL[2] )
2140 {
2141 if ( nchits_ < (unsigned)nmdc_hit2_ || dchi2L < dchi2cutf_anal[layerid] )
2142 {
2143 nchits_++;
2144 if ( flag_ster ) nster_++;
2145 Ea( eaNewL );
2146 a( aNewL );
2147 chiSq_ += dchi2L;
2148 dchi2_loc = dchi2L;
2149 if ( l_mass_ == lead_ ) { HitMdc.LR( -1 ); }
2150 update_bit( HitMdc.wire().layer().layerId() );
2151 }
2152 }
2153 }
2154 if ( dchi2_loc > dchi2_max_ )
2155 {
2156 dchi2_max_ = dchi2_loc;
2157 r_max_ = pivot().perp();
2158 }
2159 dchi2out = dchi2_loc;
2160 // if(dchi2out == 0) {
2161 // dchi2out = ( (dchi2L < dchi2R ) ? dchi2L : dchi2R ) ;
2162 // }
2163 return chiSq_;
2164}
2165
2166double KalFitTrack::update_hits_csmalign( KalFitHelixSeg& HelixSeg, int inext,
2167 Hep3Vector& meas, int way, double& dchi2out,
2168 int csmflag ) {
2169
2170 HepPoint3D ip( 0., 0., 0. );
2171
2172 KalFitHitMdc* HitMdc = HelixSeg.HitMdc();
2173 double lr = HitMdc->LR();
2174 int layerid = ( *HitMdc ).wire().layer().layerId();
2175 // cout<<"layerid: "<<layerid<<endl;
2176 double entrangle = HitMdc->rechitptr()->getEntra();
2177
2178 if ( debug_ == 4 )
2179 {
2180 std::cout << "in update_hits(kalFitHelixSeg,...) : the layerid =" << layerid << std::endl;
2181 std::cout << " update_hits: lr= " << lr << std::endl;
2182 }
2183 // For taking the propagation delay into account :
2184 const KalFitWire& Wire = HitMdc->wire();
2185 int wire_ID = Wire.geoID();
2186
2187 if ( wire_ID < 0 || wire_ID > 6796 )
2188 { // bes
2189 std::cout << " KalFitTrack: wire_ID problem : " << wire_ID << std::endl;
2190 return 0;
2191 }
2192 int flag_ster = Wire.stereo();
2193 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
2194 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
2195 double tofest( 0 );
2196 double phi = fmod( phi0() + M_PI4, M_PI2 );
2197 double csf0 = cos( phi );
2198 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
2199 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
2200 if ( phi > M_PI ) snf0 = -snf0;
2201 if ( Tof_correc_ )
2202 {
2203 double t = tanl();
2204 double dphi( 0 );
2205
2206 Helix work = *(Helix*)this;
2207 work.ignoreErrorMatrix();
2208 double phi_ip( 0 );
2209 Hep3Vector ip( 0, 0, 0 );
2210 work.pivot( ip );
2211 phi_ip = work.phi0();
2212 if ( fabs( phi - phi_ip ) > M_PI )
2213 {
2214 if ( phi > phi_ip ) phi -= 2 * M_PI;
2215 else phi_ip -= 2 * M_PI;
2216 }
2217 dphi = phi - phi_ip;
2218
2219 double l = fabs( radius() * dphi * sqrt( 1 + t * t ) );
2220 double pmag( sqrt( 1.0 + t * t ) / kappa() );
2221 double mass_over_p( mass_ / pmag );
2222 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
2223 tofest = l / ( 29.9792458 * beta );
2224 if ( csmflag == 1 && ( *HitMdc ).wire().y() > 0. ) tofest = -1. * tofest;
2225 }
2226
2227 const HepSymMatrix& ea = Ea();
2228 HelixSeg.Ea_pre_fwd( ea );
2229 HelixSeg.Ea_filt_fwd( ea );
2230
2231 const HepVector& v_a = a();
2232 HelixSeg.a_pre_fwd( v_a );
2233 HelixSeg.a_filt_fwd( v_a );
2234
2235 /*
2236
2237 HepPoint3D pivot_work = pivot();
2238 HepVector a_work = a();
2239 HepSymMatrix ea_work = Ea();
2240 KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
2241 helix_work.pivot(ip);
2242
2243 HepVector a_temp = helix_work.a();
2244 HepSymMatrix ea_temp = helix_work.Ea();
2245
2246 HelixSeg.Ea_pre_fwd(ea_temp);
2247 HelixSeg.a_pre_fwd(a_temp);
2248
2249 // My God, for the protection purpose
2250 HelixSeg.Ea_filt_fwd(ea_temp);
2251 HelixSeg.a_filt_fwd(a_temp);
2252
2253 */
2254
2255 double dchi2R( 99999999 ), dchi2L( 99999999 );
2256
2257 HepVector v_H( 5, 0 );
2258 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
2259 v_H[3] = -meas.z();
2260 HepMatrix v_HT = v_H.T();
2261
2262 double estim = ( v_HT * v_a )[0];
2263 HepVector ea_v_H = ea * v_H;
2264 HepMatrix ea_v_HT = ( ea_v_H ).T();
2265 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
2266
2267 HepSymMatrix eaNewL( 5 ), eaNewR( 5 );
2268 HepVector aNewL( 5 ), aNewR( 5 );
2269#ifdef YDEBUG
2270 cout << "phi " << phi << " snf0 " << snf0 << " csf0 " << csf0 << endl
2271 << " meas: " << meas << endl;
2272 cout << "the related matrix:" << endl;
2273 cout << "v_H " << v_H << endl;
2274 cout << "v_a " << v_a << endl;
2275 cout << "ea " << ea << endl;
2276 cout << "v_H_T_ea_v_H " << v_H_T_ea_v_H << endl;
2277 cout << "LR_= " << LR_ << "lr= " << lr << endl;
2278#endif
2279
2280 double time = ( *HitMdc ).tdc();
2281 // if (Tof_correc_)
2282 // time = time - way*tofest;
2283
2284 // double dd = 1.0e-4 * 40.0 * time;
2285 // the following 3 line : tempory
2286
2287 double drifttime = getDriftTime( *HitMdc, tofest );
2288 double ddl = getDriftDist( *HitMdc, drifttime, 0 );
2289 double ddr = getDriftDist( *HitMdc, drifttime, 1 );
2290 double erddl = getSigma( *HitMdc, a()[4], 0, ddl );
2291 double erddr = getSigma( *HitMdc, a()[4], 1, ddr );
2292
2293 if ( debug_ == 4 )
2294 {
2295 std::cout << "the pure drift time is " << drifttime << std::endl;
2296 std::cout << "the drift dist left hypothesis is " << ddl << std::endl;
2297 std::cout << "the drift dist right hypothesis is " << ddr << std::endl;
2298 std::cout << "the error sigma left hypothesis is " << erddl << std::endl;
2299 std::cout << "the error sigma right hypothesis is " << erddr << std::endl;
2300 }
2301 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr }; // millimeter to centimeter
2302 double er_dmeas2[2] = { 0., 0. };
2303
2304 if ( resolflag_ == 1 )
2305 {
2306 er_dmeas2[0] = 0.1 * erddl;
2307 er_dmeas2[1] = 0.1 * erddr;
2308 }
2309 else if ( resolflag_ == 0 )
2310 {
2311 // int layid = (*HitMdc).wire().layer().layerId();
2312 // double sigma = getSigma(layid, dd);
2313 // er_dmeas2[0] = er_dmeas2[1] = sigma;
2314 }
2315
2316 // Left hypothesis :
2317 if ( ( LR_ == 0 && lr != 1.0 ) || ( LR_ == 1 && lr == -1.0 ) )
2318 {
2319
2320 double er_dmeasL, dmeasL;
2321 if ( Tof_correc_ )
2322 {
2323 dmeasL = ( -1.0 ) * fabs( dmeas2[0] );
2324 er_dmeasL = er_dmeas2[0];
2325 }
2326 else
2327 {
2328 dmeasL = ( -1.0 ) * fabs( ( *HitMdc ).dist()[0] );
2329 er_dmeasL = ( *HitMdc ).erdist()[0];
2330 }
2331
2332 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
2333 eaNewL.assign( ea - ea_v_H * AL * ea_v_HT );
2334 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
2335 if ( 0. == RkL ) RkL = 1.e-4;
2336
2337 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
2338
2339 aNewL = v_a + diffL;
2340 double sigL = dmeasL - ( v_HT * aNewL )[0];
2341 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
2342 }
2343
2344 if ( ( LR_ == 0 && lr != -1.0 ) || ( LR_ == 1 && lr == 1.0 ) )
2345 {
2346
2347 double er_dmeasR, dmeasR;
2348 if ( Tof_correc_ )
2349 {
2350 dmeasR = dmeas2[1];
2351 er_dmeasR = er_dmeas2[1];
2352 }
2353 else
2354 {
2355 dmeasR = fabs( ( *HitMdc ).dist()[1] );
2356 er_dmeasR = ( *HitMdc ).erdist()[1];
2357 }
2358
2359 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
2360 eaNewR.assign( ea - ea_v_H * AR * ea_v_HT );
2361 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
2362 if ( 0. == RkR ) RkR = 1.e-4;
2363
2364 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
2365
2366 aNewR = v_a + diffR;
2367 double sigR = dmeasR - ( v_HT * aNewR )[0];
2368 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
2369 }
2370
2371 // Update Kalman result :
2372 double dchi2_loc( 0 );
2373
2374#ifdef YDEBUG
2375 cout << " track::update_hits......" << endl;
2376 std::cout << " dchi2R = " << dchi2R << ", dchi2L = " << dchi2L
2377 << " estimate = " << estim << std::endl;
2378 std::cout << " dmeasR = " << dmeas2[1] << ", dmeasL = " << ( -1.0 ) * fabs( dmeas2[0] )
2379 << ", check HitMdc.ddl = " << ( *HitMdc ).dist()[0] << std::endl;
2380 std::cout << "dchi2L : " << dchi2L << " ,dchi2R: " << dchi2R << std::endl;
2381#endif
2382
2383 if ( fabs( dchi2R - dchi2L ) < 10. && inext > 0 )
2384 {
2385
2386 KalFitHitMdc& HitMdc_next = HitsMdc_[inext];
2387
2388 Helix H_fromR( pivot(), aNewR, eaNewR );
2389 double chi2_fromR( chi2_next( H_fromR, HitMdc_next, csmflag ) );
2390 // double chi2_fromR(chi2_next(H_fromR, HitMdc_next));
2391
2392 Helix H_fromL( pivot(), aNewL, eaNewL );
2393 double chi2_fromL( chi2_next( H_fromL, HitMdc_next, csmflag ) );
2394 // double chi2_fromL(chi2_next(H_fromL, HitMdc_next));
2395#ifdef YDEBUG
2396 std::cout << " chi2_fromL = " << chi2_fromL << ", chi2_fromR = " << chi2_fromR
2397 << std::endl;
2398#endif
2399 if ( fabs( chi2_fromR - chi2_fromL ) < 10. )
2400 {
2401 int inext2( -1 );
2402 if ( inext + 1 < HitsMdc_.size() )
2403 for ( int k = inext + 1; k < HitsMdc_.size(); k++ )
2404 if ( !( HitsMdc_[k].chi2() < 0 ) )
2405 {
2406 inext2 = k;
2407 break;
2408 }
2409
2410 if ( inext2 != -1 )
2411 {
2412 KalFitHitMdc& HitMdc_next2 = HitsMdc_[inext2];
2413 // double chi2_fromR2(chi2_next(H_fromR, HitMdc_next2));
2414 // double chi2_fromL2(chi2_next(H_fromL, HitMdc_next2));
2415 double chi2_fromR2( chi2_next( H_fromR, HitMdc_next2, csmflag ) );
2416 double chi2_fromL2( chi2_next( H_fromL, HitMdc_next2, csmflag ) );
2417#ifdef YDEBUG
2418 std::cout << " chi2_fromL2 = " << chi2_fromL2 << ", chi2_fromR2 = " << chi2_fromR2
2419 << std::endl;
2420#endif
2421 if ( fabs( dchi2R + chi2_fromR + chi2_fromR2 -
2422 ( dchi2L + chi2_fromL + chi2_fromL2 ) ) < 2 )
2423 {
2424 if ( chi2_fromR2 < chi2_fromL2 ) dchi2L = DBL_MAX;
2425 else dchi2R = DBL_MAX;
2426 }
2427 }
2428 }
2429
2430 if ( !( dchi2L == DBL_MAX && dchi2R == DBL_MAX ) )
2431 {
2432 if ( dchi2R + chi2_fromR < dchi2L + chi2_fromL )
2433 {
2434 dchi2L = DBL_MAX;
2435#ifdef YDEBUG
2436 std::cout << " choose right..." << std::endl;
2437#endif
2438 }
2439 else
2440 {
2441 dchi2R = DBL_MAX;
2442#ifdef YDEBUG
2443 std::cout << " choose left..." << std::endl;
2444#endif
2445 }
2446 }
2447 }
2448
2449 if ( ( 0 < dchi2R && dchi2R < dchi2cutf_calib[layerid] ) ||
2450 ( 0 < dchi2L && dchi2L < dchi2cutf_calib[layerid] ) )
2451 {
2452
2453 if ( ( ( LR_ == 0 && dchi2R < dchi2L && lr != -1 ) || ( LR_ == 1 && lr == 1 ) ) &&
2454 fabs( aNewR[2] - a()[2] ) < 1000. && aNewR[2] )
2455 {
2456 if ( nchits_ < (unsigned)nmdc_hit2_ || dchi2R < dchi2cutf_calib[layerid] )
2457 {
2458 nchits_++;
2459 if ( flag_ster ) nster_++;
2460 if ( layerid > 19 )
2461 {
2462 Ea( eaNewR );
2463 HelixSeg.Ea_filt_fwd( eaNewR );
2464 a( aNewR );
2465 HelixSeg.a_filt_fwd( aNewR );
2466 }
2467 /*
2468 Ea(eaNewR);
2469 a(aNewR);
2470
2471 KalFitTrack helixR(pivot_work, aNewR, eaNewR, 0, 0, 0);
2472 helixR.pivot(ip);
2473
2474 a_temp = helixR.a();
2475 ea_temp = helixR.Ea();
2476
2477 HelixSeg.Ea_filt_fwd(ea_temp);
2478 HelixSeg.a_filt_fwd(a_temp);
2479 //HelixSeg.filt_include(1);
2480
2481 */
2482
2483 chiSq_ += dchi2R;
2484 dchi2_loc = dchi2R;
2485 if ( l_mass_ == lead_ )
2486 {
2487 ( *HitMdc ).LR( 1 );
2488 HelixSeg.LR( 1 );
2489 }
2490 update_bit( ( *HitMdc ).wire().layer().layerId() );
2491 }
2492 }
2493 else if ( ( ( LR_ == 0 && dchi2L <= dchi2R && lr != 1 ) || ( LR_ == 1 && lr == -1 ) ) &&
2494 fabs( aNewL[2] - a()[2] ) < 1000. && aNewL[2] )
2495 {
2496 if ( nchits_ < (unsigned)nmdc_hit2_ || dchi2L < dchi2cutf_calib[layerid] )
2497 {
2498 nchits_++;
2499 if ( flag_ster ) nster_++;
2500 if ( layerid > 19 )
2501 {
2502 Ea( eaNewL );
2503 HelixSeg.Ea_filt_fwd( eaNewL );
2504 a( aNewL );
2505 HelixSeg.a_filt_fwd( aNewL );
2506 }
2507
2508 /*
2509 Ea(eaNewL);
2510 a(aNewL);
2511
2512 KalFitTrack helixL(pivot_work, aNewL, eaNewL, 0, 0, 0);
2513 helixL.pivot(ip);
2514 a_temp = helixL.a();
2515 ea_temp = helixL.Ea();
2516 HelixSeg.Ea_filt_fwd(ea_temp);
2517 HelixSeg.a_filt_fwd(a_temp);
2518 //HelixSeg.filt_include(1);
2519
2520 */
2521
2522 chiSq_ += dchi2L;
2523 dchi2_loc = dchi2L;
2524 if ( l_mass_ == lead_ )
2525 {
2526 ( *HitMdc ).LR( -1 );
2527 HelixSeg.LR( -1 );
2528 }
2529 update_bit( ( *HitMdc ).wire().layer().layerId() );
2530 }
2531 }
2532 }
2533
2534 if ( dchi2_loc > dchi2_max_ )
2535 {
2536 dchi2_max_ = dchi2_loc;
2537 r_max_ = pivot().perp();
2538 }
2539 dchi2out = dchi2_loc;
2540 // if(dchi2out == 0) {
2541 // dchi2out = ( (dchi2L < dchi2R ) ? dchi2L : dchi2R ) ;
2542 // }
2543 return chiSq_;
2544}
2545
2546double KalFitTrack::update_hits( KalFitHelixSeg& HelixSeg, int inext, Hep3Vector& meas,
2547 int way, double& dchi2out, int csmflag ) {
2548
2549 HepPoint3D ip( 0., 0., 0. );
2550
2551 KalFitHitMdc* HitMdc = HelixSeg.HitMdc();
2552 double lr = HitMdc->LR();
2553 int layerid = ( *HitMdc ).wire().layer().layerId();
2554 double entrangle = HitMdc->rechitptr()->getEntra();
2555
2556 if ( debug_ == 4 )
2557 {
2558 std::cout << "in update_hits(kalFitHelixSeg,...) : the layerid =" << layerid << std::endl;
2559 std::cout << " update_hits: lr= " << lr << std::endl;
2560 }
2561 // For taking the propagation delay into account :
2562 const KalFitWire& Wire = HitMdc->wire();
2563 int wire_ID = Wire.geoID();
2564
2565 if ( wire_ID < 0 || wire_ID > 6796 )
2566 { // bes
2567 std::cout << " KalFitTrack: wire_ID problem : " << wire_ID << std::endl;
2568 return 0;
2569 }
2570 int flag_ster = Wire.stereo();
2571 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
2572 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
2573 double tofest( 0 );
2574 double phi = fmod( phi0() + M_PI4, M_PI2 );
2575 double csf0 = cos( phi );
2576 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
2577 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
2578 if ( phi > M_PI ) snf0 = -snf0;
2579 if ( Tof_correc_ )
2580 {
2581 double t = tanl();
2582 double dphi( 0 );
2583
2584 Helix work = *(Helix*)this;
2585 work.ignoreErrorMatrix();
2586 double phi_ip( 0 );
2587 Hep3Vector ip( 0, 0, 0 );
2588 work.pivot( ip );
2589 phi_ip = work.phi0();
2590 if ( fabs( phi - phi_ip ) > M_PI )
2591 {
2592 if ( phi > phi_ip ) phi -= 2 * M_PI;
2593 else phi_ip -= 2 * M_PI;
2594 }
2595 dphi = phi - phi_ip;
2596
2597 double l = fabs( radius() * dphi * sqrt( 1 + t * t ) );
2598 double pmag( sqrt( 1.0 + t * t ) / kappa() );
2599 double mass_over_p( mass_ / pmag );
2600 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
2601 tofest = l / ( 29.9792458 * beta );
2602 if ( csmflag == 1 && ( *HitMdc ).wire().y() > 0. ) tofest = -1. * tofest;
2603 }
2604
2605 const HepSymMatrix& ea = Ea();
2606 HelixSeg.Ea_pre_fwd( ea );
2607 HelixSeg.Ea_filt_fwd( ea );
2608
2609 const HepVector& v_a = a();
2610 HelixSeg.a_pre_fwd( v_a );
2611 HelixSeg.a_filt_fwd( v_a );
2612
2613 /*
2614
2615 HepPoint3D pivot_work = pivot();
2616 HepVector a_work = a();
2617 HepSymMatrix ea_work = Ea();
2618 KalFitTrack helix_work(pivot_work, a_work, ea_work, 0, 0, 0);
2619 helix_work.pivot(ip);
2620
2621 HepVector a_temp = helix_work.a();
2622 HepSymMatrix ea_temp = helix_work.Ea();
2623
2624 HelixSeg.Ea_pre_fwd(ea_temp);
2625 HelixSeg.a_pre_fwd(a_temp);
2626
2627 // My God, for the protection purpose
2628 HelixSeg.Ea_filt_fwd(ea_temp);
2629 HelixSeg.a_filt_fwd(a_temp);
2630
2631 */
2632
2633 double dchi2R( 99999999 ), dchi2L( 99999999 );
2634
2635 HepVector v_H( 5, 0 );
2636 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
2637 v_H[3] = -meas.z();
2638 HepMatrix v_HT = v_H.T();
2639
2640 double estim = ( v_HT * v_a )[0];
2641 HepVector ea_v_H = ea * v_H;
2642 HepMatrix ea_v_HT = ( ea_v_H ).T();
2643 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
2644
2645 HepSymMatrix eaNewL( 5 ), eaNewR( 5 );
2646 HepVector aNewL( 5 ), aNewR( 5 );
2647#ifdef YDEBUG
2648 cout << "phi " << phi << " snf0 " << snf0 << " csf0 " << csf0 << endl
2649 << " meas: " << meas << endl;
2650 cout << "the related matrix:" << endl;
2651 cout << "v_H " << v_H << endl;
2652 cout << "v_a " << v_a << endl;
2653 cout << "ea " << ea << endl;
2654 cout << "v_H_T_ea_v_H " << v_H_T_ea_v_H << endl;
2655 cout << "LR_= " << LR_ << "lr= " << lr << endl;
2656#endif
2657
2658 double time = ( *HitMdc ).tdc();
2659 // if (Tof_correc_)
2660 // time = time - way*tofest;
2661
2662 // double dd = 1.0e-4 * 40.0 * time;
2663 // the following 3 line : tempory
2664
2665 double drifttime = getDriftTime( *HitMdc, tofest );
2666 double ddl = getDriftDist( *HitMdc, drifttime, 0 );
2667 double ddr = getDriftDist( *HitMdc, drifttime, 1 );
2668 double erddl = getSigma( *HitMdc, a()[4], 0, ddl );
2669 double erddr = getSigma( *HitMdc, a()[4], 1, ddr );
2670
2671 if ( debug_ == 4 )
2672 {
2673 std::cout << "the pure drift time is " << drifttime << std::endl;
2674 std::cout << "the drift dist left hypothesis is " << ddl << std::endl;
2675 std::cout << "the drift dist right hypothesis is " << ddr << std::endl;
2676 std::cout << "the error sigma left hypothesis is " << erddl << std::endl;
2677 std::cout << "the error sigma right hypothesis is " << erddr << std::endl;
2678 }
2679 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr }; // millimeter to centimeter
2680 double er_dmeas2[2] = { 0., 0. };
2681
2682 if ( resolflag_ == 1 )
2683 {
2684 er_dmeas2[0] = 0.1 * erddl;
2685 er_dmeas2[1] = 0.1 * erddr;
2686 }
2687 else if ( resolflag_ == 0 )
2688 {
2689 // int layid = (*HitMdc).wire().layer().layerId();
2690 // double sigma = getSigma(layid, dd);
2691 // er_dmeas2[0] = er_dmeas2[1] = sigma;
2692 }
2693
2694 // Left hypothesis :
2695 if ( ( LR_ == 0 && lr != 1.0 ) || ( LR_ == 1 && lr == -1.0 ) )
2696 {
2697
2698 double er_dmeasL, dmeasL;
2699 if ( Tof_correc_ )
2700 {
2701 dmeasL = ( -1.0 ) * fabs( dmeas2[0] );
2702 er_dmeasL = er_dmeas2[0];
2703 }
2704 else
2705 {
2706 dmeasL = ( -1.0 ) * fabs( ( *HitMdc ).dist()[0] );
2707 er_dmeasL = ( *HitMdc ).erdist()[0];
2708 }
2709
2710 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
2711 eaNewL.assign( ea - ea_v_H * AL * ea_v_HT );
2712 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
2713 if ( 0. == RkL ) RkL = 1.e-4;
2714
2715 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
2716
2717 aNewL = v_a + diffL;
2718 double sigL = dmeasL - ( v_HT * aNewL )[0];
2719 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
2720 }
2721
2722 if ( ( LR_ == 0 && lr != -1.0 ) || ( LR_ == 1 && lr == 1.0 ) )
2723 {
2724
2725 double er_dmeasR, dmeasR;
2726 if ( Tof_correc_ )
2727 {
2728 dmeasR = dmeas2[1];
2729 er_dmeasR = er_dmeas2[1];
2730 }
2731 else
2732 {
2733 dmeasR = fabs( ( *HitMdc ).dist()[1] );
2734 er_dmeasR = ( *HitMdc ).erdist()[1];
2735 }
2736
2737 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
2738 eaNewR.assign( ea - ea_v_H * AR * ea_v_HT );
2739 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
2740 if ( 0. == RkR ) RkR = 1.e-4;
2741
2742 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
2743
2744 aNewR = v_a + diffR;
2745 double sigR = dmeasR - ( v_HT * aNewR )[0];
2746 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
2747 }
2748
2749 // Update Kalman result :
2750 double dchi2_loc( 0 );
2751
2752#ifdef YDEBUG
2753 cout << " track::update_hits......" << endl;
2754 std::cout << " dchi2R = " << dchi2R << ", dchi2L = " << dchi2L
2755 << " estimate = " << estim << std::endl;
2756 std::cout << " dmeasR = " << dmeas2[1] << ", dmeasL = " << ( -1.0 ) * fabs( dmeas2[0] )
2757 << ", check HitMdc.ddl = " << ( *HitMdc ).dist()[0] << std::endl;
2758#endif
2759
2760 if ( fabs( dchi2R - dchi2L ) < 10. && inext > 0 )
2761 {
2762
2763 KalFitHitMdc& HitMdc_next = HitsMdc_[inext];
2764
2765 Helix H_fromR( pivot(), aNewR, eaNewR );
2766 double chi2_fromR( chi2_next( H_fromR, HitMdc_next, csmflag ) );
2767
2768 Helix H_fromL( pivot(), aNewL, eaNewL );
2769 double chi2_fromL( chi2_next( H_fromL, HitMdc_next, csmflag ) );
2770#ifdef YDEBUG
2771 std::cout << " chi2_fromL = " << chi2_fromL << ", chi2_fromR = " << chi2_fromR
2772 << std::endl;
2773#endif
2774 if ( fabs( chi2_fromR - chi2_fromL ) < 10. )
2775 {
2776 int inext2( -1 );
2777 if ( inext + 1 < HitsMdc_.size() )
2778 for ( int k = inext + 1; k < HitsMdc_.size(); k++ )
2779 if ( !( HitsMdc_[k].chi2() < 0 ) )
2780 {
2781 inext2 = k;
2782 break;
2783 }
2784
2785 if ( inext2 != -1 )
2786 {
2787 KalFitHitMdc& HitMdc_next2 = HitsMdc_[inext2];
2788 double chi2_fromR2( chi2_next( H_fromR, HitMdc_next2, csmflag ) );
2789 double chi2_fromL2( chi2_next( H_fromL, HitMdc_next2, csmflag ) );
2790#ifdef YDEBUG
2791 std::cout << " chi2_fromL2 = " << chi2_fromL2 << ", chi2_fromR2 = " << chi2_fromR2
2792 << std::endl;
2793#endif
2794 if ( fabs( dchi2R + chi2_fromR + chi2_fromR2 -
2795 ( dchi2L + chi2_fromL + chi2_fromL2 ) ) < 2 )
2796 {
2797 if ( chi2_fromR2 < chi2_fromL2 ) dchi2L = DBL_MAX;
2798 else dchi2R = DBL_MAX;
2799 }
2800 }
2801 }
2802
2803 if ( !( dchi2L == DBL_MAX && dchi2R == DBL_MAX ) )
2804 {
2805 if ( dchi2R + chi2_fromR < dchi2L + chi2_fromL )
2806 {
2807 dchi2L = DBL_MAX;
2808#ifdef YDEBUG
2809 std::cout << " choose right..." << std::endl;
2810#endif
2811 }
2812 else
2813 {
2814 dchi2R = DBL_MAX;
2815#ifdef YDEBUG
2816 std::cout << " choose left..." << std::endl;
2817#endif
2818 }
2819 }
2820 }
2821
2822 if ( ( 0 < dchi2R && dchi2R < dchi2cutf_calib[layerid] ) ||
2823 ( 0 < dchi2L && dchi2L < dchi2cutf_calib[layerid] ) )
2824 {
2825
2826 if ( ( ( LR_ == 0 && dchi2R < dchi2L && lr != -1 ) || ( LR_ == 1 && lr == 1 ) ) &&
2827 fabs( aNewR[2] - a()[2] ) < 1000. && aNewR[2] )
2828 {
2829 if ( nchits_ < (unsigned)nmdc_hit2_ || dchi2R < dchi2cutf_calib[layerid] )
2830 {
2831 nchits_++;
2832 if ( flag_ster ) nster_++;
2833
2834 Ea( eaNewR );
2835 HelixSeg.Ea_filt_fwd( eaNewR );
2836 a( aNewR );
2837 HelixSeg.a_filt_fwd( aNewR );
2838
2839 /*
2840 Ea(eaNewR);
2841 a(aNewR);
2842
2843 KalFitTrack helixR(pivot_work, aNewR, eaNewR, 0, 0, 0);
2844 helixR.pivot(ip);
2845
2846 a_temp = helixR.a();
2847 ea_temp = helixR.Ea();
2848
2849 HelixSeg.Ea_filt_fwd(ea_temp);
2850 HelixSeg.a_filt_fwd(a_temp);
2851 //HelixSeg.filt_include(1);
2852
2853 */
2854
2855 chiSq_ += dchi2R;
2856 dchi2_loc = dchi2R;
2857 if ( l_mass_ == lead_ )
2858 {
2859 ( *HitMdc ).LR( 1 );
2860 HelixSeg.LR( 1 );
2861 }
2862 update_bit( ( *HitMdc ).wire().layer().layerId() );
2863 }
2864 }
2865 else if ( ( ( LR_ == 0 && dchi2L <= dchi2R && lr != 1 ) || ( LR_ == 1 && lr == -1 ) ) &&
2866 fabs( aNewL[2] - a()[2] ) < 1000. && aNewL[2] )
2867 {
2868 if ( nchits_ < (unsigned)nmdc_hit2_ || dchi2L < dchi2cutf_calib[layerid] )
2869 {
2870 nchits_++;
2871 if ( flag_ster ) nster_++;
2872 Ea( eaNewL );
2873 HelixSeg.Ea_filt_fwd( eaNewL );
2874 a( aNewL );
2875 HelixSeg.a_filt_fwd( aNewL );
2876
2877 /*
2878 Ea(eaNewL);
2879 a(aNewL);
2880
2881 KalFitTrack helixL(pivot_work, aNewL, eaNewL, 0, 0, 0);
2882 helixL.pivot(ip);
2883 a_temp = helixL.a();
2884 ea_temp = helixL.Ea();
2885 HelixSeg.Ea_filt_fwd(ea_temp);
2886 HelixSeg.a_filt_fwd(a_temp);
2887 //HelixSeg.filt_include(1);
2888
2889 */
2890
2891 chiSq_ += dchi2L;
2892 dchi2_loc = dchi2L;
2893 if ( l_mass_ == lead_ )
2894 {
2895 ( *HitMdc ).LR( -1 );
2896 HelixSeg.LR( -1 );
2897 }
2898 update_bit( ( *HitMdc ).wire().layer().layerId() );
2899 }
2900 }
2901 }
2902
2903 if ( dchi2_loc > dchi2_max_ )
2904 {
2905 dchi2_max_ = dchi2_loc;
2906 r_max_ = pivot().perp();
2907 }
2908 dchi2out = dchi2_loc;
2909 // if(dchi2out == 0) {
2910 // dchi2out = ( (dchi2L < dchi2R ) ? dchi2L : dchi2R ) ;
2911 // }
2912 return chiSq_;
2913}
2914
2915double KalFitTrack::chi2_next( Helix& H, KalFitHitMdc& HitMdc ) {
2916
2917 double lr = HitMdc.LR();
2918 const KalFitWire& Wire = HitMdc.wire();
2919 int wire_ID = Wire.geoID();
2920 int layerid = HitMdc.wire().layer().layerId();
2921 double entrangle = HitMdc.rechitptr()->getEntra();
2922
2923 HepPoint3D fwd( Wire.fwd() );
2924 HepPoint3D bck( Wire.bck() );
2925 Hep3Vector wire = (Hep3Vector)fwd - (Hep3Vector)bck;
2926 Helix work = H;
2927 work.ignoreErrorMatrix();
2928 work.pivot( ( fwd + bck ) * .5 );
2929 HepPoint3D x0kal = ( work.x( 0 ).z() - bck.z() ) / wire.z() * wire + bck;
2930 H.pivot( x0kal );
2931
2932 Hep3Vector meas = H.momentum( 0 ).cross( wire ).unit();
2933
2934 if ( wire_ID < 0 || wire_ID > 6796 )
2935 { // bes
2936 std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
2937 return DBL_MAX;
2938 }
2939
2940 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
2941 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
2942 double tofest( 0 );
2943 double phi = fmod( phi0() + M_PI4, M_PI2 );
2944 double csf0 = cos( phi );
2945 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
2946 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
2947 if ( phi > M_PI ) snf0 = -snf0;
2948
2949 if ( Tof_correc_ )
2950 {
2951 Hep3Vector ip( 0, 0, 0 );
2952 Helix work = *(Helix*)this;
2953 work.ignoreErrorMatrix();
2954 work.pivot( ip );
2955 double phi_ip = work.phi0();
2956 if ( fabs( phi - phi_ip ) > M_PI )
2957 {
2958 if ( phi > phi_ip ) phi -= 2 * M_PI;
2959 else phi_ip -= 2 * M_PI;
2960 }
2961 double t = tanl();
2962 double l = fabs( radius() * ( phi - phi_ip ) * sqrt( 1 + t * t ) );
2963 double pmag( sqrt( 1.0 + t * t ) / kappa() );
2964 double mass_over_p( mass_ / pmag );
2965 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
2966 tofest = l / ( 29.9792458 * beta );
2967 // if(csmflag==1 && HitMdc.wire().y()>0.) tofest= -1. * tofest;
2968 }
2969
2970 const HepSymMatrix& ea = H.Ea();
2971 const HepVector& v_a = H.a();
2972 double dchi2R( DBL_MAX ), dchi2L( DBL_MAX );
2973
2974 HepVector v_H( 5, 0 );
2975 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
2976 v_H[3] = -meas.z();
2977 HepMatrix v_HT = v_H.T();
2978
2979 double estim = ( v_HT * v_a )[0];
2980 HepVector ea_v_H = ea * v_H;
2981 HepMatrix ea_v_HT = ( ea_v_H ).T();
2982 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
2983
2984 HepSymMatrix eaNewL( 5 ), eaNewR( 5 );
2985 HepVector aNewL( 5 ), aNewR( 5 );
2986
2987 // double time = HitMdc.tdc();
2988 // if (Tof_correc_)
2989 // time = time - tofest;
2990 double drifttime = getDriftTime( HitMdc, tofest );
2991 double ddl = getDriftDist( HitMdc, drifttime, 0 );
2992 double ddr = getDriftDist( HitMdc, drifttime, 1 );
2993 double erddl = getSigma( HitMdc, H.a()[4], 0, ddl );
2994 double erddr = getSigma( HitMdc, H.a()[4], 1, ddr );
2995
2996 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr };
2997 double er_dmeas2[2] = { 0., 0. };
2998 if ( resolflag_ == 1 )
2999 {
3000 er_dmeas2[0] = 0.1 * erddl;
3001 er_dmeas2[1] = 0.1 * erddr;
3002 }
3003 else if ( resolflag_ == 0 )
3004 {
3005 // int layid = HitMdc.wire().layer().layerId();
3006 // double sigma = getSigma(layid, dd);
3007 // er_dmeas2[0] = er_dmeas2[1] = sigma;
3008 }
3009
3010 if ( ( LR_ == 0 && lr != 1.0 ) || ( LR_ == 1 && lr == -1.0 ) )
3011 {
3012
3013 double er_dmeasL, dmeasL;
3014 if ( Tof_correc_ )
3015 {
3016 dmeasL = ( -1.0 ) * fabs( dmeas2[0] );
3017 er_dmeasL = er_dmeas2[0];
3018 }
3019 else
3020 {
3021 dmeasL = ( -1.0 ) * fabs( HitMdc.dist()[0] );
3022 er_dmeasL = HitMdc.erdist()[0];
3023 }
3024
3025 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
3026 eaNewL.assign( ea - ea_v_H * AL * ea_v_HT );
3027 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
3028 if ( 0. == RkL ) RkL = 1.e-4;
3029
3030 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
3031 aNewL = v_a + diffL;
3032 double sigL = dmeasL - ( v_HT * aNewL )[0];
3033 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
3034 }
3035
3036 if ( ( LR_ == 0 && lr != -1.0 ) || ( LR_ == 1 && lr == 1.0 ) )
3037 {
3038
3039 double er_dmeasR, dmeasR;
3040 if ( Tof_correc_ )
3041 {
3042 dmeasR = dmeas2[1];
3043 er_dmeasR = er_dmeas2[1];
3044 }
3045 else
3046 {
3047 dmeasR = fabs( HitMdc.dist()[1] );
3048 er_dmeasR = HitMdc.erdist()[1];
3049 }
3050
3051 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
3052 eaNewR.assign( ea - ea_v_H * AR * ea_v_HT );
3053 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
3054 if ( 0. == RkR ) RkR = 1.e-4;
3055
3056 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
3057 aNewR = v_a + diffR;
3058 double sigR = dmeasR - ( v_HT * aNewR )[0];
3059 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
3060 }
3061
3062 if ( dchi2R < dchi2L )
3063 {
3064 H.a( aNewR );
3065 H.Ea( eaNewR );
3066 }
3067 else
3068 {
3069 H.a( aNewL );
3070 H.Ea( eaNewL );
3071 }
3072 return ( ( dchi2R < dchi2L ) ? dchi2R : dchi2L );
3073}
3074
3075double KalFitTrack::chi2_next( Helix& H, KalFitHitMdc& HitMdc, int csmflag ) {
3076
3077 double lr = HitMdc.LR();
3078 const KalFitWire& Wire = HitMdc.wire();
3079 int wire_ID = Wire.geoID();
3080 int layerid = HitMdc.wire().layer().layerId();
3081 double entrangle = HitMdc.rechitptr()->getEntra();
3082
3083 HepPoint3D fwd( Wire.fwd() );
3084 HepPoint3D bck( Wire.bck() );
3085 Hep3Vector wire = (Hep3Vector)fwd - (Hep3Vector)bck;
3086 Helix work = H;
3087 work.ignoreErrorMatrix();
3088 work.pivot( ( fwd + bck ) * .5 );
3089 HepPoint3D x0kal = ( work.x( 0 ).z() - bck.z() ) / wire.z() * wire + bck;
3090 H.pivot( x0kal );
3091
3092 Hep3Vector meas = H.momentum( 0 ).cross( wire ).unit();
3093
3094 if ( wire_ID < 0 || wire_ID > 6796 )
3095 { // bes
3096 std::cout << "KalFitTrack : wire_ID problem : " << wire_ID << std::endl;
3097 return DBL_MAX;
3098 }
3099
3100 double x[3] = { pivot().x(), pivot().y(), pivot().z() };
3101 double pmom[3] = { momentum().x(), momentum().y(), momentum().z() };
3102 double tofest( 0 );
3103 double phi = fmod( phi0() + M_PI4, M_PI2 );
3104 double csf0 = cos( phi );
3105 double snf0 = ( 1. - csf0 ) * ( 1. + csf0 );
3106 snf0 = sqrt( ( snf0 > 0. ) ? snf0 : 0. );
3107 if ( phi > M_PI ) snf0 = -snf0;
3108
3109 if ( Tof_correc_ )
3110 {
3111 Hep3Vector ip( 0, 0, 0 );
3112 Helix work = *(Helix*)this;
3113 work.ignoreErrorMatrix();
3114 work.pivot( ip );
3115 double phi_ip = work.phi0();
3116 if ( fabs( phi - phi_ip ) > M_PI )
3117 {
3118 if ( phi > phi_ip ) phi -= 2 * M_PI;
3119 else phi_ip -= 2 * M_PI;
3120 }
3121 double t = tanl();
3122 double l = fabs( radius() * ( phi - phi_ip ) * sqrt( 1 + t * t ) );
3123 double pmag( sqrt( 1.0 + t * t ) / kappa() );
3124 double mass_over_p( mass_ / pmag );
3125 double beta( 1.0 / sqrt( 1.0 + mass_over_p * mass_over_p ) );
3126 tofest = l / ( 29.9792458 * beta );
3127 if ( csmflag == 1 && HitMdc.wire().y() > 0. ) tofest = -1. * tofest;
3128 }
3129
3130 const HepSymMatrix& ea = H.Ea();
3131 const HepVector& v_a = H.a();
3132 double dchi2R( DBL_MAX ), dchi2L( DBL_MAX );
3133
3134 HepVector v_H( 5, 0 );
3135 v_H[0] = -csf0 * meas.x() - snf0 * meas.y();
3136 v_H[3] = -meas.z();
3137 HepMatrix v_HT = v_H.T();
3138
3139 double estim = ( v_HT * v_a )[0];
3140 HepVector ea_v_H = ea * v_H;
3141 HepMatrix ea_v_HT = ( ea_v_H ).T();
3142 HepVector v_H_T_ea_v_H = v_HT * ea_v_H;
3143
3144 HepSymMatrix eaNewL( 5 ), eaNewR( 5 );
3145 HepVector aNewL( 5 ), aNewR( 5 );
3146
3147 // double time = HitMdc.tdc();
3148 // if (Tof_correc_)
3149 // time = time - tofest;
3150 double drifttime = getDriftTime( HitMdc, tofest );
3151 double ddl = getDriftDist( HitMdc, drifttime, 0 );
3152 double ddr = getDriftDist( HitMdc, drifttime, 1 );
3153 double erddl = getSigma( HitMdc, H.a()[4], 0, ddl );
3154 double erddr = getSigma( HitMdc, H.a()[4], 1, ddr );
3155
3156 double dmeas2[2] = { 0.1 * ddl, 0.1 * ddr };
3157 double er_dmeas2[2] = { 0., 0. };
3158 if ( resolflag_ == 1 )
3159 {
3160 er_dmeas2[0] = 0.1 * erddl;
3161 er_dmeas2[1] = 0.1 * erddr;
3162 }
3163 else if ( resolflag_ == 0 )
3164 {
3165 // int layid = HitMdc.wire().layer().layerId();
3166 // double sigma = getSigma(layid, dd);
3167 // er_dmeas2[0] = er_dmeas2[1] = sigma;
3168 }
3169
3170 if ( ( LR_ == 0 && lr != 1.0 ) || ( LR_ == 1 && lr == -1.0 ) )
3171 {
3172
3173 double er_dmeasL, dmeasL;
3174 if ( Tof_correc_ )
3175 {
3176 dmeasL = ( -1.0 ) * fabs( dmeas2[0] );
3177 er_dmeasL = er_dmeas2[0];
3178 }
3179 else
3180 {
3181 dmeasL = ( -1.0 ) * fabs( HitMdc.dist()[0] );
3182 er_dmeasL = HitMdc.erdist()[0];
3183 }
3184
3185 double AL = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasL * er_dmeasL );
3186 eaNewL.assign( ea - ea_v_H * AL * ea_v_HT );
3187 double RkL = 1 - ( v_H_T_ea_v_H )[0] * AL;
3188 if ( 0. == RkL ) RkL = 1.e-4;
3189
3190 HepVector diffL = ea_v_H * AL * ( dmeasL - estim );
3191 aNewL = v_a + diffL;
3192 double sigL = dmeasL - ( v_HT * aNewL )[0];
3193 dchi2L = ( sigL * sigL ) / ( RkL * er_dmeasL * er_dmeasL );
3194 }
3195
3196 if ( ( LR_ == 0 && lr != -1.0 ) || ( LR_ == 1 && lr == 1.0 ) )
3197 {
3198
3199 double er_dmeasR, dmeasR;
3200 if ( Tof_correc_ )
3201 {
3202 dmeasR = dmeas2[1];
3203 er_dmeasR = er_dmeas2[1];
3204 }
3205 else
3206 {
3207 dmeasR = fabs( HitMdc.dist()[1] );
3208 er_dmeasR = HitMdc.erdist()[1];
3209 }
3210
3211 double AR = 1 / ( ( v_H_T_ea_v_H )[0] + er_dmeasR * er_dmeasR );
3212 eaNewR.assign( ea - ea_v_H * AR * ea_v_HT );
3213 double RkR = 1 - ( v_H_T_ea_v_H )[0] * AR;
3214 if ( 0. == RkR ) RkR = 1.e-4;
3215
3216 HepVector diffR = ea_v_H * AR * ( dmeasR - estim );
3217 aNewR = v_a + diffR;
3218 double sigR = dmeasR - ( v_HT * aNewR )[0];
3219 dchi2R = ( sigR * sigR ) / ( RkR * er_dmeasR * er_dmeasR );
3220 }
3221
3222 if ( dchi2R < dchi2L )
3223 {
3224 H.a( aNewR );
3225 H.Ea( eaNewR );
3226 }
3227 else
3228 {
3229 H.a( aNewL );
3230 H.Ea( eaNewL );
3231 }
3232 return ( ( dchi2R < dchi2L ) ? dchi2R : dchi2L );
3233}
3234
3235double KalFitTrack::getSigma( int layerId, double driftDist ) const {
3236 double sigma1, sigma2, f;
3237 driftDist *= 10; // mm
3238 if ( layerId < 8 )
3239 {
3240 if ( driftDist < 0.5 )
3241 {
3242 sigma1 = 0.112784;
3243 sigma2 = 0.229274;
3244 f = 0.666;
3245 }
3246 else if ( driftDist < 1. )
3247 {
3248 sigma1 = 0.103123;
3249 sigma2 = 0.269797;
3250 f = 0.934;
3251 }
3252 else if ( driftDist < 1.5 )
3253 {
3254 sigma1 = 0.08276;
3255 sigma2 = 0.17493;
3256 f = 0.89;
3257 }
3258 else if ( driftDist < 2. )
3259 {
3260 sigma1 = 0.070109;
3261 sigma2 = 0.149859;
3262 f = 0.89;
3263 }
3264 else if ( driftDist < 2.5 )
3265 {
3266 sigma1 = 0.064453;
3267 sigma2 = 0.130149;
3268 f = 0.886;
3269 }
3270 else if ( driftDist < 3. )
3271 {
3272 sigma1 = 0.062383;
3273 sigma2 = 0.138806;
3274 f = 0.942;
3275 }
3276 else if ( driftDist < 3.5 )
3277 {
3278 sigma1 = 0.061873;
3279 sigma2 = 0.145696;
3280 f = 0.946;
3281 }
3282 else if ( driftDist < 4. )
3283 {
3284 sigma1 = 0.061236;
3285 sigma2 = 0.119584;
3286 f = 0.891;
3287 }
3288 else if ( driftDist < 4.5 )
3289 {
3290 sigma1 = 0.066292;
3291 sigma2 = 0.148426;
3292 f = 0.917;
3293 }
3294 else if ( driftDist < 5. )
3295 {
3296 sigma1 = 0.078074;
3297 sigma2 = 0.188148;
3298 f = 0.911;
3299 }
3300 else if ( driftDist < 5.5 )
3301 {
3302 sigma1 = 0.088657;
3303 sigma2 = 0.27548;
3304 f = 0.838;
3305 }
3306 else
3307 {
3308 sigma1 = 0.093089;
3309 sigma2 = 0.115556;
3310 f = 0.367;
3311 }
3312 }
3313 else
3314 {
3315 if ( driftDist < 0.5 )
3316 {
3317 sigma1 = 0.112433;
3318 sigma2 = 0.327548;
3319 f = 0.645;
3320 }
3321 else if ( driftDist < 1. )
3322 {
3323 sigma1 = 0.096703;
3324 sigma2 = 0.305206;
3325 f = 0.897;
3326 }
3327 else if ( driftDist < 1.5 )
3328 {
3329 sigma1 = 0.082518;
3330 sigma2 = 0.248913;
3331 f = 0.934;
3332 }
3333 else if ( driftDist < 2. )
3334 {
3335 sigma1 = 0.072501;
3336 sigma2 = 0.153868;
3337 f = 0.899;
3338 }
3339 else if ( driftDist < 2.5 )
3340 {
3341 sigma1 = 0.065535;
3342 sigma2 = 0.14246;
3343 f = 0.914;
3344 }
3345 else if ( driftDist < 3. )
3346 {
3347 sigma1 = 0.060497;
3348 sigma2 = 0.126489;
3349 f = 0.918;
3350 }
3351 else if ( driftDist < 3.5 )
3352 {
3353 sigma1 = 0.057643;
3354 sigma2 = 0.112927;
3355 f = 0.892;
3356 }
3357 else if ( driftDist < 4. )
3358 {
3359 sigma1 = 0.055266;
3360 sigma2 = 0.094833;
3361 f = 0.887;
3362 }
3363 else if ( driftDist < 4.5 )
3364 {
3365 sigma1 = 0.056263;
3366 sigma2 = 0.124419;
3367 f = 0.932;
3368 }
3369 else if ( driftDist < 5. )
3370 {
3371 sigma1 = 0.056599;
3372 sigma2 = 0.124248;
3373 f = 0.923;
3374 }
3375 else if ( driftDist < 5.5 )
3376 {
3377 sigma1 = 0.061377;
3378 sigma2 = 0.146147;
3379 f = 0.964;
3380 }
3381 else if ( driftDist < 6. )
3382 {
3383 sigma1 = 0.063978;
3384 sigma2 = 0.150591;
3385 f = 0.942;
3386 }
3387 else if ( driftDist < 6.5 )
3388 {
3389 sigma1 = 0.072951;
3390 sigma2 = 0.15685;
3391 f = 0.913;
3392 }
3393 else if ( driftDist < 7. )
3394 {
3395 sigma1 = 0.085438;
3396 sigma2 = 0.255109;
3397 f = 0.931;
3398 }
3399 else if ( driftDist < 7.5 )
3400 {
3401 sigma1 = 0.101635;
3402 sigma2 = 0.315529;
3403 f = 0.878;
3404 }
3405 else
3406 {
3407 sigma1 = 0.149529;
3408 sigma2 = 0.374697;
3409 f = 0.89;
3410 }
3411 }
3412 double sigmax = sqrt( f * sigma1 * sigma1 + ( 1 - f ) * sigma2 * sigma2 ) * 0.1;
3413 return sigmax; // cm
3414}
HepVector3D
HepGeom::Vector3D< double > HepVector3D
Definition Analysis/VertexFit/src/BField.cxx:13
M_PI8
const double M_PI8
Definition Analysis/VertexFit/src/Helix.cxx:29
M_PI2
const double M_PI2
Definition Analysis/VertexFit/src/Helix.cxx:25
M_PI4
const double M_PI4
Definition Analysis/VertexFit/src/Helix.cxx:27
momentum
**********INTEGER nmxhep !maximum number of particles DOUBLE PRECISION vhep INTEGER jdahep COMMON hepevt $ !serial number $ !number of particles $ !status code $ !particle ident KF $ !parent particles $ !childreen particles $ !four momentum
Definition BesBdkRc/src/fortran/HepEvt.h:23
HepPoint3D
HepGeom::Point3D< double > HepPoint3D
Definition CosmicGenerator.h:36
f
TFile f("ana_bhabha660a_dqa_mcPat_zy_old.root")
x
Double_t x[10]
Definition DataBase/tau_mode.c:49
time
Double_t time
Definition DataBase/tau_mode.c:7
HepTransform3D
HepGeom::Transform3D HepTransform3D
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/KalFitTrack.h:22
cos
double cos(const BesAngle a)
Definition InstallArea/x86_64-el9-gcc13-dbg/include/MdcGeom/BesAngle.h:187
m_C
********INTEGER modcns REAL m_C
Definition PseuMar.h:13
M_PI
#define M_PI
Definition TConstant.h:4
KalFitLayer_Mdc::layerId
const int layerId(void) const
returns layer ID
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/KalFitLayer_Mdc.h:91
KalFitMaterial::del_E
double del_E(double mass, double path, double p) const
Calculate the straggling of energy loss.
Definition KalFitMaterial.cxx:84
KalFitMaterial::mcs_angle
double mcs_angle(double mass, double path, double p) const
Calculate Multiple Scattering angle.
Definition KalFitMaterial.cxx:75
KalFitMaterial::dE
double dE(double mass, double path, double p) const
Calculate energy loss.
Definition KalFitMaterial.cxx:31
KalFitTrack::~KalFitTrack
~KalFitTrack(void)
destructor
Definition KalFitTrack.cxx:111
KalFitTrack::chi2_next
double chi2_next(Helix &H, KalFitHitMdc &HitMdc, int csmflag)
Definition KalFitTrack.cxx:3075
KalFitTrack::lead
static int lead(void)
Magnetic field map.
Definition KalFitTrack.cxx:1865
KalFitTrack::getDriftTime
double getDriftTime(KalFitHitMdc &hitmdc, double toftime) const
Definition KalFitTrack2.cxx:62
KalFitTrack::KalFitTrack
KalFitTrack(const HepPoint3D &pivot, const CLHEP::HepVector &a, const CLHEP::HepSymMatrix &Ea, unsigned int m, double chiSq, unsigned int nhits)
constructor
KalFitTrack::nmass
static int nmass(void)
Definition KalFitTrack.cxx:1857
KalFitTrack::filter
double filter(double v_m, const CLHEP::HepVector &m_H, double v_d, double m_V)
KalFitTrack::back
static int back(void)
Definition KalFitTrack.cxx:1867
KalFitTrack::update_bit
void update_bit(int i)
Definition KalFitTrack.cxx:1843
KalFitTrack::order_wirhit
void order_wirhit(int index)
Definition KalFitTrack.cxx:367
KalFitTrack::getSigma
double getSigma(int layerId, double driftDist) const
Definition KalFitTrack.cxx:3235
KalFitTrack::addTofSM
void addTofSM(double time)
Definition KalFitTrack.cxx:1361
KalFitTrack::chgmass
void chgmass(int i)
Definition KalFitTrack.cxx:1859
KalFitTrack::factor_strag_
static double factor_strag_
factor of energy loss straggling for electron
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/KalFitTrack.h:300
KalFitTrack::pivot_numf
const HepPoint3D & pivot_numf(const HepPoint3D &newPivot)
Sets pivot position in a given mag field.
Definition KalFitTrack.cxx:1428
KalFitTrack::ms
void ms(double path, const KalFitMaterial &m, int index)
Definition KalFitTrack.cxx:178
KalFitTrack::update_forMdc
void update_forMdc(void)
Definition KalFitTrack.cxx:121
KalFitTrack::intersect_cylinder
double intersect_cylinder(double r) const
Intersection with different geometry.
Definition KalFitTrack.cxx:127
KalFitTrack::fiTerm
void fiTerm(double fi)
Definition KalFitTrack.cxx:1363
KalFitTrack::number_wirhit
void number_wirhit(void)
Definition KalFitTrack.cxx:449
KalFitTrack::numf_
static int numf_
Flag for treatment of non-uniform mag field.
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/KalFitTrack.h:274
KalFitTrack::path_add
void path_add(double path)
Update the path length estimation.
Definition KalFitTrack.cxx:1354
KalFitTrack::update_hits_csmalign
double update_hits_csmalign(KalFitHelixSeg &HelixSeg, int inext, CLHEP::Hep3Vector &meas, int way, double &dchi2, int csmflag)
KalFitTrack::smoother_Mdc
double smoother_Mdc(KalFitHelixSeg &seg, CLHEP::Hep3Vector &meas, int &flg, int csmflag)
Kalman smoother for Mdc.
KalFitTrack::msgasmdc
void msgasmdc(double path, int index)
Calculate multiple scattering angle.
Definition KalFitTrack.cxx:214
KalFitTrack::appendHitsMdc
void appendHitsMdc(KalFitHitMdc h)
Functions for Mdc hits list.
Definition KalFitTrack.cxx:1874
KalFitTrack::addPathSM
void addPathSM(double path)
Definition KalFitTrack.cxx:1359
KalFitTrack::resol
static int resol(void)
Definition KalFitTrack.cxx:1869
KalFitTrack::eloss
void eloss(double path, const KalFitMaterial &m, int index)
Calculate total energy lost in material.
Definition KalFitTrack.cxx:272
KalFitTrack::update_last
void update_last(void)
Record the current parameters as ..._last information :
Definition KalFitTrack.cxx:115
KalFitTrack::LR
static void LR(int x)
Definition KalFitTrack.cxx:1872
KalFitTrack::getDriftDist
double getDriftDist(KalFitHitMdc &hitmdc, double drifttime, int lr) const
Definition KalFitTrack2.cxx:186
KalFitTrack::intersect_yz_plane
double intersect_yz_plane(const HepTransform3D &plane, double x) const
Definition KalFitTrack.cxx:157
KalFitTrack::numf
static int numf(void)
Definition KalFitTrack.cxx:1871
KalFitTrack::intersect_zx_plane
double intersect_zx_plane(const HepTransform3D &plane, double y) const
Definition KalFitTrack.cxx:142
KalFitTrack::order_hits
void order_hits(void)
Definition KalFitTrack.cxx:434
KalFitTrack::intersect_xy_plane
double intersect_xy_plane(double z) const
Definition KalFitTrack.cxx:172
KalFitTrack::radius_numf
double radius_numf(void) const
Estimation of the radius in a given mag field.
Definition KalFitTrack.cxx:1393
KalFitTrack::smoother_Mdc_csmalign
double smoother_Mdc_csmalign(KalFitHelixSeg &seg, CLHEP::Hep3Vector &meas, int &flg, int csmflag)
KalFitTrack::update_hits
double update_hits(KalFitHitMdc &HitMdc, int inext, CLHEP::Hep3Vector &meas, int way, double &dchi2, double &dtrack, double &dtracknew, double &dtdc, int csmflag)
Include the Mdc wire hits.
KalFitTrack::LR_
static int LR_
Use L/R decision from MdcRecHit information :
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/KalFitTrack.h:309
KalFitTrack::strag_
static int strag_
Flag to take account of energy loss straggling :
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/KalFitTrack.h:298
KalmanFit::Helix::delApDelA
HepMatrix delApDelA(const HepVector &ap) const
Definition Reconstruction/KalFitAlg/src/helix/Helix.cxx:425
KalmanFit::Helix::center
const HepPoint3D & center(void) const
returns position of helix center(z = 0.);
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/helix/Helix.h:183
KalmanFit::Helix::Ea
const HepSymMatrix & Ea(void) const
returns error matrix.
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/helix/Helix.h:205
KalmanFit::Helix::x
HepPoint3D x(double dPhi=0.) const
returns position after rotating angle dPhi in phi direction.
Definition Reconstruction/KalFitAlg/src/helix/Helix.cxx:121
KalmanFit::Helix::dr
double dr(void) const
returns an element of parameters.
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/helix/Helix.h:191
KalmanFit::Helix::a
const HepVector & a(void) const
returns helix parameters.
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/helix/Helix.h:203
KalmanFit::Helix::Helix
Helix(const HepPoint3D &pivot, const HepVector &a, const HepSymMatrix &Ea)
Constructor with pivot, helix parameter a, and its error matrix.
Definition Reconstruction/KalFitAlg/src/helix/Helix.cxx:44
KalmanFit::Helix::pivot
const HepPoint3D & pivot(void) const
returns pivot position.
Definition InstallArea/x86_64-el9-gcc13-dbg/include/KalFitAlg/helix/Helix.h:185
H
IMPLICIT REAL *A H
Definition myXsection.h:1
t
int t()
Definition t.c:1