Hall-D Software  alpha
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
DTrackFitterKalmanSIMD.cc
Go to the documentation of this file.
1 //************************************************************************
2 // DTrackFitterKalmanSIMD.cc
3 //************************************************************************
4 
6 #include "CDC/DCDCTrackHit.h"
9 #include "HDGEOMETRY/DRootGeom.h"
10 #include "DANA/DApplication.h"
11 #include <JANA/JCalibration.h>
12 #include "PID/DParticleID.h"
13 
14 #include <TH2F.h>
15 #include <TH1I.h>
16 #include <TROOT.h>
17 #include <TMath.h>
18 #include <DMatrix.h>
19 
20 #include <iomanip>
21 #include <math.h>
22 
23 #define MAX_TB_PASSES 20
24 #define MAX_WB_PASSES 20
25 #define MAX_P 12.0
26 #define ALPHA 1./137.
27 #define CHISQ_DELTA 0.01
28 #define MIN_ITER 3
29 
30 
31 // Local boolean routines for sorting
32 //bool static DKalmanSIMDHit_cmp(DKalmanSIMDHit_t *a, DKalmanSIMDHit_t *b){
33 // return a->z<b->z;
34 //}
35 
37  if (fabs(a->z-b->z)<EPS){
38  if (fabs(a->t-b->t)<EPS){
39  double tsum_1=a->hit->t_u+a->hit->t_v;
40  double tsum_2=b->hit->t_u+b->hit->t_v;
41  if (fabs(tsum_1-tsum_2)<EPS){
42  return (a->dE>b->dE);
43  }
44  return (tsum_1<tsum_2);
45  }
46  return(a->t<b->t);
47  }
48  return a->z<b->z;
49 }
51  if (a==NULL || b==NULL){
52  cout << "Null pointer in CDC hit list??" << endl;
53  return false;
54  }
55  const DCDCWire *wire_a= a->hit->wire;
56  const DCDCWire *wire_b= b->hit->wire;
57  if(wire_b->ring == wire_a->ring){
58  return wire_b->straw < wire_a->straw;
59  }
60 
61  return (wire_b->ring>wire_a->ring);
62 }
63 
64 
65 // Locate a position in array xx given x
66 void DTrackFitterKalmanSIMD::locate(const double *xx,int n,double x,int *j){
67  int jl=-1;
68  int ju=n;
69  int ascnd=(xx[n-1]>=xx[0]);
70  while(ju-jl>1){
71  int jm=(ju+jl)>>1;
72  if ( (x>=xx[jm])==ascnd)
73  jl=jm;
74  else
75  ju=jm;
76  }
77  if (x==xx[0]) *j=0;
78  else if (x==xx[n-1]) *j=n-2;
79  else *j=jl;
80 }
81 
82 
83 
84 // Locate a position in vector xx given x
85 unsigned int DTrackFitterKalmanSIMD::locate(vector<double>&xx,double x){
86  int n=xx.size();
87  if (x==xx[0]) return 0;
88  else if (x==xx[n-1]) return n-2;
89 
90  int jl=-1;
91  int ju=n;
92  int ascnd=(xx[n-1]>=xx[0]);
93  while(ju-jl>1){
94  int jm=(ju+jl)>>1;
95  if ( (x>=xx[jm])==ascnd)
96  jl=jm;
97  else
98  ju=jm;
99  }
100  return jl;
101 }
102 
103 // Crude approximation for the variance in drift distance due to smearing
105  //return FDC_ANODE_VARIANCE;
106  if (t<5.) t=5.;
107  double sigma=DRIFT_RES_PARMS[0]/(t+1.)+DRIFT_RES_PARMS[1]+DRIFT_RES_PARMS[2]*t*t;
108 
109  return sigma*sigma;
110 }
111 
112 // Convert time to distance for the cdc and compute variance
113 void DTrackFitterKalmanSIMD::ComputeCDCDrift(double dphi,double delta,double t,
114  double B,
115  double &d, double &V, double &tcorr){
116  //d=0.39; // initialize at half-cell
117  //V=0.0507; // initialize with (cell size)/12.
118  tcorr = t; // Need this value even when t is negative for calibration
119  if (t>0){
120  //double dtc =(CDC_DRIFT_BSCALE_PAR1 + CDC_DRIFT_BSCALE_PAR2 * B)* t;
121  //tcorr=t-dtc;
122 
123  // CDC_RES_PAR2=0.005;
124  double sigma=CDC_RES_PAR1/(tcorr+1.) + CDC_RES_PAR2 + CDC_RES_PAR3*tcorr;
125 
126  // Variables to store values for time-to-distance functions for delta=0
127  // and delta!=0
128  double f_0=0.;
129  double f_delta=0.;
130  // Derivative of d with respect to t, needed to add t0 variance
131  // dependence to sigma
132  double dd_dt=0;
133  // Scale factor to account for affect of B-field on maximum drift time
135  tcorr=t*Bscale;
136 
137  // if (delta>0)
138  if (delta>-EPS2){
139  double a1=long_drift_func[0][0];
140  double a2=long_drift_func[0][1];
141  double b1=long_drift_func[1][0];
142  double b2=long_drift_func[1][1];
143  double c1=long_drift_func[2][0];
144  double c2=long_drift_func[2][1];
145  double c3=long_drift_func[2][2];
146 
147  // use "long side" functional form
148  double my_t=0.001*tcorr;
149  double sqrt_t=sqrt(my_t);
150  double t3=my_t*my_t*my_t;
151  double delta_mag=fabs(delta);
152  double a=a1+a2*delta_mag;
153  double b=b1+b2*delta_mag;
154  double c=c1+c2*delta_mag+c3*delta*delta;
155  f_delta=a*sqrt_t+b*my_t+c*t3;
156  f_0=a1*sqrt_t+b1*my_t+c1*t3;
157 
158  dd_dt=0.001*(0.5*a/sqrt_t+b+3.*c*my_t*my_t);
159  }
160  else{
161  double my_t=0.001*tcorr;
162  double sqrt_t=sqrt(my_t);
163  double delta_mag=fabs(delta);
164 
165  // use "short side" functional form
166  double a1=short_drift_func[0][0];
167  double a2=short_drift_func[0][1];
168  double a3=short_drift_func[0][2];
169  double b1=short_drift_func[1][0];
170  double b2=short_drift_func[1][1];
171  double b3=short_drift_func[1][2];
172 
173  double delta_sq=delta*delta;
174  double a=a1+a2*delta_mag+a3*delta_sq;
175  double b=b1+b2*delta_mag+b3*delta_sq;
176  f_delta=a*sqrt_t+b*my_t;
177  f_0=a1*sqrt_t+b1*my_t;
178 
179  dd_dt=0.001*(0.5*a/sqrt_t+b);
180  }
181 
182  unsigned int max_index=cdc_drift_table.size()-1;
183  if (tcorr>cdc_drift_table[max_index]){
184  //_DBG_ << "t: " << tcorr <<" d " << f_delta <<endl;
185  d=f_delta;
186  V=sigma*sigma+mVarT0*dd_dt*dd_dt;
187 
188  return;
189  }
190 
191  // Drift time is within range of table -- interpolate...
192  unsigned int index=0;
193  index=locate(cdc_drift_table,tcorr);
194  double dt=cdc_drift_table[index+1]-cdc_drift_table[index];
195  double frac=(tcorr-cdc_drift_table[index])/dt;
196  double d_0=0.01*(double(index)+frac);
197 
198  if (fabs(delta) < EPS2){
199  d=d_0;
200  V=sigma*sigma+mVarT0*dd_dt*dd_dt;
201  }
202  else{
203  double P=0.;
204  double tcut=250.0; // ns
205  if (tcorr<tcut) {
206  P=(tcut-tcorr)/tcut;
207  }
208  d=f_delta*(d_0/f_0*P+1.-P);
209  V=sigma*sigma+mVarT0*dd_dt*dd_dt;
210  }
211  }
212  else { // Time is negative, or exactly zero, choose position at wire, with error of t=0 hit
213  d=0.;
215  double dt=cdc_drift_table[1]-cdc_drift_table[0];
216  V=sigma*sigma+mVarT0*0.0001/(dt*dt);
217  //V=0.0507; // straw radius^2 / 12
218  }
219 
220 }
221 
222 #define FDC_T0_OFFSET 17.6
223 // Interpolate on a table to convert time to distance for the fdc
224 /*
225  double DTrackFitterKalmanSIMD::fdc_drift_distance(double t,double Bz) const {
226  double a=93.31,b=4.614,Bref=2.143;
227  t*=(a+b*Bref)/(a+b*Bz);
228  int id=int((t+FDC_T0_OFFSET)/2.);
229  if (id<0) id=0;
230  if (id>138) id=138;
231  double d=fdc_drift_table[id];
232  if (id!=138){
233  double frac=0.5*(t+FDC_T0_OFFSET-2.*double(id));
234  double dd=fdc_drift_table[id+1]-fdc_drift_table[id];
235  d+=frac*dd;
236  }
237 
238  return d;
239  }
240  */
241 
242 // parametrization of time-to-distance for FDC
243 double DTrackFitterKalmanSIMD::fdc_drift_distance(double time,double Bz) const {
244  if (time<0.) return 0.;
245  double d=0.;
247  double tsq=time*time;
248  double t_high=DRIFT_FUNC_PARMS[4];
249 
250  if (time<t_high){
251  d=DRIFT_FUNC_PARMS[0]*sqrt(time)+DRIFT_FUNC_PARMS[1]*time
252  +DRIFT_FUNC_PARMS[2]*tsq+DRIFT_FUNC_PARMS[3]*tsq*time;
253  }
254  else{
255  double t_high_sq=t_high*t_high;
256  d=DRIFT_FUNC_PARMS[0]*sqrt(t_high)+DRIFT_FUNC_PARMS[1]*t_high
257  +DRIFT_FUNC_PARMS[2]*t_high_sq+DRIFT_FUNC_PARMS[3]*t_high_sq*t_high;
258  d+=DRIFT_FUNC_PARMS[5]*(time-t_high);
259  }
260 
261  return d;
262 }
263 
264 
266  FactorForSenseOfRotation=(bfield->GetBz(0.,0.,65.)>0.)?-1.:1.;
267 
268  // Some useful values
271 
272  // Get the position of the CDC downstream endplate from DGeometry
273  double endplate_rmin,endplate_rmax;
274  geom->GetCDCEndplate(endplate_z,endplate_dz,endplate_rmin,endplate_rmax);
275  endplate_z-=0.5*endplate_dz;
277  endplate_rmin+=0.1; // put just inside CDC
278  endplate_r2min=endplate_rmin*endplate_rmin;
279  endplate_r2max=endplate_rmax*endplate_rmax;
280 
281  // Beginning of the cdc
282  vector<double>cdc_center;
283  vector<double>cdc_upstream_endplate_pos;
284  vector<double>cdc_endplate_dim;
285  geom->Get("//posXYZ[@volume='CentralDC'/@X_Y_Z",cdc_origin);
286  geom->Get("//posXYZ[@volume='centralDC']/@X_Y_Z",cdc_center);
287  geom->Get("//posXYZ[@volume='CDPU']/@X_Y_Z",cdc_upstream_endplate_pos);
288  geom->Get("//tubs[@name='CDPU']/@Rio_Z",cdc_endplate_dim);
289  cdc_origin[2]+=cdc_center[2]+cdc_upstream_endplate_pos[2]
290  +0.5*cdc_endplate_dim[2];
291 
293  // geom->GetCDCRmid(cdc_rmid); // THIS ISN'T IMPLEMENTED!!
294  // extract the "mean" radius of each ring from the wire data
295  for(uint ring=0; ring<cdcwires.size(); ring++)
296  cdc_rmid.push_back( cdcwires[ring][0]->origin.Perp() );
297 
298  // Outer detector geometry parameters
299  geom->GetFCALZ(dFCALz);
300  if (geom->GetDIRCZ(dDIRCz)==false) dDIRCz=1000.;
301  vector<double>tof_face;
302  geom->Get("//section/composition/posXYZ[@volume='ForwardTOF']/@X_Y_Z",
303  tof_face);
304  vector<double>tof_plane;
305  geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='0']", tof_plane);
306  dTOFz=tof_face[2]+tof_plane[2];
307  geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='1']", tof_plane);
308  dTOFz+=tof_face[2]+tof_plane[2];
309  dTOFz*=0.5; // mid plane between tof planes
310 
311 
312 
313  // Get start counter geometry;
315  // Create vector of direction vectors in scintillator planes
316  for (int i=0;i<30;i++){
317  vector<DVector3>temp;
318  for (unsigned int j=0;j<sc_pos[i].size()-1;j++){
319  double dx=sc_pos[i][j+1].x()-sc_pos[i][j].x();
320  double dy=sc_pos[i][j+1].y()-sc_pos[i][j].y();
321  double dz=sc_pos[i][j+1].z()-sc_pos[i][j].z();
322  temp.push_back(DVector3(dx/dz,dy/dz,1.));
323  }
324  sc_dir.push_back(temp);
325  }
326  SC_END_NOSE_Z=sc_pos[0][12].z();
327  SC_BARREL_R2=sc_pos[0][0].Perp2();
328  SC_PHI_SECTOR1=sc_pos[0][0].Phi();
329  }
330 
331  // Get z positions of fdc wire planes
333  // for now, assume the z extent of a package is the difference between the positions
334  // of the two wire planes. save half of this distance
338 
339  ADD_VERTEX_POINT=false;
340  gPARMS->SetDefaultParameter("KALMAN:ADD_VERTEX_POINT", ADD_VERTEX_POINT);
341 
342  THETA_CUT=60.0;
343  gPARMS->SetDefaultParameter("KALMAN:THETA_CUT", THETA_CUT);
344 
345  RING_TO_SKIP=0;
346  gPARMS->SetDefaultParameter("KALMAN:RING_TO_SKIP",RING_TO_SKIP);
347 
348  PLANE_TO_SKIP=0;
349  gPARMS->SetDefaultParameter("KALMAN:PLANE_TO_SKIP",PLANE_TO_SKIP);
350 
352  gPARMS->SetDefaultParameter("KALMAN:MINIMUM_HIT_FRACTION",MINIMUM_HIT_FRACTION);
354  gPARMS->SetDefaultParameter("KALMAN:MIN_HITS_FOR_REFIT", MIN_HITS_FOR_REFIT);
355  PHOTON_ENERGY_CUTOFF=0.125;
356  gPARMS->SetDefaultParameter("KALMAN:PHOTON_ENERGY_CUTOFF",
358 
359  USE_FDC_HITS=true;
360  gPARMS->SetDefaultParameter("TRKFIT:USE_FDC_HITS",USE_FDC_HITS);
361  USE_CDC_HITS=true;
362  gPARMS->SetDefaultParameter("TRKFIT:USE_CDC_HITS",USE_CDC_HITS);
363 
364  // Flag to enable calculation of alignment derivatives
365  ALIGNMENT=false;
366  gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT",ALIGNMENT);
367 
368  ALIGNMENT_FORWARD=false;
369  gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT_FORWARD",ALIGNMENT_FORWARD);
370 
371  ALIGNMENT_CENTRAL=false;
372  gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT_CENTRAL",ALIGNMENT_CENTRAL);
373 
375 
376  DEBUG_HISTS=false;
377  gPARMS->SetDefaultParameter("KALMAN:DEBUG_HISTS", DEBUG_HISTS);
378 
379  DEBUG_LEVEL=0;
380  gPARMS->SetDefaultParameter("KALMAN:DEBUG_LEVEL", DEBUG_LEVEL);
381 
383  gPARMS->SetDefaultParameter("KALMAN:USE_T0_FROM_WIRES", USE_T0_FROM_WIRES);
384 
385  ESTIMATE_T0_TB=false;
386  gPARMS->SetDefaultParameter("KALMAN:ESTIMATE_T0_TB",ESTIMATE_T0_TB);
387 
389  gPARMS->SetDefaultParameter("GEOM:ENABLE_BOUNDARY_CHECK",
391 
392  USE_MULS_COVARIANCE=true;
393  gPARMS->SetDefaultParameter("TRKFIT:USE_MULS_COVARIANCE",
395 
396  USE_PASS1_TIME_MODE=false;
397  gPARMS->SetDefaultParameter("KALMAN:USE_PASS1_TIME_MODE",USE_PASS1_TIME_MODE);
398 
399  USE_FDC_DRIFT_TIMES=true;
400  gPARMS->SetDefaultParameter("TRKFIT:USE_FDC_DRIFT_TIMES",
402 
404  gPARMS->SetDefaultParameter("KALMAN:RECOVER_BROKEN_TRACKS",RECOVER_BROKEN_TRACKS);
405 
406  NUM_CDC_SIGMA_CUT=5.0;
407  NUM_FDC_SIGMA_CUT=5.0;
408  gPARMS->SetDefaultParameter("KALMAN:NUM_CDC_SIGMA_CUT",NUM_CDC_SIGMA_CUT,
409  "maximum distance in number of sigmas away from projection to accept cdc hit");
410  gPARMS->SetDefaultParameter("KALMAN:NUM_FDC_SIGMA_CUT",NUM_FDC_SIGMA_CUT,
411  "maximum distance in number of sigmas away from projection to accept fdc hit");
412 
413  ANNEAL_SCALE=9.0;
414  ANNEAL_POW_CONST=1.5;
415  gPARMS->SetDefaultParameter("KALMAN:ANNEAL_SCALE",ANNEAL_SCALE,
416  "Scale factor for annealing");
417  gPARMS->SetDefaultParameter("KALMAN:ANNEAL_POW_CONST",ANNEAL_POW_CONST,
418  "Annealing parameter");
421  gPARMS->SetDefaultParameter("KALMAN:FORWARD_ANNEAL_SCALE",
423  "Scale factor for annealing");
424  gPARMS->SetDefaultParameter("KALMAN:FORWARD_ANNEAL_POW_CONST",
426  "Annealing parameter");
427 
428  FORWARD_PARMS_COV=false;
429  gPARMS->SetDefaultParameter("KALMAN:FORWARD_PARMS_COV",FORWARD_PARMS_COV);
430 
432  gPARMS->SetDefaultParameter("KALMAN:CDC_VAR_SCALE_FACTOR",CDC_VAR_SCALE_FACTOR);
433  CDC_T_DRIFT_MIN=-12.; // One f125 clock = 8 ns
434  gPARMS->SetDefaultParameter("KALMAN:CDC_T_DRIFT_MIN",CDC_T_DRIFT_MIN);
435 
436  MOLIERE_FRACTION=0.9;
437  gPARMS->SetDefaultParameter("KALMAN:MOLIERE_FRACTION",MOLIERE_FRACTION);
438  MS_SCALE_FACTOR=2.0;
439  gPARMS->SetDefaultParameter("KALMAN:MS_SCALE_FACTOR",MS_SCALE_FACTOR);
441  MOLIERE_RATIO2=MS_SCALE_FACTOR*1.e-6/(1.+MOLIERE_FRACTION*MOLIERE_FRACTION); //scale_factor/(1+F*F)
442 
444  gPARMS->SetDefaultParameter("KALMAN:COVARIANCE_SCALE_FACTOR_CENTRAL",
446 
448  gPARMS->SetDefaultParameter("KALMAN:COVARIANCE_SCALE_FACTOR_FORWARD",
450 
452  gPARMS->SetDefaultParameter("KALMAN:WRITE_ML_TRAINING_OUTPUT",
454 
456  mlfile.open("mltraining.dat");
457  }
458 
459 
460  DApplication* dapp = dynamic_cast<DApplication*>(loop->GetJApplication());
461  JCalibration *jcalib = dapp->GetJCalibration((loop->GetJEvent()).GetRunNumber());
462  vector< map<string, double> > tvals;
463  cdc_drift_table.clear();
464  if (jcalib->Get("CDC/cdc_drift_table", tvals)==false){
465  for(unsigned int i=0; i<tvals.size(); i++){
466  map<string, double> &row = tvals[i];
467  cdc_drift_table.push_back(1000.*row["t"]);
468  }
469  }
470  else{
471  jerr << " CDC time-to-distance table not available... bailing..." << endl;
472  exit(0);
473  }
474 
475  int straw_number[28]={42,42,54,54,66,66,80,80,93,93,106,106,
476  123,123,135,135,146,146,158,158,170,170,
477  182,182,197,197,209,209};
478  max_sag.clear();
479  sag_phi_offset.clear();
480  int straw_count=0,ring_count=0;
481  if (jcalib->Get("CDC/sag_parameters", tvals)==false){
482  vector<double>temp,temp2;
483  for(unsigned int i=0; i<tvals.size(); i++){
484  map<string, double> &row = tvals[i];
485 
486  temp.push_back(row["offset"]);
487  temp2.push_back(row["phi"]);
488 
489  straw_count++;
490  if (straw_count==straw_number[ring_count]){
491  max_sag.push_back(temp);
492  sag_phi_offset.push_back(temp2);
493  temp.clear();
494  temp2.clear();
495  straw_count=0;
496  ring_count++;
497  }
498  }
499  }
500 
501  if (jcalib->Get("CDC/drift_parameters", tvals)==false){
502  map<string, double> &row = tvals[0]; // long drift side
503  long_drift_func[0][0]=row["a1"];
504  long_drift_func[0][1]=row["a2"];
505  long_drift_func[0][2]=row["a3"];
506  long_drift_func[1][0]=row["b1"];
507  long_drift_func[1][1]=row["b2"];
508  long_drift_func[1][2]=row["b3"];
509  long_drift_func[2][0]=row["c1"];
510  long_drift_func[2][1]=row["c2"];
511  long_drift_func[2][2]=row["c3"];
512  long_drift_Bscale_par1=row["B1"];
513  long_drift_Bscale_par2=row["B2"];
514 
515  row = tvals[1]; // short drift side
516  short_drift_func[0][0]=row["a1"];
517  short_drift_func[0][1]=row["a2"];
518  short_drift_func[0][2]=row["a3"];
519  short_drift_func[1][0]=row["b1"];
520  short_drift_func[1][1]=row["b2"];
521  short_drift_func[1][2]=row["b3"];
522  short_drift_func[2][0]=row["c1"];
523  short_drift_func[2][1]=row["c2"];
524  short_drift_func[2][2]=row["c3"];
525  short_drift_Bscale_par1=row["B1"];
526  short_drift_Bscale_par2=row["B2"];
527  }
528 
529  map<string, double> cdc_drift_parms;
530  jcalib->Get("CDC/cdc_drift_parms", cdc_drift_parms);
531  CDC_DRIFT_BSCALE_PAR1 = cdc_drift_parms["bscale_par1"];
532  CDC_DRIFT_BSCALE_PAR2 = cdc_drift_parms["bscale_par2"];
533 
534  map<string, double> cdc_res_parms;
535  jcalib->Get("CDC/cdc_resolution_parms_v2", cdc_res_parms);
536  CDC_RES_PAR1 = cdc_res_parms["res_par1"];
537  CDC_RES_PAR2 = cdc_res_parms["res_par2"];
538  CDC_RES_PAR3 = cdc_res_parms["res_par3"];
539 
540  // Parameters for correcting for deflection due to Lorentz force
541  map<string,double>lorentz_parms;
542  jcalib->Get("FDC/lorentz_deflection_parms",lorentz_parms);
543  LORENTZ_NR_PAR1=lorentz_parms["nr_par1"];
544  LORENTZ_NR_PAR2=lorentz_parms["nr_par2"];
545  LORENTZ_NZ_PAR1=lorentz_parms["nz_par1"];
546  LORENTZ_NZ_PAR2=lorentz_parms["nz_par2"];
547 
548  // Parameters for accounting for variation in drift distance from FDC
549  map<string,double>drift_res_parms;
550  jcalib->Get("FDC/drift_resolution_parms",drift_res_parms);
551  DRIFT_RES_PARMS[0]=drift_res_parms["p0"];
552  DRIFT_RES_PARMS[1]=drift_res_parms["p1"];
553  DRIFT_RES_PARMS[2]=drift_res_parms["p2"];
554 
555  // Time-to-distance function parameters for FDC
556  map<string,double>drift_func_parms;
557  jcalib->Get("FDC/drift_function_parms",drift_func_parms);
558  DRIFT_FUNC_PARMS[0]=drift_func_parms["p0"];
559  DRIFT_FUNC_PARMS[1]=drift_func_parms["p1"];
560  DRIFT_FUNC_PARMS[2]=drift_func_parms["p2"];
561  DRIFT_FUNC_PARMS[3]=drift_func_parms["p3"];
562  DRIFT_FUNC_PARMS[4]=1000.;
563  DRIFT_FUNC_PARMS[5]=0.;
564  map<string,double>drift_func_ext;
565  if (jcalib->Get("FDC/drift_function_ext",drift_func_ext)==false){
566  DRIFT_FUNC_PARMS[4]=drift_func_ext["p4"];
567  DRIFT_FUNC_PARMS[5]=drift_func_ext["p5"];
568  }
569  // Factors for taking care of B-dependence of drift time for FDC
570  map<string, double> fdc_drift_parms;
571  jcalib->Get("FDC/fdc_drift_parms", fdc_drift_parms);
572  FDC_DRIFT_BSCALE_PAR1 = fdc_drift_parms["bscale_par1"];
573  FDC_DRIFT_BSCALE_PAR2 = fdc_drift_parms["bscale_par2"];
574 
575 
576  /*
577  if (jcalib->Get("FDC/fdc_drift2", tvals)==false){
578  for(unsigned int i=0; i<tvals.size(); i++){
579  map<string, float> &row = tvals[i];
580  iter_float iter = row.begin();
581  fdc_drift_table[i] = iter->second;
582  }
583  }
584  else{
585  jerr << " FDC time-to-distance table not available... bailing..." << endl;
586  exit(0);
587  }
588  */
589 
590  for (unsigned int i=0;i<5;i++)I5x5(i,i)=1.;
591 
592 
593  // center of the target
594  map<string, double> targetparms;
595  if (jcalib->Get("TARGET/target_parms",targetparms)==false){
596  TARGET_Z = targetparms["TARGET_Z_POSITION"];
597  }
598  else{
600  }
601  if (ADD_VERTEX_POINT){
602  gPARMS->SetDefaultParameter("KALMAN:VERTEX_POSITION",TARGET_Z);
603  }
604 
605  // Beam position and direction
606  map<string, double> beam_vals;
607  jcalib->Get("PHOTON_BEAM/beam_spot",beam_vals);
608  beam_center.Set(beam_vals["x"],beam_vals["y"]);
609  beam_dir.Set(beam_vals["dxdz"],beam_vals["dydz"]);
610  jout << " Beam spot: x=" << beam_center.X() << " y=" << beam_center.Y()
611  << " z=" << beam_vals["z"]
612  << " dx/dz=" << beam_dir.X() << " dy/dz=" << beam_dir.Y() << endl;
613  beam_z0=beam_vals["z"];
614 
615  // Inform user of some configuration settings
616  static bool config_printed = false;
617  if(!config_printed){
618  config_printed = true;
619  stringstream ss;
620  ss << "vertex constraint: " ;
621  if(ADD_VERTEX_POINT){
622  ss << "z = " << TARGET_Z << "cm" << endl;
623  }else{
624  ss << "<none>" << endl;
625  }
626  jout << ss.str();
627  } // config_printed
628 
629  if(DEBUG_HISTS){
630  for (auto i=0; i < 46; i++){
631  double min = -10., max=10.;
632  if(i%23<12) {min=-5; max=5;}
633  if(i<23)alignDerivHists[i]=new TH1I(Form("CentralDeriv%i",i),Form("CentralDeriv%i",i),200, min, max);
634  else alignDerivHists[i]=new TH1I(Form("ForwardDeriv%i",i%23),Form("ForwardDeriv%i",i%23),200, min, max);
635  }
636  brentCheckHists[0]=new TH2I("ForwardBrentCheck","DOCA vs ds", 100, -5., 5., 100, 0.0, 1.5);
637  brentCheckHists[1]=new TH2I("CentralBrentCheck","DOCA vs ds", 100, -5., 5., 100, 0.0, 1.5);
638  }
639 
640  dResourcePool_TMatrixFSym = std::make_shared<DResourcePool<TMatrixFSym>>();
641  dResourcePool_TMatrixFSym->Set_ControlParams(20, 20, 50);
642 
643  my_fdchits.reserve(24);
644  my_cdchits.reserve(28);
645  fdc_updates.reserve(24);
646  cdc_updates.reserve(28);
647  cdc_used_in_fit.reserve(28);
648  fdc_used_in_fit.reserve(24);
649 }
650 
651 //-----------------
652 // ResetKalmanSIMD
653 //-----------------
655 {
657 
658  for (unsigned int i=0;i<my_cdchits.size();i++){
659  delete my_cdchits[i];
660  }
661  for (unsigned int i=0;i<my_fdchits.size();i++){
662  delete my_fdchits[i];
663  }
664  central_traj.clear();
665  forward_traj.clear();
666  my_fdchits.clear();
667  my_cdchits.clear();
668  fdc_updates.clear();
669  cdc_updates.clear();
670  fdc_used_in_fit.clear();
671  cdc_used_in_fit.clear();
672 
673  cov.clear();
674  fcov.clear();
675 
676  len = 0.0;
677  ftime=0.0;
678  var_ftime=0.0;
679  x_=0.,y_=0.,tx_=0.,ty_=0.,q_over_p_ = 0.0;
680  z_=0.,phi_=0.,tanl_=0.,q_over_pt_ =0, D_= 0.0;
681  chisq_ = 0.0;
682  ndf_ = 0;
683  MASS=0.13957;
684  mass2=MASS*MASS;
685  Bx=By=0.;
686  Bz=2.;
687  dBxdx=0.,dBxdy=0.,dBxdz=0.,dBydx=0.,dBydy=0.,dBydy=0.,dBzdx=0.,dBzdy=0.,dBzdz=0.;
688  // Step sizes
689  mStepSizeS=2.0;
690  mStepSizeZ=2.0;
691  //mStepSizeZ=0.5;
692 
693  //if (fit_type==kTimeBased){
694  // mStepSizeS=0.5;
695  // mStepSizeZ=0.5;
696  // }
697 
698 
699  mT0=0.,mT0MinimumDriftTime=1e6;
700  mVarT0=25.;
701 
703  //mCDCInternalStepSize=1.0;
704  //mCentralStepSize=0.75;
705  mCentralStepSize=0.75;
706 
708 
709  IsHadron=true;
710  IsElectron=false;
711  IsPositron=false;
712 
713  PT_MIN=0.01;
714  Q_OVER_P_MAX=100.;
715 
716  // These variables provide the approximate location along the trajectory
717  // where there is an indication of a kink in the track
721 
722 }
723 
724 //-----------------
725 // FitTrack
726 //-----------------
728 {
729  // Reset member data and free an memory associated with the last fit,
730  // but some of which only for wire-based fits
731  ResetKalmanSIMD();
732 
733  // Check that we have enough FDC and CDC hits to proceed
734  if (cdchits.size()==0 && fdchits.size()<4) return kFitNotDone;
735  if (cdchits.size()+fdchits.size() < 6) return kFitNotDone;
736 
737  // Copy hits from base class into structures specific to DTrackFitterKalmanSIMD
738  if (USE_CDC_HITS)
739  for(unsigned int i=0; i<cdchits.size(); i++)AddCDCHit(cdchits[i]);
740  if (USE_FDC_HITS)
741  for(unsigned int i=0; i<fdchits.size(); i++)AddFDCHit(fdchits[i]);
742 
743  unsigned int num_good_cdchits=my_cdchits.size();
744  unsigned int num_good_fdchits=my_fdchits.size();
745 
746  // keep track of the range of detector elements that could be hit
747  // for calculating the number of expected hits later on
748  //int min_cdc_ring=-1, max_cdc_ring=-1;
749 
750  // Order the cdc hits by ring number
751  if (num_good_cdchits>0){
752  stable_sort(my_cdchits.begin(),my_cdchits.end(),DKalmanSIMDCDCHit_cmp);
753 
754  //min_cdc_ring = my_cdchits[0]->hit->wire->ring;
755  //max_cdc_ring = my_cdchits[my_cdchits.size()-1]->hit->wire->ring;
756 
757  // Look for multiple hits on the same wire
758  for (unsigned int i=0;i<my_cdchits.size()-1;i++){
759  if (my_cdchits[i]->hit->wire->ring==my_cdchits[i+1]->hit->wire->ring &&
760  my_cdchits[i]->hit->wire->straw==my_cdchits[i+1]->hit->wire->straw){
761  num_good_cdchits--;
762  if (my_cdchits[i]->tdrift<my_cdchits[i+1]->tdrift){
763  my_cdchits[i+1]->status=late_hit;
764  }
765  else{
766  my_cdchits[i]->status=late_hit;
767  }
768  }
769  }
770 
771  }
772  // Order the fdc hits by z
773  if (num_good_fdchits>0){
774  stable_sort(my_fdchits.begin(),my_fdchits.end(),DKalmanSIMDFDCHit_cmp);
775 
776  // Look for multiple hits on the same wire
777  for (unsigned int i=0;i<my_fdchits.size()-1;i++){
778  if (my_fdchits[i]->hit->wire->layer==my_fdchits[i+1]->hit->wire->layer &&
779  my_fdchits[i]->hit->wire->wire==my_fdchits[i+1]->hit->wire->wire){
780  num_good_fdchits--;
781  if (fabs(my_fdchits[i]->t-my_fdchits[i+1]->t)<EPS){
782  double tsum_1=my_fdchits[i]->hit->t_u+my_fdchits[i]->hit->t_v;
783  double tsum_2=my_fdchits[i+1]->hit->t_u+my_fdchits[i+1]->hit->t_v;
784  if (tsum_1<tsum_2){
785  my_fdchits[i+1]->status=late_hit;
786  }
787  else{
788  my_fdchits[i]->status=late_hit;
789  }
790  }
791  else if (my_fdchits[i]->t<my_fdchits[i+1]->t){
792  my_fdchits[i+1]->status=late_hit;
793  }
794  else{
795  my_fdchits[i]->status=late_hit;
796  }
797  }
798  }
799  }
800  if (num_good_cdchits==0 && num_good_fdchits<4) return kFitNotDone;
801  if(num_good_cdchits+num_good_fdchits < 6) return kFitNotDone;
802 
803  // Create vectors of updates (from hits) to S and C
804  if (my_cdchits.size()>0){
805  cdc_updates=vector<DKalmanUpdate_t>(my_cdchits.size());
806  // Initialize vector to keep track of whether or not a hit is used in
807  // the fit
808  cdc_used_in_fit=vector<bool>(my_cdchits.size());
809  }
810  if (my_fdchits.size()>0){
811  fdc_updates=vector<DKalmanUpdate_t>(my_fdchits.size());
812  // Initialize vector to keep track of whether or not a hit is used in
813  // the fit
814  fdc_used_in_fit=vector<bool>(my_fdchits.size());
815  }
816 
817  // start time and variance
818  if (fit_type==kTimeBased){
819  mT0=input_params.t0();
820  switch(input_params.t0_detector()){
821  case SYS_TOF:
822  mVarT0=0.01;
823  break;
824  case SYS_CDC:
825  mVarT0=7.5;
826  break;
827  case SYS_FDC:
828  mVarT0=7.5;
829  break;
830  case SYS_BCAL:
831  mVarT0=0.25;
832  break;
833  default:
834  mVarT0=0.09;
835  break;
836  }
837  }
838 
839  //_DBG_ << SystemName(input_params.t0_detector()) << " " << mT0 <<endl;
840 
841  //Set the mass
843  mass2=MASS*MASS;
846 
847  // Is this particle an electron or positron?
848  if (MASS<0.001){
849  IsHadron=false;
850  if (input_params.charge()<0.) IsElectron=true;
851  else IsPositron=true;
852  }
853  if (DEBUG_LEVEL>0)
854  {
855  _DBG_ << "------Starting "
856  <<(fit_type==kTimeBased?"Time-based":"Wire-based")
857  << " Fit with " << my_fdchits.size() << " FDC hits and "
858  << my_cdchits.size() << " CDC hits.-------" <<endl;
859  if (fit_type==kTimeBased){
860  _DBG_ << " Using t0=" << mT0 << " from DET="
861  << input_params.t0_detector() <<endl;
862  }
863  }
864  // Do the fit
865  jerror_t error = KalmanLoop();
866  if (error!=NOERROR){
867  if (DEBUG_LEVEL>0)
868  _DBG_ << "Fit failed with Error = " << error <<endl;
869  return kFitFailed;
870  }
871 
872  // Copy fit results into DTrackFitter base-class data members
873  DVector3 mom,pos;
874  GetPosition(pos);
875  GetMomentum(mom);
876  double charge = GetCharge();
877  fit_params.setPosition(pos);
878  fit_params.setMomentum(mom);
880  fit_params.setPID(IDTrack(charge, MASS));
882 
883  if (DEBUG_LEVEL>0){
884  _DBG_ << "----- Pass: "
885  << (fit_type==kTimeBased?"Time-based ---":"Wire-based ---")
886  << " Mass: " << MASS
887  << " p=" << mom.Mag()
888  << " theta=" << 90.0-180./M_PI*atan(tanl_)
889  << " vertex=(" << x_ << "," << y_ << "," << z_<<")"
890  << " chi2=" << chisq_
891  <<endl;
892  if(DEBUG_LEVEL>3){
893  //Dump pulls
894  for (unsigned int iPull = 0; iPull < pulls.size(); iPull++){
895  if (pulls[iPull].cdc_hit != NULL){
896  _DBG_ << " ring: " << pulls[iPull].cdc_hit->wire->ring
897  << " straw: " << pulls[iPull].cdc_hit->wire->straw
898  << " Residual: " << pulls[iPull].resi
899  << " Err: " << pulls[iPull].err
900  << " tdrift: " << pulls[iPull].tdrift
901  << " doca: " << pulls[iPull].d
902  << " docaphi: " << pulls[iPull].docaphi
903  << " z: " << pulls[iPull].z
904  << " cos(theta_rel): " << pulls[iPull].cosThetaRel
905  << " tcorr: " << pulls[iPull].tcorr
906  << endl;
907  }
908  }
909  }
910  }
911 
912  DMatrixDSym errMatrix(5);
913  // Fill the tracking error matrix and the one needed for kinematic fitting
914  if (fcov.size()!=0){
915  // We MUST fill the entire matrix (not just upper right) even though
916  // this is a DMatrixDSym
917  for (unsigned int i=0;i<5;i++){
918  for (unsigned int j=0;j<5;j++){
919  errMatrix(i,j)=fcov[i][j];
920  }
921  }
922  if (FORWARD_PARMS_COV){
925 
926  // Compute and fill the error matrix needed for kinematic fitting
928  }
929  else {
932 
933  // Compute and fill the error matrix needed for kinematic fitting
935  }
936  }
937  else if (cov.size()!=0){
939 
940  // We MUST fill the entire matrix (not just upper right) even though
941  // this is a DMatrixDSym
942  for (unsigned int i=0;i<5;i++){
943  for (unsigned int j=0;j<5;j++){
944  errMatrix(i,j)=cov[i][j];
945  }
946  }
948 
949  // Compute and fill the error matrix needed for kinematic fitting
951  }
952  auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource();
953  locTrackingCovarianceMatrix->ResizeTo(5, 5);
954  for(unsigned int loc_i = 0; loc_i < 5; ++loc_i)
955  {
956  for(unsigned int loc_j = 0; loc_j < 5; ++loc_j)
957  (*locTrackingCovarianceMatrix)(loc_i, loc_j) = errMatrix(loc_i, loc_j);
958 
959  }
960  fit_params.setTrackingErrorMatrix(locTrackingCovarianceMatrix);
961  this->chisq = GetChiSq();
962  this->Ndof = GetNDF();
964 
965  // figure out the number of expected hits for this track based on the final fit
966  set<const DCDCWire *> expected_hit_straws;
967  set<int> expected_hit_fdc_planes;
968 
969  for(uint i=0; i<extrapolations[SYS_CDC].size(); i++) {
970  // figure out the radial position of the point to see which ring it's in
971  double r = extrapolations[SYS_CDC][i].position.Perp();
972  uint ring=0;
973  for(; ring<cdc_rmid.size(); ring++) {
974  //_DBG_ << "Rs = " << r << " " << cdc_rmid[ring] << endl;
975  if( (r<cdc_rmid[ring]-0.78) || (fabs(r-cdc_rmid[ring])<0.78) )
976  break;
977  }
978  if(ring == cdc_rmid.size()) ring--;
979  //_DBG_ << "ring = " << ring << endl;
980  //_DBG_ << "ring = " << ring << " stereo = " << cdcwires[ring][0]->stereo << endl;
981  int best_straw=0;
982  double best_dist_diff=fabs((extrapolations[SYS_CDC][i].position
983  - cdcwires[ring][0]->origin).Mag());
984  // match based on straw center
985  for(uint straw=1; straw<cdcwires[ring].size(); straw++) {
986  DVector3 wire_position = cdcwires[ring][straw]->origin; // start with the nominal wire center
987  // now take into account the z dependence due to the stereo angle
988  double dz = extrapolations[SYS_CDC][i].position.Z() - cdcwires[ring][straw]->origin.Z();
989  double ds = dz*tan(cdcwires[ring][straw]->stereo);
990  wire_position += DVector3(-ds*sin(cdcwires[ring][straw]->origin.Phi()), ds*cos(cdcwires[ring][straw]->origin.Phi()), dz);
991  double diff = fabs((extrapolations[SYS_CDC][i].position
992  - wire_position).Mag());
993  if( diff < best_dist_diff )
994  best_straw = straw;
995  }
996 
997  expected_hit_straws.insert(cdcwires[ring][best_straw]);
998  }
999 
1000  for(uint i=0; i<extrapolations[SYS_FDC].size(); i++) {
1001  // check to make sure that the track goes through the sensitive region of the FDC
1002  // assume one hit per plane
1003  double z = extrapolations[SYS_FDC][i].position.Z();
1004  double r = extrapolations[SYS_FDC][i].position.Perp();
1005 
1006  // see if we're in the "sensitive area" of a package
1007  for(uint plane=0; plane<fdc_z_wires.size(); plane++) {
1008  int package = plane/6;
1009  if(fabs(z-fdc_z_wires[plane]) < fdc_package_size) {
1010  if( r<fdc_rmax && r>fdc_rmin_packages[package]) {
1011  expected_hit_fdc_planes.insert(plane);
1012  }
1013  break; // found the right plane
1014  }
1015  }
1016  }
1017 
1018  potential_cdc_hits_on_track = expected_hit_straws.size();
1019  potential_fdc_hits_on_track = expected_hit_fdc_planes.size();
1020 
1021  if(DEBUG_LEVEL>0) {
1022  _DBG_ << " CDC hits/potential hits " << my_cdchits.size() << "/" << potential_cdc_hits_on_track
1023  << " FDC hits/potential hits " << my_fdchits.size() << "/" << potential_fdc_hits_on_track << endl;
1024  }
1025 
1026  //_DBG_ << "========= done!" << endl;
1027 
1028  return fit_status;
1029 }
1030 
1031 //-----------------
1032 // ChiSq
1033 //-----------------
1034 double DTrackFitterKalmanSIMD::ChiSq(fit_type_t fit_type, DReferenceTrajectory *rt, double *chisq_ptr, int *dof_ptr, vector<pull_t> *pulls_ptr)
1035 {
1036  // This simply returns whatever was left in for the chisq/NDF from the last fit.
1037  // Using a DReferenceTrajectory is not really appropriate here so the base class'
1038  // requirement of it should be reviewed.
1039  double chisq = GetChiSq();
1040  unsigned int ndf = GetNDF();
1041 
1042  if(chisq_ptr)*chisq_ptr = chisq;
1043  if(dof_ptr)*dof_ptr = int(ndf);
1044  if(pulls_ptr)*pulls_ptr = pulls;
1045 
1046  return chisq/double(ndf);
1047 }
1048 
1049 // Return the momentum at the distance of closest approach to the origin.
1051  double pt=1./fabs(q_over_pt_);
1052  mom.SetXYZ(pt*cos(phi_),pt*sin(phi_),pt*tanl_);
1053 }
1054 
1055 // Return the "vertex" position (position at which track crosses beam line)
1057  pos.SetXYZ(x_,y_,z_);
1058 }
1059 
1060 // Add FDC hits
1063 
1064  hit->t=fdchit->time;
1065  hit->uwire=fdchit->w;
1066  hit->vstrip=fdchit->s;
1067  hit->vvar=fdchit->ds*fdchit->ds;
1068  hit->z=fdchit->wire->origin.z();
1069  hit->cosa=cos(fdchit->wire->angle);
1070  hit->sina=sin(fdchit->wire->angle);
1071  hit->phiX=fdchit->wire->angles.X();
1072  hit->phiY=fdchit->wire->angles.Y();
1073  hit->phiZ=fdchit->wire->angles.Z();
1074 
1075  hit->nr=0.;
1076  hit->nz=0.;
1077  hit->dE=1e6*fdchit->dE;
1078  hit->hit=fdchit;
1079  hit->status=good_hit;
1080 
1081  my_fdchits.push_back(hit);
1082 
1083  return NOERROR;
1084 }
1085 
1086 // Add CDC hits
1089 
1090  hit->hit=cdchit;
1091  hit->status=good_hit;
1092  hit->origin.Set(cdchit->wire->origin.x(),cdchit->wire->origin.y());
1093  double one_over_uz=1./cdchit->wire->udir.z();
1094  hit->dir.Set(one_over_uz*cdchit->wire->udir.x(),
1095  one_over_uz*cdchit->wire->udir.y());
1096  hit->z0wire=cdchit->wire->origin.z();
1097  hit->cosstereo=cos(cdchit->wire->stereo);
1098  hit->tdrift=cdchit->tdrift;
1099  my_cdchits.push_back(hit);
1100 
1101  return NOERROR;
1102 }
1103 
1104 // Calculate the derivative of the state vector with respect to z
1106  const DMatrix5x1 &S,
1107  double dEdx,
1108  DMatrix5x1 &D){
1109  double tx=S(state_tx),ty=S(state_ty);
1110  double q_over_p=S(state_q_over_p);
1111 
1112  // Turn off dEdx if the magnitude of the momentum drops below some cutoff
1113  if (fabs(q_over_p)>Q_OVER_P_MAX){
1114  dEdx=0.;
1115  }
1116  // Try to keep the direction tangents from heading towards 90 degrees
1117  if (fabs(tx)>TAN_MAX) tx=TAN_MAX*(tx>0.0?1.:-1.);
1118  if (fabs(ty)>TAN_MAX) ty=TAN_MAX*(ty>0.0?1.:-1.);
1119 
1120  // useful combinations of terms
1121  double kq_over_p=qBr2p*q_over_p;
1122  double tx2=tx*tx;
1123  double ty2=ty*ty;
1124  double txty=tx*ty;
1125  double one_plus_tx2=1.+tx2;
1126  double dsdz=sqrt(one_plus_tx2+ty2);
1127  double dtx_Bfac=ty*Bz+txty*Bx-one_plus_tx2*By;
1128  double dty_Bfac=Bx*(1.+ty2)-txty*By-tx*Bz;
1129  double kq_over_p_dsdz=kq_over_p*dsdz;
1130 
1131  // Derivative of S with respect to z
1132  D(state_x)=tx;
1133  D(state_y)=ty;
1134  D(state_tx)=kq_over_p_dsdz*dtx_Bfac;
1135  D(state_ty)=kq_over_p_dsdz*dty_Bfac;
1136 
1137  D(state_q_over_p)=0.;
1138  if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS){
1139  double q_over_p_sq=q_over_p*q_over_p;
1140  double E=sqrt(1./q_over_p_sq+mass2);
1141  D(state_q_over_p)=-q_over_p_sq*q_over_p*E*dEdx*dsdz;
1142  }
1143  return NOERROR;
1144 }
1145 
1146 // Reference trajectory for forward tracks in CDC region
1147 // At each point we store the state vector and the Jacobian needed to get to
1148 //this state along z from the previous state.
1150  int i=0,forward_traj_length=forward_traj.size();
1151  double z=z_;
1152  double r2=0.;
1153  bool stepped_to_boundary=false;
1154 
1155  // Magnetic field and gradient at beginning of trajectory
1156  //bfield->GetField(x_,y_,z_,Bx,By,Bz);
1160 
1161  // Reset cdc status flags
1162  for (unsigned int i=0;i<my_cdchits.size();i++){
1163  if (my_cdchits[i]->status!=late_hit) my_cdchits[i]->status=good_hit;
1164  }
1165 
1166  // Continue adding to the trajectory until we have reached the endplate
1167  // or the maximum radius
1168  while(z<endplate_z_downstream && z>cdc_origin[2] &&
1170  && fabs(S(state_tx))<TAN_MAX
1171  && fabs(S(state_ty))<TAN_MAX
1172  ){
1173  if (PropagateForwardCDC(forward_traj_length,i,z,r2,S,stepped_to_boundary)
1174  !=NOERROR) return UNRECOVERABLE_ERROR;
1175  }
1176 
1177  // Only use hits that would fall within the range of the reference trajectory
1178  /*
1179  for (unsigned int i=0;i<my_cdchits.size();i++){
1180  DKalmanSIMDCDCHit_t *hit=my_cdchits[i];
1181  double my_r2=(hit->origin+(z-hit->z0wire)*hit->dir).Mod2();
1182  if (my_r2>r2) hit->status=bad_hit;
1183  }
1184  */
1185 
1186  // If the current length of the trajectory deque is less than the previous
1187  // trajectory deque, remove the extra elements and shrink the deque
1188  if (i<(int)forward_traj.size()){
1189  forward_traj_length=forward_traj.size();
1190  for (int j=0;j<forward_traj_length-i;j++){
1191  forward_traj.pop_front();
1192  }
1193  }
1194 
1195  // return an error if there are not enough entries in the trajectory
1196  if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE;
1197 
1198  if (DEBUG_LEVEL>20)
1199  {
1200  cout << "--- Forward cdc trajectory ---" <<endl;
1201  for (unsigned int m=0;m<forward_traj.size();m++){
1202  // DMatrix5x1 S=*(forward_traj[m].S);
1203  DMatrix5x1 S=(forward_traj[m].S);
1204  double tx=S(state_tx),ty=S(state_ty);
1205  double phi=atan2(ty,tx);
1206  double cosphi=cos(phi);
1207  double sinphi=sin(phi);
1208  double p=fabs(1./S(state_q_over_p));
1209  double tanl=1./sqrt(tx*tx+ty*ty);
1210  double sinl=sin(atan(tanl));
1211  double cosl=cos(atan(tanl));
1212  cout
1213  << setiosflags(ios::fixed)<< "pos: " << setprecision(4)
1214  << forward_traj[m].S(state_x) << ", "
1215  << forward_traj[m].S(state_y) << ", "
1216  << forward_traj[m].z << " mom: "
1217  << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", "
1218  << p*sinl << " -> " << p
1219  <<" s: " << setprecision(3)
1220  << forward_traj[m].s
1221  <<" t: " << setprecision(3)
1222  << forward_traj[m].t/SPEED_OF_LIGHT
1223  <<" B: " << forward_traj[m].B
1224  << endl;
1225  }
1226  }
1227 
1228  // Current state vector
1229  S=forward_traj[0].S;
1230 
1231  // position at the end of the swim
1232  x_=forward_traj[0].S(state_x);
1233  y_=forward_traj[0].S(state_y);
1234  z_=forward_traj[0].z;
1235 
1236  return NOERROR;
1237 }
1238 
1239 // Routine that extracts the state vector propagation part out of the reference
1240 // trajectory loop
1242  double &z,double &r2,
1243  DMatrix5x1 &S,
1244  bool &stepped_to_boundary){
1245  DMatrix5x5 J,Q;
1247  int my_i=0;
1248  temp.h_id=0;
1249  temp.num_hits=0;
1250  double dEdx=0.;
1251  double s_to_boundary=1e6;
1252  double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
1253 
1254  // current position
1255  DVector3 pos(S(state_x),S(state_y),z);
1256  temp.z=z;
1257  // squared radius
1258  r2=pos.Perp2();
1259 
1260  temp.s=len;
1261  temp.t=ftime;
1262  temp.S=S;
1263 
1264  // Kinematic variables
1265  double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
1266  double one_over_beta2=1.+mass2*q_over_p_sq;
1267  if (one_over_beta2>BIG) one_over_beta2=BIG;
1268 
1269  // get material properties from the Root Geometry
1271  DVector3 mom(S(state_tx),S(state_ty),1.);
1272  if(geom->FindMatKalman(pos,mom,temp.K_rho_Z_over_A,
1273  temp.rho_Z_over_A,temp.LnI,temp.Z,
1274  temp.chi2c_factor,temp.chi2a_factor,
1275  temp.chi2a_corr,
1277  &s_to_boundary)!=NOERROR){
1278  return UNRECOVERABLE_ERROR;
1279  }
1280  }
1281  else
1282  {
1283  if(geom->FindMatKalman(pos,temp.K_rho_Z_over_A,
1284  temp.rho_Z_over_A,temp.LnI,temp.Z,
1285  temp.chi2c_factor,temp.chi2a_factor,
1286  temp.chi2a_corr,
1287  last_material_map)!=NOERROR){
1288  return UNRECOVERABLE_ERROR;
1289  }
1290  }
1291 
1292  // Get dEdx for the upcoming step
1293  if (CORRECT_FOR_ELOSS){
1295  temp.LnI,temp.Z);
1296  }
1297 
1298  index++;
1299  if (index<=length){
1300  my_i=length-index;
1301  forward_traj[my_i].s=temp.s;
1302  forward_traj[my_i].t=temp.t;
1303  forward_traj[my_i].h_id=temp.h_id;
1304  forward_traj[my_i].num_hits=0;
1305  forward_traj[my_i].z=temp.z;
1306  forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A;
1307  forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A;
1308  forward_traj[my_i].LnI=temp.LnI;
1309  forward_traj[my_i].Z=temp.Z;
1310  forward_traj[my_i].S=S;
1311  }
1312 
1313  // Determine the step size based on energy loss
1314  //double step=mStepSizeS*dz_ds;
1315  double max_step_size
1316  =(z<endplate_z&& r2>endplate_r2min)?mCDCInternalStepSize:mStepSizeS;
1317  double ds=mStepSizeS;
1318  if (z<endplate_z && r2<endplate_r2max && z>cdc_origin[2]){
1319  if (!stepped_to_boundary){
1320  stepped_to_boundary=false;
1321  if (fabs(dEdx)>EPS){
1322  ds=DE_PER_STEP/fabs(dEdx);
1323  }
1324  if (ds>max_step_size) ds=max_step_size;
1325  if (s_to_boundary<ds){
1326  ds=s_to_boundary+EPS3;
1327  stepped_to_boundary=true;
1328  }
1329  if(ds<MIN_STEP_SIZE)ds=MIN_STEP_SIZE;
1330  }
1331  else{
1332  ds=MIN_STEP_SIZE;
1333  stepped_to_boundary=false;
1334  }
1335  }
1336  double newz=z+ds*dz_ds; // new z position
1337 
1338  // Store magnetic field
1339  temp.B=sqrt(Bx*Bx+By*By+Bz*Bz);
1340 
1341  // Step through field
1342  ds=FasterStep(z,newz,dEdx,S);
1343 
1344  // update path length
1345  len+=fabs(ds);
1346 
1347  // Update flight time
1348  ftime+=ds*sqrt(one_over_beta2);// in units where c=1
1349 
1350  // Get the contribution to the covariance matrix due to multiple
1351  // scattering
1353  temp.S,Q);
1354 
1355  // Energy loss straggling
1356  if (CORRECT_FOR_ELOSS){
1357  double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A);
1358  Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2;
1359  }
1360 
1361  // Compute the Jacobian matrix and its transpose
1362  StepJacobian(newz,z,S,dEdx,J);
1363 
1364  // update the trajectory
1365  if (index<=length){
1366  forward_traj[my_i].B=temp.B;
1367  forward_traj[my_i].Q=Q;
1368  forward_traj[my_i].J=J;
1369  }
1370  else{
1371  temp.Q=Q;
1372  temp.J=J;
1373  temp.Ckk=Zero5x5;
1374  temp.Skk=Zero5x1;
1375  forward_traj.push_front(temp);
1376  }
1377 
1378  //update z
1379  z=newz;
1380 
1381  return NOERROR;
1382 }
1383 
1384 // Routine that extracts the state vector propagation part out of the reference
1385 // trajectory loop
1387  DVector2 &my_xy,
1388  double &var_t_factor,
1389  DMatrix5x1 &Sc,
1390  bool &stepped_to_boundary){
1392  DMatrix5x5 J; // State vector Jacobian matrix
1393  DMatrix5x5 Q; // Process noise covariance matrix
1394 
1395  //Initialize some variables needed later
1396  double dEdx=0.;
1397  double s_to_boundary=1e6;
1398  int my_i=0;
1399  // Kinematic variables
1400  double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
1401  double q_over_p_sq=q_over_p*q_over_p;
1402  double one_over_beta2=1.+mass2*q_over_p_sq;
1403  if (one_over_beta2>BIG) one_over_beta2=BIG;
1404 
1405  // Reset D to zero
1406  Sc(state_D)=0.;
1407 
1408  temp.xy=my_xy;
1409  temp.s=len;
1410  temp.t=ftime;
1411  temp.h_id=0;
1412  temp.S=Sc;
1413 
1414  // Store magnitude of magnetic field
1415  temp.B=sqrt(Bx*Bx+By*By+Bz*Bz);
1416 
1417  // get material properties from the Root Geometry
1418  DVector3 pos3d(my_xy.X(),my_xy.Y(),Sc(state_z));
1420  DVector3 mom(cos(Sc(state_phi)),sin(Sc(state_phi)),Sc(state_tanl));
1421  if(geom->FindMatKalman(pos3d,mom,temp.K_rho_Z_over_A,
1422  temp.rho_Z_over_A,temp.LnI,temp.Z,
1423  temp.chi2c_factor,temp.chi2a_factor,
1424  temp.chi2a_corr,
1426  &s_to_boundary)
1427  !=NOERROR){
1428  return UNRECOVERABLE_ERROR;
1429  }
1430  }
1431  else if(geom->FindMatKalman(pos3d,temp.K_rho_Z_over_A,
1432  temp.rho_Z_over_A,temp.LnI,temp.Z,
1433  temp.chi2c_factor,temp.chi2a_factor,
1434  temp.chi2a_corr,
1435  last_material_map)!=NOERROR){
1436  return UNRECOVERABLE_ERROR;
1437  }
1438 
1439  if (CORRECT_FOR_ELOSS){
1440  dEdx=GetdEdx(q_over_p,temp.K_rho_Z_over_A,temp.rho_Z_over_A,temp.LnI,
1441  temp.Z);
1442  }
1443 
1444  // If the deque already exists, update it
1445  index++;
1446  if (index<=length){
1447  my_i=length-index;
1448  central_traj[my_i].B=temp.B;
1449  central_traj[my_i].s=temp.s;
1450  central_traj[my_i].t=temp.t;
1451  central_traj[my_i].h_id=0;
1452  central_traj[my_i].xy=temp.xy;
1453  central_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A;
1454  central_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A;
1455  central_traj[my_i].LnI=temp.LnI;
1456  central_traj[my_i].Z=temp.Z;
1457  central_traj[my_i].S=Sc;
1458  }
1459 
1460  // Adjust the step size
1461  double step_size=mStepSizeS;
1462  if (stepped_to_boundary){
1463  step_size=MIN_STEP_SIZE;
1464  stepped_to_boundary=false;
1465  }
1466  else{
1467  if (fabs(dEdx)>EPS){
1468  step_size=DE_PER_STEP/fabs(dEdx);
1469  }
1470  if(step_size>mStepSizeS) step_size=mStepSizeS;
1471  if (s_to_boundary<step_size){
1472  step_size=s_to_boundary+EPS3;
1473  stepped_to_boundary=true;
1474  }
1475  if(step_size<MIN_STEP_SIZE)step_size=MIN_STEP_SIZE;
1476  }
1477  double r2=my_xy.Mod2();
1478  if (r2>endplate_r2min
1479  && step_size>mCDCInternalStepSize) step_size=mCDCInternalStepSize;
1480  // Propagate the state through the field
1481  FasterStep(my_xy,step_size,Sc,dEdx);
1482 
1483  // update path length
1484  len+=step_size;
1485 
1486  // Update flight time
1487  double dt=step_size*sqrt(one_over_beta2); // in units of c=1
1488  double one_minus_beta2=1.-1./one_over_beta2;
1489  ftime+=dt;
1490  var_ftime+=dt*dt*one_minus_beta2*one_minus_beta2*0.0004;
1491  var_t_factor=dt*dt*one_minus_beta2*one_minus_beta2;
1492 
1493  //printf("t %f sigt %f\n",TIME_UNIT_CONVERSION*ftime,TIME_UNIT_CONVERSION*sqrt(var_ftime));
1494 
1495  // Multiple scattering
1496  GetProcessNoiseCentral(step_size,temp.chi2c_factor,temp.chi2a_factor,
1497  temp.chi2a_corr,temp.S,Q);
1498 
1499  // Energy loss straggling in the approximation of thick absorbers
1500  if (CORRECT_FOR_ELOSS){
1501  double varE
1502  =GetEnergyVariance(step_size,one_over_beta2,temp.K_rho_Z_over_A);
1504  +=varE*temp.S(state_q_over_pt)*temp.S(state_q_over_pt)*one_over_beta2
1505  *q_over_p_sq;
1506  }
1507 
1508  // B-field and gradient at current (x,y,z)
1509  bfield->GetFieldAndGradient(my_xy.X(),my_xy.Y(),Sc(state_z),Bx,By,Bz,
1512 
1513  // Compute the Jacobian matrix and its transpose
1514  StepJacobian(my_xy,temp.xy-my_xy,-step_size,Sc,dEdx,J);
1515 
1516  // Update the trajectory info
1517  if (index<=length){
1518  central_traj[my_i].Q=Q;
1519  central_traj[my_i].J=J;
1520  }
1521  else{
1522  temp.Q=Q;
1523  temp.J=J;
1524  temp.Ckk=Zero5x5;
1525  temp.Skk=Zero5x1;
1526  central_traj.push_front(temp);
1527  }
1528 
1529  return NOERROR;
1530 }
1531 
1532 
1533 
1534 // Reference trajectory for central tracks
1535 // At each point we store the state vector and the Jacobian needed to get to this state
1536 // along s from the previous state.
1537 // The tricky part is that we swim out from the target to find Sc and pos along the trajectory
1538 // but we need the Jacobians for the opposite direction, because the filter proceeds from
1539 // the outer hits toward the target.
1541  DMatrix5x1 &Sc){
1542  bool stepped_to_boundary=false;
1543  int i=0,central_traj_length=central_traj.size();
1544  // factor for scaling momentum resolution to propagate variance in flight
1545  // time
1546  double var_t_factor=0;
1547 
1548  // Magnetic field and gradient at beginning of trajectory
1549  //bfield->GetField(x_,y_,z_,Bx,By,Bz);
1553 
1554  // Copy of initial position in xy
1555  DVector2 my_xy=xy;
1556  double r2=xy.Mod2(),z=z_;
1557 
1558  // Reset cdc status flags
1559  for (unsigned int j=0;j<my_cdchits.size();j++){
1560  if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit;
1561  }
1562 
1563  // Continue adding to the trajectory until we have reached the endplate
1564  // or the maximum radius
1565  while(z<endplate_z && z>=Z_MIN && r2<endplate_r2max
1566  && fabs(Sc(state_q_over_pt))<Q_OVER_PT_MAX
1567  ){
1568  if (PropagateCentral(central_traj_length,i,my_xy,var_t_factor,Sc,stepped_to_boundary)
1569  !=NOERROR) return UNRECOVERABLE_ERROR;
1570  z=Sc(state_z);
1571  r2=my_xy.Mod2();
1572  }
1573 
1574  // If the current length of the trajectory deque is less than the previous
1575  // trajectory deque, remove the extra elements and shrink the deque
1576  if (i<(int)central_traj.size()){
1577  int central_traj_length=central_traj.size();
1578  for (int j=0;j<central_traj_length-i;j++){
1579  central_traj.pop_front();
1580  }
1581  }
1582 
1583  // Only use hits that would fall within the range of the reference trajectory
1584  /*for (unsigned int j=0;j<my_cdchits.size();j++){
1585  DKalmanSIMDCDCHit_t *hit=my_cdchits[j];
1586  double my_r2=(hit->origin+(z-hit->z0wire)*hit->dir).Mod2();
1587  if (my_r2>r2) hit->status=bad_hit;
1588  }
1589  */
1590 
1591  // return an error if there are not enough entries in the trajectory
1592  if (central_traj.size()<2) return RESOURCE_UNAVAILABLE;
1593 
1594  if (DEBUG_LEVEL>20)
1595  {
1596  cout << "---------" << central_traj.size() <<" entries------" <<endl;
1597  for (unsigned int m=0;m<central_traj.size();m++){
1598  DMatrix5x1 S=central_traj[m].S;
1599  double cosphi=cos(S(state_phi));
1600  double sinphi=sin(S(state_phi));
1601  double pt=fabs(1./S(state_q_over_pt));
1602  double tanl=S(state_tanl);
1603 
1604  cout
1605  << m << "::"
1606  << setiosflags(ios::fixed)<< "pos: " << setprecision(4)
1607  << central_traj[m].xy.X() << ", "
1608  << central_traj[m].xy.Y() << ", "
1609  << central_traj[m].S(state_z) << " mom: "
1610  << pt*cosphi << ", " << pt*sinphi << ", "
1611  << pt*tanl << " -> " << pt/cos(atan(tanl))
1612  <<" s: " << setprecision(3)
1613  << central_traj[m].s
1614  <<" t: " << setprecision(3)
1615  << central_traj[m].t/SPEED_OF_LIGHT
1616  <<" B: " << central_traj[m].B
1617  << endl;
1618  }
1619  }
1620 
1621  // State at end of swim
1622  Sc=central_traj[0].S;
1623 
1624  return NOERROR;
1625 }
1626 
1627 // Routine that extracts the state vector propagation part out of the reference
1628 // trajectory loop
1629 jerror_t DTrackFitterKalmanSIMD::PropagateForward(int length,int &i,
1630  double &z,double zhit,
1631  DMatrix5x1 &S, bool &done,
1632  bool &stepped_to_boundary,
1633  bool &stepped_to_endplate){
1634  DMatrix5x5 J,Q;
1636 
1637  // Initialize some variables
1638  temp.h_id=0;
1639  temp.num_hits=0;
1640  int my_i=0;
1641  double s_to_boundary=1e6;
1642  double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
1643 
1644  // current position
1645  DVector3 pos(S(state_x),S(state_y),z);
1646  double r2=pos.Perp2();
1647 
1648  temp.s=len;
1649  temp.t=ftime;
1650  temp.z=z;
1651  temp.S=S;
1652 
1653  // Kinematic variables
1654  double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
1655  double one_over_beta2=1.+mass2*q_over_p_sq;
1656  if (one_over_beta2>BIG) one_over_beta2=BIG;
1657 
1658  // get material properties from the Root Geometry
1660  DVector3 mom(S(state_tx),S(state_ty),1.);
1661  if (geom->FindMatKalman(pos,mom,temp.K_rho_Z_over_A,
1662  temp.rho_Z_over_A,temp.LnI,temp.Z,
1663  temp.chi2c_factor,temp.chi2a_factor,
1664  temp.chi2a_corr,
1666  &s_to_boundary)
1667  !=NOERROR){
1668  return UNRECOVERABLE_ERROR;
1669  }
1670  }
1671  else
1672  {
1673  if (geom->FindMatKalman(pos,temp.K_rho_Z_over_A,
1674  temp.rho_Z_over_A,temp.LnI,temp.Z,
1675  temp.chi2c_factor,temp.chi2a_factor,
1676  temp.chi2a_corr,
1677  last_material_map)!=NOERROR){
1678  return UNRECOVERABLE_ERROR;
1679  }
1680  }
1681 
1682  // Get dEdx for the upcoming step
1683  double dEdx=0.;
1684  if (CORRECT_FOR_ELOSS){
1686  temp.rho_Z_over_A,temp.LnI,temp.Z);
1687  }
1688  i++;
1689  my_i=length-i;
1690  if (i<=length){
1691  forward_traj[my_i].s=temp.s;
1692  forward_traj[my_i].t=temp.t;
1693  forward_traj[my_i].h_id=temp.h_id;
1694  forward_traj[my_i].num_hits=temp.num_hits;
1695  forward_traj[my_i].z=temp.z;
1696  forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A;
1697  forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A;
1698  forward_traj[my_i].LnI=temp.LnI;
1699  forward_traj[my_i].S=S;
1700  }
1701  else{
1702  temp.S=S;
1703  }
1704 
1705  // Determine the step size based on energy loss
1706  // step=mStepSizeS*dz_ds;
1707  double max_step_size
1708  =(z<endplate_z&& r2>endplate_r2min)?mCentralStepSize:mStepSizeS;
1709  double ds=mStepSizeS;
1710  if (z>cdc_origin[2]){
1711  if (!stepped_to_boundary){
1712  stepped_to_boundary=false;
1713  if (fabs(dEdx)>EPS){
1714  ds=DE_PER_STEP/fabs(dEdx);
1715  }
1716  if (ds>max_step_size) ds=max_step_size;
1717  if (s_to_boundary<ds){
1718  ds=s_to_boundary+EPS3;
1719  stepped_to_boundary=true;
1720  }
1721  if(ds<MIN_STEP_SIZE)ds=MIN_STEP_SIZE;
1722 
1723  }
1724  else{
1725  ds=MIN_STEP_SIZE;
1726  stepped_to_boundary=false;
1727  }
1728  }
1729 
1730  double dz=stepped_to_endplate ? endplate_dz : ds*dz_ds;
1731  double newz=z+dz; // new z position
1732  // Check if we are stepping through the CDC endplate
1733  if (newz>endplate_z && z<endplate_z){
1734  // _DBG_ << endl;
1735  newz=endplate_z+EPS3;
1736  stepped_to_endplate=true;
1737  }
1738 
1739  // Check if we are about to step to one of the wire planes
1740  done=false;
1741  if (newz>zhit){
1742  newz=zhit;
1743  done=true;
1744  }
1745 
1746  // Store magnitude of magnetic field
1747  temp.B=sqrt(Bx*Bx+By*By+Bz*Bz);
1748 
1749  // Step through field
1750  ds=FasterStep(z,newz,dEdx,S);
1751 
1752  // update path length
1753  len+=ds;
1754 
1755  // update flight time
1756  ftime+=ds*sqrt(one_over_beta2); // in units where c=1
1757 
1758  // Get the contribution to the covariance matrix due to multiple
1759  // scattering
1761  temp.S,Q);
1762 
1763  // Energy loss straggling
1764  if (CORRECT_FOR_ELOSS){
1765  double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A);
1766  Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2;
1767  }
1768 
1769  // Compute the Jacobian matrix and its transpose
1770  StepJacobian(newz,z,S,dEdx,J);
1771 
1772  // update the trajectory data
1773  if (i<=length){
1774  forward_traj[my_i].B=temp.B;
1775  forward_traj[my_i].Q=Q;
1776  forward_traj[my_i].J=J;
1777  }
1778  else{
1779  temp.Q=Q;
1780  temp.J=J;
1781  temp.Ckk=Zero5x5;
1782  temp.Skk=Zero5x1;
1783  forward_traj.push_front(temp);
1784  }
1785 
1786  // update z
1787  z=newz;
1788 
1789  return NOERROR;
1790 }
1791 
1792 // Reference trajectory for trajectories with hits in the forward direction
1793 // At each point we store the state vector and the Jacobian needed to get to this state
1794 // along z from the previous state.
1796 
1797  // Magnetic field and gradient at beginning of trajectory
1798  //bfield->GetField(x_,y_,z_,Bx,By,Bz);
1802 
1803  // progress in z from hit to hit
1804  double z=z_;
1805  int i=0;
1806 
1807  int forward_traj_length=forward_traj.size();
1808  // loop over the fdc hits
1809  double zhit=0.,old_zhit=0.;
1810  bool stepped_to_boundary=false;
1811  bool stepped_to_endplate=false;
1812  unsigned int m=0;
1813  for (m=0;m<my_fdchits.size();m++){
1814  if (fabs(S(state_q_over_p))>Q_OVER_P_MAX
1815  || fabs(S(state_tx))>TAN_MAX
1816  || fabs(S(state_ty))>TAN_MAX
1817  || S(state_x)*S(state_x)+S(state_y)*S(state_y)>50.*50.
1818  || z>400. || z<Z_MIN
1819  ){
1820  break;
1821  }
1822 
1823  zhit=my_fdchits[m]->z;
1824  if (fabs(old_zhit-zhit)>EPS){
1825  bool done=false;
1826  while (!done){
1827  if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX
1828  || fabs(S(state_tx))>TAN_MAX
1829  || fabs(S(state_ty))>TAN_MAX
1830  || S(state_x)*S(state_x)+S(state_y)*S(state_y)>50.*50.
1831  || z>400. || z< Z_MIN
1832  ){
1833  break;
1834  }
1835 
1836  if (PropagateForward(forward_traj_length,i,z,zhit,S,done,
1837  stepped_to_boundary,stepped_to_endplate)
1838  !=NOERROR)
1839  return UNRECOVERABLE_ERROR;
1840  }
1841  }
1842  old_zhit=zhit;
1843  }
1844 
1845  // If m<2 then no useable FDC hits survived the check on the magnitude on the
1846  // momentum
1847  if (m<2) return UNRECOVERABLE_ERROR;
1848 
1849  // Make sure the reference trajectory goes one step beyond the most
1850  // downstream hit plane
1851  if (m==my_fdchits.size()){
1852  bool done=false;
1853  if (PropagateForward(forward_traj_length,i,z,400.,S,done,
1854  stepped_to_boundary,stepped_to_endplate)
1855  !=NOERROR)
1856  return UNRECOVERABLE_ERROR;
1857  if (PropagateForward(forward_traj_length,i,z,400.,S,done,
1858  stepped_to_boundary,stepped_to_endplate)
1859  !=NOERROR)
1860  return UNRECOVERABLE_ERROR;
1861  }
1862 
1863  // Shrink the deque if the new trajectory has less points in it than the
1864  // old trajectory
1865  if (i<(int)forward_traj.size()){
1866  int mylen=forward_traj.size();
1867  //_DBG_ << "Shrinking: " << mylen << " to " << i << endl;
1868  for (int j=0;j<mylen-i;j++){
1869  forward_traj.pop_front();
1870  }
1871  // _DBG_ << " Now " << forward_traj.size() << endl;
1872  }
1873 
1874  // If we lopped off some hits on the downstream end, truncate the trajectory to
1875  // the point in z just beyond the last valid hit
1876  unsigned int my_id=my_fdchits.size();
1877  if (m<my_id){
1878  if (zhit<z) my_id=m;
1879  else my_id=m-1;
1880  zhit=my_fdchits[my_id-1]->z;
1881  //_DBG_ << "Shrinking: " << forward_traj.size()<< endl;
1882  for (;;){
1883  z=forward_traj[0].z;
1884  if (z<zhit+EPS2) break;
1885  forward_traj.pop_front();
1886  }
1887  //_DBG_ << " Now " << forward_traj.size() << endl;
1888 
1889  // Temporory structure keeping state and trajectory information
1891  temp.h_id=0;
1892  temp.num_hits=0;
1893  temp.B=0.;
1894  temp.K_rho_Z_over_A=temp.rho_Z_over_A=temp.LnI=0.;
1895  temp.Q=Zero5x5;
1896 
1897  // last S vector
1898  S=forward_traj[0].S;
1899 
1900  // Step just beyond the last hit
1901  double newz=z+0.01;
1902  double ds=Step(z,newz,0.,S); // ignore energy loss for this small step
1903  temp.s=forward_traj[0].s+ds;
1904  temp.z=newz;
1905  temp.S=S;
1906 
1907  // Flight time
1908  double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
1909  double one_over_beta2=1.+mass2*q_over_p_sq;
1910  if (one_over_beta2>BIG) one_over_beta2=BIG;
1911  temp.t=forward_traj[0].t+ds*sqrt(one_over_beta2); // in units where c=1
1912 
1913  // Covariance and state vector needed for smoothing code
1914  temp.Ckk=Zero5x5;
1915  temp.Skk=Zero5x1;
1916 
1917  // Jacobian matrices
1918  temp.J=I5x5;
1919 
1920  forward_traj.push_front(temp);
1921  }
1922 
1923  // return an error if there are not enough entries in the trajectory
1924  if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE;
1925 
1926  // Fill in Lorentz deflection parameters
1927  for (unsigned int m=0;m<forward_traj.size();m++){
1928  if (my_id>0){
1929  unsigned int hit_id=my_id-1;
1930  double z=forward_traj[m].z;
1931  if (fabs(z-my_fdchits[hit_id]->z)<EPS2){
1932  forward_traj[m].h_id=my_id;
1933 
1934  // Get the magnetic field at this position along the trajectory
1936  z,Bx,By,Bz);
1937  double Br=sqrt(Bx*Bx+By*By);
1938 
1939  // Angle between B and wire
1940  double my_phi=0.;
1941  if (Br>0.) my_phi=acos((Bx*my_fdchits[hit_id]->sina
1942  +By*my_fdchits[hit_id]->cosa)/Br);
1943  /*
1944  lorentz_def->GetLorentzCorrectionParameters(forward_traj[m].pos.x(),
1945  forward_traj[m].pos.y(),
1946  forward_traj[m].pos.z(),
1947  tanz,tanr);
1948  my_fdchits[hit_id]->nr=tanr;
1949  my_fdchits[hit_id]->nz=tanz;
1950  */
1951 
1952  my_fdchits[hit_id]->nr=LORENTZ_NR_PAR1*Bz*(1.+LORENTZ_NR_PAR2*Br);
1953  my_fdchits[hit_id]->nz=(LORENTZ_NZ_PAR1+LORENTZ_NZ_PAR2*Bz)*(Br*cos(my_phi));
1954 
1955 
1956  my_id--;
1957 
1958  unsigned int num=1;
1959  while (hit_id>0
1960  && fabs(my_fdchits[hit_id]->z-my_fdchits[hit_id-1]->z)<EPS){
1961  hit_id=my_id-1;
1962  num++;
1963  my_id--;
1964  }
1965  forward_traj[m].num_hits=num;
1966  }
1967 
1968  }
1969  }
1970 
1971  if (DEBUG_LEVEL>20)
1972  {
1973  cout << "--- Forward fdc trajectory ---" <<endl;
1974  for (unsigned int m=0;m<forward_traj.size();m++){
1975  DMatrix5x1 S=(forward_traj[m].S);
1976  double tx=S(state_tx),ty=S(state_ty);
1977  double phi=atan2(ty,tx);
1978  double cosphi=cos(phi);
1979  double sinphi=sin(phi);
1980  double p=fabs(1./S(state_q_over_p));
1981  double tanl=1./sqrt(tx*tx+ty*ty);
1982  double sinl=sin(atan(tanl));
1983  double cosl=cos(atan(tanl));
1984  cout
1985  << setiosflags(ios::fixed)<< "pos: " << setprecision(4)
1986  << forward_traj[m].S(state_x) << ", "
1987  << forward_traj[m].S(state_y) << ", "
1988  << forward_traj[m].z << " mom: "
1989  << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", "
1990  << p*sinl << " -> " << p
1991  <<" s: " << setprecision(3)
1992  << forward_traj[m].s
1993  <<" t: " << setprecision(3)
1994  << forward_traj[m].t/SPEED_OF_LIGHT
1995  <<" id: " << forward_traj[m].h_id
1996  << endl;
1997  }
1998  }
1999 
2000 
2001  // position at the end of the swim
2002  z_=z;
2003  x_=S(state_x);
2004  y_=S(state_y);
2005 
2006  return NOERROR;
2007 }
2008 
2009 // Step the state vector through the field from oldz to newz.
2010 // Uses the 4th-order Runga-Kutte algorithm.
2011 double DTrackFitterKalmanSIMD::Step(double oldz,double newz, double dEdx,
2012  DMatrix5x1 &S){
2013  double delta_z=newz-oldz;
2014  if (fabs(delta_z)<EPS) return 0.; // skip if the step is too small
2015 
2016  // Direction tangents
2017  double tx=S(state_tx);
2018  double ty=S(state_ty);
2019  double ds=sqrt(1.+tx*tx+ty*ty)*delta_z;
2020 
2021  double delta_z_over_2=0.5*delta_z;
2022  double midz=oldz+delta_z_over_2;
2023  DMatrix5x1 D1,D2,D3,D4;
2024 
2025  //B-field and gradient at at (x,y,z)
2029  double Bx0=Bx,By0=By,Bz0=Bz;
2030 
2031  // Calculate the derivative and propagate the state to the next point
2032  CalcDeriv(oldz,S,dEdx,D1);
2033  DMatrix5x1 S1=S+delta_z_over_2*D1;
2034 
2035  // Calculate the field at the first intermediate point
2036  double dx=S1(state_x)-S(state_x);
2037  double dy=S1(state_y)-S(state_y);
2038  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2;
2039  By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2;
2040  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2;
2041 
2042  // Calculate the derivative and propagate the state to the next point
2043  CalcDeriv(midz,S1,dEdx,D2);
2044  S1=S+delta_z_over_2*D2;
2045 
2046  // Calculate the field at the second intermediate point
2047  dx=S1(state_x)-S(state_x);
2048  dy=S1(state_y)-S(state_y);
2049  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2;
2050  By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2;
2051  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2;
2052 
2053  // Calculate the derivative and propagate the state to the next point
2054  CalcDeriv(midz,S1,dEdx,D3);
2055  S1=S+delta_z*D3;
2056 
2057  // Calculate the field at the final point
2058  dx=S1(state_x)-S(state_x);
2059  dy=S1(state_y)-S(state_y);
2060  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z;
2061  By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z;
2062  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z;
2063 
2064  // Final derivative
2065  CalcDeriv(newz,S1,dEdx,D4);
2066 
2067  // S+=delta_z*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4);
2068  double dz_over_6=delta_z*ONE_SIXTH;
2069  double dz_over_3=delta_z*ONE_THIRD;
2070  S+=dz_over_6*D1;
2071  S+=dz_over_3*D2;
2072  S+=dz_over_3*D3;
2073  S+=dz_over_6*D4;
2074 
2075  // Don't let the magnitude of the momentum drop below some cutoff
2076  //if (fabs(S(state_q_over_p))>Q_OVER_P_MAX)
2077  // S(state_q_over_p)=Q_OVER_P_MAX*(S(state_q_over_p)>0.0?1.:-1.);
2078  // Try to keep the direction tangents from heading towards 90 degrees
2079  //if (fabs(S(state_tx))>TAN_MAX)
2080  // S(state_tx)=TAN_MAX*(S(state_tx)>0.0?1.:-1.);
2081  //if (fabs(S(state_ty))>TAN_MAX)
2082  // S(state_ty)=TAN_MAX*(S(state_ty)>0.0?1.:-1.);
2083 
2084  return ds;
2085 }
2086 // Step the state vector through the field from oldz to newz.
2087 // Uses the 4th-order Runga-Kutte algorithm.
2088 // Uses the gradient to compute the field at the intermediate and last
2089 // points.
2090 double DTrackFitterKalmanSIMD::FasterStep(double oldz,double newz, double dEdx,
2091  DMatrix5x1 &S){
2092  double delta_z=newz-oldz;
2093  if (fabs(delta_z)<EPS) return 0.; // skip if the step is too small
2094 
2095  // Direction tangents
2096  double tx=S(state_tx);
2097  double ty=S(state_ty);
2098  double ds=sqrt(1.+tx*tx+ty*ty)*delta_z;
2099 
2100  double delta_z_over_2=0.5*delta_z;
2101  double midz=oldz+delta_z_over_2;
2102  DMatrix5x1 D1,D2,D3,D4;
2103  double Bx0=Bx,By0=By,Bz0=Bz;
2104 
2105  // The magnetic field at the beginning of the step is assumed to be
2106  // obtained at the end of the previous step through StepJacobian
2107 
2108  // Calculate the derivative and propagate the state to the next point
2109  CalcDeriv(oldz,S,dEdx,D1);
2110  DMatrix5x1 S1=S+delta_z_over_2*D1;
2111 
2112  // Calculate the field at the first intermediate point
2113  double dx=S1(state_x)-S(state_x);
2114  double dy=S1(state_y)-S(state_y);
2115  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2;
2116  By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2;
2117  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2;
2118 
2119  // Calculate the derivative and propagate the state to the next point
2120  CalcDeriv(midz,S1,dEdx,D2);
2121  S1=S+delta_z_over_2*D2;
2122 
2123  // Calculate the field at the second intermediate point
2124  dx=S1(state_x)-S(state_x);
2125  dy=S1(state_y)-S(state_y);
2126  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2;
2127  By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2;
2128  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2;
2129 
2130  // Calculate the derivative and propagate the state to the next point
2131  CalcDeriv(midz,S1,dEdx,D3);
2132  S1=S+delta_z*D3;
2133 
2134  // Calculate the field at the final point
2135  dx=S1(state_x)-S(state_x);
2136  dy=S1(state_y)-S(state_y);
2137  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z;
2138  By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z;
2139  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z;
2140 
2141  // Final derivative
2142  CalcDeriv(newz,S1,dEdx,D4);
2143 
2144  // S+=delta_z*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4);
2145  double dz_over_6=delta_z*ONE_SIXTH;
2146  double dz_over_3=delta_z*ONE_THIRD;
2147  S+=dz_over_6*D1;
2148  S+=dz_over_3*D2;
2149  S+=dz_over_3*D3;
2150  S+=dz_over_6*D4;
2151 
2152  // Don't let the magnitude of the momentum drop below some cutoff
2153  //if (fabs(S(state_q_over_p))>Q_OVER_P_MAX)
2154  // S(state_q_over_p)=Q_OVER_P_MAX*(S(state_q_over_p)>0.0?1.:-1.);
2155  // Try to keep the direction tangents from heading towards 90 degrees
2156  //if (fabs(S(state_tx))>TAN_MAX)
2157  // S(state_tx)=TAN_MAX*(S(state_tx)>0.0?1.:-1.);
2158  //if (fabs(S(state_ty))>TAN_MAX)
2159  // S(state_ty)=TAN_MAX*(S(state_ty)>0.0?1.:-1.);
2160 
2161  return ds;
2162 }
2163 
2164 
2165 
2166 // Compute the Jacobian matrix for the forward parametrization.
2167 jerror_t DTrackFitterKalmanSIMD::StepJacobian(double oldz,double newz,
2168  const DMatrix5x1 &S,
2169  double dEdx,DMatrix5x5 &J){
2170  // Initialize the Jacobian matrix
2171  //J.Zero();
2172  //for (int i=0;i<5;i++) J(i,i)=1.;
2173  J=I5x5;
2174 
2175  // Step in z
2176  double delta_z=newz-oldz;
2177  if (fabs(delta_z)<EPS) return NOERROR; //skip if the step is too small
2178 
2179  // Current values of state vector variables
2180  double x=S(state_x), y=S(state_y),tx=S(state_tx),ty=S(state_ty);
2181  double q_over_p=S(state_q_over_p);
2182 
2183  //B-field and field gradient at (x,y,z)
2184  //if (get_field)
2186  dBxdz,dBydx,dBydy,
2188 
2189  // Don't let the magnitude of the momentum drop below some cutoff
2190  if (fabs(q_over_p)>Q_OVER_P_MAX){
2191  q_over_p=Q_OVER_P_MAX*(q_over_p>0.0?1.:-1.);
2192  dEdx=0.;
2193  }
2194  // Try to keep the direction tangents from heading towards 90 degrees
2195  if (fabs(tx)>TAN_MAX) tx=TAN_MAX*(tx>0.0?1.:-1.);
2196  if (fabs(ty)>TAN_MAX) ty=TAN_MAX*(ty>0.0?1.:-1.);
2197  // useful combinations of terms
2198  double kq_over_p=qBr2p*q_over_p;
2199  double tx2=tx*tx;
2200  double twotx2=2.*tx2;
2201  double ty2=ty*ty;
2202  double twoty2=2.*ty2;
2203  double txty=tx*ty;
2204  double one_plus_tx2=1.+tx2;
2205  double one_plus_ty2=1.+ty2;
2206  double one_plus_twotx2_plus_ty2=one_plus_ty2+twotx2;
2207  double one_plus_twoty2_plus_tx2=one_plus_tx2+twoty2;
2208  double dsdz=sqrt(1.+tx2+ty2);
2209  double ds=dsdz*delta_z;
2210  double kds=qBr2p*ds;
2211  double kqdz_over_p_over_dsdz=kq_over_p*delta_z/dsdz;
2212  double kq_over_p_ds=kq_over_p*ds;
2213  double dtx_Bdep=ty*Bz+txty*Bx-one_plus_tx2*By;
2214  double dty_Bdep=Bx*one_plus_ty2-txty*By-tx*Bz;
2215  double Bxty=Bx*ty;
2216  double Bytx=By*tx;
2217  double Bztxty=Bz*txty;
2218  double Byty=By*ty;
2219  double Bxtx=Bx*tx;
2220 
2221  // Jacobian
2222  J(state_x,state_tx)=J(state_y,state_ty)=delta_z;
2223  J(state_tx,state_q_over_p)=kds*dtx_Bdep;
2224  J(state_ty,state_q_over_p)=kds*dty_Bdep;
2225  J(state_tx,state_tx)+=kqdz_over_p_over_dsdz*(Bxty*(one_plus_twotx2_plus_ty2)
2226  -Bytx*(3.*one_plus_tx2+twoty2)
2227  +Bztxty);
2228  J(state_tx,state_x)=kq_over_p_ds*(ty*dBzdx+txty*dBxdx-one_plus_tx2*dBydx);
2229  J(state_ty,state_ty)+=kqdz_over_p_over_dsdz*(Bxty*(3.*one_plus_ty2+twotx2)
2230  -Bytx*(one_plus_twoty2_plus_tx2)
2231  -Bztxty);
2232  J(state_ty,state_y)= kq_over_p_ds*(one_plus_ty2*dBxdy-txty*dBydy-tx*dBzdy);
2233  J(state_tx,state_ty)=kqdz_over_p_over_dsdz
2234  *((Bxtx+Bz)*(one_plus_twoty2_plus_tx2)-Byty*one_plus_tx2);
2235  J(state_tx,state_y)= kq_over_p_ds*(tx*dBzdy+txty*dBxdy-one_plus_tx2*dBydy);
2236  J(state_ty,state_tx)=-kqdz_over_p_over_dsdz*((Byty+Bz)*(one_plus_twotx2_plus_ty2)
2237  -Bxtx*one_plus_ty2);
2238  J(state_ty,state_x)=kq_over_p_ds*(one_plus_ty2*dBxdx-txty*dBydx-tx*dBzdx);
2239  if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS){
2240  double one_over_p_sq=q_over_p*q_over_p;
2241  double E=sqrt(1./one_over_p_sq+mass2);
2242  J(state_q_over_p,state_q_over_p)-=dEdx*ds/E*(2.+3.*mass2*one_over_p_sq);
2243  double temp=-(q_over_p*one_over_p_sq/dsdz)*E*dEdx*delta_z;
2246  }
2247 
2248  return NOERROR;
2249 }
2250 
2251 // Calculate the derivative for the alternate set of parameters {q/pT, phi,
2252 // tan(lambda),D,z}
2254  double dEdx,DMatrix5x1 &D1){
2255  //Direction at current point
2256  double tanl=S(state_tanl);
2257  // Don't let tanl exceed some maximum
2258  if (fabs(tanl)>TAN_MAX) tanl=TAN_MAX*(tanl>0.0?1.:-1.);
2259 
2260  double phi=S(state_phi);
2261  double cosphi=cos(phi);
2262  double sinphi=sin(phi);
2263  double lambda=atan(tanl);
2264  double sinl=sin(lambda);
2265  double cosl=cos(lambda);
2266  // Other parameters
2267  double q_over_pt=S(state_q_over_pt);
2268  double pt=fabs(1./q_over_pt);
2269 
2270  // Turn off dEdx if the pt drops below some minimum
2271  if (pt<PT_MIN) {
2272  dEdx=0.;
2273  }
2274  double kq_over_pt=qBr2p*q_over_pt;
2275 
2276  // Derivative of S with respect to s
2277  double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi;
2278  D1(state_q_over_pt)
2279  =kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2280  double one_over_cosl=1./cosl;
2281  if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS){
2282  double p=pt*one_over_cosl;
2283  double p_sq=p*p;
2284  double E=sqrt(p_sq+mass2);
2285  D1(state_q_over_pt)-=q_over_pt*E*dEdx/p_sq;
2286  }
2287  // D1(state_phi)
2288  // =kq_over_pt*(Bx*cosphi*sinl+By*sinphi*sinl-Bz*cosl);
2289  D1(state_phi)=kq_over_pt*((Bx*cosphi+By*sinphi)*sinl-Bz*cosl);
2290  D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2291  D1(state_z)=sinl;
2292 
2293  // New direction
2294  dpos.Set(cosl*cosphi,cosl*sinphi);
2295 
2296  return NOERROR;
2297 }
2298 
2299 // Calculate the derivative and Jacobian matrices for the alternate set of
2300 // parameters {q/pT, phi, tan(lambda),D,z}
2302  DVector2 &dxy,
2303  const DMatrix5x1 &S,
2304  double dEdx,
2305  DMatrix5x5 &J1,
2306  DMatrix5x1 &D1){
2307  //Direction at current point
2308  double tanl=S(state_tanl);
2309  // Don't let tanl exceed some maximum
2310  if (fabs(tanl)>TAN_MAX) tanl=TAN_MAX*(tanl>0.0?1.:-1.);
2311 
2312  double phi=S(state_phi);
2313  double cosphi=cos(phi);
2314  double sinphi=sin(phi);
2315  double lambda=atan(tanl);
2316  double sinl=sin(lambda);
2317  double cosl=cos(lambda);
2318  double cosl2=cosl*cosl;
2319  double cosl3=cosl*cosl2;
2320  double one_over_cosl=1./cosl;
2321  // Other parameters
2322  double q_over_pt=S(state_q_over_pt);
2323  double pt=fabs(1./q_over_pt);
2324  double q=pt*q_over_pt;
2325 
2326  // Turn off dEdx if pt drops below some minimum
2327  if (pt<PT_MIN) {
2328  dEdx=0.;
2329  }
2330  double kq_over_pt=qBr2p*q_over_pt;
2331 
2332  // B-field and gradient at (x,y,z)
2333  bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz,
2336 
2337  // Derivative of S with respect to s
2338  double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi;
2339  double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi;
2340  D1(state_q_over_pt)=kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2341  D1(state_phi)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl);
2342  D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2343  D1(state_z)=sinl;
2344 
2345  // New direction
2346  dxy.Set(cosl*cosphi,cosl*sinphi);
2347 
2348  // Jacobian matrix elements
2349  J1(state_phi,state_phi)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2351  =qBr2p*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl);
2352  J1(state_phi,state_tanl)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*cosl
2353  +Bz*sinl)*cosl2;
2354  J1(state_phi,state_z)
2355  =kq_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl);
2356 
2357  J1(state_tanl,state_phi)=-kq_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl;
2358  J1(state_tanl,state_q_over_pt)=qBr2p*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2359  J1(state_tanl,state_tanl)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2360  J1(state_tanl,state_z)=kq_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl;
2362  =-kq_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi;
2364  =2.*kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2366  =kq_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi;
2367  if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS){
2368  double p=pt*one_over_cosl;
2369  double p_sq=p*p;
2370  double m2_over_p2=mass2/p_sq;
2371  double E=sqrt(p_sq+mass2);
2372 
2373  D1(state_q_over_pt)-=q_over_pt*E/p_sq*dEdx;
2374  J1(state_q_over_pt,state_q_over_pt)-=dEdx*(2.+3.*m2_over_p2)/E;
2375  J1(state_q_over_pt,state_tanl)+=q*dEdx*sinl*(1.+2.*m2_over_p2)/(p*E);
2376  }
2378  =kq_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi);
2379  J1(state_z,state_tanl)=cosl3;
2380 
2381  return NOERROR;
2382 }
2383 
2384 // Step the state and the covariance matrix through the field
2386  double ds,
2387  double dEdx,
2388  DMatrix5x1 &S,
2389  DMatrix5x5 &J,
2390  DMatrix5x5 &C){
2391  //Initialize the Jacobian matrix
2392  J=I5x5;
2393  if (fabs(ds)<EPS) return NOERROR; // break out if ds is too small
2394 
2395  // B-field and gradient at current (x,y,z)
2396  bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz,
2399  double Bx0=Bx,By0=By,Bz0=Bz;
2400 
2401  // Matrices for intermediate steps
2402  DMatrix5x1 D1,D2,D3,D4;
2403  DMatrix5x1 S1;
2404  DMatrix5x5 J1;
2405  DVector2 dxy1,dxy2,dxy3,dxy4;
2406  double ds_2=0.5*ds;
2407 
2408  // Find the derivative at the first point, propagate the state to the
2409  // first intermediate point and start filling the Jacobian matrix
2410  CalcDerivAndJacobian(xy,dxy1,S,dEdx,J1,D1);
2411  S1=S+ds_2*D1;
2412 
2413  // Calculate the field at the first intermediate point
2414  double dz=S1(state_z)-S(state_z);
2415  double dx=ds_2*dxy1.X();
2416  double dy=ds_2*dxy1.Y();
2417  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2418  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2419  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2420 
2421  // Calculate the derivative and propagate the state to the next point
2422  CalcDeriv(dxy2,S1,dEdx,D2);
2423  S1=S+ds_2*D2;
2424 
2425  // Calculate the field at the second intermediate point
2426  dz=S1(state_z)-S(state_z);
2427  dx=ds_2*dxy2.X();
2428  dy=ds_2*dxy2.Y();
2429  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2430  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2431  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2432 
2433  // Calculate the derivative and propagate the state to the next point
2434  CalcDeriv(dxy3,S1,dEdx,D3);
2435  S1=S+ds*D3;
2436 
2437  // Calculate the field at the final point
2438  dz=S1(state_z)-S(state_z);
2439  dx=ds*dxy3.X();
2440  dy=ds*dxy3.Y();
2441  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2442  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2443  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2444 
2445  // Final derivative
2446  CalcDeriv(dxy4,S1,dEdx,D4);
2447 
2448  // Position vector increment
2449  //DVector3 dpos
2450  // =ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4);
2451  double ds_over_6=ds*ONE_SIXTH;
2452  double ds_over_3=ds*ONE_THIRD;
2453  DVector2 dxy=ds_over_6*dxy1;
2454  dxy+=ds_over_3*dxy2;
2455  dxy+=ds_over_3*dxy3;
2456  dxy+=ds_over_6*dxy4;
2457 
2458  // New Jacobian matrix
2459  J+=ds*J1;
2460 
2461  // Deal with changes in D
2462  double B=sqrt(Bx0*Bx0+By0*By0+Bz0*Bz0);
2463  //double qrc_old=qpt/(qBr2p*Bz_);
2464  double qpt=1./S(state_q_over_pt);
2465  double q=(qpt>0.)?1.:-1.;
2466  double qrc_old=qpt/(qBr2p*B);
2467  double sinphi=sin(S(state_phi));
2468  double cosphi=cos(S(state_phi));
2469  double qrc_plus_D=S(state_D)+qrc_old;
2470  dx=dxy.X();
2471  dy=dxy.Y();
2472  double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi;
2473  double rc=sqrt(dxy.Mod2()
2474  +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi)
2475  +qrc_plus_D*qrc_plus_D);
2476  double q_over_rc=q/rc;
2477 
2478  J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D);
2479  J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.);
2480  J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi);
2481 
2482  // New xy vector
2483  xy+=dxy;
2484 
2485  // New state vector
2486  //S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4);
2487  S+=ds_over_6*D1;
2488  S+=ds_over_3*D2;
2489  S+=ds_over_3*D3;
2490  S+=ds_over_6*D4;
2491 
2492  // Don't let the pt drop below some minimum
2493  //if (fabs(1./S(state_q_over_pt))<PT_MIN) {
2494  // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.);
2495  // }
2496  // Don't let tanl exceed some maximum
2497  if (fabs(S(state_tanl))>TAN_MAX){
2498  S(state_tanl)=TAN_MAX*(S(state_tanl)>0.0?1.:-1.);
2499  }
2500 
2501  // New covariance matrix
2502  // C=J C J^T
2503  //C=C.SandwichMultiply(J);
2504  C=J*C*J.Transpose();
2505 
2506  return NOERROR;
2507 }
2508 
2509 // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z}
2510 // Uses the gradient to compute the field at the intermediate and last
2511 // points.
2513  DMatrix5x1 &S,
2514  double dEdx){
2515  if (fabs(ds)<EPS) return NOERROR; // break out if ds is too small
2516 
2517  // Matrices for intermediate steps
2518  DMatrix5x1 D1,D2,D3,D4;
2519  DMatrix5x1 S1;
2520  DVector2 dxy1,dxy2,dxy3,dxy4;
2521  double ds_2=0.5*ds;
2522  double Bx0=Bx,By0=By,Bz0=Bz;
2523 
2524  // The magnetic field at the beginning of the step is assumed to be
2525  // obtained at the end of the previous step through StepJacobian
2526 
2527  // Calculate the derivative and propagate the state to the next point
2528  CalcDeriv(dxy1,S,dEdx,D1);
2529  S1=S+ds_2*D1;
2530 
2531  // Calculate the field at the first intermediate point
2532  double dz=S1(state_z)-S(state_z);
2533  double dx=ds_2*dxy1.X();
2534  double dy=ds_2*dxy1.Y();
2535  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2536  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2537  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2538 
2539  // Calculate the derivative and propagate the state to the next point
2540  CalcDeriv(dxy2,S1,dEdx,D2);
2541  S1=S+ds_2*D2;
2542 
2543  // Calculate the field at the second intermediate point
2544  dz=S1(state_z)-S(state_z);
2545  dx=ds_2*dxy2.X();
2546  dy=ds_2*dxy2.Y();
2547  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2548  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2549  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2550 
2551  // Calculate the derivative and propagate the state to the next point
2552  CalcDeriv(dxy3,S1,dEdx,D3);
2553  S1=S+ds*D3;
2554 
2555  // Calculate the field at the final point
2556  dz=S1(state_z)-S(state_z);
2557  dx=ds*dxy3.X();
2558  dy=ds*dxy3.Y();
2559  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2560  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2561  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2562 
2563  // Final derivative
2564  CalcDeriv(dxy4,S1,dEdx,D4);
2565 
2566  // New state vector
2567  // S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4);
2568  double ds_over_6=ds*ONE_SIXTH;
2569  double ds_over_3=ds*ONE_THIRD;
2570  S+=ds_over_6*D1;
2571  S+=ds_over_3*D2;
2572  S+=ds_over_3*D3;
2573  S+=ds_over_6*D4;
2574 
2575  // New position
2576  //pos+=ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4);
2577  xy+=ds_over_6*dxy1;
2578  xy+=ds_over_3*dxy2;
2579  xy+=ds_over_3*dxy3;
2580  xy+=ds_over_6*dxy4;
2581 
2582  // Don't let the pt drop below some minimum
2583  //if (fabs(1./S(state_q_over_pt))<PT_MIN) {
2584  // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.);
2585  //}
2586  // Don't let tanl exceed some maximum
2587  if (fabs(S(state_tanl))>TAN_MAX){
2588  S(state_tanl)=TAN_MAX*(S(state_tanl)>0.0?1.:-1.);
2589  }
2590 
2591  return NOERROR;
2592 }
2593 
2594 // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z}
2596  DMatrix5x1 &S,
2597  double dEdx){
2598  if (fabs(ds)<EPS) return NOERROR; // break out if ds is too small
2599 
2600  // B-field and gradient at current (x,y,z)
2601  bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz,
2604  double Bx0=Bx,By0=By,Bz0=Bz;
2605 
2606  // Matrices for intermediate steps
2607  DMatrix5x1 D1,D2,D3,D4;
2608  DMatrix5x1 S1;
2609  DVector2 dxy1,dxy2,dxy3,dxy4;
2610  double ds_2=0.5*ds;
2611 
2612  // Find the derivative at the first point, propagate the state to the
2613  // first intermediate point
2614  CalcDeriv(dxy1,S,dEdx,D1);
2615  S1=S+ds_2*D1;
2616 
2617  // Calculate the field at the first intermediate point
2618  double dz=S1(state_z)-S(state_z);
2619  double dx=ds_2*dxy1.X();
2620  double dy=ds_2*dxy1.Y();
2621  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2622  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2623  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2624 
2625  // Calculate the derivative and propagate the state to the next point
2626  CalcDeriv(dxy2,S1,dEdx,D2);
2627  S1=S+ds_2*D2;
2628 
2629  // Calculate the field at the second intermediate point
2630  dz=S1(state_z)-S(state_z);
2631  dx=ds_2*dxy2.X();
2632  dy=ds_2*dxy2.Y();
2633  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2634  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2635  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2636 
2637  // Calculate the derivative and propagate the state to the next point
2638  CalcDeriv(dxy3,S1,dEdx,D3);
2639  S1=S+ds*D3;
2640 
2641  // Calculate the field at the final point
2642  dz=S1(state_z)-S(state_z);
2643  dx=ds*dxy3.X();
2644  dy=ds*dxy3.Y();
2645  Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz;
2646  By=By0+dBydx*dx+dBydy*dy+dBydz*dz;
2647  Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz;
2648 
2649  // Final derivative
2650  CalcDeriv(dxy4,S1,dEdx,D4);
2651 
2652  // New state vector
2653  // S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4);
2654  double ds_over_6=ds*ONE_SIXTH;
2655  double ds_over_3=ds*ONE_THIRD;
2656  S+=ds_over_6*D1;
2657  S+=ds_over_3*D2;
2658  S+=ds_over_3*D3;
2659  S+=ds_over_6*D4;
2660 
2661  // New position
2662  //pos+=ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4);
2663  xy+=ds_over_6*dxy1;
2664  xy+=ds_over_3*dxy2;
2665  xy+=ds_over_3*dxy3;
2666  xy+=ds_over_6*dxy4;
2667 
2668  // Don't let the pt drop below some minimum
2669  //if (fabs(1./S(state_q_over_pt))<PT_MIN) {
2670  // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.);
2671  //}
2672  // Don't let tanl exceed some maximum
2673  if (fabs(S(state_tanl))>TAN_MAX){
2674  S(state_tanl)=TAN_MAX*(S(state_tanl)>0.0?1.:-1.);
2675  }
2676 
2677  return NOERROR;
2678 }
2679 
2680 // Assuming that the magnetic field is constant over the step, use a helical
2681 // model to step directly to the next point along the trajectory.
2682 void DTrackFitterKalmanSIMD::FastStep(double &z,double ds, double dEdx,
2683  DMatrix5x1 &S){
2684 
2685  // Compute convenience terms involving Bx, By, Bz
2686  double one_over_p=fabs(S(state_q_over_p));
2687  double p=1./one_over_p;
2688  double tx=S(state_tx),ty=S(state_ty);
2689  double denom=sqrt(1.+tx*tx+ty*ty);
2690  double px=p*tx/denom;
2691  double py=p*ty/denom;
2692  double pz=p/denom;
2693  double q=S(state_q_over_p)>0?1.:-1.;
2694  double k_q=qBr2p*q;
2695  double ds_over_p=ds*one_over_p;
2696  double factor=k_q*(0.25*ds_over_p);
2697  double Ax=factor*Bx,Ay=factor*By,Az=factor*Bz;
2698  double Ax2=Ax*Ax,Ay2=Ay*Ay,Az2=Az*Az;
2699  double AxAy=Ax*Ay,AxAz=Ax*Az,AyAz=Ay*Az;
2700  double one_plus_Ax2=1.+Ax2;
2701  double scale=ds_over_p/(one_plus_Ax2+Ay2+Az2);
2702 
2703  // Compute new position
2704  double dx=scale*(px*one_plus_Ax2+py*(AxAy+Az)+pz*(AxAz-Ay));
2705  double dy=scale*(px*(AxAy-Az)+py*(1.+Ay2)+pz*(AyAz+Ax));
2706  double dz=scale*(px*(AxAz+Ay)+py*(AyAz-Ax)+pz*(1.+Az2));
2707  S(state_x)+=dx;
2708  S(state_y)+=dy;
2709  z+=dz;
2710 
2711  // Compute new momentum
2712  px+=k_q*(Bz*dy-By*dz);
2713  py+=k_q*(Bx*dz-Bz*dx);
2714  pz+=k_q*(By*dx-Bx*dy);
2715  S(state_tx)=px/pz;
2716  S(state_ty)=py/pz;
2717  if (fabs(dEdx)>EPS){
2718  double one_over_p_sq=one_over_p*one_over_p;
2719  double E=sqrt(1./one_over_p_sq+mass2);
2720  S(state_q_over_p)-=S(state_q_over_p)*one_over_p_sq*E*dEdx*ds;
2721  }
2722 }
2723 // Assuming that the magnetic field is constant over the step, use a helical
2724 // model to step directly to the next point along the trajectory.
2725 void DTrackFitterKalmanSIMD::FastStep(DVector2 &xy,double ds, double dEdx,
2726  DMatrix5x1 &S){
2727 
2728  // Compute convenience terms involving Bx, By, Bz
2729  double pt=fabs(1./S(state_q_over_pt));
2730  double one_over_p=cos(atan(S(state_tanl)))/pt;
2731  double px=pt*cos(S(state_phi));
2732  double py=pt*sin(S(state_phi));
2733  double pz=pt*S(state_tanl);
2734  double q=S(state_q_over_pt)>0?1.:-1.;
2735  double k_q=qBr2p*q;
2736  double ds_over_p=ds*one_over_p;
2737  double factor=k_q*(0.25*ds_over_p);
2738  double Ax=factor*Bx,Ay=factor*By,Az=factor*Bz;
2739  double Ax2=Ax*Ax,Ay2=Ay*Ay,Az2=Az*Az;
2740  double AxAy=Ax*Ay,AxAz=Ax*Az,AyAz=Ay*Az;
2741  double one_plus_Ax2=1.+Ax2;
2742  double scale=ds_over_p/(one_plus_Ax2+Ay2+Az2);
2743 
2744  // Compute new position
2745  double dx=scale*(px*one_plus_Ax2+py*(AxAy+Az)+pz*(AxAz-Ay));
2746  double dy=scale*(px*(AxAy-Az)+py*(1.+Ay2)+pz*(AyAz+Ax));
2747  double dz=scale*(px*(AxAz+Ay)+py*(AyAz-Ax)+pz*(1.+Az2));
2748  xy.Set(xy.X()+dx,xy.Y()+dy);
2749  S(state_z)+=dz;
2750 
2751  // Compute new momentum
2752  px+=k_q*(Bz*dy-By*dz);
2753  py+=k_q*(Bx*dz-Bz*dx);
2754  pz+=k_q*(By*dx-Bx*dy);
2755  pt=sqrt(px*px+py*py);
2756  S(state_q_over_pt)=q/pt;
2757  S(state_phi)=atan2(py,px);
2758  S(state_tanl)=pz/pt;
2759  if (fabs(dEdx)>EPS){
2760  double one_over_p_sq=one_over_p*one_over_p;
2761  double E=sqrt(1./one_over_p_sq+mass2);
2762  S(state_q_over_p)-=S(state_q_over_pt)*one_over_p_sq*E*dEdx*ds;
2763  }
2764 }
2765 
2766 // Calculate the jacobian matrix for the alternate parameter set
2767 // {q/pT,phi,tanl(lambda),D,z}
2769  const DVector2 &dxy,
2770  double ds,const DMatrix5x1 &S,
2771  double dEdx,DMatrix5x5 &J){
2772  // Initialize the Jacobian matrix
2773  //J.Zero();
2774  //for (int i=0;i<5;i++) J(i,i)=1.;
2775  J=I5x5;
2776 
2777  if (fabs(ds)<EPS) return NOERROR; // break out if ds is too small
2778  // B-field and gradient at current (x,y,z)
2779  //bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz,
2780  //dBxdx,dBxdy,dBxdz,dBydx,
2781  //dBydy,dBydz,dBzdx,dBzdy,dBzdz);
2782 
2783  // Charge
2784  double q=(S(state_q_over_pt)>0.0)?1.:-1.;
2785 
2786  //kinematic quantities
2787  double q_over_pt=S(state_q_over_pt);
2788  double sinphi=sin(S(state_phi));
2789  double cosphi=cos(S(state_phi));
2790  double D=S(state_D);
2791  double lambda=atan(S(state_tanl));
2792  double sinl=sin(lambda);
2793  double cosl=cos(lambda);
2794  double cosl2=cosl*cosl;
2795  double cosl3=cosl*cosl2;
2796  double one_over_cosl=1./cosl;
2797  double pt=fabs(1./q_over_pt);
2798 
2799  // Turn off dEdx if pt drops below some minimum
2800  if (pt<PT_MIN) {
2801  dEdx=0.;
2802  }
2803  double kds=qBr2p*ds;
2804  double kq_ds_over_pt=kds*q_over_pt;
2805  double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi;
2806  double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi;
2807 
2808  // Jacobian matrix elements
2809  J(state_phi,state_phi)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2810  J(state_phi,state_q_over_pt)=kds*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl);
2811  J(state_phi,state_tanl)=kq_ds_over_pt*(By_sinphi_plus_Bx_cosphi*cosl
2812  +Bz*sinl)*cosl2;
2813  J(state_phi,state_z)
2814  =kq_ds_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl);
2815 
2816  J(state_tanl,state_phi)=-kq_ds_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl;
2817  J(state_tanl,state_q_over_pt)=kds*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2818  J(state_tanl,state_tanl)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2819  J(state_tanl,state_z)=kq_ds_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl;
2821  =-kq_ds_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi;
2823  +=2.*kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2825  =kq_ds_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi;
2826  if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS){
2827  double p=pt*one_over_cosl;
2828  double p_sq=p*p;
2829  double m2_over_p2=mass2/p_sq;
2830  double E=sqrt(p_sq+mass2);
2831  double dE_over_E=dEdx*ds/E;
2832 
2833  J(state_q_over_pt,state_q_over_pt)-=dE_over_E*(2.+3.*m2_over_p2);
2834  J(state_q_over_pt,state_tanl)+=q*dE_over_E*sinl*(1.+2.*m2_over_p2)/p;
2835  }
2837  =kq_ds_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi);
2838  J(state_z,state_tanl)=cosl3*ds;
2839 
2840  // Deal with changes in D
2841  double B=sqrt(Bx*Bx+By*By+Bz*Bz);
2842  //double qrc_old=qpt/(qBr2p*fabs(Bz));
2843  double qpt=FactorForSenseOfRotation/q_over_pt;
2844  double qrc_old=qpt/(qBr2p*B);
2845  double qrc_plus_D=D+qrc_old;
2846  double dx=dxy.X();
2847  double dy=dxy.Y();
2848  double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi;
2849  double rc=sqrt(dxy.Mod2()
2850  +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi)
2851  +qrc_plus_D*qrc_plus_D);
2852  double q_over_rc=FactorForSenseOfRotation*q/rc;
2853 
2854  J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D);
2855  J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.);
2856  J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi);
2857 
2858  return NOERROR;
2859 }
2860 
2861 
2862 
2863 
2864 // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z}
2866  double ds,const DMatrix5x1 &S,
2867  double dEdx,DMatrix5x5 &J){
2868  // Initialize the Jacobian matrix
2869  //J.Zero();
2870  //for (int i=0;i<5;i++) J(i,i)=1.;
2871  J=I5x5;
2872 
2873  if (fabs(ds)<EPS) return NOERROR; // break out if ds is too small
2874 
2875  // Matrices for intermediate steps
2876  DMatrix5x5 J1;
2877  DMatrix5x1 D1;
2878  DVector2 dxy1;
2879 
2880  // charge
2881  double q=(S(state_q_over_pt)>0.0)?1.:-1.;
2883 
2884  //kinematic quantities
2885  double qpt=1./S(state_q_over_pt) * FactorForSenseOfRotation;
2886  double sinphi=sin(S(state_phi));
2887  double cosphi=cos(S(state_phi));
2888  double D=S(state_D);
2889 
2890  CalcDerivAndJacobian(xy,dxy1,S,dEdx,J1,D1);
2891  // double Bz_=fabs(Bz); // needed for computing D
2892 
2893  // New Jacobian matrix
2894  J+=ds*J1;
2895 
2896  // change in position
2897  DVector2 dxy =ds*dxy1;
2898 
2899  // Deal with changes in D
2900  double B=sqrt(Bx*Bx+By*By+Bz*Bz);
2901  //double qrc_old=qpt/(qBr2p*Bz_);
2902  double qrc_old=qpt/(qBr2p*B);
2903  double qrc_plus_D=D+qrc_old;
2904  double dx=dxy.X();
2905  double dy=dxy.Y();
2906  double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi;
2907  double rc=sqrt(dxy.Mod2()
2908  +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi)
2909  +qrc_plus_D*qrc_plus_D);
2910  double q_over_rc=q/rc;
2911 
2912  J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D);
2913  J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.);
2914  J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi);
2915 
2916  return NOERROR;
2917 }
2918 
2919 // Compute contributions to the covariance matrix due to multiple scattering
2920 // using the Lynch/Dahl empirical formulas
2922  double chi2c_factor,
2923  double chi2a_factor,
2924  double chi2a_corr,
2925  const DMatrix5x1 &Sc,
2926  DMatrix5x5 &Q){
2927  Q.Zero();
2928  //return NOERROR;
2929  if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS){
2930  double tanl=Sc(state_tanl);
2931  double tanl2=tanl*tanl;
2932  double one_plus_tanl2=1.+tanl2;
2933  double one_over_pt=fabs(Sc(state_q_over_pt));
2934  double my_ds=fabs(ds);
2935  double my_ds_2=0.5*my_ds;
2936 
2937  Q(state_phi,state_phi)=one_plus_tanl2;
2938  Q(state_tanl,state_tanl)=one_plus_tanl2*one_plus_tanl2;
2939  Q(state_q_over_pt,state_q_over_pt)=one_over_pt*one_over_pt*tanl2;
2941  =Sc(state_q_over_pt)*tanl*one_plus_tanl2;
2942  Q(state_D,state_D)=ONE_THIRD*ds*ds;
2943 
2944  // I am not sure the following is correct...
2945  double sign_D=(Sc(state_D)>0.0)?1.:-1.;
2946  Q(state_D,state_phi)=Q(state_phi,state_D)=sign_D*my_ds_2*sqrt(one_plus_tanl2);
2947  Q(state_D,state_tanl)=Q(state_tanl,state_D)=sign_D*my_ds_2*one_plus_tanl2;
2948  Q(state_D,state_q_over_pt)=Q(state_q_over_pt,state_D)=sign_D*my_ds_2*tanl*Sc(state_q_over_pt);
2949 
2950  double one_over_p_sq=one_over_pt*one_over_pt/one_plus_tanl2;
2951  double one_over_beta2=1.+mass2*one_over_p_sq;
2952  double chi2c_p_sq=chi2c_factor*my_ds*one_over_beta2;
2953  double chi2a_p_sq=chi2a_factor*(1.+chi2a_corr*one_over_beta2);
2954  // F=Fraction of Moliere distribution to be taken into account
2955  // nu=0.5*chi2c/(chi2a*(1.-F));
2956  double nu=MOLIERE_RATIO1*chi2c_p_sq/chi2a_p_sq;
2957  double one_plus_nu=1.+nu;
2958  // sig2_ms=chi2c*1e-6/(1.+F*F)*((one_plus_nu)/nu*log(one_plus_nu)-1.);
2959  double sig2_ms=chi2c_p_sq*one_over_p_sq*MOLIERE_RATIO2
2960  *(one_plus_nu/nu*log(one_plus_nu)-1.);
2961 
2962  Q*=sig2_ms;
2963  }
2964 
2965  return NOERROR;
2966 }
2967 
2968 // Compute contributions to the covariance matrix due to multiple scattering
2969 // using the Lynch/Dahl empirical formulas
2970 jerror_t DTrackFitterKalmanSIMD::GetProcessNoise(double z, double ds,
2971  double chi2c_factor,
2972  double chi2a_factor,
2973  double chi2a_corr,
2974  const DMatrix5x1 &S,
2975  DMatrix5x5 &Q){
2976 
2977  Q.Zero();
2978  //return NOERROR;
2979  if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS){
2980  double tx=S(state_tx),ty=S(state_ty);
2981  double one_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
2982  double my_ds=fabs(ds);
2983  double my_ds_2=0.5*my_ds;
2984  double tx2=tx*tx;
2985  double ty2=ty*ty;
2986  double one_plus_tx2=1.+tx2;
2987  double one_plus_ty2=1.+ty2;
2988  double tsquare=tx2+ty2;
2989  double one_plus_tsquare=1.+tsquare;
2990 
2991  Q(state_tx,state_tx)=one_plus_tx2*one_plus_tsquare;
2992  Q(state_ty,state_ty)=one_plus_ty2*one_plus_tsquare;
2993  Q(state_tx,state_ty)=Q(state_ty,state_tx)=tx*ty*one_plus_tsquare;
2994 
2995  Q(state_x,state_x)=ONE_THIRD*ds*ds;
2998  = my_ds_2*sqrt(one_plus_tsquare*one_plus_ty2);
3000  = my_ds_2*sqrt(one_plus_tsquare*one_plus_tx2);
3001 
3002  double one_over_beta2=1.+one_over_p_sq*mass2;
3003  double chi2c_p_sq=chi2c_factor*my_ds*one_over_beta2;
3004  double chi2a_p_sq=chi2a_factor*(1.+chi2a_corr*one_over_beta2);
3005  // F=MOLIERE_FRACTION =Fraction of Moliere distribution to be taken into account
3006  // nu=0.5*chi2c/(chi2a*(1.-F));
3007  double nu=MOLIERE_RATIO1*chi2c_p_sq/chi2a_p_sq;
3008  double one_plus_nu=1.+nu;
3009  double sig2_ms=chi2c_p_sq*one_over_p_sq*MOLIERE_RATIO2
3010  *(one_plus_nu/nu*log(one_plus_nu)-1.);
3011 
3012  // printf("fac %f %f %f\n",chi2c_factor,chi2a_factor,chi2a_corr);
3013  //printf("omega %f\n",chi2c/chi2a);
3014 
3015 
3016  Q*=sig2_ms;
3017  }
3018 
3019  return NOERROR;
3020 }
3021 
3022 // Calculate the energy loss per unit length given properties of the material
3023 // through which a particle of momentum p is passing
3024 double DTrackFitterKalmanSIMD::GetdEdx(double q_over_p,double K_rho_Z_over_A,
3025  double rho_Z_over_A,double LnI,double Z){
3026  if (rho_Z_over_A<=0.) return 0.;
3027  //return 0.;
3028 
3029  double p=fabs(1./q_over_p);
3030  double betagamma=p/MASS;
3031  double betagamma2=betagamma*betagamma;
3032  double gamma2=1.+betagamma2;
3033  double beta2=betagamma2/gamma2;
3034  if (beta2<EPS) beta2=EPS;
3035 
3036  // density effect
3037  double delta=CalcDensityEffect(betagamma,rho_Z_over_A,LnI);
3038 
3039  double dEdx=0.;
3040  // For particles heavier than electrons:
3041  if (IsHadron){
3042  double two_Me_betagamma_sq=two_m_e*betagamma2;
3043  double Tmax
3044  =two_Me_betagamma_sq/(1.+2.*sqrt(gamma2)*m_ratio+m_ratio_sq);
3045 
3046  dEdx=K_rho_Z_over_A/beta2*(-log(two_Me_betagamma_sq*Tmax)
3047  +2.*(LnI + beta2)+delta);
3048  }
3049  else{
3050  // relativistic kinetic energy in units of M_e c^2
3051  double tau=sqrt(gamma2)-1.;
3052  double tau_sq=tau*tau;
3053  double tau_plus_1=tau+1.;
3054  double tau_plus_2=tau+2.;
3055  double tau_plus_2_sq=tau_plus_2*tau_plus_2;
3056  double f=0.; // function that depends on tau; see Leo (2nd ed.), p. 38.
3057  if (IsElectron){
3058  f=1.-beta2+(0.125*tau_sq-(2.*tau+1.)*log(2.))/(tau_plus_1*tau_plus_1);
3059  }
3060  else{
3061  f=2.*log(2.)-(beta2/12.)*(23.+14./tau_plus_2+10./tau_plus_2_sq
3062  +4./(tau_plus_2*tau_plus_2_sq));
3063  }
3064 
3065  // collision loss (Leo eq. 2.66)
3066  double dEdx_coll
3067  =-K_rho_Z_over_A/beta2*(log(0.5*tau_sq*tau_plus_2*m_e_sq)-LnI+f-delta);
3068 
3069  // radiation loss (Leo eqs. 2.74, 2.76 with Z^2 -> Z(Z+1)
3070  double a=Z*ALPHA;
3071  double a2=a*a;
3072  double a4=a2*a2;
3073  double epsilon=1.-PHOTON_ENERGY_CUTOFF;
3074  double epsilon2=epsilon*epsilon;
3075  double f_Z=a2*(1./(1.+a2)+0.20206-0.0369*a2+0.0083*a4-0.002*a2*a4);
3076  // The expression below is the integral of the photon energy weighted
3077  // by the bremsstrahlung cross section up to a maximum photon energy
3078  // expressed as a fraction of the incident electron energy.
3079  double dEdx_rad=-K_rho_Z_over_A*tau_plus_1*(2.*ALPHA/M_PI)*(Z+1.)
3080  *((log(183.*pow(Z,-1./3.))-f_Z)
3081  *(1.-epsilon-(1./3.)*(epsilon2-epsilon*epsilon2))
3082  +1./18.*(1.-epsilon2));
3083 
3084 
3085  // dEdx_rad=0.;
3086 
3087  dEdx=dEdx_coll+dEdx_rad;
3088  }
3089 
3090  return dEdx;
3091 }
3092 
3093 // Calculate the variance in the energy loss in a Gaussian approximation.
3094 // The standard deviation of the energy loss distribution is
3095 // var=0.1535*density*(Z/A)*x*(1-0.5*beta^2)*Tmax [MeV]
3096 // where Tmax is the maximum energy transfer.
3097 // (derived from Leo (2nd ed.), eq. 2.100. Note that I think there is a typo
3098 // in this formula in the text...)
3100  double one_over_beta2,
3101  double K_rho_Z_over_A){
3102  if (K_rho_Z_over_A<=0.) return 0.;
3103 
3104  double betagamma2=1./(one_over_beta2-1.);
3105  double gamma2=betagamma2*one_over_beta2;
3106  double two_Me_betagamma_sq=two_m_e*betagamma2;
3107  double Tmax=two_Me_betagamma_sq/(1.+2.*sqrt(gamma2)*m_ratio+m_ratio_sq);
3108  double var=K_rho_Z_over_A*one_over_beta2*fabs(ds)*Tmax*(1.-0.5/one_over_beta2);
3109  return var;
3110 }
3111 
3112 // Interface routine for Kalman filter
3114  if (z_<Z_MIN) return VALUE_OUT_OF_RANGE;
3115 
3116  // Vector to store the list of hits used in the fit for the forward parametrization
3117  vector<const DCDCTrackHit*>forward_cdc_used_in_fit;
3118 
3119  // State vector and initial guess for covariance matrix
3120  DMatrix5x1 S0;
3121  DMatrix5x5 C0;
3122 
3123  chisq_=-1.;
3124 
3125  // Angle with respect to beam line
3126  double theta_deg=(180/M_PI)*input_params.momentum().Theta();
3127  //double theta_deg_sq=theta_deg*theta_deg;
3128  double tanl0=tanl_=tan(M_PI_2-input_params.momentum().Theta());
3129 
3130  // Azimuthal angle
3131  double phi0=phi_=input_params.momentum().Phi();
3132 
3133  // Guess for momentum error
3134  double dpt_over_pt=0.1;
3135  /*
3136  if (theta_deg<15){
3137  dpt_over_pt=0.107-0.0178*theta_deg+0.000966*theta_deg_sq;
3138  }
3139  else {
3140  dpt_over_pt=0.0288+0.00579*theta_deg-2.77e-5*theta_deg_sq;
3141  }
3142  */
3143  /*
3144  if (theta_deg<28.){
3145  theta_deg=28.;
3146  theta_deg_sq=theta_deg*theta_deg;
3147  }
3148  else if (theta_deg>125.){
3149  theta_deg=125.;
3150  theta_deg_sq=theta_deg*theta_deg;
3151  }
3152  */
3153  double sig_lambda=0.02;
3154  double dp_over_p_sq
3155  =dpt_over_pt*dpt_over_pt+tanl_*tanl_*sig_lambda*sig_lambda;
3156 
3157  // Input charge
3158  double q=input_params.charge();
3159 
3160  // Input momentum
3162  double p_mag=pvec.Mag();
3163  double px=pvec.x();
3164  double py=pvec.y();
3165  double pz=pvec.z();
3166  double q_over_p0=q_over_p_=q/p_mag;
3167  double q_over_pt0=q_over_pt_=q/pvec.Perp();
3168 
3169  // Initial position
3170  double x0=x_=input_params.position().x();
3171  double y0=y_=input_params.position().y();
3172  double z0=z_=input_params.position().z();
3173 
3174  if (fit_type==kWireBased && theta_deg>10.){
3175  double Bz=fabs(bfield->GetBz(x0,y0,z0));
3176  double sperp=25.; // empirical guess
3177  if (my_fdchits.size()>0){
3178  double my_z=my_fdchits[0]->z;
3179  double my_x=my_fdchits[0]->hit->xy.X();
3180  double my_y=my_fdchits[0]->hit->xy.Y();
3181  Bz+=fabs(bfield->GetBz(my_x,my_y,my_z));
3182  Bz*=0.5; // crude average
3183  sperp=(my_z-z0)/tanl_;
3184  }
3185  double twokappa=qBr2p*Bz*q_over_pt0*FactorForSenseOfRotation;
3186  double one_over_2k=1./twokappa;
3187  if (my_fdchits.size()==0){
3188  for (unsigned int i=my_cdchits.size()-1;i>1;i--){
3189  // Use outermost axial straw to estimate a resonable arc length
3190  if (my_cdchits[i]->hit->is_stereo==false){
3191  double tworc=2.*fabs(one_over_2k);
3192  double ratio=(my_cdchits[i]->hit->wire->origin
3193  -input_params.position()).Perp()/tworc;
3194  sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_2;
3195  if (sperp<25.) sperp=25.;
3196  break;
3197  }
3198  }
3199  }
3200  double twoks=twokappa*sperp;
3201  double cosphi=cos(phi0);
3202  double sinphi=sin(phi0);
3203  double sin2ks=sin(twoks);
3204  double cos2ks=cos(twoks);
3205  double one_minus_cos2ks=1.-cos2ks;
3206  double myx=x0+one_over_2k*(cosphi*sin2ks-sinphi*one_minus_cos2ks);
3207  double myy=y0+one_over_2k*(sinphi*sin2ks+cosphi*one_minus_cos2ks);
3208  double mypx=px*cos2ks-py*sin2ks;
3209  double mypy=py*cos2ks+px*sin2ks;
3210  double myphi=atan2(mypy,mypx);
3211  phi0=phi_=myphi;
3212  px=mypx;
3213  py=mypy;
3214  x0=x_=myx;
3215  y0=y_=myy;
3216  z0+=tanl_*sperp;
3217  z_=z0;
3218  }
3219 
3220  // Check integrity of input parameters
3221  if (!isfinite(x0) || !isfinite(y0) || !isfinite(q_over_p0)){
3222  if (DEBUG_LEVEL>0) _DBG_ << "Invalid input parameters!" <<endl;
3223  return UNRECOVERABLE_ERROR;
3224  }
3225 
3226  // Initial direction tangents
3227  double tx0=tx_=px/pz;
3228  double ty0=ty_=py/pz;
3229  double one_plus_tsquare=1.+tx_*tx_+ty_*ty_;
3230 
3231  // deal with hits in FDC
3232  double fdc_prob=0.,fdc_chisq=-1.;
3233  unsigned int fdc_ndf=0;
3234  if (my_fdchits.size()>0
3235  && // Make sure that these parameters are valid for forward-going tracks
3236  (isfinite(tx0) && isfinite(ty0))
3237  ){
3238  if (DEBUG_LEVEL>0){
3239  _DBG_ << "Using forward parameterization." <<endl;
3240  }
3241 
3242  // Initial guess for the state vector
3243  S0(state_x)=x_;
3244  S0(state_y)=y_;
3245  S0(state_tx)=tx_;
3246  S0(state_ty)=ty_;
3248 
3249  // Initial guess for forward representation covariance matrix
3250  C0(state_x,state_x)=2.0;
3251  C0(state_y,state_y)=2.0;
3252  double temp=sig_lambda*one_plus_tsquare;
3256 
3257  if (my_cdchits.size()>0){
3258  mCDCInternalStepSize=0.25;
3259  }
3260 
3261  // The position from the track candidate is reported just outside the
3262  // start counter for tracks containing cdc hits. Propagate to the distance
3263  // of closest approach to the beam line
3265 
3266  kalman_error_t error=ForwardFit(S0,C0);
3267  if (error==FIT_SUCCEEDED) return NOERROR;
3268  if ((error==FIT_FAILED || ndf_==0) && my_cdchits.size()<6){
3269  return UNRECOVERABLE_ERROR;
3270  }
3271 
3272  fdc_prob=TMath::Prob(chisq_,ndf_);
3273  fdc_ndf=ndf_;
3274  fdc_chisq=chisq_;
3275  }
3276 
3277  // Deal with hits in the CDC
3278  if (my_cdchits.size()>5){
3280  kalman_error_t cdc_error=FIT_NOT_DONE;
3281 
3282  // Save the current state of the extrapolation vector if it exists
3283  map<DetectorSystem_t,vector<Extrapolation_t> >saved_extrapolations;
3284  if (!extrapolations.empty()){
3285  saved_extrapolations=extrapolations;
3287  }
3288  bool save_IsSmoothed=IsSmoothed;
3289 
3290  // Chi-squared, degrees of freedom, and probability
3291  double forward_prob=0.;
3292  double chisq_forward=-1.;
3293  unsigned int ndof_forward=0;
3294 
3295  // Parameters at "vertex"
3296  double phi=phi_,q_over_pt=q_over_pt_,tanl=tanl_,x=x_,y=y_,z=z_;
3297  vector< vector <double> > fcov_save;
3298  vector<pull_t>pulls_save;
3299  pulls_save.assign(pulls.begin(),pulls.end());
3300  if (!fcov.empty()){
3301  fcov_save.assign(fcov.begin(),fcov.end());
3302  }
3303  if (my_fdchits.size()>0){
3304  if (error==INVALID_FIT) _DBG_<< "Invalid fit " << fcov.size() << " " << fdc_ndf <<endl;
3305  }
3306 
3307  // Use forward parameters for CDC-only tracks with theta<THETA_CUT degrees
3308  if (theta_deg<THETA_CUT){
3309  if (DEBUG_LEVEL>0){
3310  _DBG_ << "Using forward parameterization." <<endl;
3311  }
3312 
3313  // Step size
3315 
3316  // Initialize the state vector
3317  S0(state_x)=x_=x0;
3318  S0(state_y)=y_=y0;
3319  S0(state_tx)=tx_=tx0;
3320  S0(state_ty)=ty_=ty0;
3321  S0(state_q_over_p)=q_over_p_=q_over_p0;
3322  z_=z0;
3323 
3324  // Initial guess for forward representation covariance matrix
3325  double temp=sig_lambda*one_plus_tsquare;
3326  C0(state_x,state_x)=2.0;
3327  C0(state_y,state_y)=2.0;
3331 
3332  //C0*=1.+1./tsquare;
3333 
3334  // The position from the track candidate is reported just outside the
3335  // start counter for tracks containing cdc hits. Propagate to the
3336  // distance of closest approach to the beam line
3338 
3339  error=ForwardCDCFit(S0,C0);
3340  if (error==FIT_SUCCEEDED) return NOERROR;
3341 
3342  // Find the CL of the fit
3343  forward_prob=TMath::Prob(chisq_,ndf_);
3344  if (my_fdchits.size()>0){
3345  if (fdc_ndf==0 || forward_prob>fdc_prob){
3346  // We did not end up using the fdc hits after all...
3347  fdchits_used_in_fit.clear();
3348  }
3349  else{
3350  chisq_=fdc_chisq;
3351  ndf_=fdc_ndf;
3352  x_=x;
3353  y_=y;
3354  z_=z;
3355  phi_=phi;
3356  tanl_=tanl;
3357  q_over_pt_=q_over_pt;
3358  if (!fcov_save.empty()){
3359  fcov.assign(fcov_save.begin(),fcov_save.end());
3360  }
3361  if (!saved_extrapolations.empty()){
3362  extrapolations=saved_extrapolations;
3363  }
3364  IsSmoothed=save_IsSmoothed;
3365  pulls.assign(pulls_save.begin(),pulls_save.end());
3366 
3367  // _DBG_ << endl;
3368  return NOERROR;
3369  }
3370 
3371  // Save the best values for the parameters and chi2 for now
3372  chisq_forward=chisq_;
3373  ndof_forward=ndf_;
3374  x=x_;
3375  y=y_;
3376  z=z_;
3377  phi=phi_;
3378  tanl=tanl_;
3379  q_over_pt=q_over_pt_;
3380  fcov_save.assign(fcov.begin(),fcov.end());
3381  pulls_save.assign(pulls.begin(),pulls.end());
3382  save_IsSmoothed=IsSmoothed;
3383  if (!extrapolations.empty()){
3384  saved_extrapolations=extrapolations;
3386  }
3387 
3388  // Save the list of hits used in the fit
3389  forward_cdc_used_in_fit.assign(cdchits_used_in_fit.begin(),cdchits_used_in_fit.end());
3390 
3391  }
3392  }
3393 
3394  // Attempt to fit the track using the central parametrization.
3395  if (DEBUG_LEVEL>0){
3396  _DBG_ << "Using central parameterization." <<endl;
3397  }
3398 
3399  // Step size
3401 
3402  // Initialize the state vector
3403  S0(state_q_over_pt)=q_over_pt_=q_over_pt0;
3404  S0(state_phi)=phi_=phi0;
3405  S0(state_tanl)=tanl_=tanl0;
3406  S0(state_z)=z_=z0;
3407  S0(state_D)=0.;
3408 
3409  // Initialize the covariance matrix
3410  double dz=1.0;
3411  C0(state_z,state_z)=dz*dz;
3413  =q_over_pt_*q_over_pt_*dpt_over_pt*dpt_over_pt;
3414  double dphi=0.02;
3415  C0(state_phi,state_phi)=dphi*dphi;
3416  C0(state_D,state_D)=1.0;
3417  double tanl2=tanl_*tanl_;
3418  double one_plus_tanl2=1.+tanl2;
3419  C0(state_tanl,state_tanl)=(one_plus_tanl2)*(one_plus_tanl2)
3420  *sig_lambda*sig_lambda;
3422 
3423  //if (theta_deg>90.) C0*=1.+5.*tanl2;
3424  //else C0*=1.+5.*tanl2*tanl2;
3425 
3426  mCentralStepSize=0.4;
3428 
3429  // The position from the track candidate is reported just outside the
3430  // start counter for tracks containing cdc hits. Propagate to the
3431  // distance of closest approach to the beam line
3432  DVector2 xy(x0,y0);
3433  if (fit_type==kWireBased){
3434  ExtrapolateToVertex(xy,S0);
3435  }
3436 
3437  cdc_error=CentralFit(xy,S0,C0);
3438  if (cdc_error==FIT_SUCCEEDED){
3439  // if the result of the fit using the forward parameterization succeeded
3440  // but the chi2 was too high, it still may provide the best estimate
3441  // for the track parameters...
3442  double central_prob=TMath::Prob(chisq_,ndf_);
3443 
3444  if (central_prob<forward_prob){
3445  phi_=phi;
3446  q_over_pt_=q_over_pt;
3447  tanl_=tanl;
3448  x_=x;
3449  y_=y;
3450  z_=z;
3451  chisq_=chisq_forward;
3452  ndf_= ndof_forward;
3453  fcov.assign(fcov_save.begin(),fcov_save.end());
3454  pulls.assign(pulls_save.begin(),pulls_save.end());
3455  IsSmoothed=save_IsSmoothed;
3456  if (!saved_extrapolations.empty()){
3457  extrapolations=saved_extrapolations;
3458  }
3459 
3460  cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end());
3461 
3462  // We did not end up using any fdc hits...
3463  fdchits_used_in_fit.clear();
3464 
3465  }
3466  return NOERROR;
3467 
3468  }
3469  // otherwise if the fit using the forward parametrization worked, return that
3470  else if (error!=FIT_FAILED){
3471  phi_=phi;
3472  q_over_pt_=q_over_pt;
3473  tanl_=tanl;
3474  x_=x;
3475  y_=y;
3476  z_=z;
3477  chisq_=chisq_forward;
3478  ndf_= ndof_forward;
3479 
3480  if (!saved_extrapolations.empty()){
3481  extrapolations=saved_extrapolations;
3482  }
3483  IsSmoothed=save_IsSmoothed;
3484  fcov.assign(fcov_save.begin(),fcov_save.end());
3485  pulls.assign(pulls_save.begin(),pulls_save.end());
3486  cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end());
3487 
3488  // We did not end up using any fdc hits...
3489  fdchits_used_in_fit.clear();
3490 
3491  return NOERROR;
3492  }
3493  else return UNRECOVERABLE_ERROR;
3494  }
3495 
3496  if (ndf_==0) return UNRECOVERABLE_ERROR;
3497 
3498  return NOERROR;
3499 }
3500 
3501 #define ITMAX 20
3502 #define CGOLD 0.3819660
3503 #define ZEPS 1.0e-10
3504 #define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
3505 #define SIGN(a,b) ((b)>=0.0?fabs(a):-fabs(a))
3506 // Routine for finding the minimum of a function bracketed between two values
3507 // (see Numerical Recipes in C, pp. 404-405).
3508 jerror_t DTrackFitterKalmanSIMD::BrentsAlgorithm(double ds1,double ds2,
3509  double dedx,DVector2 &pos,
3510  const double z0wire,
3511  const DVector2 &origin,
3512  const DVector2 &dir,
3513  DMatrix5x1 &Sc,
3514  double &ds_out){
3515  double d=0.;
3516  double e=0.0; // will be distance moved on step before last
3517  double ax=0.;
3518  double bx=-ds1;
3519  double cx=-ds1-ds2;
3520 
3521  double a=(ax<cx?ax:cx);
3522  double b=(ax>cx?ax:cx);
3523  double x=bx,w=bx,v=bx;
3524 
3525  // printf("ds1 %f ds2 %f\n",ds1,ds2);
3526  // Initialize return step size
3527  ds_out=0.;
3528 
3529  // Save the starting position
3530  // DVector3 pos0=pos;
3531  // DMatrix S0(Sc);
3532 
3533  // Step to intermediate point
3534  Step(pos,x,Sc,dedx);
3535  // Bail if the transverse momentum has dropped below some minimum
3536  if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX){
3537  if (DEBUG_LEVEL>2)
3538  {
3539  _DBG_ << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt))
3540  << endl;
3541  }
3542  return VALUE_OUT_OF_RANGE;
3543  }
3544 
3545  DVector2 wirepos=origin+(Sc(state_z)-z0wire)*dir;
3546  double u_old=x;
3547  double u=0.;
3548 
3549  // initialization
3550  double fw=(pos-wirepos).Mod2();
3551  double fv=fw,fx=fw;
3552 
3553  // main loop
3554  for (unsigned int iter=1;iter<=ITMAX;iter++){
3555  double xm=0.5*(a+b);
3556  double tol1=EPS2*fabs(x)+ZEPS;
3557  double tol2=2.0*tol1;
3558 
3559  if (fabs(x-xm)<=(tol2-0.5*(b-a))){
3560  if (Sc(state_z)<=cdc_origin[2]){
3561  unsigned int iter2=0;
3562  double ds_temp=0.;
3563  while (fabs(Sc(state_z)-cdc_origin[2])>EPS2 && iter2<20){
3564  u=x-(cdc_origin[2]-Sc(state_z))*sin(atan(Sc(state_tanl)));
3565  x=u;
3566  ds_temp+=u_old-u;
3567  // Bail if the transverse momentum has dropped below some minimum
3568  if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX){
3569  if (DEBUG_LEVEL>2)
3570  {
3571  _DBG_ << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt))
3572  << endl;
3573  }
3574  return VALUE_OUT_OF_RANGE;
3575  }
3576 
3577  // Function evaluation
3578  Step(pos,u_old-u,Sc,dedx);
3579  u_old=u;
3580  iter2++;
3581  }
3582  //printf("new z %f ds %f \n",pos.z(),x);
3583  ds_out=ds_temp;
3584  return NOERROR;
3585  }
3586  else if (Sc(state_z)>=endplate_z){
3587  unsigned int iter2=0;
3588  double ds_temp=0.;
3589  while (fabs(Sc(state_z)-endplate_z)>EPS2 && iter2<20){
3590  u=x-(endplate_z-Sc(state_z))*sin(atan(Sc(state_tanl)));
3591  x=u;
3592  ds_temp+=u_old-u;
3593 
3594  // Bail if the transverse momentum has dropped below some minimum
3595  if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX){
3596  if (DEBUG_LEVEL>2)
3597  {
3598  _DBG_ << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt))
3599  << endl;
3600  }
3601  return VALUE_OUT_OF_RANGE;
3602  }
3603 
3604  // Function evaluation
3605  Step(pos,u_old-u,Sc,dedx);
3606  u_old=u;
3607  iter2++;
3608  }
3609  //printf("new z %f ds %f \n",pos.z(),x);
3610  ds_out=ds_temp;
3611  return NOERROR;
3612  }
3613  ds_out=cx-x;
3614  return NOERROR;
3615  }
3616  // trial parabolic fit
3617  if (fabs(e)>tol1){
3618  double x_minus_w=x-w;
3619  double x_minus_v=x-v;
3620  double r=x_minus_w*(fx-fv);
3621  double q=x_minus_v*(fx-fw);
3622  double p=x_minus_v*q-x_minus_w*r;
3623  q=2.0*(q-r);
3624  if (q>0.0) p=-p;
3625  q=fabs(q);
3626  double etemp=e;
3627  e=d;
3628  if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x))
3629  // fall back on the Golden Section technique
3630  d=CGOLD*(e=(x>=xm?a-x:b-x));
3631  else{
3632  // parabolic step
3633  d=p/q;
3634  u=x+d;
3635  if (u-a<tol2 || b-u <tol2)
3636  d=SIGN(tol1,xm-x);
3637  }
3638  } else{
3639  d=CGOLD*(e=(x>=xm?a-x:b-x));
3640  }
3641  u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d));
3642 
3643  // Bail if the transverse momentum has dropped below some minimum
3644  if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX){
3645  if (DEBUG_LEVEL>2)
3646  {
3647  _DBG_ << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt))
3648  << endl;
3649  }
3650  return VALUE_OUT_OF_RANGE;
3651  }
3652 
3653  // Function evaluation
3654  Step(pos,u_old-u,Sc,dedx);
3655  u_old=u;
3656 
3657  wirepos=origin;
3658  wirepos+=(Sc(state_z)-z0wire)*dir;
3659  double fu=(pos-wirepos).Mod2();
3660 
3661  //cout << "Brent: z="<<Sc(state_z) << " d="<<sqrt(fu) << endl;
3662 
3663  if (fu<=fx){
3664  if (u>=x) a=x; else b=x;
3665  SHFT(v,w,x,u);
3666  SHFT(fv,fw,fx,fu);
3667  }
3668  else {
3669  if (u<x) a=u; else b=u;
3670  if (fu<=fw || w==x){
3671  v=w;
3672  w=u;
3673  fv=fw;
3674  fw=fu;
3675  }
3676  else if (fu<=fv || v==x || v==w){
3677  v=u;
3678  fv=fu;
3679  }
3680  }
3681  }
3682  ds_out=cx-x;
3683 
3684  return NOERROR;
3685 }
3686 
3687 // Routine for finding the minimum of a function bracketed between two values
3688 // (see Numerical Recipes in C, pp. 404-405).
3689 jerror_t DTrackFitterKalmanSIMD::BrentsAlgorithm(double z,double dz,
3690  double dedx,
3691  const double z0wire,
3692  const DVector2 &origin,
3693  const DVector2 &dir,
3694  DMatrix5x1 &S,
3695  double &dz_out){
3696  double d=0.,u=0.;
3697  double e=0.0; // will be distance moved on step before last
3698  double ax=0.;
3699  double bx=-dz;
3700  double cx=-2.*dz;
3701 
3702  double a=(ax<cx?ax:cx);
3703  double b=(ax>cx?ax:cx);
3704  double x=bx,w=bx,v=bx;
3705 
3706  // Initialize dz_out
3707  dz_out=0.;
3708 
3709  // Step to intermediate point
3710  double z_new=z+x;
3711  Step(z,z_new,dedx,S);
3712  // Bail if the momentum has dropped below some minimum
3713  if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){
3714  if (DEBUG_LEVEL>2)
3715  {
3716  _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p))
3717  << endl;
3718  }
3719  return VALUE_OUT_OF_RANGE;
3720  }
3721 
3722  double dz0wire=z-z0wire;
3723  DVector2 wirepos=origin+(dz0wire+x)*dir;
3724  DVector2 pos(S(state_x),S(state_y));
3725  double z_old=z_new;
3726 
3727  // initialization
3728  double fw=(pos-wirepos).Mod2();
3729  double fv=fw;
3730  double fx=fw;
3731 
3732  // main loop
3733  for (unsigned int iter=1;iter<=ITMAX;iter++){
3734  double xm=0.5*(a+b);
3735  double tol1=EPS2*fabs(x)+ZEPS;
3736  double tol2=2.0*tol1;
3737  if (fabs(x-xm)<=(tol2-0.5*(b-a))){
3738  if (z_new>=endplate_z){
3739  x=endplate_z-z_new;
3740 
3741  // Bail if the momentum has dropped below some minimum
3742  if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){
3743  if (DEBUG_LEVEL>2)
3744  {
3745  _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p))
3746  << endl;
3747  }
3748  return VALUE_OUT_OF_RANGE;
3749  }
3750  if (!isfinite(S(state_x)) || !isfinite(S(state_y))){
3751  _DBG_ <<endl;
3752  return VALUE_OUT_OF_RANGE;
3753  }
3754  Step(z_new,endplate_z,dedx,S);
3755  }
3756  dz_out=x;
3757  return NOERROR;
3758  }
3759  // trial parabolic fit
3760  if (fabs(e)>tol1){
3761  double x_minus_w=x-w;
3762  double x_minus_v=x-v;
3763  double r=x_minus_w*(fx-fv);
3764  double q=x_minus_v*(fx-fw);
3765  double p=x_minus_v*q-x_minus_w*r;
3766  q=2.0*(q-r);
3767  if (q>0.0) p=-p;
3768  q=fabs(q);
3769  double etemp=e;
3770  e=d;
3771  if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x))
3772  // fall back on the Golden Section technique
3773  d=CGOLD*(e=(x>=xm?a-x:b-x));
3774  else{
3775  // parabolic step
3776  d=p/q;
3777  u=x+d;
3778  if (u-a<tol2 || b-u <tol2)
3779  d=SIGN(tol1,xm-x);
3780  }
3781  } else{
3782  d=CGOLD*(e=(x>=xm?a-x:b-x));
3783  }
3784  u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d));
3785 
3786  // Function evaluation
3787  //S=S0;
3788  z_new=z+u;
3789  // Bail if the momentum has dropped below some minimum
3790  if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){
3791  if (DEBUG_LEVEL>2)
3792  {
3793  _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p))
3794  << endl;
3795  }
3796  return VALUE_OUT_OF_RANGE;
3797  }
3798 
3799  Step(z_old,z_new,dedx,S);
3800  z_old=z_new;
3801 
3802  wirepos=origin;
3803  wirepos+=(dz0wire+u)*dir;
3804  pos.Set(S(state_x),S(state_y));
3805  double fu=(pos-wirepos).Mod2();
3806 
3807  // _DBG_ << "Brent: z="<< z+u << " d^2="<<fu << endl;
3808 
3809  if (fu<=fx){
3810  if (u>=x) a=x; else b=x;
3811  SHFT(v,w,x,u);
3812  SHFT(fv,fw,fx,fu);
3813  }
3814  else {
3815  if (u<x) a=u; else b=u;
3816  if (fu<=fw || w==x){
3817  v=w;
3818  w=u;
3819  fv=fw;
3820  fw=fu;
3821  }
3822  else if (fu<=fv || v==x || v==w){
3823  v=u;
3824  fv=fu;
3825  }
3826  }
3827  }
3828  dz_out=x;
3829  return NOERROR;
3830 }
3831 
3832 // Kalman engine for Central tracks; updates the position on the trajectory
3833 // after the last hit (closest to the target) is added
3835  DMatrix5x1 &Sc,
3836  DMatrix5x5 &Cc,
3837  DVector2 &xy,double &chisq,
3838  unsigned int &my_ndf
3839  ){
3840  DMatrix1x5 H; // Track projection matrix
3841  DMatrix5x1 H_T; // Transpose of track projection matrix
3842  DMatrix5x5 J; // State vector Jacobian matrix
3843  DMatrix5x5 Q; // Process noise covariance matrix
3844  DMatrix5x1 K; // KalmanSIMD gain matrix
3845  DMatrix5x5 Ctest; // covariance matrix
3846  // double V=0.2028; //1.56*1.56/12.; // Measurement variance
3847  double V=0.0507;
3848  double InvV; // inverse of variance
3849  //DMatrix5x1 dS; // perturbation in state vector
3850  DMatrix5x1 S0,S0_; // state vector
3851 
3852  // set the used_in_fit flags to false for cdc hits
3853  unsigned int num_cdc=cdc_used_in_fit.size();
3854  for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false;
3855  for (unsigned int i=0;i<central_traj.size();i++){
3856  central_traj[i].h_id=0;
3857  }
3858 
3859  // Initialize the chi2 for this part of the track
3860  chisq=0.;
3861  my_ndf=0;
3862 
3863  double chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT;
3864 
3865  // path length increment
3866  double ds2=0.;
3867 
3868  //printf(">>>>>>>>>>>>>>>>\n");
3869 
3870  // beginning position
3871  double phi=Sc(state_phi);
3872  xy.Set(central_traj[break_point_step_index].xy.X()-Sc(state_D)*sin(phi),
3873  central_traj[break_point_step_index].xy.Y()+Sc(state_D)*cos(phi));
3874 
3875  // Wire origin and direction
3876  // unsigned int cdc_index=my_cdchits.size()-1;
3877  unsigned int cdc_index=break_point_cdc_index;
3878  if (break_point_cdc_index<num_cdc-1){
3879  num_cdc=break_point_cdc_index+1;
3880  }
3881 
3882  if (num_cdc<MIN_HITS_FOR_REFIT) chi2cut=BIG;
3883 
3884  // Wire origin and direction
3885  DVector2 origin=my_cdchits[cdc_index]->origin;
3886  double z0w=my_cdchits[cdc_index]->z0wire;
3887  DVector2 dir=my_cdchits[cdc_index]->dir;
3888  DVector2 wirexy=origin+(Sc(state_z)-z0w)*dir;
3889 
3890  // Save the starting values for C and S in the deque
3893 
3894  // doca variables
3895  double doca2,old_doca2=(xy-wirexy).Mod2();
3896 
3897  // energy loss
3898  double dedx=0.;
3899 
3900  // Boolean for flagging when we are done with measurements
3901  bool more_measurements=true;
3902 
3903  // Initialize S0_ and perform the loop over the trajectory
3905 
3906  for (unsigned int k=break_point_step_index+1;k<central_traj.size();k++){
3907  unsigned int k_minus_1=k-1;
3908 
3909  // Check that C matrix is positive definite
3910  if (!Cc.IsPosDef()){
3911  if (DEBUG_LEVEL>0) _DBG_ << "Broken covariance matrix!" <<endl;
3912  return BROKEN_COVARIANCE_MATRIX;
3913  }
3914 
3915  // Get the state vector, jacobian matrix, and multiple scattering matrix
3916  // from reference trajectory
3917  S0=central_traj[k].S;
3918  J=central_traj[k].J;
3919  Q=central_traj[k].Q;
3920 
3921  //Q.Print();
3922  //J.Print();
3923 
3924  // State S is perturbation about a seed S0
3925  //dS=Sc-S0_;
3926  //dS.Zero();
3927 
3928  // Update the actual state vector and covariance matrix
3929  Sc=S0+J*(Sc-S0_);
3930  // Cc=J*(Cc*JT)+Q;
3931  // Cc=Q.AddSym(Cc.SandwichMultiply(J));
3932  Cc=Q.AddSym(J*Cc*J.Transpose());
3933 
3934  // Save the current state and covariance matrix in the deque
3935  if (fit_type==kTimeBased){
3936  central_traj[k].Skk=Sc;
3937  central_traj[k].Ckk=Cc;
3938  }
3939 
3940  // update position based on new doca to reference trajectory
3941  xy.Set(central_traj[k].xy.X()-Sc(state_D)*sin(Sc(state_phi)),
3942  central_traj[k].xy.Y()+Sc(state_D)*cos(Sc(state_phi)));
3943 
3944  // Bail if the position is grossly outside of the tracking volume
3945  if (xy.Mod2()>R2_MAX || Sc(state_z)<Z_MIN || Sc(state_z)>Z_MAX){
3946  if (DEBUG_LEVEL>2)
3947  {
3948  _DBG_<< "Went outside of tracking volume at z="<<Sc(state_z)
3949  << " r="<<xy.Mod()<<endl;
3950  _DBG_ << " Break indexes: " << break_point_cdc_index <<","
3951  << break_point_step_index << endl;
3952  }
3953  return POSITION_OUT_OF_RANGE;
3954  }
3955  // Bail if the transverse momentum has dropped below some minimum
3956  if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX){
3957  if (DEBUG_LEVEL>2)
3958  {
3959  _DBG_ << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt))
3960  << " at step " << k
3961  << endl;
3962  }
3963  return MOMENTUM_OUT_OF_RANGE;
3964  }
3965 
3966 
3967  // Save the current state of the reference trajectory
3968  S0_=S0;
3969 
3970  // new wire position
3971  wirexy=origin;
3972  wirexy+=(Sc(state_z)-z0w)*dir;
3973 
3974  // new doca
3975  doca2=(xy-wirexy).Mod2();
3976 
3977  // Check if the doca is no longer decreasing
3978  if (more_measurements && (doca2>old_doca2 && Sc(state_z)>cdc_origin[2])){
3979  if (my_cdchits[cdc_index]->status==good_hit){
3980  if (DEBUG_LEVEL>9) {
3981  _DBG_ << " Good Hit Ring " << my_cdchits[cdc_index]->hit->wire->ring << " Straw " << my_cdchits[cdc_index]->hit->wire->straw << endl;
3982  _DBG_ << " doca " << sqrt(doca2) << endl;
3983  }
3984 
3985  // Save values at end of current step
3986  DVector2 xy0=central_traj[k].xy;
3987 
3988  // Variables for the computation of D at the doca to the wire
3989  double D=Sc(state_D);
3990  double q=(Sc(state_q_over_pt)>0.0)?1.:-1.;
3991 
3993 
3994  double qpt=1./Sc(state_q_over_pt) * FactorForSenseOfRotation;
3995  double sinphi=sin(Sc(state_phi));
3996  double cosphi=cos(Sc(state_phi));
3997  //double qrc_old=qpt/fabs(qBr2p*bfield->GetBz(pos.x(),pos.y(),pos.z()));
3998  double qrc_old=qpt/fabs(qBr2p*central_traj[k].B);
3999  double qrc_plus_D=D+qrc_old;
4000 
4001  // wire direction variable
4002  double ux=dir.X();
4003  double uy=dir.Y();
4004  double cosstereo=my_cdchits[cdc_index]->cosstereo;
4005  // Variables relating wire direction and track direction
4006  //double my_ux=ux*sinl-cosl*cosphi;
4007  //double my_uy=uy*sinl-cosl*sinphi;
4008  //double denom=my_ux*my_ux+my_uy*my_uy;
4009  // distance variables
4010  DVector2 diff,dxy1;
4011 
4012  // use Brent's algorithm to find the poca to the wire
4013  // See Numerical Recipes in C, pp 404-405
4014 
4015  // dEdx for current position along trajectory
4016  double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
4017  if (CORRECT_FOR_ELOSS){
4018  dedx=GetdEdx(q_over_p, central_traj[k].K_rho_Z_over_A,
4019  central_traj[k].rho_Z_over_A,
4020  central_traj[k].LnI,central_traj[k].Z);
4021  }
4022 
4023  if (BrentCentral(dedx,xy,z0w,origin,dir,Sc,ds2)!=NOERROR) return MOMENTUM_OUT_OF_RANGE;
4024 
4025  //Step along the reference trajectory and compute the new covariance matrix
4026  StepStateAndCovariance(xy0,ds2,dedx,S0,J,Cc);
4027 
4028  // Compute the value of D (signed distance to the reference trajectory)
4029  // at the doca to the wire
4030  dxy1=xy0-central_traj[k].xy;
4031  double rc=sqrt(dxy1.Mod2()
4032  +2.*qrc_plus_D*(dxy1.X()*sinphi-dxy1.Y()*cosphi)
4033  +qrc_plus_D*qrc_plus_D);
4034  Sc(state_D)=q*rc-qrc_old;
4035 
4036  // wire position
4037  wirexy=origin;
4038  wirexy+=(Sc(state_z)-z0w)*dir;
4039  diff=xy-wirexy;
4040 
4041  // prediction for measurement
4042  double doca=diff.Mod()+EPS;
4043  double prediction=doca*cosstereo;
4044 
4045  // Measurement
4046  double measurement=0.39,tdrift=0.,tcorr=0.,dDdt0=0.;
4048  // Find offset of wire with respect to the center of the
4049  // straw at this z position
4050  const DCDCWire *mywire=my_cdchits[cdc_index]->hit->wire;
4051  int ring_index=mywire->ring-1;
4052  int straw_index=mywire->straw-1;
4053  double dz=Sc(state_z)-z0w;
4054  double phi_d=diff.Phi();
4055  double delta
4056  =max_sag[ring_index][straw_index]*(1.-dz*dz/5625.)
4057  *cos(phi_d + sag_phi_offset[ring_index][straw_index]);
4058  double dphi=phi_d-mywire->origin.Phi();
4059  while (dphi>M_PI) dphi-=2*M_PI;
4060  while (dphi<-M_PI) dphi+=2*M_PI;
4061  if (mywire->origin.Y()<0) dphi*=-1.;
4062 
4063  tdrift=my_cdchits[cdc_index]->tdrift-mT0
4064  -central_traj[k_minus_1].t*TIME_UNIT_CONVERSION;
4065  double B=central_traj[k_minus_1].B;
4066  ComputeCDCDrift(dphi,delta,tdrift,B,measurement,V,tcorr);
4067  V*=anneal_factor;
4068  if (ALIGNMENT_CENTRAL){
4069  double myV=0.;
4070  double mytcorr=0.;
4071  double d_shifted;
4072  double dt=2.0;
4073  ComputeCDCDrift(dphi,delta,tdrift+dt,B,d_shifted,myV,mytcorr);
4074  dDdt0=(d_shifted-measurement)/dt;
4075  }
4076 
4077  //_DBG_ << tcorr << " " << dphi << " " << dm << endl;
4078 
4079  }
4080 
4081  // Projection matrix
4082  sinphi=sin(Sc(state_phi));
4083  cosphi=cos(Sc(state_phi));
4084  double dx=diff.X();
4085  double dy=diff.Y();
4086  double cosstereo_over_doca=cosstereo/doca;
4087  H_T(state_D)=(dy*cosphi-dx*sinphi)*cosstereo_over_doca;
4088  H_T(state_phi)
4089  =-Sc(state_D)*cosstereo_over_doca*(dx*cosphi+dy*sinphi);
4090  H_T(state_z)=-cosstereo_over_doca*(dx*ux+dy*uy);
4091  H(state_tanl)=0.;
4092  H_T(state_tanl)=0.;
4093  H(state_D)=H_T(state_D);
4094  H(state_z)=H_T(state_z);
4095  H(state_phi)=H_T(state_phi);
4096 
4097  // Difference and inverse of variance
4098  //InvV=1./(V+H*(Cc*H_T));
4099  //double Vproj=Cc.SandwichMultiply(H_T);
4100  double Vproj=H*Cc*H_T;
4101  InvV=1./(V+Vproj);
4102  double dm=measurement-prediction;
4103 
4104  if (InvV<0.){
4105  if (DEBUG_LEVEL>1)
4106  _DBG_ << k <<" "<< central_traj.size()-1<<" Negative variance??? " << V << " " << H*(Cc*H_T) <<endl;
4107 
4108  break_point_cdc_index=(3*num_cdc)/4;
4109  return NEGATIVE_VARIANCE;
4110  }
4111  /*
4112  if (fabs(cosstereo)<1.){
4113  printf("t %f delta %f sigma %f V %f chi2 %f\n",my_cdchits[cdc_index]->hit->tdrift-mT0,dm,sqrt(V),1./InvV,dm*dm*InvV);
4114  }
4115  */
4116 
4117  // Check how far this hit is from the expected position
4118  double chi2check=dm*dm*InvV;
4119  if (DEBUG_LEVEL>9) _DBG_ << " Prediction " << prediction << " Measurement " << measurement << " Chi2 " << chi2check << endl;
4120  if (chi2check<chi2cut)
4121  {
4122  if (DEBUG_LEVEL>9) _DBG_ << " Passed Chi^2 check Ring " << my_cdchits[cdc_index]->hit->wire->ring << " Straw " << my_cdchits[cdc_index]->hit->wire->straw << endl;
4123 
4124  // Compute Kalman gain matrix
4125  K=InvV*(Cc*H_T);
4126 
4127  // Update state vector covariance matrix
4128  //Cc=Cc-(K*(H*Cc));
4129  Ctest=Cc.SubSym(K*(H*Cc));
4130  // Joseph form
4131  // C = (I-KH)C(I-KH)^T + KVK^T
4132  //Ctest=Cc.SandwichMultiply(I5x5-K*H)+V*MultiplyTranspose(K);
4133  // Check that Ctest is positive definite
4134  if (!Ctest.IsPosDef()){
4135  if (DEBUG_LEVEL>0) _DBG_ << "Broken covariance matrix!" <<endl;
4136  return BROKEN_COVARIANCE_MATRIX;
4137  }
4138  bool skip_ring
4139  =(my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP);
4140  //Update covariance matrix and state vector
4141  if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){
4142  Cc=Ctest;
4143  Sc+=dm*K;
4144  }
4145 
4146  // Mark point on ref trajectory with a hit id for the straw
4147  central_traj[k].h_id=cdc_index+1;
4148  if (DEBUG_LEVEL>9) _DBG_ << " Marked Trajectory central_traj[k].h_id=cdc_index+1 (k cdc_index)" << k << " " << cdc_index << endl;
4149 
4150  // Save some updated information for this hit
4151  double scale=(skip_ring)?1.:(1.-H*K);
4152  cdc_updates[cdc_index].tcorr=tcorr;
4153  cdc_updates[cdc_index].tdrift=tdrift;
4154  cdc_updates[cdc_index].doca=measurement;
4155  cdc_updates[cdc_index].variance=V;
4156  cdc_updates[cdc_index].dDdt0=dDdt0;
4157  cdc_used_in_fit[cdc_index]=true;
4158  if (tdrift < CDC_T_DRIFT_MIN) cdc_used_in_fit[cdc_index]=false;
4159 
4160  // Update chi2 for this hit
4161  if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){
4162  chisq+=scale*dm*dm/V;
4163  my_ndf++;
4164  }
4165  if (DEBUG_LEVEL>10)
4166  cout
4167  << "ring " << my_cdchits[cdc_index]->hit->wire->ring
4168  << " t " << my_cdchits[cdc_index]->hit->tdrift
4169  << " Dm-Dpred " << dm
4170  << " chi2 " << (1.-H*K)*dm*dm/V
4171  << endl;
4172 
4173  break_point_cdc_index=cdc_index;
4174  break_point_step_index=k_minus_1;
4175 
4176  //else printf("Negative variance!!!\n");
4177 
4178 
4179  }
4180 
4181  // Move back to the right step along the reference trajectory.
4182  StepStateAndCovariance(xy,-ds2,dedx,Sc,J,Cc);
4183 
4184  // Save state and covariance matrix to update vector
4185  cdc_updates[cdc_index].S=Sc;
4186  cdc_updates[cdc_index].C=Cc;
4187 
4188  //Sc.Print();
4189  //Cc.Print();
4190 
4191  // update position on current trajectory based on corrected doca to
4192  // reference trajectory
4193  xy.Set(central_traj[k].xy.X()-Sc(state_D)*sin(Sc(state_phi)),
4194  central_traj[k].xy.Y()+Sc(state_D)*cos(Sc(state_phi)));
4195 
4196  }
4197 
4198  // new wire origin and direction
4199  if (cdc_index>0){
4200  cdc_index--;
4201  origin=my_cdchits[cdc_index]->origin;
4202  z0w=my_cdchits[cdc_index]->z0wire;
4203  dir=my_cdchits[cdc_index]->dir;
4204  }
4205  else{
4206  more_measurements=false;
4207  }
4208 
4209  // Update the wire position
4210  wirexy=origin+(Sc(state_z)-z0w)*dir;
4211 
4212  //s+=ds2;
4213  // new doca
4214  doca2=(xy-wirexy).Mod2();
4215  }
4216 
4217  old_doca2=doca2;
4218 
4219  }
4220 
4221  // If there are not enough degrees of freedom, something went wrong...
4222  if (my_ndf<6){
4223  chisq=-1.;
4224  my_ndf=0;
4225  return PRUNED_TOO_MANY_HITS;
4226  }
4227  else my_ndf-=5;
4228 
4229  // Check if the momentum is unphysically large
4230  double p=cos(atan(Sc(state_tanl)))/fabs(Sc(state_q_over_pt));
4231  if (p>12.0){
4232  if (DEBUG_LEVEL>2)
4233  {
4234  _DBG_ << "Unphysical momentum: P = " << p <<endl;
4235  }
4236  return MOMENTUM_OUT_OF_RANGE;
4237  }
4238 
4239  // Check if we have a kink in the track or threw away too many cdc hits
4240  if (num_cdc>=MIN_HITS_FOR_REFIT){
4241  if (break_point_cdc_index>1){
4242  if (break_point_cdc_index<num_cdc/2){
4243  break_point_cdc_index=(3*num_cdc)/4;
4244  }
4245  return BREAK_POINT_FOUND;
4246  }
4247 
4248  unsigned int num_good=0;
4249  for (unsigned int j=0;j<num_cdc;j++){
4250  if (cdc_used_in_fit[j]) num_good++;
4251  }
4252  if (double(num_good)/double(num_cdc)<MINIMUM_HIT_FRACTION){
4253  return PRUNED_TOO_MANY_HITS;
4254  }
4255  }
4256 
4257  return FIT_SUCCEEDED;
4258 }
4259 
4260 // Kalman engine for forward tracks
4262  double cdc_anneal_factor,
4263  DMatrix5x1 &S,
4264  DMatrix5x5 &C,
4265  double &chisq,
4266  unsigned int &numdof){
4267  DMatrix2x1 Mdiff; // difference between measurement and prediction
4268  DMatrix2x5 H; // Track projection matrix
4269  DMatrix5x2 H_T; // Transpose of track projection matrix
4270  DMatrix1x5 Hc; // Track projection matrix for cdc hits
4271  DMatrix5x1 Hc_T; // Transpose of track projection matrix for cdc hits
4272  DMatrix5x5 J; // State vector Jacobian matrix
4273  //DMatrix5x5 J_T; // transpose of this matrix
4274  DMatrix5x5 Q; // Process noise covariance matrix
4275  DMatrix5x2 K; // Kalman gain matrix
4276  DMatrix5x1 Kc; // Kalman gain matrix for cdc hits
4277  DMatrix2x2 V(0.0833,0.,0.,FDC_CATHODE_VARIANCE); // Measurement covariance matrix
4278  DMatrix2x1 R; // Filtered residual
4279  DMatrix2x2 RC; // Covariance of filtered residual
4280  DMatrix5x1 S0,S0_; //State vector
4281  DMatrix5x5 Ctest; // Covariance matrix
4282  DMatrix2x2 InvV; // Inverse of error matrix
4283 
4284  double Vc=0.0507;
4285 
4286  // Vectors for cdc wires
4287  DVector2 origin,dir,wirepos;
4288  double z0w=0.; // origin in z for wire
4289 
4290  // Set used_in_fit flags to false for fdc and cdc hits
4291  unsigned int num_cdc=cdc_used_in_fit.size();
4292  unsigned int num_fdc=fdc_used_in_fit.size();
4293  for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false;
4294  for (unsigned int i=0;i<num_fdc;i++) fdc_used_in_fit[i]=false;
4295  for (unsigned int i=0;i<forward_traj.size();i++){
4296  if (forward_traj[i].h_id>999)
4297  forward_traj[i].h_id=0;
4298  }
4299 
4300  // Save the starting values for C and S in the deque
4303 
4304  // Initialize chi squared
4305  chisq=0;
4306 
4307  // Initialize number of degrees of freedom
4308  numdof=0;
4309 
4310  double fdc_chi2cut=NUM_FDC_SIGMA_CUT*NUM_FDC_SIGMA_CUT;
4311  double cdc_chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT;
4312 
4313  unsigned int num_fdc_hits=break_point_fdc_index+1;
4314  unsigned int max_num_fdc_used_in_fit=num_fdc_hits;
4315  unsigned int num_cdc_hits=my_cdchits.size();
4316  unsigned int cdc_index=0;
4317  if (num_cdc_hits>0) cdc_index=num_cdc_hits-1;
4318  bool more_cdc_measurements=(num_cdc_hits>0);
4319  double old_doca2=1e6;
4320 
4321  if (num_fdc_hits+num_cdc_hits<MIN_HITS_FOR_REFIT){
4322  cdc_chi2cut=BIG;
4323  fdc_chi2cut=BIG;
4324  }
4325 
4326  if (more_cdc_measurements){
4327  origin=my_cdchits[cdc_index]->origin;
4328  dir=my_cdchits[cdc_index]->dir;
4329  z0w=my_cdchits[cdc_index]->z0wire;
4330  wirepos=origin+(forward_traj[break_point_step_index].z-z0w)*dir;
4331  }
4332 
4334 
4335  if (DEBUG_LEVEL > 25){
4336  jout << "Entering DTrackFitterKalmanSIMD::KalmanForward ================================" << endl;
4337  jout << " We have the following starting parameters for our fit. S = State vector, C = Covariance matrix" << endl;
4338  S.Print();
4339  C.Print();
4340  jout << setprecision(6);
4341  jout << " There are " << num_cdc << " CDC Updates and " << num_fdc << " FDC Updates on this track" << endl;
4342  jout << " There are " << num_cdc_hits << " CDC Hits and " << num_fdc_hits << " FDC Hits on this track" << endl;
4343  jout << " With NUM_FDC_SIGMA_CUT = " << NUM_FDC_SIGMA_CUT << " and NUM_CDC_SIGMA_CUT = " << NUM_CDC_SIGMA_CUT << endl;
4344  jout << " fdc_anneal_factor = " << fdc_anneal_factor << " cdc_anneal_factor = " << cdc_anneal_factor << endl;
4345  jout << " yields fdc_chi2cut = " << fdc_chi2cut << " cdc_chi2cut = " << cdc_chi2cut << endl;
4346  jout << " Starting from break_point_step_index " << break_point_step_index << endl;
4347  jout << " S0_ which is the state vector of the reference trajectory at the break point step:" << endl;
4348  S0_.Print();
4349  jout << " ===== Beginning pass over reference trajectory ======== " << endl;
4350  }
4351 
4352  for (unsigned int k=break_point_step_index+1;k<forward_traj.size();k++){
4353  unsigned int k_minus_1=k-1;
4354 
4355  // Check that C matrix is positive definite
4356  if (!C.IsPosDef()){
4357  if (DEBUG_LEVEL>0) _DBG_ << "Broken covariance matrix!" <<endl;
4358  return BROKEN_COVARIANCE_MATRIX;
4359  }
4360 
4361  // Get the state vector, jacobian matrix, and multiple scattering matrix
4362  // from reference trajectory
4363  S0=(forward_traj[k].S);
4364  J=(forward_traj[k].J);
4365  Q=(forward_traj[k].Q);
4366 
4367  // State S is perturbation about a seed S0
4368  //dS=S-S0_;
4369 
4370  // Update the actual state vector and covariance matrix
4371  S=S0+J*(S-S0_);
4372 
4373  // Bail if the momentum has dropped below some minimum
4374  if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX){
4375  if (DEBUG_LEVEL>2)
4376  {
4377  _DBG_ << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl;
4378  }
4379  break_point_fdc_index=(3*num_fdc)/4;
4380  return MOMENTUM_OUT_OF_RANGE;
4381  }
4382 
4383 
4384  //C=J*(C*J_T)+Q;
4385  //C=Q.AddSym(C.SandwichMultiply(J));
4386  C=Q.AddSym(J*C*J.Transpose());
4387 
4388  // Save the current state and covariance matrix in the deque
4389  forward_traj[k].Skk=S;
4390  forward_traj[k].Ckk=C;
4391 
4392  // Save the current state of the reference trajectory
4393  S0_=S0;
4394 
4395  // Z position along the trajectory
4396  double z=forward_traj[k].z;
4397 
4398  if (DEBUG_LEVEL > 25){
4399  jout << " At reference trajectory index " << k << " at z=" << z << endl;
4400  jout << " The State vector from the reference trajectory" << endl;
4401  S0.Print();
4402  jout << " The Jacobian matrix " << endl;
4403  J.Print();
4404  jout << " The Q matrix "<< endl;
4405  Q.Print();
4406  jout << " The updated State vector S=S0+J*(S-S0_)" << endl;
4407  S.Print();
4408  jout << " The updated Covariance matrix C=J*(C*J_T)+Q;" << endl;
4409  C.Print();
4410  }
4411 
4412  // Add the hit
4413  if (num_fdc_hits>0){
4414  if (forward_traj[k].h_id>0 && forward_traj[k].h_id<1000){
4415  unsigned int id=forward_traj[k].h_id-1;
4416 
4417  // Make the small alignment rotations
4418  // Use small-angle form.
4419 
4420  // Position and direction from state vector
4421  double x=S(state_x) + my_fdchits[id]->phiZ*S(state_y);
4422  double y=S(state_y) - my_fdchits[id]->phiZ*S(state_x);
4423  double tx = (S(state_tx) + my_fdchits[id]->phiZ*S(state_ty) - my_fdchits[id]->phiY) ;
4424  double ty = (S(state_ty) - my_fdchits[id]->phiZ*S(state_tx) + my_fdchits[id]->phiX) ;
4425 
4426  double cosa=my_fdchits[id]->cosa;
4427  double sina=my_fdchits[id]->sina;
4428  double u=my_fdchits[id]->uwire;
4429  double v=my_fdchits[id]->vstrip;
4430 
4431  // Projected position along the wire without doca-dependent corrections
4432  double vpred_uncorrected=x*sina+y*cosa;
4433 
4434  // Projected postion in the plane of the wires transverse to the wires
4435  double upred=x*cosa-y*sina;
4436 
4437  // Direction tangent in the u-z plane
4438  double tu=tx*cosa-ty*sina;
4439  double alpha=atan(tu);
4440  double cosalpha=cos(alpha);
4441  double cosalpha2=cosalpha*cosalpha;
4442  double sinalpha=sin(alpha);
4443 
4444  // (signed) distance of closest approach to wire
4445  double du=upred-u;
4446  double doca=du*cosalpha;
4447 
4448  // Correction for lorentz effect
4449  double nz=my_fdchits[id]->nz;
4450  double nr=my_fdchits[id]->nr;
4451  double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha;
4452 
4453  // Variance in coordinate along wire
4454  V(1,1)=my_fdchits[id]->vvar*fdc_anneal_factor;
4455 
4456  // Difference between measurement and projection
4457  double tv=tx*sina+ty*cosa;
4458  Mdiff(1)=v-(vpred_uncorrected+doca*(nz_sinalpha_plus_nr_cosalpha
4459  -tv*sinalpha
4460  ));
4461  Mdiff(0)=-doca;
4462 
4464  double drift_time=my_fdchits[id]->t-mT0
4466  //double drift=DRIFT_SPEED*drift_time*(du>0?1.:-1.);
4467  double drift=(du>0.0?1.:-1.)*fdc_drift_distance(drift_time,forward_traj[k].B);
4468 
4469  Mdiff(0)=drift-doca;
4470 
4471  // Variance in drift distance
4472  V(0,0)=fdc_drift_variance(drift_time)*fdc_anneal_factor;
4473  }
4474 
4475  // To transform from (x,y) to (u,v), need to do a rotation:
4476  // u = x*cosa-y*sina
4477  // v = y*cosa+x*sina
4478  double temp2=nz_sinalpha_plus_nr_cosalpha-tv*sinalpha;
4479  H_T(state_x,1)=sina+cosa*cosalpha*temp2;
4480  H_T(state_y,1)=cosa-sina*cosalpha*temp2;
4481 
4482  double cos2_minus_sin2=cosalpha2-sinalpha*sinalpha;
4483  double fac=nz*cos2_minus_sin2-2.*nr*cosalpha*sinalpha;
4484  double doca_cosalpha=doca*cosalpha;
4485  double temp=doca_cosalpha*fac;
4486  H_T(state_tx,1)=cosa*temp
4487  -doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2)
4488  ;
4489  H_T(state_ty,1)=-sina*temp
4490  -doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2)
4491  ;
4492 
4493  H_T(state_x,0)=cosa*cosalpha;
4494  H_T(state_y,0)=-sina*cosalpha;
4495  double one_plus_tu2=1.+tu*tu;
4496  double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2;
4497  H_T(state_ty,0)=sina*factor;
4498  H_T(state_tx,0)=-cosa*factor;
4499 
4500  // Matrix transpose H_T -> H
4501  H=Transpose(H_T);
4502 
4503  // Check to see if we have multiple hits in the same plane
4504  if (!ALIGNMENT_FORWARD && forward_traj[k].num_hits>1){
4505  // If we do have multiple hits, then all of the hits within some
4506  // validation region are included with weights determined by how
4507  // close the hits are to the track projection of the state to the
4508  // "hit space".
4509  vector<DMatrix5x2> Klist;
4510  vector<DMatrix2x1> Mlist;
4511  vector<DMatrix2x5> Hlist;
4512  vector<DMatrix5x2> HTlist;
4513  vector<DMatrix2x2> Vlist;
4514  vector<double>probs;
4515  vector<unsigned int>used_ids;
4516 
4517  // Deal with the first hit:
4518  //double Vtemp=V+H*C*H_T;
4519  DMatrix2x2 Vtemp=V+H*C*H_T;
4520  InvV=Vtemp.Invert();
4521 
4522  //probability
4523  double chi2_hit=Vtemp.Chi2(Mdiff);
4524  double prob_hit=exp(-0.5*chi2_hit)
4525  /(M_TWO_PI*sqrt(Vtemp.Determinant()));
4526 
4527  if (DEBUG_LEVEL > 25) jout << " == There are multiple (" << forward_traj[k].num_hits << ") FDC hits" << endl;
4528 
4529  // Cut out outliers
4530  if (chi2_hit<fdc_chi2cut && my_fdchits[id]->status==good_hit){
4531  probs.push_back(prob_hit);
4532  Vlist.push_back(V);
4533  Hlist.push_back(H);
4534  HTlist.push_back(H_T);
4535  Mlist.push_back(Mdiff);
4536  Klist.push_back(C*H_T*InvV); // Kalman gain
4537 
4538  used_ids.push_back(id);
4539  fdc_used_in_fit[id]=true;
4540  }
4541 
4542  // loop over the remaining hits
4543  for (unsigned int m=1;m<forward_traj[k].num_hits;m++){
4544  unsigned int my_id=id-m;
4545  if (my_fdchits[my_id]->status==good_hit){
4546  u=my_fdchits[my_id]->uwire;
4547  v=my_fdchits[my_id]->vstrip;
4548 
4549  // Doca to this wire
4550  du=upred-u;
4551  doca=du*cosalpha;
4552 
4553  // variance for coordinate along the wire
4554  V(1,1)=my_fdchits[my_id]->vvar;
4555 
4556  // Difference between measurement and projection
4557  Mdiff(1)=v-(vpred_uncorrected+doca*(nz_sinalpha_plus_nr_cosalpha
4558  -tv*sinalpha
4559  ));
4560  Mdiff(0)=-doca;
4562  double drift_time=my_fdchits[id]->t-mT0
4564  //double drift=DRIFT_SPEED*drift_time*(du>0?1.:-1.);
4565  double drift=(du>0.0?1.:-1.)*fdc_drift_distance(drift_time,forward_traj[k].B);
4566 
4567  Mdiff(0)=drift-doca;
4568 
4569  // Variance in drift distance
4570  V(0,0)=fdc_drift_variance(drift_time);
4571 
4572  }
4573 
4574  // Update the terms in H/H_T that depend on the particular hit
4575  doca_cosalpha=doca*cosalpha;
4576  temp=doca_cosalpha*fac;
4577  H_T(state_tx,1)=cosa*temp
4578  -doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2)
4579  ;
4580  H_T(state_ty,1)=-sina*temp
4581  -doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2)
4582  ;
4583  factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2;
4584  H_T(state_ty,0)=sina*factor;
4585  H_T(state_tx,0)=-cosa*factor;
4586 
4587  // Matrix transpose H_T -> H
4588  H(1,state_tx)=H_T(state_tx,1);
4589  H(1,state_ty)=H_T(state_ty,1);
4590  H(0,state_ty)=H_T(state_ty,0);
4591  H(0,state_tx)=H_T(state_tx,0);
4592 
4593  // Calculate the kalman gain for this hit
4594  ///Vtemp=V+H*C*H_T;
4595  Vtemp=V+H*C*H_T;
4596  InvV=Vtemp.Invert();
4597 
4598  // probability
4599  double chi2_hit=Vtemp.Chi2(Mdiff);
4600  double prob_hit=exp(-0.5*chi2_hit)
4601  /(M_TWO_PI*sqrt(Vtemp.Determinant()));
4602 
4603  // Cut out outliers
4604  if(chi2_hit<fdc_chi2cut){
4605  probs.push_back(prob_hit);
4606  Mlist.push_back(Mdiff);
4607  Vlist.push_back(V);
4608  Hlist.push_back(H);
4609  HTlist.push_back(H_T);
4610  Klist.push_back(C*H_T*InvV);
4611 
4612  used_ids.push_back(my_id);
4613  fdc_used_in_fit[my_id]=true;
4614 
4615  }
4616  }
4617  }
4618  double prob_tot=1e-100;
4619  for (unsigned int m=0;m<probs.size();m++){
4620  prob_tot+=probs[m];
4621  }
4622 
4623  // Adjust the state vector and the covariance using the hit
4624  //information
4625  bool skip_plane=(my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP);
4626  if (skip_plane==false){
4628  DMatrix5x5 sum2;
4629  for (unsigned int m=0;m<Klist.size();m++){
4630  double my_prob=probs[m]/prob_tot;
4631  S+=my_prob*(Klist[m]*Mlist[m]);
4632  sum-=my_prob*(Klist[m]*Hlist[m]);
4633  sum2+=(my_prob*my_prob)*(Klist[m]*Vlist[m]*Transpose(Klist[m]));
4634 
4635  // Update chi2
4636  DMatrix2x2 HK=Hlist[m]*Klist[m];
4637  R=Mlist[m]-HK*Mlist[m];
4638  RC=Vlist[m]-HK*Vlist[m];
4639  chisq+=my_prob*RC.Chi2(R);
4640 
4641  unsigned int my_id=used_ids[m];
4642  fdc_updates[my_id].V=RC;
4643 
4644  if (DEBUG_LEVEL > 25) {
4645  jout << " Adjusting state vector for FDC hit " << m << " with prob " << my_prob << " S:" << endl;
4646  S.Print();
4647  }
4648  }
4649  // C=C.SandwichMultiply(sum)+sum2;
4650  C=sum2.AddSym(sum*C*sum.Transpose());
4651 
4652  if (DEBUG_LEVEL > 25) { jout << " C: " << endl; C.Print();}
4653  }
4654 
4655  for (unsigned int m=0;m<Hlist.size();m++){
4656  unsigned int my_id=used_ids[m];
4657  fdc_updates[my_id].S=S;
4658  fdc_updates[my_id].C=C;
4659  fdc_updates[my_id].tdrift
4661  fdc_updates[my_id].tcorr=fdc_updates[my_id].tdrift; // temporary!
4662  fdc_updates[my_id].doca=doca;
4663 
4664  if (skip_plane){
4665  fdc_updates[my_id].V=Vlist[m];
4666  }
4667  }
4668 
4669  // update number of degrees of freedom
4670  if (skip_plane==false){
4671  numdof+=2;
4672  }
4673  }
4674  else{
4675  if (DEBUG_LEVEL > 25) jout << " == There is a single FDC hit on this plane" << endl;
4676 
4677  // Variance for this hit
4678  DMatrix2x2 Vtemp=V+H*C*H_T;
4679  InvV=Vtemp.Invert();
4680 
4681  // Check if this hit is an outlier
4682  double chi2_hit=Vtemp.Chi2(Mdiff);
4683  if (chi2_hit<fdc_chi2cut){
4684  // Compute Kalman gain matrix
4685  K=C*H_T*InvV;
4686 
4687  bool skip_plane=(my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP);
4688  if (skip_plane==false){
4689  // Update the state vector
4690  S+=K*Mdiff;
4691 
4692  // Update state vector covariance matrix
4693  //C=C-K*(H*C);
4694  C=C.SubSym(K*(H*C));
4695 
4696  if (DEBUG_LEVEL > 25) {
4697  jout << "S Update: " << endl; S.Print();
4698  jout << "C Uodate: " << endl; C.Print();
4699  }
4700  }
4701 
4702  // Store the "improved" values for the state vector and covariance
4703  fdc_updates[id].S=S;
4704  fdc_updates[id].C=C;
4705  fdc_updates[id].tdrift
4707  fdc_updates[id].tcorr=fdc_updates[id].tdrift; // temporary!
4708  fdc_updates[id].doca=doca;
4709  fdc_used_in_fit[id]=true;
4710 
4711  if (skip_plane==false){
4712  // Filtered residual and covariance of filtered residual
4713  R=Mdiff-H*K*Mdiff;
4714  RC=V-H*(C*H_T);
4715 
4716  fdc_updates[id].V=RC;
4717 
4718  // Update chi2 for this segment
4719  chisq+=RC.Chi2(R);
4720 
4721  // update number of degrees of freedom
4722  numdof+=2;
4723 
4724 
4725  if (DEBUG_LEVEL>20)
4726  {
4727  printf("hit %d p %5.2f t %f dm %5.2f sig %f chi2 %5.2f z %5.2f\n",
4728  id,1./S(state_q_over_p),fdc_updates[id].tdrift,Mdiff(1),
4729  sqrt(V(1,1)),RC.Chi2(R),
4730  forward_traj[k].z);
4731 
4732  }
4733  }
4734  else{
4735  fdc_updates[id].V=V;
4736  }
4737 
4740  }
4741  }
4742  if (num_fdc_hits>=forward_traj[k].num_hits)
4743  num_fdc_hits-=forward_traj[k].num_hits;
4744  }
4745  }
4746  else if (more_cdc_measurements /* && z<endplate_z*/){
4747  // new wire position
4748  wirepos=origin;
4749  wirepos+=(z-z0w)*dir;
4750 
4751  // doca variables
4752  double dx=S(state_x)-wirepos.X();
4753  double dy=S(state_y)-wirepos.Y();
4754  double doca2=dx*dx+dy*dy;
4755 
4756  // Check if the doca is no longer decreasing
4757  if (doca2>old_doca2 /* && z<endplate_z */){
4758  if(my_cdchits[cdc_index]->status==good_hit){
4759  double newz=z;
4760 
4761  // energy loss
4762  double dedx=0.;
4763 
4764  // track direction variables
4765  double tx=S(state_tx);
4766  double ty=S(state_ty);
4767  double tanl=1./sqrt(tx*tx+ty*ty);
4768  double sinl=sin(atan(tanl));
4769 
4770  // Wire direction variables
4771  double ux=dir.X();
4772  double uy=dir.Y();
4773  // Variables relating wire direction and track direction
4774  double my_ux=tx-ux;
4775  double my_uy=ty-uy;
4776  double denom=my_ux*my_ux+my_uy*my_uy;
4777  double dz=0.;
4778 
4779 
4780  // variables for dealing with propagation of S and C if we
4781  // need to use Brent's algorithm to find the doca to the wire
4782  int num_steps=0;
4783  double dz3=0.;
4784  double my_dz=0.;
4785 
4786  // if the path length increment is small relative to the radius
4787  // of curvature, use a linear approximation to find dz
4788  bool do_brent=false;
4789  double step1=mStepSizeZ;
4790  double step2=mStepSizeZ;
4791  if (k>=2){
4792  step1=-forward_traj[k].z+forward_traj[k_minus_1].z;
4793  step2=-forward_traj[k_minus_1].z+forward_traj[k-2].z;
4794  }
4795  //printf("step1 %f step 2 %f \n",step1,step2);
4796  double two_step=step1+step2;
4797  if (fabs(qBr2p*S(state_q_over_p)
4798  *bfield->GetBz(S(state_x),S(state_y),z)
4799  *two_step/sinl)<0.05
4800  && denom>EPS)
4801  {
4802  double dzw=z-z0w;
4803  dz=-((S(state_x)-origin.X()-ux*dzw)*my_ux
4804  +(S(state_y)-origin.Y()-uy*dzw)*my_uy)/denom;
4805 
4806  if (fabs(dz)>two_step || dz<0){
4807  do_brent=true;
4808  }
4809  else{
4810  newz=z+dz;
4811  // Check for exiting the straw
4812  if (newz>endplate_z){
4813  dz=endplate_z-z;
4814  }
4815  }
4816  }
4817  else do_brent=true;
4818  if (do_brent){
4819  if (CORRECT_FOR_ELOSS){
4820  dedx=GetdEdx(S(state_q_over_p),
4821  forward_traj[k].K_rho_Z_over_A,
4822  forward_traj[k].rho_Z_over_A,
4823  forward_traj[k].LnI,forward_traj[k].Z);
4824  }
4825 
4826  // We have bracketed the minimum doca: use Brent's agorithm
4827  if (BrentForward(z,dedx,z0w,origin,dir,S,dz)!=NOERROR){
4828  break_point_fdc_index=(3*num_fdc)/4;
4829  return MOMENTUM_OUT_OF_RANGE;
4830  }
4831 
4832  // Step the state and covariance through the field
4833  if (fabs(dz)>mStepSizeZ){
4834  my_dz=(dz>0?1.0:-1.)*mStepSizeZ;
4835  num_steps=int(fabs(dz/my_dz));
4836  dz3=dz-num_steps*my_dz;
4837 
4838  double my_z=z;
4839  for (int m=0;m<num_steps;m++){
4840  newz=my_z+my_dz;
4841 
4842  // Step current state by my_dz
4843  //Step(z,newz,dedx,S);
4844 
4845  // propagate error matrix to z-position of hit
4846  StepJacobian(my_z,newz,S0,dedx,J);
4847  C=J*C*J.Transpose();
4848  //C=C.SandwichMultiply(J);
4849 
4850  // Step reference trajectory by my_dz
4851  Step(my_z,newz,dedx,S0);
4852 
4853  my_z=newz;
4854  }
4855 
4856  newz=my_z+dz3;
4857 
4858  // Step current state by dz3
4859  //Step(my_z,newz,dedx,S);
4860 
4861  // propagate error matrix to z-position of hit
4862  StepJacobian(my_z,newz,S0,dedx,J);
4863  C=J*C*J.Transpose();
4864  //C=C.SandwichMultiply(J);
4865 
4866  // Step reference trajectory by dz3
4867  Step(my_z,newz,dedx,S0);
4868  }
4869  else{
4870  newz = z + dz;
4871 
4872  // Step current state by dz
4873  //Step(z,newz,dedx,S);
4874 
4875  // propagate error matrix to z-position of hit
4876  StepJacobian(z,newz,S0,dedx,J);
4877  C=J*C*J.Transpose();
4878  //C=C.SandwichMultiply(J);
4879 
4880  // Step reference trajectory by dz
4881  Step(z,newz,dedx,S0);
4882  }
4883  }
4884 
4885  // Wire position at current z
4886  wirepos=origin;
4887  wirepos+=(newz-z0w)*dir;
4888 
4889  double xw=wirepos.X();
4890  double yw=wirepos.Y();
4891 
4892  // predicted doca taking into account the orientation of the wire
4893  if (do_brent==false){
4894  // In this case we did not have to swim to find the doca and
4895  // the transformation from the state vector space to the
4896  // measurement space is straight-forward.
4897  dy=S(state_y)+S(state_ty)*dz-yw;
4898  dx=S(state_x)+S(state_tx)*dz-xw;
4899  }
4900  else{
4901  // In this case we swam the state vector to the position of
4902  // the doca
4903  dy=S(state_y)-yw;
4904  dx=S(state_x)-xw;
4905  }
4906  double cosstereo=my_cdchits[cdc_index]->cosstereo;
4907  double d=sqrt(dx*dx+dy*dy)*cosstereo+EPS;
4908 
4909  // Track projection
4910  double cosstereo2_over_d=cosstereo*cosstereo/d;
4911  Hc_T(state_x)=dx*cosstereo2_over_d;
4912  Hc(state_x)=Hc_T(state_x);
4913  Hc_T(state_y)=dy*cosstereo2_over_d;
4914  Hc(state_y)=Hc_T(state_y);
4915  if (do_brent==false){
4916  Hc_T(state_ty)=Hc_T(state_y)*dz;
4917  Hc(state_ty)=Hc_T(state_ty);
4918  Hc_T(state_tx)=Hc_T(state_x)*dz;
4919  Hc(state_tx)=Hc_T(state_tx);
4920  }
4921  else{
4922  Hc_T(state_ty)=0.;
4923  Hc(state_ty)=0.;
4924  Hc_T(state_tx)=0.;
4925  Hc(state_tx)=0.;
4926  }
4927 
4928  //H.Print();
4929 
4930  // The next measurement
4931  double dm=0.39,tdrift=0.,tcorr=0.,dDdt0=0.;
4932  if (fit_type==kTimeBased)
4933  {
4934  // Find offset of wire with respect to the center of the
4935  // straw at this z position
4936  const DCDCWire *mywire=my_cdchits[cdc_index]->hit->wire;
4937  int ring_index=mywire->ring-1;
4938  int straw_index=mywire->straw-1;
4939  double dz=newz-z0w;
4940  double phi_d=atan2(dy,dx);
4941  double delta
4942  =max_sag[ring_index][straw_index]*(1.-dz*dz/5625.)
4943  *cos(phi_d +