Hall-D Software  alpha
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
DTrackFitterStraightTrack.cc
Go to the documentation of this file.
1 // $Id$
2 //
3 // File: DTrackFitterStraightTrack.cc
4 // Created: Tue Mar 12 10:22:32 EDT 2019
5 // Creator: staylor (on Linux ifarm1402.jlab.org 3.10.0-327.el7.x86_64 x86_64)
6 //
7 
9 
10 //---------------------------------
11 // DTrackFitterStraightTrack (Constructor)
12 //---------------------------------
14  int runnumber=(loop->GetJEvent()).GetRunNumber();
15  DApplication* dapp = dynamic_cast<DApplication*>(loop->GetJApplication());
16  JCalibration *jcalib = dapp->GetJCalibration(runnumber);
17  const DGeometry *geom = dapp->GetDGeometry(runnumber);
18 
19  // Get pointer to TrackFinder object
20  vector<const DTrackFinder *> finders;
21  loop->Get(finders);
22 
23  // Drop the const qualifier from the DTrackFinder pointer
24  finder = const_cast<DTrackFinder*>(finders[0]);
25 
26  // Resource pool for matrices
27  dResourcePool_TMatrixFSym = std::make_shared<DResourcePool<TMatrixFSym>>
28  ();
29  dResourcePool_TMatrixFSym->Set_ControlParams(20, 20, 50);
30 
31  //****************
32  // FDC parameters
33  //****************
34 
35  map<string,double>drift_res_parms;
36  jcalib->Get("FDC/drift_resolution_parms",drift_res_parms);
37  DRIFT_RES_PARMS[0]=drift_res_parms["p0"];
38  DRIFT_RES_PARMS[1]=drift_res_parms["p1"];
39  DRIFT_RES_PARMS[2]=drift_res_parms["p2"];
40 
41  // Time-to-distance function parameters for FDC
42  map<string,double>drift_func_parms;
43  jcalib->Get("FDC/drift_function_parms",drift_func_parms);
44  DRIFT_FUNC_PARMS[0]=drift_func_parms["p0"];
45  DRIFT_FUNC_PARMS[1]=drift_func_parms["p1"];
46  DRIFT_FUNC_PARMS[2]=drift_func_parms["p2"];
47  DRIFT_FUNC_PARMS[3]=drift_func_parms["p3"];
48  DRIFT_FUNC_PARMS[4]=1000.;
49  DRIFT_FUNC_PARMS[5]=0.;
50  map<string,double>drift_func_ext;
51  if (jcalib->Get("FDC/drift_function_ext",drift_func_ext)==false){
52  DRIFT_FUNC_PARMS[4]=drift_func_ext["p4"];
53  DRIFT_FUNC_PARMS[5]=drift_func_ext["p5"];
54  }
55 
56  PLANE_TO_SKIP=0;
57  gPARMS->SetDefaultParameter("TRKFIT:PLANE_TO_SKIP",PLANE_TO_SKIP);
58 
59  //****************
60  // CDC parameters
61  //****************
62  RING_TO_SKIP=0;
63  gPARMS->SetDefaultParameter("TRKFIT:RING_TO_SKIP",RING_TO_SKIP);
64 
65  map<string, double> cdc_res_parms;
66  jcalib->Get("CDC/cdc_resolution_parms_v2", cdc_res_parms);
67  CDC_RES_PAR1 = cdc_res_parms["res_par1"];
68  CDC_RES_PAR2 = cdc_res_parms["res_par2"];
69  CDC_RES_PAR3 = cdc_res_parms["res_par3"];
70 
71  vector< map<string, double> > tvals;
72  cdc_drift_table.clear();
73  if (jcalib->Get("CDC/cdc_drift_table", tvals)==false){
74  for(unsigned int i=0; i<tvals.size(); i++){
75  map<string, double> &row = tvals[i];
76  cdc_drift_table.push_back(1000.*row["t"]);
77  }
78  }
79  else{
80  jerr << " CDC time-to-distance table not available... bailing..." << endl;
81  exit(0);
82  }
83 
84  if (jcalib->Get("CDC/drift_parameters", tvals)==false){
85  map<string, double> &row = tvals[0]; //long drift side
86  long_drift_func[0][0]=row["a1"];
87  long_drift_func[0][1]=row["a2"];
88  long_drift_func[0][2]=row["a3"];
89  long_drift_func[1][0]=row["b1"];
90  long_drift_func[1][1]=row["b2"];
91  long_drift_func[1][2]=row["b3"];
92  long_drift_func[2][0]=row["c1"];
93  long_drift_func[2][1]=row["c2"];
94  long_drift_func[2][2]=row["c3"];
95 
96  row = tvals[1]; // short drift side
97  short_drift_func[0][0]=row["a1"];
98  short_drift_func[0][1]=row["a2"];
99  short_drift_func[0][2]=row["a3"];
100  short_drift_func[1][0]=row["b1"];
101  short_drift_func[1][1]=row["b2"];
102  short_drift_func[1][2]=row["b3"];
103  short_drift_func[2][0]=row["c1"];
104  short_drift_func[2][1]=row["c2"];
105  short_drift_func[2][2]=row["c3"];
106  }
107 
108  // Get the straw sag parameters from the database
109  max_sag.clear();
110  sag_phi_offset.clear();
111  unsigned int numstraws[28]={42,42,54,54,66,66,80,80,93,93,106,106,123,123,
112  135,135,146,146,158,158,170,170,182,182,197,197,209,209};
113  unsigned int straw_count=0,ring_count=0;
114  if (jcalib->Get("CDC/sag_parameters", tvals)==false){
115  vector<double>temp,temp2;
116  for(unsigned int i=0; i<tvals.size(); i++){
117  map<string, double> &row = tvals[i];
118 
119  temp.push_back(row["offset"]);
120  temp2.push_back(row["phi"]);
121 
122  straw_count++;
123  if (straw_count==numstraws[ring_count]){
124  max_sag.push_back(temp);
125  sag_phi_offset.push_back(temp2);
126  temp.clear();
127  temp2.clear();
128  straw_count=0;
129  ring_count++;
130  }
131  }
132  }
133 
134  // CDC Geometry
135  vector<double>cdc_origin;
136  vector<double>cdc_center;
137  vector<double>cdc_upstream_endplate_pos;
138  vector<double>cdc_endplate_dim;
139  geom->Get("//posXYZ[@volume='CentralDC'/@X_Y_Z",cdc_origin);
140  geom->Get("//posXYZ[@volume='centralDC']/@X_Y_Z",cdc_center);
141  geom->Get("//posXYZ[@volume='CDPU']/@X_Y_Z",cdc_upstream_endplate_pos);
142  geom->Get("//tubs[@name='CDPU']/@Rio_Z",cdc_endplate_dim);
143  cdc_origin[2]+=cdc_center[2]+cdc_upstream_endplate_pos[2]
144  +0.5*cdc_endplate_dim[2];
145  upstreamEndplate=cdc_origin[2];
146 
147  double endplate_dz=0.;
150  downstreamEndplate-=0.5*endplate_dz;
151 
152  // Outer detector geometry parameters
153  if (geom->GetDIRCZ(dDIRCz)==false) dDIRCz=1000.;
154  geom->GetFCALZ(dFCALz);
155  vector<double>tof_face;
156  geom->Get("//section/composition/posXYZ[@volume='ForwardTOF']/@X_Y_Z",
157  tof_face);
158  vector<double>tof_plane;
159  geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='0']", tof_plane);
160  dTOFz=tof_face[2]+tof_plane[2];
161  geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='1']", tof_plane);
162  dTOFz+=tof_face[2]+tof_plane[2];
163  dTOFz*=0.5; // mid plane between tof planes
164 
165  // Get start counter geometry;
166  if (geom->GetStartCounterGeom(sc_pos,sc_norm)){
167  // Create vector of direction vectors in scintillator planes
168  for (int i=0;i<30;i++){
169  vector<DVector3>temp;
170  for (unsigned int j=0;j<sc_pos[i].size()-1;j++){
171  double dx=sc_pos[i][j+1].x()-sc_pos[i][j].x();
172  double dy=sc_pos[i][j+1].y()-sc_pos[i][j].y();
173  double dz=sc_pos[i][j+1].z()-sc_pos[i][j].z();
174  temp.push_back(DVector3(dx/dz,dy/dz,1.));
175  }
176  sc_dir.push_back(temp);
177  }
178  SC_END_NOSE_Z=sc_pos[0][12].z();
179  SC_BARREL_R=sc_pos[0][0].Perp();
180  SC_PHI_SECTOR1=sc_pos[0][0].Phi();
181  }
182 
183 
184  //*************************
185  // Command-line parameters
186  //*************************
187  VERBOSE=0;
188  gPARMS->SetDefaultParameter("TRKFIT:VERBOSE",VERBOSE);
189  COSMICS=false;
190  gPARMS->SetDefaultParameter("TRKFIND:COSMICS",COSMICS);
191  CHI2CUT = 15.0;
192  gPARMS->SetDefaultParameter("TRKFIT:CHI2CUT",CHI2CUT);
193  DO_PRUNING = true;
194  gPARMS->SetDefaultParameter("TRKFIT:DO_PRUNING",DO_PRUNING);
195 }
196 
197 //---------------------------------
198 // ~DTrackFitterStraightTrack (Destructor)
199 //---------------------------------
201 {
202 
203 }
204 
205 //----------------------------------------------------
206 // Comparison routines for sorting
207 //----------------------------------------------------
209  const DCDCTrackHit *b){
210 
211  return(a->wire->origin.Y()>b->wire->origin.Y());
212 }
213 
215  const DCDCTrackHit *b){
216 
217  return(a->wire->origin.Y()<b->wire->origin.Y());
218 }
219 
221  const DCDCTrackHit *b){
222  if (a==NULL || b==NULL){
223  cout << "Null pointer in CDC hit list??" << endl;
224  return false;
225  }
226  const DCDCWire *wire_a= a->wire;
227  const DCDCWire *wire_b= b->wire;
228  if(wire_a->ring == wire_b->ring){
229  return wire_a->straw < wire_b->straw;
230  }
231 
232  return (wire_a->ring<wire_b->ring);
233 }
234 
236  const DFDCPseudo *b){
237 
238  return(a->wire->origin.z()<b->wire->origin.z());
239 }
240 
241 //----------------------------------------------------
242 // CDC fitting routines
243 //----------------------------------------------------
244 
245 // Smearing function derived from fitting residuals
246 inline double DTrackFitterStraightTrack::CDCDriftVariance(double t) const {
247  if (t<0.) t=0.;
248  double sigma=CDC_RES_PAR1/(t+1.)+CDC_RES_PAR2 + CDC_RES_PAR3*t;
249  return sigma*sigma;
250 }
251 
252 // Convert time to distance for the cdc
253 double DTrackFitterStraightTrack::CDCDriftDistance(double dphi,double delta,
254  double t) const{
255  double d=0.;
256  if (t>0){
257  double f_0=0.;
258  double f_delta=0.;
259 
260  if (delta>0){
261  double a1=long_drift_func[0][0];
262  double a2=long_drift_func[0][1];
263  double b1=long_drift_func[1][0];
264  double b2=long_drift_func[1][1];
265  double c1=long_drift_func[2][0];
266  double c2=long_drift_func[2][1];
267  double c3=long_drift_func[2][2];
268 
269  // use "long side" functional form
270  double my_t=0.001*t;
271  double sqrt_t=sqrt(my_t);
272  double t3=my_t*my_t*my_t;
273  double delta_mag=fabs(delta);
274  f_delta=(a1+a2*delta_mag)*sqrt_t+(b1+b2*delta_mag)*my_t
275  +(c1+c2*delta_mag+c3*delta*delta)*t3;
276  f_0=a1*sqrt_t+b1*my_t+c1*t3;
277  }
278  else{
279  double my_t=0.001*t;
280  double sqrt_t=sqrt(my_t);
281  double delta_mag=fabs(delta);
282 
283  // use "short side" functional form
284  double a1=short_drift_func[0][0];
285  double a2=short_drift_func[0][1];
286  double a3=short_drift_func[0][2];
287  double b1=short_drift_func[1][0];
288  double b2=short_drift_func[1][1];
289  double b3=short_drift_func[1][2];
290 
291  double delta_sq=delta*delta;
292  f_delta= (a1+a2*delta_mag+a3*delta_sq)*sqrt_t
293  +(b1+b2*delta_mag+b3*delta_sq)*my_t;
294  f_0=a1*sqrt_t+b1*my_t;
295  }
296 
297  unsigned int max_index=cdc_drift_table.size()-1;
298  if (t>cdc_drift_table[max_index]){
299  //_DBG_ << "t: " << t <<" d " << f_delta <<endl;
300  d=f_delta;
301 
302  return d;
303  }
304 
305  // Drift time is within range of table -- interpolate...
306  unsigned int index=0;
307  index=Locate(cdc_drift_table,t);
308  double dt=cdc_drift_table[index+1]-cdc_drift_table[index];
309  double frac=(t-cdc_drift_table[index])/dt;
310  double d_0=0.01*(double(index)+frac);
311 
312  double P=0.;
313  double tcut=250.0; // ns
314  if (t<tcut) {
315  P=(tcut-t)/tcut;
316  }
317  d=f_delta*(d_0/f_0*P+1.-P);
318  }
319  return d;
320 }
321 
322 // Perform the Kalman Filter for the current set of cdc hits
324  vector<int>&used_hits,
325  vector<cdc_update_t>&updates,
326  double &chi2,
327  int &ndof,
328  unsigned int iter){
329  DMatrix1x4 H; // Track projection matrix
330  DMatrix4x1 H_T; // Transpose of track projection matrix
331  DMatrix4x1 K; // Kalman gain matrix
332  DMatrix4x4 J; // Jacobian matrix
333  DMatrix4x1 S0; // State vector from reference trajectory
334  double V=1.15*(0.78*0.78/12.); // sigma=cell_size/sqrt(12.)*scale_factor
335 
336  const double d_EPS=1e-8;
337 
338  // Zero out the vector of used hit flags
339  for (unsigned int i=0;i<used_hits.size();i++) used_hits[i]=0;
340 
341  //Initialize chi2 and ndof
342  chi2=0.;
343  ndof=0;
344 
345  //Save the starting values for C and S
346  trajectory[0].Skk=S;
347  trajectory[0].Ckk=C;
348 
349  double doca2=0.;
350 
351  // CDC index and wire position variables
352  unsigned int cdc_index=cdchits.size()-1;
353  bool more_hits=true;
354  const DCDCWire *wire=cdchits[cdc_index]->wire;
355  DVector3 origin=wire->origin;
356  double z0=origin.z();
357  double vz=wire->udir.z();
358  if (VERBOSE) jout << " Starting in Ring " << wire->ring << endl;
359  DVector3 wdir=(1./vz)*wire->udir;
360  DVector3 wirepos=origin+(trajectory[0].z-z0)*wdir;
361 
362  /// compute initial doca^2 to first wire
363  double dx=S(state_x)-wirepos.X();
364  double dy=S(state_y)-wirepos.Y();
365  double old_doca2=dx*dx+dy*dy;
366 
367  // Loop over all steps in the trajectory
368  S0=trajectory[0].S;
369  J=trajectory[0].J;
370  for (unsigned int k=1;k<trajectory.size();k++){
371  if (!C.IsPosDef()) return UNRECOVERABLE_ERROR;
372 
373  // Propagate the state and covariance matrix forward in z
374  S=trajectory[k].S+J*(S-S0);
375  C=J*C*J.Transpose();
376 
377  // Save the current state and covariance matrix in the deque
378  trajectory[k].Skk=S;
379  trajectory[k].Ckk=C;
380 
381  // Save S and J for the next step
382  S0=trajectory[k].S;
383  J=trajectory[k].J;
384 
385  // Position along wire
386  wirepos=origin+(trajectory[k].z-z0)*wdir;
387 
388  // New doca^2
389  dx=S(state_x)-wirepos.X();
390  dy=S(state_y)-wirepos.Y();
391  doca2=dx*dx+dy*dy;
392  if (VERBOSE > 10) jout<< "At Position " << S(state_x) << " " << S(state_y) << " " << trajectory[k].z << " doca2 " << doca2 << endl;
393 
394  if (doca2>old_doca2 && more_hits){
395 
396  // zero-position and direction of line describing particle trajectory
397  double tx=S(state_tx),ty=S(state_ty);
398  DVector3 pos0(S(state_x),S(state_y),trajectory[k].z);
399  DVector3 tdir(tx,ty,1.);
400 
401  // Find the true doca to the wire
402  DVector3 diff=pos0-origin;
403  double dx0=diff.x(),dy0=diff.y();
404  double wdir_dot_diff=diff.Dot(wdir);
405  double tdir_dot_diff=diff.Dot(tdir);
406  double tdir_dot_wdir=tdir.Dot(wdir);
407  double tdir2=tdir.Mag2();
408  double wdir2=wdir.Mag2();
409  double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
410  double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
411  double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
412  double scale=1./D;
413  double s=scale*N;
414  double t=scale*N1;
415  diff+=s*tdir-t*wdir;
416  double d=diff.Mag()+d_EPS; // prevent division by zero
417 
418  // The next measurement and its variance
419  double tdrift=cdchits[cdc_index]->tdrift-trajectory[k].t;
420  double dmeas=0.39;
421  double delta=0.0;
422  if (fit_type==kTimeBased){
423  V=CDCDriftVariance(tdrift);
424 
425  double phi_d=diff.Phi();
426  double dphi=phi_d-origin.Phi();
427  while (dphi>M_PI) dphi-=2*M_PI;
428  while (dphi<-M_PI) dphi+=2*M_PI;
429 
430  int ring_index=cdchits[cdc_index]->wire->ring-1;
431  int straw_index=cdchits[cdc_index]->wire->straw-1;
432  double dz=t*wdir.z();
433  delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.)
434  *cos(phi_d+sag_phi_offset[ring_index][straw_index]);
435  dmeas=CDCDriftDistance(dphi,delta,tdrift);
436  }
437 
438  // residual
439  double res=dmeas-d;
440  if (VERBOSE>5) jout << " Residual " << res << endl;
441 
442  // Track projection
443 
444  double one_over_d=1./d;
445  double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
446  double wx=wdir.x(),wy=wdir.y();
447 
448  double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
449  double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
450  double dtdtx=scale*(dN1dtx-t*dDdtx);
451 
452  double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
453  double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
454  double dtdty=scale*(dN1dty-t*dDdty);
455 
456  double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
457  double dsdtx=scale*(dNdtx-s*dDdtx);
458 
459  double dNdty=wy*wdir_dot_diff-wdir2*dy0;
460  double dsdty=scale*(dNdty-s*dDdty);
461 
462  H(state_tx)=H_T(state_tx)
463  =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
464  +diffz*(dsdtx-dtdtx));
465  H(state_ty)=H_T(state_ty)
466  =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
467  +diffz*(dsdty-dtdty));
468 
469  double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx);
470  double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx);
471  double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
472  double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
473 
474  H(state_x)=H_T(state_x)
475  =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
476  +diffz*(dsdx-dtdx));
477  H(state_y)=H_T(state_y)
478  =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
479  +diffz*(dsdy-dtdy));
480 
481  double InvV=1./(V+H*C*H_T);
482 
483  // Check how far this hit is from the projection
484  double chi2check=res*res*InvV;
485  if (chi2check < CHI2CUT || DO_PRUNING == 0 || (COSMICS && iter == 0)){
486  if (VERBOSE>5) jout << "Hit Added to track " << endl;
487  if (cdchits[cdc_index]->wire->ring!=RING_TO_SKIP){
488 
489  // Compute Kalman gain matrix
490  K=InvV*(C*H_T);
491 
492  // Update state vector covariance matrix
493  DMatrix4x4 Ctest=C-K*(H*C);
494 
495  //C.Print();
496  //K.Print();
497  //Ctest.Print();
498 
499  // Check that Ctest is positive definite
500  if (!Ctest.IsPosDef()) return VALUE_OUT_OF_RANGE;
501  C=Ctest;
502  if(VERBOSE>10) C.Print();
503  // Update the state vector
504  //S=S+res*K;
505  S+=res*K;
506  if(VERBOSE>10) S.Print();
507 
508  // Compute new residual
509  //d=finder->FindDoca(trajectory[k].z,S,wdir,origin);
510  res=res-H*K*res;
511 
512  // Update chi2
513  double fit_V=V-H*C*H_T;
514  chi2+=res*res/fit_V;
515  ndof++;
516 
517  // fill pull vector entry
518  updates[cdc_index].V=fit_V;
519  }
520  else {
521  updates[cdc_index].V=V;
522  }
523 
524  // Flag that we used this hit
525  used_hits[cdc_index]=1;
526 
527  // fill updates
528  updates[cdc_index].resi=res;
529  updates[cdc_index].d=d;
530  updates[cdc_index].delta=delta;
531  updates[cdc_index].S=S;
532  updates[cdc_index].C=C;
533  updates[cdc_index].tdrift=tdrift;
534  updates[cdc_index].ddrift=dmeas;
535  updates[cdc_index].s=29.98*trajectory[k].t; // assume beta=1
536  trajectory[k].id=cdc_index+1;
537  }
538  // move to next cdc hit
539  if (cdc_index>0){
540  cdc_index--;
541 
542  //New wire position
543  wire=cdchits[cdc_index]->wire;
544  if (VERBOSE>5) {
545  jout << " Next Wire ring " << wire->ring << " straw " << wire->straw << " origin udir" << endl;
546  wire->origin.Print(); wire->udir.Print();
547  }
548  origin=wire->origin;
549  z0=origin.z();
550  vz=wire->udir.z();
551  wdir=(1./vz)*wire->udir;
552  wirepos=origin+((trajectory[k].z-z0))*wdir;
553 
554  // New doca^2
555  dx=S(state_x)-wirepos.x();
556  dy=S(state_y)-wirepos.y();
557  doca2=dx*dx+dy*dy;
558 
559  }
560  else more_hits=false;
561  }
562 
563  old_doca2=doca2;
564  }
565  if (ndof<=4) return VALUE_OUT_OF_RANGE;
566 
567  ndof-=4;
568 
569  return NOERROR;
570 }
571 
572 // Smooth the CDC only tracks
573 jerror_t DTrackFitterStraightTrack::Smooth(vector<cdc_update_t>&cdc_updates){
574  unsigned int max=best_trajectory.size()-1;
576  DMatrix4x4 C=(best_trajectory[max].Ckk);
577  DMatrix4x4 JT=best_trajectory[max].J.Transpose();
578  DMatrix4x1 Ss=S;
579  DMatrix4x4 Cs=C;
580  DMatrix4x4 A,dC;
581  DMatrix1x4 H; // Track projection matrix
582  DMatrix4x1 H_T; // Transpose of track projection matrix
583 
584  const double d_EPS=1e-8;
585 
586  for (unsigned int m=max-1;m>0;m--){
587  if (best_trajectory[m].id>0){
588  unsigned int id=best_trajectory[m].id-1;
589  A=cdc_updates[id].C*JT*C.Invert();
590  Ss=cdc_updates[id].S+A*(Ss-S);
591 
592  dC=A*(Cs-C)*A.Transpose();
593  Cs=cdc_updates[id].C+dC;
594  if (VERBOSE > 10) {
595  jout << " In Smoothing Step Using ID " << id << "/" << cdc_updates.size() << " for ring " << cdchits[id]->wire->ring << endl;
596  jout << " A cdc_updates[id].C Ss Cs " << endl;
597  A.Print(); cdc_updates[id].C.Print(); Ss.Print(); Cs.Print();
598  }
599  if(!Cs.IsPosDef()) {
600  if (VERBOSE) jout << "Cs is not PosDef!" << endl;
601  return VALUE_OUT_OF_RANGE;
602  }
603 
604  const DCDCWire *wire=cdchits[id]->wire;
605  DVector3 origin=wire->origin;
606  double z0=origin.z();
607  double vz=wire->udir.z();
608  DVector3 wdir=(1./vz)*wire->udir;
609  DVector3 wirepos=origin+(best_trajectory[m].z-z0)*wdir;
610  // Position and direction from state vector
611  double x=Ss(state_x);
612  double y=Ss(state_y);
613  double tx=Ss(state_tx);
614  double ty=Ss(state_ty);
615 
616  DVector3 pos0(x,y,best_trajectory[m].z);
617  DVector3 tdir(tx,ty,1.);
618 
619  // Find the true doca to the wire
620  DVector3 diff=pos0-origin;
621  double dx0=diff.x(),dy0=diff.y();
622  double wdir_dot_diff=diff.Dot(wdir);
623  double tdir_dot_diff=diff.Dot(tdir);
624  double tdir_dot_wdir=tdir.Dot(wdir);
625  double tdir2=tdir.Mag2();
626  double wdir2=wdir.Mag2();
627  double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
628  double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
629  double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
630  double scale=1./D;
631  double s=scale*N;
632  double t=scale*N1;
633  diff+=s*tdir-t*wdir;
634  double d=diff.Mag()+d_EPS; // prevent division by zero
635  double ddrift = cdc_updates[id].ddrift;
636 
637  double resi = ddrift - d;
638  // Track projection
639 
640  {
641  double one_over_d=1./d;
642  double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
643  double wx=wdir.x(),wy=wdir.y();
644 
645  double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
646  double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
647  double dtdtx=scale*(dN1dtx-t*dDdtx);
648 
649  double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
650  double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
651  double dtdty=scale*(dN1dty-t*dDdty);
652 
653  double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
654  double dsdtx=scale*(dNdtx-s*dDdtx);
655 
656  double dNdty=wy*wdir_dot_diff-wdir2*dy0;
657  double dsdty=scale*(dNdty-s*dDdty);
658 
659  H(state_tx)=H_T(state_tx)
660  =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
661  +diffz*(dsdtx-dtdtx));
662  H(state_ty)=H_T(state_ty)
663  =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
664  +diffz*(dsdty-dtdty));
665 
666  double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx);
667  double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx);
668  double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
669  double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
670 
671  H(state_x)=H_T(state_x)
672  =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
673  +diffz*(dsdx-dtdx));
674  H(state_y)=H_T(state_y)
675  =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
676  +diffz*(dsdy-dtdy));
677  }
678  double V=cdc_updates[id].V;
679 
680  if (VERBOSE > 10) jout << " d " << d << " H*S " << H*S << endl;
681  if (cdchits[id]->wire->ring==RING_TO_SKIP){
682  V=V+H*Cs*H_T;
683  }
684  else{
685  V=V-H*Cs*H_T;
686  }
687  if (V<0) return VALUE_OUT_OF_RANGE;
688 
689  // Add the pull
690  double myscale=1./sqrt(1.+tx*tx+ty*ty);
691  double cosThetaRel=wire->udir.Dot(DVector3(myscale*tx,myscale*ty,myscale));
692  pull_t thisPull(resi,sqrt(V),
694  cdc_updates[id].tdrift,
695  d,cdchits[id], NULL,
696  diff.Phi(), //docaphi
697  best_trajectory[m].z,cosThetaRel,
698  cdc_updates[id].tdrift);
699 
700  // Derivatives for alignment
701  double wtx=wire->udir.X(), wty=wire->udir.Y(), wtz=wire->udir.Z();
702  double wx=wire->origin.X(), wy=wire->origin.Y(), wz=wire->origin.Z();
703 
704  double z=best_trajectory[m].z;
705  double tx2=tx*tx, ty2=ty*ty;
706  double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz;
707  double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz) +d_EPS;
708  double denom2=denom*denom;
709  double c1=-(wtx - tx*wtz)*(wy - y);
710  double c2=wty*(wx - tx*wz - x + tx*z);
711  double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z);
712  double dscale=0.5*(1./d);
713 
714  vector<double> derivatives(11);
715 
716  derivatives[CDCTrackD::dDOCAdOriginX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3))/denom;
717 
718  derivatives[CDCTrackD::dDOCAdOriginY]=dscale*(2*(-wtx + tx*wtz)*(c1 + c2 + c3))/denom;
719 
720  derivatives[CDCTrackD::dDOCAdOriginZ]=dscale*(2*(ty*wtx - tx*wty)*(c1 + c2 + c3))/denom;
721 
722  derivatives[CDCTrackD::dDOCAdDirX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3)*
723  (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
724  wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2;
725 
726  derivatives[CDCTrackD::dDOCAdDirY]=dscale*(-2*(wtx - tx*wtz)*(c1 + c2 + c3)*
727  (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
728  wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2;
729 
730  derivatives[CDCTrackD::dDOCAdDirZ]=dscale*(-2*(ty*wtx - tx*wty)*(c1 + c2 + c3)*
731  (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) +
732  wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2;
733 
734  derivatives[CDCTrackD::dDOCAdS0]=-derivatives[CDCTrackD::dDOCAdOriginX];
735 
736  derivatives[CDCTrackD::dDOCAdS1]=-derivatives[CDCTrackD::dDOCAdOriginY];
737 
738  derivatives[CDCTrackD::dDOCAdS2]=dscale*(2*(wty - ty*wtz)*(-c1 - c2 - c3)*
739  (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
740  tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
741  ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
742 
743  derivatives[CDCTrackD::dDOCAdS3]=dscale*(2*(wtx - tx*wtz)*(c1 + c2 + c3)*
744  (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
745  tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
746  ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
747 
748  thisPull.AddTrackDerivatives(derivatives);
749  pulls.push_back(thisPull);
750  }
751  else{
752  A=best_trajectory[m].Ckk*JT*C.Invert();
753  Ss=best_trajectory[m].Skk+A*(Ss-S);
754  Cs=best_trajectory[m].Ckk+A*(Cs-C)*A.Transpose();
755  }
756 
757  S=best_trajectory[m].Skk;
758  C=best_trajectory[m].Ckk;
759  JT=best_trajectory[m].J.Transpose();
760  }
761 
762  return NOERROR;
763 }
764 
765 //Reference trajectory for the track for cdc tracks
767  DMatrix4x1 &S,
768  const DCDCTrackHit *last_cdc,
769  double &dzsign){
770  DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
771 
772  double ds=0.5;
773  double t=t0;
774 
775  // last y position of hit (approximate, using center of wire)
776  double last_y=last_cdc->wire->origin.y();
777  double last_r2=last_cdc->wire->origin.Perp2();
778  // Check that track points towards last wire, otherwise swap deltaz
779  DVector3 dir(S(state_tx),S(state_ty),dzsign);
780  if (fabs(dir.Theta() - M_PI_2) < 0.2) ds = 0.1;
781 
782  //jout << "dPhi " << dphi << " theta " << dir.Theta() << endl;
783  double dz=dzsign*ds/sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
784 
785  if (VERBOSE) {
786  if (COSMICS) jout << " Swimming Reference Trajectory last CDC y " << last_y << " dz "<< dz << endl;
787  else jout << " Swimming Reference Trajectory last CDC r2 " << last_r2 << " dz "<< dz << endl;
788  jout << " S" << endl; S.Print(); jout << "z= "<< z << endl;
789  jout << " Last CDC ring " << last_cdc->wire->ring << " straw " << last_cdc->wire->straw << endl;
790  }
791  unsigned int numsteps=0;
792  const unsigned int MAX_STEPS=5000;
793  bool done=false;
794  do{
795  numsteps++;
796  z+=dz;
797  J(state_x,state_tx)=-dz;
798  J(state_y,state_ty)=-dz;
799  // Flight time: assume particle is moving at the speed of light
800  t+=ds/29.98;
801  //propagate the state to the next z position
802  S(state_x)+=S(state_tx)*dz;
803  S(state_y)+=S(state_ty)*dz;
804 
805  if (z > downstreamEndplate && dz < 0) continue;
806  if (z < upstreamEndplate && dz > 0) continue;
807  trajectory.push_front(trajectory_t(z,t,S,J,DMatrix4x1(),DMatrix4x4()));
808 
809  done=((z>downstreamEndplate) || (z<upstreamEndplate));
810  if (COSMICS==false){
811  double r2=S(state_x)*S(state_x)+S(state_y)*S(state_y);
812  done |= (r2>last_r2);
813  }
814  }while (!done && numsteps<MAX_STEPS);
815 
816  if (VERBOSE)
817  {
818  if (VERBOSE > 10){
819  printf("Trajectory:\n");
820  for (unsigned int i=0;i<trajectory.size();i++){
821  printf(" x %f y %f z %f\n",trajectory[i].S(state_x),
822  trajectory[i].S(state_y),trajectory[i].z);
823  }
824  }
825  else{
826  printf("%i step trajectory Begin/End:\n", numsteps);
827  printf(" x %f y %f z %f\n",trajectory[0].S(state_x), trajectory[0].S(state_y), trajectory[0].z);
828  if (trajectory.size() > 1) printf(" x %f y %f z %f\n",trajectory[trajectory.size()-1].S(state_x),
829  trajectory[trajectory.size()-1].S(state_y),trajectory[trajectory.size()-1].z);
830  }
831  }
832  if (trajectory.size()<2) return UNRECOVERABLE_ERROR;
833  return NOERROR;
834 }
835 
836 // Routine for steering the fit of the track
839  if (cdchits.size()+fdchits.size() < 4) return status;
840 
841  // Initial guess for state vector
842  DVector3 input_pos=input_params.position();
843  DVector3 input_mom=input_params.momentum();
844  double pz=input_mom.z();
845  DMatrix4x1 S(input_pos.x(),input_pos.y(),input_mom.x()/pz,input_mom.y()/pz);
846 
847  // Initial guess for covariance matrix
848  DMatrix4x4 C;
849  if (fit_type==kWireBased){
850  C(state_x,state_x)=C(state_y,state_y)=4.0;
852  }
853  else{
854  C(state_x,state_x)=C(state_y,state_y)=1.;
855  C(state_tx,state_tx)=C(state_ty,state_ty)=0.01;
856  }
857 
858  // Starting z-position and time
859  double z=input_pos.z();
860  double t0=input_params.t0();
861 
862  // Chisq and ndof
863  chisq=1e16;
864  Ndof=0;
865 
866  // Sort the CDC hits by radius or y (for cosmics)
867  if (cdchits.size()>0){
868  if (COSMICS){
869  stable_sort(cdchits.begin(),cdchits.end(),DTrackFitterStraightTrack_cdc_hit_cmp);
870  }
871  else{
873  }
874  }
875 
876  if (fdchits.size()>0){
877  // Sort FDC hits by z
878  stable_sort(fdchits.begin(),fdchits.end(),DTrackFitterStraightTrack_fdc_hit_cmp);
879  status=FitForwardTrack(t0,z,S,C,chisq,Ndof);
880  }
881  else if (cdchits.size()>0){
882  double dzsign=(pz>0)?1.:-1.;
883  status=FitCentralTrack(z,t0,dzsign,S,C,chisq,Ndof);
884  }
885 
886  if (status==DTrackFitter::kFitSuccess){
887  // Output fit results
888  double tx=S(state_tx),ty=S(state_ty);
889  double phi=atan2(ty,tx);
890  double tanl=1./sqrt(tx*tx+ty*ty);
891  // Check for tracks heading upstream
892  if (cdchits_used_in_fit.size()>0){
893  double phi_diff=phi-cdchits_used_in_fit[0]->wire->origin.Phi()-M_PI;
894  if (phi_diff<-M_PI) phi_diff+=2.*M_PI;
895  if (phi_diff> M_PI) phi_diff-=2.*M_PI;
896  if (fabs(phi_diff)<M_PI_2){
897  tanl*=-1;
898  phi+=M_PI;
899  }
900  }
901  double pt=5.0*cos(atan(tanl)); // arbitrary magnitude...
902  DVector3 mom(pt*cos(phi),pt*sin(phi),pt*tanl);
903 
904  // Convert 4x4 covariance matrix to a TMatrixFSym for output
905  TMatrixFSym errMatrix(5);
906  for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){
907  for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){
908  errMatrix(loc_i, loc_j) = C(loc_i, loc_j);
909  }
910  }
911  errMatrix(4,4)=1.;
912 
913  // Add 7x7 covariance matrix to output
914  double sign=(mom.Theta()>M_PI_2)?-1.:1.;
915  fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix,S,sign));
916 
917  DVector3 pos;
918  if (COSMICS==false){
919  DVector3 beamdir(0.,0.,1.);
920  DVector3 beampos(0.,0.,65.);
921  finder->FindDoca(z,S,beamdir,beampos,&pos);
922 
923  // Jacobian matrix
924  double dz=pos.z()-z;
925  DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
926  J(state_x,state_tx)=dz;
927  J(state_y,state_ty)=dz;
928  // Transform the matrix to the position of the doca
929  C=J*C*J.Transpose();
930  }
931  else{
932  pos.SetXYZ(S(state_x),S(state_y),z);
933  }
934 
936 
937  fit_params.setPosition(pos);
938  fit_params.setMomentum(mom);
939 
940  // Add tracking covariance matrix to output
941  auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource();
942  locTrackingCovarianceMatrix->ResizeTo(5, 5);
943  locTrackingCovarianceMatrix->Zero();
944  for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){
945  for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){
946  (*locTrackingCovarianceMatrix)(loc_i, loc_j) = C(loc_i, loc_j);
947  }
948  }
949  (*locTrackingCovarianceMatrix)(4,4)=1.;
950  fit_params.setTrackingErrorMatrix(locTrackingCovarianceMatrix);
951 
952  // Get extrapolations to other detectors
953  GetExtrapolations(pos,mom);
954 
955  }
956 
957  return status;
958 }
959 
960 // Locate a position in vector xx given x
961 unsigned int DTrackFitterStraightTrack::Locate(const vector<double>&xx,double x) const{
962  int n=xx.size();
963  if (x==xx[0]) return 0;
964  else if (x==xx[n-1]) return n-2;
965 
966  int jl=-1;
967  int ju=n;
968  int ascnd=(xx[n-1]>=xx[0]);
969  while(ju-jl>1){
970  int jm=(ju+jl)>>1;
971  if ( (x>=xx[jm])==ascnd)
972  jl=jm;
973  else
974  ju=jm;
975  }
976  return jl;
977 }
978 
979 // Steering routine for fitting CDC-only tracks
981  double dzsign,
982  DMatrix4x1 &Sbest,
983  DMatrix4x4 &Cbest,
984  double &chi2_best,
985  int &ndof_best){
986  // State vector and covariance matrix
987  DMatrix4x1 S(Sbest);
988  DMatrix4x4 C(Cbest),C0(Cbest);
989 
990  double chi2=chi2_best;
991  int ndof=ndof_best;
992 
993  unsigned int numhits=cdchits.size();
994  unsigned int maxindex=numhits-1;
995 
996  // vectors of indexes to cdc hits used in the fit
997  vector<int> used_cdc_hits(numhits);
998  vector<int> used_cdc_hits_best_fit(numhits);
999 
1000  // vectors of residual information
1001  vector<cdc_update_t>updates(numhits);
1002  vector<cdc_update_t>best_updates(numhits);
1003 
1004  // Rest deque for "best" trajectory
1005  best_trajectory.clear();
1006 
1007  // Perform the fit
1008  int iter=0;
1009  for(iter=0;iter<20;iter++){
1010  if (VERBOSE) jout << " Performing Pass iter " << iter << endl;
1011 
1012  trajectory.clear();
1013  if (SetReferenceTrajectory(t0,z0,S,cdchits[maxindex],dzsign)
1014  !=NOERROR) break;
1015 
1016  if (VERBOSE) jout << " Reference Trajectory Set " << endl;
1017  C=C0;
1018  if (KalmanFilter(S,C,used_cdc_hits,updates,chi2,ndof,iter)!=NOERROR) break;
1019  if (VERBOSE) jout << " Filter returns NOERROR" << endl;
1020  if (iter>0 && (fabs(chi2_best-chi2)<0.1 || chi2>chi2_best)) break;
1021 
1022  // Save the current state and covariance matrixes
1023  Cbest=C;
1024  Sbest=S;
1025 
1026  // Save the current used hit and trajectory information
1027  best_trajectory.assign(trajectory.begin(),trajectory.end());
1028  used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end());
1029  best_updates.assign(updates.begin(),updates.end());
1030 
1031  chi2_best=chi2;
1032  ndof_best=ndof;
1033  }
1034  if (iter==0) return DTrackFitter::kFitFailed;
1035 
1036  //Final z position (closest to beam line)
1037  z0=best_trajectory[best_trajectory.size()-1].z;
1038 
1039  //Run the smoother
1040  if (Smooth(best_updates) == NOERROR) IsSmoothed=true;
1041 
1042  // output list of cdc hits used in the fit
1043  cdchits_used_in_fit.clear();
1044  for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){
1045  if (used_cdc_hits_best_fit[m]){
1046  cdchits_used_in_fit.push_back(cdchits[m]);
1047  }
1048  }
1049 
1051 }
1052 
1053 
1054 //----------------------------------------------------
1055 // FDC fitting routines
1056 //----------------------------------------------------
1057 
1058 // parametrization of time-to-distance for FDC
1060  if (time<0.) return 0.;
1061  double d=0.;
1062  double tsq=time*time;
1063  double t_high=DRIFT_FUNC_PARMS[4];
1064 
1065  if (time<t_high){
1066  d=DRIFT_FUNC_PARMS[0]*sqrt(time)+DRIFT_FUNC_PARMS[1]*time
1067  +DRIFT_FUNC_PARMS[2]*tsq+DRIFT_FUNC_PARMS[3]*tsq*time;
1068  }
1069  else{
1070  double t_high_sq=t_high*t_high;
1071  d=DRIFT_FUNC_PARMS[0]*sqrt(t_high)+DRIFT_FUNC_PARMS[1]*t_high
1072  +DRIFT_FUNC_PARMS[2]*t_high_sq+DRIFT_FUNC_PARMS[3]*t_high_sq*t_high;
1073  d+=DRIFT_FUNC_PARMS[5]*(time-t_high);
1074  }
1075 
1076  return d;
1077 
1078 }
1079 
1080 // Crude approximation for the variance in drift distance due to smearing
1082  //return FDC_ANODE_VARIANCE;
1083  if (t<5.) t=5.;
1084  double sigma=DRIFT_RES_PARMS[0]/(t+1.)+DRIFT_RES_PARMS[1]+DRIFT_RES_PARMS[2]*t*t;
1085 
1086  return sigma*sigma;
1087 }
1088 
1089 // Steering routine for the kalman filter
1092  DMatrix4x1 &Sbest,DMatrix4x4 &Cbest,double &chi2_best,int &ndof_best){
1093  // State vector and covariance matrix
1094  DMatrix4x1 S(Sbest);
1095  DMatrix4x4 C(Cbest),C0(Cbest);
1096 
1097  // vectors of indexes to fdc hits used in the fit
1098  unsigned int numfdchits=fdchits.size();
1099  vector<int> used_fdc_hits(numfdchits);
1100  vector<int> used_fdc_hits_best_fit(numfdchits);
1101  // vectors of indexes to cdc hits used in the fit
1102  unsigned int numcdchits=cdchits.size();
1103  vector<int> used_cdc_hits(numcdchits);
1104  vector<int> used_cdc_hits_best_fit(numcdchits);
1105 
1106  // vectors of residual information
1107  vector<fdc_update_t>updates(numfdchits);
1108  vector<fdc_update_t>best_updates(numfdchits);
1109  vector<cdc_update_t>cdc_updates(numcdchits);
1110  vector<cdc_update_t>best_cdc_updates(numcdchits);
1111 
1112  vector<const DCDCTrackHit *> matchedCDCHits;
1113 
1114  // Chi-squared and degrees of freedom
1115  double chi2=chi2_best;
1116  int ndof=ndof_best;
1117 
1118  // Rest deque for "best" trajectory
1119  best_trajectory.clear();
1120 
1121  unsigned iter=0;
1122  // First pass
1123  for(iter=0;iter<20;iter++){
1124  trajectory.clear();
1125  if (SetReferenceTrajectory(t0,start_z,S)!=NOERROR) break;
1126 
1127  C=C0;
1128  if (KalmanFilter(S,C,used_fdc_hits,used_cdc_hits,updates,cdc_updates,
1129  chi2,ndof)!=NOERROR) break;
1130 
1131  // printf(" == iter %d =====chi2 %f ndof %d \n",iter,chi2,ndof);
1132  if (iter>0 && (chi2>chi2_best || fabs(chi2_best-chi2)<0.1)) break;
1133 
1134  // Save the current state and covariance matrixes
1135  Cbest=C;
1136  Sbest=S;
1137 
1138  // Save the current used hit and trajectory information
1139  best_trajectory.assign(trajectory.begin(),trajectory.end());
1140  used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end());
1141  used_fdc_hits_best_fit.assign(used_fdc_hits.begin(),used_fdc_hits.end());
1142  best_updates.assign(updates.begin(),updates.end());
1143  best_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end());
1144 
1145  chi2_best=chi2;
1146  ndof_best=ndof;
1147  }
1148  if (iter==0) return DTrackFitter::kFitFailed;
1149 
1150  //Final z position (closest to beam line)
1151  start_z=best_trajectory[best_trajectory.size()-1].z;
1152 
1153  //Run the smoother
1154  if (Smooth(best_updates,best_cdc_updates) == NOERROR) IsSmoothed=true;
1155 
1156  // output list of hits used in the fit
1157  cdchits_used_in_fit.clear();
1158  for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){
1159  if (used_cdc_hits_best_fit[m]){
1160  cdchits_used_in_fit.push_back(cdchits[m]);
1161  }
1162  }
1163  fdchits_used_in_fit.clear();
1164  for (unsigned int m=0;m<used_fdc_hits_best_fit.size();m++){
1165  if (used_fdc_hits_best_fit[m]){
1166  fdchits_used_in_fit.push_back(fdchits[m]);
1167  }
1168  }
1169 
1170 
1172 }
1173 
1174 
1175 // Reference trajectory for the track
1176 jerror_t
1178  DMatrix4x1 &S){
1179  const double EPS=1e-3;
1180 
1181  // Jacobian matrix
1182  DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
1183 
1184  double dz=1.1;
1185  double t=t0;
1186  trajectory.push_front(trajectory_t(z,t,S,J,DMatrix4x1(),DMatrix4x4()));
1187 
1188  double zhit=z;
1189  double old_zhit=z;
1190  for (unsigned int i=0;i<fdchits.size();i++){
1191  zhit=fdchits[i]->wire->origin.z();
1192  dz=1.1;
1193 
1194  if (fabs(zhit-old_zhit)<EPS && i>0){
1195  trajectory[0].numhits++;
1196  continue;
1197  }
1198  // propagate until we would step beyond the FDC hit plane
1199  bool done=false;
1200  while (!done){
1201  double new_z=z+dz;
1202 
1203  if (new_z>zhit){
1204  dz=zhit-z;
1205  new_z=zhit;
1206  done=true;
1207  }
1208  J(state_x,state_tx)=-dz;
1209  J(state_y,state_ty)=-dz;
1210  // Flight time: assume particle is moving at the speed of light
1211  t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98;
1212  //propagate the state to the next z position
1213  S(state_x)+=S(state_tx)*dz;
1214  S(state_y)+=S(state_ty)*dz;
1215 
1216  trajectory.push_front(trajectory_t(new_z,t,S,J,DMatrix4x1(),
1217  DMatrix4x4()));
1218  if (done){
1219  trajectory[0].id=i+1;
1220  trajectory[0].numhits=1;
1221  }
1222 
1223  z=new_z;
1224  }
1225  old_zhit=zhit;
1226  }
1227  // One last step
1228  dz=1.1;
1229  J(state_x,state_tx)=-dz;
1230  J(state_y,state_ty)=-dz;
1231 
1232  // Flight time: assume particle is moving at the speed of light
1233  t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98;
1234 
1235  //propagate the state to the next z position
1236  S(state_x)+=S(state_tx)*dz;
1237  S(state_y)+=S(state_ty)*dz;
1238  trajectory.push_front(trajectory_t(z+dz,t,S,J,DMatrix4x1(),DMatrix4x4()));
1239 
1240  if (false)
1241  {
1242  printf("Trajectory:\n");
1243  for (unsigned int i=0;i<trajectory.size();i++){
1244  printf(" x %f y %f z %f first hit %d num in layer %d\n",trajectory[i].S(state_x),
1245  trajectory[i].S(state_y),trajectory[i].z,trajectory[i].id,
1246  trajectory[i].numhits);
1247  }
1248  }
1249 
1250  return NOERROR;
1251 }
1252 
1253 // Perform Kalman Filter for the current trajectory
1255  vector<int>&used_fdc_hits,
1256  vector<int>&used_cdc_hits,
1257  vector<fdc_update_t>&updates,
1258  vector<cdc_update_t>&cdc_updates,
1259  double &chi2,int &ndof){
1260  DMatrix2x4 H; // Track projection matrix
1261  DMatrix4x2 H_T; // Transpose of track projection matrix
1262  DMatrix4x2 K; // Kalman gain matrix
1263  DMatrix2x2 V(0.0833,0.,0.,0.000256); // Measurement variance
1264  DMatrix2x2 Vtemp,InvV;
1265  DMatrix2x1 Mdiff;
1266  DMatrix4x4 I; // identity matrix
1267  DMatrix4x4 J; // Jacobian matrix
1268  DMatrix4x1 S0; // State vector from reference trajectory
1269 
1270  DMatrix1x4 H_CDC; // Track projection matrix
1271  DMatrix4x1 H_T_CDC; // Transpose of track projection matrix
1272  DMatrix4x1 K_CDC; // Kalman gain matrix
1273  double V_CDC;
1274 
1275  const double d_EPS=1e-8;
1276 
1277  // Zero out the vectors of used hit flags
1278  for (unsigned int i=0;i<used_fdc_hits.size();i++) used_fdc_hits[i]=0;
1279  for (unsigned int i=0;i<used_cdc_hits.size();i++) used_cdc_hits[i]=0;
1280 
1281  //Initialize chi2 and ndof
1282  chi2=0.;
1283  ndof=0;
1284 
1285  // Save the starting values for C and S in the deque
1286  trajectory[0].Skk=S;
1287  trajectory[0].Ckk=C;
1288 
1289  // Loop over all steps in the trajectory
1290  S0=trajectory[0].S;
1291  J=trajectory[0].J;
1292 
1293  // CDC index and wire position variables
1294  bool more_hits = cdchits.size() == 0 ? false: true;
1295  bool firstCDCStep=true;
1296  unsigned int cdc_index=0;
1297  const DCDCWire *wire;
1298  DVector3 origin,wdir,wirepos;
1299  double doca2=0.0, old_doca2=0.0;
1300  if(more_hits){
1301  cdc_index=cdchits.size()-1;
1302  wire=cdchits[cdc_index]->wire;
1303  origin=wire->origin;
1304  double vz=wire->udir.z();
1305  if (VERBOSE) jout << " Additional CDC Hits in FDC track Starting in Ring " << wire->ring << endl;
1306  wdir=(1./vz)*wire->udir;
1307  }
1308 
1309  for (unsigned int k=1;k<trajectory.size();k++){
1310  if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.)
1311  return UNRECOVERABLE_ERROR;
1312 
1313  // Propagate the state and covariance matrix forward in z
1314  S=trajectory[k].S+J*(S-S0);
1315  C=J*C*J.Transpose();
1316 
1317  // Save the current state and covariance matrix in the deque
1318  trajectory[k].Skk=S;
1319  trajectory[k].Ckk=C;
1320 
1321  // Save S and J for the next step
1322  S0=trajectory[k].S;
1323  J=trajectory[k].J;
1324 
1325  // Correct S and C for the hit
1326  if (trajectory[k].id>0){
1327  unsigned int id=trajectory[k].id-1;
1328 
1329  double cospsi=cos(fdchits[id]->wire->angle);
1330  double sinpsi=sin(fdchits[id]->wire->angle);
1331 
1332  // Small angle alignment correction
1333  double x = S(state_x) + fdchits[id]->wire->angles.Z()*S(state_y);
1334  double y = S(state_y) - fdchits[id]->wire->angles.Z()*S(state_x);
1335  //tz = 1. + my_fdchits[id]->phiY*tx - my_fdchits[id]->phiX*ty;
1336  double tx = (S(state_tx) + fdchits[id]->wire->angles.Z()*S(state_ty) - fdchits[id]->wire->angles.Y());
1337  double ty = (S(state_ty) - fdchits[id]->wire->angles.Z()*S(state_tx) + fdchits[id]->wire->angles.X());
1338 
1339  if (std::isnan(x) || std::isnan(y)) return UNRECOVERABLE_ERROR;
1340 
1341  // x,y and tx,ty in local coordinate system
1342  // To transform from (x,y) to (u,v), need to do a rotation:
1343  // u = x*cos(psi)-y*sin(psi)
1344  // v = y*cos(psi)+x*sin(psi)
1345  // (without alignment offsets)
1346  double vpred_wire_plane=y*cospsi+x*sinpsi;
1347  double upred_wire_plane=x*cospsi-y*sinpsi;
1348  double tu=tx*cospsi-ty*sinpsi;
1349  double tv=tx*sinpsi+ty*cospsi;
1350 
1351  // Variables for angle of incidence with respect to the z-direction in
1352  // the u-z plane
1353  double alpha=atan(tu);
1354  double cosalpha=cos(alpha);
1355  double cos2_alpha=cosalpha*cosalpha;
1356  double sinalpha=sin(alpha);
1357  double sin2_alpha=sinalpha*sinalpha;
1358  double cos2_alpha_minus_sin2_alpha=cos2_alpha-sin2_alpha;
1359 
1360  // Difference between measurement and projection
1361  for (int m=trajectory[k].numhits-1;m>=0;m--){
1362  unsigned int my_id=id+m;
1363  double uwire=fdchits[my_id]->w;
1364  // (signed) distance of closest approach to wire
1365  double du=upred_wire_plane-uwire;
1366  double doca=du*cosalpha;
1367 
1368  // Predicted avalanche position along the wire
1369  double vpred=vpred_wire_plane;
1370 
1371  // Measured position of hit along wire
1372  double v=fdchits[my_id]->s;
1373 
1374  // Difference between measurements and predictions
1375  double drift=0.; // assume hit at wire position
1376  if (fit_type==kTimeBased){
1377  double drift_time=fdchits[my_id]->time-trajectory[k].t;
1378  drift=(du>0.0?1.:-1.)*fdc_drift_distance(drift_time);
1379 
1380  V(0,0)=fdc_drift_variance(drift_time);
1381  }
1382  Mdiff(0)=drift-doca;
1383  Mdiff(1)=v-vpred;
1384 
1385  // Matrix for transforming from state-vector space to measurement space
1386  H_T(state_x,0)=cospsi*cosalpha;
1387  H_T(state_y,0)=-sinpsi*cosalpha;
1388  double temp=-du*sinalpha*cos2_alpha;
1389  H_T(state_tx,0)=cospsi*temp;
1390  H_T(state_ty,0)=-sinpsi*temp;
1391  double temp2=cosalpha*sinalpha*tv;
1392  H_T(state_x,1)=sinpsi-temp2*cospsi;
1393  H_T(state_y,1)=cospsi+temp2*sinpsi;
1394  double temp4=sinalpha*doca;
1395  double temp5=tv*cos2_alpha*du*cos2_alpha_minus_sin2_alpha;
1396  H_T(state_tx,1)=-sinpsi*temp4-cospsi*temp5;
1397  H_T(state_ty,1)=-cospsi*temp4+sinpsi*temp5;
1398 
1399  // Matrix transpose H_T -> H
1400  H(0,state_x)=H_T(state_x,0);
1401  H(0,state_y)=H_T(state_y,0);
1402  H(0,state_tx)=H_T(state_tx,0);
1403  H(0,state_ty)=H_T(state_ty,0);
1404  H(1,state_x)=H_T(state_x,1);
1405  H(1,state_y)=H_T(state_y,1);
1406  H(1,state_tx)=H_T(state_tx,1);
1407  H(1,state_ty)=H_T(state_ty,1);
1408 
1409  // Variance for this hit
1410  InvV=(V+H*C*H_T).Invert();
1411 
1412  // Compute Kalman gain matrix
1413  K=(C*H_T)*InvV;
1414 
1415  if (fdchits[my_id]->wire->layer!=PLANE_TO_SKIP){
1416  /*
1417  if(DEBUG_HISTS){
1418  hFDCOccTrkFit->Fill(fdchits[my_id]->wire->layer);
1419  }
1420  */
1421  // Update the state vector
1422  S+=K*Mdiff;
1423  if(VERBOSE) S.Print();
1424  // Update state vector covariance matrix
1425  C=C-K*(H*C);
1426 
1427  // Update the filtered measurement covariane matrix and put results in
1428  // update vector
1429  DMatrix2x2 RC=V-H*C*H_T;
1430  DMatrix2x1 res=Mdiff-H*K*Mdiff;
1431 
1432  chi2+=RC.Chi2(res);
1433  ndof+=2;
1434 
1435  // fill pull vector entries
1436  updates[my_id].V=RC;
1437  }
1438  else{
1439  updates[my_id].V=V;
1440  }
1441 
1442  used_fdc_hits[my_id]=1;
1443 
1444  // fill pull vector
1445  updates[my_id].d=doca;
1446  updates[my_id].S=S;
1447  updates[my_id].C=C;
1448  updates[my_id].tdrift=fdchits[my_id]->time-trajectory[k].t;
1449  updates[my_id].s=29.98*trajectory[k].t; // assume beta=1
1450  }
1451  }
1452 
1453  if (more_hits && trajectory[k].z < downstreamEndplate){
1454  // Position along wire
1455  double z0=origin.Z();
1456  wirepos=origin+(trajectory[k].z-z0)*wdir;
1457 
1458  // New doca^2
1459  double dx=S(state_x)-wirepos.X();
1460  double dy=S(state_y)-wirepos.Y();
1461  doca2=dx*dx+dy*dy;
1462  if (VERBOSE > 10) jout<< "At Position " << S(state_x) << " " << S(state_y) << " " << trajectory[k].z << " doca2 " << doca2 << endl;
1463 
1464  if (doca2>old_doca2 && more_hits && !firstCDCStep){
1465 
1466  // zero-position and direction of line describing particle trajectory
1467  double tx=S(state_tx),ty=S(state_ty);
1468  DVector3 pos0(S(state_x),S(state_y),trajectory[k].z);
1469  DVector3 tdir(tx,ty,1.);
1470 
1471  // Find the true doca to the wire
1472  DVector3 diff=pos0-origin;
1473  double dx0=diff.x(),dy0=diff.y();
1474  double wdir_dot_diff=diff.Dot(wdir);
1475  double tdir_dot_diff=diff.Dot(tdir);
1476  double tdir_dot_wdir=tdir.Dot(wdir);
1477  double tdir2=tdir.Mag2();
1478  double wdir2=wdir.Mag2();
1479  double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
1480  double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
1481  double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
1482  double scale=1./D;
1483  double s=scale*N;
1484  double t=scale*N1;
1485  diff+=s*tdir-t*wdir;
1486  double d=diff.Mag()+d_EPS; // prevent division by zero
1487 
1488  // The next measurement and its variance
1489  double tdrift=cdchits[cdc_index]->tdrift-trajectory[k].t;
1490  V_CDC=CDCDriftVariance(tdrift);
1491 
1492  double phi_d=diff.Phi();
1493  double dphi=phi_d-origin.Phi();
1494  while (dphi>M_PI) dphi-=2*M_PI;
1495  while (dphi<-M_PI) dphi+=2*M_PI;
1496 
1497  int ring_index=cdchits[cdc_index]->wire->ring-1;
1498  int straw_index=cdchits[cdc_index]->wire->straw-1;
1499  double dz=t*wdir.z();
1500  double delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.)
1501  *cos(phi_d+sag_phi_offset[ring_index][straw_index]);
1502  double dmeas=CDCDriftDistance(dphi,delta,tdrift);
1503 
1504  // residual
1505  double res=dmeas-d;
1506  if (VERBOSE>5) jout << " Residual " << res << endl;
1507  // Track projection
1508  double one_over_d=1./d;
1509  double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
1510  double wx=wdir.x(),wy=wdir.y();
1511 
1512  double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
1513  double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
1514  double dtdtx=scale*(dN1dtx-t*dDdtx);
1515 
1516  double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
1517  double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
1518  double dtdty=scale*(dN1dty-t*dDdty);
1519 
1520  double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
1521  double dsdtx=scale*(dNdtx-s*dDdtx);
1522 
1523  double dNdty=wy*wdir_dot_diff-wdir2*dy0;
1524  double dsdty=scale*(dNdty-s*dDdty);
1525 
1526  H_CDC(state_tx)=H_T_CDC(state_tx)
1527  =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
1528  +diffz*(dsdtx-dtdtx));
1529  H_CDC(state_ty)=H_T_CDC(state_ty)
1530  =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
1531  +diffz*(dsdty-dtdty));
1532 
1533  double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx);
1534  double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx);
1535  double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
1536  double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
1537 
1538  H_CDC(state_x)=H_T_CDC(state_x)
1539  =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
1540  +diffz*(dsdx-dtdx));
1541  H_CDC(state_y)=H_T_CDC(state_y)
1542  =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
1543  +diffz*(dsdy-dtdy));
1544 
1545  double InvV=1./(V_CDC+H_CDC*C*H_T_CDC);
1546 
1547  // Check how far this hit is from the projection
1548  double chi2check=res*res*InvV;
1549  if (chi2check < CHI2CUT || DO_PRUNING == 0){
1550  if (VERBOSE) jout << "CDC Hit Added to FDC track " << endl;
1551  if (cdchits[cdc_index]->wire->ring!=RING_TO_SKIP){
1552 
1553  // Compute Kalman gain matrix
1554  K_CDC=InvV*(C*H_T_CDC);
1555  // Update state vector covariance matrix
1556  DMatrix4x4 Ctest=C-K_CDC*(H_CDC*C);
1557 
1558  //C.Print();
1559  //K.Print();
1560  //Ctest.Print();
1561 
1562  // Check that Ctest is positive definite
1563  if (!Ctest.IsPosDef()) return VALUE_OUT_OF_RANGE;
1564  C=Ctest;
1565  if(VERBOSE>10) C.Print();
1566  // Update the state vector
1567  //S=S+res*K;
1568  S+=res*K_CDC;
1569  if(VERBOSE) {jout << "traj[z]=" << trajectory[k].z<< endl; S.Print();}
1570 
1571  // Compute new residual
1572  //d=finder->FindDoca(trajectory[k].z,S,wdir,origin);
1573  res=res-H_CDC*K_CDC*res;
1574 
1575  // Update chi2
1576  double fit_V=V_CDC-H_CDC*C*H_T_CDC;
1577  chi2+=res*res/fit_V;
1578  ndof++;
1579 
1580  // fill pull vector entry
1581  cdc_updates[cdc_index].V=fit_V;
1582  }
1583  else {
1584  cdc_updates[cdc_index].V=V_CDC;
1585  }
1586 
1587  // fill updates
1588  cdc_updates[cdc_index].resi=res;
1589  cdc_updates[cdc_index].d=d;
1590  cdc_updates[cdc_index].delta=delta;
1591  cdc_updates[cdc_index].S=S;
1592  cdc_updates[cdc_index].C=C;
1593  cdc_updates[cdc_index].tdrift=tdrift;
1594  cdc_updates[cdc_index].ddrift=dmeas;
1595  cdc_updates[cdc_index].s=29.98*trajectory[k].t; // assume beta=1
1596  trajectory[k].id=cdc_index+1000;
1597 
1598  used_cdc_hits[cdc_index]=1;
1599  }
1600  // move to next cdc hit
1601  if (cdc_index>0){
1602  cdc_index--;
1603 
1604  //New wire position
1605  wire=cdchits[cdc_index]->wire;
1606  if (VERBOSE>5) jout << " Next Wire ring " << wire->ring << endl;
1607  origin=wire->origin;
1608  double vz=wire->udir.z();
1609  wdir=(1./vz)*wire->udir;
1610  wirepos=origin+((trajectory[k].z-z0))*wdir;
1611 
1612  // New doca^2
1613  dx=S(state_x)-wirepos.x();
1614  dy=S(state_y)-wirepos.y();
1615  doca2=dx*dx+dy*dy;
1616 
1617  }
1618  else more_hits=false;
1619  }
1620  firstCDCStep=false;
1621  old_doca2=doca2;
1622  }
1623  }
1624 
1625  ndof-=4;
1626 
1627  return NOERROR;
1628 }
1629 
1630 
1631 // Smoothing algorithm for the forward trajectory. Updates the state vector
1632 // at each step (going in the reverse direction to the filter) based on the
1633 // information from all the steps and outputs the state vector at the
1634 // outermost step.
1635 
1636 jerror_t
1637 DTrackFitterStraightTrack::Smooth(vector<fdc_update_t>&fdc_updates,
1638  vector<cdc_update_t>&cdc_updates){
1639  unsigned int max=best_trajectory.size()-1;
1641  DMatrix4x4 C=(best_trajectory[max].Ckk);
1642  DMatrix4x4 JT=best_trajectory[max].J.Transpose();
1643  DMatrix4x1 Ss=S;
1644  DMatrix4x4 Cs=C;
1645  DMatrix4x4 A,dC;
1646 
1647  const double d_EPS=1e-8;
1648 
1649  for (unsigned int m=max-1;m>0;m--){
1650  if (best_trajectory[m].id>0 && best_trajectory[m].id<1000){ // FDC Hit
1651  unsigned int id=best_trajectory[m].id-1;
1652  A=fdc_updates[id].C*JT*C.Invert();
1653  Ss=fdc_updates[id].S+A*(Ss-S);
1654 
1655  dC=A*(Cs-C)*A.Transpose();
1656  Cs=fdc_updates[id].C+dC;
1657 
1658  double cosa=cos(fdchits[id]->wire->angle);
1659  double cos2a=cos(2*fdchits[id]->wire->angle);
1660  double sina=sin(fdchits[id]->wire->angle);
1661  double u=fdchits[id]->w;
1662  double v=fdchits[id]->s;
1663 
1664  // Small angle alignment correction
1665  double x = S(state_x) + fdchits[id]->wire->angles.Z()*S(state_y);
1666  double y = S(state_y) - fdchits[id]->wire->angles.Z()*S(state_x);
1667  //tz = 1. + my_fdchits[id]->phiY*tx - my_fdchits[id]->phiX*ty;
1668  double tx = (S(state_tx) + fdchits[id]->wire->angles.Z()*S(state_ty) - fdchits[id]->wire->angles.Y());
1669  double ty = (S(state_ty) - fdchits[id]->wire->angles.Z()*S(state_tx) + fdchits[id]->wire->angles.X());
1670 
1671  // Projected position along the wire
1672  double vpred=x*sina+y*cosa;
1673 
1674  // Projected position in the plane of the wires transverse to the wires
1675  double upred=x*cosa-y*sina;
1676 
1677  // Direction tangent in the u-z plane
1678  double tu=tx*cosa-ty*sina;
1679  double alpha=atan(tu);
1680  double cosalpha=cos(alpha);
1681  //double cosalpha2=cosalpha*cosalpha;
1682  double sinalpha=sin(alpha);
1683 
1684  // (signed) distance of closest approach to wire
1685  double du=upred-u;
1686  double doca=du*cosalpha;
1687  // Difference between measurement and projection for the cathodes
1688  double tv=tx*sina+ty*cosa;
1689  double resi_c=v-vpred;
1690 
1691  // Difference between measurement and projection perpendicular to the wire
1692  double drift=0.; // assume hit at wire position
1693  if (fit_type==kTimeBased){
1694  double drift_time=fdc_updates[id].tdrift;
1695  drift=(du>0.0?1.:-1.)*fdc_drift_distance(drift_time);
1696  }
1697  double resi_a=drift-doca;
1698 
1699  // Variance from filter step
1700  DMatrix2x2 V=fdc_updates[id].V;
1701  // Compute projection matrix and find the variance for the residual
1702  DMatrix4x2 H_T;
1703  double temp2=-tv*sinalpha;
1704  H_T(state_x,1)=sina+cosa*cosalpha*temp2;
1705  H_T(state_y,1)=cosa-sina*cosalpha*temp2;
1706 
1707  double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha;
1708  double doca_cosalpha=doca*cosalpha;
1709  H_T(state_tx,1)=-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2);
1710  H_T(state_ty,1)=-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2);
1711 
1712  H_T(state_x,0)=cosa*cosalpha;
1713  H_T(state_y,0)=-sina*cosalpha;
1714  double one_plus_tu2=1.+tu*tu;
1715  double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2;
1716  H_T(state_ty,0)=sina*factor;
1717  H_T(state_tx,0)=-cosa*factor;
1718 
1719  // Matrix transpose H_T -> H
1720  DMatrix2x4 H;
1721  H(0,state_x)=H_T(state_x,0);
1722  H(0,state_y)=H_T(state_y,0);
1723  H(0,state_tx)=H_T(state_tx,0);
1724  H(0,state_ty)=H_T(state_ty,0);
1725  H(1,state_x)=H_T(state_x,1);
1726  H(1,state_y)=H_T(state_y,1);
1727  H(1,state_tx)=H_T(state_tx,1);
1728  H(1,state_ty)=H_T(state_ty,1);
1729 
1730  if (fdchits[id]->wire->layer==PLANE_TO_SKIP){
1731  //V+=Cs.SandwichMultiply(H_T);
1732  V=V+H*Cs*H_T;
1733  }
1734  else{
1735  //V-=dC.SandwichMultiply(H_T);
1736  V=V-H*Cs*H_T;
1737  }
1738  /*
1739  if(DEBUG_HISTS){
1740  hFDCOccTrkSmooth->Fill(fdchits[id]->wire->layer);
1741  }
1742  */
1743  // Implement derivatives wrt track parameters needed for millepede alignment
1744  // Add the pull
1745  double scale=1./sqrt(1.+tx*tx+ty*ty);
1746  double cosThetaRel=fdchits[id]->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale));
1747  DTrackFitter::pull_t thisPull(resi_a,sqrt(V(0,0)),
1749  fdc_updates[id].tdrift,
1750  fdc_updates[id].d,
1751  NULL,fdchits[id],
1752  0.0, //docaphi
1753  best_trajectory[m].z,cosThetaRel,
1754  0.0, //tcorr
1755  resi_c, sqrt(V(1,1))
1756  );
1757 
1758  if (fdchits[id]->wire->layer!=PLANE_TO_SKIP){
1759  vector<double> derivatives;
1760  derivatives.resize(FDCTrackD::size);
1761 
1762  //dDOCAW/dDeltaX
1763  derivatives[FDCTrackD::dDOCAW_dDeltaX] = -(1/sqrt(1 + pow(tx*cosa - ty*sina,2)));
1764 
1765  //dDOCAW/dDeltaZ
1766  derivatives[FDCTrackD::dDOCAW_dDeltaZ] = (tx*cosa - ty*sina)/sqrt(1 + pow(tx*cosa - ty*sina,2));
1767 
1768  //dDOCAW/ddeltaPhiX
1769  derivatives[FDCTrackD::dDOCAW_dDeltaPhiX] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5);
1770 
1771  //dDOCAW/ddeltaphiY
1772  derivatives[FDCTrackD::dDOCAW_dDeltaPhiY] = (cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5);
1773 
1774  //dDOCAW/ddeltaphiZ
1775  derivatives[FDCTrackD::dDOCAW_dDeltaPhiZ] = (tx*ty*u*cos2a + (x + pow(ty,2)*x - tx*ty*y)*sina +
1776  cosa*(-(tx*ty*x) + y + pow(tx,2)*y + (pow(tx,2) - pow(ty,2))*u*sina))/
1777  pow(1 + pow(tx*cosa - ty*sina,2),1.5);
1778 
1779  // dDOCAW/dx
1780  derivatives[FDCTrackD::dDOCAW_dx] = cosa/sqrt(1 + pow(tx*cosa - ty*sina,2));
1781 
1782  // dDOCAW/dy
1783  derivatives[FDCTrackD::dDOCAW_dy] = -(sina/sqrt(1 + pow(tx*cosa - ty*sina,2)));
1784 
1785  // dDOCAW/dtx
1786  derivatives[FDCTrackD::dDOCAW_dtx] = -((cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5));
1787 
1788  // dDOCAW/dty
1789  derivatives[FDCTrackD::dDOCAW_dty] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5);
1790 
1791  // And the cathodes
1792  //dDOCAW/ddeltax
1793  derivatives[FDCTrackD::dDOCAC_dDeltaX] = 0.;
1794 
1795  //dDOCAW/ddeltax
1796  derivatives[FDCTrackD::dDOCAC_dDeltaZ] = ty*cosa + tx*sina;
1797 
1798  //dDOCAW/ddeltaPhiX
1799  derivatives[FDCTrackD::dDOCAC_dDeltaPhiX] = 0.;
1800 
1801  //dDOCAW/ddeltaPhiX
1802  derivatives[FDCTrackD::dDOCAC_dDeltaPhiY] = 0.;
1803 
1804  //dDOCAW/ddeltaPhiX
1805  derivatives[FDCTrackD::dDOCAC_dDeltaPhiZ] = -(x*cosa) + y*sina;
1806 
1807  // dDOCAW/dx
1808  derivatives[FDCTrackD::dDOCAC_dx] = sina;
1809 
1810  // dDOCAW/dy
1811  derivatives[FDCTrackD::dDOCAW_dy] = cosa;
1812 
1813  // dDOCAW/dtx
1814  derivatives[FDCTrackD::dDOCAW_dtx] = 0.;
1815 
1816  // dDOCAW/dty
1817  derivatives[FDCTrackD::dDOCAW_dty] = 0.;
1818 
1819  thisPull.AddTrackDerivatives(derivatives);
1820  }
1821  pulls.push_back(thisPull);
1822  }
1823  else if (best_trajectory[m].id>=1000){ // CDC Hit
1824  unsigned int id=best_trajectory[m].id-1000;
1825  A=cdc_updates[id].C*JT*C.Invert();
1826  Ss=cdc_updates[id].S+A*(Ss-S);
1827 
1828  dC=A*(Cs-C)*A.Transpose();
1829  Cs=cdc_updates[id].C+dC;
1830  if (VERBOSE > 10) {
1831  jout << " In Smoothing Step Using ID " << id << "/" << cdc_updates.size() << " for ring " << cdchits[id]->wire->ring << endl;
1832  jout << " A cdc_updates[id].C Ss Cs " << endl;
1833  A.Print(); cdc_updates[id].C.Print(); Ss.Print(); Cs.Print();
1834  }
1835  if(!Cs.IsPosDef()) {
1836  if (VERBOSE) jout << "Cs is not PosDef!" << endl;
1837  return VALUE_OUT_OF_RANGE;
1838  }
1839 
1840  const DCDCWire *wire=cdchits[id]->wire;
1841  DVector3 origin=wire->origin;
1842  double z0=origin.z();
1843  double vz=wire->udir.z();
1844  DVector3 wdir=(1./vz)*wire->udir;
1845  DVector3 wirepos=origin+(best_trajectory[m].z-z0)*wdir;
1846  // Position and direction from state vector
1847  double x=Ss(state_x);
1848  double y=Ss(state_y);
1849  double tx=Ss(state_tx);
1850  double ty=Ss(state_ty);
1851 
1852  DVector3 pos0(x,y,best_trajectory[m].z);
1853  DVector3 tdir(tx,ty,1.);
1854 
1855  // Find the true doca to the wire
1856  DVector3 diff=pos0-origin;
1857  double dx0=diff.x(),dy0=diff.y();
1858  double wdir_dot_diff=diff.Dot(wdir);
1859  double tdir_dot_diff=diff.Dot(tdir);
1860  double tdir_dot_wdir=tdir.Dot(wdir);
1861  double tdir2=tdir.Mag2();
1862  double wdir2=wdir.Mag2();
1863  double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
1864  double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
1865  double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
1866  double scale=1./D;
1867  double s=scale*N;
1868  double t=scale*N1;
1869  diff+=s*tdir-t*wdir;
1870  double d=diff.Mag()+d_EPS; // prevent division by zero
1871  double ddrift = cdc_updates[id].ddrift;
1872 
1873  double resi = ddrift - d;
1874 
1875 
1876  // Track projection
1877  DMatrix1x4 H; DMatrix4x1 H_T;
1878  {
1879  double one_over_d=1./d;
1880  double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
1881  double wx=wdir.x(),wy=wdir.y();
1882 
1883  double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
1884  double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
1885  double dtdtx=scale*(dN1dtx-t*dDdtx);
1886 
1887  double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
1888  double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
1889  double dtdty=scale*(dN1dty-t*dDdty);
1890 
1891  double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
1892  double dsdtx=scale*(dNdtx-s*dDdtx);
1893 
1894  double dNdty=wy*wdir_dot_diff-wdir2*dy0;
1895  double dsdty=scale*(dNdty-s*dDdty);
1896 
1897  H(state_tx)=H_T(state_tx)
1898  =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
1899  +diffz*(dsdtx-dtdtx));
1900  H(state_ty)=H_T(state_ty)
1901  =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
1902  +diffz*(dsdty-dtdty));
1903 
1904  double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx);
1905  double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx);
1906  double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
1907  double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
1908 
1909  H(state_x)=H_T(state_x)
1910  =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
1911  +diffz*(dsdx-dtdx));
1912  H(state_y)=H_T(state_y)
1913  =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
1914  +diffz*(dsdy-dtdy));
1915  }
1916  double V=cdc_updates[id].V;
1917 
1918  if (VERBOSE > 10) jout << " d " << d << " H*S " << H*S << endl;
1919  if (cdchits[id]->wire->ring==RING_TO_SKIP){
1920  V=V+H*Cs*H_T;
1921  }
1922  else{
1923  V=V-H*Cs*H_T;
1924  }
1925  if (V<0) return VALUE_OUT_OF_RANGE;
1926 
1927  // Add the pull
1928  double myscale=1./sqrt(1.+tx*tx+ty*ty);
1929  double cosThetaRel=wire->udir.Dot(DVector3(myscale*tx,myscale*ty,myscale));
1930  DTrackFitter::pull_t thisPull(resi,sqrt(V),
1932  cdc_updates[id].tdrift,
1933  d,cdchits[id], NULL,
1934  diff.Phi(), //docaphi
1935  best_trajectory[m].z,cosThetaRel,
1936  cdc_updates[id].tdrift);
1937 
1938  // Derivatives for alignment
1939  double wtx=wire->udir.X(), wty=wire->udir.Y(), wtz=wire->udir.Z();
1940  double wx=wire->origin.X(), wy=wire->origin.Y(), wz=wire->origin.Z();
1941 
1942  double z=best_trajectory[m].z;
1943  double tx2=tx*tx, ty2=ty*ty;
1944  double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz;
1945  double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz)+d_EPS;
1946  double denom2=denom*denom;
1947  double c1=-(wtx - tx*wtz)*(wy - y);
1948  double c2=wty*(wx - tx*wz - x + tx*z);
1949  double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z);
1950  double dscale=0.5*(1/d);
1951 
1952  vector<double> derivatives(11);
1953 
1954  derivatives[CDCTrackD::dDOCAdOriginX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3))/denom;
1955 
1956  derivatives[CDCTrackD::dDOCAdOriginY]=dscale*(2*(-wtx + tx*wtz)*(c1 + c2 + c3))/denom;
1957 
1958  derivatives[CDCTrackD::dDOCAdOriginZ]=dscale*(2*(ty*wtx - tx*wty)*(c1 + c2 + c3))/denom;
1959 
1960  derivatives[CDCTrackD::dDOCAdDirX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3)*
1961  (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
1962  wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2;
1963 
1964  derivatives[CDCTrackD::dDOCAdDirY]=dscale*(-2*(wtx - tx*wtz)*(c1 + c2 + c3)*
1965  (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) +
1966  wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2;
1967 
1968  derivatives[CDCTrackD::dDOCAdDirZ]=dscale*(-2*(ty*wtx - tx*wty)*(c1 + c2 + c3)*
1969  (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) +
1970  wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2;
1971 
1972  derivatives[CDCTrackD::dDOCAdS0]=-derivatives[CDCTrackD::dDOCAdOriginX];
1973 
1974  derivatives[CDCTrackD::dDOCAdS1]=-derivatives[CDCTrackD::dDOCAdOriginY];
1975 
1976  derivatives[CDCTrackD::dDOCAdS2]=dscale*(2*(wty - ty*wtz)*(-c1 - c2 - c3)*
1977  (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
1978  tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
1979  ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
1980 
1981  derivatives[CDCTrackD::dDOCAdS3]=dscale*(2*(wtx - tx*wtz)*(c1 + c2 + c3)*
1982  (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z +
1983  tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) +
1984  ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2;
1985 
1986  thisPull.AddTrackDerivatives(derivatives);
1987 
1988  pulls.push_back(thisPull);
1989  }
1990  else{
1991  A=best_trajectory[m].Ckk*JT*C.Invert();
1992  Ss=best_trajectory[m].Skk+A*(Ss-S);
1993  Cs=best_trajectory[m].Ckk+A*(Cs-C)*A.Transpose();
1994  }
1995 
1996  S=best_trajectory[m].Skk;
1997  C=best_trajectory[m].Ckk;
1998  JT=best_trajectory[m].J.Transpose();
1999  }
2000 
2001  return NOERROR;
2002 }
2003 
2004 shared_ptr<TMatrixFSym>
2006  auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource();
2007  C7x7->ResizeTo(7, 7);
2008  DMatrix J(7,5);
2009 
2010  double p=5.; // fixed: cannot measure
2011  double tx_=S(state_tx);
2012  double ty_=S(state_ty);
2013  double x_=S(state_x);
2014  double y_=S(state_y);
2015  double tanl=sign/sqrt(tx_*tx_+ty_*ty_);
2016  double tanl2=tanl*tanl;
2017  double lambda=atan(tanl);
2018  double sinl=sin(lambda);
2019  double sinl3=sinl*sinl*sinl;
2020 
2021  J(state_X,state_x)=J(state_Y,state_y)=1.;
2022  J(state_Pz,state_ty)=-p*ty_*sinl3;
2023  J(state_Pz,state_tx)=-p*tx_*sinl3;
2024  J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3;
2025  J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3;
2026  J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3;
2027  J(state_Pz,4)=-p*p*sinl;
2028  J(state_Px,4)=tx_*J(state_Pz,4);
2029  J(state_Py,4)=ty_*J(state_Pz,4);
2030  J(state_Z,state_x)=-tx_*tanl2;
2031  J(state_Z,state_y)=-ty_*tanl2;
2032  double diff=tx_*tx_-ty_*ty_;
2033  double frac=tanl2*tanl2;
2034  J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac;
2035  J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac;
2036 
2037  // C'= JCJ^T
2038  *C7x7=C.Similarity(J);
2039 
2040  return C7x7;
2041 }
2042 
2043 // Routine to get extrapolations to other detectors
2045  const DVector3 &dir){
2046  double z0=pos0.z();
2047  double uz=dir.z();
2049 
2050  // Extrapolate to DIRC
2051  DVector3 diff=((dDIRCz-z0)/uz)*dir;
2052  DVector3 pos=pos0+diff;
2053  double s=diff.Mag();
2054  double t=s/29.98;
2055  extrapolations[SYS_DIRC].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s));
2056 
2057  // Extrapolate to TOF
2058  diff=((dTOFz-z0)/uz)*dir;
2059  pos=pos0+diff;
2060  s=diff.Mag();
2061  t=s/29.98;
2062  extrapolations[SYS_TOF].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s));
2063 
2064  // Extrapolate to FCAL
2065  diff=((dFCALz-z0)/uz)*dir;
2066  pos=pos0+diff;
2067  s=diff.Mag();
2068  t=s/29.98;
2069  extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s));
2070  // extrapolate to exit of FCAL
2071  diff=((dFCALz+45.-z0)/uz)*dir;
2072  pos=pos0+diff;
2073  s=diff.Mag();
2074  t=s/29.98;
2075  extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s));
2076 
2077  // Extrapolate to Start Counter and BCAL
2078  double R=pos0.Perp();
2079  diff.SetMag(0.);
2080  double z=z0;
2081  while (R<89.0 && z>17. && z<410.){
2082  diff+=(1./dir.z())*dir;
2083  pos=pos0+diff;
2084  R=pos.Perp();
2085  z=pos.z();
2086  s=diff.Mag();
2087  t=s/29.98;
2088  // printf("R %f z %f\n",R,z);
2089  // start counter
2090  if (sc_pos.empty()==false && R<SC_BARREL_R && z<SC_END_NOSE_Z){
2091  double d_old=1000.,d=1000.;
2092  unsigned int index=0;
2093  for (unsigned int m=0;m<12;m++){
2094  double dphi=pos.Phi()-SC_PHI_SECTOR1;
2095  if (dphi<0) dphi+=2.*M_PI;
2096  index=int(floor(dphi/(2.*M_PI/30.)));
2097  if (index>29) index=0;
2098  d=sc_norm[index][m].Dot(pos-sc_pos[index][m]);
2099  if (d*d_old<0){ // break if we cross the current plane
2100  // Find the new distance to the start counter (which could
2101  // now be to a plane in the one adjacent to the one before the
2102  // step...)
2103  int count=0;
2104  while (fabs(d)>0.05 && count<20){
2105  // Find the index for the nearest start counter paddle
2106  double dphi=pos.Phi()-SC_PHI_SECTOR1;
2107  if (dphi<0) dphi+=2.*M_PI;
2108  index=int(floor(dphi/(2.*M_PI/30.)));
2109  d=sc_norm[index][m].Dot(pos-sc_pos[index][m]);
2110  pos+=d*dir;
2111  count++;
2112  }
2113  extrapolations[SYS_START].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s));
2114  break;
2115  }
2116  d_old=d;
2117  }
2118  }
2119  if (R>64.){
2120  extrapolations[SYS_BCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s));
2121  }
2122  }
2123 
2124 }
deque< trajectory_t > best_trajectory
void setMomentum(const DVector3 &aMomentum)
shared_ptr< TMatrixFSym > Get7x7ErrorMatrix(TMatrixFSym C, DMatrix4x1 &S, double sign)
vector< pull_t > pulls
Definition: DTrackFitter.h:237
DApplication * dapp
DTrackFitter::fit_status_t FitCentralTrack(double &z0, double t0, double dzsign, DMatrix4x1 &Sbest, DMatrix4x4 &Cbest, double &chi2_best, int &ndof_best)
#define SPEED_OF_LIGHT
TMatrixD DMatrix
Definition: DMatrix.h:14
double t0(void) const
Definition: DTrackingData.h:23
const DCDCWire * wire
Definition: DCDCTrackHit.h:37
The DTrackFitter class is a base class for different charged track fitting algorithms. It does not actually fit the track itself, but provides the interface and some common support features most algorthims will need to implement.
Definition: DTrackFitter.h:61
vector< vector< DVector3 > > sc_dir
void Print()
Definition: DMatrix4x4.h:203
TVector3 DVector3
Definition: DVector3.h:14
void ClearExtrapolations(void)
Definition: DTrackFitter.h:141
Double_t x[NCHANNELS]
Definition: st_tw_resols.C:39
#define EPS
#define C0
Definition: src/md5.cpp:24
fit_type_t fit_type
Definition: DTrackFitter.h:227
double fdc_drift_variance(double t) const
#define y
unsigned int Locate(const vector< double > &xx, double x) const
shared_ptr< DResourcePool< TMatrixFSym > > dResourcePool_TMatrixFSym
bool Get(string xpath, string &sval) const
Definition: DGeometry.h:50
const DVector3 & position(void) const
void setTrackingErrorMatrix(const shared_ptr< const TMatrixFSym > &aMatrix)
Definition: DTrackingData.h:29
const DFDCWire * wire
DFDCWire for this wire.
Definition: DFDCPseudo.h:92
void setForwardParmFlag(bool aFlag)
Definition: DTrackingData.h:28
bool DTrackFitterStraightTrack_fdc_hit_cmp(const DFDCPseudo *a, const DFDCPseudo *b)
class DFDCPseudo: definition for a reconstructed point in the FDC
Definition: DFDCPseudo.h:74
map< DetectorSystem_t, vector< Extrapolation_t > > extrapolations
Definition: DTrackFitter.h:238
vector< const DFDCPseudo * > fdchits_used_in_fit
Definition: DTrackFitter.h:242
DMatrix4x4 Transpose()
Definition: DMatrix4x4.h:174
bool GetFCALZ(double &z_fcal) const
z-location of front face of CCAL in cm
Definition: DGeometry.cc:1718
static char index(char c)
Definition: base64.cpp:115
Definition: GlueX.h:19
Double_t c1[2][NMODULES]
Definition: tw_corr.C:68
jerror_t Smooth(vector< cdc_update_t > &cdc_updates)
double CDCDriftDistance(double dphi, double delta, double t) const
Double_t c2[2][NMODULES]
Definition: tw_corr.C:69
vector< vector< double > > max_sag
vector< vector< double > > sag_phi_offset
void setErrorMatrix(const shared_ptr< const TMatrixFSym > &aMatrix)
DTrackFitter::fit_status_t FitForwardTrack(double t0, double &start_z, DMatrix4x1 &Sbest, DMatrix4x4 &Cbest, double &chi2_best, int &ndof_best)
TEllipse * e
vector< const DCDCTrackHit * > cdchits
Definition: DTrackFitter.h:224
const double alpha
const map< DetectorSystem_t, vector< Extrapolation_t > > & GetExtrapolations(void) const
Definition: DTrackFitter.h:163
#define H(x, y, z)
const DGeometry * geom
Definition: DTrackFitter.h:229
jerror_t SetReferenceTrajectory(double t0, double z, DMatrix4x1 &S, const DCDCTrackHit *last_cdc, double &dzsign)
TLatex tx
#define MAX_STEPS
Definition: GlueX.h:20
int straw
Definition: DCDCWire.h:81
vector< vector< DVector3 > > sc_pos
DMatrix4x4 Invert()
Definition: DMatrix4x4.h:161
DGeometry * GetDGeometry(unsigned int run_number)
Definition: GlueX.h:22
Definition: GlueX.h:26
Double_t sigma[NCHANNELS]
Definition: st_tw_resols.C:37
vector< const DCDCTrackHit * > cdchits_used_in_fit
Definition: DTrackFitter.h:241
bool DTrackFitterStraightTrack_cdc_hit_reverse_cmp(const DCDCTrackHit *a, const DCDCTrackHit *b)
bool DTrackFitterStraightTrack_cdc_hit_radius_cmp(const DCDCTrackHit *a, const DCDCTrackHit *b)
double sqrt(double)
vector< const DFDCPseudo * > fdchits
Definition: DTrackFitter.h:225
bool IsPosDef()
Definition: DMatrix4x4.h:195
double sin(double)
vector< vector< DVector3 > > sc_norm
#define S(str)
Definition: hddm-c.cpp:84
void AddTrackDerivatives(vector< double > d)
Definition: DTrackFitter.h:122
const DVector3 & momentum(void) const
double fdc_drift_distance(double time) const
DTrackingData input_params
Definition: DTrackFitter.h:226
int ring
Definition: DCDCWire.h:80
void Print()
Definition: DMatrix4x1.h:61
bool GetCDCEndplate(double &z, double &dz, double &rmin, double &rmax) const
Definition: DGeometry.cc:1497
bool GetDIRCZ(double &z_dirc) const
z-location of DIRC in cm
Definition: DGeometry.cc:1735
void setPosition(const DVector3 &aPosition)
jerror_t KalmanFilter(DMatrix4x1 &S, DMatrix4x4 &C, vector< int > &used_hits, vector< cdc_update_t > &updates, double &chi2, int &ndof, unsigned int iter)
TDirectory * dir
Definition: bcal_hist_eff.C:31
TCanvas * c3
double CDCDriftVariance(double t) const
double FindDoca(double z, const DMatrix4x1 &S, const DVector3 &wdir, const DVector3 &origin, DVector3 *poca=NULL) const
DTrackingData fit_params
Definition: DTrackFitter.h:234
printf("string=%s", string)
TH2F * temp
union @6::@8 u
#define I(x, y, z)
bool DTrackFitterStraightTrack_cdc_hit_cmp(const DCDCTrackHit *a, const DCDCTrackHit *b)
bool GetStartCounterGeom(vector< vector< DVector3 > > &pos, vector< vector< DVector3 > > &norm) const
Definition: DGeometry.cc:1983