Bug Summary

File:libraries/TRACKING/DTrackFitterStraightTrack.cc
Location:line 2141, column 7
Description:Value stored to 'z' is never read

Annotated Source Code

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