File: | /home/sdobbs/work/clang/halld_recon/src/libraries/TRACKING/DTrackFitterStraightTrack.cc |
Warning: | line 2204, column 12 Value stored to 't' during its initialization is never read |
Press '?' to see keyboard shortcuts
Keyboard shortcuts:
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 | //--------------------------------- |
13 | DTrackFitterStraightTrack::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(); |
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 | //--------------------------------- |
201 | DTrackFitterStraightTrack::~DTrackFitterStraightTrack() |
202 | { |
203 | |
204 | } |
205 | |
206 | //---------------------------------------------------- |
207 | // Comparison routines for sorting |
208 | //---------------------------------------------------- |
209 | bool DTrackFitterStraightTrack_cdc_hit_cmp(const DCDCTrackHit *a, |
210 | const DCDCTrackHit *b){ |
211 | |
212 | return(a->wire->origin.Y()>b->wire->origin.Y()); |
213 | } |
214 | |
215 | bool 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 | |
221 | bool 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 | |
236 | bool 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 |
247 | inline 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. |
254 | void 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 | |
263 | d=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 |
351 | jerror_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 |
598 | jerror_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 |
791 | jerror_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 |
862 | DTrackFitter::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 | if (fit_type==kWireBased){ |
875 | C(state_x,state_x)=C(state_y,state_y)=4.0; |
876 | C(state_tx,state_tx)=C(state_ty,state_ty)=1.0; |
877 | } |
878 | else{ |
879 | C(state_x,state_x)=C(state_y,state_y)=1.; |
880 | C(state_tx,state_tx)=C(state_ty,state_ty)=0.01; |
881 | } |
882 | |
883 | // Starting z-position and time |
884 | double z=input_pos.z(); |
885 | double t0=input_params.t0(); |
886 | // Variance in start time |
887 | switch(input_params.t0_detector()){ |
888 | case SYS_TOF: |
889 | mVarT0=0.01; |
890 | break; |
891 | case SYS_CDC: |
892 | mVarT0=25.; |
893 | break; |
894 | case SYS_FDC: |
895 | mVarT0=25.; |
896 | break; |
897 | case SYS_BCAL: |
898 | mVarT0=0.25; |
899 | break; |
900 | case SYS_START: |
901 | mVarT0=0.09; |
902 | break; |
903 | default: |
904 | mVarT0=0.; |
905 | break; |
906 | } |
907 | |
908 | // Chisq and ndof |
909 | chisq=1e16; |
910 | Ndof=0; |
911 | |
912 | // Sort the CDC hits by radius or y (for cosmics) |
913 | if (cdchits.size()>0){ |
914 | if (COSMICS){ |
915 | stable_sort(cdchits.begin(),cdchits.end(),DTrackFitterStraightTrack_cdc_hit_cmp); |
916 | } |
917 | else{ |
918 | stable_sort(cdchits.begin(),cdchits.end(),DTrackFitterStraightTrack_cdc_hit_radius_cmp); |
919 | } |
920 | } |
921 | |
922 | if (fdchits.size()>0){ |
923 | // Sort FDC hits by z |
924 | stable_sort(fdchits.begin(),fdchits.end(),DTrackFitterStraightTrack_fdc_hit_cmp); |
925 | status=FitForwardTrack(t0,z,S,C,chisq,Ndof); |
926 | } |
927 | else if (cdchits.size()>0){ |
928 | double dzsign=(pz>0)?1.:-1.; |
929 | status=FitCentralTrack(z,t0,dzsign,S,C,chisq,Ndof); |
930 | } |
931 | |
932 | if (status==DTrackFitter::kFitSuccess){ |
933 | // Output fit results |
934 | double tx=S(state_tx),ty=S(state_ty); |
935 | double phi=atan2(ty,tx); |
936 | double tanl=1./sqrt(tx*tx+ty*ty); |
937 | // Check for tracks heading upstream |
938 | if (cdchits_used_in_fit.size()>0){ |
939 | double phi_diff=phi-cdchits_used_in_fit[0]->wire->origin.Phi()-M_PI3.14159265358979323846; |
940 | if (phi_diff<-M_PI3.14159265358979323846) phi_diff+=2.*M_PI3.14159265358979323846; |
941 | if (phi_diff> M_PI3.14159265358979323846) phi_diff-=2.*M_PI3.14159265358979323846; |
942 | if (fabs(phi_diff)<M_PI_21.57079632679489661923){ |
943 | tanl*=-1; |
944 | phi+=M_PI3.14159265358979323846; |
945 | } |
946 | } |
947 | double pt=5.0*cos(atan(tanl)); // arbitrary magnitude... |
948 | DVector3 mom(pt*cos(phi),pt*sin(phi),pt*tanl); |
949 | |
950 | // Convert 4x4 covariance matrix to a TMatrixFSym for output |
951 | TMatrixFSym errMatrix(5); |
952 | for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){ |
953 | for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){ |
954 | errMatrix(loc_i, loc_j) = C(loc_i, loc_j); |
955 | } |
956 | } |
957 | errMatrix(4,4)=1.; |
958 | |
959 | // Add 7x7 covariance matrix to output |
960 | double sign=(mom.Theta()>M_PI_21.57079632679489661923)?-1.:1.; |
961 | fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix,S,sign)); |
962 | |
963 | DVector3 pos; |
964 | if (COSMICS==false){ |
965 | DVector3 beamdir(0.,0.,1.); |
966 | DVector3 beampos(0.,0.,65.); |
967 | finder->FindDoca(z,S,beamdir,beampos,&pos); |
968 | |
969 | // Jacobian matrix |
970 | double dz=pos.z()-z; |
971 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
972 | J(state_x,state_tx)=dz; |
973 | J(state_y,state_ty)=dz; |
974 | // Transform the matrix to the position of the doca |
975 | C=J*C*J.Transpose(); |
976 | } |
977 | else{ |
978 | pos.SetXYZ(S(state_x),S(state_y),z); |
979 | } |
980 | |
981 | fit_params.setForwardParmFlag(true); |
982 | |
983 | fit_params.setPosition(pos); |
984 | fit_params.setMomentum(mom); |
985 | |
986 | // Add tracking covariance matrix to output |
987 | auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource(); |
988 | locTrackingCovarianceMatrix->ResizeTo(5, 5); |
989 | locTrackingCovarianceMatrix->Zero(); |
990 | for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){ |
991 | for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){ |
992 | (*locTrackingCovarianceMatrix)(loc_i, loc_j) = C(loc_i, loc_j); |
993 | } |
994 | } |
995 | (*locTrackingCovarianceMatrix)(4,4)=1.; |
996 | fit_params.setTrackingErrorMatrix(locTrackingCovarianceMatrix); |
997 | |
998 | // Get extrapolations to other detectors |
999 | GetExtrapolations(pos,mom); |
1000 | |
1001 | } |
1002 | |
1003 | return status; |
1004 | } |
1005 | |
1006 | // Locate a position in vector xx given x |
1007 | unsigned int DTrackFitterStraightTrack::Locate(const vector<double>&xx,double x) const{ |
1008 | int n=xx.size(); |
1009 | if (x==xx[0]) return 0; |
1010 | else if (x==xx[n-1]) return n-2; |
1011 | |
1012 | int jl=-1; |
1013 | int ju=n; |
1014 | int ascnd=(xx[n-1]>=xx[0]); |
1015 | while(ju-jl>1){ |
1016 | int jm=(ju+jl)>>1; |
1017 | if ( (x>=xx[jm])==ascnd) |
1018 | jl=jm; |
1019 | else |
1020 | ju=jm; |
1021 | } |
1022 | return jl; |
1023 | } |
1024 | |
1025 | // Steering routine for fitting CDC-only tracks |
1026 | DTrackFitter::fit_status_t DTrackFitterStraightTrack::FitCentralTrack(double &z0,double t0, |
1027 | double dzsign, |
1028 | DMatrix4x1 &Sbest, |
1029 | DMatrix4x4 &Cbest, |
1030 | double &chi2_best, |
1031 | int &ndof_best){ |
1032 | // State vector and covariance matrix |
1033 | DMatrix4x1 S(Sbest); |
1034 | DMatrix4x4 C(Cbest),C0(Cbest); |
1035 | |
1036 | double chi2=chi2_best; |
1037 | int ndof=ndof_best; |
1038 | |
1039 | unsigned int numhits=cdchits.size(); |
1040 | unsigned int maxindex=numhits-1; |
1041 | |
1042 | // vectors of indexes to cdc hits used in the fit |
1043 | vector<int> used_cdc_hits(numhits); |
1044 | vector<int> used_cdc_hits_best_fit(numhits); |
1045 | |
1046 | // vectors of residual information |
1047 | vector<cdc_update_t>updates(numhits); |
1048 | vector<cdc_update_t>best_updates(numhits); |
1049 | |
1050 | // Rest deque for "best" trajectory |
1051 | best_trajectory.clear(); |
1052 | |
1053 | // Perform the fit |
1054 | int iter=0; |
1055 | for(iter=0;iter<20;iter++){ |
1056 | if (VERBOSE) jout << " Performing Pass iter " << iter << endl; |
1057 | |
1058 | trajectory.clear(); |
1059 | if (SetReferenceTrajectory(t0,z0,S,cdchits[maxindex],dzsign) |
1060 | !=NOERROR) break; |
1061 | |
1062 | if (VERBOSE) jout << " Reference Trajectory Set " << endl; |
1063 | C=C0; |
1064 | if (KalmanFilter(S,C,used_cdc_hits,updates,chi2,ndof,iter)!=NOERROR) break; |
1065 | if (VERBOSE) jout << " Filter returns NOERROR" << endl; |
1066 | if (iter>0 && (fabs(chi2_best-chi2)<0.1 || chi2>chi2_best)) break; |
1067 | |
1068 | // Save the current state and covariance matrixes |
1069 | Cbest=C; |
1070 | Sbest=S; |
1071 | |
1072 | // Save the current used hit and trajectory information |
1073 | best_trajectory.assign(trajectory.begin(),trajectory.end()); |
1074 | used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end()); |
1075 | best_updates.assign(updates.begin(),updates.end()); |
1076 | |
1077 | chi2_best=chi2; |
1078 | ndof_best=ndof; |
1079 | } |
1080 | if (iter==0) return DTrackFitter::kFitFailed; |
1081 | |
1082 | //Final z position (closest to beam line) |
1083 | z0=best_trajectory[best_trajectory.size()-1].z; |
1084 | |
1085 | //Run the smoother |
1086 | if (Smooth(best_updates) == NOERROR) IsSmoothed=true; |
1087 | |
1088 | // output list of cdc hits used in the fit |
1089 | cdchits_used_in_fit.clear(); |
1090 | for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){ |
1091 | if (used_cdc_hits_best_fit[m]){ |
1092 | cdchits_used_in_fit.push_back(cdchits[m]); |
1093 | } |
1094 | } |
1095 | |
1096 | return DTrackFitter::kFitSuccess; |
1097 | } |
1098 | |
1099 | |
1100 | //---------------------------------------------------- |
1101 | // FDC fitting routines |
1102 | //---------------------------------------------------- |
1103 | |
1104 | // parametrization of time-to-distance for FDC |
1105 | double DTrackFitterStraightTrack::fdc_drift_distance(double time) const { |
1106 | if (time<0.) return 0.; |
1107 | double d=0.; |
1108 | double tsq=time*time; |
1109 | double t_high=DRIFT_FUNC_PARMS[4]; |
1110 | |
1111 | if (time<t_high){ |
1112 | d=DRIFT_FUNC_PARMS[0]*sqrt(time)+DRIFT_FUNC_PARMS[1]*time |
1113 | +DRIFT_FUNC_PARMS[2]*tsq+DRIFT_FUNC_PARMS[3]*tsq*time; |
1114 | } |
1115 | else{ |
1116 | double t_high_sq=t_high*t_high; |
1117 | d=DRIFT_FUNC_PARMS[0]*sqrt(t_high)+DRIFT_FUNC_PARMS[1]*t_high |
1118 | +DRIFT_FUNC_PARMS[2]*t_high_sq+DRIFT_FUNC_PARMS[3]*t_high_sq*t_high; |
1119 | d+=DRIFT_FUNC_PARMS[5]*(time-t_high); |
1120 | } |
1121 | |
1122 | return d; |
1123 | |
1124 | } |
1125 | |
1126 | // Crude approximation for the variance in drift distance due to smearing |
1127 | double DTrackFitterStraightTrack::fdc_drift_variance(double t) const{ |
1128 | //return FDC_ANODE_VARIANCE; |
1129 | if (t<5.) t=5.; |
1130 | double sigma=DRIFT_RES_PARMS[0]/(t+1.)+DRIFT_RES_PARMS[1]+DRIFT_RES_PARMS[2]*t*t; |
1131 | |
1132 | return sigma*sigma; |
1133 | } |
1134 | |
1135 | // Steering routine for the kalman filter |
1136 | DTrackFitter::fit_status_t |
1137 | DTrackFitterStraightTrack::FitForwardTrack(double t0,double &start_z, |
1138 | DMatrix4x1 &Sbest,DMatrix4x4 &Cbest,double &chi2_best,int &ndof_best){ |
1139 | // State vector and covariance matrix |
1140 | DMatrix4x1 S(Sbest); |
1141 | DMatrix4x4 C(Cbest),C0(Cbest); |
1142 | |
1143 | // vectors of indexes to fdc hits used in the fit |
1144 | unsigned int numfdchits=fdchits.size(); |
1145 | vector<int> used_fdc_hits(numfdchits); |
1146 | vector<int> used_fdc_hits_best_fit(numfdchits); |
1147 | // vectors of indexes to cdc hits used in the fit |
1148 | unsigned int numcdchits=cdchits.size(); |
1149 | vector<int> used_cdc_hits(numcdchits); |
1150 | vector<int> used_cdc_hits_best_fit(numcdchits); |
1151 | |
1152 | // vectors of residual information |
1153 | vector<fdc_update_t>updates(numfdchits); |
1154 | vector<fdc_update_t>best_updates(numfdchits); |
1155 | vector<cdc_update_t>cdc_updates(numcdchits); |
1156 | vector<cdc_update_t>best_cdc_updates(numcdchits); |
1157 | |
1158 | vector<const DCDCTrackHit *> matchedCDCHits; |
1159 | |
1160 | // Chi-squared and degrees of freedom |
1161 | double chi2=chi2_best; |
1162 | int ndof=ndof_best; |
1163 | |
1164 | // Rest deque for "best" trajectory |
1165 | best_trajectory.clear(); |
1166 | |
1167 | unsigned iter=0; |
1168 | // First pass |
1169 | for(iter=0;iter<20;iter++){ |
1170 | trajectory.clear(); |
1171 | if (SetReferenceTrajectory(t0,start_z,S)!=NOERROR) break; |
1172 | |
1173 | C=C0; |
1174 | if (KalmanFilter(S,C,used_fdc_hits,used_cdc_hits,updates,cdc_updates, |
1175 | chi2,ndof)!=NOERROR) break; |
1176 | |
1177 | // printf(" == iter %d =====chi2 %f ndof %d \n",iter,chi2,ndof); |
1178 | if (iter>0 && (chi2>chi2_best || fabs(chi2_best-chi2)<0.1)) break; |
1179 | |
1180 | // Save the current state and covariance matrixes |
1181 | Cbest=C; |
1182 | Sbest=S; |
1183 | |
1184 | // Save the current used hit and trajectory information |
1185 | best_trajectory.assign(trajectory.begin(),trajectory.end()); |
1186 | used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end()); |
1187 | used_fdc_hits_best_fit.assign(used_fdc_hits.begin(),used_fdc_hits.end()); |
1188 | best_updates.assign(updates.begin(),updates.end()); |
1189 | best_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end()); |
1190 | |
1191 | chi2_best=chi2; |
1192 | ndof_best=ndof; |
1193 | } |
1194 | if (iter==0) return DTrackFitter::kFitFailed; |
1195 | |
1196 | //Final z position (closest to beam line) |
1197 | start_z=best_trajectory[best_trajectory.size()-1].z; |
1198 | |
1199 | //Run the smoother |
1200 | if (Smooth(best_updates,best_cdc_updates) == NOERROR) IsSmoothed=true; |
1201 | |
1202 | // output list of hits used in the fit |
1203 | cdchits_used_in_fit.clear(); |
1204 | for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){ |
1205 | if (used_cdc_hits_best_fit[m]){ |
1206 | cdchits_used_in_fit.push_back(cdchits[m]); |
1207 | } |
1208 | } |
1209 | fdchits_used_in_fit.clear(); |
1210 | for (unsigned int m=0;m<used_fdc_hits_best_fit.size();m++){ |
1211 | if (used_fdc_hits_best_fit[m]){ |
1212 | fdchits_used_in_fit.push_back(fdchits[m]); |
1213 | } |
1214 | } |
1215 | |
1216 | |
1217 | return DTrackFitter::kFitSuccess; |
1218 | } |
1219 | |
1220 | |
1221 | // Reference trajectory for the track |
1222 | jerror_t |
1223 | DTrackFitterStraightTrack::SetReferenceTrajectory(double t0,double z, |
1224 | DMatrix4x1 &S){ |
1225 | const double EPS=1e-3; |
1226 | |
1227 | // Jacobian matrix |
1228 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
1229 | |
1230 | double dz=1.1; |
1231 | double t=t0; |
1232 | trajectory.push_front(trajectory_t(z,t,S,J,DMatrix4x1(),DMatrix4x4())); |
1233 | |
1234 | double zhit=z; |
1235 | double old_zhit=z; |
1236 | for (unsigned int i=0;i<fdchits.size();i++){ |
1237 | zhit=fdchits[i]->wire->origin.z(); |
1238 | dz=1.1; |
1239 | |
1240 | if (fabs(zhit-old_zhit)<EPS && i>0){ |
1241 | trajectory[0].numhits++; |
1242 | continue; |
1243 | } |
1244 | // propagate until we would step beyond the FDC hit plane |
1245 | bool done=false; |
1246 | while (!done){ |
1247 | double new_z=z+dz; |
1248 | |
1249 | if (new_z>zhit){ |
1250 | dz=zhit-z; |
1251 | new_z=zhit; |
1252 | done=true; |
1253 | } |
1254 | J(state_x,state_tx)=-dz; |
1255 | J(state_y,state_ty)=-dz; |
1256 | // Flight time: assume particle is moving at the speed of light |
1257 | t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98; |
1258 | //propagate the state to the next z position |
1259 | S(state_x)+=S(state_tx)*dz; |
1260 | S(state_y)+=S(state_ty)*dz; |
1261 | |
1262 | trajectory.push_front(trajectory_t(new_z,t,S,J,DMatrix4x1(), |
1263 | DMatrix4x4())); |
1264 | if (done){ |
1265 | trajectory[0].id=i+1; |
1266 | trajectory[0].numhits=1; |
1267 | } |
1268 | |
1269 | z=new_z; |
1270 | } |
1271 | old_zhit=zhit; |
1272 | } |
1273 | // One last step |
1274 | dz=1.1; |
1275 | J(state_x,state_tx)=-dz; |
1276 | J(state_y,state_ty)=-dz; |
1277 | |
1278 | // Flight time: assume particle is moving at the speed of light |
1279 | t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98; |
1280 | |
1281 | //propagate the state to the next z position |
1282 | S(state_x)+=S(state_tx)*dz; |
1283 | S(state_y)+=S(state_ty)*dz; |
1284 | trajectory.push_front(trajectory_t(z+dz,t,S,J,DMatrix4x1(),DMatrix4x4())); |
1285 | |
1286 | if (false) |
1287 | { |
1288 | printf("Trajectory:\n"); |
1289 | for (unsigned int i=0;i<trajectory.size();i++){ |
1290 | printf(" x %f y %f z %f first hit %d num in layer %d\n",trajectory[i].S(state_x), |
1291 | trajectory[i].S(state_y),trajectory[i].z,trajectory[i].id, |
1292 | trajectory[i].numhits); |
1293 | } |
1294 | } |
1295 | |
1296 | return NOERROR; |
1297 | } |
1298 | |
1299 | // Perform Kalman Filter for the current trajectory |
1300 | jerror_t DTrackFitterStraightTrack::KalmanFilter(DMatrix4x1 &S,DMatrix4x4 &C, |
1301 | vector<int>&used_fdc_hits, |
1302 | vector<int>&used_cdc_hits, |
1303 | vector<fdc_update_t>&updates, |
1304 | vector<cdc_update_t>&cdc_updates, |
1305 | double &chi2,int &ndof){ |
1306 | DMatrix2x4 H; // Track projection matrix |
1307 | DMatrix4x2 H_T; // Transpose of track projection matrix |
1308 | DMatrix4x2 K; // Kalman gain matrix |
1309 | double VarMs=0.001; // kludge for material |
1310 | DMatrix2x2 V(0.0833,0.,0.,0.000256+VarMs); // Measurement variance |
1311 | DMatrix2x2 Vtemp,InvV; |
1312 | DMatrix2x1 Mdiff; |
1313 | DMatrix4x4 I; // identity matrix |
1314 | DMatrix4x4 J; // Jacobian matrix |
1315 | DMatrix4x1 S0; // State vector from reference trajectory |
1316 | |
1317 | DMatrix1x4 H_CDC; // Track projection matrix |
1318 | DMatrix4x1 H_T_CDC; // Transpose of track projection matrix |
1319 | DMatrix4x1 K_CDC; // Kalman gain matrix |
1320 | double V_CDC; |
1321 | |
1322 | const double d_EPS=1e-8; |
1323 | |
1324 | // Zero out the vectors of used hit flags |
1325 | for (unsigned int i=0;i<used_fdc_hits.size();i++) used_fdc_hits[i]=0; |
1326 | for (unsigned int i=0;i<used_cdc_hits.size();i++) used_cdc_hits[i]=0; |
1327 | |
1328 | //Initialize chi2 and ndof |
1329 | chi2=0.; |
1330 | ndof=0; |
1331 | |
1332 | // Save the starting values for C and S in the deque |
1333 | trajectory[0].Skk=S; |
1334 | trajectory[0].Ckk=C; |
1335 | |
1336 | // Loop over all steps in the trajectory |
1337 | S0=trajectory[0].S; |
1338 | J=trajectory[0].J; |
1339 | |
1340 | // CDC index and wire position variables |
1341 | bool more_hits = cdchits.size() == 0 ? false: true; |
1342 | bool firstCDCStep=true; |
1343 | unsigned int cdc_index=0; |
1344 | const DCDCWire *wire; |
1345 | DVector3 origin,wdir,wirepos; |
1346 | double doca2=0.0, old_doca2=0.0; |
1347 | if(more_hits){ |
1348 | cdc_index=cdchits.size()-1; |
1349 | wire=cdchits[cdc_index]->wire; |
1350 | origin=wire->origin; |
1351 | double vz=wire->udir.z(); |
1352 | if (VERBOSE) jout << " Additional CDC Hits in FDC track Starting in Ring " << wire->ring << endl; |
1353 | wdir=(1./vz)*wire->udir; |
1354 | } |
1355 | |
1356 | for (unsigned int k=1;k<trajectory.size();k++){ |
1357 | if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.) |
1358 | return UNRECOVERABLE_ERROR; |
1359 | |
1360 | // Propagate the state and covariance matrix forward in z |
1361 | S=trajectory[k].S+J*(S-S0); |
1362 | C=J*C*J.Transpose(); |
1363 | |
1364 | // Save the current state and covariance matrix in the deque |
1365 | trajectory[k].Skk=S; |
1366 | trajectory[k].Ckk=C; |
1367 | |
1368 | // Save S and J for the next step |
1369 | S0=trajectory[k].S; |
1370 | J=trajectory[k].J; |
1371 | |
1372 | // Correct S and C for the hit |
1373 | if (trajectory[k].id>0){ |
1374 | unsigned int id=trajectory[k].id-1; |
1375 | |
1376 | double cospsi=cos(fdchits[id]->wire->angle); |
1377 | double sinpsi=sin(fdchits[id]->wire->angle); |
1378 | |
1379 | // Small angle alignment correction |
1380 | double x = S(state_x) + fdchits[id]->wire->angles.Z()*S(state_y); |
1381 | double y = S(state_y) - fdchits[id]->wire->angles.Z()*S(state_x); |
1382 | //tz = 1. + my_fdchits[id]->phiY*tx - my_fdchits[id]->phiX*ty; |
1383 | double tx = (S(state_tx) + fdchits[id]->wire->angles.Z()*S(state_ty) - fdchits[id]->wire->angles.Y()); |
1384 | double ty = (S(state_ty) - fdchits[id]->wire->angles.Z()*S(state_tx) + fdchits[id]->wire->angles.X()); |
1385 | |
1386 | if (std::isnan(x) || std::isnan(y)) return UNRECOVERABLE_ERROR; |
1387 | |
1388 | // x,y and tx,ty in local coordinate system |
1389 | // To transform from (x,y) to (u,v), need to do a rotation: |
1390 | // u = x*cos(psi)-y*sin(psi) |
1391 | // v = y*cos(psi)+x*sin(psi) |
1392 | // (without alignment offsets) |
1393 | double vpred_wire_plane=y*cospsi+x*sinpsi; |
1394 | double upred_wire_plane=x*cospsi-y*sinpsi; |
1395 | double tu=tx*cospsi-ty*sinpsi; |
1396 | double tv=tx*sinpsi+ty*cospsi; |
1397 | |
1398 | // Variables for angle of incidence with respect to the z-direction in |
1399 | // the u-z plane |
1400 | double alpha=atan(tu); |
1401 | double cosalpha=cos(alpha); |
1402 | double cos2_alpha=cosalpha*cosalpha; |
1403 | double sinalpha=sin(alpha); |
1404 | double sin2_alpha=sinalpha*sinalpha; |
1405 | double cos2_alpha_minus_sin2_alpha=cos2_alpha-sin2_alpha; |
1406 | |
1407 | // Difference between measurement and projection |
1408 | for (int m=trajectory[k].numhits-1;m>=0;m--){ |
1409 | unsigned int my_id=id+m; |
1410 | double uwire=fdchits[my_id]->w; |
1411 | // (signed) distance of closest approach to wire |
1412 | double du=upred_wire_plane-uwire; |
1413 | double doca=du*cosalpha; |
1414 | |
1415 | // Predicted avalanche position along the wire |
1416 | double vpred=vpred_wire_plane; |
1417 | |
1418 | // Measured position of hit along wire |
1419 | double v=fdchits[my_id]->s; |
1420 | |
1421 | // Difference between measurements and predictions |
1422 | double drift=0.; // assume hit at wire position |
1423 | if (fit_type==kTimeBased){ |
1424 | double drift_time=fdchits[my_id]->time-trajectory[k].t; |
1425 | drift=(du>0.0?1.:-1.)*fdc_drift_distance(drift_time); |
1426 | V(0,0)=fdc_drift_variance(drift_time)+VarMs; |
1427 | } |
1428 | Mdiff(0)=drift-doca; |
1429 | Mdiff(1)=v-vpred; |
1430 | |
1431 | // Matrix for transforming from state-vector space to measurement space |
1432 | H_T(state_x,0)=cospsi*cosalpha; |
1433 | H_T(state_y,0)=-sinpsi*cosalpha; |
1434 | double temp=-du*sinalpha*cos2_alpha; |
1435 | H_T(state_tx,0)=cospsi*temp; |
1436 | H_T(state_ty,0)=-sinpsi*temp; |
1437 | double temp2=cosalpha*sinalpha*tv; |
1438 | H_T(state_x,1)=sinpsi-temp2*cospsi; |
1439 | H_T(state_y,1)=cospsi+temp2*sinpsi; |
1440 | double temp4=sinalpha*doca; |
1441 | double temp5=tv*cos2_alpha*du*cos2_alpha_minus_sin2_alpha; |
1442 | H_T(state_tx,1)=-sinpsi*temp4-cospsi*temp5; |
1443 | H_T(state_ty,1)=-cospsi*temp4+sinpsi*temp5; |
1444 | |
1445 | // Matrix transpose H_T -> H |
1446 | H(0,state_x)=H_T(state_x,0); |
1447 | H(0,state_y)=H_T(state_y,0); |
1448 | H(0,state_tx)=H_T(state_tx,0); |
1449 | H(0,state_ty)=H_T(state_ty,0); |
1450 | H(1,state_x)=H_T(state_x,1); |
1451 | H(1,state_y)=H_T(state_y,1); |
1452 | H(1,state_tx)=H_T(state_tx,1); |
1453 | H(1,state_ty)=H_T(state_ty,1); |
1454 | |
1455 | // Variance for this hit |
1456 | InvV=(V+H*C*H_T).Invert(); |
1457 | |
1458 | // Compute Kalman gain matrix |
1459 | K=(C*H_T)*InvV; |
1460 | |
1461 | if (fdchits[my_id]->wire->layer!=PLANE_TO_SKIP){ |
1462 | /* |
1463 | if(DEBUG_HISTS){ |
1464 | hFDCOccTrkFit->Fill(fdchits[my_id]->wire->layer); |
1465 | } |
1466 | */ |
1467 | // Update the state vector |
1468 | S+=K*Mdiff; |
1469 | if(VERBOSE) S.Print(); |
1470 | // Update state vector covariance matrix |
1471 | C=C-K*(H*C); |
1472 | |
1473 | // Update the filtered measurement covariane matrix and put results in |
1474 | // update vector |
1475 | DMatrix2x2 RC=V-H*C*H_T; |
1476 | DMatrix2x1 res=Mdiff-H*K*Mdiff; |
1477 | |
1478 | chi2+=RC.Chi2(res); |
1479 | ndof+=2; |
1480 | |
1481 | // fill pull vector entries |
1482 | updates[my_id].V=RC; |
1483 | } |
1484 | else{ |
1485 | updates[my_id].V=V; |
1486 | } |
1487 | |
1488 | used_fdc_hits[my_id]=1; |
1489 | |
1490 | // fill pull vector |
1491 | updates[my_id].d=doca; |
1492 | updates[my_id].S=S; |
1493 | updates[my_id].C=C; |
1494 | updates[my_id].tdrift=fdchits[my_id]->time-trajectory[k].t; |
1495 | updates[my_id].s=29.98*trajectory[k].t; // assume beta=1 |
1496 | } |
1497 | } |
1498 | |
1499 | if (more_hits && trajectory[k].z < downstreamEndplate){ |
1500 | // Position along wire |
1501 | double z0=origin.Z(); |
1502 | wirepos=origin+(trajectory[k].z-z0)*wdir; |
1503 | |
1504 | // New doca^2 |
1505 | double dx=S(state_x)-wirepos.X(); |
1506 | double dy=S(state_y)-wirepos.Y(); |
1507 | doca2=dx*dx+dy*dy; |
1508 | if (VERBOSE > 10) jout<< "At Position " << S(state_x) << " " << S(state_y) << " " << trajectory[k].z << " doca2 " << doca2 << endl; |
1509 | |
1510 | if (doca2>old_doca2 && more_hits && !firstCDCStep){ |
1511 | |
1512 | // zero-position and direction of line describing particle trajectory |
1513 | double tx=S(state_tx),ty=S(state_ty); |
1514 | DVector3 pos0(S(state_x),S(state_y),trajectory[k].z); |
1515 | DVector3 tdir(tx,ty,1.); |
1516 | |
1517 | // Find the true doca to the wire |
1518 | DVector3 diff=pos0-origin; |
1519 | double dx0=diff.x(),dy0=diff.y(); |
1520 | double wdir_dot_diff=diff.Dot(wdir); |
1521 | double tdir_dot_diff=diff.Dot(tdir); |
1522 | double tdir_dot_wdir=tdir.Dot(wdir); |
1523 | double tdir2=tdir.Mag2(); |
1524 | double wdir2=wdir.Mag2(); |
1525 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
1526 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
1527 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
1528 | double scale=1./D; |
1529 | double s=scale*N; |
1530 | double t=scale*N1; |
1531 | diff+=s*tdir-t*wdir; |
1532 | double d=diff.Mag()+d_EPS; // prevent division by zero |
1533 | |
1534 | // The next measurement |
1535 | double dmeas=0.39,delta=0.; |
1536 | double tdrift=cdchits[cdc_index]->tdrift-trajectory[k].t; |
1537 | if (fit_type==kTimeBased){ |
1538 | double phi_d=diff.Phi(); |
1539 | double dphi=phi_d-origin.Phi(); |
1540 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
1541 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
1542 | |
1543 | int ring_index=cdchits[cdc_index]->wire->ring-1; |
1544 | int straw_index=cdchits[cdc_index]->wire->straw-1; |
1545 | double dz=t*wdir.z(); |
1546 | double delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.) |
1547 | *cos(phi_d+sag_phi_offset[ring_index][straw_index]); |
1548 | CDCDriftParameters(dphi,delta,tdrift,dmeas,V_CDC); |
1549 | } |
1550 | |
1551 | // residual |
1552 | double res=dmeas-d; |
1553 | if (VERBOSE>5) jout << " Residual " << res << endl; |
1554 | // Track projection |
1555 | double one_over_d=1./d; |
1556 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
1557 | double wx=wdir.x(),wy=wdir.y(); |
1558 | |
1559 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
1560 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
1561 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
1562 | |
1563 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
1564 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
1565 | double dtdty=scale*(dN1dty-t*dDdty); |
1566 | |
1567 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
1568 | double dsdtx=scale*(dNdtx-s*dDdtx); |
1569 | |
1570 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
1571 | double dsdty=scale*(dNdty-s*dDdty); |
1572 | |
1573 | H_CDC(state_tx)=H_T_CDC(state_tx) |
1574 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
1575 | +diffz*(dsdtx-dtdtx)); |
1576 | H_CDC(state_ty)=H_T_CDC(state_ty) |
1577 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
1578 | +diffz*(dsdty-dtdty)); |
1579 | |
1580 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
1581 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
1582 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
1583 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
1584 | |
1585 | H_CDC(state_x)=H_T_CDC(state_x) |
1586 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
1587 | +diffz*(dsdx-dtdx)); |
1588 | H_CDC(state_y)=H_T_CDC(state_y) |
1589 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
1590 | +diffz*(dsdy-dtdy)); |
1591 | |
1592 | double InvV=1./(V_CDC+H_CDC*C*H_T_CDC); |
1593 | |
1594 | // Check how far this hit is from the projection |
1595 | double chi2check=res*res*InvV; |
1596 | if (chi2check < CHI2CUT || DO_PRUNING == 0){ |
1597 | if (VERBOSE) jout << "CDC Hit Added to FDC track " << endl; |
1598 | if (cdchits[cdc_index]->wire->ring!=RING_TO_SKIP){ |
1599 | |
1600 | // Compute Kalman gain matrix |
1601 | K_CDC=InvV*(C*H_T_CDC); |
1602 | // Update state vector covariance matrix |
1603 | DMatrix4x4 Ctest=C-K_CDC*(H_CDC*C); |
1604 | |
1605 | //C.Print(); |
1606 | //K.Print(); |
1607 | //Ctest.Print(); |
1608 | |
1609 | // Check that Ctest is positive definite |
1610 | if (!Ctest.IsPosDef()) return VALUE_OUT_OF_RANGE; |
1611 | C=Ctest; |
1612 | if(VERBOSE>10) C.Print(); |
1613 | // Update the state vector |
1614 | //S=S+res*K; |
1615 | S+=res*K_CDC; |
1616 | if(VERBOSE) {jout << "traj[z]=" << trajectory[k].z<< endl; S.Print();} |
1617 | |
1618 | // Compute new residual |
1619 | //d=finder->FindDoca(trajectory[k].z,S,wdir,origin); |
1620 | res=res-H_CDC*K_CDC*res; |
1621 | |
1622 | // Update chi2 |
1623 | double fit_V=V_CDC-H_CDC*C*H_T_CDC; |
1624 | chi2+=res*res/fit_V; |
1625 | ndof++; |
1626 | |
1627 | // fill pull vector entry |
1628 | cdc_updates[cdc_index].V=fit_V; |
1629 | } |
1630 | else { |
1631 | cdc_updates[cdc_index].V=V_CDC; |
1632 | } |
1633 | |
1634 | // fill updates |
1635 | cdc_updates[cdc_index].resi=res; |
1636 | cdc_updates[cdc_index].d=d; |
1637 | cdc_updates[cdc_index].delta=delta; |
1638 | cdc_updates[cdc_index].S=S; |
1639 | cdc_updates[cdc_index].C=C; |
1640 | cdc_updates[cdc_index].tdrift=tdrift; |
1641 | cdc_updates[cdc_index].ddrift=dmeas; |
1642 | cdc_updates[cdc_index].s=29.98*trajectory[k].t; // assume beta=1 |
1643 | trajectory[k].id=cdc_index+1000; |
1644 | |
1645 | used_cdc_hits[cdc_index]=1; |
1646 | } |
1647 | // move to next cdc hit |
1648 | if (cdc_index>0){ |
1649 | cdc_index--; |
1650 | |
1651 | //New wire position |
1652 | wire=cdchits[cdc_index]->wire; |
1653 | if (VERBOSE>5) jout << " Next Wire ring " << wire->ring << endl; |
1654 | origin=wire->origin; |
1655 | double vz=wire->udir.z(); |
1656 | wdir=(1./vz)*wire->udir; |
1657 | wirepos=origin+((trajectory[k].z-z0))*wdir; |
1658 | |
1659 | // New doca^2 |
1660 | dx=S(state_x)-wirepos.x(); |
1661 | dy=S(state_y)-wirepos.y(); |
1662 | doca2=dx*dx+dy*dy; |
1663 | |
1664 | } |
1665 | else more_hits=false; |
1666 | } |
1667 | firstCDCStep=false; |
1668 | old_doca2=doca2; |
1669 | } |
1670 | } |
1671 | |
1672 | ndof-=4; |
1673 | |
1674 | return NOERROR; |
1675 | } |
1676 | |
1677 | |
1678 | // Smoothing algorithm for the forward trajectory. Updates the state vector |
1679 | // at each step (going in the reverse direction to the filter) based on the |
1680 | // information from all the steps and outputs the state vector at the |
1681 | // outermost step. |
1682 | |
1683 | jerror_t |
1684 | DTrackFitterStraightTrack::Smooth(vector<fdc_update_t>&fdc_updates, |
1685 | vector<cdc_update_t>&cdc_updates){ |
1686 | unsigned int max=best_trajectory.size()-1; |
1687 | DMatrix4x1 S=(best_trajectory[max].Skk); |
1688 | DMatrix4x4 C=(best_trajectory[max].Ckk); |
1689 | DMatrix4x4 JT=best_trajectory[max].J.Transpose(); |
1690 | DMatrix4x1 Ss=S; |
1691 | DMatrix4x4 Cs=C; |
1692 | DMatrix4x4 A,dC; |
1693 | |
1694 | const double d_EPS=1e-8; |
1695 | |
1696 | for (unsigned int m=max-1;m>0;m--){ |
1697 | if (best_trajectory[m].id>0 && best_trajectory[m].id<1000){ // FDC Hit |
1698 | unsigned int id=best_trajectory[m].id-1; |
1699 | A=fdc_updates[id].C*JT*C.Invert(); |
1700 | Ss=fdc_updates[id].S+A*(Ss-S); |
1701 | |
1702 | dC=A*(Cs-C)*A.Transpose(); |
1703 | Cs=fdc_updates[id].C+dC; |
1704 | |
1705 | double cosa=cos(fdchits[id]->wire->angle); |
1706 | double cos2a=cos(2*fdchits[id]->wire->angle); |
1707 | double sina=sin(fdchits[id]->wire->angle); |
1708 | double u=fdchits[id]->w; |
1709 | double v=fdchits[id]->s; |
1710 | |
1711 | // Small angle alignment correction |
1712 | double x = Ss(state_x) + fdchits[id]->wire->angles.Z() * Ss(state_y); |
1713 | double y = Ss(state_y) - fdchits[id]->wire->angles.Z() * Ss(state_x); |
1714 | // tz = 1.0 + my_fdchits[id]->phiY * tx - my_fdchits[id]->phiX * ty; |
1715 | double tx = Ss(state_tx) + fdchits[id]->wire->angles.Z() * Ss(state_ty) - fdchits[id]->wire->angles.Y(); |
1716 | double ty = Ss(state_ty) - fdchits[id]->wire->angles.Z() * Ss(state_tx) + fdchits[id]->wire->angles.X(); |
1717 | |
1718 | // Projected position along the wire |
1719 | double vpred=x*sina+y*cosa; |
1720 | |
1721 | // Projected position in the plane of the wires transverse to the wires |
1722 | double upred=x*cosa-y*sina; |
1723 | |
1724 | // Direction tangent in the u-z plane |
1725 | double tu=tx*cosa-ty*sina; |
1726 | double alpha=atan(tu); |
1727 | double cosalpha=cos(alpha); |
1728 | //double cosalpha2=cosalpha*cosalpha; |
1729 | double sinalpha=sin(alpha); |
1730 | |
1731 | // (signed) distance of closest approach to wire |
1732 | double du=upred-u; |
1733 | double doca=du*cosalpha; |
1734 | // Difference between measurement and projection for the cathodes |
1735 | double tv=tx*sina+ty*cosa; |
1736 | double resi_c=v-vpred; |
1737 | |
1738 | // Difference between measurement and projection perpendicular to the wire |
1739 | double drift = 0.0; // assume hit at wire position |
1740 | int left_right = -999; |
1741 | double drift_time = fdc_updates[id].tdrift; |
1742 | if (fit_type == kTimeBased) { |
1743 | drift = (du > 0.0 ? 1.0 : -1.0) * fdc_drift_distance(drift_time); |
1744 | left_right = (du > 0.0 ? +1 : -1); |
1745 | } |
1746 | double resi_a = drift - doca; |
1747 | |
1748 | // Variance from filter step |
1749 | DMatrix2x2 V=fdc_updates[id].V; |
1750 | // Compute projection matrix and find the variance for the residual |
1751 | DMatrix4x2 H_T; |
1752 | double temp2=-tv*sinalpha; |
1753 | H_T(state_x,1)=sina+cosa*cosalpha*temp2; |
1754 | H_T(state_y,1)=cosa-sina*cosalpha*temp2; |
1755 | |
1756 | double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha; |
1757 | double doca_cosalpha=doca*cosalpha; |
1758 | H_T(state_tx,1)=-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2); |
1759 | H_T(state_ty,1)=-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2); |
1760 | |
1761 | H_T(state_x,0)=cosa*cosalpha; |
1762 | H_T(state_y,0)=-sina*cosalpha; |
1763 | double one_plus_tu2=1.+tu*tu; |
1764 | double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2; |
1765 | H_T(state_ty,0)=sina*factor; |
1766 | H_T(state_tx,0)=-cosa*factor; |
1767 | |
1768 | // Matrix transpose H_T -> H |
1769 | DMatrix2x4 H; |
1770 | H(0,state_x)=H_T(state_x,0); |
1771 | H(0,state_y)=H_T(state_y,0); |
1772 | H(0,state_tx)=H_T(state_tx,0); |
1773 | H(0,state_ty)=H_T(state_ty,0); |
1774 | H(1,state_x)=H_T(state_x,1); |
1775 | H(1,state_y)=H_T(state_y,1); |
1776 | H(1,state_tx)=H_T(state_tx,1); |
1777 | H(1,state_ty)=H_T(state_ty,1); |
1778 | |
1779 | if (fdchits[id]->wire->layer == PLANE_TO_SKIP) { |
1780 | // V += Cs.SandwichMultiply(H_T); |
1781 | V = V + H * Cs * H_T; |
1782 | } else { |
1783 | // V -= dC.SandwichMultiply(H_T); |
1784 | |
1785 | // R. Fruehwirth, Nucl. Instrum. Methods Phys. Res. A 262, 444 (1987). |
1786 | // Eq. (9) |
1787 | // The following V (lhs) corresponds to R^n_k in the paper. |
1788 | // dC corresponds to 'A_k * (C^n_{k+1} - C^k_{k+1}) * A_k^T' in the paper. |
1789 | V = V - H * dC * H_T; |
1790 | } |
1791 | |
1792 | /* |
1793 | if(DEBUG_HISTS){ |
1794 | hFDCOccTrkSmooth->Fill(fdchits[id]->wire->layer); |
1795 | } |
1796 | */ |
1797 | // Implement derivatives wrt track parameters needed for millepede alignment |
1798 | // Add the pull |
1799 | double scale=1./sqrt(1.+tx*tx+ty*ty); |
1800 | double cosThetaRel=fdchits[id]->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale)); |
1801 | DTrackFitter::pull_t thisPull(resi_a,sqrt(V(0,0)), |
1802 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
1803 | fdc_updates[id].tdrift, |
1804 | fdc_updates[id].d, |
1805 | NULL__null,fdchits[id], |
1806 | 0.0, //docaphi |
1807 | best_trajectory[m].z,cosThetaRel, |
1808 | 0.0, //tcorr |
1809 | resi_c, sqrt(V(1,1)) |
1810 | ); |
1811 | thisPull.left_right = left_right; |
1812 | |
1813 | if (fdchits[id]->wire->layer!=PLANE_TO_SKIP){ |
1814 | vector<double> derivatives; |
1815 | derivatives.resize(FDCTrackD::size); |
1816 | |
1817 | //dDOCAW/dDeltaX |
1818 | derivatives[FDCTrackD::dDOCAW_dDeltaX] = -(1/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
1819 | |
1820 | //dDOCAW/dDeltaZ |
1821 | derivatives[FDCTrackD::dDOCAW_dDeltaZ] = (tx*cosa - ty*sina)/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
1822 | |
1823 | //dDOCAW/ddeltaPhiX |
1824 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiX] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1825 | |
1826 | //dDOCAW/ddeltaphiY |
1827 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiY] = (cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1828 | |
1829 | //dDOCAW/ddeltaphiZ |
1830 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiZ] = (tx*ty*u*cos2a + (x + pow(ty,2)*x - tx*ty*y)*sina + |
1831 | cosa*(-(tx*ty*x) + y + pow(tx,2)*y + (pow(tx,2) - pow(ty,2))*u*sina))/ |
1832 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1833 | |
1834 | // dDOCAW/dx |
1835 | derivatives[FDCTrackD::dDOCAW_dx] = cosa/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
1836 | |
1837 | // dDOCAW/dy |
1838 | derivatives[FDCTrackD::dDOCAW_dy] = -(sina/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
1839 | |
1840 | // dDOCAW/dtx |
1841 | derivatives[FDCTrackD::dDOCAW_dtx] = -((cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5)); |
1842 | |
1843 | // dDOCAW/dty |
1844 | derivatives[FDCTrackD::dDOCAW_dty] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
1845 | |
1846 | // dDOCAW/dt0 |
1847 | double t0shift = 4.0; // ns |
1848 | double drift_shift = 0.0; |
1849 | if (drift_time < 0.0) { |
1850 | drift_shift = drift; |
1851 | } else { |
1852 | drift_shift = |
1853 | (du > 0.0 ? 1.0 : -1.0) * |
1854 | fdc_drift_distance(drift_time + t0shift); |
1855 | } |
1856 | derivatives[FDCTrackD::dW_dt0] = (drift_shift - drift) / t0shift; |
1857 | |
1858 | // And the cathodes |
1859 | //dDOCAW/ddeltax |
1860 | derivatives[FDCTrackD::dDOCAC_dDeltaX] = 0.; |
1861 | |
1862 | //dDOCAW/ddeltax |
1863 | derivatives[FDCTrackD::dDOCAC_dDeltaZ] = ty*cosa + tx*sina; |
1864 | |
1865 | //dDOCAW/ddeltaPhiX |
1866 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiX] = 0.; |
1867 | |
1868 | //dDOCAW/ddeltaPhiX |
1869 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiY] = 0.; |
1870 | |
1871 | //dDOCAW/ddeltaPhiX |
1872 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiZ] = -(x*cosa) + y*sina; |
1873 | |
1874 | // dDOCAW/dx |
1875 | derivatives[FDCTrackD::dDOCAC_dx] = sina; |
1876 | |
1877 | // dDOCAW/dy |
1878 | derivatives[FDCTrackD::dDOCAW_dy] = cosa; |
1879 | |
1880 | // dDOCAW/dtx |
1881 | derivatives[FDCTrackD::dDOCAW_dtx] = 0.; |
1882 | |
1883 | // dDOCAW/dty |
1884 | derivatives[FDCTrackD::dDOCAW_dty] = 0.; |
1885 | |
1886 | thisPull.AddTrackDerivatives(derivatives); |
1887 | } |
1888 | pulls.push_back(thisPull); |
1889 | } |
1890 | else if (best_trajectory[m].id>=1000){ // CDC Hit |
1891 | unsigned int id=best_trajectory[m].id-1000; |
1892 | A=cdc_updates[id].C*JT*C.Invert(); |
1893 | Ss=cdc_updates[id].S+A*(Ss-S); |
1894 | |
1895 | dC=A*(Cs-C)*A.Transpose(); |
1896 | Cs=cdc_updates[id].C+dC; |
1897 | if (VERBOSE > 10) { |
1898 | jout << " In Smoothing Step Using ID " << id << "/" << cdc_updates.size() << " for ring " << cdchits[id]->wire->ring << endl; |
1899 | jout << " A cdc_updates[id].C Ss Cs " << endl; |
1900 | A.Print(); cdc_updates[id].C.Print(); Ss.Print(); Cs.Print(); |
1901 | } |
1902 | if(!Cs.IsPosDef()) { |
1903 | if (VERBOSE) jout << "Cs is not PosDef!" << endl; |
1904 | return VALUE_OUT_OF_RANGE; |
1905 | } |
1906 | |
1907 | const DCDCWire *wire=cdchits[id]->wire; |
1908 | DVector3 origin=wire->origin; |
1909 | double z0=origin.z(); |
1910 | double vz=wire->udir.z(); |
1911 | DVector3 wdir=(1./vz)*wire->udir; |
1912 | DVector3 wirepos=origin+(best_trajectory[m].z-z0)*wdir; |
1913 | // Position and direction from state vector |
1914 | double x=Ss(state_x); |
1915 | double y=Ss(state_y); |
1916 | double tx=Ss(state_tx); |
1917 | double ty=Ss(state_ty); |
1918 | |
1919 | DVector3 pos0(x,y,best_trajectory[m].z); |
1920 | DVector3 tdir(tx,ty,1.); |
1921 | |
1922 | // Find the true doca to the wire |
1923 | DVector3 diff=pos0-origin; |
1924 | double dx0=diff.x(),dy0=diff.y(); |
1925 | double wdir_dot_diff=diff.Dot(wdir); |
1926 | double tdir_dot_diff=diff.Dot(tdir); |
1927 | double tdir_dot_wdir=tdir.Dot(wdir); |
1928 | double tdir2=tdir.Mag2(); |
1929 | double wdir2=wdir.Mag2(); |
1930 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
1931 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
1932 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
1933 | double scale=1./D; |
1934 | double s=scale*N; |
1935 | double t=scale*N1; |
1936 | diff+=s*tdir-t*wdir; |
1937 | double d=diff.Mag()+d_EPS; // prevent division by zero |
1938 | double ddrift = cdc_updates[id].ddrift; |
1939 | |
1940 | double resi = ddrift - d; |
1941 | |
1942 | |
1943 | // Track projection |
1944 | DMatrix1x4 H; DMatrix4x1 H_T; |
1945 | { |
1946 | double one_over_d=1./d; |
1947 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
1948 | double wx=wdir.x(),wy=wdir.y(); |
1949 | |
1950 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
1951 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
1952 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
1953 | |
1954 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
1955 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
1956 | double dtdty=scale*(dN1dty-t*dDdty); |
1957 | |
1958 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
1959 | double dsdtx=scale*(dNdtx-s*dDdtx); |
1960 | |
1961 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
1962 | double dsdty=scale*(dNdty-s*dDdty); |
1963 | |
1964 | H(state_tx)=H_T(state_tx) |
1965 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
1966 | +diffz*(dsdtx-dtdtx)); |
1967 | H(state_ty)=H_T(state_ty) |
1968 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
1969 | +diffz*(dsdty-dtdty)); |
1970 | |
1971 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
1972 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
1973 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
1974 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
1975 | |
1976 | H(state_x)=H_T(state_x) |
1977 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
1978 | +diffz*(dsdx-dtdx)); |
1979 | H(state_y)=H_T(state_y) |
1980 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
1981 | +diffz*(dsdy-dtdy)); |
1982 | } |
1983 | double V=cdc_updates[id].V; |
1984 | |
1985 | if (VERBOSE > 10) jout << " d " << d << " H*S " << H*S << endl; |
1986 | if (cdchits[id]->wire->ring==RING_TO_SKIP){ |
1987 | V=V+H*Cs*H_T; |
1988 | } |
1989 | else{ |
1990 | V=V-H*Cs*H_T; |
1991 | } |
1992 | if (V<0) return VALUE_OUT_OF_RANGE; |
1993 | |
1994 | // Add the pull |
1995 | double myscale=1./sqrt(1.+tx*tx+ty*ty); |
1996 | double cosThetaRel=wire->udir.Dot(DVector3(myscale*tx,myscale*ty,myscale)); |
1997 | DTrackFitter::pull_t thisPull(resi,sqrt(V), |
1998 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
1999 | cdc_updates[id].tdrift, |
2000 | d,cdchits[id], NULL__null, |
2001 | diff.Phi(), //docaphi |
2002 | best_trajectory[m].z,cosThetaRel, |
2003 | cdc_updates[id].tdrift); |
2004 | |
2005 | // Derivatives for alignment |
2006 | double wtx=wire->udir.X(), wty=wire->udir.Y(), wtz=wire->udir.Z(); |
2007 | double wx=wire->origin.X(), wy=wire->origin.Y(), wz=wire->origin.Z(); |
2008 | |
2009 | double z=best_trajectory[m].z; |
2010 | double tx2=tx*tx, ty2=ty*ty; |
2011 | double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz; |
2012 | double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz)+d_EPS; |
2013 | double denom2=denom*denom; |
2014 | double c1=-(wtx - tx*wtz)*(wy - y); |
2015 | double c2=wty*(wx - tx*wz - x + tx*z); |
2016 | double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z); |
2017 | double dscale=0.5*(1/d); |
2018 | |
2019 | vector<double> derivatives(11); |
2020 | |
2021 | derivatives[CDCTrackD::dDOCAdOriginX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3))/denom; |
2022 | |
2023 | derivatives[CDCTrackD::dDOCAdOriginY]=dscale*(2*(-wtx + tx*wtz)*(c1 + c2 + c3))/denom; |
2024 | |
2025 | derivatives[CDCTrackD::dDOCAdOriginZ]=dscale*(2*(ty*wtx - tx*wty)*(c1 + c2 + c3))/denom; |
2026 | |
2027 | derivatives[CDCTrackD::dDOCAdDirX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3)* |
2028 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
2029 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
2030 | |
2031 | derivatives[CDCTrackD::dDOCAdDirY]=dscale*(-2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
2032 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
2033 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
2034 | |
2035 | derivatives[CDCTrackD::dDOCAdDirZ]=dscale*(-2*(ty*wtx - tx*wty)*(c1 + c2 + c3)* |
2036 | (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) + |
2037 | wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2; |
2038 | |
2039 | derivatives[CDCTrackD::dDOCAdS0]=-derivatives[CDCTrackD::dDOCAdOriginX]; |
2040 | |
2041 | derivatives[CDCTrackD::dDOCAdS1]=-derivatives[CDCTrackD::dDOCAdOriginY]; |
2042 | |
2043 | derivatives[CDCTrackD::dDOCAdS2]=dscale*(2*(wty - ty*wtz)*(-c1 - c2 - c3)* |
2044 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
2045 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
2046 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
2047 | |
2048 | derivatives[CDCTrackD::dDOCAdS3]=dscale*(2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
2049 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
2050 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
2051 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
2052 | |
2053 | thisPull.AddTrackDerivatives(derivatives); |
2054 | |
2055 | pulls.push_back(thisPull); |
2056 | } |
2057 | else{ |
2058 | A=best_trajectory[m].Ckk*JT*C.Invert(); |
2059 | Ss=best_trajectory[m].Skk+A*(Ss-S); |
2060 | Cs=best_trajectory[m].Ckk+A*(Cs-C)*A.Transpose(); |
2061 | } |
2062 | |
2063 | S=best_trajectory[m].Skk; |
2064 | C=best_trajectory[m].Ckk; |
2065 | JT=best_trajectory[m].J.Transpose(); |
2066 | } |
2067 | |
2068 | return NOERROR; |
2069 | } |
2070 | |
2071 | shared_ptr<TMatrixFSym> |
2072 | DTrackFitterStraightTrack::Get7x7ErrorMatrix(TMatrixFSym C,DMatrix4x1 &S,double sign){ |
2073 | auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); |
2074 | C7x7->ResizeTo(7, 7); |
2075 | DMatrix J(7,5); |
2076 | |
2077 | double p=5.; // fixed: cannot measure |
2078 | double tx_=S(state_tx); |
2079 | double ty_=S(state_ty); |
2080 | double x_=S(state_x); |
2081 | double y_=S(state_y); |
2082 | double tanl=sign/sqrt(tx_*tx_+ty_*ty_); |
2083 | double tanl2=tanl*tanl; |
2084 | double lambda=atan(tanl); |
2085 | double sinl=sin(lambda); |
2086 | double sinl3=sinl*sinl*sinl; |
2087 | |
2088 | J(state_X,state_x)=J(state_Y,state_y)=1.; |
2089 | J(state_Pz,state_ty)=-p*ty_*sinl3; |
2090 | J(state_Pz,state_tx)=-p*tx_*sinl3; |
2091 | J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3; |
2092 | J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3; |
2093 | J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3; |
2094 | J(state_Pz,4)=-p*p*sinl; |
2095 | J(state_Px,4)=tx_*J(state_Pz,4); |
2096 | J(state_Py,4)=ty_*J(state_Pz,4); |
2097 | J(state_Z,state_x)=-tx_*tanl2; |
2098 | J(state_Z,state_y)=-ty_*tanl2; |
2099 | double diff=tx_*tx_-ty_*ty_; |
2100 | double frac=tanl2*tanl2; |
2101 | J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac; |
2102 | J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac; |
2103 | |
2104 | // C'= JCJ^T |
2105 | *C7x7=C.Similarity(J); |
2106 | |
2107 | return C7x7; |
2108 | } |
2109 | |
2110 | // Routine to get extrapolations to other detectors |
2111 | void DTrackFitterStraightTrack::GetExtrapolations(const DVector3 &pos0, |
2112 | const DVector3 &dir){ |
2113 | double x0=pos0.x(),y0=pos0.y(),z0=pos0.z(); |
2114 | double s=0.,t=0.; |
2115 | DVector3 pos(0,0,0); |
2116 | DVector3 diff(0,0,0); |
2117 | ClearExtrapolations(); |
2118 | |
2119 | double z=z0; |
2120 | double dz=0.1; |
2121 | double uz=dir.z(); |
2122 | double ux=dir.x()/uz,ux2=ux*ux; |
2123 | double uy=dir.y()/uz,uy2=uy*uy; |
2124 | |
2125 | // Extrapolate to start counter |
2126 | double Rd=SC_BARREL_R; |
2127 | double A=ux*x0 + uy*y0; |
2128 | double B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
2129 | double C=ux2 + uy2; |
2130 | if (sc_pos.empty()==false && z<SC_END_NOSE_Z && B>0){ |
2131 | dz=-(A-sqrt(B))/C; |
2132 | pos=pos0+(dz/uz)*dir; |
2133 | z=pos.z(); |
2134 | bool done=(z<sc_pos[0][0].z()||z>SC_END_NOSE_Z); |
2135 | while(!done){ |
2136 | double d_old=1000.,d=1000.; |
2137 | unsigned int index=0; |
2138 | |
2139 | for (unsigned int m=0;m<12;m++){ |
2140 | double dphi=pos.Phi()-SC_PHI_SECTOR1; |
2141 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
2142 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
2143 | if (index>29) index=0; |
2144 | d=sc_norm[index][m].Dot(pos-sc_pos[index][m]); |
2145 | if (d*d_old<0){ // break if we cross the current plane |
2146 | pos+=d*dir; |
2147 | s=(pos-pos0).Mag(); |
2148 | t=s/29.98; |
2149 | extrapolations[SYS_START].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2150 | done=true; |
2151 | break; |
2152 | } |
2153 | d_old=d; |
2154 | } |
2155 | pos-=(0.1/uz)*dir; |
2156 | z=pos.z(); |
2157 | } |
2158 | } |
2159 | // Extrapolate to BCAL |
2160 | Rd=65.; // approximate BCAL inner radius |
2161 | B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
2162 | if (B>0){ |
2163 | diff=-(A-sqrt(B))/(C*uz)*dir; |
2164 | s=diff.Mag(); |
2165 | t=s/29.98; |
2166 | pos=pos0+diff; |
2167 | extrapolations[SYS_BCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2168 | Rd=89.; // approximate BCAL outer radius |
2169 | B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
2170 | if (B>0){ |
2171 | diff=-(A-sqrt(B))/(C*uz)*dir; |
2172 | s=diff.Mag(); |
2173 | t=s/29.98; |
2174 | pos=pos0+diff; |
2175 | extrapolations[SYS_BCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2176 | } |
2177 | } |
2178 | |
2179 | // Extrapolate to TRD |
2180 | for (unsigned int i=0;i<dTRDz_vec.size();i++){ |
2181 | diff=((dTRDz_vec[i]-z0)/uz)*dir; |
2182 | pos=pos0+diff; |
2183 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2184 | s=diff.Mag(); |
2185 | t=s/29.98; |
2186 | extrapolations[SYS_TRD].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2187 | } |
2188 | } |
2189 | |
2190 | // Extrapolate to DIRC |
2191 | diff=((dDIRCz-z0)/uz)*dir; |
2192 | pos=pos0+diff; |
2193 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2194 | s=diff.Mag(); |
2195 | t=s/29.98; |
2196 | extrapolations[SYS_DIRC].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2197 | } |
2198 | |
2199 | // Extrapolate to TOF |
2200 | diff=((dTOFz-z0)/uz)*dir; |
2201 | pos=pos0+diff; |
2202 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2203 | double s=diff.Mag(); |
2204 | double t=s/29.98; |
Value stored to 't' during its initialization is never read | |
2205 | s=diff.Mag(); |
2206 | t=s/29.98; |
2207 | extrapolations[SYS_TOF].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2208 | } |
2209 | |
2210 | // Extrapolate to FCAL |
2211 | diff=((dFCALz-z0)/uz)*dir; |
2212 | pos=pos0+diff; |
2213 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
2214 | s=diff.Mag(); |
2215 | t=s/29.98; |
2216 | extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2217 | |
2218 | // extrapolate to exit of FCAL |
2219 | diff=((dFCALz+45.-z0)/uz)*dir; |
2220 | pos=pos0+diff; |
2221 | s=diff.Mag(); |
2222 | t=s/29.98; |
2223 | extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
2224 | } |
2225 | } |