File: | libraries/TRACKING/DTrackFitterKalmanSIMD.cc |
Location: | line 8706, column 10 |
Description: | Value stored to 'newz' during its initialization is never read |
1 | //************************************************************************ |
2 | // DTrackFitterKalmanSIMD.cc |
3 | //************************************************************************ |
4 | |
5 | #include "DTrackFitterKalmanSIMD.h" |
6 | #include "CDC/DCDCTrackHit.h" |
7 | #include "HDGEOMETRY/DLorentzDeflections.h" |
8 | #include "HDGEOMETRY/DMaterialMap.h" |
9 | #include "HDGEOMETRY/DRootGeom.h" |
10 | #include "DANA/DApplication.h" |
11 | #include <JANA/JCalibration.h> |
12 | #include "PID/DParticleID.h" |
13 | |
14 | #include <TH2F.h> |
15 | #include <TH1I.h> |
16 | #include <TROOT.h> |
17 | #include <TMath.h> |
18 | #include <DMatrix.h> |
19 | |
20 | #include <iomanip> |
21 | #include <math.h> |
22 | |
23 | #define MAX_TB_PASSES20 20 |
24 | #define MAX_WB_PASSES20 20 |
25 | #define MAX_P12.0 12.0 |
26 | #define ALPHA1./137. 1./137. |
27 | #define CHISQ_DELTA0.01 0.01 |
28 | #define MIN_ITER3 3 |
29 | |
30 | // Only print messages for one thread whenever run number change |
31 | static pthread_mutex_t print_mutex = PTHREAD_MUTEX_INITIALIZER{ { 0, 0, 0, 0, 0, 0, 0, { 0, 0 } } }; |
32 | static set<int> runs_announced; |
33 | |
34 | // Local boolean routines for sorting |
35 | //bool static DKalmanSIMDHit_cmp(DKalmanSIMDHit_t *a, DKalmanSIMDHit_t *b){ |
36 | // return a->z<b->z; |
37 | //} |
38 | |
39 | inline bool static DKalmanSIMDFDCHit_cmp(DKalmanSIMDFDCHit_t *a, DKalmanSIMDFDCHit_t *b){ |
40 | if (fabs(a->z-b->z)<EPS3.0e-8){ |
41 | if (a->hit!=NULL__null && b->hit!=NULL__null && fabs(a->t-b->t)<EPS3.0e-8){ |
42 | double tsum_1=a->hit->t_u+a->hit->t_v; |
43 | double tsum_2=b->hit->t_u+b->hit->t_v; |
44 | if (fabs(tsum_1-tsum_2)<EPS3.0e-8){ |
45 | return (a->dE>b->dE); |
46 | } |
47 | return (tsum_1<tsum_2); |
48 | } |
49 | return(a->t<b->t); |
50 | } |
51 | return a->z<b->z; |
52 | } |
53 | inline bool static DKalmanSIMDCDCHit_cmp(DKalmanSIMDCDCHit_t *a, DKalmanSIMDCDCHit_t *b){ |
54 | if (a==NULL__null || b==NULL__null){ |
55 | cout << "Null pointer in CDC hit list??" << endl; |
56 | return false; |
57 | } |
58 | const DCDCWire *wire_a= a->hit->wire; |
59 | const DCDCWire *wire_b= b->hit->wire; |
60 | if(wire_b->ring == wire_a->ring){ |
61 | return wire_b->straw < wire_a->straw; |
62 | } |
63 | |
64 | return (wire_b->ring>wire_a->ring); |
65 | } |
66 | |
67 | |
68 | // Locate a position in array xx given x |
69 | void DTrackFitterKalmanSIMD::locate(const double *xx,int n,double x,int *j){ |
70 | int jl=-1; |
71 | int ju=n; |
72 | int ascnd=(xx[n-1]>=xx[0]); |
73 | while(ju-jl>1){ |
74 | int jm=(ju+jl)>>1; |
75 | if ( (x>=xx[jm])==ascnd) |
76 | jl=jm; |
77 | else |
78 | ju=jm; |
79 | } |
80 | if (x==xx[0]) *j=0; |
81 | else if (x==xx[n-1]) *j=n-2; |
82 | else *j=jl; |
83 | } |
84 | |
85 | |
86 | |
87 | // Locate a position in vector xx given x |
88 | unsigned int DTrackFitterKalmanSIMD::locate(vector<double>&xx,double x){ |
89 | int n=xx.size(); |
90 | if (x==xx[0]) return 0; |
91 | else if (x==xx[n-1]) return n-2; |
92 | |
93 | int jl=-1; |
94 | int ju=n; |
95 | int ascnd=(xx[n-1]>=xx[0]); |
96 | while(ju-jl>1){ |
97 | int jm=(ju+jl)>>1; |
98 | if ( (x>=xx[jm])==ascnd) |
99 | jl=jm; |
100 | else |
101 | ju=jm; |
102 | } |
103 | return jl; |
104 | } |
105 | |
106 | // Crude approximation for the variance in drift distance due to smearing |
107 | double DTrackFitterKalmanSIMD::fdc_drift_variance(double t) const { |
108 | //return FDC_ANODE_VARIANCE; |
109 | double dt=0.; |
110 | double t_lo=DRIFT_RES_PARMS[3]; |
111 | double t_hi=DRIFT_RES_PARMS[4]; |
112 | if (t<t_lo) t=t_lo; |
113 | if (t>t_hi){ |
114 | t=t_hi; |
115 | dt=t-t_hi; |
116 | } |
117 | double sigma=DRIFT_RES_PARMS[0]/(t+1.)+DRIFT_RES_PARMS[1]+DRIFT_RES_PARMS[2]*t*t; |
118 | if (dt>0){ |
119 | sigma+=DRIFT_RES_PARMS[5]*dt; |
120 | } |
121 | |
122 | return sigma*sigma; |
123 | } |
124 | |
125 | // Convert time to distance for the cdc and compute variance |
126 | void DTrackFitterKalmanSIMD::ComputeCDCDrift(double dphi,double delta,double t, |
127 | double B, |
128 | double &d, double &V, double &tcorr){ |
129 | //d=0.39; // initialize at half-cell |
130 | //V=0.0507; // initialize with (cell size)/12. |
131 | tcorr = t; // Need this value even when t is negative for calibration |
132 | if (t>0){ |
133 | double dtc =(CDC_DRIFT_BSCALE_PAR1 + CDC_DRIFT_BSCALE_PAR2 * B)* t; |
134 | tcorr=t-dtc; |
135 | |
136 | // CDC_RES_PAR2=0.005; |
137 | double sigma=CDC_RES_PAR1/(tcorr+1.) + CDC_RES_PAR2 + CDC_RES_PAR3*tcorr; |
138 | |
139 | // Variables to store values for time-to-distance functions for delta=0 |
140 | // and delta!=0 |
141 | double f_0=0.; |
142 | double f_delta=0.; |
143 | // Derivative of d with respect to t, needed to add t0 variance |
144 | // dependence to sigma |
145 | double dd_dt=0; |
146 | // Scale factor to account for effect of B-field on maximum drift time |
147 | //double Bscale=long_drift_Bscale_par1+long_drift_Bscale_par2*B; |
148 | // tcorr=t*Bscale; |
149 | |
150 | // NSJ 26 May 2020 included long side a3, b3 and short side c1, c2, c3 |
151 | // Previously these parameters were not used (0 in ccdb) for production runs |
152 | // except intensity scan run 72312 by accident 5 May 2020, superseded 8 May. |
153 | // They were used in 2015 for runs 0-3050. |
154 | |
155 | // if (delta>0) |
156 | if (delta>-EPS21.e-4){ |
157 | double a1=long_drift_func[0][0]; |
158 | double a2=long_drift_func[0][1]; |
159 | double a3=long_drift_func[0][2]; |
160 | double b1=long_drift_func[1][0]; |
161 | double b2=long_drift_func[1][1]; |
162 | double b3=long_drift_func[1][2]; |
163 | double c1=long_drift_func[2][0]; |
164 | double c2=long_drift_func[2][1]; |
165 | double c3=long_drift_func[2][2]; |
166 | |
167 | // use "long side" functional form |
168 | double my_t=0.001*tcorr; |
169 | double sqrt_t=sqrt(my_t); |
170 | double t3=my_t*my_t*my_t; |
171 | double delta_mag=fabs(delta); |
172 | |
173 | double delta_sq=delta*delta; |
174 | double a=a1+a2*delta_mag+a3*delta_sq; |
175 | double b=b1+b2*delta_mag+b3*delta_sq; |
176 | double c=c1+c2*delta_mag+c3*delta_sq; |
177 | |
178 | f_delta=a*sqrt_t+b*my_t+c*t3; |
179 | f_0=a1*sqrt_t+b1*my_t+c1*t3; |
180 | |
181 | dd_dt=0.001*(0.5*a/sqrt_t+b+3.*c*my_t*my_t); |
182 | } |
183 | else{ |
184 | double my_t=0.001*tcorr; |
185 | double sqrt_t=sqrt(my_t); |
186 | double t3=my_t*my_t*my_t; |
187 | double delta_mag=fabs(delta); |
188 | |
189 | // use "short side" functional form |
190 | double a1=short_drift_func[0][0]; |
191 | double a2=short_drift_func[0][1]; |
192 | double a3=short_drift_func[0][2]; |
193 | double b1=short_drift_func[1][0]; |
194 | double b2=short_drift_func[1][1]; |
195 | double b3=short_drift_func[1][2]; |
196 | double c1=short_drift_func[2][0]; |
197 | double c2=short_drift_func[2][1]; |
198 | double c3=short_drift_func[2][2]; |
199 | |
200 | double delta_sq=delta*delta; |
201 | double a=a1+a2*delta_mag+a3*delta_sq; |
202 | double b=b1+b2*delta_mag+b3*delta_sq; |
203 | double c=c1+c2*delta_mag+c3*delta_sq; |
204 | |
205 | f_delta=a*sqrt_t+b*my_t+c*t3; |
206 | f_0=a1*sqrt_t+b1*my_t+c1*t3; |
207 | |
208 | dd_dt=0.001*(0.5*a/sqrt_t+b+3.*c*my_t*my_t); |
209 | |
210 | } |
211 | |
212 | unsigned int max_index=cdc_drift_table.size()-1; |
213 | if (tcorr>cdc_drift_table[max_index]){ |
214 | //_DBG_ << "t: " << tcorr <<" d " << f_delta <<endl; |
215 | d=f_delta; |
216 | V=sigma*sigma+mVarT0*dd_dt*dd_dt; |
217 | |
218 | return; |
219 | } |
220 | |
221 | // Drift time is within range of table -- interpolate... |
222 | unsigned int index=0; |
223 | index=locate(cdc_drift_table,tcorr); |
224 | double dt=cdc_drift_table[index+1]-cdc_drift_table[index]; |
225 | double frac=(tcorr-cdc_drift_table[index])/dt; |
226 | double d_0=0.01*(double(index)+frac); |
227 | |
228 | if (fabs(delta) < EPS21.e-4){ |
229 | d=d_0; |
230 | V=sigma*sigma+mVarT0*dd_dt*dd_dt; |
231 | } |
232 | else{ |
233 | double P=0.; |
234 | double tcut=250.0; // ns |
235 | if (tcorr<tcut) { |
236 | P=(tcut-tcorr)/tcut; |
237 | } |
238 | d=f_delta*(d_0/f_0*P+1.-P); |
239 | V=sigma*sigma+mVarT0*dd_dt*dd_dt; |
240 | } |
241 | } |
242 | else { // Time is negative, or exactly zero, choose position at wire, with error of t=0 hit |
243 | d=0.; |
244 | double sigma = CDC_RES_PAR1+CDC_RES_PAR2; |
245 | double dt=cdc_drift_table[1]-cdc_drift_table[0]; |
246 | V=sigma*sigma+mVarT0*0.0001/(dt*dt); |
247 | //V=0.0507; // straw radius^2 / 12 |
248 | } |
249 | |
250 | } |
251 | |
252 | #define FDC_T0_OFFSET17.6 17.6 |
253 | // Interpolate on a table to convert time to distance for the fdc |
254 | /* |
255 | double DTrackFitterKalmanSIMD::fdc_drift_distance(double t,double Bz) const { |
256 | double a=93.31,b=4.614,Bref=2.143; |
257 | t*=(a+b*Bref)/(a+b*Bz); |
258 | int id=int((t+FDC_T0_OFFSET)/2.); |
259 | if (id<0) id=0; |
260 | if (id>138) id=138; |
261 | double d=fdc_drift_table[id]; |
262 | if (id!=138){ |
263 | double frac=0.5*(t+FDC_T0_OFFSET-2.*double(id)); |
264 | double dd=fdc_drift_table[id+1]-fdc_drift_table[id]; |
265 | d+=frac*dd; |
266 | } |
267 | |
268 | return d; |
269 | } |
270 | */ |
271 | |
272 | // parametrization of time-to-distance for FDC |
273 | double DTrackFitterKalmanSIMD::fdc_drift_distance(double time,double Bz) const { |
274 | if (time<0.) return 0.; |
275 | double d=0.; |
276 | time/=1.+FDC_DRIFT_BSCALE_PAR1+FDC_DRIFT_BSCALE_PAR2*Bz*Bz; |
277 | double tsq=time*time; |
278 | double t_high=DRIFT_FUNC_PARMS[4]; |
279 | |
280 | if (time<t_high){ |
281 | d=DRIFT_FUNC_PARMS[0]*sqrt(time)+DRIFT_FUNC_PARMS[1]*time |
282 | +DRIFT_FUNC_PARMS[2]*tsq+DRIFT_FUNC_PARMS[3]*tsq*time; |
283 | } |
284 | else{ |
285 | double t_high_sq=t_high*t_high; |
286 | d=DRIFT_FUNC_PARMS[0]*sqrt(t_high)+DRIFT_FUNC_PARMS[1]*t_high |
287 | +DRIFT_FUNC_PARMS[2]*t_high_sq+DRIFT_FUNC_PARMS[3]*t_high_sq*t_high; |
288 | d+=DRIFT_FUNC_PARMS[5]*(time-t_high); |
289 | } |
290 | |
291 | return d; |
292 | } |
293 | |
294 | |
295 | DTrackFitterKalmanSIMD::DTrackFitterKalmanSIMD(JEventLoop *loop):DTrackFitter(loop){ |
296 | FactorForSenseOfRotation=(bfield->GetBz(0.,0.,65.)>0.)?-1.:1.; |
297 | |
298 | // keep track of which runs we print out messages for |
299 | int32_t runnumber = loop->GetJEvent().GetRunNumber(); |
300 | pthread_mutex_lock(&print_mutex); |
301 | bool print_messages = false; |
302 | if(runs_announced.find(runnumber) == runs_announced.end()){ |
303 | print_messages = true; |
304 | runs_announced.insert(runnumber); |
305 | } |
306 | pthread_mutex_unlock(&print_mutex); |
307 | |
308 | // Some useful values |
309 | two_m_e=2.*ELECTRON_MASS0.000511; |
310 | m_e_sq=ELECTRON_MASS0.000511*ELECTRON_MASS0.000511; |
311 | |
312 | // Get the position of the CDC downstream endplate from DGeometry |
313 | double endplate_rmin,endplate_rmax; |
314 | geom->GetCDCEndplate(endplate_z,endplate_dz,endplate_rmin,endplate_rmax); |
315 | endplate_z-=0.5*endplate_dz; |
316 | endplate_z_downstream=endplate_z+endplate_dz; |
317 | endplate_rmin+=0.1; // put just inside CDC |
318 | endplate_r2min=endplate_rmin*endplate_rmin; |
319 | endplate_r2max=endplate_rmax*endplate_rmax; |
320 | |
321 | // Beginning of the cdc |
322 | vector<double>cdc_center; |
323 | vector<double>cdc_upstream_endplate_pos; |
324 | vector<double>cdc_endplate_dim; |
325 | geom->Get("//posXYZ[@volume='CentralDC'/@X_Y_Z",cdc_origin); |
326 | geom->Get("//posXYZ[@volume='centralDC']/@X_Y_Z",cdc_center); |
327 | geom->Get("//posXYZ[@volume='CDPU']/@X_Y_Z",cdc_upstream_endplate_pos); |
328 | geom->Get("//tubs[@name='CDPU']/@Rio_Z",cdc_endplate_dim); |
329 | cdc_origin[2]+=cdc_center[2]+cdc_upstream_endplate_pos[2] |
330 | +0.5*cdc_endplate_dim[2]; |
331 | |
332 | // Outer detector geometry parameters |
333 | geom->GetFCALZ(dFCALz); |
334 | dFCALzBack=dFCALz+45.; |
335 | if (geom->GetDIRCZ(dDIRCz)==false) dDIRCz=1000.; |
336 | geom->GetFMWPCZ_vec(dFMWPCz_vec); |
337 | geom->GetFMWPCSize(dFMWPCsize); |
338 | geom->GetCTOFZ(dCTOFz); |
339 | |
340 | vector<double>tof_face; |
341 | geom->Get("//section/composition/posXYZ[@volume='ForwardTOF']/@X_Y_Z", |
342 | tof_face); |
343 | vector<double>tof_plane; |
344 | geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='0']", tof_plane); |
345 | dTOFz=tof_face[2]+tof_plane[2]; |
346 | geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='1']", tof_plane); |
347 | dTOFz+=tof_face[2]+tof_plane[2]; |
348 | dTOFz*=0.5; // mid plane between tof planes |
349 | geom->GetTRDZ(dTRDz_vec); // TRD planes |
350 | |
351 | // Get start counter geometry; |
352 | if (geom->GetStartCounterGeom(sc_pos, sc_norm)){ |
353 | // Create vector of direction vectors in scintillator planes |
354 | for (int i=0;i<30;i++){ |
355 | vector<DVector3>temp; |
356 | for (unsigned int j=0;j<sc_pos[i].size()-1;j++){ |
357 | double dx=sc_pos[i][j+1].x()-sc_pos[i][j].x(); |
358 | double dy=sc_pos[i][j+1].y()-sc_pos[i][j].y(); |
359 | double dz=sc_pos[i][j+1].z()-sc_pos[i][j].z(); |
360 | temp.push_back(DVector3(dx/dz,dy/dz,1.)); |
361 | } |
362 | sc_dir.push_back(temp); |
363 | } |
364 | SC_END_NOSE_Z=sc_pos[0][12].z(); |
365 | SC_BARREL_R2=sc_pos[0][0].Perp2(); |
366 | SC_PHI_SECTOR1=sc_pos[0][0].Phi(); |
367 | } |
368 | |
369 | // Get z positions of fdc wire planes |
370 | geom->GetFDCZ(fdc_z_wires); |
371 | |
372 | ADD_VERTEX_POINT=false; |
373 | gPARMS->SetDefaultParameter("KALMAN:ADD_VERTEX_POINT", ADD_VERTEX_POINT); |
374 | |
375 | THETA_CUT=60.0; |
376 | gPARMS->SetDefaultParameter("KALMAN:THETA_CUT", THETA_CUT); |
377 | |
378 | RING_TO_SKIP=0; |
379 | gPARMS->SetDefaultParameter("KALMAN:RING_TO_SKIP",RING_TO_SKIP); |
380 | |
381 | PLANE_TO_SKIP=0; |
382 | gPARMS->SetDefaultParameter("KALMAN:PLANE_TO_SKIP",PLANE_TO_SKIP); |
383 | |
384 | MINIMUM_HIT_FRACTION=0.7; |
385 | gPARMS->SetDefaultParameter("KALMAN:MINIMUM_HIT_FRACTION",MINIMUM_HIT_FRACTION); |
386 | MIN_HITS_FOR_REFIT=6; |
387 | gPARMS->SetDefaultParameter("KALMAN:MIN_HITS_FOR_REFIT", MIN_HITS_FOR_REFIT); |
388 | PHOTON_ENERGY_CUTOFF=0.125; |
389 | gPARMS->SetDefaultParameter("KALMAN:PHOTON_ENERGY_CUTOFF", |
390 | PHOTON_ENERGY_CUTOFF); |
391 | |
392 | USE_FDC_HITS=true; |
393 | gPARMS->SetDefaultParameter("TRKFIT:USE_FDC_HITS",USE_FDC_HITS); |
394 | USE_CDC_HITS=true; |
395 | gPARMS->SetDefaultParameter("TRKFIT:USE_CDC_HITS",USE_CDC_HITS); |
396 | USE_TRD_HITS=false; |
397 | gPARMS->SetDefaultParameter("TRKFIT:USE_TRD_HITS",USE_TRD_HITS); |
398 | USE_TRD_DRIFT_TIMES=true; |
399 | gPARMS->SetDefaultParameter("TRKFIT:USE_TRD_DRIFT_TIMES",USE_TRD_DRIFT_TIMES); |
400 | USE_GEM_HITS=false; |
401 | gPARMS->SetDefaultParameter("TRKFIT:USE_GEM_HITS",USE_GEM_HITS); |
402 | |
403 | |
404 | // Flag to enable calculation of alignment derivatives |
405 | ALIGNMENT=false; |
406 | gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT",ALIGNMENT); |
407 | |
408 | ALIGNMENT_FORWARD=false; |
409 | gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT_FORWARD",ALIGNMENT_FORWARD); |
410 | |
411 | ALIGNMENT_CENTRAL=false; |
412 | gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT_CENTRAL",ALIGNMENT_CENTRAL); |
413 | |
414 | if(ALIGNMENT){ALIGNMENT_FORWARD=true;ALIGNMENT_CENTRAL=true;} |
415 | |
416 | DEBUG_HISTS=false; |
417 | gPARMS->SetDefaultParameter("KALMAN:DEBUG_HISTS", DEBUG_HISTS); |
418 | |
419 | DEBUG_LEVEL=0; |
420 | gPARMS->SetDefaultParameter("KALMAN:DEBUG_LEVEL", DEBUG_LEVEL); |
421 | |
422 | USE_T0_FROM_WIRES=0; |
423 | gPARMS->SetDefaultParameter("KALMAN:USE_T0_FROM_WIRES", USE_T0_FROM_WIRES); |
424 | |
425 | ESTIMATE_T0_TB=false; |
426 | gPARMS->SetDefaultParameter("KALMAN:ESTIMATE_T0_TB",ESTIMATE_T0_TB); |
427 | |
428 | ENABLE_BOUNDARY_CHECK=true; |
429 | gPARMS->SetDefaultParameter("GEOM:ENABLE_BOUNDARY_CHECK", |
430 | ENABLE_BOUNDARY_CHECK); |
431 | |
432 | USE_MULS_COVARIANCE=true; |
433 | gPARMS->SetDefaultParameter("TRKFIT:USE_MULS_COVARIANCE", |
434 | USE_MULS_COVARIANCE); |
435 | |
436 | USE_PASS1_TIME_MODE=false; |
437 | gPARMS->SetDefaultParameter("KALMAN:USE_PASS1_TIME_MODE",USE_PASS1_TIME_MODE); |
438 | |
439 | USE_FDC_DRIFT_TIMES=true; |
440 | gPARMS->SetDefaultParameter("TRKFIT:USE_FDC_DRIFT_TIMES", |
441 | USE_FDC_DRIFT_TIMES); |
442 | |
443 | RECOVER_BROKEN_TRACKS=true; |
444 | gPARMS->SetDefaultParameter("KALMAN:RECOVER_BROKEN_TRACKS",RECOVER_BROKEN_TRACKS); |
445 | |
446 | NUM_CDC_SIGMA_CUT=5.0; |
447 | NUM_FDC_SIGMA_CUT=5.0; |
448 | gPARMS->SetDefaultParameter("KALMAN:NUM_CDC_SIGMA_CUT",NUM_CDC_SIGMA_CUT, |
449 | "maximum distance in number of sigmas away from projection to accept cdc hit"); |
450 | gPARMS->SetDefaultParameter("KALMAN:NUM_FDC_SIGMA_CUT",NUM_FDC_SIGMA_CUT, |
451 | "maximum distance in number of sigmas away from projection to accept fdc hit"); |
452 | |
453 | ANNEAL_SCALE=9.0; |
454 | ANNEAL_POW_CONST=1.5; |
455 | gPARMS->SetDefaultParameter("KALMAN:ANNEAL_SCALE",ANNEAL_SCALE, |
456 | "Scale factor for annealing"); |
457 | gPARMS->SetDefaultParameter("KALMAN:ANNEAL_POW_CONST",ANNEAL_POW_CONST, |
458 | "Annealing parameter"); |
459 | FORWARD_ANNEAL_SCALE=9.; |
460 | FORWARD_ANNEAL_POW_CONST=1.5; |
461 | gPARMS->SetDefaultParameter("KALMAN:FORWARD_ANNEAL_SCALE", |
462 | FORWARD_ANNEAL_SCALE, |
463 | "Scale factor for annealing"); |
464 | gPARMS->SetDefaultParameter("KALMAN:FORWARD_ANNEAL_POW_CONST", |
465 | FORWARD_ANNEAL_POW_CONST, |
466 | "Annealing parameter"); |
467 | |
468 | FORWARD_PARMS_COV=false; |
469 | gPARMS->SetDefaultParameter("KALMAN:FORWARD_PARMS_COV",FORWARD_PARMS_COV); |
470 | |
471 | CDC_VAR_SCALE_FACTOR=1.; |
472 | gPARMS->SetDefaultParameter("KALMAN:CDC_VAR_SCALE_FACTOR",CDC_VAR_SCALE_FACTOR); |
473 | CDC_T_DRIFT_MIN=-12.; // One f125 clock = 8 ns |
474 | gPARMS->SetDefaultParameter("KALMAN:CDC_T_DRIFT_MIN",CDC_T_DRIFT_MIN); |
475 | |
476 | MOLIERE_FRACTION=0.9; |
477 | gPARMS->SetDefaultParameter("KALMAN:MOLIERE_FRACTION",MOLIERE_FRACTION); |
478 | MS_SCALE_FACTOR=2.0; |
479 | gPARMS->SetDefaultParameter("KALMAN:MS_SCALE_FACTOR",MS_SCALE_FACTOR); |
480 | MOLIERE_RATIO1=0.5/(1.-MOLIERE_FRACTION); |
481 | MOLIERE_RATIO2=MS_SCALE_FACTOR*1.e-6/(1.+MOLIERE_FRACTION*MOLIERE_FRACTION); //scale_factor/(1+F*F) |
482 | |
483 | COVARIANCE_SCALE_FACTOR_CENTRAL=2.0; |
484 | gPARMS->SetDefaultParameter("KALMAN:COVARIANCE_SCALE_FACTOR_CENTRAL", |
485 | COVARIANCE_SCALE_FACTOR_CENTRAL); |
486 | |
487 | COVARIANCE_SCALE_FACTOR_FORWARD=2.0; |
488 | gPARMS->SetDefaultParameter("KALMAN:COVARIANCE_SCALE_FACTOR_FORWARD", |
489 | COVARIANCE_SCALE_FACTOR_FORWARD); |
490 | |
491 | WRITE_ML_TRAINING_OUTPUT=false; |
492 | gPARMS->SetDefaultParameter("KALMAN:WRITE_ML_TRAINING_OUTPUT", |
493 | WRITE_ML_TRAINING_OUTPUT); |
494 | |
495 | if (WRITE_ML_TRAINING_OUTPUT){ |
496 | mlfile.open("mltraining.dat"); |
497 | } |
498 | |
499 | |
500 | DApplication* dapp = dynamic_cast<DApplication*>(loop->GetJApplication()); |
501 | JCalibration *jcalib = dapp->GetJCalibration((loop->GetJEvent()).GetRunNumber()); |
502 | vector< map<string, double> > tvals; |
503 | cdc_drift_table.clear(); |
504 | if (jcalib->Get("CDC/cdc_drift_table", tvals)==false){ |
505 | for(unsigned int i=0; i<tvals.size(); i++){ |
506 | map<string, double> &row = tvals[i]; |
507 | cdc_drift_table.push_back(1000.*row["t"]); |
508 | } |
509 | } |
510 | else{ |
511 | jerr << " CDC time-to-distance table not available... bailing..." << endl; |
512 | exit(0); |
513 | } |
514 | |
515 | int straw_number[28]={42,42,54,54,66,66,80,80,93,93,106,106, |
516 | 123,123,135,135,146,146,158,158,170,170, |
517 | 182,182,197,197,209,209}; |
518 | max_sag.clear(); |
519 | sag_phi_offset.clear(); |
520 | int straw_count=0,ring_count=0; |
521 | if (jcalib->Get("CDC/sag_parameters", tvals)==false){ |
522 | vector<double>temp,temp2; |
523 | for(unsigned int i=0; i<tvals.size(); i++){ |
524 | map<string, double> &row = tvals[i]; |
525 | |
526 | temp.push_back(row["offset"]); |
527 | temp2.push_back(row["phi"]); |
528 | |
529 | straw_count++; |
530 | if (straw_count==straw_number[ring_count]){ |
531 | max_sag.push_back(temp); |
532 | sag_phi_offset.push_back(temp2); |
533 | temp.clear(); |
534 | temp2.clear(); |
535 | straw_count=0; |
536 | ring_count++; |
537 | } |
538 | } |
539 | } |
540 | |
541 | if (jcalib->Get("CDC/drift_parameters", tvals)==false){ |
542 | map<string, double> &row = tvals[0]; // long drift side |
543 | long_drift_func[0][0]=row["a1"]; |
544 | long_drift_func[0][1]=row["a2"]; |
545 | long_drift_func[0][2]=row["a3"]; |
546 | long_drift_func[1][0]=row["b1"]; |
547 | long_drift_func[1][1]=row["b2"]; |
548 | long_drift_func[1][2]=row["b3"]; |
549 | long_drift_func[2][0]=row["c1"]; |
550 | long_drift_func[2][1]=row["c2"]; |
551 | long_drift_func[2][2]=row["c3"]; |
552 | long_drift_Bscale_par1=row["B1"]; |
553 | long_drift_Bscale_par2=row["B2"]; |
554 | |
555 | row = tvals[1]; // short drift side |
556 | short_drift_func[0][0]=row["a1"]; |
557 | short_drift_func[0][1]=row["a2"]; |
558 | short_drift_func[0][2]=row["a3"]; |
559 | short_drift_func[1][0]=row["b1"]; |
560 | short_drift_func[1][1]=row["b2"]; |
561 | short_drift_func[1][2]=row["b3"]; |
562 | short_drift_func[2][0]=row["c1"]; |
563 | short_drift_func[2][1]=row["c2"]; |
564 | short_drift_func[2][2]=row["c3"]; |
565 | short_drift_Bscale_par1=row["B1"]; |
566 | short_drift_Bscale_par2=row["B2"]; |
567 | } |
568 | |
569 | map<string, double> cdc_drift_parms; |
570 | jcalib->Get("CDC/cdc_drift_parms", cdc_drift_parms); |
571 | CDC_DRIFT_BSCALE_PAR1 = cdc_drift_parms["bscale_par1"]; |
572 | CDC_DRIFT_BSCALE_PAR2 = cdc_drift_parms["bscale_par2"]; |
573 | |
574 | map<string, double> cdc_res_parms; |
575 | jcalib->Get("CDC/cdc_resolution_parms_v2", cdc_res_parms); |
576 | CDC_RES_PAR1 = cdc_res_parms["res_par1"]; |
577 | CDC_RES_PAR2 = cdc_res_parms["res_par2"]; |
578 | CDC_RES_PAR3 = cdc_res_parms["res_par3"]; |
579 | |
580 | // Parameters for correcting for deflection due to Lorentz force |
581 | map<string,double>lorentz_parms; |
582 | jcalib->Get("FDC/lorentz_deflection_parms",lorentz_parms); |
583 | LORENTZ_NR_PAR1=lorentz_parms["nr_par1"]; |
584 | LORENTZ_NR_PAR2=lorentz_parms["nr_par2"]; |
585 | LORENTZ_NZ_PAR1=lorentz_parms["nz_par1"]; |
586 | LORENTZ_NZ_PAR2=lorentz_parms["nz_par2"]; |
587 | |
588 | // Parameters for accounting for variation in drift distance from FDC |
589 | map<string,double>drift_res_parms; |
590 | jcalib->Get("FDC/drift_resolution_parms",drift_res_parms); |
591 | DRIFT_RES_PARMS[0]=drift_res_parms["p0"]; |
592 | DRIFT_RES_PARMS[1]=drift_res_parms["p1"]; |
593 | DRIFT_RES_PARMS[2]=drift_res_parms["p2"]; |
594 | map<string,double>drift_res_ext; |
595 | jcalib->Get("FDC/drift_resolution_ext",drift_res_ext); |
596 | DRIFT_RES_PARMS[3]=drift_res_ext["t_low"]; |
597 | DRIFT_RES_PARMS[4]=drift_res_ext["t_high"]; |
598 | DRIFT_RES_PARMS[5]=drift_res_ext["res_slope"]; |
599 | |
600 | // Time-to-distance function parameters for FDC |
601 | map<string,double>drift_func_parms; |
602 | jcalib->Get("FDC/drift_function_parms",drift_func_parms); |
603 | DRIFT_FUNC_PARMS[0]=drift_func_parms["p0"]; |
604 | DRIFT_FUNC_PARMS[1]=drift_func_parms["p1"]; |
605 | DRIFT_FUNC_PARMS[2]=drift_func_parms["p2"]; |
606 | DRIFT_FUNC_PARMS[3]=drift_func_parms["p3"]; |
607 | DRIFT_FUNC_PARMS[4]=1000.; |
608 | DRIFT_FUNC_PARMS[5]=0.; |
609 | map<string,double>drift_func_ext; |
610 | if (jcalib->Get("FDC/drift_function_ext",drift_func_ext)==false){ |
611 | DRIFT_FUNC_PARMS[4]=drift_func_ext["p4"]; |
612 | DRIFT_FUNC_PARMS[5]=drift_func_ext["p5"]; |
613 | } |
614 | // Factors for taking care of B-dependence of drift time for FDC |
615 | map<string, double> fdc_drift_parms; |
616 | jcalib->Get("FDC/fdc_drift_parms", fdc_drift_parms); |
617 | FDC_DRIFT_BSCALE_PAR1 = fdc_drift_parms["bscale_par1"]; |
618 | FDC_DRIFT_BSCALE_PAR2 = fdc_drift_parms["bscale_par2"]; |
619 | |
620 | |
621 | /* |
622 | if (jcalib->Get("FDC/fdc_drift2", tvals)==false){ |
623 | for(unsigned int i=0; i<tvals.size(); i++){ |
624 | map<string, float> &row = tvals[i]; |
625 | iter_float iter = row.begin(); |
626 | fdc_drift_table[i] = iter->second; |
627 | } |
628 | } |
629 | else{ |
630 | jerr << " FDC time-to-distance table not available... bailing..." << endl; |
631 | exit(0); |
632 | } |
633 | */ |
634 | |
635 | for (unsigned int i=0;i<5;i++)I5x5(i,i)=1.; |
636 | |
637 | |
638 | // center of the target |
639 | map<string, double> targetparms; |
640 | if (jcalib->Get("TARGET/target_parms",targetparms)==false){ |
641 | TARGET_Z = targetparms["TARGET_Z_POSITION"]; |
642 | } |
643 | else{ |
644 | geom->GetTargetZ(TARGET_Z); |
645 | } |
646 | if (ADD_VERTEX_POINT){ |
647 | gPARMS->SetDefaultParameter("KALMAN:VERTEX_POSITION",TARGET_Z); |
648 | } |
649 | |
650 | // Beam position and direction |
651 | map<string, double> beam_vals; |
652 | jcalib->Get("PHOTON_BEAM/beam_spot",beam_vals); |
653 | beam_center.Set(beam_vals["x"],beam_vals["y"]); |
654 | beam_dir.Set(beam_vals["dxdz"],beam_vals["dydz"]); |
655 | |
656 | if(print_messages) |
657 | jout << " Beam spot: x=" << beam_center.X() << " y=" << beam_center.Y() |
658 | << " z=" << beam_vals["z"] |
659 | << " dx/dz=" << beam_dir.X() << " dy/dz=" << beam_dir.Y() << endl; |
660 | beam_z0=beam_vals["z"]; |
661 | |
662 | // Inform user of some configuration settings |
663 | static bool config_printed = false; |
664 | if(!config_printed){ |
665 | config_printed = true; |
666 | stringstream ss; |
667 | ss << "vertex constraint: " ; |
668 | if(ADD_VERTEX_POINT){ |
669 | ss << "z = " << TARGET_Z << "cm" << endl; |
670 | }else{ |
671 | ss << "<none>" << endl; |
672 | } |
673 | jout << ss.str(); |
674 | } // config_printed |
675 | |
676 | if(DEBUG_HISTS){ |
677 | for (auto i=0; i < 46; i++){ |
678 | double min = -10., max=10.; |
679 | if(i%23<12) {min=-5; max=5;} |
680 | if(i<23)alignDerivHists[i]=new TH1I(Form("CentralDeriv%i",i),Form("CentralDeriv%i",i),200, min, max); |
681 | else alignDerivHists[i]=new TH1I(Form("ForwardDeriv%i",i%23),Form("ForwardDeriv%i",i%23),200, min, max); |
682 | } |
683 | brentCheckHists[0]=new TH2I("ForwardBrentCheck","DOCA vs ds", 100, -5., 5., 100, 0.0, 1.5); |
684 | brentCheckHists[1]=new TH2I("CentralBrentCheck","DOCA vs ds", 100, -5., 5., 100, 0.0, 1.5); |
685 | } |
686 | |
687 | dResourcePool_TMatrixFSym = std::make_shared<DResourcePool<TMatrixFSym>>(); |
688 | dResourcePool_TMatrixFSym->Set_ControlParams(20, 20, 50); |
689 | |
690 | my_fdchits.reserve(24); |
691 | my_cdchits.reserve(28); |
692 | fdc_updates.reserve(24); |
693 | cdc_updates.reserve(28); |
694 | cdc_used_in_fit.reserve(28); |
695 | fdc_used_in_fit.reserve(24); |
696 | } |
697 | |
698 | //----------------- |
699 | // ResetKalmanSIMD |
700 | //----------------- |
701 | void DTrackFitterKalmanSIMD::ResetKalmanSIMD(void) |
702 | { |
703 | last_material_map=0; |
704 | |
705 | for (unsigned int i=0;i<my_cdchits.size();i++){ |
706 | delete my_cdchits[i]; |
707 | } |
708 | for (unsigned int i=0;i<my_fdchits.size();i++){ |
709 | delete my_fdchits[i]; |
710 | } |
711 | central_traj.clear(); |
712 | forward_traj.clear(); |
713 | my_fdchits.clear(); |
714 | my_cdchits.clear(); |
715 | fdc_updates.clear(); |
716 | cdc_updates.clear(); |
717 | fdc_used_in_fit.clear(); |
718 | cdc_used_in_fit.clear(); |
719 | got_trd_gem_hits=false; |
720 | |
721 | cov.clear(); |
722 | fcov.clear(); |
723 | |
724 | len = 0.0; |
725 | ftime=0.0; |
726 | var_ftime=0.0; |
727 | x_=0.,y_=0.,tx_=0.,ty_=0.,q_over_p_ = 0.0; |
728 | z_=0.,phi_=0.,tanl_=0.,q_over_pt_ =0, D_= 0.0; |
729 | chisq_ = 0.0; |
730 | ndf_ = 0; |
731 | MASS=0.13957; |
732 | mass2=MASS*MASS; |
733 | Bx=By=0.; |
734 | Bz=2.; |
735 | dBxdx=0.,dBxdy=0.,dBxdz=0.,dBydx=0.,dBydy=0.,dBydy=0.,dBzdx=0.,dBzdy=0.,dBzdz=0.; |
736 | // Step sizes |
737 | mStepSizeS=2.0; |
738 | mStepSizeZ=2.0; |
739 | //mStepSizeZ=0.5; |
740 | |
741 | //if (fit_type==kTimeBased){ |
742 | // mStepSizeS=0.5; |
743 | // mStepSizeZ=0.5; |
744 | // } |
745 | |
746 | |
747 | mT0=0.,mT0MinimumDriftTime=1e6; |
748 | mVarT0=25.; |
749 | |
750 | mCDCInternalStepSize=0.5; |
751 | //mCDCInternalStepSize=1.0; |
752 | //mCentralStepSize=0.75; |
753 | mCentralStepSize=0.75; |
754 | |
755 | mT0Detector=SYS_CDC; |
756 | |
757 | IsHadron=true; |
758 | IsElectron=false; |
759 | IsPositron=false; |
760 | |
761 | PT_MIN=0.01; |
762 | Q_OVER_P_MAX=100.; |
763 | |
764 | // These variables provide the approximate location along the trajectory |
765 | // where there is an indication of a kink in the track |
766 | break_point_fdc_index=0; |
767 | break_point_cdc_index=0; |
768 | break_point_step_index=0; |
769 | |
770 | } |
771 | |
772 | //----------------- |
773 | // FitTrack |
774 | //----------------- |
775 | DTrackFitter::fit_status_t DTrackFitterKalmanSIMD::FitTrack(void) |
776 | { |
777 | // Reset member data and free an memory associated with the last fit, |
778 | // but some of which only for wire-based fits |
779 | ResetKalmanSIMD(); |
780 | |
781 | // Check that we have enough FDC and CDC hits to proceed |
782 | if (cdchits.size()==0 && fdchits.size()<4) return kFitNotDone; |
783 | if (cdchits.size()+fdchits.size() < 6) return kFitNotDone; |
784 | |
785 | // Copy hits from base class into structures specific to DTrackFitterKalmanSIMD |
786 | if (USE_CDC_HITS) |
787 | for(unsigned int i=0; i<cdchits.size(); i++)AddCDCHit(cdchits[i]); |
788 | if (USE_FDC_HITS) |
789 | for(unsigned int i=0; i<fdchits.size(); i++)AddFDCHit(fdchits[i]); |
790 | if (USE_TRD_HITS){ |
791 | for(unsigned int i=0; i<trdhits.size(); i++)AddTRDHit(trdhits[i]); |
792 | if (trdhits.size()>0){ |
793 | //_DBG_ << "Got TRD" <<endl; |
794 | got_trd_gem_hits=true; |
795 | } |
796 | } |
797 | if (USE_GEM_HITS){ |
798 | for(unsigned int i=0; i<gemhits.size(); i++)AddGEMHit(gemhits[i]); |
799 | if (gemhits.size()>0){ |
800 | //_DBG_ << " Got GEM" << endl; |
801 | got_trd_gem_hits=true; |
802 | } |
803 | } |
804 | |
805 | unsigned int num_good_cdchits=my_cdchits.size(); |
806 | unsigned int num_good_fdchits=my_fdchits.size(); |
807 | |
808 | // keep track of the range of detector elements that could be hit |
809 | // for calculating the number of expected hits later on |
810 | //int min_cdc_ring=-1, max_cdc_ring=-1; |
811 | |
812 | // Order the cdc hits by ring number |
813 | if (num_good_cdchits>0){ |
814 | stable_sort(my_cdchits.begin(),my_cdchits.end(),DKalmanSIMDCDCHit_cmp); |
815 | |
816 | //min_cdc_ring = my_cdchits[0]->hit->wire->ring; |
817 | //max_cdc_ring = my_cdchits[my_cdchits.size()-1]->hit->wire->ring; |
818 | |
819 | // Look for multiple hits on the same wire |
820 | for (unsigned int i=0;i<my_cdchits.size()-1;i++){ |
821 | if (my_cdchits[i]->hit->wire->ring==my_cdchits[i+1]->hit->wire->ring && |
822 | my_cdchits[i]->hit->wire->straw==my_cdchits[i+1]->hit->wire->straw){ |
823 | num_good_cdchits--; |
824 | if (my_cdchits[i]->tdrift<my_cdchits[i+1]->tdrift){ |
825 | my_cdchits[i+1]->status=late_hit; |
826 | } |
827 | else{ |
828 | my_cdchits[i]->status=late_hit; |
829 | } |
830 | } |
831 | } |
832 | |
833 | } |
834 | // Order the fdc hits by z |
835 | if (num_good_fdchits>0){ |
836 | stable_sort(my_fdchits.begin(),my_fdchits.end(),DKalmanSIMDFDCHit_cmp); |
837 | |
838 | // Look for multiple hits on the same wire |
839 | for (unsigned int i=0;i<my_fdchits.size()-1;i++){ |
840 | if (my_fdchits[i]->hit==NULL__null || my_fdchits[i+1]->hit==NULL__null) continue; |
841 | if (my_fdchits[i]->hit->wire->layer==my_fdchits[i+1]->hit->wire->layer && |
842 | my_fdchits[i]->hit->wire->wire==my_fdchits[i+1]->hit->wire->wire){ |
843 | num_good_fdchits--; |
844 | if (fabs(my_fdchits[i]->t-my_fdchits[i+1]->t)<EPS3.0e-8){ |
845 | double tsum_1=my_fdchits[i]->hit->t_u+my_fdchits[i]->hit->t_v; |
846 | double tsum_2=my_fdchits[i+1]->hit->t_u+my_fdchits[i+1]->hit->t_v; |
847 | if (tsum_1<tsum_2){ |
848 | my_fdchits[i+1]->status=late_hit; |
849 | } |
850 | else{ |
851 | my_fdchits[i]->status=late_hit; |
852 | } |
853 | } |
854 | else if (my_fdchits[i]->t<my_fdchits[i+1]->t){ |
855 | my_fdchits[i+1]->status=late_hit; |
856 | } |
857 | else{ |
858 | my_fdchits[i]->status=late_hit; |
859 | } |
860 | } |
861 | } |
862 | } |
863 | if (num_good_cdchits==0 && num_good_fdchits<4) return kFitNotDone; |
864 | if (num_good_cdchits+num_good_fdchits < 6) return kFitNotDone; |
865 | |
866 | // Create vectors of updates (from hits) to S and C |
867 | if (my_cdchits.size()>0){ |
868 | cdc_updates=vector<DKalmanUpdate_t>(my_cdchits.size()); |
869 | // Initialize vector to keep track of whether or not a hit is used in |
870 | // the fit |
871 | cdc_used_in_fit=vector<bool>(my_cdchits.size()); |
872 | } |
873 | if (my_fdchits.size()>0){ |
874 | fdc_updates=vector<DKalmanUpdate_t>(my_fdchits.size()); |
875 | // Initialize vector to keep track of whether or not a hit is used in |
876 | // the fit |
877 | fdc_used_in_fit=vector<bool>(my_fdchits.size()); |
878 | } |
879 | |
880 | // start time and variance |
881 | if (fit_type==kTimeBased){ |
882 | mT0=input_params.t0(); |
883 | switch(input_params.t0_detector()){ |
884 | case SYS_TOF: |
885 | mVarT0=0.01; |
886 | break; |
887 | case SYS_CDC: |
888 | mVarT0=7.5; |
889 | break; |
890 | case SYS_FDC: |
891 | mVarT0=7.5; |
892 | break; |
893 | case SYS_BCAL: |
894 | mVarT0=0.25; |
895 | break; |
896 | default: |
897 | mVarT0=0.09; |
898 | break; |
899 | } |
900 | } |
901 | |
902 | //_DBG_ << SystemName(input_params.t0_detector()) << " " << mT0 <<endl; |
903 | |
904 | //Set the mass |
905 | MASS=input_params.mass(); |
906 | mass2=MASS*MASS; |
907 | m_ratio=ELECTRON_MASS0.000511/MASS; |
908 | m_ratio_sq=m_ratio*m_ratio; |
909 | |
910 | // Is this particle an electron or positron? |
911 | if (MASS<0.001){ |
912 | IsHadron=false; |
913 | if (input_params.charge()<0.) IsElectron=true; |
914 | else IsPositron=true; |
915 | } |
916 | if (DEBUG_LEVEL>0) |
917 | { |
918 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<918<<" " << "------Starting " |
919 | <<(fit_type==kTimeBased?"Time-based":"Wire-based") |
920 | << " Fit with " << my_fdchits.size() << " FDC hits and " |
921 | << my_cdchits.size() << " CDC hits.-------" <<endl; |
922 | if (fit_type==kTimeBased){ |
923 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<923<<" " << " Using t0=" << mT0 << " from DET=" |
924 | << input_params.t0_detector() <<endl; |
925 | } |
926 | } |
927 | // Do the fit |
928 | jerror_t error = KalmanLoop(); |
929 | if (error!=NOERROR){ |
930 | if (DEBUG_LEVEL>0) |
931 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<931<<" " << "Fit failed with Error = " << error <<endl; |
932 | return kFitFailed; |
933 | } |
934 | |
935 | // Copy fit results into DTrackFitter base-class data members |
936 | DVector3 mom,pos; |
937 | GetPosition(pos); |
938 | GetMomentum(mom); |
939 | double charge = GetCharge(); |
940 | fit_params.setPosition(pos); |
941 | fit_params.setMomentum(mom); |
942 | fit_params.setTime(mT0MinimumDriftTime); |
943 | fit_params.setPID(IDTrack(charge, MASS)); |
944 | fit_params.setT0(mT0MinimumDriftTime,4.,mT0Detector); |
945 | |
946 | if (DEBUG_LEVEL>0){ |
947 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<947<<" " << "----- Pass: " |
948 | << (fit_type==kTimeBased?"Time-based ---":"Wire-based ---") |
949 | << " Mass: " << MASS |
950 | << " p=" << mom.Mag() |
951 | << " theta=" << 90.0-180./M_PI3.14159265358979323846*atan(tanl_) |
952 | << " vertex=(" << x_ << "," << y_ << "," << z_<<")" |
953 | << " chi2=" << chisq_ |
954 | <<endl; |
955 | if(DEBUG_LEVEL>3){ |
956 | //Dump pulls |
957 | for (unsigned int iPull = 0; iPull < pulls.size(); iPull++){ |
958 | if (pulls[iPull].cdc_hit != NULL__null){ |
959 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<959<<" " << " ring: " << pulls[iPull].cdc_hit->wire->ring |
960 | << " straw: " << pulls[iPull].cdc_hit->wire->straw |
961 | << " Residual: " << pulls[iPull].resi |
962 | << " Err: " << pulls[iPull].err |
963 | << " tdrift: " << pulls[iPull].tdrift |
964 | << " doca: " << pulls[iPull].d |
965 | << " docaphi: " << pulls[iPull].docaphi |
966 | << " z: " << pulls[iPull].z |
967 | << " cos(theta_rel): " << pulls[iPull].cosThetaRel |
968 | << " tcorr: " << pulls[iPull].tcorr |
969 | << endl; |
970 | } |
971 | } |
972 | } |
973 | } |
974 | |
975 | DMatrixDSym errMatrix(5); |
976 | // Fill the tracking error matrix and the one needed for kinematic fitting |
977 | if (fcov.size()!=0){ |
978 | // We MUST fill the entire matrix (not just upper right) even though |
979 | // this is a DMatrixDSym |
980 | for (unsigned int i=0;i<5;i++){ |
981 | for (unsigned int j=0;j<5;j++){ |
982 | errMatrix(i,j)=fcov[i][j]; |
983 | } |
984 | } |
985 | if (FORWARD_PARMS_COV){ |
986 | fit_params.setForwardParmFlag(true); |
987 | fit_params.setTrackingStateVector(x_,y_,tx_,ty_,q_over_p_); |
988 | |
989 | // Compute and fill the error matrix needed for kinematic fitting |
990 | fit_params.setErrorMatrix(Get7x7ErrorMatrixForward(errMatrix)); |
991 | } |
992 | else { |
993 | fit_params.setForwardParmFlag(false); |
994 | fit_params.setTrackingStateVector(q_over_pt_,phi_,tanl_,D_,z_); |
995 | |
996 | // Compute and fill the error matrix needed for kinematic fitting |
997 | fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix)); |
998 | } |
999 | } |
1000 | else if (cov.size()!=0){ |
1001 | fit_params.setForwardParmFlag(false); |
1002 | |
1003 | // We MUST fill the entire matrix (not just upper right) even though |
1004 | // this is a DMatrixDSym |
1005 | for (unsigned int i=0;i<5;i++){ |
1006 | for (unsigned int j=0;j<5;j++){ |
1007 | errMatrix(i,j)=cov[i][j]; |
1008 | } |
1009 | } |
1010 | fit_params.setTrackingStateVector(q_over_pt_,phi_,tanl_,D_,z_); |
1011 | |
1012 | // Compute and fill the error matrix needed for kinematic fitting |
1013 | fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix)); |
1014 | } |
1015 | auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource(); |
1016 | locTrackingCovarianceMatrix->ResizeTo(5, 5); |
1017 | for(unsigned int loc_i = 0; loc_i < 5; ++loc_i) |
1018 | { |
1019 | for(unsigned int loc_j = 0; loc_j < 5; ++loc_j) |
1020 | (*locTrackingCovarianceMatrix)(loc_i, loc_j) = errMatrix(loc_i, loc_j); |
1021 | |
1022 | } |
1023 | fit_params.setTrackingErrorMatrix(locTrackingCovarianceMatrix); |
1024 | this->chisq = GetChiSq(); |
1025 | this->Ndof = GetNDF(); |
1026 | fit_status = kFitSuccess; |
1027 | |
1028 | //_DBG_ << "========= done!" << endl; |
1029 | |
1030 | return fit_status; |
1031 | } |
1032 | |
1033 | //----------------- |
1034 | // ChiSq |
1035 | //----------------- |
1036 | double DTrackFitterKalmanSIMD::ChiSq(fit_type_t fit_type, DReferenceTrajectory *rt, double *chisq_ptr, int *dof_ptr, vector<pull_t> *pulls_ptr) |
1037 | { |
1038 | // This simply returns whatever was left in for the chisq/NDF from the last fit. |
1039 | // Using a DReferenceTrajectory is not really appropriate here so the base class' |
1040 | // requirement of it should be reviewed. |
1041 | double chisq = GetChiSq(); |
1042 | unsigned int ndf = GetNDF(); |
1043 | |
1044 | if(chisq_ptr)*chisq_ptr = chisq; |
1045 | if(dof_ptr)*dof_ptr = int(ndf); |
1046 | if(pulls_ptr)*pulls_ptr = pulls; |
1047 | |
1048 | return chisq/double(ndf); |
1049 | } |
1050 | |
1051 | // Return the momentum at the distance of closest approach to the origin. |
1052 | inline void DTrackFitterKalmanSIMD::GetMomentum(DVector3 &mom){ |
1053 | double pt=1./fabs(q_over_pt_); |
1054 | mom.SetXYZ(pt*cos(phi_),pt*sin(phi_),pt*tanl_); |
1055 | } |
1056 | |
1057 | // Return the "vertex" position (position at which track crosses beam line) |
1058 | inline void DTrackFitterKalmanSIMD::GetPosition(DVector3 &pos){ |
1059 | DVector2 beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
1060 | pos.SetXYZ(x_+beam_pos.X(),y_+beam_pos.Y(),z_); |
1061 | } |
1062 | |
1063 | // Add GEM points |
1064 | void DTrackFitterKalmanSIMD::AddGEMHit(const DGEMPoint *gemhit){ |
1065 | DKalmanSIMDFDCHit_t *hit= new DKalmanSIMDFDCHit_t; |
1066 | |
1067 | hit->t=gemhit->time; |
1068 | hit->uwire=gemhit->x; |
1069 | hit->vstrip=gemhit->y; |
1070 | // From Justin (12/12/19): |
1071 | // DGEMPoint (GEM2 SRS, plane closest to the DIRC): |
1072 | // sigma_X = sigma_Y = 100 um |
1073 | hit->vvar=0.01*0.01; |
1074 | hit->uvar=hit->vvar; |
1075 | hit->z=gemhit->z; |
1076 | hit->cosa=1.; |
1077 | hit->sina=0.; |
1078 | hit->phiX=0.; |
1079 | hit->phiY=0.; |
1080 | hit->phiZ=0.; |
1081 | hit->nr=0.; |
1082 | hit->nz=0.; |
1083 | hit->status=gem_hit; |
1084 | hit->hit=NULL__null; |
1085 | |
1086 | my_fdchits.push_back(hit); |
1087 | } |
1088 | |
1089 | |
1090 | |
1091 | // Add TRD points |
1092 | void DTrackFitterKalmanSIMD::AddTRDHit(const DTRDPoint *trdhit){ |
1093 | DKalmanSIMDFDCHit_t *hit= new DKalmanSIMDFDCHit_t; |
1094 | |
1095 | hit->t=trdhit->time; |
1096 | hit->uwire=trdhit->x; |
1097 | hit->vstrip=trdhit->y; |
1098 | // From Justin (12/12/19): |
1099 | // sigma_X = 1 mm / sqrt(12) |
1100 | // sigma_Y = 300 um |
1101 | hit->vvar=0.03*0.03; |
1102 | hit->uvar=0.1*0.1/12.; |
1103 | hit->z=trdhit->z; |
1104 | hit->cosa=1.; |
1105 | hit->sina=0.; |
1106 | hit->phiX=0.; |
1107 | hit->phiY=0.; |
1108 | hit->phiZ=0.; |
1109 | hit->nr=0.; |
1110 | hit->nz=0.; |
1111 | hit->status=trd_hit; |
1112 | hit->hit=NULL__null; |
1113 | |
1114 | my_fdchits.push_back(hit); |
1115 | } |
1116 | |
1117 | // Add FDC hits |
1118 | void DTrackFitterKalmanSIMD::AddFDCHit(const DFDCPseudo *fdchit){ |
1119 | DKalmanSIMDFDCHit_t *hit= new DKalmanSIMDFDCHit_t; |
1120 | |
1121 | hit->t=fdchit->time; |
1122 | hit->uwire=fdchit->w; |
1123 | hit->vstrip=fdchit->s; |
1124 | hit->uvar=0.0833; |
1125 | hit->vvar=fdchit->ds*fdchit->ds; |
1126 | hit->z=fdchit->wire->origin.z(); |
1127 | hit->cosa=cos(fdchit->wire->angle); |
1128 | hit->sina=sin(fdchit->wire->angle); |
1129 | hit->phiX=fdchit->wire->angles.X(); |
1130 | hit->phiY=fdchit->wire->angles.Y(); |
1131 | hit->phiZ=fdchit->wire->angles.Z(); |
1132 | |
1133 | hit->nr=0.; |
1134 | hit->nz=0.; |
1135 | hit->dE=1e6*fdchit->dE; |
1136 | hit->hit=fdchit; |
1137 | hit->status=good_hit; |
1138 | |
1139 | my_fdchits.push_back(hit); |
1140 | } |
1141 | |
1142 | // Add CDC hits |
1143 | void DTrackFitterKalmanSIMD::AddCDCHit (const DCDCTrackHit *cdchit){ |
1144 | DKalmanSIMDCDCHit_t *hit= new DKalmanSIMDCDCHit_t; |
1145 | |
1146 | hit->hit=cdchit; |
1147 | hit->status=good_hit; |
1148 | hit->origin.Set(cdchit->wire->origin.x(),cdchit->wire->origin.y()); |
1149 | double one_over_uz=1./cdchit->wire->udir.z(); |
1150 | hit->dir.Set(one_over_uz*cdchit->wire->udir.x(), |
1151 | one_over_uz*cdchit->wire->udir.y()); |
1152 | hit->z0wire=cdchit->wire->origin.z(); |
1153 | hit->cosstereo=cos(cdchit->wire->stereo); |
1154 | hit->tdrift=cdchit->tdrift; |
1155 | my_cdchits.push_back(hit); |
1156 | } |
1157 | |
1158 | // Calculate the derivative of the state vector with respect to z |
1159 | jerror_t DTrackFitterKalmanSIMD::CalcDeriv(double z, |
1160 | const DMatrix5x1 &S, |
1161 | double dEdx, |
1162 | DMatrix5x1 &D){ |
1163 | double tx=S(state_tx),ty=S(state_ty); |
1164 | double q_over_p=S(state_q_over_p); |
1165 | |
1166 | // Turn off dEdx if the magnitude of the momentum drops below some cutoff |
1167 | if (fabs(q_over_p)>Q_OVER_P_MAX){ |
1168 | dEdx=0.; |
1169 | } |
1170 | // Try to keep the direction tangents from heading towards 90 degrees |
1171 | if (fabs(tx)>TAN_MAX10.) tx=TAN_MAX10.*(tx>0.0?1.:-1.); |
1172 | if (fabs(ty)>TAN_MAX10.) ty=TAN_MAX10.*(ty>0.0?1.:-1.); |
1173 | |
1174 | // useful combinations of terms |
1175 | double kq_over_p=qBr2p0.003*q_over_p; |
1176 | double tx2=tx*tx; |
1177 | double ty2=ty*ty; |
1178 | double txty=tx*ty; |
1179 | double one_plus_tx2=1.+tx2; |
1180 | double dsdz=sqrt(one_plus_tx2+ty2); |
1181 | double dtx_Bfac=ty*Bz+txty*Bx-one_plus_tx2*By; |
1182 | double dty_Bfac=Bx*(1.+ty2)-txty*By-tx*Bz; |
1183 | double kq_over_p_dsdz=kq_over_p*dsdz; |
1184 | |
1185 | // Derivative of S with respect to z |
1186 | D(state_x)=tx; |
1187 | D(state_y)=ty; |
1188 | D(state_tx)=kq_over_p_dsdz*dtx_Bfac; |
1189 | D(state_ty)=kq_over_p_dsdz*dty_Bfac; |
1190 | |
1191 | D(state_q_over_p)=0.; |
1192 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
1193 | double q_over_p_sq=q_over_p*q_over_p; |
1194 | double E=sqrt(1./q_over_p_sq+mass2); |
1195 | D(state_q_over_p)=-q_over_p_sq*q_over_p*E*dEdx*dsdz; |
1196 | } |
1197 | return NOERROR; |
1198 | } |
1199 | |
1200 | // Reference trajectory for forward tracks in CDC region |
1201 | // At each point we store the state vector and the Jacobian needed to get to |
1202 | //this state along z from the previous state. |
1203 | jerror_t DTrackFitterKalmanSIMD::SetCDCForwardReferenceTrajectory(DMatrix5x1 &S){ |
1204 | int i=0,forward_traj_length=forward_traj.size(); |
1205 | double z=z_; |
1206 | double r2=0.; |
1207 | bool stepped_to_boundary=false; |
1208 | |
1209 | // Magnetic field and gradient at beginning of trajectory |
1210 | //bfield->GetField(x_,y_,z_,Bx,By,Bz); |
1211 | bfield->GetFieldAndGradient(x_,y_,z_,Bx,By,Bz, |
1212 | dBxdx,dBxdy,dBxdz,dBydx, |
1213 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1214 | |
1215 | // Reset cdc status flags |
1216 | for (unsigned int i=0;i<my_cdchits.size();i++){ |
1217 | if (my_cdchits[i]->status!=late_hit) my_cdchits[i]->status=good_hit; |
1218 | } |
1219 | |
1220 | // Continue adding to the trajectory until we have reached the endplate |
1221 | // or the maximum radius |
1222 | while(z<endplate_z_downstream && z>cdc_origin[2] && |
1223 | r2<endplate_r2max && fabs(S(state_q_over_p))<Q_OVER_P_MAX |
1224 | && fabs(S(state_tx))<TAN_MAX10. |
1225 | && fabs(S(state_ty))<TAN_MAX10. |
1226 | ){ |
1227 | if (PropagateForwardCDC(forward_traj_length,i,z,r2,S,stepped_to_boundary) |
1228 | !=NOERROR) return UNRECOVERABLE_ERROR; |
1229 | } |
1230 | |
1231 | // Only use hits that would fall within the range of the reference trajectory |
1232 | /* |
1233 | for (unsigned int i=0;i<my_cdchits.size();i++){ |
1234 | DKalmanSIMDCDCHit_t *hit=my_cdchits[i]; |
1235 | double my_r2=(hit->origin+(z-hit->z0wire)*hit->dir).Mod2(); |
1236 | if (my_r2>r2) hit->status=bad_hit; |
1237 | } |
1238 | */ |
1239 | |
1240 | // If the current length of the trajectory deque is less than the previous |
1241 | // trajectory deque, remove the extra elements and shrink the deque |
1242 | if (i<(int)forward_traj.size()){ |
1243 | forward_traj_length=forward_traj.size(); |
1244 | for (int j=0;j<forward_traj_length-i;j++){ |
1245 | forward_traj.pop_front(); |
1246 | } |
1247 | } |
1248 | |
1249 | // return an error if there are not enough entries in the trajectory |
1250 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
1251 | |
1252 | if (DEBUG_LEVEL>20) |
1253 | { |
1254 | cout << "--- Forward cdc trajectory ---" <<endl; |
1255 | for (unsigned int m=0;m<forward_traj.size();m++){ |
1256 | // DMatrix5x1 S=*(forward_traj[m].S); |
1257 | DMatrix5x1 S=(forward_traj[m].S); |
1258 | double tx=S(state_tx),ty=S(state_ty); |
1259 | double phi=atan2(ty,tx); |
1260 | double cosphi=cos(phi); |
1261 | double sinphi=sin(phi); |
1262 | double p=fabs(1./S(state_q_over_p)); |
1263 | double tanl=1./sqrt(tx*tx+ty*ty); |
1264 | double sinl=sin(atan(tanl)); |
1265 | double cosl=cos(atan(tanl)); |
1266 | cout |
1267 | << setiosflags(ios::fixed)<< "pos: " << setprecision(4) |
1268 | << forward_traj[m].S(state_x) << ", " |
1269 | << forward_traj[m].S(state_y) << ", " |
1270 | << forward_traj[m].z << " mom: " |
1271 | << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", " |
1272 | << p*sinl << " -> " << p |
1273 | <<" s: " << setprecision(3) |
1274 | << forward_traj[m].s |
1275 | <<" t: " << setprecision(3) |
1276 | << forward_traj[m].t/SPEED_OF_LIGHT29.9792458 |
1277 | <<" B: " << forward_traj[m].B |
1278 | << endl; |
1279 | } |
1280 | } |
1281 | |
1282 | // Current state vector |
1283 | S=forward_traj[0].S; |
1284 | |
1285 | // position at the end of the swim |
1286 | x_=forward_traj[0].S(state_x); |
1287 | y_=forward_traj[0].S(state_y); |
1288 | z_=forward_traj[0].z; |
1289 | |
1290 | return NOERROR; |
1291 | } |
1292 | |
1293 | // Routine that extracts the state vector propagation part out of the reference |
1294 | // trajectory loop |
1295 | jerror_t DTrackFitterKalmanSIMD::PropagateForwardCDC(int length,int &index, |
1296 | double &z,double &r2, |
1297 | DMatrix5x1 &S, |
1298 | bool &stepped_to_boundary){ |
1299 | DMatrix5x5 J,Q; |
1300 | DKalmanForwardTrajectory_t temp; |
1301 | int my_i=0; |
1302 | temp.h_id=0; |
1303 | temp.num_hits=0; |
1304 | double dEdx=0.; |
1305 | double s_to_boundary=1e6; |
1306 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
1307 | |
1308 | // current position |
1309 | DVector3 pos(S(state_x),S(state_y),z); |
1310 | temp.z=z; |
1311 | // squared radius |
1312 | r2=pos.Perp2(); |
1313 | |
1314 | temp.s=len; |
1315 | temp.t=ftime; |
1316 | temp.S=S; |
1317 | |
1318 | // Kinematic variables |
1319 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
1320 | double one_over_beta2=1.+mass2*q_over_p_sq; |
1321 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
1322 | |
1323 | // get material properties from the Root Geometry |
1324 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
1325 | DVector3 mom(S(state_tx),S(state_ty),1.); |
1326 | if(geom->FindMatKalman(pos,mom,temp.K_rho_Z_over_A, |
1327 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1328 | temp.chi2c_factor,temp.chi2a_factor, |
1329 | temp.chi2a_corr, |
1330 | last_material_map, |
1331 | &s_to_boundary)!=NOERROR){ |
1332 | return UNRECOVERABLE_ERROR; |
1333 | } |
1334 | } |
1335 | else |
1336 | { |
1337 | if(geom->FindMatKalman(pos,temp.K_rho_Z_over_A, |
1338 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1339 | temp.chi2c_factor,temp.chi2a_factor, |
1340 | temp.chi2a_corr, |
1341 | last_material_map)!=NOERROR){ |
1342 | return UNRECOVERABLE_ERROR; |
1343 | } |
1344 | } |
1345 | |
1346 | // Get dEdx for the upcoming step |
1347 | if (CORRECT_FOR_ELOSS){ |
1348 | dEdx=GetdEdx(S(state_q_over_p),temp.K_rho_Z_over_A,temp.rho_Z_over_A, |
1349 | temp.LnI,temp.Z); |
1350 | } |
1351 | |
1352 | index++; |
1353 | if (index<=length){ |
1354 | my_i=length-index; |
1355 | forward_traj[my_i].s=temp.s; |
1356 | forward_traj[my_i].t=temp.t; |
1357 | forward_traj[my_i].h_id=temp.h_id; |
1358 | forward_traj[my_i].num_hits=0; |
1359 | forward_traj[my_i].z=temp.z; |
1360 | forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A; |
1361 | forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A; |
1362 | forward_traj[my_i].LnI=temp.LnI; |
1363 | forward_traj[my_i].Z=temp.Z; |
1364 | forward_traj[my_i].S=S; |
1365 | } |
1366 | |
1367 | // Determine the step size based on energy loss |
1368 | //double step=mStepSizeS*dz_ds; |
1369 | double max_step_size |
1370 | =(z<endplate_z&& r2>endplate_r2min)?mCDCInternalStepSize:mStepSizeS; |
1371 | double ds=mStepSizeS; |
1372 | if (z<endplate_z && r2<endplate_r2max && z>cdc_origin[2]){ |
1373 | if (!stepped_to_boundary){ |
1374 | stepped_to_boundary=false; |
1375 | if (fabs(dEdx)>EPS3.0e-8){ |
1376 | ds=DE_PER_STEP0.001/fabs(dEdx); |
1377 | } |
1378 | if (ds>max_step_size) ds=max_step_size; |
1379 | if (s_to_boundary<ds){ |
1380 | ds=s_to_boundary+EPS31.e-2; |
1381 | stepped_to_boundary=true; |
1382 | } |
1383 | if(ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1; |
1384 | } |
1385 | else{ |
1386 | ds=MIN_STEP_SIZE0.1; |
1387 | stepped_to_boundary=false; |
1388 | } |
1389 | } |
1390 | double newz=z+ds*dz_ds; // new z position |
1391 | |
1392 | // Store magnetic field |
1393 | temp.B=sqrt(Bx*Bx+By*By+Bz*Bz); |
1394 | |
1395 | // Step through field |
1396 | ds=FasterStep(z,newz,dEdx,S); |
1397 | |
1398 | // update path length |
1399 | len+=fabs(ds); |
1400 | |
1401 | // Update flight time |
1402 | ftime+=ds*sqrt(one_over_beta2);// in units where c=1 |
1403 | |
1404 | // Get the contribution to the covariance matrix due to multiple |
1405 | // scattering |
1406 | GetProcessNoise(z,ds,temp.chi2c_factor,temp.chi2a_factor,temp.chi2a_corr, |
1407 | temp.S,Q); |
1408 | |
1409 | // Energy loss straggling |
1410 | if (CORRECT_FOR_ELOSS){ |
1411 | double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A); |
1412 | Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2; |
1413 | } |
1414 | |
1415 | // Compute the Jacobian matrix and its transpose |
1416 | StepJacobian(newz,z,S,dEdx,J); |
1417 | |
1418 | // update the trajectory |
1419 | if (index<=length){ |
1420 | forward_traj[my_i].B=temp.B; |
1421 | forward_traj[my_i].Q=Q; |
1422 | forward_traj[my_i].J=J; |
1423 | } |
1424 | else{ |
1425 | temp.Q=Q; |
1426 | temp.J=J; |
1427 | temp.Ckk=Zero5x5; |
1428 | temp.Skk=Zero5x1; |
1429 | forward_traj.push_front(temp); |
1430 | } |
1431 | |
1432 | //update z |
1433 | z=newz; |
1434 | |
1435 | return NOERROR; |
1436 | } |
1437 | |
1438 | // Routine that extracts the state vector propagation part out of the reference |
1439 | // trajectory loop |
1440 | jerror_t DTrackFitterKalmanSIMD::PropagateCentral(int length, int &index, |
1441 | DVector2 &my_xy, |
1442 | double &var_t_factor, |
1443 | DMatrix5x1 &Sc, |
1444 | bool &stepped_to_boundary){ |
1445 | DKalmanCentralTrajectory_t temp; |
1446 | DMatrix5x5 J; // State vector Jacobian matrix |
1447 | DMatrix5x5 Q; // Process noise covariance matrix |
1448 | |
1449 | //Initialize some variables needed later |
1450 | double dEdx=0.; |
1451 | double s_to_boundary=1e6; |
1452 | int my_i=0; |
1453 | // Kinematic variables |
1454 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
1455 | double q_over_p_sq=q_over_p*q_over_p; |
1456 | double one_over_beta2=1.+mass2*q_over_p_sq; |
1457 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
1458 | |
1459 | // Reset D to zero |
1460 | Sc(state_D)=0.; |
1461 | |
1462 | temp.xy=my_xy; |
1463 | temp.s=len; |
1464 | temp.t=ftime; |
1465 | temp.h_id=0; |
1466 | temp.S=Sc; |
1467 | |
1468 | // Store magnitude of magnetic field |
1469 | temp.B=sqrt(Bx*Bx+By*By+Bz*Bz); |
1470 | |
1471 | // get material properties from the Root Geometry |
1472 | DVector3 pos3d(my_xy.X(),my_xy.Y(),Sc(state_z)); |
1473 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
1474 | DVector3 mom(cos(Sc(state_phi)),sin(Sc(state_phi)),Sc(state_tanl)); |
1475 | if(geom->FindMatKalman(pos3d,mom,temp.K_rho_Z_over_A, |
1476 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1477 | temp.chi2c_factor,temp.chi2a_factor, |
1478 | temp.chi2a_corr, |
1479 | last_material_map, |
1480 | &s_to_boundary) |
1481 | !=NOERROR){ |
1482 | return UNRECOVERABLE_ERROR; |
1483 | } |
1484 | } |
1485 | else if(geom->FindMatKalman(pos3d,temp.K_rho_Z_over_A, |
1486 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1487 | temp.chi2c_factor,temp.chi2a_factor, |
1488 | temp.chi2a_corr, |
1489 | last_material_map)!=NOERROR){ |
1490 | return UNRECOVERABLE_ERROR; |
1491 | } |
1492 | |
1493 | if (CORRECT_FOR_ELOSS){ |
1494 | dEdx=GetdEdx(q_over_p,temp.K_rho_Z_over_A,temp.rho_Z_over_A,temp.LnI, |
1495 | temp.Z); |
1496 | } |
1497 | |
1498 | // If the deque already exists, update it |
1499 | index++; |
1500 | if (index<=length){ |
1501 | my_i=length-index; |
1502 | central_traj[my_i].B=temp.B; |
1503 | central_traj[my_i].s=temp.s; |
1504 | central_traj[my_i].t=temp.t; |
1505 | central_traj[my_i].h_id=0; |
1506 | central_traj[my_i].xy=temp.xy; |
1507 | central_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A; |
1508 | central_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A; |
1509 | central_traj[my_i].LnI=temp.LnI; |
1510 | central_traj[my_i].Z=temp.Z; |
1511 | central_traj[my_i].S=Sc; |
1512 | } |
1513 | |
1514 | // Adjust the step size |
1515 | double step_size=mStepSizeS; |
1516 | if (stepped_to_boundary){ |
1517 | step_size=MIN_STEP_SIZE0.1; |
1518 | stepped_to_boundary=false; |
1519 | } |
1520 | else{ |
1521 | if (fabs(dEdx)>EPS3.0e-8){ |
1522 | step_size=DE_PER_STEP0.001/fabs(dEdx); |
1523 | } |
1524 | if(step_size>mStepSizeS) step_size=mStepSizeS; |
1525 | if (s_to_boundary<step_size){ |
1526 | step_size=s_to_boundary+EPS31.e-2; |
1527 | stepped_to_boundary=true; |
1528 | } |
1529 | if(step_size<MIN_STEP_SIZE0.1)step_size=MIN_STEP_SIZE0.1; |
1530 | } |
1531 | double r2=my_xy.Mod2(); |
1532 | if (r2>endplate_r2min |
1533 | && step_size>mCDCInternalStepSize) step_size=mCDCInternalStepSize; |
1534 | // Propagate the state through the field |
1535 | FasterStep(my_xy,step_size,Sc,dEdx); |
1536 | |
1537 | // update path length |
1538 | len+=step_size; |
1539 | |
1540 | // Update flight time |
1541 | double dt=step_size*sqrt(one_over_beta2); // in units of c=1 |
1542 | double one_minus_beta2=1.-1./one_over_beta2; |
1543 | ftime+=dt; |
1544 | var_ftime+=dt*dt*one_minus_beta2*one_minus_beta2*0.0004; |
1545 | var_t_factor=dt*dt*one_minus_beta2*one_minus_beta2; |
1546 | |
1547 | //printf("t %f sigt %f\n",TIME_UNIT_CONVERSION*ftime,TIME_UNIT_CONVERSION*sqrt(var_ftime)); |
1548 | |
1549 | // Multiple scattering |
1550 | GetProcessNoiseCentral(step_size,temp.chi2c_factor,temp.chi2a_factor, |
1551 | temp.chi2a_corr,temp.S,Q); |
1552 | |
1553 | // Energy loss straggling in the approximation of thick absorbers |
1554 | if (CORRECT_FOR_ELOSS){ |
1555 | double varE |
1556 | =GetEnergyVariance(step_size,one_over_beta2,temp.K_rho_Z_over_A); |
1557 | Q(state_q_over_pt,state_q_over_pt) |
1558 | +=varE*temp.S(state_q_over_pt)*temp.S(state_q_over_pt)*one_over_beta2 |
1559 | *q_over_p_sq; |
1560 | } |
1561 | |
1562 | // B-field and gradient at current (x,y,z) |
1563 | bfield->GetFieldAndGradient(my_xy.X(),my_xy.Y(),Sc(state_z),Bx,By,Bz, |
1564 | dBxdx,dBxdy,dBxdz,dBydx, |
1565 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1566 | |
1567 | // Compute the Jacobian matrix and its transpose |
1568 | StepJacobian(my_xy,temp.xy-my_xy,-step_size,Sc,dEdx,J); |
1569 | |
1570 | // Update the trajectory info |
1571 | if (index<=length){ |
1572 | central_traj[my_i].Q=Q; |
1573 | central_traj[my_i].J=J; |
1574 | } |
1575 | else{ |
1576 | temp.Q=Q; |
1577 | temp.J=J; |
1578 | temp.Ckk=Zero5x5; |
1579 | temp.Skk=Zero5x1; |
1580 | central_traj.push_front(temp); |
1581 | } |
1582 | |
1583 | return NOERROR; |
1584 | } |
1585 | |
1586 | |
1587 | |
1588 | // Reference trajectory for central tracks |
1589 | // At each point we store the state vector and the Jacobian needed to get to this state |
1590 | // along s from the previous state. |
1591 | // The tricky part is that we swim out from the target to find Sc and pos along the trajectory |
1592 | // but we need the Jacobians for the opposite direction, because the filter proceeds from |
1593 | // the outer hits toward the target. |
1594 | jerror_t DTrackFitterKalmanSIMD::SetCDCReferenceTrajectory(const DVector2 &xy, |
1595 | DMatrix5x1 &Sc){ |
1596 | bool stepped_to_boundary=false; |
1597 | int i=0,central_traj_length=central_traj.size(); |
1598 | // factor for scaling momentum resolution to propagate variance in flight |
1599 | // time |
1600 | double var_t_factor=0; |
1601 | |
1602 | // Magnetic field and gradient at beginning of trajectory |
1603 | //bfield->GetField(x_,y_,z_,Bx,By,Bz); |
1604 | bfield->GetFieldAndGradient(x_,y_,z_,Bx,By,Bz, |
1605 | dBxdx,dBxdy,dBxdz,dBydx, |
1606 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1607 | |
1608 | // Copy of initial position in xy |
1609 | DVector2 my_xy=xy; |
1610 | double r2=xy.Mod2(),z=z_; |
1611 | |
1612 | // Reset cdc status flags |
1613 | for (unsigned int j=0;j<my_cdchits.size();j++){ |
1614 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
1615 | } |
1616 | |
1617 | // Continue adding to the trajectory until we have reached the endplate |
1618 | // or the maximum radius |
1619 | while(z<endplate_z && z>=Z_MIN-100. && r2<endplate_r2max |
1620 | && fabs(Sc(state_q_over_pt))<Q_OVER_PT_MAX100. |
1621 | ){ |
1622 | if (PropagateCentral(central_traj_length,i,my_xy,var_t_factor,Sc,stepped_to_boundary) |
1623 | !=NOERROR) return UNRECOVERABLE_ERROR; |
1624 | z=Sc(state_z); |
1625 | r2=my_xy.Mod2(); |
1626 | } |
1627 | |
1628 | // If the current length of the trajectory deque is less than the previous |
1629 | // trajectory deque, remove the extra elements and shrink the deque |
1630 | if (i<(int)central_traj.size()){ |
1631 | int central_traj_length=central_traj.size(); |
1632 | for (int j=0;j<central_traj_length-i;j++){ |
1633 | central_traj.pop_front(); |
1634 | } |
1635 | } |
1636 | |
1637 | // Only use hits that would fall within the range of the reference trajectory |
1638 | /*for (unsigned int j=0;j<my_cdchits.size();j++){ |
1639 | DKalmanSIMDCDCHit_t *hit=my_cdchits[j]; |
1640 | double my_r2=(hit->origin+(z-hit->z0wire)*hit->dir).Mod2(); |
1641 | if (my_r2>r2) hit->status=bad_hit; |
1642 | } |
1643 | */ |
1644 | |
1645 | // return an error if there are not enough entries in the trajectory |
1646 | if (central_traj.size()<2) return RESOURCE_UNAVAILABLE; |
1647 | |
1648 | if (DEBUG_LEVEL>20) |
1649 | { |
1650 | cout << "---------" << central_traj.size() <<" entries------" <<endl; |
1651 | for (unsigned int m=0;m<central_traj.size();m++){ |
1652 | DMatrix5x1 S=central_traj[m].S; |
1653 | double cosphi=cos(S(state_phi)); |
1654 | double sinphi=sin(S(state_phi)); |
1655 | double pt=fabs(1./S(state_q_over_pt)); |
1656 | double tanl=S(state_tanl); |
1657 | |
1658 | cout |
1659 | << m << "::" |
1660 | << setiosflags(ios::fixed)<< "pos: " << setprecision(4) |
1661 | << central_traj[m].xy.X() << ", " |
1662 | << central_traj[m].xy.Y() << ", " |
1663 | << central_traj[m].S(state_z) << " mom: " |
1664 | << pt*cosphi << ", " << pt*sinphi << ", " |
1665 | << pt*tanl << " -> " << pt/cos(atan(tanl)) |
1666 | <<" s: " << setprecision(3) |
1667 | << central_traj[m].s |
1668 | <<" t: " << setprecision(3) |
1669 | << central_traj[m].t/SPEED_OF_LIGHT29.9792458 |
1670 | <<" B: " << central_traj[m].B |
1671 | << endl; |
1672 | } |
1673 | } |
1674 | |
1675 | // State at end of swim |
1676 | Sc=central_traj[0].S; |
1677 | |
1678 | return NOERROR; |
1679 | } |
1680 | |
1681 | // Routine that extracts the state vector propagation part out of the reference |
1682 | // trajectory loop |
1683 | jerror_t DTrackFitterKalmanSIMD::PropagateForward(int length,int &i, |
1684 | double &z,double zhit, |
1685 | DMatrix5x1 &S, bool &done, |
1686 | bool &stepped_to_boundary, |
1687 | bool &stepped_to_endplate){ |
1688 | DMatrix5x5 J,Q; |
1689 | DKalmanForwardTrajectory_t temp; |
1690 | |
1691 | // Initialize some variables |
1692 | temp.h_id=0; |
1693 | temp.num_hits=0; |
1694 | int my_i=0; |
1695 | double s_to_boundary=1e6; |
1696 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
1697 | |
1698 | // current position |
1699 | DVector3 pos(S(state_x),S(state_y),z); |
1700 | double r2=pos.Perp2(); |
1701 | |
1702 | temp.s=len; |
1703 | temp.t=ftime; |
1704 | temp.z=z; |
1705 | temp.S=S; |
1706 | |
1707 | // Kinematic variables |
1708 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
1709 | double one_over_beta2=1.+mass2*q_over_p_sq; |
1710 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
1711 | |
1712 | // get material properties from the Root Geometry |
1713 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
1714 | DVector3 mom(S(state_tx),S(state_ty),1.); |
1715 | if (geom->FindMatKalman(pos,mom,temp.K_rho_Z_over_A, |
1716 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1717 | temp.chi2c_factor,temp.chi2a_factor, |
1718 | temp.chi2a_corr, |
1719 | last_material_map, |
1720 | &s_to_boundary) |
1721 | !=NOERROR){ |
1722 | return UNRECOVERABLE_ERROR; |
1723 | } |
1724 | } |
1725 | else |
1726 | { |
1727 | if (geom->FindMatKalman(pos,temp.K_rho_Z_over_A, |
1728 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1729 | temp.chi2c_factor,temp.chi2a_factor, |
1730 | temp.chi2a_corr, |
1731 | last_material_map)!=NOERROR){ |
1732 | return UNRECOVERABLE_ERROR; |
1733 | } |
1734 | } |
1735 | |
1736 | // Get dEdx for the upcoming step |
1737 | double dEdx=0.; |
1738 | if (CORRECT_FOR_ELOSS){ |
1739 | dEdx=GetdEdx(S(state_q_over_p),temp.K_rho_Z_over_A, |
1740 | temp.rho_Z_over_A,temp.LnI,temp.Z); |
1741 | } |
1742 | i++; |
1743 | my_i=length-i; |
1744 | if (i<=length){ |
1745 | forward_traj[my_i].s=temp.s; |
1746 | forward_traj[my_i].t=temp.t; |
1747 | forward_traj[my_i].h_id=temp.h_id; |
1748 | forward_traj[my_i].num_hits=temp.num_hits; |
1749 | forward_traj[my_i].z=temp.z; |
1750 | forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A; |
1751 | forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A; |
1752 | forward_traj[my_i].LnI=temp.LnI; |
1753 | forward_traj[my_i].Z=temp.Z; |
1754 | forward_traj[my_i].S=S; |
1755 | } |
1756 | else{ |
1757 | temp.S=S; |
1758 | } |
1759 | |
1760 | // Determine the step size based on energy loss |
1761 | // step=mStepSizeS*dz_ds; |
1762 | double max_step_size |
1763 | =(z<endplate_z&& r2>endplate_r2min)?mCentralStepSize:mStepSizeS; |
1764 | double ds=mStepSizeS; |
1765 | if (z>cdc_origin[2]){ |
1766 | if (!stepped_to_boundary){ |
1767 | stepped_to_boundary=false; |
1768 | if (fabs(dEdx)>EPS3.0e-8){ |
1769 | ds=DE_PER_STEP0.001/fabs(dEdx); |
1770 | } |
1771 | if (ds>max_step_size) ds=max_step_size; |
1772 | if (s_to_boundary<ds){ |
1773 | ds=s_to_boundary+EPS31.e-2; |
1774 | stepped_to_boundary=true; |
1775 | } |
1776 | if(ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1; |
1777 | |
1778 | } |
1779 | else{ |
1780 | ds=MIN_STEP_SIZE0.1; |
1781 | stepped_to_boundary=false; |
1782 | } |
1783 | } |
1784 | |
1785 | double dz=stepped_to_endplate ? endplate_dz : ds*dz_ds; |
1786 | double newz=z+dz; // new z position |
1787 | // Check if we are stepping through the CDC endplate |
1788 | if (newz>endplate_z && z<endplate_z){ |
1789 | // _DBG_ << endl; |
1790 | newz=endplate_z+EPS31.e-2; |
1791 | stepped_to_endplate=true; |
1792 | } |
1793 | |
1794 | // Check if we are about to step to one of the wire planes |
1795 | done=false; |
1796 | if (newz>zhit){ |
1797 | newz=zhit; |
1798 | done=true; |
1799 | } |
1800 | |
1801 | // Store magnitude of magnetic field |
1802 | temp.B=sqrt(Bx*Bx+By*By+Bz*Bz); |
1803 | |
1804 | // Step through field |
1805 | ds=FasterStep(z,newz,dEdx,S); |
1806 | |
1807 | // update path length |
1808 | len+=ds; |
1809 | |
1810 | // update flight time |
1811 | ftime+=ds*sqrt(one_over_beta2); // in units where c=1 |
1812 | |
1813 | // Get the contribution to the covariance matrix due to multiple |
1814 | // scattering |
1815 | GetProcessNoise(z,ds,temp.chi2c_factor,temp.chi2a_factor,temp.chi2a_corr, |
1816 | temp.S,Q); |
1817 | |
1818 | // Energy loss straggling |
1819 | if (CORRECT_FOR_ELOSS){ |
1820 | double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A); |
1821 | Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2; |
1822 | } |
1823 | |
1824 | // Compute the Jacobian matrix and its transpose |
1825 | StepJacobian(newz,z,S,dEdx,J); |
1826 | |
1827 | // update the trajectory data |
1828 | if (i<=length){ |
1829 | forward_traj[my_i].B=temp.B; |
1830 | forward_traj[my_i].Q=Q; |
1831 | forward_traj[my_i].J=J; |
1832 | } |
1833 | else{ |
1834 | temp.Q=Q; |
1835 | temp.J=J; |
1836 | temp.Ckk=Zero5x5; |
1837 | temp.Skk=Zero5x1; |
1838 | forward_traj.push_front(temp); |
1839 | } |
1840 | |
1841 | // update z |
1842 | z=newz; |
1843 | |
1844 | return NOERROR; |
1845 | } |
1846 | |
1847 | // Reference trajectory for trajectories with hits in the forward direction |
1848 | // At each point we store the state vector and the Jacobian needed to get to this state |
1849 | // along z from the previous state. |
1850 | jerror_t DTrackFitterKalmanSIMD::SetReferenceTrajectory(DMatrix5x1 &S){ |
1851 | |
1852 | // Magnetic field and gradient at beginning of trajectory |
1853 | //bfield->GetField(x_,y_,z_,Bx,By,Bz); |
1854 | bfield->GetFieldAndGradient(x_,y_,z_,Bx,By,Bz, |
1855 | dBxdx,dBxdy,dBxdz,dBydx, |
1856 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1857 | |
1858 | // progress in z from hit to hit |
1859 | double z=z_; |
1860 | int i=0; |
1861 | |
1862 | int forward_traj_length=forward_traj.size(); |
1863 | // loop over the fdc hits |
1864 | double zhit=0.,old_zhit=0.; |
1865 | bool stepped_to_boundary=false; |
1866 | bool stepped_to_endplate=false; |
1867 | unsigned int m=0; |
1868 | double z_max=400.; |
1869 | double r2max=50.*50.; |
1870 | if (got_trd_gem_hits){ |
1871 | z_max=600.; |
1872 | r2max=100.*100.; |
1873 | } |
1874 | for (m=0;m<my_fdchits.size();m++){ |
1875 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX |
1876 | || fabs(S(state_tx))>TAN_MAX10. |
1877 | || fabs(S(state_ty))>TAN_MAX10. |
1878 | || S(state_x)*S(state_x)+S(state_y)*S(state_y)>r2max |
1879 | || z>z_max || z<Z_MIN-100. |
1880 | ){ |
1881 | break; |
1882 | } |
1883 | |
1884 | zhit=my_fdchits[m]->z; |
1885 | if (fabs(old_zhit-zhit)>EPS3.0e-8){ |
1886 | bool done=false; |
1887 | while (!done){ |
1888 | if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX |
1889 | || fabs(S(state_tx))>TAN_MAX10. |
1890 | || fabs(S(state_ty))>TAN_MAX10. |
1891 | || S(state_x)*S(state_x)+S(state_y)*S(state_y)>r2max |
1892 | || z>z_max || z< Z_MIN-100. |
1893 | ){ |
1894 | break; |
1895 | } |
1896 | |
1897 | if (PropagateForward(forward_traj_length,i,z,zhit,S,done, |
1898 | stepped_to_boundary,stepped_to_endplate) |
1899 | !=NOERROR) |
1900 | return UNRECOVERABLE_ERROR; |
1901 | } |
1902 | } |
1903 | old_zhit=zhit; |
1904 | } |
1905 | |
1906 | // If m<2 then no useable FDC hits survived the check on the magnitude on the |
1907 | // momentum |
1908 | if (m<2) return UNRECOVERABLE_ERROR; |
1909 | |
1910 | // Make sure the reference trajectory goes one step beyond the most |
1911 | // downstream hit plane |
1912 | if (m==my_fdchits.size()){ |
1913 | bool done=false; |
1914 | if (PropagateForward(forward_traj_length,i,z,z_max,S,done, |
1915 | stepped_to_boundary,stepped_to_endplate) |
1916 | !=NOERROR) |
1917 | return UNRECOVERABLE_ERROR; |
1918 | if (PropagateForward(forward_traj_length,i,z,z_max,S,done, |
1919 | stepped_to_boundary,stepped_to_endplate) |
1920 | !=NOERROR) |
1921 | return UNRECOVERABLE_ERROR; |
1922 | } |
1923 | |
1924 | // Shrink the deque if the new trajectory has less points in it than the |
1925 | // old trajectory |
1926 | if (i<(int)forward_traj.size()){ |
1927 | int mylen=forward_traj.size(); |
1928 | //_DBG_ << "Shrinking: " << mylen << " to " << i << endl; |
1929 | for (int j=0;j<mylen-i;j++){ |
1930 | forward_traj.pop_front(); |
1931 | } |
1932 | // _DBG_ << " Now " << forward_traj.size() << endl; |
1933 | } |
1934 | |
1935 | // If we lopped off some hits on the downstream end, truncate the trajectory to |
1936 | // the point in z just beyond the last valid hit |
1937 | unsigned int my_id=my_fdchits.size(); |
1938 | if (m<my_id){ |
1939 | if (zhit<z) my_id=m; |
1940 | else my_id=m-1; |
1941 | zhit=my_fdchits[my_id-1]->z; |
1942 | //_DBG_ << "Shrinking: " << forward_traj.size()<< endl; |
1943 | for (;;){ |
1944 | z=forward_traj[0].z; |
1945 | if (z<zhit+EPS21.e-4) break; |
1946 | forward_traj.pop_front(); |
1947 | } |
1948 | //_DBG_ << " Now " << forward_traj.size() << endl; |
1949 | |
1950 | // Temporory structure keeping state and trajectory information |
1951 | DKalmanForwardTrajectory_t temp; |
1952 | temp.h_id=0; |
1953 | temp.num_hits=0; |
1954 | temp.B=0.; |
1955 | temp.K_rho_Z_over_A=temp.rho_Z_over_A=temp.LnI=0.; |
1956 | temp.Z=0.; |
1957 | temp.Q=Zero5x5; |
1958 | |
1959 | // last S vector |
1960 | S=forward_traj[0].S; |
1961 | |
1962 | // Step just beyond the last hit |
1963 | double newz=z+0.01; |
1964 | double ds=Step(z,newz,0.,S); // ignore energy loss for this small step |
1965 | temp.s=forward_traj[0].s+ds; |
1966 | temp.z=newz; |
1967 | temp.S=S; |
1968 | |
1969 | // Flight time |
1970 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
1971 | double one_over_beta2=1.+mass2*q_over_p_sq; |
1972 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
1973 | temp.t=forward_traj[0].t+ds*sqrt(one_over_beta2); // in units where c=1 |
1974 | |
1975 | // Covariance and state vector needed for smoothing code |
1976 | temp.Ckk=Zero5x5; |
1977 | temp.Skk=Zero5x1; |
1978 | |
1979 | // Jacobian matrices |
1980 | temp.J=I5x5; |
1981 | |
1982 | forward_traj.push_front(temp); |
1983 | } |
1984 | |
1985 | // return an error if there are not enough entries in the trajectory |
1986 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
1987 | |
1988 | // Fill in Lorentz deflection parameters |
1989 | for (unsigned int m=0;m<forward_traj.size();m++){ |
1990 | if (my_id>0){ |
1991 | unsigned int hit_id=my_id-1; |
1992 | double z=forward_traj[m].z; |
1993 | if (fabs(z-my_fdchits[hit_id]->z)<EPS21.e-4){ |
1994 | forward_traj[m].h_id=my_id; |
1995 | |
1996 | if (my_fdchits[hit_id]->hit!=NULL__null){ |
1997 | // Get the magnetic field at this position along the trajectory |
1998 | bfield->GetField(forward_traj[m].S(state_x),forward_traj[m].S(state_y), |
1999 | z,Bx,By,Bz); |
2000 | double Br=sqrt(Bx*Bx+By*By); |
2001 | |
2002 | // Angle between B and wire |
2003 | double my_phi=0.; |
2004 | if (Br>0.) my_phi=acos((Bx*my_fdchits[hit_id]->sina |
2005 | +By*my_fdchits[hit_id]->cosa)/Br); |
2006 | /* |
2007 | lorentz_def->GetLorentzCorrectionParameters(forward_traj[m].pos.x(), |
2008 | forward_traj[m].pos.y(), |
2009 | forward_traj[m].pos.z(), |
2010 | tanz,tanr); |
2011 | my_fdchits[hit_id]->nr=tanr; |
2012 | my_fdchits[hit_id]->nz=tanz; |
2013 | */ |
2014 | |
2015 | my_fdchits[hit_id]->nr=LORENTZ_NR_PAR1*Bz*(1.+LORENTZ_NR_PAR2*Br); |
2016 | my_fdchits[hit_id]->nz=(LORENTZ_NZ_PAR1+LORENTZ_NZ_PAR2*Bz)*(Br*cos(my_phi)); |
2017 | } |
2018 | |
2019 | my_id--; |
2020 | |
2021 | unsigned int num=1; |
2022 | while (hit_id>0 |
2023 | && fabs(my_fdchits[hit_id]->z-my_fdchits[hit_id-1]->z)<EPS3.0e-8){ |
2024 | hit_id=my_id-1; |
2025 | num++; |
2026 | my_id--; |
2027 | } |
2028 | forward_traj[m].num_hits=num; |
2029 | } |
2030 | |
2031 | } |
2032 | } |
2033 | |
2034 | if (DEBUG_LEVEL>20) |
2035 | { |
2036 | cout << "--- Forward fdc trajectory ---" <<endl; |
2037 | for (unsigned int m=0;m<forward_traj.size();m++){ |
2038 | DMatrix5x1 S=(forward_traj[m].S); |
2039 | double tx=S(state_tx),ty=S(state_ty); |
2040 | double phi=atan2(ty,tx); |
2041 | double cosphi=cos(phi); |
2042 | double sinphi=sin(phi); |
2043 | double p=fabs(1./S(state_q_over_p)); |
2044 | double tanl=1./sqrt(tx*tx+ty*ty); |
2045 | double sinl=sin(atan(tanl)); |
2046 | double cosl=cos(atan(tanl)); |
2047 | cout |
2048 | << setiosflags(ios::fixed)<< "pos: " << setprecision(4) |
2049 | << forward_traj[m].S(state_x) << ", " |
2050 | << forward_traj[m].S(state_y) << ", " |
2051 | << forward_traj[m].z << " mom: " |
2052 | << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", " |
2053 | << p*sinl << " -> " << p |
2054 | <<" s: " << setprecision(3) |
2055 | << forward_traj[m].s |
2056 | <<" t: " << setprecision(3) |
2057 | << forward_traj[m].t/SPEED_OF_LIGHT29.9792458 |
2058 | <<" id: " << forward_traj[m].h_id |
2059 | << endl; |
2060 | } |
2061 | } |
2062 | |
2063 | |
2064 | // position at the end of the swim |
2065 | z_=z; |
2066 | x_=S(state_x); |
2067 | y_=S(state_y); |
2068 | |
2069 | return NOERROR; |
2070 | } |
2071 | |
2072 | // Step the state vector through the field from oldz to newz. |
2073 | // Uses the 4th-order Runga-Kutte algorithm. |
2074 | double DTrackFitterKalmanSIMD::Step(double oldz,double newz, double dEdx, |
2075 | DMatrix5x1 &S){ |
2076 | double delta_z=newz-oldz; |
2077 | if (fabs(delta_z)<EPS3.0e-8) return 0.; // skip if the step is too small |
2078 | |
2079 | // Direction tangents |
2080 | double tx=S(state_tx); |
2081 | double ty=S(state_ty); |
2082 | double ds=sqrt(1.+tx*tx+ty*ty)*delta_z; |
2083 | |
2084 | double delta_z_over_2=0.5*delta_z; |
2085 | double midz=oldz+delta_z_over_2; |
2086 | DMatrix5x1 D1,D2,D3,D4; |
2087 | |
2088 | //B-field and gradient at at (x,y,z) |
2089 | bfield->GetFieldAndGradient(S(state_x),S(state_y),oldz,Bx,By,Bz, |
2090 | dBxdx,dBxdy,dBxdz,dBydx, |
2091 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2092 | double Bx0=Bx,By0=By,Bz0=Bz; |
2093 | |
2094 | // Calculate the derivative and propagate the state to the next point |
2095 | CalcDeriv(oldz,S,dEdx,D1); |
2096 | DMatrix5x1 S1=S+delta_z_over_2*D1; |
2097 | |
2098 | // Calculate the field at the first intermediate point |
2099 | double dx=S1(state_x)-S(state_x); |
2100 | double dy=S1(state_y)-S(state_y); |
2101 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2102 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2103 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2104 | |
2105 | // Calculate the derivative and propagate the state to the next point |
2106 | CalcDeriv(midz,S1,dEdx,D2); |
2107 | S1=S+delta_z_over_2*D2; |
2108 | |
2109 | // Calculate the field at the second intermediate point |
2110 | dx=S1(state_x)-S(state_x); |
2111 | dy=S1(state_y)-S(state_y); |
2112 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2113 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2114 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2115 | |
2116 | // Calculate the derivative and propagate the state to the next point |
2117 | CalcDeriv(midz,S1,dEdx,D3); |
2118 | S1=S+delta_z*D3; |
2119 | |
2120 | // Calculate the field at the final point |
2121 | dx=S1(state_x)-S(state_x); |
2122 | dy=S1(state_y)-S(state_y); |
2123 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z; |
2124 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z; |
2125 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z; |
2126 | |
2127 | // Final derivative |
2128 | CalcDeriv(newz,S1,dEdx,D4); |
2129 | |
2130 | // S+=delta_z*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2131 | double dz_over_6=delta_z*ONE_SIXTH0.16666666666666667; |
2132 | double dz_over_3=delta_z*ONE_THIRD0.33333333333333333; |
2133 | S+=dz_over_6*D1; |
2134 | S+=dz_over_3*D2; |
2135 | S+=dz_over_3*D3; |
2136 | S+=dz_over_6*D4; |
2137 | |
2138 | // Don't let the magnitude of the momentum drop below some cutoff |
2139 | //if (fabs(S(state_q_over_p))>Q_OVER_P_MAX) |
2140 | // S(state_q_over_p)=Q_OVER_P_MAX*(S(state_q_over_p)>0.0?1.:-1.); |
2141 | // Try to keep the direction tangents from heading towards 90 degrees |
2142 | //if (fabs(S(state_tx))>TAN_MAX) |
2143 | // S(state_tx)=TAN_MAX*(S(state_tx)>0.0?1.:-1.); |
2144 | //if (fabs(S(state_ty))>TAN_MAX) |
2145 | // S(state_ty)=TAN_MAX*(S(state_ty)>0.0?1.:-1.); |
2146 | |
2147 | return ds; |
2148 | } |
2149 | // Step the state vector through the field from oldz to newz. |
2150 | // Uses the 4th-order Runga-Kutte algorithm. |
2151 | // Uses the gradient to compute the field at the intermediate and last |
2152 | // points. |
2153 | double DTrackFitterKalmanSIMD::FasterStep(double oldz,double newz, double dEdx, |
2154 | DMatrix5x1 &S){ |
2155 | double delta_z=newz-oldz; |
2156 | if (fabs(delta_z)<EPS3.0e-8) return 0.; // skip if the step is too small |
2157 | |
2158 | // Direction tangents |
2159 | double tx=S(state_tx); |
2160 | double ty=S(state_ty); |
2161 | double ds=sqrt(1.+tx*tx+ty*ty)*delta_z; |
2162 | |
2163 | double delta_z_over_2=0.5*delta_z; |
2164 | double midz=oldz+delta_z_over_2; |
2165 | DMatrix5x1 D1,D2,D3,D4; |
2166 | double Bx0=Bx,By0=By,Bz0=Bz; |
2167 | |
2168 | // The magnetic field at the beginning of the step is assumed to be |
2169 | // obtained at the end of the previous step through StepJacobian |
2170 | |
2171 | // Calculate the derivative and propagate the state to the next point |
2172 | CalcDeriv(oldz,S,dEdx,D1); |
2173 | DMatrix5x1 S1=S+delta_z_over_2*D1; |
2174 | |
2175 | // Calculate the field at the first intermediate point |
2176 | double dx=S1(state_x)-S(state_x); |
2177 | double dy=S1(state_y)-S(state_y); |
2178 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2179 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2180 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2181 | |
2182 | // Calculate the derivative and propagate the state to the next point |
2183 | CalcDeriv(midz,S1,dEdx,D2); |
2184 | S1=S+delta_z_over_2*D2; |
2185 | |
2186 | // Calculate the field at the second intermediate point |
2187 | dx=S1(state_x)-S(state_x); |
2188 | dy=S1(state_y)-S(state_y); |
2189 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2190 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2191 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2192 | |
2193 | // Calculate the derivative and propagate the state to the next point |
2194 | CalcDeriv(midz,S1,dEdx,D3); |
2195 | S1=S+delta_z*D3; |
2196 | |
2197 | // Calculate the field at the final point |
2198 | dx=S1(state_x)-S(state_x); |
2199 | dy=S1(state_y)-S(state_y); |
2200 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z; |
2201 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z; |
2202 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z; |
2203 | |
2204 | // Final derivative |
2205 | CalcDeriv(newz,S1,dEdx,D4); |
2206 | |
2207 | // S+=delta_z*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2208 | double dz_over_6=delta_z*ONE_SIXTH0.16666666666666667; |
2209 | double dz_over_3=delta_z*ONE_THIRD0.33333333333333333; |
2210 | S+=dz_over_6*D1; |
2211 | S+=dz_over_3*D2; |
2212 | S+=dz_over_3*D3; |
2213 | S+=dz_over_6*D4; |
2214 | |
2215 | // Don't let the magnitude of the momentum drop below some cutoff |
2216 | //if (fabs(S(state_q_over_p))>Q_OVER_P_MAX) |
2217 | // S(state_q_over_p)=Q_OVER_P_MAX*(S(state_q_over_p)>0.0?1.:-1.); |
2218 | // Try to keep the direction tangents from heading towards 90 degrees |
2219 | //if (fabs(S(state_tx))>TAN_MAX) |
2220 | // S(state_tx)=TAN_MAX*(S(state_tx)>0.0?1.:-1.); |
2221 | //if (fabs(S(state_ty))>TAN_MAX) |
2222 | // S(state_ty)=TAN_MAX*(S(state_ty)>0.0?1.:-1.); |
2223 | |
2224 | return ds; |
2225 | } |
2226 | |
2227 | |
2228 | |
2229 | // Compute the Jacobian matrix for the forward parametrization. |
2230 | jerror_t DTrackFitterKalmanSIMD::StepJacobian(double oldz,double newz, |
2231 | const DMatrix5x1 &S, |
2232 | double dEdx,DMatrix5x5 &J){ |
2233 | // Initialize the Jacobian matrix |
2234 | //J.Zero(); |
2235 | //for (int i=0;i<5;i++) J(i,i)=1.; |
2236 | J=I5x5; |
2237 | |
2238 | // Step in z |
2239 | double delta_z=newz-oldz; |
2240 | if (fabs(delta_z)<EPS3.0e-8) return NOERROR; //skip if the step is too small |
2241 | |
2242 | // Current values of state vector variables |
2243 | double x=S(state_x), y=S(state_y),tx=S(state_tx),ty=S(state_ty); |
2244 | double q_over_p=S(state_q_over_p); |
2245 | |
2246 | //B-field and field gradient at (x,y,z) |
2247 | //if (get_field) |
2248 | bfield->GetFieldAndGradient(x,y,oldz,Bx,By,Bz,dBxdx,dBxdy, |
2249 | dBxdz,dBydx,dBydy, |
2250 | dBydz,dBzdx,dBzdy,dBzdz); |
2251 | |
2252 | // Don't let the magnitude of the momentum drop below some cutoff |
2253 | if (fabs(q_over_p)>Q_OVER_P_MAX){ |
2254 | q_over_p=Q_OVER_P_MAX*(q_over_p>0.0?1.:-1.); |
2255 | dEdx=0.; |
2256 | } |
2257 | // Try to keep the direction tangents from heading towards 90 degrees |
2258 | if (fabs(tx)>TAN_MAX10.) tx=TAN_MAX10.*(tx>0.0?1.:-1.); |
2259 | if (fabs(ty)>TAN_MAX10.) ty=TAN_MAX10.*(ty>0.0?1.:-1.); |
2260 | // useful combinations of terms |
2261 | double kq_over_p=qBr2p0.003*q_over_p; |
2262 | double tx2=tx*tx; |
2263 | double twotx2=2.*tx2; |
2264 | double ty2=ty*ty; |
2265 | double twoty2=2.*ty2; |
2266 | double txty=tx*ty; |
2267 | double one_plus_tx2=1.+tx2; |
2268 | double one_plus_ty2=1.+ty2; |
2269 | double one_plus_twotx2_plus_ty2=one_plus_ty2+twotx2; |
2270 | double one_plus_twoty2_plus_tx2=one_plus_tx2+twoty2; |
2271 | double dsdz=sqrt(1.+tx2+ty2); |
2272 | double ds=dsdz*delta_z; |
2273 | double kds=qBr2p0.003*ds; |
2274 | double kqdz_over_p_over_dsdz=kq_over_p*delta_z/dsdz; |
2275 | double kq_over_p_ds=kq_over_p*ds; |
2276 | double dtx_Bdep=ty*Bz+txty*Bx-one_plus_tx2*By; |
2277 | double dty_Bdep=Bx*one_plus_ty2-txty*By-tx*Bz; |
2278 | double Bxty=Bx*ty; |
2279 | double Bytx=By*tx; |
2280 | double Bztxty=Bz*txty; |
2281 | double Byty=By*ty; |
2282 | double Bxtx=Bx*tx; |
2283 | |
2284 | // Jacobian |
2285 | J(state_x,state_tx)=J(state_y,state_ty)=delta_z; |
2286 | J(state_tx,state_q_over_p)=kds*dtx_Bdep; |
2287 | J(state_ty,state_q_over_p)=kds*dty_Bdep; |
2288 | J(state_tx,state_tx)+=kqdz_over_p_over_dsdz*(Bxty*(one_plus_twotx2_plus_ty2) |
2289 | -Bytx*(3.*one_plus_tx2+twoty2) |
2290 | +Bztxty); |
2291 | J(state_tx,state_x)=kq_over_p_ds*(ty*dBzdx+txty*dBxdx-one_plus_tx2*dBydx); |
2292 | J(state_ty,state_ty)+=kqdz_over_p_over_dsdz*(Bxty*(3.*one_plus_ty2+twotx2) |
2293 | -Bytx*(one_plus_twoty2_plus_tx2) |
2294 | -Bztxty); |
2295 | J(state_ty,state_y)= kq_over_p_ds*(one_plus_ty2*dBxdy-txty*dBydy-tx*dBzdy); |
2296 | J(state_tx,state_ty)=kqdz_over_p_over_dsdz |
2297 | *((Bxtx+Bz)*(one_plus_twoty2_plus_tx2)-Byty*one_plus_tx2); |
2298 | J(state_tx,state_y)= kq_over_p_ds*(tx*dBzdy+txty*dBxdy-one_plus_tx2*dBydy); |
2299 | J(state_ty,state_tx)=-kqdz_over_p_over_dsdz*((Byty+Bz)*(one_plus_twotx2_plus_ty2) |
2300 | -Bxtx*one_plus_ty2); |
2301 | J(state_ty,state_x)=kq_over_p_ds*(one_plus_ty2*dBxdx-txty*dBydx-tx*dBzdx); |
2302 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2303 | double one_over_p_sq=q_over_p*q_over_p; |
2304 | double E=sqrt(1./one_over_p_sq+mass2); |
2305 | J(state_q_over_p,state_q_over_p)-=dEdx*ds/E*(2.+3.*mass2*one_over_p_sq); |
2306 | double temp=-(q_over_p*one_over_p_sq/dsdz)*E*dEdx*delta_z; |
2307 | J(state_q_over_p,state_tx)=tx*temp; |
2308 | J(state_q_over_p,state_ty)=ty*temp; |
2309 | } |
2310 | |
2311 | return NOERROR; |
2312 | } |
2313 | |
2314 | // Calculate the derivative for the alternate set of parameters {q/pT, phi, |
2315 | // tan(lambda),D,z} |
2316 | jerror_t DTrackFitterKalmanSIMD::CalcDeriv(DVector2 &dpos,const DMatrix5x1 &S, |
2317 | double dEdx,DMatrix5x1 &D1){ |
2318 | //Direction at current point |
2319 | double tanl=S(state_tanl); |
2320 | // Don't let tanl exceed some maximum |
2321 | if (fabs(tanl)>TAN_MAX10.) tanl=TAN_MAX10.*(tanl>0.0?1.:-1.); |
2322 | |
2323 | double phi=S(state_phi); |
2324 | double cosphi=cos(phi); |
2325 | double sinphi=sin(phi); |
2326 | double lambda=atan(tanl); |
2327 | double sinl=sin(lambda); |
2328 | double cosl=cos(lambda); |
2329 | // Other parameters |
2330 | double q_over_pt=S(state_q_over_pt); |
2331 | double pt=fabs(1./q_over_pt); |
2332 | |
2333 | // Turn off dEdx if the pt drops below some minimum |
2334 | if (pt<PT_MIN) { |
2335 | dEdx=0.; |
2336 | } |
2337 | double kq_over_pt=qBr2p0.003*q_over_pt; |
2338 | |
2339 | // Derivative of S with respect to s |
2340 | double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi; |
2341 | D1(state_q_over_pt) |
2342 | =kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2343 | double one_over_cosl=1./cosl; |
2344 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2345 | double p=pt*one_over_cosl; |
2346 | double p_sq=p*p; |
2347 | double E=sqrt(p_sq+mass2); |
2348 | D1(state_q_over_pt)-=q_over_pt*E*dEdx/p_sq; |
2349 | } |
2350 | // D1(state_phi) |
2351 | // =kq_over_pt*(Bx*cosphi*sinl+By*sinphi*sinl-Bz*cosl); |
2352 | D1(state_phi)=kq_over_pt*((Bx*cosphi+By*sinphi)*sinl-Bz*cosl); |
2353 | D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2354 | D1(state_z)=sinl; |
2355 | |
2356 | // New direction |
2357 | dpos.Set(cosl*cosphi,cosl*sinphi); |
2358 | |
2359 | return NOERROR; |
2360 | } |
2361 | |
2362 | // Calculate the derivative and Jacobian matrices for the alternate set of |
2363 | // parameters {q/pT, phi, tan(lambda),D,z} |
2364 | jerror_t DTrackFitterKalmanSIMD::CalcDerivAndJacobian(const DVector2 &xy, |
2365 | DVector2 &dxy, |
2366 | const DMatrix5x1 &S, |
2367 | double dEdx, |
2368 | DMatrix5x5 &J1, |
2369 | DMatrix5x1 &D1){ |
2370 | //Direction at current point |
2371 | double tanl=S(state_tanl); |
2372 | // Don't let tanl exceed some maximum |
2373 | if (fabs(tanl)>TAN_MAX10.) tanl=TAN_MAX10.*(tanl>0.0?1.:-1.); |
2374 | |
2375 | double phi=S(state_phi); |
2376 | double cosphi=cos(phi); |
2377 | double sinphi=sin(phi); |
2378 | double lambda=atan(tanl); |
2379 | double sinl=sin(lambda); |
2380 | double cosl=cos(lambda); |
2381 | double cosl2=cosl*cosl; |
2382 | double cosl3=cosl*cosl2; |
2383 | double one_over_cosl=1./cosl; |
2384 | // Other parameters |
2385 | double q_over_pt=S(state_q_over_pt); |
2386 | double pt=fabs(1./q_over_pt); |
2387 | double q=pt*q_over_pt; |
2388 | |
2389 | // Turn off dEdx if pt drops below some minimum |
2390 | if (pt<PT_MIN) { |
2391 | dEdx=0.; |
2392 | } |
2393 | double kq_over_pt=qBr2p0.003*q_over_pt; |
2394 | |
2395 | // B-field and gradient at (x,y,z) |
2396 | bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2397 | dBxdx,dBxdy,dBxdz,dBydx, |
2398 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2399 | |
2400 | // Derivative of S with respect to s |
2401 | double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi; |
2402 | double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi; |
2403 | D1(state_q_over_pt)=kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2404 | D1(state_phi)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl); |
2405 | D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2406 | D1(state_z)=sinl; |
2407 | |
2408 | // New direction |
2409 | dxy.Set(cosl*cosphi,cosl*sinphi); |
2410 | |
2411 | // Jacobian matrix elements |
2412 | J1(state_phi,state_phi)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2413 | J1(state_phi,state_q_over_pt) |
2414 | =qBr2p0.003*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl); |
2415 | J1(state_phi,state_tanl)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*cosl |
2416 | +Bz*sinl)*cosl2; |
2417 | J1(state_phi,state_z) |
2418 | =kq_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl); |
2419 | |
2420 | J1(state_tanl,state_phi)=-kq_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl; |
2421 | J1(state_tanl,state_q_over_pt)=qBr2p0.003*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2422 | J1(state_tanl,state_tanl)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2423 | J1(state_tanl,state_z)=kq_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl; |
2424 | J1(state_q_over_pt,state_phi) |
2425 | =-kq_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi; |
2426 | J1(state_q_over_pt,state_q_over_pt) |
2427 | =2.*kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2428 | J1(state_q_over_pt,state_tanl) |
2429 | =kq_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi; |
2430 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2431 | double p=pt*one_over_cosl; |
2432 | double p_sq=p*p; |
2433 | double m2_over_p2=mass2/p_sq; |
2434 | double E=sqrt(p_sq+mass2); |
2435 | |
2436 | D1(state_q_over_pt)-=q_over_pt*E/p_sq*dEdx; |
2437 | J1(state_q_over_pt,state_q_over_pt)-=dEdx*(2.+3.*m2_over_p2)/E; |
2438 | J1(state_q_over_pt,state_tanl)+=q*dEdx*sinl*(1.+2.*m2_over_p2)/(p*E); |
2439 | } |
2440 | J1(state_q_over_pt,state_z) |
2441 | =kq_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi); |
2442 | J1(state_z,state_tanl)=cosl3; |
2443 | |
2444 | return NOERROR; |
2445 | } |
2446 | |
2447 | // Step the state and the covariance matrix through the field |
2448 | jerror_t DTrackFitterKalmanSIMD::StepStateAndCovariance(DVector2 &xy, |
2449 | double ds, |
2450 | double dEdx, |
2451 | DMatrix5x1 &S, |
2452 | DMatrix5x5 &J, |
2453 | DMatrix5x5 &C){ |
2454 | //Initialize the Jacobian matrix |
2455 | J=I5x5; |
2456 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2457 | |
2458 | // B-field and gradient at current (x,y,z) |
2459 | bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2460 | dBxdx,dBxdy,dBxdz,dBydx, |
2461 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2462 | double Bx0=Bx,By0=By,Bz0=Bz; |
2463 | |
2464 | // Matrices for intermediate steps |
2465 | DMatrix5x1 D1,D2,D3,D4; |
2466 | DMatrix5x1 S1; |
2467 | DMatrix5x5 J1; |
2468 | DVector2 dxy1,dxy2,dxy3,dxy4; |
2469 | double ds_2=0.5*ds; |
2470 | |
2471 | // Find the derivative at the first point, propagate the state to the |
2472 | // first intermediate point and start filling the Jacobian matrix |
2473 | CalcDerivAndJacobian(xy,dxy1,S,dEdx,J1,D1); |
2474 | S1=S+ds_2*D1; |
2475 | |
2476 | // Calculate the field at the first intermediate point |
2477 | double dz=S1(state_z)-S(state_z); |
2478 | double dx=ds_2*dxy1.X(); |
2479 | double dy=ds_2*dxy1.Y(); |
2480 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2481 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2482 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2483 | |
2484 | // Calculate the derivative and propagate the state to the next point |
2485 | CalcDeriv(dxy2,S1,dEdx,D2); |
2486 | S1=S+ds_2*D2; |
2487 | |
2488 | // Calculate the field at the second intermediate point |
2489 | dz=S1(state_z)-S(state_z); |
2490 | dx=ds_2*dxy2.X(); |
2491 | dy=ds_2*dxy2.Y(); |
2492 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2493 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2494 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2495 | |
2496 | // Calculate the derivative and propagate the state to the next point |
2497 | CalcDeriv(dxy3,S1,dEdx,D3); |
2498 | S1=S+ds*D3; |
2499 | |
2500 | // Calculate the field at the final point |
2501 | dz=S1(state_z)-S(state_z); |
2502 | dx=ds*dxy3.X(); |
2503 | dy=ds*dxy3.Y(); |
2504 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2505 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2506 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2507 | |
2508 | // Final derivative |
2509 | CalcDeriv(dxy4,S1,dEdx,D4); |
2510 | |
2511 | // Position vector increment |
2512 | //DVector3 dpos |
2513 | // =ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4); |
2514 | double ds_over_6=ds*ONE_SIXTH0.16666666666666667; |
2515 | double ds_over_3=ds*ONE_THIRD0.33333333333333333; |
2516 | DVector2 dxy=ds_over_6*dxy1; |
2517 | dxy+=ds_over_3*dxy2; |
2518 | dxy+=ds_over_3*dxy3; |
2519 | dxy+=ds_over_6*dxy4; |
2520 | |
2521 | // New Jacobian matrix |
2522 | J+=ds*J1; |
2523 | |
2524 | // Deal with changes in D |
2525 | double B=sqrt(Bx0*Bx0+By0*By0+Bz0*Bz0); |
2526 | //double qrc_old=qpt/(qBr2p*Bz_); |
2527 | double qpt=1./S(state_q_over_pt); |
2528 | double q=(qpt>0.)?1.:-1.; |
2529 | double qrc_old=qpt/(qBr2p0.003*B); |
2530 | double sinphi=sin(S(state_phi)); |
2531 | double cosphi=cos(S(state_phi)); |
2532 | double qrc_plus_D=S(state_D)+qrc_old; |
2533 | dx=dxy.X(); |
2534 | dy=dxy.Y(); |
2535 | double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi; |
2536 | double rc=sqrt(dxy.Mod2() |
2537 | +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi) |
2538 | +qrc_plus_D*qrc_plus_D); |
2539 | double q_over_rc=q/rc; |
2540 | |
2541 | J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D); |
2542 | J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.); |
2543 | J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi); |
2544 | |
2545 | // New xy vector |
2546 | xy+=dxy; |
2547 | |
2548 | // New state vector |
2549 | //S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2550 | S+=ds_over_6*D1; |
2551 | S+=ds_over_3*D2; |
2552 | S+=ds_over_3*D3; |
2553 | S+=ds_over_6*D4; |
2554 | |
2555 | // Don't let the pt drop below some minimum |
2556 | //if (fabs(1./S(state_q_over_pt))<PT_MIN) { |
2557 | // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.); |
2558 | // } |
2559 | // Don't let tanl exceed some maximum |
2560 | if (fabs(S(state_tanl))>TAN_MAX10.){ |
2561 | S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0.0?1.:-1.); |
2562 | } |
2563 | |
2564 | // New covariance matrix |
2565 | // C=J C J^T |
2566 | //C=C.SandwichMultiply(J); |
2567 | C=J*C*J.Transpose(); |
2568 | |
2569 | return NOERROR; |
2570 | } |
2571 | |
2572 | // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z} |
2573 | // Uses the gradient to compute the field at the intermediate and last |
2574 | // points. |
2575 | jerror_t DTrackFitterKalmanSIMD::FasterStep(DVector2 &xy,double ds, |
2576 | DMatrix5x1 &S, |
2577 | double dEdx){ |
2578 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2579 | |
2580 | // Matrices for intermediate steps |
2581 | DMatrix5x1 D1,D2,D3,D4; |
2582 | DMatrix5x1 S1; |
2583 | DVector2 dxy1,dxy2,dxy3,dxy4; |
2584 | double ds_2=0.5*ds; |
2585 | double Bx0=Bx,By0=By,Bz0=Bz; |
2586 | |
2587 | // The magnetic field at the beginning of the step is assumed to be |
2588 | // obtained at the end of the previous step through StepJacobian |
2589 | |
2590 | // Calculate the derivative and propagate the state to the next point |
2591 | CalcDeriv(dxy1,S,dEdx,D1); |
2592 | S1=S+ds_2*D1; |
2593 | |
2594 | // Calculate the field at the first intermediate point |
2595 | double dz=S1(state_z)-S(state_z); |
2596 | double dx=ds_2*dxy1.X(); |
2597 | double dy=ds_2*dxy1.Y(); |
2598 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2599 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2600 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2601 | |
2602 | // Calculate the derivative and propagate the state to the next point |
2603 | CalcDeriv(dxy2,S1,dEdx,D2); |
2604 | S1=S+ds_2*D2; |
2605 | |
2606 | // Calculate the field at the second intermediate point |
2607 | dz=S1(state_z)-S(state_z); |
2608 | dx=ds_2*dxy2.X(); |
2609 | dy=ds_2*dxy2.Y(); |
2610 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2611 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2612 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2613 | |
2614 | // Calculate the derivative and propagate the state to the next point |
2615 | CalcDeriv(dxy3,S1,dEdx,D3); |
2616 | S1=S+ds*D3; |
2617 | |
2618 | // Calculate the field at the final point |
2619 | dz=S1(state_z)-S(state_z); |
2620 | dx=ds*dxy3.X(); |
2621 | dy=ds*dxy3.Y(); |
2622 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2623 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2624 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2625 | |
2626 | // Final derivative |
2627 | CalcDeriv(dxy4,S1,dEdx,D4); |
2628 | |
2629 | // New state vector |
2630 | // S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2631 | double ds_over_6=ds*ONE_SIXTH0.16666666666666667; |
2632 | double ds_over_3=ds*ONE_THIRD0.33333333333333333; |
2633 | S+=ds_over_6*D1; |
2634 | S+=ds_over_3*D2; |
2635 | S+=ds_over_3*D3; |
2636 | S+=ds_over_6*D4; |
2637 | |
2638 | // New position |
2639 | //pos+=ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4); |
2640 | xy+=ds_over_6*dxy1; |
2641 | xy+=ds_over_3*dxy2; |
2642 | xy+=ds_over_3*dxy3; |
2643 | xy+=ds_over_6*dxy4; |
2644 | |
2645 | // Don't let the pt drop below some minimum |
2646 | //if (fabs(1./S(state_q_over_pt))<PT_MIN) { |
2647 | // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.); |
2648 | //} |
2649 | // Don't let tanl exceed some maximum |
2650 | if (fabs(S(state_tanl))>TAN_MAX10.){ |
2651 | S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0.0?1.:-1.); |
2652 | } |
2653 | |
2654 | return NOERROR; |
2655 | } |
2656 | |
2657 | // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z} |
2658 | jerror_t DTrackFitterKalmanSIMD::Step(DVector2 &xy,double ds, |
2659 | DMatrix5x1 &S, |
2660 | double dEdx){ |
2661 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2662 | |
2663 | // B-field and gradient at current (x,y,z) |
2664 | bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2665 | dBxdx,dBxdy,dBxdz,dBydx, |
2666 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2667 | double Bx0=Bx,By0=By,Bz0=Bz; |
2668 | |
2669 | // Matrices for intermediate steps |
2670 | DMatrix5x1 D1,D2,D3,D4; |
2671 | DMatrix5x1 S1; |
2672 | DVector2 dxy1,dxy2,dxy3,dxy4; |
2673 | double ds_2=0.5*ds; |
2674 | |
2675 | // Find the derivative at the first point, propagate the state to the |
2676 | // first intermediate point |
2677 | CalcDeriv(dxy1,S,dEdx,D1); |
2678 | S1=S+ds_2*D1; |
2679 | |
2680 | // Calculate the field at the first intermediate point |
2681 | double dz=S1(state_z)-S(state_z); |
2682 | double dx=ds_2*dxy1.X(); |
2683 | double dy=ds_2*dxy1.Y(); |
2684 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2685 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2686 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2687 | |
2688 | // Calculate the derivative and propagate the state to the next point |
2689 | CalcDeriv(dxy2,S1,dEdx,D2); |
2690 | S1=S+ds_2*D2; |
2691 | |
2692 | // Calculate the field at the second intermediate point |
2693 | dz=S1(state_z)-S(state_z); |
2694 | dx=ds_2*dxy2.X(); |
2695 | dy=ds_2*dxy2.Y(); |
2696 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2697 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2698 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2699 | |
2700 | // Calculate the derivative and propagate the state to the next point |
2701 | CalcDeriv(dxy3,S1,dEdx,D3); |
2702 | S1=S+ds*D3; |
2703 | |
2704 | // Calculate the field at the final point |
2705 | dz=S1(state_z)-S(state_z); |
2706 | dx=ds*dxy3.X(); |
2707 | dy=ds*dxy3.Y(); |
2708 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2709 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2710 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2711 | |
2712 | // Final derivative |
2713 | CalcDeriv(dxy4,S1,dEdx,D4); |
2714 | |
2715 | // New state vector |
2716 | // S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2717 | double ds_over_6=ds*ONE_SIXTH0.16666666666666667; |
2718 | double ds_over_3=ds*ONE_THIRD0.33333333333333333; |
2719 | S+=ds_over_6*D1; |
2720 | S+=ds_over_3*D2; |
2721 | S+=ds_over_3*D3; |
2722 | S+=ds_over_6*D4; |
2723 | |
2724 | // New position |
2725 | //pos+=ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4); |
2726 | xy+=ds_over_6*dxy1; |
2727 | xy+=ds_over_3*dxy2; |
2728 | xy+=ds_over_3*dxy3; |
2729 | xy+=ds_over_6*dxy4; |
2730 | |
2731 | // Don't let the pt drop below some minimum |
2732 | //if (fabs(1./S(state_q_over_pt))<PT_MIN) { |
2733 | // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.); |
2734 | //} |
2735 | // Don't let tanl exceed some maximum |
2736 | if (fabs(S(state_tanl))>TAN_MAX10.){ |
2737 | S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0.0?1.:-1.); |
2738 | } |
2739 | |
2740 | return NOERROR; |
2741 | } |
2742 | |
2743 | // Assuming that the magnetic field is constant over the step, use a helical |
2744 | // model to step directly to the next point along the trajectory. |
2745 | void DTrackFitterKalmanSIMD::FastStep(double &z,double ds, double dEdx, |
2746 | DMatrix5x1 &S){ |
2747 | |
2748 | // Compute convenience terms involving Bx, By, Bz |
2749 | double one_over_p=fabs(S(state_q_over_p)); |
2750 | double p=1./one_over_p; |
2751 | double tx=S(state_tx),ty=S(state_ty); |
2752 | double denom=sqrt(1.+tx*tx+ty*ty); |
2753 | double px=p*tx/denom; |
2754 | double py=p*ty/denom; |
2755 | double pz=p/denom; |
2756 | double q=S(state_q_over_p)>0?1.:-1.; |
2757 | double k_q=qBr2p0.003*q; |
2758 | double ds_over_p=ds*one_over_p; |
2759 | double factor=k_q*(0.25*ds_over_p); |
2760 | double Ax=factor*Bx,Ay=factor*By,Az=factor*Bz; |
2761 | double Ax2=Ax*Ax,Ay2=Ay*Ay,Az2=Az*Az; |
2762 | double AxAy=Ax*Ay,AxAz=Ax*Az,AyAz=Ay*Az; |
2763 | double one_plus_Ax2=1.+Ax2; |
2764 | double scale=ds_over_p/(one_plus_Ax2+Ay2+Az2); |
2765 | |
2766 | // Compute new position |
2767 | double dx=scale*(px*one_plus_Ax2+py*(AxAy+Az)+pz*(AxAz-Ay)); |
2768 | double dy=scale*(px*(AxAy-Az)+py*(1.+Ay2)+pz*(AyAz+Ax)); |
2769 | double dz=scale*(px*(AxAz+Ay)+py*(AyAz-Ax)+pz*(1.+Az2)); |
2770 | S(state_x)+=dx; |
2771 | S(state_y)+=dy; |
2772 | z+=dz; |
2773 | |
2774 | // Compute new momentum |
2775 | px+=k_q*(Bz*dy-By*dz); |
2776 | py+=k_q*(Bx*dz-Bz*dx); |
2777 | pz+=k_q*(By*dx-Bx*dy); |
2778 | S(state_tx)=px/pz; |
2779 | S(state_ty)=py/pz; |
2780 | if (fabs(dEdx)>EPS3.0e-8){ |
2781 | double one_over_p_sq=one_over_p*one_over_p; |
2782 | double E=sqrt(1./one_over_p_sq+mass2); |
2783 | S(state_q_over_p)-=S(state_q_over_p)*one_over_p_sq*E*dEdx*ds; |
2784 | } |
2785 | } |
2786 | // Assuming that the magnetic field is constant over the step, use a helical |
2787 | // model to step directly to the next point along the trajectory. |
2788 | void DTrackFitterKalmanSIMD::FastStep(DVector2 &xy,double ds, double dEdx, |
2789 | DMatrix5x1 &S){ |
2790 | |
2791 | // Compute convenience terms involving Bx, By, Bz |
2792 | double pt=fabs(1./S(state_q_over_pt)); |
2793 | double one_over_p=cos(atan(S(state_tanl)))/pt; |
2794 | double px=pt*cos(S(state_phi)); |
2795 | double py=pt*sin(S(state_phi)); |
2796 | double pz=pt*S(state_tanl); |
2797 | double q=S(state_q_over_pt)>0?1.:-1.; |
2798 | double k_q=qBr2p0.003*q; |
2799 | double ds_over_p=ds*one_over_p; |
2800 | double factor=k_q*(0.25*ds_over_p); |
2801 | double Ax=factor*Bx,Ay=factor*By,Az=factor*Bz; |
2802 | double Ax2=Ax*Ax,Ay2=Ay*Ay,Az2=Az*Az; |
2803 | double AxAy=Ax*Ay,AxAz=Ax*Az,AyAz=Ay*Az; |
2804 | double one_plus_Ax2=1.+Ax2; |
2805 | double scale=ds_over_p/(one_plus_Ax2+Ay2+Az2); |
2806 | |
2807 | // Compute new position |
2808 | double dx=scale*(px*one_plus_Ax2+py*(AxAy+Az)+pz*(AxAz-Ay)); |
2809 | double dy=scale*(px*(AxAy-Az)+py*(1.+Ay2)+pz*(AyAz+Ax)); |
2810 | double dz=scale*(px*(AxAz+Ay)+py*(AyAz-Ax)+pz*(1.+Az2)); |
2811 | xy.Set(xy.X()+dx,xy.Y()+dy); |
2812 | S(state_z)+=dz; |
2813 | |
2814 | // Compute new momentum |
2815 | px+=k_q*(Bz*dy-By*dz); |
2816 | py+=k_q*(Bx*dz-Bz*dx); |
2817 | pz+=k_q*(By*dx-Bx*dy); |
2818 | pt=sqrt(px*px+py*py); |
2819 | S(state_q_over_pt)=q/pt; |
2820 | S(state_phi)=atan2(py,px); |
2821 | S(state_tanl)=pz/pt; |
2822 | if (fabs(dEdx)>EPS3.0e-8){ |
2823 | double one_over_p_sq=one_over_p*one_over_p; |
2824 | double E=sqrt(1./one_over_p_sq+mass2); |
2825 | S(state_q_over_p)-=S(state_q_over_pt)*one_over_p_sq*E*dEdx*ds; |
2826 | } |
2827 | } |
2828 | |
2829 | // Calculate the jacobian matrix for the alternate parameter set |
2830 | // {q/pT,phi,tanl(lambda),D,z} |
2831 | jerror_t DTrackFitterKalmanSIMD::StepJacobian(const DVector2 &xy, |
2832 | const DVector2 &dxy, |
2833 | double ds,const DMatrix5x1 &S, |
2834 | double dEdx,DMatrix5x5 &J){ |
2835 | // Initialize the Jacobian matrix |
2836 | //J.Zero(); |
2837 | //for (int i=0;i<5;i++) J(i,i)=1.; |
2838 | J=I5x5; |
2839 | |
2840 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2841 | // B-field and gradient at current (x,y,z) |
2842 | //bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2843 | //dBxdx,dBxdy,dBxdz,dBydx, |
2844 | //dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2845 | |
2846 | // Charge |
2847 | double q=(S(state_q_over_pt)>0.0)?1.:-1.; |
2848 | |
2849 | //kinematic quantities |
2850 | double q_over_pt=S(state_q_over_pt); |
2851 | double sinphi=sin(S(state_phi)); |
2852 | double cosphi=cos(S(state_phi)); |
2853 | double D=S(state_D); |
2854 | double lambda=atan(S(state_tanl)); |
2855 | double sinl=sin(lambda); |
2856 | double cosl=cos(lambda); |
2857 | double cosl2=cosl*cosl; |
2858 | double cosl3=cosl*cosl2; |
2859 | double one_over_cosl=1./cosl; |
2860 | double pt=fabs(1./q_over_pt); |
2861 | |
2862 | // Turn off dEdx if pt drops below some minimum |
2863 | if (pt<PT_MIN) { |
2864 | dEdx=0.; |
2865 | } |
2866 | double kds=qBr2p0.003*ds; |
2867 | double kq_ds_over_pt=kds*q_over_pt; |
2868 | double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi; |
2869 | double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi; |
2870 | |
2871 | // Jacobian matrix elements |
2872 | J(state_phi,state_phi)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2873 | J(state_phi,state_q_over_pt)=kds*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl); |
2874 | J(state_phi,state_tanl)=kq_ds_over_pt*(By_sinphi_plus_Bx_cosphi*cosl |
2875 | +Bz*sinl)*cosl2; |
2876 | J(state_phi,state_z) |
2877 | =kq_ds_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl); |
2878 | |
2879 | J(state_tanl,state_phi)=-kq_ds_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl; |
2880 | J(state_tanl,state_q_over_pt)=kds*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2881 | J(state_tanl,state_tanl)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2882 | J(state_tanl,state_z)=kq_ds_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl; |
2883 | J(state_q_over_pt,state_phi) |
2884 | =-kq_ds_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi; |
2885 | J(state_q_over_pt,state_q_over_pt) |
2886 | +=2.*kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2887 | J(state_q_over_pt,state_tanl) |
2888 | =kq_ds_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi; |
2889 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2890 | double p=pt*one_over_cosl; |
2891 | double p_sq=p*p; |
2892 | double m2_over_p2=mass2/p_sq; |
2893 | double E=sqrt(p_sq+mass2); |
2894 | double dE_over_E=dEdx*ds/E; |
2895 | |
2896 | J(state_q_over_pt,state_q_over_pt)-=dE_over_E*(2.+3.*m2_over_p2); |
2897 | J(state_q_over_pt,state_tanl)+=q*dE_over_E*sinl*(1.+2.*m2_over_p2)/p; |
2898 | } |
2899 | J(state_q_over_pt,state_z) |
2900 | =kq_ds_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi); |
2901 | J(state_z,state_tanl)=cosl3*ds; |
2902 | |
2903 | // Deal with changes in D |
2904 | double B=sqrt(Bx*Bx+By*By+Bz*Bz); |
2905 | //double qrc_old=qpt/(qBr2p*fabs(Bz)); |
2906 | double qpt=FactorForSenseOfRotation/q_over_pt; |
2907 | double qrc_old=qpt/(qBr2p0.003*B); |
2908 | double qrc_plus_D=D+qrc_old; |
2909 | double dx=dxy.X(); |
2910 | double dy=dxy.Y(); |
2911 | double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi; |
2912 | double rc=sqrt(dxy.Mod2() |
2913 | +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi) |
2914 | +qrc_plus_D*qrc_plus_D); |
2915 | double q_over_rc=FactorForSenseOfRotation*q/rc; |
2916 | |
2917 | J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D); |
2918 | J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.); |
2919 | J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi); |
2920 | |
2921 | return NOERROR; |
2922 | } |
2923 | |
2924 | |
2925 | |
2926 | |
2927 | // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z} |
2928 | jerror_t DTrackFitterKalmanSIMD::StepJacobian(const DVector2 &xy, |
2929 | double ds,const DMatrix5x1 &S, |
2930 | double dEdx,DMatrix5x5 &J){ |
2931 | // Initialize the Jacobian matrix |
2932 | //J.Zero(); |
2933 | //for (int i=0;i<5;i++) J(i,i)=1.; |
2934 | J=I5x5; |
2935 | |
2936 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2937 | |
2938 | // Matrices for intermediate steps |
2939 | DMatrix5x5 J1; |
2940 | DMatrix5x1 D1; |
2941 | DVector2 dxy1; |
2942 | |
2943 | // charge |
2944 | double q=(S(state_q_over_pt)>0.0)?1.:-1.; |
2945 | q*=FactorForSenseOfRotation; |
2946 | |
2947 | //kinematic quantities |
2948 | double qpt=1./S(state_q_over_pt) * FactorForSenseOfRotation; |
2949 | double sinphi=sin(S(state_phi)); |
2950 | double cosphi=cos(S(state_phi)); |
2951 | double D=S(state_D); |
2952 | |
2953 | CalcDerivAndJacobian(xy,dxy1,S,dEdx,J1,D1); |
2954 | // double Bz_=fabs(Bz); // needed for computing D |
2955 | |
2956 | // New Jacobian matrix |
2957 | J+=ds*J1; |
2958 | |
2959 | // change in position |
2960 | DVector2 dxy =ds*dxy1; |
2961 | |
2962 | // Deal with changes in D |
2963 | double B=sqrt(Bx*Bx+By*By+Bz*Bz); |
2964 | //double qrc_old=qpt/(qBr2p*Bz_); |
2965 | double qrc_old=qpt/(qBr2p0.003*B); |
2966 | double qrc_plus_D=D+qrc_old; |
2967 | double dx=dxy.X(); |
2968 | double dy=dxy.Y(); |
2969 | double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi; |
2970 | double rc=sqrt(dxy.Mod2() |
2971 | +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi) |
2972 | +qrc_plus_D*qrc_plus_D); |
2973 | double q_over_rc=q/rc; |
2974 | |
2975 | J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D); |
2976 | J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.); |
2977 | J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi); |
2978 | |
2979 | return NOERROR; |
2980 | } |
2981 | |
2982 | // Compute contributions to the covariance matrix due to multiple scattering |
2983 | // using the Lynch/Dahl empirical formulas |
2984 | jerror_t DTrackFitterKalmanSIMD::GetProcessNoiseCentral(double ds, |
2985 | double chi2c_factor, |
2986 | double chi2a_factor, |
2987 | double chi2a_corr, |
2988 | const DMatrix5x1 &Sc, |
2989 | DMatrix5x5 &Q){ |
2990 | Q.Zero(); |
2991 | //return NOERROR; |
2992 | if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS3.0e-8){ |
2993 | double tanl=Sc(state_tanl); |
2994 | double tanl2=tanl*tanl; |
2995 | double one_plus_tanl2=1.+tanl2; |
2996 | double one_over_pt=fabs(Sc(state_q_over_pt)); |
2997 | double my_ds=fabs(ds); |
2998 | double my_ds_2=0.5*my_ds; |
2999 | |
3000 | Q(state_phi,state_phi)=one_plus_tanl2; |
3001 | Q(state_tanl,state_tanl)=one_plus_tanl2*one_plus_tanl2; |
3002 | Q(state_q_over_pt,state_q_over_pt)=one_over_pt*one_over_pt*tanl2; |
3003 | Q(state_q_over_pt,state_tanl)=Q(state_tanl,state_q_over_pt) |
3004 | =Sc(state_q_over_pt)*tanl*one_plus_tanl2; |
3005 | Q(state_D,state_D)=ONE_THIRD0.33333333333333333*ds*ds; |
3006 | |
3007 | // I am not sure the following is correct... |
3008 | double sign_D=(Sc(state_D)>0.0)?1.:-1.; |
3009 | Q(state_D,state_phi)=Q(state_phi,state_D)=sign_D*my_ds_2*sqrt(one_plus_tanl2); |
3010 | Q(state_D,state_tanl)=Q(state_tanl,state_D)=sign_D*my_ds_2*one_plus_tanl2; |
3011 | Q(state_D,state_q_over_pt)=Q(state_q_over_pt,state_D)=sign_D*my_ds_2*tanl*Sc(state_q_over_pt); |
3012 | |
3013 | double one_over_p_sq=one_over_pt*one_over_pt/one_plus_tanl2; |
3014 | double one_over_beta2=1.+mass2*one_over_p_sq; |
3015 | double chi2c_p_sq=chi2c_factor*my_ds*one_over_beta2; |
3016 | double chi2a_p_sq=chi2a_factor*(1.+chi2a_corr*one_over_beta2); |
3017 | // F=Fraction of Moliere distribution to be taken into account |
3018 | // nu=0.5*chi2c/(chi2a*(1.-F)); |
3019 | double nu=MOLIERE_RATIO1*chi2c_p_sq/chi2a_p_sq; |
3020 | double one_plus_nu=1.+nu; |
3021 | // sig2_ms=chi2c*1e-6/(1.+F*F)*((one_plus_nu)/nu*log(one_plus_nu)-1.); |
3022 | double sig2_ms=chi2c_p_sq*one_over_p_sq*MOLIERE_RATIO2 |
3023 | *(one_plus_nu/nu*log(one_plus_nu)-1.); |
3024 | |
3025 | Q*=sig2_ms; |
3026 | } |
3027 | |
3028 | return NOERROR; |
3029 | } |
3030 | |
3031 | // Compute contributions to the covariance matrix due to multiple scattering |
3032 | // using the Lynch/Dahl empirical formulas |
3033 | jerror_t DTrackFitterKalmanSIMD::GetProcessNoise(double z, double ds, |
3034 | double chi2c_factor, |
3035 | double chi2a_factor, |
3036 | double chi2a_corr, |
3037 | const DMatrix5x1 &S, |
3038 | DMatrix5x5 &Q){ |
3039 | |
3040 | Q.Zero(); |
3041 | //return NOERROR; |
3042 | if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS3.0e-8){ |
3043 | double tx=S(state_tx),ty=S(state_ty); |
3044 | double one_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
3045 | double my_ds=fabs(ds); |
3046 | double my_ds_2=0.5*my_ds; |
3047 | double tx2=tx*tx; |
3048 | double ty2=ty*ty; |
3049 | double one_plus_tx2=1.+tx2; |
3050 | double one_plus_ty2=1.+ty2; |
3051 | double tsquare=tx2+ty2; |
3052 | double one_plus_tsquare=1.+tsquare; |
3053 | |
3054 | Q(state_tx,state_tx)=one_plus_tx2*one_plus_tsquare; |
3055 | Q(state_ty,state_ty)=one_plus_ty2*one_plus_tsquare; |
3056 | Q(state_tx,state_ty)=Q(state_ty,state_tx)=tx*ty*one_plus_tsquare; |
3057 | |
3058 | Q(state_x,state_x)=ONE_THIRD0.33333333333333333*ds*ds; |
3059 | Q(state_y,state_y)=Q(state_x,state_x); |
3060 | Q(state_y,state_ty)=Q(state_ty,state_y) |
3061 | = my_ds_2*sqrt(one_plus_tsquare*one_plus_ty2); |
3062 | Q(state_x,state_tx)=Q(state_tx,state_x) |
3063 | = my_ds_2*sqrt(one_plus_tsquare*one_plus_tx2); |
3064 | |
3065 | double one_over_beta2=1.+one_over_p_sq*mass2; |
3066 | double chi2c_p_sq=chi2c_factor*my_ds*one_over_beta2; |
3067 | double chi2a_p_sq=chi2a_factor*(1.+chi2a_corr*one_over_beta2); |
3068 | // F=MOLIERE_FRACTION =Fraction of Moliere distribution to be taken into account |
3069 | // nu=0.5*chi2c/(chi2a*(1.-F)); |
3070 | double nu=MOLIERE_RATIO1*chi2c_p_sq/chi2a_p_sq; |
3071 | double one_plus_nu=1.+nu; |
3072 | double sig2_ms=chi2c_p_sq*one_over_p_sq*MOLIERE_RATIO2 |
3073 | *(one_plus_nu/nu*log(one_plus_nu)-1.); |
3074 | |
3075 | // printf("fac %f %f %f\n",chi2c_factor,chi2a_factor,chi2a_corr); |
3076 | //printf("omega %f\n",chi2c/chi2a); |
3077 | |
3078 | |
3079 | Q*=sig2_ms; |
3080 | } |
3081 | |
3082 | return NOERROR; |
3083 | } |
3084 | |
3085 | // Calculate the energy loss per unit length given properties of the material |
3086 | // through which a particle of momentum p is passing |
3087 | double DTrackFitterKalmanSIMD::GetdEdx(double q_over_p,double K_rho_Z_over_A, |
3088 | double rho_Z_over_A,double LnI,double Z){ |
3089 | if (rho_Z_over_A<=0.) return 0.; |
3090 | //return 0.; |
3091 | |
3092 | double p=fabs(1./q_over_p); |
3093 | double betagamma=p/MASS; |
3094 | double betagamma2=betagamma*betagamma; |
3095 | double gamma2=1.+betagamma2; |
3096 | double beta2=betagamma2/gamma2; |
3097 | if (beta2<EPS3.0e-8) beta2=EPS3.0e-8; |
3098 | |
3099 | // density effect |
3100 | double delta=CalcDensityEffect(betagamma,rho_Z_over_A,LnI); |
3101 | |
3102 | double dEdx=0.; |
3103 | // For particles heavier than electrons: |
3104 | if (IsHadron){ |
3105 | double two_Me_betagamma_sq=two_m_e*betagamma2; |
3106 | double Tmax |
3107 | =two_Me_betagamma_sq/(1.+2.*sqrt(gamma2)*m_ratio+m_ratio_sq); |
3108 | |
3109 | dEdx=K_rho_Z_over_A/beta2*(-log(two_Me_betagamma_sq*Tmax) |
3110 | +2.*(LnI + beta2)+delta); |
3111 | } |
3112 | else{ |
3113 | // relativistic kinetic energy in units of M_e c^2 |
3114 | double tau=sqrt(gamma2)-1.; |
3115 | double tau_sq=tau*tau; |
3116 | double tau_plus_1=tau+1.; |
3117 | double tau_plus_2=tau+2.; |
3118 | double tau_plus_2_sq=tau_plus_2*tau_plus_2; |
3119 | double f=0.; // function that depends on tau; see Leo (2nd ed.), p. 38. |
3120 | if (IsElectron){ |
3121 | f=1.-beta2+(0.125*tau_sq-(2.*tau+1.)*log(2.))/(tau_plus_1*tau_plus_1); |
3122 | } |
3123 | else{ |
3124 | f=2.*log(2.)-(beta2/12.)*(23.+14./tau_plus_2+10./tau_plus_2_sq |
3125 | +4./(tau_plus_2*tau_plus_2_sq)); |
3126 | } |
3127 | |
3128 | // collision loss (Leo eq. 2.66) |
3129 | double dEdx_coll |
3130 | =-K_rho_Z_over_A/beta2*(log(0.5*tau_sq*tau_plus_2*m_e_sq)-LnI+f-delta); |
3131 | |
3132 | // radiation loss (Leo eqs. 2.74, 2.76 with Z^2 -> Z(Z+1) |
3133 | double a=Z*ALPHA1./137.; |
3134 | double a2=a*a; |
3135 | double a4=a2*a2; |
3136 | double epsilon=1.-PHOTON_ENERGY_CUTOFF; |
3137 | double epsilon2=epsilon*epsilon; |
3138 | double f_Z=a2*(1./(1.+a2)+0.20206-0.0369*a2+0.0083*a4-0.002*a2*a4); |
3139 | // The expression below is the integral of the photon energy weighted |
3140 | // by the bremsstrahlung cross section up to a maximum photon energy |
3141 | // expressed as a fraction of the incident electron energy. |
3142 | double dEdx_rad=-K_rho_Z_over_A*tau_plus_1*(2.*ALPHA1./137./M_PI3.14159265358979323846)*(Z+1.) |
3143 | *((log(183.*pow(Z,-1./3.))-f_Z) |
3144 | *(1.-epsilon-(1./3.)*(epsilon2-epsilon*epsilon2)) |
3145 | +1./18.*(1.-epsilon2)); |
3146 | |
3147 | |
3148 | // dEdx_rad=0.; |
3149 | |
3150 | dEdx=dEdx_coll+dEdx_rad; |
3151 | } |
3152 | |
3153 | return dEdx; |
3154 | } |
3155 | |
3156 | // Calculate the variance in the energy loss in a Gaussian approximation. |
3157 | // The standard deviation of the energy loss distribution is |
3158 | // var=0.1535*density*(Z/A)*x*(1-0.5*beta^2)*Tmax [MeV] |
3159 | // where Tmax is the maximum energy transfer. |
3160 | // (derived from Leo (2nd ed.), eq. 2.100. Note that I think there is a typo |
3161 | // in this formula in the text...) |
3162 | double DTrackFitterKalmanSIMD::GetEnergyVariance(double ds, |
3163 | double one_over_beta2, |
3164 | double K_rho_Z_over_A){ |
3165 | if (K_rho_Z_over_A<=0.) return 0.; |
3166 | |
3167 | double betagamma2=1./(one_over_beta2-1.); |
3168 | double gamma2=betagamma2*one_over_beta2; |
3169 | double two_Me_betagamma_sq=two_m_e*betagamma2; |
3170 | double Tmax=two_Me_betagamma_sq/(1.+2.*sqrt(gamma2)*m_ratio+m_ratio_sq); |
3171 | double var=K_rho_Z_over_A*one_over_beta2*fabs(ds)*Tmax*(1.-0.5/one_over_beta2); |
3172 | return var; |
3173 | } |
3174 | |
3175 | // Interface routine for Kalman filter |
3176 | jerror_t DTrackFitterKalmanSIMD::KalmanLoop(void){ |
3177 | if (z_<Z_MIN-100.) return VALUE_OUT_OF_RANGE; |
3178 | |
3179 | // Vector to store the list of hits used in the fit for the forward parametrization |
3180 | vector<const DCDCTrackHit*>forward_cdc_used_in_fit; |
3181 | |
3182 | // State vector and initial guess for covariance matrix |
3183 | DMatrix5x1 S0; |
3184 | DMatrix5x5 C0; |
3185 | |
3186 | chisq_=-1.; |
3187 | |
3188 | // Angle with respect to beam line |
3189 | double theta_deg=(180/M_PI3.14159265358979323846)*input_params.momentum().Theta(); |
3190 | //double theta_deg_sq=theta_deg*theta_deg; |
3191 | double tanl0=tanl_=tan(M_PI_21.57079632679489661923-input_params.momentum().Theta()); |
3192 | |
3193 | // Azimuthal angle |
3194 | double phi0=phi_=input_params.momentum().Phi(); |
3195 | |
3196 | // Guess for momentum error |
3197 | double dpt_over_pt=0.1; |
3198 | /* |
3199 | if (theta_deg<15){ |
3200 | dpt_over_pt=0.107-0.0178*theta_deg+0.000966*theta_deg_sq; |
3201 | } |
3202 | else { |
3203 | dpt_over_pt=0.0288+0.00579*theta_deg-2.77e-5*theta_deg_sq; |
3204 | } |
3205 | */ |
3206 | /* |
3207 | if (theta_deg<28.){ |
3208 | theta_deg=28.; |
3209 | theta_deg_sq=theta_deg*theta_deg; |
3210 | } |
3211 | else if (theta_deg>125.){ |
3212 | theta_deg=125.; |
3213 | theta_deg_sq=theta_deg*theta_deg; |
3214 | } |
3215 | */ |
3216 | double sig_lambda=0.02; |
3217 | double dp_over_p_sq |
3218 | =dpt_over_pt*dpt_over_pt+tanl_*tanl_*sig_lambda*sig_lambda; |
3219 | |
3220 | // Input charge |
3221 | double q=input_params.charge(); |
3222 | |
3223 | // Input momentum |
3224 | DVector3 pvec=input_params.momentum(); |
3225 | double p_mag=pvec.Mag(); |
3226 | double px=pvec.x(); |
3227 | double py=pvec.y(); |
3228 | double pz=pvec.z(); |
3229 | double q_over_p0=q_over_p_=q/p_mag; |
3230 | double q_over_pt0=q_over_pt_=q/pvec.Perp(); |
3231 | |
3232 | // Initial position |
3233 | double x0=x_=input_params.position().x(); |
3234 | double y0=y_=input_params.position().y(); |
3235 | double z0=z_=input_params.position().z(); |
3236 | |
3237 | if (fit_type==kWireBased && theta_deg>10.){ |
3238 | double Bz=fabs(bfield->GetBz(x0,y0,z0)); |
3239 | double sperp=25.; // empirical guess |
3240 | if (my_fdchits.size()>0 && my_fdchits[0]->hit!=NULL__null){ |
3241 | double my_z=my_fdchits[0]->z; |
3242 | double my_x=my_fdchits[0]->hit->xy.X(); |
3243 | double my_y=my_fdchits[0]->hit->xy.Y(); |
3244 | Bz+=fabs(bfield->GetBz(my_x,my_y,my_z)); |
3245 | Bz*=0.5; // crude average |
3246 | sperp=(my_z-z0)/tanl_; |
3247 | } |
3248 | double twokappa=qBr2p0.003*Bz*q_over_pt0*FactorForSenseOfRotation; |
3249 | double one_over_2k=1./twokappa; |
3250 | if (my_fdchits.size()==0){ |
3251 | for (unsigned int i=my_cdchits.size()-1;i>1;i--){ |
3252 | // Use outermost axial straw to estimate a resonable arc length |
3253 | if (my_cdchits[i]->hit->is_stereo==false){ |
3254 | double tworc=2.*fabs(one_over_2k); |
3255 | double ratio=(my_cdchits[i]->hit->wire->origin |
3256 | -input_params.position()).Perp()/tworc; |
3257 | sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_21.57079632679489661923; |
3258 | if (sperp<25.) sperp=25.; |
3259 | break; |
3260 | } |
3261 | } |
3262 | } |
3263 | double twoks=twokappa*sperp; |
3264 | double cosphi=cos(phi0); |
3265 | double sinphi=sin(phi0); |
3266 | double sin2ks=sin(twoks); |
3267 | double cos2ks=cos(twoks); |
3268 | double one_minus_cos2ks=1.-cos2ks; |
3269 | double myx=x0+one_over_2k*(cosphi*sin2ks-sinphi*one_minus_cos2ks); |
3270 | double myy=y0+one_over_2k*(sinphi*sin2ks+cosphi*one_minus_cos2ks); |
3271 | double mypx=px*cos2ks-py*sin2ks; |
3272 | double mypy=py*cos2ks+px*sin2ks; |
3273 | double myphi=atan2(mypy,mypx); |
3274 | phi0=phi_=myphi; |
3275 | px=mypx; |
3276 | py=mypy; |
3277 | x0=x_=myx; |
3278 | y0=y_=myy; |
3279 | z0+=tanl_*sperp; |
3280 | z_=z0; |
3281 | } |
3282 | |
3283 | // Check integrity of input parameters |
3284 | if (!isfinite(x0) || !isfinite(y0) || !isfinite(q_over_p0)){ |
3285 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3285<<" " << "Invalid input parameters!" <<endl; |
3286 | return UNRECOVERABLE_ERROR; |
3287 | } |
3288 | |
3289 | // Initial direction tangents |
3290 | double tx0=tx_=px/pz; |
3291 | double ty0=ty_=py/pz; |
3292 | double one_plus_tsquare=1.+tx_*tx_+ty_*ty_; |
3293 | |
3294 | // deal with hits in FDC |
3295 | double fdc_prob=0.,fdc_chisq=-1.; |
3296 | unsigned int fdc_ndf=0; |
3297 | if (my_fdchits.size()>0 |
3298 | && // Make sure that these parameters are valid for forward-going tracks |
3299 | (isfinite(tx0) && isfinite(ty0)) |
3300 | ){ |
3301 | if (DEBUG_LEVEL>0){ |
3302 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3302<<" " << "Using forward parameterization." <<endl; |
3303 | } |
3304 | |
3305 | // Initial guess for the state vector |
3306 | S0(state_x)=x_; |
3307 | S0(state_y)=y_; |
3308 | S0(state_tx)=tx_; |
3309 | S0(state_ty)=ty_; |
3310 | S0(state_q_over_p)=q_over_p_; |
3311 | |
3312 | // Initial guess for forward representation covariance matrix |
3313 | C0(state_x,state_x)=2.0; |
3314 | C0(state_y,state_y)=2.0; |
3315 | double temp=sig_lambda*one_plus_tsquare; |
3316 | C0(state_tx,state_tx)=C0(state_ty,state_ty)=temp*temp; |
3317 | C0(state_q_over_p,state_q_over_p)=dp_over_p_sq*q_over_p_*q_over_p_; |
3318 | C0*=COVARIANCE_SCALE_FACTOR_FORWARD; |
3319 | |
3320 | if (my_cdchits.size()>0){ |
3321 | mCDCInternalStepSize=0.25; |
3322 | } |
3323 | |
3324 | // The position from the track candidate is reported just outside the |
3325 | // start counter for tracks containing cdc hits. Propagate to the distance |
3326 | // of closest approach to the beam line |
3327 | if (fit_type==kWireBased) ExtrapolateToVertex(S0); |
3328 | |
3329 | kalman_error_t error=ForwardFit(S0,C0); |
3330 | if (error==FIT_SUCCEEDED) return NOERROR; |
3331 | if ((error==FIT_FAILED || ndf_==0) && my_cdchits.size()<6){ |
3332 | return UNRECOVERABLE_ERROR; |
3333 | } |
3334 | |
3335 | fdc_prob=TMath::Prob(chisq_,ndf_); |
3336 | fdc_ndf=ndf_; |
3337 | fdc_chisq=chisq_; |
3338 | } |
3339 | |
3340 | // Deal with hits in the CDC |
3341 | if (my_cdchits.size()>5){ |
3342 | kalman_error_t error=FIT_NOT_DONE; |
3343 | kalman_error_t cdc_error=FIT_NOT_DONE; |
3344 | |
3345 | // Save the current state of the extrapolation vector if it exists |
3346 | map<DetectorSystem_t,vector<Extrapolation_t> >saved_extrapolations; |
3347 | if (!extrapolations.empty()){ |
3348 | saved_extrapolations=extrapolations; |
3349 | ClearExtrapolations(); |
3350 | } |
3351 | bool save_IsSmoothed=IsSmoothed; |
3352 | |
3353 | // Chi-squared, degrees of freedom, and probability |
3354 | double forward_prob=0.; |
3355 | double chisq_forward=-1.; |
3356 | unsigned int ndof_forward=0; |
3357 | |
3358 | // Parameters at "vertex" |
3359 | double phi=phi_,q_over_pt=q_over_pt_,tanl=tanl_,x=x_,y=y_,z=z_; |
3360 | vector< vector <double> > fcov_save; |
3361 | vector<pull_t>pulls_save; |
3362 | pulls_save.assign(pulls.begin(),pulls.end()); |
3363 | if (!fcov.empty()){ |
3364 | fcov_save.assign(fcov.begin(),fcov.end()); |
3365 | } |
3366 | if (my_fdchits.size()>0){ |
3367 | if (error==INVALID_FIT) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3367<<" "<< "Invalid fit " << fcov.size() << " " << fdc_ndf <<endl; |
3368 | } |
3369 | |
3370 | // Use forward parameters for CDC-only tracks with theta<THETA_CUT degrees |
3371 | if (theta_deg<THETA_CUT){ |
3372 | if (DEBUG_LEVEL>0){ |
3373 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3373<<" " << "Using forward parameterization." <<endl; |
3374 | } |
3375 | |
3376 | // Step size |
3377 | mStepSizeS=mCentralStepSize; |
3378 | |
3379 | // Initialize the state vector |
3380 | S0(state_x)=x_=x0; |
3381 | S0(state_y)=y_=y0; |
3382 | S0(state_tx)=tx_=tx0; |
3383 | S0(state_ty)=ty_=ty0; |
3384 | S0(state_q_over_p)=q_over_p_=q_over_p0; |
3385 | z_=z0; |
3386 | |
3387 | // Initial guess for forward representation covariance matrix |
3388 | double temp=sig_lambda*one_plus_tsquare; |
3389 | C0(state_x,state_x)=2.0; |
3390 | C0(state_y,state_y)=2.0; |
3391 | C0(state_tx,state_tx)=C0(state_ty,state_ty)=temp*temp; |
3392 | C0(state_q_over_p,state_q_over_p)=dp_over_p_sq*q_over_p_*q_over_p_; |
3393 | C0*=COVARIANCE_SCALE_FACTOR_FORWARD; |
3394 | |
3395 | //C0*=1.+1./tsquare; |
3396 | |
3397 | // The position from the track candidate is reported just outside the |
3398 | // start counter for tracks containing cdc hits. Propagate to the |
3399 | // distance of closest approach to the beam line |
3400 | if (fit_type==kWireBased) ExtrapolateToVertex(S0); |
3401 | |
3402 | error=ForwardCDCFit(S0,C0); |
3403 | if (error==FIT_SUCCEEDED) return NOERROR; |
3404 | |
3405 | // Find the CL of the fit |
3406 | forward_prob=TMath::Prob(chisq_,ndf_); |
3407 | if (my_fdchits.size()>0){ |
3408 | if (fdc_ndf==0 || forward_prob>fdc_prob){ |
3409 | // We did not end up using the fdc hits after all... |
3410 | fdchits_used_in_fit.clear(); |
3411 | } |
3412 | else{ |
3413 | chisq_=fdc_chisq; |
3414 | ndf_=fdc_ndf; |
3415 | x_=x; |
3416 | y_=y; |
3417 | z_=z; |
3418 | phi_=phi; |
3419 | tanl_=tanl; |
3420 | q_over_pt_=q_over_pt; |
3421 | if (!fcov_save.empty()){ |
3422 | fcov.assign(fcov_save.begin(),fcov_save.end()); |
3423 | } |
3424 | if (!saved_extrapolations.empty()){ |
3425 | extrapolations=saved_extrapolations; |
3426 | } |
3427 | IsSmoothed=save_IsSmoothed; |
3428 | pulls.assign(pulls_save.begin(),pulls_save.end()); |
3429 | |
3430 | // _DBG_ << endl; |
3431 | return NOERROR; |
3432 | } |
3433 | |
3434 | // Save the best values for the parameters and chi2 for now |
3435 | chisq_forward=chisq_; |
3436 | ndof_forward=ndf_; |
3437 | x=x_; |
3438 | y=y_; |
3439 | z=z_; |
3440 | phi=phi_; |
3441 | tanl=tanl_; |
3442 | q_over_pt=q_over_pt_; |
3443 | fcov_save.assign(fcov.begin(),fcov.end()); |
3444 | pulls_save.assign(pulls.begin(),pulls.end()); |
3445 | save_IsSmoothed=IsSmoothed; |
3446 | if (!extrapolations.empty()){ |
3447 | saved_extrapolations=extrapolations; |
3448 | ClearExtrapolations(); |
3449 | } |
3450 | |
3451 | // Save the list of hits used in the fit |
3452 | forward_cdc_used_in_fit.assign(cdchits_used_in_fit.begin(),cdchits_used_in_fit.end()); |
3453 | |
3454 | } |
3455 | } |
3456 | |
3457 | // Attempt to fit the track using the central parametrization. |
3458 | if (DEBUG_LEVEL>0){ |
3459 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3459<<" " << "Using central parameterization." <<endl; |
3460 | } |
3461 | |
3462 | // Step size |
3463 | mStepSizeS=mCentralStepSize; |
3464 | |
3465 | // Initialize the state vector |
3466 | S0(state_q_over_pt)=q_over_pt_=q_over_pt0; |
3467 | S0(state_phi)=phi_=phi0; |
3468 | S0(state_tanl)=tanl_=tanl0; |
3469 | S0(state_z)=z_=z0; |
3470 | S0(state_D)=0.; |
3471 | |
3472 | // Initialize the covariance matrix |
3473 | double dz=1.0; |
3474 | C0(state_z,state_z)=dz*dz; |
3475 | C0(state_q_over_pt,state_q_over_pt) |
3476 | =q_over_pt_*q_over_pt_*dpt_over_pt*dpt_over_pt; |
3477 | double dphi=0.02; |
3478 | C0(state_phi,state_phi)=dphi*dphi; |
3479 | C0(state_D,state_D)=1.0; |
3480 | double tanl2=tanl_*tanl_; |
3481 | double one_plus_tanl2=1.+tanl2; |
3482 | C0(state_tanl,state_tanl)=(one_plus_tanl2)*(one_plus_tanl2) |
3483 | *sig_lambda*sig_lambda; |
3484 | C0*=COVARIANCE_SCALE_FACTOR_CENTRAL; |
3485 | |
3486 | //if (theta_deg>90.) C0*=1.+5.*tanl2; |
3487 | //else C0*=1.+5.*tanl2*tanl2; |
3488 | |
3489 | mCentralStepSize=0.4; |
3490 | mCDCInternalStepSize=0.2; |
3491 | |
3492 | // The position from the track candidate is reported just outside the |
3493 | // start counter for tracks containing cdc hits. Propagate to the |
3494 | // distance of closest approach to the beam line |
3495 | DVector2 xy(x0,y0); |
3496 | if (fit_type==kWireBased){ |
3497 | ExtrapolateToVertex(xy,S0); |
3498 | } |
3499 | |
3500 | cdc_error=CentralFit(xy,S0,C0); |
3501 | if (cdc_error==FIT_SUCCEEDED){ |
3502 | // if the result of the fit using the forward parameterization succeeded |
3503 | // but the chi2 was too high, it still may provide the best estimate |
3504 | // for the track parameters... |
3505 | double central_prob=TMath::Prob(chisq_,ndf_); |
3506 | |
3507 | if (central_prob<forward_prob){ |
3508 | phi_=phi; |
3509 | q_over_pt_=q_over_pt; |
3510 | tanl_=tanl; |
3511 | x_=x; |
3512 | y_=y; |
3513 | z_=z; |
3514 | chisq_=chisq_forward; |
3515 | ndf_= ndof_forward; |
3516 | fcov.assign(fcov_save.begin(),fcov_save.end()); |
3517 | pulls.assign(pulls_save.begin(),pulls_save.end()); |
3518 | IsSmoothed=save_IsSmoothed; |
3519 | if (!saved_extrapolations.empty()){ |
3520 | extrapolations=saved_extrapolations; |
3521 | } |
3522 | |
3523 | cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end()); |
3524 | |
3525 | // We did not end up using any fdc hits... |
3526 | fdchits_used_in_fit.clear(); |
3527 | |
3528 | } |
3529 | return NOERROR; |
3530 | |
3531 | } |
3532 | // otherwise if the fit using the forward parametrization worked, return that |
3533 | else if (error!=FIT_FAILED){ |
3534 | phi_=phi; |
3535 | q_over_pt_=q_over_pt; |
3536 | tanl_=tanl; |
3537 | x_=x; |
3538 | y_=y; |
3539 | z_=z; |
3540 | chisq_=chisq_forward; |
3541 | ndf_= ndof_forward; |
3542 | |
3543 | if (!saved_extrapolations.empty()){ |
3544 | extrapolations=saved_extrapolations; |
3545 | } |
3546 | IsSmoothed=save_IsSmoothed; |
3547 | fcov.assign(fcov_save.begin(),fcov_save.end()); |
3548 | pulls.assign(pulls_save.begin(),pulls_save.end()); |
3549 | cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end()); |
3550 | |
3551 | // We did not end up using any fdc hits... |
3552 | fdchits_used_in_fit.clear(); |
3553 | |
3554 | return NOERROR; |
3555 | } |
3556 | else return UNRECOVERABLE_ERROR; |
3557 | } |
3558 | |
3559 | if (ndf_==0) return UNRECOVERABLE_ERROR; |
3560 | |
3561 | return NOERROR; |
3562 | } |
3563 | |
3564 | #define ITMAX20 20 |
3565 | #define CGOLD0.3819660 0.3819660 |
3566 | #define ZEPS1.0e-10 1.0e-10 |
3567 | #define SHFT(a,b,c,d)(a)=(b);(b)=(c);(c)=(d); (a)=(b);(b)=(c);(c)=(d); |
3568 | #define SIGN(a,b)((b)>=0.0?fabs(a):-fabs(a)) ((b)>=0.0?fabs(a):-fabs(a)) |
3569 | // Routine for finding the minimum of a function bracketed between two values |
3570 | // (see Numerical Recipes in C, pp. 404-405). |
3571 | jerror_t DTrackFitterKalmanSIMD::BrentsAlgorithm(double ds1,double ds2, |
3572 | double dedx,DVector2 &pos, |
3573 | const double z0wire, |
3574 | const DVector2 &origin, |
3575 | const DVector2 &dir, |
3576 | DMatrix5x1 &Sc, |
3577 | double &ds_out){ |
3578 | double d=0.; |
3579 | double e=0.0; // will be distance moved on step before last |
3580 | double ax=0.; |
3581 | double bx=-ds1; |
3582 | double cx=-ds1-ds2; |
3583 | |
3584 | double a=(ax<cx?ax:cx); |
3585 | double b=(ax>cx?ax:cx); |
3586 | double x=bx,w=bx,v=bx; |
3587 | |
3588 | // printf("ds1 %f ds2 %f\n",ds1,ds2); |
3589 | // Initialize return step size |
3590 | ds_out=0.; |
3591 | |
3592 | // Save the starting position |
3593 | // DVector3 pos0=pos; |
3594 | // DMatrix S0(Sc); |
3595 | |
3596 | // Step to intermediate point |
3597 | Step(pos,x,Sc,dedx); |
3598 | // Bail if the transverse momentum has dropped below some minimum |
3599 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3600 | if (DEBUG_LEVEL>2) |
3601 | { |
3602 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3602<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3603 | << endl; |
3604 | } |
3605 | return VALUE_OUT_OF_RANGE; |
3606 | } |
3607 | |
3608 | DVector2 wirepos=origin+(Sc(state_z)-z0wire)*dir; |
3609 | double u_old=x; |
3610 | double u=0.; |
3611 | |
3612 | // initialization |
3613 | double fw=(pos-wirepos).Mod2(); |
3614 | double fv=fw,fx=fw; |
3615 | |
3616 | // main loop |
3617 | for (unsigned int iter=1;iter<=ITMAX20;iter++){ |
3618 | double xm=0.5*(a+b); |
3619 | double tol1=EPS21.e-4*fabs(x)+ZEPS1.0e-10; |
3620 | double tol2=2.0*tol1; |
3621 | |
3622 | if (fabs(x-xm)<=(tol2-0.5*(b-a))){ |
3623 | if (Sc(state_z)<=cdc_origin[2]){ |
3624 | unsigned int iter2=0; |
3625 | double ds_temp=0.; |
3626 | while (fabs(Sc(state_z)-cdc_origin[2])>EPS21.e-4 && iter2<20){ |
3627 | u=x-(cdc_origin[2]-Sc(state_z))*sin(atan(Sc(state_tanl))); |
3628 | x=u; |
3629 | ds_temp+=u_old-u; |
3630 | // Bail if the transverse momentum has dropped below some minimum |
3631 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3632 | if (DEBUG_LEVEL>2) |
3633 | { |
3634 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3634<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3635 | << endl; |
3636 | } |
3637 | return VALUE_OUT_OF_RANGE; |
3638 | } |
3639 | |
3640 | // Function evaluation |
3641 | Step(pos,u_old-u,Sc,dedx); |
3642 | u_old=u; |
3643 | iter2++; |
3644 | } |
3645 | //printf("new z %f ds %f \n",pos.z(),x); |
3646 | ds_out=ds_temp; |
3647 | return NOERROR; |
3648 | } |
3649 | else if (Sc(state_z)>=endplate_z){ |
3650 | unsigned int iter2=0; |
3651 | double ds_temp=0.; |
3652 | while (fabs(Sc(state_z)-endplate_z)>EPS21.e-4 && iter2<20){ |
3653 | u=x-(endplate_z-Sc(state_z))*sin(atan(Sc(state_tanl))); |
3654 | x=u; |
3655 | ds_temp+=u_old-u; |
3656 | |
3657 | // Bail if the transverse momentum has dropped below some minimum |
3658 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3659 | if (DEBUG_LEVEL>2) |
3660 | { |
3661 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3661<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3662 | << endl; |
3663 | } |
3664 | return VALUE_OUT_OF_RANGE; |
3665 | } |
3666 | |
3667 | // Function evaluation |
3668 | Step(pos,u_old-u,Sc,dedx); |
3669 | u_old=u; |
3670 | iter2++; |
3671 | } |
3672 | //printf("new z %f ds %f \n",pos.z(),x); |
3673 | ds_out=ds_temp; |
3674 | return NOERROR; |
3675 | } |
3676 | ds_out=cx-x; |
3677 | return NOERROR; |
3678 | } |
3679 | // trial parabolic fit |
3680 | if (fabs(e)>tol1){ |
3681 | double x_minus_w=x-w; |
3682 | double x_minus_v=x-v; |
3683 | double r=x_minus_w*(fx-fv); |
3684 | double q=x_minus_v*(fx-fw); |
3685 | double p=x_minus_v*q-x_minus_w*r; |
3686 | q=2.0*(q-r); |
3687 | if (q>0.0) p=-p; |
3688 | q=fabs(q); |
3689 | double etemp=e; |
3690 | e=d; |
3691 | if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x)) |
3692 | // fall back on the Golden Section technique |
3693 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3694 | else{ |
3695 | // parabolic step |
3696 | d=p/q; |
3697 | u=x+d; |
3698 | if (u-a<tol2 || b-u <tol2) |
3699 | d=SIGN(tol1,xm-x)((xm-x)>=0.0?fabs(tol1):-fabs(tol1)); |
3700 | } |
3701 | } else{ |
3702 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3703 | } |
3704 | u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d)((d)>=0.0?fabs(tol1):-fabs(tol1))); |
3705 | |
3706 | // Bail if the transverse momentum has dropped below some minimum |
3707 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3708 | if (DEBUG_LEVEL>2) |
3709 | { |
3710 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3710<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3711 | << endl; |
3712 | } |
3713 | return VALUE_OUT_OF_RANGE; |
3714 | } |
3715 | |
3716 | // Function evaluation |
3717 | Step(pos,u_old-u,Sc,dedx); |
3718 | u_old=u; |
3719 | |
3720 | wirepos=origin; |
3721 | wirepos+=(Sc(state_z)-z0wire)*dir; |
3722 | double fu=(pos-wirepos).Mod2(); |
3723 | |
3724 | //cout << "Brent: z="<<Sc(state_z) << " d="<<sqrt(fu) << endl; |
3725 | |
3726 | if (fu<=fx){ |
3727 | if (u>=x) a=x; else b=x; |
3728 | SHFT(v,w,x,u)(v)=(w);(w)=(x);(x)=(u);; |
3729 | SHFT(fv,fw,fx,fu)(fv)=(fw);(fw)=(fx);(fx)=(fu);; |
3730 | } |
3731 | else { |
3732 | if (u<x) a=u; else b=u; |
3733 | if (fu<=fw || w==x){ |
3734 | v=w; |
3735 | w=u; |
3736 | fv=fw; |
3737 | fw=fu; |
3738 | } |
3739 | else if (fu<=fv || v==x || v==w){ |
3740 | v=u; |
3741 | fv=fu; |
3742 | } |
3743 | } |
3744 | } |
3745 | ds_out=cx-x; |
3746 | |
3747 | return NOERROR; |
3748 | } |
3749 | |
3750 | // Routine for finding the minimum of a function bracketed between two values |
3751 | // (see Numerical Recipes in C, pp. 404-405). |
3752 | jerror_t DTrackFitterKalmanSIMD::BrentsAlgorithm(double z,double dz, |
3753 | double dedx, |
3754 | const double z0wire, |
3755 | const DVector2 &origin, |
3756 | const DVector2 &dir, |
3757 | DMatrix5x1 &S, |
3758 | double &dz_out){ |
3759 | double d=0.,u=0.; |
3760 | double e=0.0; // will be distance moved on step before last |
3761 | double ax=0.; |
3762 | double bx=-dz; |
3763 | double cx=-2.*dz; |
3764 | |
3765 | double a=(ax<cx?ax:cx); |
3766 | double b=(ax>cx?ax:cx); |
3767 | double x=bx,w=bx,v=bx; |
3768 | |
3769 | // Initialize dz_out |
3770 | dz_out=0.; |
3771 | |
3772 | // Step to intermediate point |
3773 | double z_new=z+x; |
3774 | Step(z,z_new,dedx,S); |
3775 | // Bail if the momentum has dropped below some minimum |
3776 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
3777 | if (DEBUG_LEVEL>2) |
3778 | { |
3779 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3779<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
3780 | << endl; |
3781 | } |
3782 | return VALUE_OUT_OF_RANGE; |
3783 | } |
3784 | |
3785 | double dz0wire=z-z0wire; |
3786 | DVector2 wirepos=origin+(dz0wire+x)*dir; |
3787 | DVector2 pos(S(state_x),S(state_y)); |
3788 | double z_old=z_new; |
3789 | |
3790 | // initialization |
3791 | double fw=(pos-wirepos).Mod2(); |
3792 | double fv=fw; |
3793 | double fx=fw; |
3794 | |
3795 | // main loop |
3796 | for (unsigned int iter=1;iter<=ITMAX20;iter++){ |
3797 | double xm=0.5*(a+b); |
3798 | double tol1=EPS21.e-4*fabs(x)+ZEPS1.0e-10; |
3799 | double tol2=2.0*tol1; |
3800 | if (fabs(x-xm)<=(tol2-0.5*(b-a))){ |
3801 | if (z_new>=endplate_z){ |
3802 | x=endplate_z-z_new; |
3803 | |
3804 | // Bail if the momentum has dropped below some minimum |
3805 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
3806 | if (DEBUG_LEVEL>2) |
3807 | { |
3808 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3808<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
3809 | << endl; |
3810 | } |
3811 | return VALUE_OUT_OF_RANGE; |
3812 | } |
3813 | if (!isfinite(S(state_x)) || !isfinite(S(state_y))){ |
3814 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3814<<" " <<endl; |
3815 | return VALUE_OUT_OF_RANGE; |
3816 | } |
3817 | Step(z_new,endplate_z,dedx,S); |
3818 | } |
3819 | dz_out=x; |
3820 | return NOERROR; |
3821 | } |
3822 | // trial parabolic fit |
3823 | if (fabs(e)>tol1){ |
3824 | double x_minus_w=x-w; |
3825 | double x_minus_v=x-v; |
3826 | double r=x_minus_w*(fx-fv); |
3827 | double q=x_minus_v*(fx-fw); |
3828 | double p=x_minus_v*q-x_minus_w*r; |
3829 | q=2.0*(q-r); |
3830 | if (q>0.0) p=-p; |
3831 | q=fabs(q); |
3832 | double etemp=e; |
3833 | e=d; |
3834 | if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x)) |
3835 | // fall back on the Golden Section technique |
3836 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3837 | else{ |
3838 | // parabolic step |
3839 | d=p/q; |
3840 | u=x+d; |
3841 | if (u-a<tol2 || b-u <tol2) |
3842 | d=SIGN(tol1,xm-x)((xm-x)>=0.0?fabs(tol1):-fabs(tol1)); |
3843 | } |
3844 | } else{ |
3845 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3846 | } |
3847 | u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d)((d)>=0.0?fabs(tol1):-fabs(tol1))); |
3848 | |
3849 | // Function evaluation |
3850 | //S=S0; |
3851 | z_new=z+u; |
3852 | // Bail if the momentum has dropped below some minimum |
3853 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
3854 | if (DEBUG_LEVEL>2) |
3855 | { |
3856 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3856<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
3857 | << endl; |
3858 | } |
3859 | return VALUE_OUT_OF_RANGE; |
3860 | } |
3861 | |
3862 | Step(z_old,z_new,dedx,S); |
3863 | z_old=z_new; |
3864 | |
3865 | wirepos=origin; |
3866 | wirepos+=(dz0wire+u)*dir; |
3867 | pos.Set(S(state_x),S(state_y)); |
3868 | double fu=(pos-wirepos).Mod2(); |
3869 | |
3870 | // _DBG_ << "Brent: z="<< z+u << " d^2="<<fu << endl; |
3871 | |
3872 | if (fu<=fx){ |
3873 | if (u>=x) a=x; else b=x; |
3874 | SHFT(v,w,x,u)(v)=(w);(w)=(x);(x)=(u);; |
3875 | SHFT(fv,fw,fx,fu)(fv)=(fw);(fw)=(fx);(fx)=(fu);; |
3876 | } |
3877 | else { |
3878 | if (u<x) a=u; else b=u; |
3879 | if (fu<=fw || w==x){ |
3880 | v=w; |
3881 | w=u; |
3882 | fv=fw; |
3883 | fw=fu; |
3884 | } |
3885 | else if (fu<=fv || v==x || v==w){ |
3886 | v=u; |
3887 | fv=fu; |
3888 | } |
3889 | } |
3890 | } |
3891 | dz_out=x; |
3892 | return NOERROR; |
3893 | } |
3894 | |
3895 | // Kalman engine for Central tracks; updates the position on the trajectory |
3896 | // after the last hit (closest to the target) is added |
3897 | kalman_error_t DTrackFitterKalmanSIMD::KalmanCentral(double anneal_factor, |
3898 | DMatrix5x1 &Sc, |
3899 | DMatrix5x5 &Cc, |
3900 | DVector2 &xy,double &chisq, |
3901 | unsigned int &my_ndf |
3902 | ){ |
3903 | DMatrix1x5 H; // Track projection matrix |
3904 | DMatrix5x1 H_T; // Transpose of track projection matrix |
3905 | DMatrix5x5 J; // State vector Jacobian matrix |
3906 | DMatrix5x5 Q; // Process noise covariance matrix |
3907 | DMatrix5x1 K; // KalmanSIMD gain matrix |
3908 | DMatrix5x5 Ctest; // covariance matrix |
3909 | // double V=0.2028; //1.56*1.56/12.; // Measurement variance |
3910 | double V=0.0507; |
3911 | double InvV; // inverse of variance |
3912 | //DMatrix5x1 dS; // perturbation in state vector |
3913 | DMatrix5x1 S0,S0_; // state vector |
3914 | |
3915 | // set the used_in_fit flags to false for cdc hits |
3916 | unsigned int num_cdc=cdc_used_in_fit.size(); |
3917 | for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false; |
3918 | for (unsigned int i=0;i<central_traj.size();i++){ |
3919 | central_traj[i].h_id=0; |
3920 | } |
3921 | |
3922 | // Initialize the chi2 for this part of the track |
3923 | chisq=0.; |
3924 | my_ndf=0; |
3925 | |
3926 | double chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
3927 | |
3928 | // path length increment |
3929 | double ds2=0.; |
3930 | |
3931 | //printf(">>>>>>>>>>>>>>>>\n"); |
3932 | |
3933 | // beginning position |
3934 | double phi=Sc(state_phi); |
3935 | xy.Set(central_traj[break_point_step_index].xy.X()-Sc(state_D)*sin(phi), |
3936 | central_traj[break_point_step_index].xy.Y()+Sc(state_D)*cos(phi)); |
3937 | |
3938 | // Wire origin and direction |
3939 | // unsigned int cdc_index=my_cdchits.size()-1; |
3940 | unsigned int cdc_index=break_point_cdc_index; |
3941 | if (break_point_cdc_index<num_cdc-1){ |
3942 | num_cdc=break_point_cdc_index+1; |
3943 | } |
3944 | |
3945 | if (num_cdc<MIN_HITS_FOR_REFIT) chi2cut=BIG1.0e8; |
3946 | |
3947 | // Wire origin and direction |
3948 | DVector2 origin=my_cdchits[cdc_index]->origin; |
3949 | double z0w=my_cdchits[cdc_index]->z0wire; |
3950 | DVector2 dir=my_cdchits[cdc_index]->dir; |
3951 | DVector2 wirexy=origin+(Sc(state_z)-z0w)*dir; |
3952 | |
3953 | // Save the starting values for C and S in the deque |
3954 | central_traj[break_point_step_index].Skk=Sc; |
3955 | central_traj[break_point_step_index].Ckk=Cc; |
3956 | |
3957 | // doca variables |
3958 | double doca2,old_doca2=(xy-wirexy).Mod2(); |
3959 | |
3960 | // energy loss |
3961 | double dedx=0.; |
3962 | |
3963 | // Boolean for flagging when we are done with measurements |
3964 | bool more_measurements=true; |
3965 | |
3966 | // Initialize S0_ and perform the loop over the trajectory |
3967 | S0_=central_traj[break_point_step_index].S; |
3968 | |
3969 | for (unsigned int k=break_point_step_index+1;k<central_traj.size();k++){ |
3970 | unsigned int k_minus_1=k-1; |
3971 | |
3972 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
3973 | // from reference trajectory |
3974 | S0=central_traj[k].S; |
3975 | J=central_traj[k].J; |
3976 | Q=central_traj[k].Q; |
3977 | |
3978 | //Q.Print(); |
3979 | //J.Print(); |
3980 | |
3981 | // State S is perturbation about a seed S0 |
3982 | //dS=Sc-S0_; |
3983 | //dS.Zero(); |
3984 | |
3985 | // Update the actual state vector and covariance matrix |
3986 | Sc=S0+J*(Sc-S0_); |
3987 | // Cc=J*(Cc*JT)+Q; |
3988 | // Cc=Q.AddSym(Cc.SandwichMultiply(J)); |
3989 | Cc=Q.AddSym(J*Cc*J.Transpose()); |
3990 | |
3991 | // Save the current state and covariance matrix in the deque |
3992 | if (fit_type==kTimeBased){ |
3993 | central_traj[k].Skk=Sc; |
3994 | central_traj[k].Ckk=Cc; |
3995 | } |
3996 | |
3997 | // update position based on new doca to reference trajectory |
3998 | xy.Set(central_traj[k].xy.X()-Sc(state_D)*sin(Sc(state_phi)), |
3999 | central_traj[k].xy.Y()+Sc(state_D)*cos(Sc(state_phi))); |
4000 | |
4001 | // Bail if the position is grossly outside of the tracking volume |
4002 | if (xy.Mod2()>R2_MAX4225.0 || Sc(state_z)<Z_MIN-100. || Sc(state_z)>Z_MAX370.0){ |
4003 | if (DEBUG_LEVEL>2) |
4004 | { |
4005 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4005<<" "<< "Went outside of tracking volume at z="<<Sc(state_z) |
4006 | << " r="<<xy.Mod()<<endl; |
4007 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4007<<" " << " Break indexes: " << break_point_cdc_index <<"," |
4008 | << break_point_step_index << endl; |
4009 | } |
4010 | return POSITION_OUT_OF_RANGE; |
4011 | } |
4012 | // Bail if the transverse momentum has dropped below some minimum |
4013 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
4014 | if (DEBUG_LEVEL>2) |
4015 | { |
4016 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4016<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
4017 | << " at step " << k |
4018 | << endl; |
4019 | } |
4020 | return MOMENTUM_OUT_OF_RANGE; |
4021 | } |
4022 | |
4023 | |
4024 | // Save the current state of the reference trajectory |
4025 | S0_=S0; |
4026 | |
4027 | // new wire position |
4028 | wirexy=origin; |
4029 | wirexy+=(Sc(state_z)-z0w)*dir; |
4030 | |
4031 | // new doca |
4032 | doca2=(xy-wirexy).Mod2(); |
4033 | |
4034 | // Check if the doca is no longer decreasing |
4035 | if (more_measurements && (doca2>old_doca2 && Sc(state_z)>cdc_origin[2])){ |
4036 | if (my_cdchits[cdc_index]->status==good_hit){ |
4037 | if (DEBUG_LEVEL>9) { |
4038 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4038<<" " << " Good Hit Ring " << my_cdchits[cdc_index]->hit->wire->ring << " Straw " << my_cdchits[cdc_index]->hit->wire->straw << endl; |
4039 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4039<<" " << " doca " << sqrt(doca2) << endl; |
4040 | } |
4041 | |
4042 | // Save values at end of current step |
4043 | DVector2 xy0=central_traj[k].xy; |
4044 | |
4045 | // Variables for the computation of D at the doca to the wire |
4046 | double D=Sc(state_D); |
4047 | double q=(Sc(state_q_over_pt)>0.0)?1.:-1.; |
4048 | |
4049 | q*=FactorForSenseOfRotation; |
4050 | |
4051 | double qpt=1./Sc(state_q_over_pt) * FactorForSenseOfRotation; |
4052 | double sinphi=sin(Sc(state_phi)); |
4053 | double cosphi=cos(Sc(state_phi)); |
4054 | //double qrc_old=qpt/fabs(qBr2p*bfield->GetBz(pos.x(),pos.y(),pos.z())); |
4055 | double qrc_old=qpt/fabs(qBr2p0.003*central_traj[k].B); |
4056 | double qrc_plus_D=D+qrc_old; |
4057 | |
4058 | // wire direction variable |
4059 | double ux=dir.X(); |
4060 | double uy=dir.Y(); |
4061 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
4062 | // Variables relating wire direction and track direction |
4063 | //double my_ux=ux*sinl-cosl*cosphi; |
4064 | //double my_uy=uy*sinl-cosl*sinphi; |
4065 | //double denom=my_ux*my_ux+my_uy*my_uy; |
4066 | // distance variables |
4067 | DVector2 diff,dxy1; |
4068 | |
4069 | // use Brent's algorithm to find the poca to the wire |
4070 | // See Numerical Recipes in C, pp 404-405 |
4071 | |
4072 | // dEdx for current position along trajectory |
4073 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
4074 | if (CORRECT_FOR_ELOSS){ |
4075 | dedx=GetdEdx(q_over_p, central_traj[k].K_rho_Z_over_A, |
4076 | central_traj[k].rho_Z_over_A, |
4077 | central_traj[k].LnI,central_traj[k].Z); |
4078 | } |
4079 | |
4080 | if (BrentCentral(dedx,xy,z0w,origin,dir,Sc,ds2)!=NOERROR) return MOMENTUM_OUT_OF_RANGE; |
4081 | |
4082 | //Step along the reference trajectory and compute the new covariance matrix |
4083 | StepStateAndCovariance(xy0,ds2,dedx,S0,J,Cc); |
4084 | |
4085 | // Compute the value of D (signed distance to the reference trajectory) |
4086 | // at the doca to the wire |
4087 | dxy1=xy0-central_traj[k].xy; |
4088 | double rc=sqrt(dxy1.Mod2() |
4089 | +2.*qrc_plus_D*(dxy1.X()*sinphi-dxy1.Y()*cosphi) |
4090 | +qrc_plus_D*qrc_plus_D); |
4091 | Sc(state_D)=q*rc-qrc_old; |
4092 | |
4093 | // wire position |
4094 | wirexy=origin; |
4095 | wirexy+=(Sc(state_z)-z0w)*dir; |
4096 | diff=xy-wirexy; |
4097 | |
4098 | // prediction for measurement |
4099 | double doca=diff.Mod()+EPS3.0e-8; |
4100 | double prediction=doca*cosstereo; |
4101 | |
4102 | // Measurement |
4103 | double measurement=0.39,tdrift=0.,tcorr=0.,dDdt0=0.; |
4104 | if (fit_type==kTimeBased || USE_PASS1_TIME_MODE){ |
4105 | // Find offset of wire with respect to the center of the |
4106 | // straw at this z position |
4107 | const DCDCWire *mywire=my_cdchits[cdc_index]->hit->wire; |
4108 | int ring_index=mywire->ring-1; |
4109 | int straw_index=mywire->straw-1; |
4110 | double dz=Sc(state_z)-z0w; |
4111 | double phi_d=diff.Phi(); |
4112 | double delta |
4113 | =max_sag[ring_index][straw_index]*(1.-dz*dz/5625.) |
4114 | *cos(phi_d + sag_phi_offset[ring_index][straw_index]); |
4115 | double dphi=phi_d-mywire->origin.Phi(); |
4116 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
4117 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
4118 | if (mywire->origin.Y()<0) dphi*=-1.; |
4119 | |
4120 | tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
4121 | -central_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
4122 | double B=central_traj[k_minus_1].B; |
4123 | ComputeCDCDrift(dphi,delta,tdrift,B,measurement,V,tcorr); |
4124 | V*=anneal_factor; |
4125 | if (ALIGNMENT_CENTRAL){ |
4126 | double myV=0.; |
4127 | double mytcorr=0.; |
4128 | double d_shifted; |
4129 | double dt=2.0; |
4130 | ComputeCDCDrift(dphi,delta,tdrift+dt,B,d_shifted,myV,mytcorr); |
4131 | dDdt0=(d_shifted-measurement)/dt; |
4132 | } |
4133 | |
4134 | //_DBG_ << tcorr << " " << dphi << " " << dm << endl; |
4135 | |
4136 | } |
4137 | |
4138 | // Projection matrix |
4139 | sinphi=sin(Sc(state_phi)); |
4140 | cosphi=cos(Sc(state_phi)); |
4141 | double dx=diff.X(); |
4142 | double dy=diff.Y(); |
4143 | double cosstereo_over_doca=cosstereo/doca; |
4144 | H_T(state_D)=(dy*cosphi-dx*sinphi)*cosstereo_over_doca; |
4145 | H_T(state_phi) |
4146 | =-Sc(state_D)*cosstereo_over_doca*(dx*cosphi+dy*sinphi); |
4147 | H_T(state_z)=-cosstereo_over_doca*(dx*ux+dy*uy); |
4148 | H(state_tanl)=0.; |
4149 | H_T(state_tanl)=0.; |
4150 | H(state_D)=H_T(state_D); |
4151 | H(state_z)=H_T(state_z); |
4152 | H(state_phi)=H_T(state_phi); |
4153 | |
4154 | // Difference and inverse of variance |
4155 | //InvV=1./(V+H*(Cc*H_T)); |
4156 | //double Vproj=Cc.SandwichMultiply(H_T); |
4157 | double Vproj=H*Cc*H_T; |
4158 | InvV=1./(V+Vproj); |
4159 | double dm=measurement-prediction; |
4160 | |
4161 | if (InvV<0.){ |
4162 | if (DEBUG_LEVEL>1) |
4163 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4163<<" " << k <<" "<< central_traj.size()-1<<" Negative variance??? " << V << " " << H*(Cc*H_T) <<endl; |
4164 | |
4165 | break_point_cdc_index=(3*num_cdc)/4; |
4166 | return NEGATIVE_VARIANCE; |
4167 | } |
4168 | /* |
4169 | if (fabs(cosstereo)<1.){ |
4170 | printf("t %f delta %f sigma %f V %f chi2 %f\n",my_cdchits[cdc_index]->hit->tdrift-mT0,dm,sqrt(V),1./InvV,dm*dm*InvV); |
4171 | } |
4172 | */ |
4173 | |
4174 | // Check how far this hit is from the expected position |
4175 | double chi2check=dm*dm*InvV; |
4176 | if (DEBUG_LEVEL>9) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4176<<" " << " Prediction " << prediction << " Measurement " << measurement << " Chi2 " << chi2check << endl; |
4177 | if (chi2check<chi2cut) |
4178 | { |
4179 | if (DEBUG_LEVEL>9) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4179<<" " << " Passed Chi^2 check Ring " << my_cdchits[cdc_index]->hit->wire->ring << " Straw " << my_cdchits[cdc_index]->hit->wire->straw << endl; |
4180 | |
4181 | // Compute Kalman gain matrix |
4182 | K=InvV*(Cc*H_T); |
4183 | |
4184 | // Update state vector covariance matrix |
4185 | //Cc=Cc-(K*(H*Cc)); |
4186 | Ctest=Cc.SubSym(K*(H*Cc)); |
4187 | // Joseph form |
4188 | // C = (I-KH)C(I-KH)^T + KVK^T |
4189 | //Ctest=Cc.SandwichMultiply(I5x5-K*H)+V*MultiplyTranspose(K); |
4190 | // Check that Ctest is positive definite |
4191 | if (!Ctest.IsPosDef()){ |
4192 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4192<<" " << "Broken covariance matrix!" <<endl; |
4193 | return BROKEN_COVARIANCE_MATRIX; |
4194 | } |
4195 | bool skip_ring |
4196 | =(my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP); |
4197 | //Update covariance matrix and state vector |
4198 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
4199 | Cc=Ctest; |
4200 | Sc+=dm*K; |
4201 | } |
4202 | |
4203 | // Mark point on ref trajectory with a hit id for the straw |
4204 | central_traj[k].h_id=cdc_index+1; |
4205 | if (DEBUG_LEVEL>9) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4205<<" " << " Marked Trajectory central_traj[k].h_id=cdc_index+1 (k cdc_index)" << k << " " << cdc_index << endl; |
4206 | |
4207 | // Save some updated information for this hit |
4208 | double scale=(skip_ring)?1.:(1.-H*K); |
4209 | cdc_updates[cdc_index].tcorr=tcorr; |
4210 | cdc_updates[cdc_index].tdrift=tdrift; |
4211 | cdc_updates[cdc_index].doca=measurement; |
4212 | cdc_updates[cdc_index].variance=V; |
4213 | cdc_updates[cdc_index].dDdt0=dDdt0; |
4214 | cdc_used_in_fit[cdc_index]=true; |
4215 | if (tdrift < CDC_T_DRIFT_MIN) cdc_used_in_fit[cdc_index]=false; |
4216 | |
4217 | // Update chi2 for this hit |
4218 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
4219 | chisq+=scale*dm*dm/V; |
4220 | my_ndf++; |
4221 | } |
4222 | if (DEBUG_LEVEL>10) |
4223 | cout |
4224 | << "ring " << my_cdchits[cdc_index]->hit->wire->ring |
4225 | << " t " << my_cdchits[cdc_index]->hit->tdrift |
4226 | << " Dm-Dpred " << dm |
4227 | << " chi2 " << (1.-H*K)*dm*dm/V |
4228 | << endl; |
4229 | |
4230 | break_point_cdc_index=cdc_index; |
4231 | break_point_step_index=k_minus_1; |
4232 | |
4233 | //else printf("Negative variance!!!\n"); |
4234 | |
4235 | |
4236 | } |
4237 | |
4238 | // Move back to the right step along the reference trajectory. |
4239 | StepStateAndCovariance(xy,-ds2,dedx,Sc,J,Cc); |
4240 | |
4241 | // Save state and covariance matrix to update vector |
4242 | cdc_updates[cdc_index].S=Sc; |
4243 | cdc_updates[cdc_index].C=Cc; |
4244 | |
4245 | //Sc.Print(); |
4246 | //Cc.Print(); |
4247 | |
4248 | // update position on current trajectory based on corrected doca to |
4249 | // reference trajectory |
4250 | xy.Set(central_traj[k].xy.X()-Sc(state_D)*sin(Sc(state_phi)), |
4251 | central_traj[k].xy.Y()+Sc(state_D)*cos(Sc(state_phi))); |
4252 | |
4253 | } |
4254 | |
4255 | // new wire origin and direction |
4256 | if (cdc_index>0){ |
4257 | cdc_index--; |
4258 | origin=my_cdchits[cdc_index]->origin; |
4259 | z0w=my_cdchits[cdc_index]->z0wire; |
4260 | dir=my_cdchits[cdc_index]->dir; |
4261 | } |
4262 | else{ |
4263 | more_measurements=false; |
4264 | } |
4265 | |
4266 | // Update the wire position |
4267 | wirexy=origin+(Sc(state_z)-z0w)*dir; |
4268 | |
4269 | //s+=ds2; |
4270 | // new doca |
4271 | doca2=(xy-wirexy).Mod2(); |
4272 | } |
4273 | |
4274 | old_doca2=doca2; |
4275 | |
4276 | } |
4277 | |
4278 | // If there are not enough degrees of freedom, something went wrong... |
4279 | if (my_ndf<6){ |
4280 | chisq=-1.; |
4281 | my_ndf=0; |
4282 | return PRUNED_TOO_MANY_HITS; |
4283 | } |
4284 | else my_ndf-=5; |
4285 | |
4286 | // Check if the momentum is unphysically large |
4287 | double p=cos(atan(Sc(state_tanl)))/fabs(Sc(state_q_over_pt)); |
4288 | if (p>12.0){ |
4289 | if (DEBUG_LEVEL>2) |
4290 | { |
4291 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4291<<" " << "Unphysical momentum: P = " << p <<endl; |
4292 | } |
4293 | return MOMENTUM_OUT_OF_RANGE; |
4294 | } |
4295 | |
4296 | // Check if we have a kink in the track or threw away too many cdc hits |
4297 | if (num_cdc>=MIN_HITS_FOR_REFIT){ |
4298 | if (break_point_cdc_index>1){ |
4299 | if (break_point_cdc_index<num_cdc/2){ |
4300 | break_point_cdc_index=(3*num_cdc)/4; |
4301 | } |
4302 | return BREAK_POINT_FOUND; |
4303 | } |
4304 | |
4305 | unsigned int num_good=0; |
4306 | for (unsigned int j=0;j<num_cdc;j++){ |
4307 | if (cdc_used_in_fit[j]) num_good++; |
4308 | } |
4309 | if (double(num_good)/double(num_cdc)<MINIMUM_HIT_FRACTION){ |
4310 | return PRUNED_TOO_MANY_HITS; |
4311 | } |
4312 | } |
4313 | |
4314 | return FIT_SUCCEEDED; |
4315 | } |
4316 | |
4317 | // Kalman engine for forward tracks |
4318 | kalman_error_t DTrackFitterKalmanSIMD::KalmanForward(double fdc_anneal_factor, |
4319 | double cdc_anneal_factor, |
4320 | DMatrix5x1 &S, |
4321 | DMatrix5x5 &C, |
4322 | double &chisq, |
4323 | unsigned int &numdof){ |
4324 | DMatrix2x1 Mdiff; // difference between measurement and prediction |
4325 | DMatrix2x5 H; // Track projection matrix |
4326 | DMatrix5x2 H_T; // Transpose of track projection matrix |
4327 | DMatrix1x5 Hc; // Track projection matrix for cdc hits |
4328 | DMatrix5x1 Hc_T; // Transpose of track projection matrix for cdc hits |
4329 | DMatrix5x5 J; // State vector Jacobian matrix |
4330 | //DMatrix5x5 J_T; // transpose of this matrix |
4331 | DMatrix5x5 Q; // Process noise covariance matrix |
4332 | DMatrix5x2 K; // Kalman gain matrix |
4333 | DMatrix5x1 Kc; // Kalman gain matrix for cdc hits |
4334 | DMatrix2x2 V(0.0833,0.,0.,FDC_CATHODE_VARIANCE0.000225); // Measurement covariance matrix |
4335 | DMatrix2x1 R; // Filtered residual |
4336 | DMatrix2x2 RC; // Covariance of filtered residual |
4337 | DMatrix5x1 S0,S0_; //State vector |
4338 | DMatrix5x5 Ctest; // Covariance matrix |
4339 | DMatrix2x2 InvV; // Inverse of error matrix |
4340 | |
4341 | double Vc=0.0507; |
4342 | |
4343 | // Vectors for cdc wires |
4344 | DVector2 origin,dir,wirepos; |
4345 | double z0w=0.; // origin in z for wire |
4346 | |
4347 | // Set used_in_fit flags to false for fdc and cdc hits |
4348 | unsigned int num_cdc=cdc_used_in_fit.size(); |
4349 | unsigned int num_fdc=fdc_used_in_fit.size(); |
4350 | for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false; |
4351 | for (unsigned int i=0;i<num_fdc;i++) fdc_used_in_fit[i]=false; |
4352 | for (unsigned int i=0;i<forward_traj.size();i++){ |
4353 | if (forward_traj[i].h_id>999) |
4354 | forward_traj[i].h_id=0; |
4355 | } |
4356 | |
4357 | // Save the starting values for C and S in the deque |
4358 | forward_traj[break_point_step_index].Skk=S; |
4359 | forward_traj[break_point_step_index].Ckk=C; |
4360 | |
4361 | // Initialize chi squared |
4362 | chisq=0; |
4363 | |
4364 | // Initialize number of degrees of freedom |
4365 | numdof=0; |
4366 | |
4367 | double fdc_chi2cut=NUM_FDC_SIGMA_CUT*NUM_FDC_SIGMA_CUT; |
4368 | double cdc_chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
4369 | |
4370 | unsigned int num_fdc_hits=break_point_fdc_index+1; |
4371 | unsigned int max_num_fdc_used_in_fit=num_fdc_hits; |
4372 | unsigned int num_cdc_hits=my_cdchits.size(); |
4373 | unsigned int cdc_index=0; |
4374 | if (num_cdc_hits>0) cdc_index=num_cdc_hits-1; |
4375 | bool more_cdc_measurements=(num_cdc_hits>0); |
4376 | double old_doca2=1e6; |
4377 | |
4378 | if (num_fdc_hits+num_cdc_hits<MIN_HITS_FOR_REFIT){ |
4379 | cdc_chi2cut=BIG1.0e8; |
4380 | fdc_chi2cut=BIG1.0e8; |
4381 | } |
4382 | |
4383 | if (more_cdc_measurements){ |
4384 | origin=my_cdchits[cdc_index]->origin; |
4385 | dir=my_cdchits[cdc_index]->dir; |
4386 | z0w=my_cdchits[cdc_index]->z0wire; |
4387 | wirepos=origin+(forward_traj[break_point_step_index].z-z0w)*dir; |
4388 | } |
4389 | |
4390 | S0_=(forward_traj[break_point_step_index].S); |
4391 | |
4392 | if (DEBUG_LEVEL > 25){ |
4393 | jout << "Entering DTrackFitterKalmanSIMD::KalmanForward ================================" << endl; |
4394 | jout << " We have the following starting parameters for our fit. S = State vector, C = Covariance matrix" << endl; |
4395 | S.Print(); |
4396 | C.Print(); |
4397 | jout << setprecision(6); |
4398 | jout << " There are " << num_cdc << " CDC Updates and " << num_fdc << " FDC Updates on this track" << endl; |
4399 | jout << " There are " << num_cdc_hits << " CDC Hits and " << num_fdc_hits << " FDC Hits on this track" << endl; |
4400 | jout << " With NUM_FDC_SIGMA_CUT = " << NUM_FDC_SIGMA_CUT << " and NUM_CDC_SIGMA_CUT = " << NUM_CDC_SIGMA_CUT << endl; |
4401 | jout << " fdc_anneal_factor = " << fdc_anneal_factor << " cdc_anneal_factor = " << cdc_anneal_factor << endl; |
4402 | jout << " yields fdc_chi2cut = " << fdc_chi2cut << " cdc_chi2cut = " << cdc_chi2cut << endl; |
4403 | jout << " Starting from break_point_step_index " << break_point_step_index << endl; |
4404 | jout << " S0_ which is the state vector of the reference trajectory at the break point step:" << endl; |
4405 | S0_.Print(); |
4406 | jout << " ===== Beginning pass over reference trajectory ======== " << endl; |
4407 | } |
4408 | |
4409 | for (unsigned int k=break_point_step_index+1;k<forward_traj.size();k++){ |
4410 | unsigned int k_minus_1=k-1; |
4411 | |
4412 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
4413 | // from reference trajectory |
4414 | S0=(forward_traj[k].S); |
4415 | J=(forward_traj[k].J); |
4416 | Q=(forward_traj[k].Q); |
4417 | |
4418 | // State S is perturbation about a seed S0 |
4419 | //dS=S-S0_; |
4420 | |
4421 | // Update the actual state vector and covariance matrix |
4422 | S=S0+J*(S-S0_); |
4423 | |
4424 | // Bail if the momentum has dropped below some minimum |
4425 | if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX){ |
4426 | if (DEBUG_LEVEL>2) |
4427 | { |
4428 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4428<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; |
4429 | } |
4430 | break_point_fdc_index=(3*num_fdc)/4; |
4431 | return MOMENTUM_OUT_OF_RANGE; |
4432 | } |
4433 | |
4434 | |
4435 | //C=J*(C*J_T)+Q; |
4436 | //C=Q.AddSym(C.SandwichMultiply(J)); |
4437 | C=Q.AddSym(J*C*J.Transpose()); |
4438 | |
4439 | // Save the current state and covariance matrix in the deque |
4440 | forward_traj[k].Skk=S; |
4441 | forward_traj[k].Ckk=C; |
4442 | |
4443 | // Save the current state of the reference trajectory |
4444 | S0_=S0; |
4445 | |
4446 | // Z position along the trajectory |
4447 | double z=forward_traj[k].z; |
4448 | |
4449 | if (DEBUG_LEVEL > 25){ |
4450 | jout << " At reference trajectory index " << k << " at z=" << z << endl; |
4451 | jout << " The State vector from the reference trajectory" << endl; |
4452 | S0.Print(); |
4453 | jout << " The Jacobian matrix " << endl; |
4454 | J.Print(); |
4455 | jout << " The Q matrix "<< endl; |
4456 | Q.Print(); |
4457 | jout << " The updated State vector S=S0+J*(S-S0_)" << endl; |
4458 | S.Print(); |
4459 | jout << " The updated Covariance matrix C=J*(C*J_T)+Q;" << endl; |
4460 | C.Print(); |
4461 | } |
4462 | |
4463 | // Add the hit |
4464 | if (num_fdc_hits>0){ |
4465 | if (forward_traj[k].h_id>0 && forward_traj[k].h_id<1000){ |
4466 | unsigned int id=forward_traj[k].h_id-1; |
4467 | // Check if this is a plane we want to skip in the fit (we still want |
4468 | // to store track and hit info at this plane, however). |
4469 | bool skip_plane=(my_fdchits[id]->hit!=NULL__null |
4470 | &&my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP); |
4471 | double upred=0,vpred=0.,doca=0.,cosalpha=0.,lorentz_factor=0.; |
4472 | FindDocaAndProjectionMatrix(my_fdchits[id],S,upred,vpred,doca,cosalpha, |
4473 | lorentz_factor,H_T); |
4474 | // Matrix transpose H_T -> H |
4475 | H=Transpose(H_T); |
4476 | |
4477 | // Variance in coordinate transverse to wire |
4478 | V(0,0)=my_fdchits[id]->uvar; |
4479 | if (my_fdchits[id]->hit==NULL__null&&my_fdchits[id]->status!=trd_hit){ |
4480 | V(0,0)*=fdc_anneal_factor; |
4481 | } |
4482 | |
4483 | // Variance in coordinate along wire |
4484 | V(1,1)=my_fdchits[id]->vvar*fdc_anneal_factor; |
4485 | |
4486 | // Residual for coordinate along wire |
4487 | Mdiff(1)=my_fdchits[id]->vstrip-vpred-doca*lorentz_factor; |
4488 | |
4489 | // Residual for coordinate transverse to wire |
4490 | Mdiff(0)=-doca; |
4491 | double drift_time=my_fdchits[id]->t-mT0-forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
4492 | if (fit_type==kTimeBased && USE_FDC_DRIFT_TIMES){ |
4493 | if (my_fdchits[id]->hit!=NULL__null){ |
4494 | double drift=(doca>0.0?1.:-1.) |
4495 | *fdc_drift_distance(drift_time,forward_traj[k].B); |
4496 | Mdiff(0)+=drift; |
4497 | |
4498 | // Variance in drift distance |
4499 | V(0,0)=fdc_drift_variance(drift_time)*fdc_anneal_factor; |
4500 | } |
4501 | else if (USE_TRD_DRIFT_TIMES&&my_fdchits[id]->status==trd_hit){ |
4502 | double drift =(doca>0.0?1.:-1.)*0.1*pow(drift_time/8./0.91,1./1.556); |
4503 | Mdiff(0)+=drift; |
4504 | |
4505 | // Variance in drift distance |
4506 | V(0,0)=0.05*0.05*fdc_anneal_factor; |
4507 | } |
4508 | } |
4509 | // Check to see if we have multiple hits in the same plane |
4510 | if (!ALIGNMENT_FORWARD && forward_traj[k].num_hits>1){ |
4511 | UpdateSandCMultiHit(forward_traj[k],upred,vpred,doca,cosalpha, |
4512 | lorentz_factor,V,Mdiff,H,H_T,S,C, |
4513 | fdc_chi2cut,skip_plane,chisq,numdof, |
4514 | fdc_anneal_factor); |
4515 | } |
4516 | else{ |
4517 | if (DEBUG_LEVEL > 25) jout << " == There is a single FDC hit on this plane" << endl; |
4518 | |
4519 | // Variance for this hit |
4520 | DMatrix2x2 Vtemp=V+H*C*H_T; |
4521 | InvV=Vtemp.Invert(); |
4522 | |
4523 | // Check if this hit is an outlier |
4524 | double chi2_hit=Vtemp.Chi2(Mdiff); |
4525 | if (chi2_hit<fdc_chi2cut){ |
4526 | // Compute Kalman gain matrix |
4527 | K=C*H_T*InvV; |
4528 | |
4529 | if (skip_plane==false){ |
4530 | // Update the state vector |
4531 | S+=K*Mdiff; |
4532 | |
4533 | // Update state vector covariance matrix |
4534 | //C=C-K*(H*C); |
4535 | C=C.SubSym(K*(H*C)); |
4536 | |
4537 | if (DEBUG_LEVEL > 25) { |
4538 | jout << "S Update: " << endl; S.Print(); |
4539 | jout << "C Uodate: " << endl; C.Print(); |
4540 | } |
4541 | } |
4542 | |
4543 | // Store the "improved" values for the state vector and covariance |
4544 | fdc_updates[id].S=S; |
4545 | fdc_updates[id].C=C; |
4546 | fdc_updates[id].tdrift=drift_time; |
4547 | fdc_updates[id].tcorr=fdc_updates[id].tdrift; // temporary! |
4548 | fdc_updates[id].doca=doca; |
4549 | fdc_used_in_fit[id]=true; |
4550 | |
4551 | if (skip_plane==false){ |
4552 | // Filtered residual and covariance of filtered residual |
4553 | R=Mdiff-H*K*Mdiff; |
4554 | RC=V-H*(C*H_T); |
4555 | |
4556 | fdc_updates[id].V=RC; |
4557 | |
4558 | // Update chi2 for this segment |
4559 | chisq+=RC.Chi2(R); |
4560 | |
4561 | // update number of degrees of freedom |
4562 | numdof+=2; |
4563 | |
4564 | if (DEBUG_LEVEL>20) |
4565 | { |
4566 | printf("hit %d p %5.2f t %f dm %5.2f sig %f chi2 %5.2f z %5.2f\n", |
4567 | id,1./S(state_q_over_p),fdc_updates[id].tdrift,Mdiff(1), |
4568 | sqrt(V(1,1)),RC.Chi2(R), |
4569 | forward_traj[k].z); |
4570 | |
4571 | } |
4572 | } |
4573 | else{ |
4574 | fdc_updates[id].V=V; |
4575 | } |
4576 | |
4577 | break_point_fdc_index=id; |
4578 | break_point_step_index=k; |
4579 | } |
4580 | } |
4581 | if (num_fdc_hits>=forward_traj[k].num_hits) |
4582 | num_fdc_hits-=forward_traj[k].num_hits; |
4583 | } |
4584 | } |
4585 | else if (more_cdc_measurements /* && z<endplate_z*/){ |
4586 | // new wire position |
4587 | wirepos=origin; |
4588 | wirepos+=(z-z0w)*dir; |
4589 | |
4590 | // doca variables |
4591 | double dx=S(state_x)-wirepos.X(); |
4592 | double dy=S(state_y)-wirepos.Y(); |
4593 | double doca2=dx*dx+dy*dy; |
4594 | |
4595 | // Check if the doca is no longer decreasing |
4596 | if (doca2>old_doca2){ |
4597 | if(my_cdchits[cdc_index]->status==good_hit){ |
4598 | find_doca_error_t swimmed_to_doca=DOCA_NO_BRENT; |
4599 | double newz=endplate_z; |
4600 | double dz=newz-z; |
4601 | // Sometimes the true doca would correspond to the case where the |
4602 | // wire would need to extend beyond the physical volume of the straw. |
4603 | // In this case, find the doca at the cdc endplate. |
4604 | if (z>endplate_z){ |
4605 | swimmed_to_doca=DOCA_ENDPLATE; |
4606 | SwimToEndplate(z,forward_traj[k],S); |
4607 | |
4608 | // wire position at the endplate |
4609 | wirepos=origin; |
4610 | wirepos+=(endplate_z-z0w)*dir; |
4611 | |
4612 | dx=S(state_x)-wirepos.X(); |
4613 | dy=S(state_y)-wirepos.Y(); |
4614 | } |
4615 | else{ |
4616 | // Find the true doca to the wire. If we had to use Brent's |
4617 | // algorithm, the routine returns true. |
4618 | double step1=mStepSizeZ; |
4619 | double step2=mStepSizeZ; |
4620 | if (k>=2){ |
4621 | step1=-forward_traj[k].z+forward_traj[k_minus_1].z; |
4622 | step2=-forward_traj[k_minus_1].z+forward_traj[k-2].z; |
4623 | } |
4624 | swimmed_to_doca=FindDoca(my_cdchits[cdc_index],forward_traj[k], |
4625 | step1,step2,S0,S,C,dx,dy,dz); |
4626 | if (swimmed_to_doca==BRENT_FAILED){ |
4627 | //break_point_fdc_index=(3*num_fdc)/4; |
4628 | return MOMENTUM_OUT_OF_RANGE; |
4629 | } |
4630 | |
4631 | newz=forward_traj[k].z+dz; |
4632 | } |
4633 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
4634 | double d=sqrt(dx*dx+dy*dy)*cosstereo+EPS3.0e-8; |
4635 | |
4636 | // Track projection |
4637 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
4638 | Hc_T(state_x)=dx*cosstereo2_over_d; |
4639 | Hc(state_x)=Hc_T(state_x); |
4640 | Hc_T(state_y)=dy*cosstereo2_over_d; |
4641 | Hc(state_y)=Hc_T(state_y); |
4642 | if (swimmed_to_doca==DOCA_NO_BRENT){ |
4643 | Hc_T(state_ty)=Hc_T(state_y)*dz; |
4644 | Hc(state_ty)=Hc_T(state_ty); |
4645 | Hc_T(state_tx)=Hc_T(state_x)*dz; |
4646 | Hc(state_tx)=Hc_T(state_tx); |
4647 | } |
4648 | else{ |
4649 | Hc_T(state_ty)=0.; |
4650 | Hc(state_ty)=0.; |
4651 | Hc_T(state_tx)=0.; |
4652 | Hc(state_tx)=0.; |
4653 | } |
4654 | |
4655 | //H.Print(); |
4656 | |
4657 | // The next measurement |
4658 | double dm=0.39,tdrift=0.,tcorr=0.,dDdt0=0.; |
4659 | if (fit_type==kTimeBased){ |
4660 | // Find offset of wire with respect to the center of the |
4661 | // straw at this z position |
4662 | double delta=0,dphi=0.; |
4663 | FindSag(dx,dy,newz-z0w,my_cdchits[cdc_index]->hit->wire,delta,dphi); |
4664 | |
4665 | // Find drift time and distance |
4666 | tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
4667 | -forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
4668 | double B=forward_traj[k_minus_1].B; |
4669 | ComputeCDCDrift(dphi,delta,tdrift,B,dm,Vc,tcorr); |
4670 | Vc*=cdc_anneal_factor; |
4671 | if (ALIGNMENT_FORWARD){ |
4672 | double myV=0.; |
4673 | double mytcorr=0.; |
4674 | double d_shifted; |
4675 | double dt=5.0; |
4676 | // Dont compute this for negative drift times |
4677 | if (tdrift < 0.) d_shifted = dm; |
4678 | else ComputeCDCDrift(dphi,delta,tdrift+dt,B,d_shifted,myV,mytcorr); |
4679 | dDdt0=(d_shifted-dm)/dt; |
4680 | } |
4681 | |
4682 | if (max_num_fdc_used_in_fit>4) |
4683 | { |
4684 | Vc*=CDC_VAR_SCALE_FACTOR; //de-weight CDC hits |
4685 | } |
4686 | //_DBG_ << "t " << tdrift << " d " << d << " delta " << delta << " dphi " << atan2(dy,dx)-mywire->origin.Phi() << endl; |
4687 | |
4688 | //_DBG_ << tcorr << " " << dphi << " " << dm << endl; |
4689 | } |
4690 | |
4691 | // Residual |
4692 | double res=dm-d; |
4693 | |
4694 | // inverse variance including prediction |
4695 | //double InvV1=1./(Vc+H*(C*H_T)); |
4696 | //double Vproj=C.SandwichMultiply(Hc_T); |
4697 | double Vproj=Hc*C*Hc_T; |
4698 | double InvV1=1./(Vc+Vproj); |
4699 | if (InvV1<0.){ |
4700 | if (DEBUG_LEVEL>0) |
4701 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4701<<" " << "Negative variance???" << endl; |
4702 | return NEGATIVE_VARIANCE; |
4703 | } |
4704 | |
4705 | // Check if this hit is an outlier |
4706 | double chi2_hit=res*res*InvV1; |
4707 | if (chi2_hit<cdc_chi2cut){ |
4708 | // Compute KalmanSIMD gain matrix |
4709 | Kc=InvV1*(C*Hc_T); |
4710 | |
4711 | // Update state vector covariance matrix |
4712 | //C=C-K*(H*C); |
4713 | Ctest=C.SubSym(Kc*(Hc*C)); |
4714 | //Ctest=C.SandwichMultiply(I5x5-K*H)+Vc*MultiplyTranspose(K); |
4715 | // Check that Ctest is positive definite |
4716 | if (!Ctest.IsPosDef()){ |
4717 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4717<<" " << "Broken covariance matrix!" <<endl; |
4718 | return BROKEN_COVARIANCE_MATRIX; |
4719 | } |
4720 | bool skip_ring |
4721 | =(my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP); |
4722 | // update covariance matrix and state vector |
4723 | if (my_cdchits[cdc_index]->hit->wire->ring!=RING_TO_SKIP && tdrift >= CDC_T_DRIFT_MIN){ |
4724 | C=Ctest; |
4725 | S+=res*Kc; |
4726 | |
4727 | if (DEBUG_LEVEL > 25) { |
4728 | jout << " == Adding CDC Hit in Ring " << my_cdchits[cdc_index]->hit->wire->ring << endl; |
4729 | jout << " New S: " << endl; S.Print(); |
4730 | jout << " New C: " << endl; C.Print(); |
4731 | } |
4732 | } |
4733 | |
4734 | // Flag the place along the reference trajectory with hit id |
4735 | forward_traj[k].h_id=1000+cdc_index; |
4736 | |
4737 | // Store updated info related to this hit |
4738 | double scale=(skip_ring)?1.:(1.-Hc*Kc); |
4739 | cdc_updates[cdc_index].tdrift=tdrift; |
4740 | cdc_updates[cdc_index].tcorr=tcorr; |
4741 | cdc_updates[cdc_index].variance=Vc; |
4742 | cdc_updates[cdc_index].doca=dm; |
4743 | cdc_updates[cdc_index].dDdt0=dDdt0; |
4744 | cdc_used_in_fit[cdc_index]=true; |
4745 | if(tdrift < CDC_T_DRIFT_MIN){ |
4746 | //_DBG_ << tdrift << endl; |
4747 | cdc_used_in_fit[cdc_index]=false; |
4748 | } |
4749 | |
4750 | // Update chi2 and number of degrees of freedom for this hit |
4751 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
4752 | chisq+=scale*res*res/Vc; |
4753 | numdof++; |
4754 | } |
4755 | |
4756 | if (DEBUG_LEVEL>10) |
4757 | jout << "Ring " << my_cdchits[cdc_index]->hit->wire->ring |
4758 | << " Straw " << my_cdchits[cdc_index]->hit->wire->straw |
4759 | << " Pred " << d << " Meas " << dm |
4760 | << " Sigma meas " << sqrt(Vc) |
4761 | << " t " << tcorr |
4762 | << " Chi2 " << (1.-Hc*Kc)*res*res/Vc << endl; |
4763 | |
4764 | break_point_cdc_index=cdc_index; |
4765 | break_point_step_index=k_minus_1; |
4766 | |
4767 | } |
4768 | |
4769 | // If we had to use Brent's algorithm to find the true doca or the |
4770 | // doca to the line corresponding to the wire is downstream of the |
4771 | // cdc endplate, we need to swim the state vector and covariance |
4772 | // matrix back to the appropriate position along the reference |
4773 | // trajectory. |
4774 | if (swimmed_to_doca!=DOCA_NO_BRENT){ |
4775 | double dedx=0.; |
4776 | if (CORRECT_FOR_ELOSS){ |
4777 | dedx=GetdEdx(S(state_q_over_p), |
4778 | forward_traj[k].K_rho_Z_over_A, |
4779 | forward_traj[k].rho_Z_over_A, |
4780 | forward_traj[k].LnI,forward_traj[k].Z); |
4781 | } |
4782 | StepBack(dedx,newz,forward_traj[k].z,S0,S,C); |
4783 | } |
4784 | |
4785 | cdc_updates[cdc_index].S=S; |
4786 | cdc_updates[cdc_index].C=C; |
4787 | } |
4788 | |
4789 | // new wire origin and direction |
4790 | if (cdc_index>0){ |
4791 | cdc_index--; |
4792 | origin=my_cdchits[cdc_index]->origin; |
4793 | z0w=my_cdchits[cdc_index]->z0wire; |
4794 | dir=my_cdchits[cdc_index]->dir; |
4795 | } |
4796 | else more_cdc_measurements=false; |
4797 | |
4798 | // Update the wire position |
4799 | wirepos=origin+(z-z0w)*dir; |
4800 | |
4801 | // new doca |
4802 | dx=S(state_x)-wirepos.X(); |
4803 | dy=S(state_y)-wirepos.Y(); |
4804 | doca2=dx*dx+dy*dy; |
4805 | } |
4806 | old_doca2=doca2; |
4807 | } |
4808 | } |
4809 | // Save final z position |
4810 | z_=forward_traj[forward_traj.size()-1].z; |
4811 | |
4812 | // The following code segment addes a fake point at a well-defined z position |
4813 | // that would correspond to a thin foil target. It should not be turned on |
4814 | // for an extended target. |
4815 | if (ADD_VERTEX_POINT){ |
4816 | double dz_to_target=TARGET_Z-z_; |
4817 | double my_dz=mStepSizeZ*(dz_to_target>0?1.:-1.); |
4818 | int num_steps=int(fabs(dz_to_target/my_dz)); |
4819 | |
4820 | for (int k=0;k<num_steps;k++){ |
4821 | double newz=z_+my_dz; |
4822 | // Step C along z |
4823 | StepJacobian(z_,newz,S,0.,J); |
4824 | C=J*C*J.Transpose(); |
4825 | //C=C.SandwichMultiply(J); |
4826 | |
4827 | // Step S along z |
4828 | Step(z_,newz,0.,S); |
4829 | |
4830 | z_=newz; |
4831 | } |
4832 | |
4833 | // Step C along z |
4834 | StepJacobian(z_,TARGET_Z,S,0.,J); |
4835 | C=J*C*J.Transpose(); |
4836 | //C=C.SandwichMultiply(J); |
4837 | |
4838 | // Step S along z |
4839 | Step(z_,TARGET_Z,0.,S); |
4840 | |
4841 | z_=TARGET_Z; |
4842 | |
4843 | // predicted doca taking into account the orientation of the wire |
4844 | double dy=S(state_y); |
4845 | double dx=S(state_x); |
4846 | double d=sqrt(dx*dx+dy*dy); |
4847 | |
4848 | // Track projection |
4849 | double one_over_d=1./d; |
4850 | Hc_T(state_x)=dx*one_over_d; |
4851 | Hc(state_x)=Hc_T(state_x); |
4852 | Hc_T(state_y)=dy*one_over_d; |
4853 | Hc(state_y)=Hc_T(state_y); |
4854 | |
4855 | // Variance of target point |
4856 | // Variance is for average beam spot size assuming triangular distribution |
4857 | // out to 2.2 mm from the beam line. |
4858 | // sigma_r = 2.2 mm/ sqrt(18) |
4859 | Vc=0.002689; |
4860 | |
4861 | // inverse variance including prediction |
4862 | double InvV1=1./(Vc+Hc*(C*Hc_T)); |
4863 | //double InvV1=1./(Vc+C.SandwichMultiply(H_T)); |
4864 | if (InvV1<0.){ |
4865 | if (DEBUG_LEVEL>0) |
4866 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4866<<" " << "Negative variance???" << endl; |
4867 | return NEGATIVE_VARIANCE; |
4868 | } |
4869 | // Compute KalmanSIMD gain matrix |
4870 | Kc=InvV1*(C*Hc_T); |
4871 | |
4872 | // Update the state vector with the target point |
4873 | // "Measurement" is average of expected beam spot size |
4874 | double res=0.1466666667-d; |
4875 | S+=res*Kc; |
4876 | // Update state vector covariance matrix |
4877 | //C=C-K*(H*C); |
4878 | C=C.SubSym(Kc*(Hc*C)); |
4879 | |
4880 | // Update chi2 for this segment |
4881 | chisq+=(1.-Hc*Kc)*res*res/Vc; |
4882 | numdof++; |
4883 | } |
4884 | |
4885 | // Check that there were enough hits to make this a valid fit |
4886 | if (numdof<6){ |
4887 | chisq=-1.; |
4888 | numdof=0; |
4889 | |
4890 | if (num_cdc==0){ |
4891 | unsigned int new_index=(3*num_fdc)/4; |
4892 | break_point_fdc_index=(new_index>=MIN_HITS_FOR_REFIT)?new_index:(MIN_HITS_FOR_REFIT-1); |
4893 | } |
4894 | else{ |
4895 | unsigned int new_index=(3*num_fdc)/4; |
4896 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
4897 | break_point_fdc_index=new_index; |
4898 | } |
4899 | else{ |
4900 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
4901 | } |
4902 | } |
4903 | return PRUNED_TOO_MANY_HITS; |
4904 | } |
4905 | |
4906 | // chisq*=anneal_factor; |
4907 | numdof-=5; |
4908 | |
4909 | // Final positions in x and y for this leg |
4910 | x_=S(state_x); |
4911 | y_=S(state_y); |
4912 | |
4913 | if (DEBUG_LEVEL>1){ |
4914 | cout << "Position after forward filter: " << x_ << ", " << y_ << ", " << z_ <<endl; |
4915 | cout << "Momentum " << 1./S(state_q_over_p) <<endl; |
4916 | } |
4917 | |
4918 | if (!S.IsFinite()) return FIT_FAILED; |
4919 | |
4920 | // Check if we have a kink in the track or threw away too many hits |
4921 | if (num_cdc>0 && break_point_fdc_index>0 && break_point_cdc_index>2){ |
4922 | if (break_point_fdc_index+num_cdc<MIN_HITS_FOR_REFIT){ |
4923 | //_DBG_ << endl; |
4924 | unsigned int new_index=(3*num_fdc)/4; |
4925 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
4926 | break_point_fdc_index=new_index; |
4927 | } |
4928 | else{ |
4929 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
4930 | } |
4931 | } |
4932 | return BREAK_POINT_FOUND; |
4933 | } |
4934 | if (num_cdc==0 && break_point_fdc_index>2){ |
4935 | //_DBG_ << endl; |
4936 | if (break_point_fdc_index<num_fdc/2){ |
4937 | break_point_fdc_index=(3*num_fdc)/4; |
4938 | } |
4939 | if (break_point_fdc_index<MIN_HITS_FOR_REFIT-1){ |
4940 | break_point_fdc_index=MIN_HITS_FOR_REFIT-1; |
4941 | } |
4942 | return BREAK_POINT_FOUND; |
4943 | } |
4944 | if (num_cdc>5 && break_point_cdc_index>2){ |
4945 | //_DBG_ << endl; |
4946 | unsigned int new_index=3*(num_fdc)/4; |
4947 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
4948 | break_point_fdc_index=new_index; |
4949 | } |
4950 | else{ |
4951 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
4952 | } |
4953 | return BREAK_POINT_FOUND; |
4954 | } |
4955 | unsigned int num_good=0; |
4956 | unsigned int num_hits=num_cdc+max_num_fdc_used_in_fit; |
4957 | for (unsigned int j=0;j<num_cdc;j++){ |
4958 | if (cdc_used_in_fit[j]) num_good++; |
4959 | } |
4960 | for (unsigned int j=0;j<num_fdc;j++){ |
4961 | if (fdc_used_in_fit[j]) num_good++; |
4962 | } |
4963 | if (double(num_good)/double(num_hits)<MINIMUM_HIT_FRACTION){ |
4964 | //_DBG_ <<endl; |
4965 | if (num_cdc==0){ |
4966 | unsigned int new_index=(3*num_fdc)/4; |
4967 | break_point_fdc_index=(new_index>=MIN_HITS_FOR_REFIT)?new_index:(MIN_HITS_FOR_REFIT-1); |
4968 | } |
4969 | else{ |
4970 | unsigned int new_index=(3*num_fdc)/4; |
4971 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
4972 | break_point_fdc_index=new_index; |
4973 | } |
4974 | else{ |
4975 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
4976 | } |
4977 | } |
4978 | return PRUNED_TOO_MANY_HITS; |
4979 | } |
4980 | |
4981 | return FIT_SUCCEEDED; |
4982 | } |
4983 | |
4984 | |
4985 | |
4986 | // Kalman engine for forward tracks -- this routine adds CDC hits |
4987 | kalman_error_t DTrackFitterKalmanSIMD::KalmanForwardCDC(double anneal, |
4988 | DMatrix5x1 &S, |
4989 | DMatrix5x5 &C, |
4990 | double &chisq, |
4991 | unsigned int &numdof){ |
4992 | DMatrix1x5 H; // Track projection matrix |
4993 | DMatrix5x1 H_T; // Transpose of track projection matrix |
4994 | DMatrix5x5 J; // State vector Jacobian matrix |
4995 | //DMatrix5x5 J_T; // transpose of this matrix |
4996 | DMatrix5x5 Q; // Process noise covariance matrix |
4997 | DMatrix5x1 K; // KalmanSIMD gain matrix |
4998 | DMatrix5x1 S0,S0_,Stest; //State vector |
4999 | DMatrix5x5 Ctest; // covariance matrix |
5000 | //DMatrix5x1 dS; // perturbation in state vector |
5001 | double V=0.0507; |
5002 | |
5003 | // set used_in_fit flags to false for cdc hits |
5004 | unsigned int num_cdc=cdc_used_in_fit.size(); |
5005 | for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false; |
5006 | for (unsigned int i=0;i<forward_traj.size();i++){ |
5007 | forward_traj[i].h_id=0; |
5008 | } |
5009 | |
5010 | // initialize chi2 info |
5011 | chisq=0.; |
5012 | numdof=0; |
5013 | |
5014 | double chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
5015 | |
5016 | // Save the starting values for C and S in the deque |
5017 | forward_traj[break_point_step_index].Skk=S; |
5018 | forward_traj[break_point_step_index].Ckk=C; |
5019 | |
5020 | // z-position |
5021 | double z=forward_traj[break_point_step_index].z; |
5022 | |
5023 | // wire information |
5024 | unsigned int cdc_index=break_point_cdc_index; |
5025 | if (cdc_index<num_cdc-1){ |
5026 | num_cdc=cdc_index+1; |
5027 | } |
5028 | |
5029 | if (num_cdc<MIN_HITS_FOR_REFIT) chi2cut=BIG1.0e8; |
5030 | |
5031 | DVector2 origin=my_cdchits[cdc_index]->origin; |
5032 | double z0w=my_cdchits[cdc_index]->z0wire; |
5033 | DVector2 dir=my_cdchits[cdc_index]->dir; |
5034 | DVector2 wirepos=origin+(z-z0w)*dir; |
5035 | bool more_measurements=true; |
5036 | |
5037 | // doca variables |
5038 | double dx=S(state_x)-wirepos.X(); |
5039 | double dy=S(state_y)-wirepos.Y(); |
5040 | double doca2=0.,old_doca2=dx*dx+dy*dy; |
5041 | |
5042 | // loop over entries in the trajectory |
5043 | S0_=(forward_traj[break_point_step_index].S); |
5044 | for (unsigned int k=break_point_step_index+1;k<forward_traj.size()/*-1*/;k++){ |
5045 | unsigned int k_minus_1=k-1; |
5046 | |
5047 | z=forward_traj[k].z; |
5048 | |
5049 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
5050 | // from reference trajectory |
5051 | S0=(forward_traj[k].S); |
5052 | J=(forward_traj[k].J); |
5053 | Q=(forward_traj[k].Q); |
5054 | |
5055 | // Update the actual state vector and covariance matrix |
5056 | S=S0+J*(S-S0_); |
5057 | |
5058 | // Bail if the position is grossly outside of the tracking volume |
5059 | if (S(state_x)*S(state_x)+S(state_y)*S(state_y)>R2_MAX4225.0){ |
5060 | if (DEBUG_LEVEL>2) |
5061 | { |
5062 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5062<<" "<< "Went outside of tracking volume at x=" << S(state_x) |
5063 | << " y=" << S(state_y) <<" z="<<z<<endl; |
5064 | } |
5065 | return POSITION_OUT_OF_RANGE; |
5066 | } |
5067 | // Bail if the momentum has dropped below some minimum |
5068 | if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX){ |
5069 | if (DEBUG_LEVEL>2) |
5070 | { |
5071 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5071<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; |
5072 | } |
5073 | return MOMENTUM_OUT_OF_RANGE; |
5074 | } |
5075 | |
5076 | //C=J*(C*J_T)+Q; |
5077 | C=Q.AddSym(J*C*J.Transpose()); |
5078 | //C=Q.AddSym(C.SandwichMultiply(J)); |
5079 | |
5080 | // Save the current state of the reference trajectory |
5081 | S0_=S0; |
5082 | |
5083 | // new wire position |
5084 | wirepos=origin; |
5085 | wirepos+=(z-z0w)*dir; |
5086 | |
5087 | // new doca |
5088 | dx=S(state_x)-wirepos.X(); |
5089 | dy=S(state_y)-wirepos.Y(); |
5090 | doca2=dx*dx+dy*dy; |
5091 | |
5092 | // Save the current state and covariance matrix in the deque |
5093 | if (fit_type==kTimeBased){ |
5094 | forward_traj[k].Skk=S; |
5095 | forward_traj[k].Ckk=C; |
5096 | } |
5097 | |
5098 | // Check if the doca is no longer decreasing |
5099 | if (more_measurements && doca2>old_doca2/* && z<endplate_z_downstream*/){ |
5100 | if (my_cdchits[cdc_index]->status==good_hit){ |
5101 | find_doca_error_t swimmed_to_doca=DOCA_NO_BRENT; |
5102 | double newz=endplate_z; |
5103 | double dz=newz-z; |
5104 | // Sometimes the true doca would correspond to the case where the |
5105 | // wire would need to extend beyond the physical volume of the straw. |
5106 | // In this case, find the doca at the cdc endplate. |
5107 | if (z>endplate_z){ |
5108 | swimmed_to_doca=DOCA_ENDPLATE; |
5109 | SwimToEndplate(z,forward_traj[k],S); |
5110 | |
5111 | // wire position at the endplate |
5112 | wirepos=origin; |
5113 | wirepos+=(endplate_z-z0w)*dir; |
5114 | |
5115 | dx=S(state_x)-wirepos.X(); |
5116 | dy=S(state_y)-wirepos.Y(); |
5117 | } |
5118 | else{ |
5119 | // Find the true doca to the wire. If we had to use Brent's |
5120 | // algorithm, the routine returns USED_BRENT |
5121 | double step1=mStepSizeZ; |
5122 | double step2=mStepSizeZ; |
5123 | if (k>=2){ |
5124 | step1=-forward_traj[k].z+forward_traj[k_minus_1].z; |
5125 | step2=-forward_traj[k_minus_1].z+forward_traj[k-2].z; |
5126 | } |
5127 | swimmed_to_doca=FindDoca(my_cdchits[cdc_index],forward_traj[k], |
5128 | step1,step2,S0,S,C,dx,dy,dz); |
5129 | if (swimmed_to_doca==BRENT_FAILED){ |
5130 | break_point_cdc_index=(3*num_cdc)/4; |
5131 | return MOMENTUM_OUT_OF_RANGE; |
5132 | } |
5133 | newz=forward_traj[k].z+dz; |
5134 | } |
5135 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
5136 | double d=sqrt(dx*dx+dy*dy)*cosstereo+EPS3.0e-8; |
5137 | |
5138 | // Track projection |
5139 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
5140 | H_T(state_x)=dx*cosstereo2_over_d; |
5141 | H(state_x)=H_T(state_x); |
5142 | H_T(state_y)=dy*cosstereo2_over_d; |
5143 | H(state_y)=H_T(state_y); |
5144 | if (swimmed_to_doca==DOCA_NO_BRENT){ |
5145 | H_T(state_ty)=H_T(state_y)*dz; |
5146 | H(state_ty)=H_T(state_ty); |
5147 | H_T(state_tx)=H_T(state_x)*dz; |
5148 | H(state_tx)=H_T(state_tx); |
5149 | } |
5150 | else{ |
5151 | H_T(state_ty)=0.; |
5152 | H(state_ty)=0.; |
5153 | H_T(state_tx)=0.; |
5154 | H(state_tx)=0.; |
5155 | } |
5156 | |
5157 | // The next measurement |
5158 | double dm=0.39,tdrift=0.,tcorr=0.,dDdt0=0.; |
5159 | if (fit_type==kTimeBased || USE_PASS1_TIME_MODE){ |
5160 | // Find offset of wire with respect to the center of the |
5161 | // straw at this z position |
5162 | double delta=0,dphi=0.; |
5163 | FindSag(dx,dy,newz-z0w,my_cdchits[cdc_index]->hit->wire,delta,dphi); |
5164 | // Find drift time and distance |
5165 | tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
5166 | -forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
5167 | double B=forward_traj[k_minus_1].B; |
5168 | ComputeCDCDrift(dphi,delta,tdrift,B,dm,V,tcorr); |
5169 | V*=anneal; |
5170 | if (ALIGNMENT_FORWARD){ |
5171 | double myV=0.; |
5172 | double mytcorr=0.; |
5173 | double d_shifted; |
5174 | double dt=2.0; |
5175 | if (tdrift < 0.) d_shifted = dm; |
5176 | else ComputeCDCDrift(dphi,delta,tdrift+dt,B,d_shifted,myV,mytcorr); |
5177 | dDdt0=(d_shifted-dm)/dt; |
5178 | } |
5179 | //_DBG_ << tcorr << " " << dphi << " " << dm << endl; |
5180 | |
5181 | } |
5182 | // residual |
5183 | double res=dm-d; |
5184 | |
5185 | // inverse of variance including prediction |
5186 | double Vproj=H*C*H_T; |
5187 | double InvV=1./(V+Vproj); |
5188 | if (InvV<0.){ |
5189 | if (DEBUG_LEVEL>0) |
5190 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5190<<" " << "Negative variance???" << endl; |
5191 | break_point_cdc_index=(3*num_cdc)/4; |
5192 | return NEGATIVE_VARIANCE; |
5193 | } |
5194 | |
5195 | // Check how far this hit is from the expected position |
5196 | double chi2check=res*res*InvV; |
5197 | if (chi2check<chi2cut){ |
5198 | // Compute KalmanSIMD gain matrix |
5199 | K=InvV*(C*H_T); |
5200 | |
5201 | // Update state vector covariance matrix |
5202 | Ctest=C.SubSym(K*(H*C)); |
5203 | // Joseph form |
5204 | // C = (I-KH)C(I-KH)^T + KVK^T |
5205 | //Ctest=C.SandwichMultiply(I5x5-K*H)+V*MultiplyTranspose(K); |
5206 | // Check that Ctest is positive definite |
5207 | if (!Ctest.IsPosDef()){ |
5208 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5208<<" " << "Broken covariance matrix!" <<endl; |
5209 | return BROKEN_COVARIANCE_MATRIX; |
5210 | } |
5211 | |
5212 | bool skip_ring |
5213 | =(my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP); |
5214 | // update covariance matrix and state vector |
5215 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
5216 | C=Ctest; |
5217 | S+=res*K; |
5218 | } |
5219 | // Mark point on ref trajectory with a hit id for the straw |
5220 | forward_traj[k].h_id=cdc_index+1000; |
5221 | |
5222 | // Store some updated values related to the hit |
5223 | double scale=(skip_ring)?1.:(1.-H*K); |
5224 | cdc_updates[cdc_index].tcorr=tcorr; |
5225 | cdc_updates[cdc_index].tdrift=tdrift; |
5226 | cdc_updates[cdc_index].doca=dm; |
5227 | cdc_updates[cdc_index].variance=V; |
5228 | cdc_updates[cdc_index].dDdt0=dDdt0; |
5229 | cdc_used_in_fit[cdc_index]=true; |
5230 | if(tdrift < CDC_T_DRIFT_MIN) cdc_used_in_fit[cdc_index]=false; |
5231 | |
5232 | // Update chi2 for this segment |
5233 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
5234 | chisq+=scale*res*res/V; |
5235 | numdof++; |
5236 | } |
5237 | break_point_cdc_index=cdc_index; |
5238 | break_point_step_index=k_minus_1; |
5239 | |
5240 | if (DEBUG_LEVEL>9) |
5241 | printf("Ring %d straw %d pred %f meas %f chi2 %f useBrent %i \n", |
5242 | my_cdchits[cdc_index]->hit->wire->ring, |
5243 | my_cdchits[cdc_index]->hit->wire->straw, |
5244 | d,dm,(1.-H*K)*res*res/V,swimmed_to_doca); |
5245 | |
5246 | } |
5247 | |
5248 | // If we had to use Brent's algorithm to find the true doca or the |
5249 | // doca to the line corresponding to the wire is downstream of the |
5250 | // cdc endplate, we need to swim the state vector and covariance |
5251 | // matrix back to the appropriate position along the reference |
5252 | // trajectory. |
5253 | if (swimmed_to_doca!=DOCA_NO_BRENT){ |
5254 | double dedx=0.; |
5255 | if (CORRECT_FOR_ELOSS){ |
5256 | dedx=GetdEdx(S(state_q_over_p), |
5257 | forward_traj[k].K_rho_Z_over_A, |
5258 | forward_traj[k].rho_Z_over_A, |
5259 | forward_traj[k].LnI,forward_traj[k].Z); |
5260 | } |
5261 | StepBack(dedx,newz,forward_traj[k].z,S0,S,C); |
5262 | } |
5263 | |
5264 | cdc_updates[cdc_index].S=S; |
5265 | cdc_updates[cdc_index].C=C; |
5266 | } |
5267 | |
5268 | // new wire origin and direction |
5269 | if (cdc_index>0){ |
5270 | cdc_index--; |
5271 | origin=my_cdchits[cdc_index]->origin; |
5272 | z0w=my_cdchits[cdc_index]->z0wire; |
5273 | dir=my_cdchits[cdc_index]->dir; |
5274 | } |
5275 | else{ |
5276 | more_measurements=false; |
5277 | } |
5278 | |
5279 | // Update the wire position |
5280 | wirepos=origin; |
5281 | wirepos+=(z-z0w)*dir; |
5282 | |
5283 | // new doca |
5284 | dx=S(state_x)-wirepos.X(); |
5285 | dy=S(state_y)-wirepos.Y(); |
5286 | doca2=dx*dx+dy*dy; |
5287 | } |
5288 | old_doca2=doca2; |
5289 | } |
5290 | |
5291 | // Check that there were enough hits to make this a valid fit |
5292 | if (numdof<6){ |
5293 | chisq=-1.; |
5294 | numdof=0; |
5295 | return PRUNED_TOO_MANY_HITS; |
5296 | } |
5297 | numdof-=5; |
5298 | |
5299 | // Final position for this leg |
5300 | x_=S(state_x); |
5301 | y_=S(state_y); |
5302 | z_=forward_traj[forward_traj.size()-1].z; |
5303 | |
5304 | if (!S.IsFinite()) return FIT_FAILED; |
5305 | |
5306 | // Check if the momentum is unphysically large |
5307 | if (1./fabs(S(state_q_over_p))>12.0){ |
5308 | if (DEBUG_LEVEL>2) |
5309 | { |
5310 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5310<<" " << "Unphysical momentum: P = " << 1./fabs(S(state_q_over_p)) |
5311 | <<endl; |
5312 | } |
5313 | return MOMENTUM_OUT_OF_RANGE; |
5314 | } |
5315 | |
5316 | // Check if we have a kink in the track or threw away too many cdc hits |
5317 | if (num_cdc>=MIN_HITS_FOR_REFIT){ |
5318 | if (break_point_cdc_index>1){ |
5319 | if (break_point_cdc_index<num_cdc/2){ |
5320 | break_point_cdc_index=(3*num_cdc)/4; |
5321 | } |
5322 | return BREAK_POINT_FOUND; |
5323 | } |
5324 | |
5325 | unsigned int num_good=0; |
5326 | for (unsigned int j=0;j<num_cdc;j++){ |
5327 | if (cdc_used_in_fit[j]) num_good++; |
5328 | } |
5329 | if (double(num_good)/double(num_cdc)<MINIMUM_HIT_FRACTION){ |
5330 | return PRUNED_TOO_MANY_HITS; |
5331 | } |
5332 | } |
5333 | |
5334 | return FIT_SUCCEEDED; |
5335 | } |
5336 | |
5337 | // Extrapolate to the point along z of closest approach to the beam line using |
5338 | // the forward track state vector parameterization. Converts to the central |
5339 | // track representation at the end. |
5340 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DMatrix5x1 &S, |
5341 | DMatrix5x5 &C){ |
5342 | DMatrix5x5 J; // Jacobian matrix |
5343 | DMatrix5x5 Q; // multiple scattering matrix |
5344 | DMatrix5x1 S1(S); // copy of S |
5345 | |
5346 | // position variables |
5347 | double z=z_,newz=z_; |
5348 | |
5349 | DVector2 beam_pos=beam_center+(z-beam_z0)*beam_dir; |
5350 | double r2_old=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5351 | double dz_old=0.; |
5352 | double dEdx=0.; |
5353 | double sign=1.; |
5354 | |
5355 | // material properties |
5356 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
5357 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5358 | |
5359 | // if (fit_type==kTimeBased)printf("------Extrapolating\n"); |
5360 | |
5361 | // printf("-----------\n"); |
5362 | // Current position |
5363 | DVector3 pos(S(state_x),S(state_y),z); |
5364 | |
5365 | // get material properties from the Root Geometry |
5366 | if (geom->FindMatKalman(pos,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5367 | chi2c_factor,chi2a_factor,chi2a_corr, |
5368 | last_material_map) |
5369 | !=NOERROR){ |
5370 | if (DEBUG_LEVEL>1) |
5371 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5371<<" " << "Material error in ExtrapolateToVertex at (x,y,z)=(" |
5372 | << pos.X() <<"," << pos.y()<<","<< pos.z()<<")"<< endl; |
5373 | return UNRECOVERABLE_ERROR; |
5374 | } |
5375 | |
5376 | // Adjust the step size |
5377 | double ds_dz=sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
5378 | double dz=-mStepSizeS/ds_dz; |
5379 | if (fabs(dEdx)>EPS3.0e-8){ |
5380 | dz=(-1.)*DE_PER_STEP0.001/fabs(dEdx)/ds_dz; |
5381 | } |
5382 | if(fabs(dz)>mStepSizeZ) dz=-mStepSizeZ; |
5383 | if(fabs(dz)<MIN_STEP_SIZE0.1)dz=-MIN_STEP_SIZE0.1; |
5384 | |
5385 | // Get dEdx for the upcoming step |
5386 | if (CORRECT_FOR_ELOSS){ |
5387 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5388 | } |
5389 | |
5390 | |
5391 | double ztest=endplate_z; |
5392 | if (my_fdchits.size()>0){ |
5393 | ztest =my_fdchits[0]->z-1.; |
5394 | } |
5395 | if (z<ztest) |
5396 | { |
5397 | // Check direction of propagation |
5398 | DMatrix5x1 S2(S); // second copy of S |
5399 | |
5400 | // Step test states through the field and compute squared radii |
5401 | Step(z,z-dz,dEdx,S1); |
5402 | // Bail if the momentum has dropped below some minimum |
5403 | if (fabs(S1(state_q_over_p))>Q_OVER_P_MAX){ |
5404 | if (DEBUG_LEVEL>2) |
5405 | { |
5406 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5406<<" " << "Bailing: P = " << 1./fabs(S1(state_q_over_p)) |
5407 | << endl; |
5408 | } |
5409 | return UNRECOVERABLE_ERROR; |
5410 | } |
5411 | beam_pos=beam_center+(z-dz-beam_z0)*beam_dir; |
5412 | double r2minus=(DVector2(S1(state_x),S1(state_y))-beam_pos).Mod2(); |
5413 | |
5414 | Step(z,z+dz,dEdx,S2); |
5415 | // Bail if the momentum has dropped below some minimum |
5416 | if (fabs(S2(state_q_over_p))>Q_OVER_P_MAX){ |
5417 | if (DEBUG_LEVEL>2) |
5418 | { |
5419 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5419<<" " << "Bailing: P = " << 1./fabs(S2(state_q_over_p)) |
5420 | << endl; |
5421 | } |
5422 | return UNRECOVERABLE_ERROR; |
5423 | } |
5424 | beam_pos=beam_center+(z+dz-beam_z0)*beam_dir; |
5425 | double r2plus=(DVector2(S2(state_x),S2(state_y))-beam_pos).Mod2(); |
5426 | // Check to see if we have already bracketed the minimum |
5427 | if (r2plus>r2_old && r2minus>r2_old){ |
5428 | newz=z+dz; |
5429 | double dz2=0.; |
5430 | if (BrentsAlgorithm(newz,dz,dEdx,0.,beam_center,beam_dir,S2,dz2)!=NOERROR){ |
5431 | if (DEBUG_LEVEL>2) |
5432 | { |
5433 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5433<<" " << "Bailing: P = " << 1./fabs(S2(state_q_over_p)) |
5434 | << endl; |
5435 | } |
5436 | return UNRECOVERABLE_ERROR; |
5437 | } |
5438 | z_=newz+dz2; |
5439 | |
5440 | // Compute the Jacobian matrix |
5441 | StepJacobian(z,z_,S,dEdx,J); |
5442 | |
5443 | // Propagate the covariance matrix |
5444 | C=J*C*J.Transpose(); |
5445 | //C=C.SandwichMultiply(J); |
5446 | |
5447 | // Step to the position of the doca |
5448 | Step(z,z_,dEdx,S); |
5449 | |
5450 | // update internal variables |
5451 | x_=S(state_x); |
5452 | y_=S(state_y); |
5453 | |
5454 | return NOERROR; |
5455 | } |
5456 | |
5457 | // Find direction to propagate toward minimum doca |
5458 | if (r2minus<r2_old && r2_old<r2plus){ |
5459 | newz=z-dz; |
5460 | |
5461 | // Compute the Jacobian matrix |
5462 | StepJacobian(z,newz,S,dEdx,J); |
5463 | |
5464 | // Propagate the covariance matrix |
5465 | C=J*C*J.Transpose(); |
5466 | //C=C.SandwichMultiply(J); |
5467 | |
5468 | S2=S; |
5469 | S=S1; |
5470 | S1=S2; |
5471 | dz*=-1.; |
5472 | sign=-1.; |
5473 | dz_old=dz; |
5474 | r2_old=r2minus; |
5475 | z=z+dz; |
5476 | } |
5477 | if (r2minus>r2_old && r2_old>r2plus){ |
5478 | newz=z+dz; |
5479 | |
5480 | // Compute the Jacobian matrix |
5481 | StepJacobian(z,newz,S,dEdx,J); |
5482 | |
5483 | // Propagate the covariance matrix |
5484 | C=J*C*J.Transpose(); |
5485 | //C=C.SandwichMultiply(J); |
5486 | |
5487 | S1=S; |
5488 | S=S2; |
5489 | dz_old=dz; |
5490 | r2_old=r2plus; |
5491 | z=z+dz; |
5492 | } |
5493 | } |
5494 | |
5495 | double r2=r2_old; |
5496 | while (z>Z_MIN-100. && r2<R2_MAX4225.0 && z<ztest && r2>EPS3.0e-8){ |
5497 | // Bail if the momentum has dropped below some minimum |
5498 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
5499 | if (DEBUG_LEVEL>2) |
5500 | { |
5501 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5501<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
5502 | << endl; |
5503 | } |
5504 | return UNRECOVERABLE_ERROR; |
5505 | } |
5506 | |
5507 | // Relationship between arc length and z |
5508 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
5509 | |
5510 | // get material properties from the Root Geometry |
5511 | pos.SetXYZ(S(state_x),S(state_y),z); |
5512 | double s_to_boundary=1.e6; |
5513 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
5514 | DVector3 mom(S(state_tx),S(state_ty),1.); |
5515 | if (geom->FindMatKalman(pos,mom,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5516 | chi2c_factor,chi2a_factor,chi2a_corr, |
5517 | last_material_map,&s_to_boundary) |
5518 | !=NOERROR){ |
5519 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5519<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5520 | return UNRECOVERABLE_ERROR; |
5521 | } |
5522 | } |
5523 | else{ |
5524 | if (geom->FindMatKalman(pos,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5525 | chi2c_factor,chi2a_factor,chi2a_corr, |
5526 | last_material_map) |
5527 | !=NOERROR){ |
5528 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5528<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5529 | break; |
5530 | } |
5531 | } |
5532 | |
5533 | // Get dEdx for the upcoming step |
5534 | if (CORRECT_FOR_ELOSS){ |
5535 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5536 | } |
5537 | |
5538 | // Adjust the step size |
5539 | //dz=-sign*mStepSizeS*dz_ds; |
5540 | double ds=mStepSizeS; |
5541 | if (fabs(dEdx)>EPS3.0e-8){ |
5542 | ds=DE_PER_STEP0.001/fabs(dEdx); |
5543 | } |
5544 | /* |
5545 | if(fabs(dz)>mStepSizeZ) dz=-sign*mStepSizeZ; |
5546 | if (fabs(dz)<z_to_boundary) dz=-sign*z_to_boundary; |
5547 | if(fabs(dz)<MIN_STEP_SIZE)dz=-sign*MIN_STEP_SIZE; |
5548 | */ |
5549 | if (ds>mStepSizeS) ds=mStepSizeS; |
5550 | if (ds>s_to_boundary) ds=s_to_boundary; |
5551 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
5552 | dz=-sign*ds*dz_ds; |
5553 | |
5554 | // Get the contribution to the covariance matrix due to multiple |
5555 | // scattering |
5556 | GetProcessNoise(z,ds,chi2c_factor,chi2a_factor,chi2a_corr,S,Q); |
5557 | |
5558 | if (CORRECT_FOR_ELOSS){ |
5559 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
5560 | double one_over_beta2=1.+mass2*q_over_p_sq; |
5561 | double varE=GetEnergyVariance(ds,one_over_beta2,K_rho_Z_over_A); |
5562 | Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2; |
5563 | } |
5564 | |
5565 | |
5566 | newz=z+dz; |
5567 | // Compute the Jacobian matrix |
5568 | StepJacobian(z,newz,S,dEdx,J); |
5569 | |
5570 | // Propagate the covariance matrix |
5571 | C=Q.AddSym(J*C*J.Transpose()); |
5572 | //C=Q.AddSym(C.SandwichMultiply(J)); |
5573 | |
5574 | // Step through field |
5575 | Step(z,newz,dEdx,S); |
5576 | |
5577 | // Check if we passed the minimum doca to the beam line |
5578 | beam_pos=beam_center+(newz-beam_z0)*beam_dir; |
5579 | r2=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5580 | //r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
5581 | if (r2>r2_old){ |
5582 | double two_step=dz+dz_old; |
5583 | |
5584 | // Find the increment/decrement in z to get to the minimum doca to the |
5585 | // beam line |
5586 | S1=S; |
5587 | if (BrentsAlgorithm(newz,0.5*two_step,dEdx,0.,beam_center,beam_dir,S,dz)!=NOERROR){ |
5588 | //_DBG_<<endl; |
5589 | return UNRECOVERABLE_ERROR; |
5590 | } |
5591 | |
5592 | // Compute the Jacobian matrix |
5593 | z_=newz+dz; |
5594 | StepJacobian(newz,z_,S1,dEdx,J); |
5595 | |
5596 | // Propagate the covariance matrix |
5597 | //C=J*C*J.Transpose()+(dz/(newz-z))*Q; |
5598 | //C=((dz/newz-z)*Q).AddSym(C.SandwichMultiply(J)); |
5599 | //C=C.SandwichMultiply(J); |
5600 | C=J*C*J.Transpose(); |
5601 | |
5602 | // update internal variables |
5603 | x_=S(state_x); |
5604 | y_=S(state_y); |
5605 | |
5606 | return NOERROR; |
5607 | } |
5608 | r2_old=r2; |
5609 | dz_old=dz; |
5610 | S1=S; |
5611 | z=newz; |
5612 | } |
5613 | // update internal variables |
5614 | x_=S(state_x); |
5615 | y_=S(state_y); |
5616 | z_=newz; |
5617 | |
5618 | return NOERROR; |
5619 | } |
5620 | |
5621 | |
5622 | // Extrapolate to the point along z of closest approach to the beam line using |
5623 | // the forward track state vector parameterization. |
5624 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DMatrix5x1 &S){ |
5625 | DMatrix5x5 J; // Jacobian matrix |
5626 | DMatrix5x1 S1(S); // copy of S |
5627 | |
5628 | // position variables |
5629 | double z=z_,newz=z_; |
5630 | DVector2 beam_pos=beam_center+(z-beam_z0)*beam_dir; |
5631 | double r2_old=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5632 | double dz_old=0.; |
5633 | double dEdx=0.; |
5634 | |
5635 | // Direction and origin for beam line |
5636 | DVector2 dir; |
5637 | DVector2 origin; |
5638 | |
5639 | // material properties |
5640 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
5641 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5642 | DVector3 pos; // current position along trajectory |
5643 | |
5644 | double r2=r2_old; |
5645 | while (z>Z_MIN-100. && r2<R2_MAX4225.0 && z<Z_MAX370.0 && r2>EPS3.0e-8){ |
5646 | // Bail if the momentum has dropped below some minimum |
5647 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
5648 | if (DEBUG_LEVEL>2) |
5649 | { |
5650 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5650<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
5651 | << endl; |
5652 | } |
5653 | return UNRECOVERABLE_ERROR; |
5654 | } |
5655 | |
5656 | // Relationship between arc length and z |
5657 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
5658 | |
5659 | // get material properties from the Root Geometry |
5660 | pos.SetXYZ(S(state_x),S(state_y),z); |
5661 | if (geom->FindMatKalman(pos,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5662 | chi2c_factor,chi2a_factor,chi2a_corr, |
5663 | last_material_map) |
5664 | !=NOERROR){ |
5665 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5665<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5666 | break; |
5667 | } |
5668 | |
5669 | // Get dEdx for the upcoming step |
5670 | if (CORRECT_FOR_ELOSS){ |
5671 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5672 | } |
5673 | |
5674 | // Adjust the step size |
5675 | double ds=mStepSizeS; |
5676 | if (fabs(dEdx)>EPS3.0e-8){ |
5677 | ds=DE_PER_STEP0.001/fabs(dEdx); |
5678 | } |
5679 | if (ds>mStepSizeS) ds=mStepSizeS; |
5680 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
5681 | double dz=-ds*dz_ds; |
5682 | |
5683 | newz=z+dz; |
5684 | |
5685 | |
5686 | // Step through field |
5687 | Step(z,newz,dEdx,S); |
5688 | |
5689 | // Check if we passed the minimum doca to the beam line |
5690 | beam_pos=beam_center+(newz-beam_z0)*beam_dir; |
5691 | r2=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5692 | |
5693 | if (r2>r2_old && newz<endplate_z){ |
5694 | double two_step=dz+dz_old; |
5695 | |
5696 | // Find the increment/decrement in z to get to the minimum doca to the |
5697 | // beam line |
5698 | if (BrentsAlgorithm(newz,0.5*two_step,dEdx,0.,beam_center,beam_dir,S,dz)!=NOERROR){ |
5699 | return UNRECOVERABLE_ERROR; |
5700 | } |
5701 | // update internal variables |
5702 | x_=S(state_x); |
5703 | y_=S(state_y); |
5704 | z_=newz+dz; |
5705 | |
5706 | return NOERROR; |
5707 | } |
5708 | r2_old=r2; |
5709 | dz_old=dz; |
5710 | z=newz; |
5711 | } |
5712 | |
5713 | // If we extrapolate beyond the fiducial volume of the detector before |
5714 | // finding the doca, abandon the extrapolation... |
5715 | if (newz<Z_MIN-100.){ |
5716 | //_DBG_ << "Extrapolated z = " << newz << endl; |
5717 | // Restore old state vector |
5718 | S=S1; |
5719 | return VALUE_OUT_OF_RANGE; |
5720 | } |
5721 | |
5722 | // update internal variables |
5723 | x_=S(state_x); |
5724 | y_=S(state_y); |
5725 | z_=newz; |
5726 | |
5727 | |
5728 | return NOERROR; |
5729 | } |
5730 | |
5731 | |
5732 | |
5733 | |
5734 | // Propagate track to point of distance of closest approach to origin |
5735 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DVector2 &xy, |
5736 | DMatrix5x1 &Sc,DMatrix5x5 &Cc){ |
5737 | DMatrix5x5 Jc=I5x5; //Jacobian matrix |
5738 | DMatrix5x5 Q; // multiple scattering matrix |
5739 | |
5740 | // Position and step variables |
5741 | DVector2 beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
5742 | double r2=(xy-beam_pos).Mod2(); |
5743 | double ds=-mStepSizeS; // step along path in cm |
5744 | double r2_old=r2; |
5745 | |
5746 | // Energy loss |
5747 | double dedx=0.; |
5748 | |
5749 | // Check direction of propagation |
5750 | DMatrix5x1 S0; |
5751 | S0=Sc; |
5752 | DVector2 xy0=xy; |
5753 | DVector2 xy1=xy; |
5754 | Step(xy0,ds,S0,dedx); |
5755 | // Bail if the transverse momentum has dropped below some minimum |
5756 | if (fabs(S0(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
5757 | if (DEBUG_LEVEL>2) |
5758 | { |
5759 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5759<<" " << "Bailing: PT = " << 1./fabs(S0(state_q_over_pt)) |
5760 | << endl; |
5761 | } |
5762 | return UNRECOVERABLE_ERROR; |
5763 | } |
5764 | beam_pos=beam_center+(S0(state_z)-beam_z0)*beam_dir; |
5765 | r2=(xy0-beam_pos).Mod2(); |
5766 | if (r2>r2_old) ds*=-1.; |
5767 | double ds_old=ds; |
5768 | |
5769 | // if (fit_type==kTimeBased)printf("------Extrapolating\n"); |
5770 | |
5771 | if (central_traj.size()==0){ |
5772 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5772<<" " << "Central trajectory size==0!" << endl; |
5773 | return UNRECOVERABLE_ERROR; |
5774 | } |
5775 | |
5776 | // D is now on the actual trajectory itself |
5777 | Sc(state_D)=0.; |
5778 | |
5779 | // Track propagation loop |
5780 | while (Sc(state_z)>Z_MIN-100. && Sc(state_z)<Z_MAX370.0 |
5781 | && r2<R2_MAX4225.0){ |
5782 | // Bail if the transverse momentum has dropped below some minimum |
5783 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
5784 | if (DEBUG_LEVEL>2) |
5785 | { |
5786 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5786<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
5787 | << endl; |
5788 | } |
5789 | return UNRECOVERABLE_ERROR; |
5790 | } |
5791 | |
5792 | // get material properties from the Root Geometry |
5793 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
5794 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5795 | DVector3 pos3d(xy.X(),xy.Y(),Sc(state_z)); |
5796 | if (geom->FindMatKalman(pos3d,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5797 | chi2c_factor,chi2a_factor,chi2a_corr, |
5798 | last_material_map) |
5799 | !=NOERROR){ |
5800 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5800<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5801 | break; |
5802 | } |
5803 | |
5804 | // Get dEdx for the upcoming step |
5805 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
5806 | if (CORRECT_FOR_ELOSS){ |
5807 | dedx=-GetdEdx(q_over_p,K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5808 | } |
5809 | // Adjust the step size |
5810 | double sign=(ds>0.0)?1.:-1.; |
5811 | if (fabs(dedx)>EPS3.0e-8){ |
5812 | ds=sign*DE_PER_STEP0.001/fabs(dedx); |
5813 | } |
5814 | if(fabs(ds)>mStepSizeS) ds=sign*mStepSizeS; |
5815 | if(fabs(ds)<MIN_STEP_SIZE0.1)ds=sign*MIN_STEP_SIZE0.1; |
5816 | |
5817 | // Multiple scattering |
5818 | GetProcessNoiseCentral(ds,chi2c_factor,chi2a_factor,chi2a_corr,Sc,Q); |
5819 | |
5820 | if (CORRECT_FOR_ELOSS){ |
5821 | double q_over_p_sq=q_over_p*q_over_p; |
5822 | double one_over_beta2=1.+mass2*q_over_p*q_over_p; |
5823 | double varE=GetEnergyVariance(ds,one_over_beta2,K_rho_Z_over_A); |
5824 | Q(state_q_over_pt,state_q_over_pt) |
5825 | +=varE*Sc(state_q_over_pt)*Sc(state_q_over_pt)*one_over_beta2 |
5826 | *q_over_p_sq; |
5827 | } |
5828 | |
5829 | // Propagate the state and covariance through the field |
5830 | S0=Sc; |
5831 | DVector2 old_xy=xy; |
5832 | StepStateAndCovariance(xy,ds,dedx,Sc,Jc,Cc); |
5833 | |
5834 | // Add contribution due to multiple scattering |
5835 | Cc=(sign*Q).AddSym(Cc); |
5836 | |
5837 | beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
5838 | r2=(xy-beam_pos).Mod2(); |
5839 | //printf("r %f r_old %f \n",sqrt(r2),sqrt(r2_old)); |
5840 | if (r2>r2_old) { |
5841 | // We've passed the true minimum; backtrack to find the "vertex" |
5842 | // position |
5843 | double my_ds=0.; |
5844 | if (BrentsAlgorithm(ds,ds_old,dedx,xy,0.,beam_center,beam_dir,Sc,my_ds)!=NOERROR){ |
5845 | //_DBG_ <<endl; |
5846 | return UNRECOVERABLE_ERROR; |
5847 | } |
5848 | //printf ("Brent min r %f\n",xy.Mod()); |
5849 | |
5850 | // Find the field and gradient at (old_x,old_y,old_z) |
5851 | bfield->GetFieldAndGradient(old_xy.X(),old_xy.Y(),S0(state_z),Bx,By,Bz, |
5852 | dBxdx,dBxdy,dBxdz,dBydx, |
5853 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
5854 | |
5855 | // Compute the Jacobian matrix |
5856 | my_ds-=ds_old; |
5857 | StepJacobian(old_xy,xy-old_xy,my_ds,S0,dedx,Jc); |
5858 | |
5859 | // Propagate the covariance matrix |
5860 | //Cc=Jc*Cc*Jc.Transpose()+(my_ds/ds_old)*Q; |
5861 | //Cc=((my_ds/ds_old)*Q).AddSym(Cc.SandwichMultiply(Jc)); |
5862 | Cc=((my_ds/ds_old)*Q).AddSym(Jc*Cc*Jc.Transpose()); |
5863 | |
5864 | break; |
5865 | } |
5866 | r2_old=r2; |
5867 | ds_old=ds; |
5868 | } |
5869 | |
5870 | return NOERROR; |
5871 | } |
5872 | |
5873 | // Propagate track to point of distance of closest approach to origin |
5874 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DVector2 &xy, |
5875 | DMatrix5x1 &Sc){ |
5876 | // Save un-extroplated quantities |
5877 | DMatrix5x1 S0(Sc); |
5878 | DVector2 xy0(xy); |
5879 | |
5880 | // Initialize the beam position = center of target, and the direction |
5881 | DVector2 origin; |
5882 | DVector2 dir; |
5883 | |
5884 | // Position and step variables |
5885 | DVector2 beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
5886 | double r2=(xy-beam_pos).Mod2(); |
5887 | double ds=-mStepSizeS; // step along path in cm |
5888 | double r2_old=r2; |
5889 | |
5890 | // Energy loss |
5891 | double dedx=0.; |
5892 | |
5893 | // Check direction of propagation |
5894 | DMatrix5x1 S1=Sc; |
5895 | DVector2 xy1=xy; |
5896 | Step(xy1,ds,S1,dedx); |
5897 | beam_pos=beam_center+(S1(state_z)-beam_z0)*beam_dir; |
5898 | r2=(xy1-beam_pos).Mod2(); |
5899 | if (r2>r2_old) ds*=-1.; |
5900 | double ds_old=ds; |
5901 | |
5902 | // Track propagation loop |
5903 | while (Sc(state_z)>Z_MIN-100. && Sc(state_z)<Z_MAX370.0 |
5904 | && r2<R2_MAX4225.0){ |
5905 | // get material properties from the Root Geometry |
5906 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0; |
5907 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5908 | DVector3 pos3d(xy.X(),xy.Y(),Sc(state_z)); |
5909 | if (geom->FindMatKalman(pos3d,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5910 | chi2c_factor,chi2a_factor,chi2a_corr, |
5911 | last_material_map) |
5912 | !=NOERROR){ |
5913 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5913<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5914 | break; |
5915 | } |
5916 | |
5917 | // Get dEdx for the upcoming step |
5918 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
5919 | if (CORRECT_FOR_ELOSS){ |
5920 | dedx=GetdEdx(q_over_p,K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5921 | } |
5922 | // Adjust the step size |
5923 | double sign=(ds>0.0)?1.:-1.; |
5924 | if (fabs(dedx)>EPS3.0e-8){ |
5925 | ds=sign*DE_PER_STEP0.001/fabs(dedx); |
5926 | } |
5927 | if(fabs(ds)>mStepSizeS) ds=sign*mStepSizeS; |
5928 | if(fabs(ds)<MIN_STEP_SIZE0.1)ds=sign*MIN_STEP_SIZE0.1; |
5929 | |
5930 | // Propagate the state through the field |
5931 | Step(xy,ds,Sc,dedx); |
5932 | |
5933 | beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
5934 | r2=(xy-beam_pos).Mod2(); |
5935 | //printf("r %f r_old %f \n",r,r_old); |
5936 | if (r2>r2_old) { |
5937 | // We've passed the true minimum; backtrack to find the "vertex" |
5938 | // position |
5939 | double my_ds=0.; |
5940 | BrentsAlgorithm(ds,ds_old,dedx,xy,0.,beam_center,beam_dir,Sc,my_ds); |
5941 | //printf ("Brent min r %f\n",pos.Perp()); |
5942 | break; |
5943 | } |
5944 | r2_old=r2; |
5945 | ds_old=ds; |
5946 | } |
5947 | |
5948 | // If we extrapolate beyond the fiducial volume of the detector before |
5949 | // finding the doca, abandon the extrapolation... |
5950 | if (Sc(state_z)<Z_MIN-100.){ |
5951 | //_DBG_ << "Extrapolated z = " << Sc(state_z) << endl; |
5952 | // Restore un-extrapolated values |
5953 | Sc=S0; |
5954 | xy=xy0; |
5955 | return VALUE_OUT_OF_RANGE; |
5956 | } |
5957 | |
5958 | return NOERROR; |
5959 | } |
5960 | |
5961 | |
5962 | |
5963 | |
5964 | // Transform the 5x5 tracking error matrix into a 7x7 error matrix in cartesian |
5965 | // coordinates |
5966 | shared_ptr<TMatrixFSym> DTrackFitterKalmanSIMD::Get7x7ErrorMatrixForward(DMatrixDSym C){ |
5967 | auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); |
5968 | C7x7->ResizeTo(7, 7); |
5969 | DMatrix J(7,5); |
5970 | |
5971 | double p=1./fabs(q_over_p_); |
5972 | double tanl=1./sqrt(tx_*tx_+ty_*ty_); |
5973 | double tanl2=tanl*tanl; |
5974 | double lambda=atan(tanl); |
5975 | double sinl=sin(lambda); |
5976 | double sinl3=sinl*sinl*sinl; |
5977 | |
5978 | J(state_X,state_x)=J(state_Y,state_y)=1.; |
5979 | J(state_Pz,state_ty)=-p*ty_*sinl3; |
5980 | J(state_Pz,state_tx)=-p*tx_*sinl3; |
5981 | J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3; |
5982 | J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3; |
5983 | J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3; |
5984 | J(state_Pz,state_q_over_p)=-p*sinl/q_over_p_; |
5985 | J(state_Px,state_q_over_p)=tx_*J(state_Pz,state_q_over_p); |
5986 | J(state_Py,state_q_over_p)=ty_*J(state_Pz,state_q_over_p); |
5987 | J(state_Z,state_x)=-tx_*tanl2; |
5988 | J(state_Z,state_y)=-ty_*tanl2; |
5989 | double diff=tx_*tx_-ty_*ty_; |
5990 | double frac=tanl2*tanl2; |
5991 | J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac; |
5992 | J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac; |
5993 | |
5994 | // C'= JCJ^T |
5995 | *C7x7=C.Similarity(J); |
5996 | |
5997 | return C7x7; |
5998 | } |
5999 | |
6000 | |
6001 | |
6002 | // Transform the 5x5 tracking error matrix into a 7x7 error matrix in cartesian |
6003 | // coordinates |
6004 | shared_ptr<TMatrixFSym> DTrackFitterKalmanSIMD::Get7x7ErrorMatrix(DMatrixDSym C){ |
6005 | auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); |
6006 | C7x7->ResizeTo(7, 7); |
6007 | DMatrix J(7,5); |
6008 | //double cosl=cos(atan(tanl_)); |
6009 | double pt=1./fabs(q_over_pt_); |
6010 | //double p=pt/cosl; |
6011 | // double p_sq=p*p; |
6012 | // double E=sqrt(mass2+p_sq); |
6013 | double pt_sq=1./(q_over_pt_*q_over_pt_); |
6014 | double cosphi=cos(phi_); |
6015 | double sinphi=sin(phi_); |
6016 | double q=(q_over_pt_>0.0)?1.:-1.; |
6017 | |
6018 | J(state_Px,state_q_over_pt)=-q*pt_sq*cosphi; |
6019 | J(state_Px,state_phi)=-pt*sinphi; |
6020 | |
6021 | J(state_Py,state_q_over_pt)=-q*pt_sq*sinphi; |
6022 | J(state_Py,state_phi)=pt*cosphi; |
6023 | |
6024 | J(state_Pz,state_q_over_pt)=-q*pt_sq*tanl_; |
6025 | J(state_Pz,state_tanl)=pt; |
6026 | |
6027 | J(state_X,state_phi)=-D_*cosphi; |
6028 | J(state_X,state_D)=-sinphi; |
6029 | |
6030 | J(state_Y,state_phi)=-D_*sinphi; |
6031 | J(state_Y,state_D)=cosphi; |
6032 | |
6033 | J(state_Z,state_z)=1.; |
6034 | |
6035 | // C'= JCJ^T |
6036 | *C7x7=C.Similarity(J); |
6037 | // C7x7->Print(); |
6038 | // _DBG_ << " " << C7x7->operator()(3,3) << " " << C7x7->operator()(4,4) << endl; |
6039 | |
6040 | return C7x7; |
6041 | } |
6042 | |
6043 | // Track recovery for Central tracks |
6044 | //----------------------------------- |
6045 | // This code attempts to recover tracks that are "broken". Sometimes the fit fails because too many hits were pruned |
6046 | // such that the number of surviving hits is less than the minimum number of degrees of freedom for a five-parameter fit. |
6047 | // This condition is flagged as PRUNED_TOO_MANY_HITS. It may also be the case that even if we used enough hits for the fit to |
6048 | // be valid (i.e., make sense in terms of the number of degrees of freedom for the number of parameters (5) we are trying |
6049 | // to determine), we throw away a number of hits near the target because the projected trajectory deviates too far away from |
6050 | // the actual hits. This may be an indication of a kink in the trajectory. This condition is flagged as BREAK_POINT_FOUND. |
6051 | // Sometimes the fit may succeed and no break point is found, but the pruning threw out a large number of hits. This |
6052 | // condition is flagged as PRUNED_TOO_MANY_HITS. The recovery code proceeds by truncating the reference trajectory to |
6053 | // to a point just after the hit where a break point was found and all hits are ignored beyond this point subject to a |
6054 | // minimum number of hits set by MIN_HITS_FOR_REFIT. The recovery code always attempts to use the hits closest to the |
6055 | // target. The code is allowed to iterate; with each iteration the trajectory and list of useable hits is further truncated. |
6056 | kalman_error_t DTrackFitterKalmanSIMD::RecoverBrokenTracks(double anneal_factor, |
6057 | DMatrix5x1 &S, |
6058 | DMatrix5x5 &C, |
6059 | const DMatrix5x5 &C0, |
6060 | DVector2 &xy, |
6061 | double &chisq, |
6062 | unsigned int &numdof){ |
6063 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6063<<" " << "Attempting to recover broken track ... " <<endl; |
6064 | |
6065 | // Initialize degrees of freedom and chi^2 |
6066 | double refit_chisq=-1.; |
6067 | unsigned int refit_ndf=0; |
6068 | // State vector and covariance matrix |
6069 | DMatrix5x5 C1; |
6070 | DMatrix5x1 S1; |
6071 | // Position vector |
6072 | DVector2 refit_xy; |
6073 | |
6074 | // save the status of the hits used in the fit |
6075 | unsigned int num_hits=cdc_used_in_fit.size(); |
6076 | vector<bool>old_cdc_used_status(num_hits); |
6077 | for (unsigned int j=0;j<num_hits;j++){ |
6078 | old_cdc_used_status[j]=cdc_used_in_fit[j]; |
6079 | } |
6080 | |
6081 | // Truncate the reference trajectory to just beyond the break point (or the minimum number of hits) |
6082 | unsigned int min_cdc_index_for_refit=MIN_HITS_FOR_REFIT-1; |
6083 | //if (break_point_cdc_index<num_hits/2) |
6084 | // break_point_cdc_index=num_hits/2; |
6085 | if (break_point_cdc_index<min_cdc_index_for_refit){ |
6086 | break_point_cdc_index=min_cdc_index_for_refit; |
6087 | } |
6088 | // Next determine where we need to truncate the trajectory |
6089 | DVector2 origin=my_cdchits[break_point_cdc_index]->origin; |
6090 | DVector2 dir=my_cdchits[break_point_cdc_index]->dir; |
6091 | double z0=my_cdchits[break_point_cdc_index]->z0wire; |
6092 | unsigned int k=0; |
6093 | for (k=central_traj.size()-1;k>1;k--){ |
6094 | double r2=central_traj[k].xy.Mod2(); |
6095 | double r2next=central_traj[k-1].xy.Mod2(); |
6096 | double r2_cdc=(origin+(central_traj[k].S(state_z)-z0)*dir).Mod2(); |
6097 | if (r2next>r2 && r2>r2_cdc) break; |
6098 | } |
6099 | break_point_step_index=k; |
6100 | |
6101 | if (break_point_step_index==central_traj.size()-1){ |
6102 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6102<<" " << "Invalid reference trajectory in track recovery..." << endl; |
6103 | return FIT_FAILED; |
6104 | } |
6105 | |
6106 | kalman_error_t refit_error=FIT_NOT_DONE; |
6107 | unsigned int old_cdc_index=break_point_cdc_index; |
6108 | unsigned int old_step_index=break_point_step_index; |
6109 | unsigned int refit_iter=0; |
6110 | unsigned int num_cdchits=my_cdchits.size(); |
6111 | while (break_point_cdc_index>4 && break_point_step_index>0 |
6112 | && break_point_step_index<central_traj.size()){ |
6113 | refit_iter++; |
6114 | |
6115 | // Flag the cdc hits within the radius of the break point cdc index |
6116 | // as good, the rest as bad. |
6117 | for (unsigned int j=0;j<=break_point_cdc_index;j++){ |
6118 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6119 | } |
6120 | for (unsigned int j=break_point_cdc_index+1;j<num_cdchits;j++){ |
6121 | my_cdchits[j]->status=bad_hit; |
6122 | } |
6123 | |
6124 | // Now refit with the truncated trajectory and list of hits |
6125 | //C1=C0; |
6126 | //C1=4.0*C0; |
6127 | C1=1.0*C0; |
6128 | S1=central_traj[break_point_step_index].S; |
6129 | refit_chisq=-1.; |
6130 | refit_ndf=0; |
6131 | refit_error=KalmanCentral(anneal_factor,S1,C1,refit_xy, |
6132 | refit_chisq,refit_ndf); |
6133 | if (refit_error==FIT_SUCCEEDED |
6134 | || (refit_error==BREAK_POINT_FOUND |
6135 | && break_point_cdc_index==1 |
6136 | ) |
6137 | //|| refit_error==PRUNED_TOO_MANY_HITS |
6138 | ){ |
6139 | C=C1; |
6140 | S=S1; |
6141 | xy=refit_xy; |
6142 | chisq=refit_chisq; |
6143 | numdof=refit_ndf; |
6144 | |
6145 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6145<<" " << "Fit recovery succeeded..." << endl; |
6146 | return FIT_SUCCEEDED; |
6147 | } |
6148 | |
6149 | break_point_cdc_index=old_cdc_index-refit_iter; |
6150 | break_point_step_index=old_step_index-refit_iter; |
6151 | } |
6152 | |
6153 | // If the refit did not work, restore the old list hits used in the fit |
6154 | // before the track recovery was attempted. |
6155 | for (unsigned int k=0;k<num_hits;k++){ |
6156 | cdc_used_in_fit[k]=old_cdc_used_status[k]; |
6157 | } |
6158 | |
6159 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6159<<" " << "Fit recovery failed..." << endl; |
6160 | return FIT_FAILED; |
6161 | } |
6162 | |
6163 | // Track recovery for forward tracks |
6164 | //----------------------------------- |
6165 | // This code attempts to recover tracks that are "broken". Sometimes the fit fails because too many hits were pruned |
6166 | // such that the number of surviving hits is less than the minimum number of degrees of freedom for a five-parameter fit. |
6167 | // This condition is flagged as PRUNED_TOO_MANY_HITS. It may also be the case that even if we used enough hits for the fit to |
6168 | // be valid (i.e., make sense in terms of the number of degrees of freedom for the number of parameters (5) we are trying |
6169 | // to determine), we throw away a number of hits near the target because the projected trajectory deviates too far away from |
6170 | // the actual hits. This may be an indication of a kink in the trajectory. This condition is flagged as BREAK_POINT_FOUND. |
6171 | // Sometimes the fit may succeed and no break point is found, but the pruning threw out a large number of hits. This |
6172 | // condition is flagged as PRUNED_TOO_MANY_HITS. The recovery code proceeds by truncating the reference trajectory to |
6173 | // to a point just after the hit where a break point was found and all hits are ignored beyond this point subject to a |
6174 | // minimum number of hits. The recovery code always attempts to use the hits closest to the target. The code is allowed to |
6175 | // iterate; with each iteration the trajectory and list of useable hits is further truncated. |
6176 | kalman_error_t DTrackFitterKalmanSIMD::RecoverBrokenTracks(double anneal_factor, |
6177 | DMatrix5x1 &S, |
6178 | DMatrix5x5 &C, |
6179 | const DMatrix5x5 &C0, |
6180 | double &chisq, |
6181 | unsigned int &numdof |
6182 | ){ |
6183 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6183<<" " << "Attempting to recover broken track ... " <<endl; |
6184 | |
6185 | unsigned int num_cdchits=my_cdchits.size(); |
6186 | unsigned int num_fdchits=my_fdchits.size(); |
6187 | |
6188 | // Initialize degrees of freedom and chi^2 |
6189 | double refit_chisq=-1.; |
6190 | unsigned int refit_ndf=0; |
6191 | // State vector and covariance matrix |
6192 | DMatrix5x5 C1; |
6193 | DMatrix5x1 S1; |
6194 | |
6195 | // save the status of the hits used in the fit |
6196 | vector<bool>old_cdc_used_status(num_cdchits); |
6197 | vector<bool>old_fdc_used_status(num_fdchits); |
6198 | for (unsigned int j=0;j<num_fdchits;j++){ |
6199 | old_fdc_used_status[j]=fdc_used_in_fit[j]; |
6200 | } |
6201 | for (unsigned int j=0;j<num_cdchits;j++){ |
6202 | old_cdc_used_status[j]=cdc_used_in_fit[j]; |
6203 | } |
6204 | |
6205 | unsigned int min_cdc_index=MIN_HITS_FOR_REFIT-1; |
6206 | if (my_fdchits.size()>0){ |
6207 | if (break_point_cdc_index<5){ |
6208 | break_point_cdc_index=0; |
6209 | min_cdc_index=5; |
6210 | } |
6211 | } |
6212 | /*else{ |
6213 | unsigned int half_num_cdchits=num_cdchits/2; |
6214 | if (break_point_cdc_index<half_num_cdchits |
6215 | && half_num_cdchits>min_cdc_index) |
6216 | break_point_cdc_index=half_num_cdchits; |
6217 | } |
6218 | */ |
6219 | if (break_point_cdc_index<min_cdc_index){ |
6220 | break_point_cdc_index=min_cdc_index; |
6221 | } |
6222 | |
6223 | // Find the index at which to truncate the reference trajectory |
6224 | DVector2 origin=my_cdchits[break_point_cdc_index]->origin; |
6225 | DVector2 dir=my_cdchits[break_point_cdc_index]->dir; |
6226 | double z0=my_cdchits[break_point_cdc_index]->z0wire; |
6227 | unsigned int k=forward_traj.size()-1; |
6228 | for (;k>1;k--){ |
6229 | DMatrix5x1 S1=forward_traj[k].S; |
6230 | double x1=S1(state_x); |
6231 | double y1=S1(state_y); |
6232 | double r2=x1*x1+y1*y1; |
6233 | DMatrix5x1 S2=forward_traj[k-1].S; |
6234 | double x2=S2(state_x); |
6235 | double y2=S2(state_y); |
6236 | double r2next=x2*x2+y2*y2; |
6237 | double r2cdc=(origin+(forward_traj[k].z-z0)*dir).Mod2(); |
6238 | |
6239 | if (r2next>r2 && r2>r2cdc) break; |
6240 | } |
6241 | break_point_step_index=k; |
6242 | |
6243 | if (break_point_step_index==forward_traj.size()-1){ |
6244 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6244<<" " << "Invalid reference trajectory in track recovery..." << endl; |
6245 | return FIT_FAILED; |
6246 | } |
6247 | |
6248 | // Attemp to refit the track using the abreviated list of hits and the truncated |
6249 | // reference trajectory. Iterates if the fit fails. |
6250 | kalman_error_t refit_error=FIT_NOT_DONE; |
6251 | unsigned int old_cdc_index=break_point_cdc_index; |
6252 | unsigned int old_step_index=break_point_step_index; |
6253 | unsigned int refit_iter=0; |
6254 | while (break_point_cdc_index>4 && break_point_step_index>0 |
6255 | && break_point_step_index<forward_traj.size()){ |
6256 | refit_iter++; |
6257 | |
6258 | // Flag the cdc hits within the radius of the break point cdc index |
6259 | // as good, the rest as bad. |
6260 | for (unsigned int j=0;j<=break_point_cdc_index;j++){ |
6261 | if (my_cdchits[j]->status!=late_hit) my_cdchits[j]->status=good_hit; |
6262 | } |
6263 | for (unsigned int j=break_point_cdc_index+1;j<num_cdchits;j++){ |
6264 | my_cdchits[j]->status=bad_hit; |
6265 | } |
6266 | |
6267 | // Re-initialize the state vector, covariance, chisq and number of degrees of freedom |
6268 | //C1=4.0*C0; |
6269 | C1=1.0*C0; |
6270 | S1=forward_traj[break_point_step_index].S; |
6271 | refit_chisq=-1.; |
6272 | refit_ndf=0; |
6273 | // Now refit with the truncated trajectory and list of hits |
6274 | refit_error=KalmanForwardCDC(anneal_factor,S1,C1,refit_chisq,refit_ndf); |
6275 | if (refit_error==FIT_SUCCEEDED |
6276 | || (refit_error==BREAK_POINT_FOUND |
6277 | && break_point_cdc_index==1 |
6278 | ) |
6279 | //|| refit_error==PRUNED_TOO_MANY_HITS |
6280 | ){ |
6281 | C=C1; |
6282 | S=S1; |
6283 | chisq=refit_chisq; |
6284 | numdof=refit_ndf; |
6285 | return FIT_SUCCEEDED; |
6286 | } |
6287 | break_point_cdc_index=old_cdc_index-refit_iter; |
6288 | break_point_step_index=old_step_index-refit_iter; |
6289 | } |
6290 | // If the refit did not work, restore the old list hits used in the fit |
6291 | // before the track recovery was attempted. |
6292 | for (unsigned int k=0;k<num_cdchits;k++){ |
6293 | cdc_used_in_fit[k]=old_cdc_used_status[k]; |
6294 | } |
6295 | for (unsigned int k=0;k<num_fdchits;k++){ |
6296 | fdc_used_in_fit[k]=old_fdc_used_status[k]; |
6297 | } |
6298 | |
6299 | return FIT_FAILED; |
6300 | } |
6301 | |
6302 | |
6303 | // Track recovery for forward-going tracks with hits in the FDC |
6304 | kalman_error_t |
6305 | DTrackFitterKalmanSIMD::RecoverBrokenForwardTracks(double fdc_anneal, |
6306 | double cdc_anneal, |
6307 | DMatrix5x1 &S, |
6308 | DMatrix5x5 &C, |
6309 | const DMatrix5x5 &C0, |
6310 | double &chisq, |
6311 | unsigned int &numdof){ |
6312 | if (DEBUG_LEVEL>1) |
6313 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6313<<" " << "Attempting to recover broken track ... " <<endl; |
6314 | unsigned int num_cdchits=my_cdchits.size(); |
6315 | unsigned int num_fdchits=fdc_updates.size(); |
6316 | |
6317 | // Initialize degrees of freedom and chi^2 |
6318 | double refit_chisq=-1.; |
6319 | unsigned int refit_ndf=0; |
6320 | // State vector and covariance matrix |
6321 | DMatrix5x5 C1; |
6322 | DMatrix5x1 S1; |
6323 | |
6324 | // save the status of the hits used in the fit |
6325 | vector<int>old_cdc_used_status(num_cdchits); |
6326 | vector<int>old_fdc_used_status(num_fdchits); |
6327 | for (unsigned int j=0;j<num_fdchits;j++){ |
6328 | old_fdc_used_status[j]=fdc_used_in_fit[j]; |
6329 | } |
6330 | for (unsigned int j=0;j<num_cdchits;j++){ |
6331 | old_cdc_used_status[j]=cdc_used_in_fit[j]; |
6332 | } |
6333 | |
6334 | // Truncate the trajectory |
6335 | double zhit=my_fdchits[break_point_fdc_index]->z; |
6336 | unsigned int k=0; |
6337 | for (;k<forward_traj.size();k++){ |
6338 | double z=forward_traj[k].z; |
6339 | if (z<zhit) break; |
6340 | } |
6341 | for (unsigned int j=0;j<=break_point_fdc_index;j++){ |
6342 | my_fdchits[j]->status=good_hit; |
6343 | } |
6344 | for (unsigned int j=break_point_fdc_index+1;j<num_fdchits;j++){ |
6345 | my_fdchits[j]->status=bad_hit; |
6346 | } |
6347 | |
6348 | if (k==forward_traj.size()) return FIT_NOT_DONE; |
6349 | |
6350 | break_point_step_index=k; |
6351 | |
6352 | // Attemp to refit the track using the abreviated list of hits and the truncated |
6353 | // reference trajectory. |
6354 | kalman_error_t refit_error=FIT_NOT_DONE; |
6355 | int refit_iter=0; |
6356 | unsigned int break_id=break_point_fdc_index; |
6357 | while (break_id+num_cdchits>=MIN_HITS_FOR_REFIT && break_id>0 |
6358 | && break_point_step_index<forward_traj.size() |
6359 | && break_point_step_index>1 |
6360 | && refit_iter<10){ |
6361 | refit_iter++; |
6362 | |
6363 | // Mark the hits as bad if they are not included |
6364 | if (break_id >= 0){ |
6365 | for (unsigned int j=0;j<num_cdchits;j++){ |
6366 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6367 | } |
6368 | for (unsigned int j=0;j<=break_id;j++){ |
6369 | my_fdchits[j]->status=good_hit; |
6370 | } |
6371 | for (unsigned int j=break_id+1;j<num_fdchits;j++){ |
6372 | my_fdchits[j]->status=bad_hit; |
6373 | } |
6374 | } |
6375 | else{ |
6376 | // BreakID should always be 0 or positive, so this should never happen, but could be investigated in the future. |
6377 | for (unsigned int j=0;j<num_cdchits+break_id;j++){ |
6378 | if (my_cdchits[j]->status!=late_hit) my_cdchits[j]->status=good_hit; |
6379 | } |
6380 | for (unsigned int j=num_cdchits+break_id;j<num_cdchits;j++){ |
6381 | my_cdchits[j]->status=bad_hit; |
6382 | } |
6383 | for (unsigned int j=0;j<num_fdchits;j++){ |
6384 | my_fdchits[j]->status=bad_hit; |
6385 | } |
6386 | } |
6387 | |
6388 | // Re-initialize the state vector, covariance, chisq and number of degrees of freedom |
6389 | //C1=4.0*C0; |
6390 | C1=1.0*C0; |
6391 | S1=forward_traj[break_point_step_index].S; |
6392 | refit_chisq=-1.; |
6393 | refit_ndf=0; |
6394 | |
6395 | // Now refit with the truncated trajectory and list of hits |
6396 | refit_error=KalmanForward(fdc_anneal,cdc_anneal,S1,C1,refit_chisq,refit_ndf); |
6397 | if (refit_error==FIT_SUCCEEDED |
6398 | //|| (refit_error==PRUNED_TOO_MANY_HITS) |
6399 | ){ |
6400 | C=C1; |
6401 | S=S1; |
6402 | chisq=refit_chisq; |
6403 | numdof=refit_ndf; |
6404 | |
6405 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6405<<" " << "Refit succeeded" << endl; |
6406 | return FIT_SUCCEEDED; |
6407 | } |
6408 | // Truncate the trajectory |
6409 | if (break_id>0) break_id--; |
6410 | else break; |
6411 | zhit=my_fdchits[break_id]->z; |
6412 | k=0; |
6413 | for (;k<forward_traj.size();k++){ |
6414 | double z=forward_traj[k].z; |
6415 | if (z<zhit) break; |
6416 | } |
6417 | break_point_step_index=k; |
6418 | |
6419 | } |
6420 | |
6421 | // If the refit did not work, restore the old list hits used in the fit |
6422 | // before the track recovery was attempted. |
6423 | for (unsigned int k=0;k<num_cdchits;k++){ |
6424 | cdc_used_in_fit[k]=old_cdc_used_status[k]; |
6425 | } |
6426 | for (unsigned int k=0;k<num_fdchits;k++){ |
6427 | fdc_used_in_fit[k]=old_fdc_used_status[k]; |
6428 | } |
6429 | |
6430 | return FIT_FAILED; |
6431 | } |
6432 | |
6433 | |
6434 | |
6435 | // Routine to fit hits in the FDC and the CDC using the forward parametrization |
6436 | kalman_error_t DTrackFitterKalmanSIMD::ForwardFit(const DMatrix5x1 &S0,const DMatrix5x5 &C0){ |
6437 | unsigned int num_cdchits=my_cdchits.size(); |
6438 | unsigned int num_fdchits=my_fdchits.size(); |
6439 | unsigned int max_fdc_index=num_fdchits-1; |
6440 | |
6441 | // Vectors to keep track of updated state vectors and covariance matrices (after |
6442 | // adding the hit information) |
6443 | vector<bool>last_fdc_used_in_fit(num_fdchits); |
6444 | vector<bool>last_cdc_used_in_fit(num_cdchits); |
6445 | vector<pull_t>forward_pulls; |
6446 | vector<pull_t>last_forward_pulls; |
6447 | |
6448 | // Charge |
6449 | // double q=input_params.charge(); |
6450 | |
6451 | // Covariance matrix and state vector |
6452 | DMatrix5x5 C; |
6453 | DMatrix5x1 S=S0; |
6454 | |
6455 | // Create matrices to store results from previous iteration |
6456 | DMatrix5x1 Slast(S); |
6457 | DMatrix5x5 Clast(C0); |
6458 | // last z position |
6459 | double last_z=z_; |
6460 | |
6461 | double fdc_anneal=FORWARD_ANNEAL_SCALE+1.; // variable for scaling cut for hit pruning |
6462 | double cdc_anneal=ANNEAL_SCALE+1.; // variable for scaling cut for hit pruning |
6463 | |
6464 | // Chi-squared and degrees of freedom |
6465 | double chisq=-1.,chisq_forward=-1.; |
6466 | unsigned int my_ndf=0; |
6467 | unsigned int last_ndf=1; |
6468 | kalman_error_t error=FIT_NOT_DONE,last_error=FIT_NOT_DONE; |
6469 | |
6470 | // Iterate over reference trajectories |
6471 | for (int iter=0; |
6472 | iter<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20); |
6473 | iter++) { |
6474 | // These variables provide the approximate location along the trajectory |
6475 | // where there is an indication of a kink in the track |
6476 | break_point_fdc_index=max_fdc_index; |
6477 | break_point_cdc_index=0; |
6478 | break_point_step_index=0; |
6479 | |
6480 | // Reset material map index |
6481 | last_material_map=0; |
6482 | |
6483 | // Abort if momentum is too low |
6484 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX) break; |
6485 | |
6486 | // Initialize path length variable and flight time |
6487 | len=0; |
6488 | ftime=0.; |
6489 | var_ftime=0.; |
6490 | |
6491 | // Scale cut for pruning hits according to the iteration number |
6492 | fdc_anneal=(iter<MIN_ITER3)?(FORWARD_ANNEAL_SCALE/pow(FORWARD_ANNEAL_POW_CONST,iter)+1.):1.; |
6493 | cdc_anneal=(iter<MIN_ITER3)?(ANNEAL_SCALE/pow(ANNEAL_POW_CONST,iter)+1.):1.; |
6494 | |
6495 | // Swim through the field out to the most upstream FDC hit |
6496 | jerror_t ref_track_error=SetReferenceTrajectory(S); |
6497 | if (ref_track_error!=NOERROR){ |
6498 | if (iter==0) return FIT_FAILED; |
6499 | break; |
6500 | } |
6501 | |
6502 | // Reset the status of the cdc hits |
6503 | for (unsigned int j=0;j<num_cdchits;j++){ |
6504 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6505 | } |
6506 | |
6507 | // perform the kalman filter |
6508 | C=C0; |
6509 | bool did_second_refit=false; |
6510 | error=KalmanForward(fdc_anneal,cdc_anneal,S,C,chisq,my_ndf); |
6511 | if (DEBUG_LEVEL>1){ |
6512 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6512<<" " << "Iter: " << iter+1 << " Chi2=" << chisq << " Ndf=" << my_ndf << " Error code: " << error << endl; |
6513 | } |
6514 | // Try to recover tracks that failed the first attempt at fitting by |
6515 | // cutting outer hits |
6516 | if (error!=FIT_SUCCEEDED && RECOVER_BROKEN_TRACKS |
6517 | && num_fdchits>2 // some minimum to make this worthwhile... |
6518 | && break_point_fdc_index<num_fdchits |
6519 | && break_point_fdc_index+num_cdchits>=MIN_HITS_FOR_REFIT |
6520 | && forward_traj.size()>2*MIN_HITS_FOR_REFIT // avoid small track stubs |
6521 | ){ |
6522 | DMatrix5x5 Ctemp=C; |
6523 | DMatrix5x1 Stemp=S; |
6524 | unsigned int temp_ndf=my_ndf; |
6525 | double temp_chi2=chisq; |
6526 | double x=x_,y=y_,z=z_; |
6527 | |
6528 | kalman_error_t refit_error=RecoverBrokenForwardTracks(fdc_anneal, |
6529 | cdc_anneal, |
6530 | S,C,C0,chisq, |
6531 | my_ndf); |
6532 | if (fit_type==kTimeBased && refit_error!=FIT_SUCCEEDED){ |
6533 | fdc_anneal=1000.; |
6534 | cdc_anneal=1000.; |
6535 | refit_error=RecoverBrokenForwardTracks(fdc_anneal, |
6536 | cdc_anneal, |
6537 | S,C,C0,chisq, |
6538 | my_ndf); |
6539 | //chisq=1e6; |
6540 | did_second_refit=true; |
6541 | } |
6542 | if (refit_error!=FIT_SUCCEEDED){ |
6543 | if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){ |
6544 | C=Ctemp; |
6545 | S=Stemp; |
6546 | my_ndf=temp_ndf; |
6547 | chisq=temp_chi2; |
6548 | x_=x,y_=y,z_=z; |
6549 | |
6550 | if (num_cdchits<6) error=FIT_SUCCEEDED; |
6551 | } |
6552 | else error=FIT_FAILED; |
6553 | } |
6554 | else error=FIT_SUCCEEDED; |
6555 | } |
6556 | if ((error==POSITION_OUT_OF_RANGE || error==MOMENTUM_OUT_OF_RANGE) |
6557 | && iter==0){ |
6558 | return FIT_FAILED; |
6559 | } |
6560 | if (error==FIT_FAILED || error==INVALID_FIT || my_ndf==0){ |
6561 | if (iter==0) return FIT_FAILED; // first iteration failed |
6562 | break; |
6563 | } |
6564 | |
6565 | if (iter>MIN_ITER3 && did_second_refit==false){ |
6566 | double new_reduced_chisq=chisq/my_ndf; |
6567 | double old_reduced_chisq=chisq_forward/last_ndf; |
6568 | double new_prob=TMath::Prob(chisq,my_ndf); |
6569 | double old_prob=TMath::Prob(chisq_forward,last_ndf); |
6570 | if (new_prob<old_prob |
6571 | || fabs(new_reduced_chisq-old_reduced_chisq)<CHISQ_DELTA0.01){ |
6572 | break; |
6573 | } |
6574 | } |
6575 | |
6576 | chisq_forward=chisq; |
6577 | last_ndf=my_ndf; |
6578 | last_error=error; |
6579 | Slast=S; |
6580 | Clast=C; |
6581 | last_z=z_; |
6582 | |
6583 | last_fdc_used_in_fit=fdc_used_in_fit; |
6584 | last_cdc_used_in_fit=cdc_used_in_fit; |
6585 | } //iteration |
6586 | |
6587 | IsSmoothed=false; |
6588 | if(fit_type==kTimeBased){ |
6589 | forward_pulls.clear(); |
6590 | if (SmoothForward(forward_pulls) == NOERROR){ |
6591 | IsSmoothed = true; |
6592 | pulls.assign(forward_pulls.begin(),forward_pulls.end()); |
6593 | } |
6594 | } |
6595 | |
6596 | // total chisq and ndf |
6597 | chisq_=chisq_forward; |
6598 | ndf_=last_ndf; |
6599 | |
6600 | // output lists of hits used in the fit and fill pull vector |
6601 | cdchits_used_in_fit.clear(); |
6602 | for (unsigned int m=0;m<last_cdc_used_in_fit.size();m++){ |
6603 | if (last_cdc_used_in_fit[m]){ |
6604 | cdchits_used_in_fit.push_back(my_cdchits[m]->hit); |
6605 | } |
6606 | } |
6607 | fdchits_used_in_fit.clear(); |
6608 | for (unsigned int m=0;m<last_fdc_used_in_fit.size();m++){ |
6609 | if (last_fdc_used_in_fit[m] && my_fdchits[m]->hit!=NULL__null){ |
6610 | fdchits_used_in_fit.push_back(my_fdchits[m]->hit); |
6611 | } |
6612 | } |
6613 | // fill pull vector |
6614 | //pulls.assign(last_forward_pulls.begin(),last_forward_pulls.end()); |
6615 | |
6616 | // fill vector of extrapolations |
6617 | ClearExtrapolations(); |
6618 | if (forward_traj.size()>1){ |
6619 | ExtrapolateToInnerDetectors(); |
6620 | if (fit_type==kTimeBased){ |
6621 | double reverse_chisq=1e16,reverse_chisq_old=1e16; |
6622 | unsigned int reverse_ndf=0,reverse_ndf_old=0; |
6623 | |
6624 | // Run the Kalman filter in the reverse direction, to get the best guess |
6625 | // for the state vector at the last FDC point on the track |
6626 | DMatrix5x5 CReverse=C; |
6627 | DMatrix5x1 SReverse=S,SDownstream,SBest; |
6628 | kalman_error_t reverse_error=FIT_NOT_DONE; |
6629 | for (int iter=0;iter<20;iter++){ |
6630 | reverse_chisq_old=reverse_chisq; |
6631 | reverse_ndf_old=reverse_ndf; |
6632 | SBest=SDownstream; |
6633 | reverse_error=KalmanReverse(fdc_anneal,cdc_anneal,SReverse,CReverse, |
6634 | SDownstream,reverse_chisq,reverse_ndf); |
6635 | if (reverse_error!=FIT_SUCCEEDED) break; |
6636 | |
6637 | SReverse=SDownstream; |
6638 | for (unsigned int k=0;k<forward_traj.size()-1;k++){ |
6639 | // Get dEdx for the upcoming step |
6640 | double dEdx=0.; |
6641 | if (CORRECT_FOR_ELOSS){ |
6642 | dEdx=GetdEdx(SReverse(state_q_over_p), |
6643 | forward_traj[k].K_rho_Z_over_A, |
6644 | forward_traj[k].rho_Z_over_A, |
6645 | forward_traj[k].LnI,forward_traj[k].Z); |
6646 | } |
6647 | // Step through field |
6648 | DMatrix5x5 J; |
6649 | double z=forward_traj[k].z; |
6650 | double newz=forward_traj[k+1].z; |
6651 | StepJacobian(z,newz,SReverse,dEdx,J); |
6652 | Step(z,newz,dEdx,SReverse); |
6653 | |
6654 | CReverse=forward_traj[k].Q.AddSym(J*CReverse*J.Transpose()); |
6655 | } |
6656 | |
6657 | double reduced_chisq=reverse_chisq/double(reverse_ndf); |
6658 | double reduced_chisq_old=reverse_chisq_old/double(reverse_ndf_old); |
6659 | if (reduced_chisq>reduced_chisq_old |
6660 | || fabs(reduced_chisq-reduced_chisq_old)<0.01) break; |
6661 | } |
6662 | |
6663 | if (reverse_error!=FIT_SUCCEEDED){ |
6664 | ExtrapolateToOuterDetectors(forward_traj[0].S); |
6665 | } |
6666 | else{ |
6667 | ExtrapolateToOuterDetectors(SBest); |
6668 | } |
6669 | } |
6670 | else{ |
6671 | ExtrapolateToOuterDetectors(forward_traj[0].S); |
6672 | } |
6673 | if (extrapolations.at(SYS_BCAL).size()==1){ |
6674 | // There needs to be some steps inside the the volume of the BCAL for |
6675 | // the extrapolation to be useful. If this is not the case, clear |
6676 | // the extrolation vector. |
6677 | extrapolations[SYS_BCAL].clear(); |
6678 | } |
6679 | } |
6680 | // Extrapolate to the point of closest approach to the beam line |
6681 | z_=last_z; |
6682 | DVector2 beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
6683 | double dx=Slast(state_x)-beam_pos.X(); |
6684 | double dy=Slast(state_y)-beam_pos.Y(); |
6685 | bool extrapolated=false; |
6686 | if (sqrt(dx*dx+dy*dy)>EPS21.e-4){ |
6687 | DMatrix5x5 Ctemp=Clast; |
6688 | DMatrix5x1 Stemp=Slast; |
6689 | double ztemp=z_; |
6690 | if (ExtrapolateToVertex(Stemp,Ctemp)==NOERROR){ |
6691 | Clast=Ctemp; |
6692 | Slast=Stemp; |
6693 | extrapolated=true; |
6694 | } |
6695 | else{ |
6696 | //_DBG_ << endl; |
6697 | z_=ztemp; |
6698 | } |
6699 | } |
6700 | |
6701 | // Final momentum, positions and tangents |
6702 | x_=Slast(state_x), y_=Slast(state_y); |
6703 | tx_=Slast(state_tx),ty_=Slast(state_ty); |
6704 | q_over_p_=Slast(state_q_over_p); |
6705 | |
6706 | // Convert from forward rep. to central rep. |
6707 | double tsquare=tx_*tx_+ty_*ty_; |
6708 | tanl_=1./sqrt(tsquare); |
6709 | double cosl=cos(atan(tanl_)); |
6710 | q_over_pt_=q_over_p_/cosl; |
6711 | phi_=atan2(ty_,tx_); |
6712 | if (FORWARD_PARMS_COV==false){ |
6713 | if (extrapolated){ |
6714 | beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
6715 | dx=x_-beam_pos.X(); |
6716 | dy=y_-beam_pos.Y(); |
6717 | } |
6718 | D_=sqrt(dx*dx+dy*dy)+EPS3.0e-8; |
6719 | x_ = dx; y_ = dy; |
6720 | double cosphi=cos(phi_); |
6721 | double sinphi=sin(phi_); |
6722 | if ((dx>0.0 && sinphi>0.0) || (dy<0.0 && cosphi>0.0) |
6723 | || (dy>0.0 && cosphi<0.0) || (dx<0.0 && sinphi<0.0)) D_*=-1.; |
6724 | TransformCovariance(Clast); |
6725 | } |
6726 | // Covariance matrix |
6727 | vector<double>dummy; |
6728 | for (unsigned int i=0;i<5;i++){ |
6729 | dummy.clear(); |
6730 | for(unsigned int j=0;j<5;j++){ |
6731 | dummy.push_back(Clast(i,j)); |
6732 | } |
6733 | fcov.push_back(dummy); |
6734 | } |
6735 | |
6736 | return last_error; |
6737 | } |
6738 | |
6739 | // Routine to fit hits in the CDC using the forward parametrization |
6740 | kalman_error_t DTrackFitterKalmanSIMD::ForwardCDCFit(const DMatrix5x1 &S0,const DMatrix5x5 &C0){ |
6741 | unsigned int num_cdchits=my_cdchits.size(); |
6742 | unsigned int max_cdc_index=num_cdchits-1; |
6743 | unsigned int min_cdc_index_for_refit=MIN_HITS_FOR_REFIT-1; |
6744 | |
6745 | // Charge |
6746 | // double q=input_params.charge(); |
6747 | |
6748 | // Covariance matrix and state vector |
6749 | DMatrix5x5 C; |
6750 | DMatrix5x1 S=S0; |
6751 | |
6752 | // Create matrices to store results from previous iteration |
6753 | DMatrix5x1 Slast(S); |
6754 | DMatrix5x5 Clast(C0); |
6755 | |
6756 | // Vectors to keep track of updated state vectors and covariance matrices (after |
6757 | // adding the hit information) |
6758 | vector<pull_t>cdc_pulls; |
6759 | vector<pull_t>last_cdc_pulls; |
6760 | vector<bool>last_cdc_used_in_fit; |
6761 | |
6762 | double anneal_factor=ANNEAL_SCALE+1.; |
6763 | kalman_error_t error=FIT_NOT_DONE,last_error=FIT_NOT_DONE; |
6764 | |
6765 | // Chi-squared and degrees of freedom |
6766 | double chisq=-1.,chisq_forward=-1.; |
6767 | unsigned int my_ndf=0; |
6768 | unsigned int last_ndf=1; |
6769 | // last z position |
6770 | double zlast=0.; |
6771 | // unsigned int last_break_point_index=0,last_break_point_step_index=0; |
6772 | |
6773 | // Iterate over reference trajectories |
6774 | for (int iter2=0; |
6775 | iter2<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20); |
6776 | iter2++){ |
6777 | if (DEBUG_LEVEL>1){ |
6778 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6778<<" " <<"-------- iteration " << iter2+1 << " -----------" <<endl; |
6779 | } |
6780 | |
6781 | // These variables provide the approximate location along the trajectory |
6782 | // where there is an indication of a kink in the track |
6783 | break_point_cdc_index=max_cdc_index; |
6784 | break_point_step_index=0; |
6785 | |
6786 | // Reset material map index |
6787 | last_material_map=0; |
6788 | |
6789 | // Abort if momentum is too low |
6790 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
6791 | //printf("Too low momentum? %f\n",1/S(state_q_over_p)); |
6792 | break; |
6793 | } |
6794 | |
6795 | // Scale cut for pruning hits according to the iteration number |
6796 | anneal_factor=(iter2<MIN_ITER3)?(ANNEAL_SCALE/pow(ANNEAL_POW_CONST,iter2)+1.):1.; |
6797 | |
6798 | // Initialize path length variable and flight time |
6799 | len=0; |
6800 | ftime=0.; |
6801 | var_ftime=0.; |
6802 | |
6803 | // Swim to create the reference trajectory |
6804 | jerror_t ref_track_error=SetCDCForwardReferenceTrajectory(S); |
6805 | if (ref_track_error!=NOERROR){ |
6806 | if (iter2==0) return FIT_FAILED; |
6807 | break; |
6808 | } |
6809 | |
6810 | // Reset the status of the cdc hits |
6811 | for (unsigned int j=0;j<num_cdchits;j++){ |
6812 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6813 | } |
6814 | |
6815 | // perform the filter |
6816 | C=C0; |
6817 | bool did_second_refit=false; |
6818 | error=KalmanForwardCDC(anneal_factor,S,C,chisq,my_ndf); |
6819 | |
6820 | // Try to recover tracks that failed the first attempt at fitting by |
6821 | // cutting outer hits |
6822 | if (error!=FIT_SUCCEEDED && RECOVER_BROKEN_TRACKS |
6823 | && num_cdchits>=MIN_HITS_FOR_REFIT){ |
6824 | DMatrix5x5 Ctemp=C; |
6825 | DMatrix5x1 Stemp=S; |
6826 | unsigned int temp_ndf=my_ndf; |
6827 | double temp_chi2=chisq; |
6828 | double x=x_,y=y_,z=z_; |
6829 | |
6830 | if (error==MOMENTUM_OUT_OF_RANGE){ |
6831 | //_DBG_ << "Momentum out of range" <<endl; |
6832 | unsigned int new_index=(3*max_cdc_index)/4; |
6833 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
6834 | } |
6835 | |
6836 | if (error==BROKEN_COVARIANCE_MATRIX){ |
6837 | break_point_cdc_index=min_cdc_index_for_refit; |
6838 | //_DBG_ << "Bad Cov" <<endl; |
6839 | } |
6840 | if (error==POSITION_OUT_OF_RANGE){ |
6841 | //_DBG_ << "Bad position" << endl; |
6842 | unsigned int new_index=(max_cdc_index)/2; |
6843 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
6844 | } |
6845 | if (error==PRUNED_TOO_MANY_HITS){ |
6846 | // _DBG_ << "Prune" << endl; |
6847 | unsigned int new_index=(3*max_cdc_index)/4; |
6848 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
6849 | // anneal_factor*=10.; |
6850 | } |
6851 | |
6852 | kalman_error_t refit_error=RecoverBrokenTracks(anneal_factor,S,C,C0,chisq,my_ndf); |
6853 | if (fit_type==kTimeBased && refit_error!=FIT_SUCCEEDED){ |
6854 | anneal_factor=1000.; |
6855 | refit_error=RecoverBrokenTracks(anneal_factor,S,C,C0,chisq,my_ndf); |
6856 | //chisq=1e6; |
6857 | did_second_refit=true; |
6858 | } |
6859 | |
6860 | if (refit_error!=FIT_SUCCEEDED){ |
6861 | if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){ |
6862 | C=Ctemp; |
6863 | S=Stemp; |
6864 | my_ndf=temp_ndf; |
6865 | chisq=temp_chi2; |
6866 | x_=x,y_=y,z_=z; |
6867 | |
6868 | // error=FIT_SUCCEEDED; |
6869 | } |
6870 | else error=FIT_FAILED; |
6871 | } |
6872 | else error=FIT_SUCCEEDED; |
6873 | } |
6874 | if ((error==POSITION_OUT_OF_RANGE || error==MOMENTUM_OUT_OF_RANGE) |
6875 | && iter2==0){ |
6876 | return FIT_FAILED; |
6877 | } |
6878 | if (error==FIT_FAILED || error==INVALID_FIT || my_ndf==0){ |
6879 | if (iter2==0) return error; |
6880 | break; |
6881 | } |
6882 | |
6883 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6883<<" " << "--> Chisq " << chisq << " NDF " |
6884 | << my_ndf |
6885 | << " Prob: " << TMath::Prob(chisq,my_ndf) |
6886 | << endl; |
6887 | |
6888 | if (iter2>MIN_ITER3 && did_second_refit==false){ |
6889 | double new_reduced_chisq=chisq/my_ndf; |
6890 | double old_reduced_chisq=chisq_forward/last_ndf; |
6891 | double new_prob=TMath::Prob(chisq,my_ndf); |
6892 | double old_prob=TMath::Prob(chisq_forward,last_ndf); |
6893 | if (new_prob<old_prob |
6894 | || fabs(new_reduced_chisq-old_reduced_chisq)<CHISQ_DELTA0.01) break; |
6895 | } |
6896 | |
6897 | chisq_forward=chisq; |
6898 | Slast=S; |
6899 | Clast=C; |
6900 | last_error=error; |
6901 | last_ndf=my_ndf; |
6902 | zlast=z_; |
6903 | |
6904 | last_cdc_used_in_fit=cdc_used_in_fit; |
6905 | } //iteration |
6906 | |
6907 | // Run the smoother |
6908 | IsSmoothed=false; |
6909 | if(fit_type==kTimeBased){ |
6910 | cdc_pulls.clear(); |
6911 | if (SmoothForwardCDC(cdc_pulls) == NOERROR){ |
6912 | IsSmoothed = true; |
6913 | pulls.assign(cdc_pulls.begin(),cdc_pulls.end()); |
6914 | } |
6915 | } |
6916 | |
6917 | // total chisq and ndf |
6918 | chisq_=chisq_forward; |
6919 | ndf_=last_ndf; |
6920 | |
6921 | // output lists of hits used in the fit and fill the pull vector |
6922 | cdchits_used_in_fit.clear(); |
6923 | for (unsigned int m=0;m<last_cdc_used_in_fit.size();m++){ |
6924 | if (last_cdc_used_in_fit[m]){ |
6925 | cdchits_used_in_fit.push_back(my_cdchits[m]->hit); |
6926 | } |
6927 | } |
6928 | // output pulls vector |
6929 | //pulls.assign(last_cdc_pulls.begin(),last_cdc_pulls.end()); |
6930 | |
6931 | // Fill extrapolation vector |
6932 | ClearExtrapolations(); |
6933 | if (forward_traj.size()>1){ |
6934 | if (fit_type==kWireBased){ |
6935 | ExtrapolateForwardToOtherDetectors(); |
6936 | } |
6937 | else{ |
6938 | ExtrapolateToInnerDetectors(); |
6939 | |
6940 | double reverse_chisq=1e16,reverse_chisq_old=1e16; |
6941 | unsigned int reverse_ndf=0,reverse_ndf_old=0; |
6942 | |
6943 | // Run the Kalman filter in the reverse direction, to get the best guess |
6944 | // for the state vector at the last FDC point on the track |
6945 | DMatrix5x5 CReverse=C; |
6946 | DMatrix5x1 SReverse=S,SDownstream,SBest; |
6947 | kalman_error_t reverse_error=FIT_NOT_DONE; |
6948 | for (int iter=0;iter<20;iter++){ |
6949 | reverse_chisq_old=reverse_chisq; |
6950 | reverse_ndf_old=reverse_ndf; |
6951 | SBest=SDownstream; |
6952 | reverse_error=KalmanReverse(0.,anneal_factor,SReverse,CReverse, |
6953 | SDownstream,reverse_chisq,reverse_ndf); |
6954 | if (reverse_error!=FIT_SUCCEEDED) break; |
6955 | |
6956 | SReverse=SDownstream; |
6957 | for (unsigned int k=0;k<forward_traj.size()-1;k++){ |
6958 | // Get dEdx for the upcoming step |
6959 | double dEdx=0.; |
6960 | if (CORRECT_FOR_ELOSS){ |
6961 | dEdx=GetdEdx(SReverse(state_q_over_p), |
6962 | forward_traj[k].K_rho_Z_over_A, |
6963 | forward_traj[k].rho_Z_over_A, |
6964 | forward_traj[k].LnI,forward_traj[k].Z); |
6965 | } |
6966 | // Step through field |
6967 | DMatrix5x5 J; |
6968 | double z=forward_traj[k].z; |
6969 | double newz=forward_traj[k+1].z; |
6970 | StepJacobian(z,newz,SReverse,dEdx,J); |
6971 | Step(z,newz,dEdx,SReverse); |
6972 | |
6973 | CReverse=forward_traj[k].Q.AddSym(J*CReverse*J.Transpose()); |
6974 | } |
6975 | |
6976 | double reduced_chisq=reverse_chisq/double(reverse_ndf); |
6977 | double reduced_chisq_old=reverse_chisq_old/double(reverse_ndf_old); |
6978 | if (reduced_chisq>reduced_chisq_old |
6979 | || fabs(reduced_chisq-reduced_chisq_old)<0.01) break; |
6980 | } |
6981 | if (reverse_error!=FIT_SUCCEEDED){ |
6982 | ExtrapolateToOuterDetectors(forward_traj[0].S); |
6983 | } |
6984 | else{ |
6985 | ExtrapolateToOuterDetectors(SBest); |
6986 | } |
6987 | } |
6988 | } |
6989 | if (extrapolations.at(SYS_BCAL).size()==1){ |
6990 | // There needs to be some steps inside the the volume of the BCAL for |
6991 | // the extrapolation to be useful. If this is not the case, clear |
6992 | // the extrolation vector. |
6993 | extrapolations[SYS_BCAL].clear(); |
6994 | } |
6995 | |
6996 | // Extrapolate to the point of closest approach to the beam line |
6997 | z_=zlast; |
6998 | DVector2 beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
6999 | double dx=Slast(state_x)-beam_pos.X(); |
7000 | double dy=Slast(state_y)-beam_pos.Y(); |
7001 | bool extrapolated=false; |
7002 | if (sqrt(dx*dx+dy*dy)>EPS21.e-4){ |
7003 | if (ExtrapolateToVertex(Slast,Clast)!=NOERROR) return EXTRAPOLATION_FAILED; |
7004 | extrapolated=true; |
7005 | } |
7006 | |
7007 | // Final momentum, positions and tangents |
7008 | x_=Slast(state_x), y_=Slast(state_y); |
7009 | tx_=Slast(state_tx),ty_=Slast(state_ty); |
7010 | q_over_p_=Slast(state_q_over_p); |
7011 | |
7012 | // Convert from forward rep. to central rep. |
7013 | double tsquare=tx_*tx_+ty_*ty_; |
7014 | tanl_=1./sqrt(tsquare); |
7015 | double cosl=cos(atan(tanl_)); |
7016 | q_over_pt_=q_over_p_/cosl; |
7017 | phi_=atan2(ty_,tx_); |
7018 | if (FORWARD_PARMS_COV==false){ |
7019 | if (extrapolated){ |
7020 | beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
7021 | dx=x_-beam_pos.X(); |
7022 | dy=y_-beam_pos.Y(); |
7023 | } |
7024 | D_=sqrt(dx*dx+dy*dy)+EPS3.0e-8; |
7025 | x_ = dx; y_ = dy; |
7026 | double cosphi=cos(phi_); |
7027 | double sinphi=sin(phi_); |
7028 | if ((dx>0.0 && sinphi>0.0) || (dy<0.0 && cosphi>0.0) |
7029 | || (dy>0.0 && cosphi<0.0) || (dx<0.0 && sinphi<0.0)) D_*=-1.; |
7030 | TransformCovariance(Clast); |
7031 | } |
7032 | // Covariance matrix |
7033 | vector<double>dummy; |
7034 | // ... forward parameterization |
7035 | fcov.clear(); |
7036 | for (unsigned int i=0;i<5;i++){ |
7037 | dummy.clear(); |
7038 | for(unsigned int j=0;j<5;j++){ |
7039 | dummy.push_back(Clast(i,j)); |
7040 | } |
7041 | fcov.push_back(dummy); |
7042 | } |
7043 | |
7044 | return last_error; |
7045 | } |
7046 | |
7047 | // Routine to fit hits in the CDC using the central parametrization |
7048 | kalman_error_t DTrackFitterKalmanSIMD::CentralFit(const DVector2 &startpos, |
7049 | const DMatrix5x1 &S0, |
7050 | const DMatrix5x5 &C0){ |
7051 | // Initial position in x and y |
7052 | DVector2 pos(startpos); |
7053 | |
7054 | // Charge |
7055 | // double q=input_params.charge(); |
7056 | |
7057 | // Covariance matrix and state vector |
7058 | DMatrix5x5 Cc; |
7059 | DMatrix5x1 Sc=S0; |
7060 | |
7061 | // Variables to store values from previous iterations |
7062 | DMatrix5x1 Sclast(Sc); |
7063 | DMatrix5x5 Cclast(C0); |
7064 | DVector2 last_pos=pos; |
7065 | |
7066 | unsigned int num_cdchits=my_cdchits.size(); |
7067 | unsigned int max_cdc_index=num_cdchits-1; |
7068 | unsigned int min_cdc_index_for_refit=MIN_HITS_FOR_REFIT-1; |
7069 | |
7070 | // Vectors to keep track of updated state vectors and covariance matrices (after |
7071 | // adding the hit information) |
7072 | vector<pull_t>last_cdc_pulls; |
7073 | vector<pull_t>cdc_pulls; |
7074 | vector<bool>last_cdc_used_in_fit(num_cdchits); |
7075 | |
7076 | double anneal_factor=ANNEAL_SCALE+1.; // variable for scaling cut for hit pruning |
7077 | |
7078 | //Initialization of chisq, ndf, and error status |
7079 | double chisq_iter=-1.,chisq=-1.; |
7080 | unsigned int my_ndf=0; |
7081 | ndf_=0.; |
7082 | unsigned int last_ndf=1; |
7083 | kalman_error_t error=FIT_NOT_DONE,last_error=FIT_NOT_DONE; |
7084 | |
7085 | // Iterate over reference trajectories |
7086 | int iter2=0; |
7087 | for (;iter2<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20); |
7088 | iter2++){ |
7089 | if (DEBUG_LEVEL>1){ |
7090 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7090<<" " <<"-------- iteration " << iter2+1 << " -----------" <<endl; |
7091 | } |
7092 | |
7093 | // These variables provide the approximate location along the trajectory |
7094 | // where there is an indication of a kink in the track |
7095 | break_point_cdc_index=max_cdc_index; |
7096 | break_point_step_index=0; |
7097 | |
7098 | // Reset material map index |
7099 | last_material_map=0; |
7100 | |
7101 | // Break out of loop if p is too small |
7102 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
7103 | if (fabs(q_over_p)>Q_OVER_P_MAX) break; |
7104 | |
7105 | // Initialize path length variable and flight time |
7106 | len=0.; |
7107 | ftime=0.; |
7108 | var_ftime=0.; |
7109 | |
7110 | // Scale cut for pruning hits according to the iteration number |
7111 | anneal_factor=(iter2<MIN_ITER3)?(ANNEAL_SCALE/pow(ANNEAL_POW_CONST,iter2)+1.):1.; |
7112 | |
7113 | // Initialize trajectory deque |
7114 | jerror_t ref_track_error=SetCDCReferenceTrajectory(last_pos,Sc); |
7115 | if (ref_track_error!=NOERROR){ |
7116 | if (iter2==0) return FIT_FAILED; |
7117 | break; |
7118 | } |
7119 | |
7120 | // Reset the status of the cdc hits |
7121 | for (unsigned int j=0;j<num_cdchits;j++){ |
7122 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
7123 | } |
7124 | |
7125 | // perform the fit |
7126 | Cc=C0; |
7127 | bool did_second_refit=false; |
7128 | error=KalmanCentral(anneal_factor,Sc,Cc,pos,chisq,my_ndf); |
7129 | |
7130 | // Try to recover tracks that failed the first attempt at fitting by |
7131 | // throwing away outer hits. |
7132 | if (error!=FIT_SUCCEEDED && RECOVER_BROKEN_TRACKS |
7133 | && num_cdchits>=MIN_HITS_FOR_REFIT){ |
7134 | DVector2 temp_pos=pos; |
7135 | DMatrix5x1 Stemp=Sc; |
7136 | DMatrix5x5 Ctemp=Cc; |
7137 | unsigned int temp_ndf=my_ndf; |
7138 | double temp_chi2=chisq; |
7139 | |
7140 | if (error==MOMENTUM_OUT_OF_RANGE){ |
7141 | //_DBG_ << "Momentum out of range" <<endl; |
7142 | unsigned int new_index=(3*max_cdc_index)/4; |
7143 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
7144 | } |
7145 | |
7146 | if (error==BROKEN_COVARIANCE_MATRIX){ |
7147 | break_point_cdc_index=min_cdc_index_for_refit; |
7148 | //_DBG_ << "Bad Cov" <<endl; |
7149 | } |
7150 | if (error==POSITION_OUT_OF_RANGE){ |
7151 | //_DBG_ << "Bad position" << endl; |
7152 | unsigned int new_index=(max_cdc_index)/2; |
7153 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
7154 | } |
7155 | if (error==PRUNED_TOO_MANY_HITS){ |
7156 | unsigned int new_index=(3*max_cdc_index)/4; |
7157 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
7158 | //anneal_factor*=10.; |
7159 | //_DBG_ << "Prune" << endl; |
7160 | } |
7161 | |
7162 | kalman_error_t refit_error=RecoverBrokenTracks(anneal_factor,Sc,Cc,C0,pos,chisq,my_ndf); |
7163 | if (fit_type==kTimeBased && refit_error!=FIT_SUCCEEDED){ |
7164 | anneal_factor=1000.; |
7165 | refit_error=RecoverBrokenTracks(anneal_factor,Sc,Cc,C0,pos,chisq,my_ndf); |
7166 | //chisq=1e6; |
7167 | did_second_refit=true; |
7168 | } |
7169 | |
7170 | if (refit_error!=FIT_SUCCEEDED){ |
7171 | //_DBG_ << error << endl; |
7172 | if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){ |
7173 | Cc=Ctemp; |
7174 | Sc=Stemp; |
7175 | my_ndf=temp_ndf; |
7176 | chisq=temp_chi2; |
7177 | pos=temp_pos; |
7178 | if (DEBUG_LEVEL > 1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7178<<" " << " Refit did not succeed, but restoring old values" << endl; |
7179 | |
7180 | //error=FIT_SUCCEEDED; |
7181 | } |
7182 | } |
7183 | else error=FIT_SUCCEEDED; |
7184 | } |
7185 | if ((error==POSITION_OUT_OF_RANGE || error==MOMENTUM_OUT_OF_RANGE) |
7186 | && iter2==0){ |
7187 | return FIT_FAILED; |
7188 | } |
7189 | if (error==FIT_FAILED || error==INVALID_FIT || my_ndf==0){ |
7190 | if (iter2==0) return error; |
7191 | break; |
7192 | } |
7193 | |
7194 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7194<<" " << "--> Chisq " << chisq << " Ndof " << my_ndf |
7195 | << " Prob: " << TMath::Prob(chisq,my_ndf) |
7196 | << endl; |
7197 | |
7198 | if (iter2>MIN_ITER3 && did_second_refit==false){ |
7199 | double new_reduced_chisq=chisq/my_ndf; |
7200 | double old_reduced_chisq=chisq_iter/last_ndf; |
7201 | double new_prob=TMath::Prob(chisq,my_ndf); |
7202 | double old_prob=TMath::Prob(chisq_iter,last_ndf); |
7203 | if (new_prob<old_prob |
7204 | || fabs(new_reduced_chisq-old_reduced_chisq)<CHISQ_DELTA0.01) break; |
7205 | } |
7206 | |
7207 | // Save the current state vector and covariance matrix |
7208 | Cclast=Cc; |
7209 | Sclast=Sc; |
7210 | last_pos=pos; |
7211 | chisq_iter=chisq; |
7212 | last_ndf=my_ndf; |
7213 | last_error=error; |
7214 | |
7215 | last_cdc_used_in_fit=cdc_used_in_fit; |
7216 | } |
7217 | |
7218 | // Run smoother and fill pulls vector |
7219 | IsSmoothed=false; |
7220 | if(fit_type==kTimeBased){ |
7221 | cdc_pulls.clear(); |
7222 | if (SmoothCentral(cdc_pulls) == NOERROR){ |
7223 | IsSmoothed = true; |
7224 | pulls.assign(cdc_pulls.begin(),cdc_pulls.end()); |
7225 | } |
7226 | } |
7227 | |
7228 | // Fill extrapolations vector |
7229 | ClearExtrapolations(); |
7230 | ExtrapolateCentralToOtherDetectors(); |
7231 | if (extrapolations.at(SYS_BCAL).size()==1){ |
7232 | // There needs to be some steps inside the the volume of the BCAL for |
7233 | // the extrapolation to be useful. If this is not the case, clear |
7234 | // the extrolation vector. |
7235 | extrapolations[SYS_BCAL].clear(); |
7236 | } |
7237 | |
7238 | // Extrapolate to the point of closest approach to the beam line |
7239 | DVector2 beam_pos=beam_center+(Sclast(state_z)-beam_z0)*beam_dir; |
7240 | bool extrapolated=false; |
7241 | if ((last_pos-beam_pos).Mod()>EPS21.e-4){ // in cm |
7242 | if (ExtrapolateToVertex(last_pos,Sclast,Cclast)!=NOERROR) return EXTRAPOLATION_FAILED; |
7243 | extrapolated=true; |
7244 | } |
7245 | |
7246 | // output lists of hits used in the fit |
7247 | cdchits_used_in_fit.clear(); |
7248 | for (unsigned int m=0;m<last_cdc_used_in_fit.size();m++){ |
7249 | if (last_cdc_used_in_fit[m]){ |
7250 | cdchits_used_in_fit.push_back(my_cdchits[m]->hit); |
7251 | } |
7252 | } |
7253 | // output the pull information |
7254 | //pulls.assign(last_cdc_pulls.begin(),last_cdc_pulls.end()); |
7255 | |
7256 | // Track Parameters at "vertex" |
7257 | phi_=Sclast(state_phi); |
7258 | q_over_pt_=Sclast(state_q_over_pt); |
7259 | tanl_=Sclast(state_tanl); |
7260 | x_=last_pos.X(); |
7261 | y_=last_pos.Y(); |
7262 | z_=Sclast(state_z); |
7263 | // Find the (signed) distance of closest approach to the beam line |
7264 | if (extrapolated){ |
7265 | beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
7266 | } |
7267 | double dx=x_-beam_pos.X(); |
7268 | double dy=y_-beam_pos.Y(); |
7269 | D_=sqrt(dx*dx+dy*dy)+EPS3.0e-8; |
7270 | x_ = dx; y_ = dy; |
7271 | double cosphi=cos(phi_); |
7272 | double sinphi=sin(phi_); |
7273 | if ((dx>0.0 && sinphi>0.0) || (dy <0.0 && cosphi>0.0) |
7274 | || (dy>0.0 && cosphi<0.0) || (dx<0.0 && sinphi<0.0)) D_*=-1.; |
7275 | // Rotate covariance matrix to coordinate system centered on x=0,y=0 in the |
7276 | // lab |
7277 | DMatrix5x5 Jc=I5x5; |
7278 | Jc(state_D,state_D)=(dy*cosphi-dx*sinphi)/D_; |
7279 | //Cclast=Cclast.SandwichMultiply(Jc); |
7280 | Cclast=Jc*Cclast*Jc.Transpose(); |
7281 | |
7282 | if (!isfinite(x_) || !isfinite(y_) || !isfinite(z_) || !isfinite(phi_) |
7283 | || !isfinite(q_over_pt_) || !isfinite(tanl_)){ |
7284 | if (DEBUG_LEVEL>0){ |
7285 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7285<<" " << "At least one parameter is NaN or +-inf!!" <<endl; |
7286 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7286<<" " << "x " << x_ << " y " << y_ << " z " << z_ << " phi " << phi_ |
7287 | << " q/pt " << q_over_pt_ << " tanl " << tanl_ << endl; |
7288 | } |
7289 | return INVALID_FIT; |
7290 | } |
7291 | |
7292 | // Covariance matrix at vertex |
7293 | fcov.clear(); |
7294 | vector<double>dummy; |
7295 | for (unsigned int i=0;i<5;i++){ |
7296 | dummy.clear(); |
7297 | for(unsigned int j=0;j<5;j++){ |
7298 | dummy.push_back(Cclast(i,j)); |
7299 | } |
7300 | cov.push_back(dummy); |
7301 | } |
7302 | |
7303 | // total chisq and ndf |
7304 | chisq_=chisq_iter; |
7305 | ndf_=last_ndf; |
7306 | //printf("NDof %d\n",ndf); |
7307 | |
7308 | return last_error; |
7309 | } |
7310 | |
7311 | // Smoothing algorithm for the forward trajectory. Updates the state vector |
7312 | // at each step (going in the reverse direction to the filter) based on the |
7313 | // information from all the steps and outputs the state vector at the |
7314 | // outermost step. |
7315 | jerror_t DTrackFitterKalmanSIMD::SmoothForward(vector<pull_t>&forward_pulls){ |
7316 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
7317 | |
7318 | unsigned int max=forward_traj.size()-1; |
7319 | DMatrix5x1 S=(forward_traj[max].Skk); |
7320 | DMatrix5x5 C=(forward_traj[max].Ckk); |
7321 | DMatrix5x5 JT=forward_traj[max].J.Transpose(); |
7322 | DMatrix5x1 Ss=S; |
7323 | DMatrix5x5 Cs=C; |
7324 | DMatrix5x5 A,dC; |
7325 | |
7326 | if (DEBUG_LEVEL>19){ |
7327 | jout << "---- Smoothed residuals ----" <<endl; |
7328 | jout << setprecision(4); |
7329 | } |
7330 | |
7331 | for (unsigned int m=max-1;m>0;m--){ |
7332 | |
7333 | if (WRITE_ML_TRAINING_OUTPUT){ |
7334 | mlfile << forward_traj[m].z; |
7335 | for (unsigned int k=0;k<5;k++){ |
7336 | mlfile << " " << Ss(k); |
7337 | } |
7338 | for (unsigned int k=0;k<5;k++){ |
7339 | mlfile << " " << Cs(k,k); |
7340 | for (unsigned int j=k+1;j<5;j++){ |
7341 | mlfile <<" " << Cs(k,j); |
7342 | } |
7343 | } |
7344 | mlfile << endl; |
7345 | } |
7346 | |
7347 | if (forward_traj[m].h_id>0){ |
7348 | if (forward_traj[m].h_id<1000){ |
7349 | unsigned int id=forward_traj[m].h_id-1; |
7350 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7350<<" " << " Smoothing FDC ID " << id << endl; |
7351 | if (fdc_used_in_fit[id] && my_fdchits[id]->status==good_hit){ |
7352 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7352<<" " << " Used in fit " << endl; |
7353 | A=fdc_updates[id].C*JT*C.InvertSym(); |
7354 | Ss=fdc_updates[id].S+A*(Ss-S); |
7355 | |
7356 | if (!Ss.IsFinite()){ |
7357 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7357<<" " << "Invalid values for smoothed parameters..." << endl; |
7358 | return VALUE_OUT_OF_RANGE; |
7359 | } |
7360 | dC=A*(Cs-C)*A.Transpose(); |
7361 | Cs=fdc_updates[id].C+dC; |
7362 | if (!Cs.IsPosDef()){ |
7363 | if (DEBUG_LEVEL>1) |
7364 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7364<<" " << "Covariance Matrix not PosDef..." << endl; |
7365 | return VALUE_OUT_OF_RANGE; |
7366 | } |
7367 | |
7368 | // Position and direction from state vector with small angle |
7369 | // alignment correction |
7370 | double x=Ss(state_x) + my_fdchits[id]->phiZ*Ss(state_y); |
7371 | double y=Ss(state_y) - my_fdchits[id]->phiZ*Ss(state_x); |
7372 | double tx=Ss(state_tx)+ my_fdchits[id]->phiZ*Ss(state_ty) |
7373 | - my_fdchits[id]->phiY; |
7374 | double ty=Ss(state_ty) - my_fdchits[id]->phiZ*Ss(state_tx) |
7375 | + my_fdchits[id]->phiX; |
7376 | |
7377 | double cosa=my_fdchits[id]->cosa; |
7378 | double sina=my_fdchits[id]->sina; |
7379 | double u=my_fdchits[id]->uwire; |
7380 | double v=my_fdchits[id]->vstrip; |
7381 | |
7382 | // Projected position along the wire without doca-dependent corrections |
7383 | double vpred_uncorrected=x*sina+y*cosa; |
7384 | |
7385 | // Projected position in the plane of the wires transverse to the wires |
7386 | double upred=x*cosa-y*sina; |
7387 | |
7388 | // Direction tangent in the u-z plane |
7389 | double tu=tx*cosa-ty*sina; |
7390 | double one_plus_tu2=1.+tu*tu; |
7391 | double alpha=atan(tu); |
7392 | double cosalpha=cos(alpha); |
7393 | //double cosalpha2=cosalpha*cosalpha; |
7394 | double sinalpha=sin(alpha); |
7395 | |
7396 | // (signed) distance of closest approach to wire |
7397 | double du=upred-u; |
7398 | double doca=du*cosalpha; |
7399 | |
7400 | // Correction for lorentz effect |
7401 | double nz=my_fdchits[id]->nz; |
7402 | double nr=my_fdchits[id]->nr; |
7403 | double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha; |
7404 | |
7405 | // Difference between measurement and projection |
7406 | double tv=tx*sina+ty*cosa; |
7407 | double resi=v-(vpred_uncorrected+doca*(nz_sinalpha_plus_nr_cosalpha |
7408 | -tv*sinalpha)); |
7409 | double drift_time=my_fdchits[id]->t-mT0 |
7410 | -forward_traj[m].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
7411 | double drift = 0.0; |
7412 | int left_right = -999; |
7413 | if (USE_FDC_DRIFT_TIMES) { |
7414 | drift = (du > 0.0 ? 1.0 : -1.0) * fdc_drift_distance(drift_time, forward_traj[m].B); |
7415 | left_right = (du > 0.0 ? +1 : -1); |
7416 | } |
7417 | |
7418 | double resi_a = drift - doca; |
7419 | |
7420 | // Variance from filter step |
7421 | // This V is really "R" in Fruhwirths notation, in the case that the track is used in the fit. |
7422 | DMatrix2x2 V=fdc_updates[id].V; |
7423 | // Compute projection matrix and find the variance for the residual |
7424 | DMatrix5x2 H_T; |
7425 | double temp2=nz_sinalpha_plus_nr_cosalpha-tv*sinalpha; |
7426 | H_T(state_x,1)=sina+cosa*cosalpha*temp2; |
7427 | H_T(state_y,1)=cosa-sina*cosalpha*temp2; |
7428 | |
7429 | double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha; |
7430 | double fac=nz*cos2_minus_sin2-2.*nr*cosalpha*sinalpha; |
7431 | double doca_cosalpha=doca*cosalpha; |
7432 | double temp=doca_cosalpha*fac; |
7433 | H_T(state_tx,1)=cosa*temp |
7434 | -doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2) |
7435 | ; |
7436 | H_T(state_ty,1)=-sina*temp |
7437 | -doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2) |
7438 | ; |
7439 | |
7440 | H_T(state_x,0)=cosa*cosalpha; |
7441 | H_T(state_y,0)=-sina*cosalpha; |
7442 | double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2; |
7443 | H_T(state_ty,0)=sina*factor; |
7444 | H_T(state_tx,0)=-cosa*factor; |
7445 | |
7446 | // Matrix transpose H_T -> H |
7447 | DMatrix2x5 H=Transpose(H_T); |
7448 | |
7449 | if (my_fdchits[id]->hit!=NULL__null |
7450 | && my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP){ |
7451 | //V+=Cs.SandwichMultiply(H_T); |
7452 | V=V+H*Cs*H_T; |
7453 | } |
7454 | else{ |
7455 | //V-=dC.SandwichMultiply(H_T); |
7456 | V=V-H*dC*H_T; |
7457 | } |
7458 | |
7459 | |
7460 | vector<double> alignmentDerivatives; |
7461 | if (ALIGNMENT_FORWARD && my_fdchits[id]->hit!=NULL__null){ |
7462 | alignmentDerivatives.resize(FDCTrackD::size); |
7463 | // Let's get the alignment derivatives |
7464 | |
7465 | // Things are assumed to be linear near the wire, derivatives can be determined analytically. |
7466 | // First for the wires |
7467 | |
7468 | //dDOCAW/ddeltax |
7469 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaX] = -(1/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
7470 | |
7471 | //dDOCAW/ddeltaz |
7472 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaZ] = (tx*cosa - ty*sina)/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
7473 | |
7474 | //dDOCAW/ddeltaPhiX |
7475 | double cos2a = cos(2.*my_fdchits[id]->hit->wire->angle); |
7476 | double sin2a = sin(2.*my_fdchits[id]->hit->wire->angle); |
7477 | double cos3a = cos(3.*my_fdchits[id]->hit->wire->angle); |
7478 | double sin3a = sin(3.*my_fdchits[id]->hit->wire->angle); |
7479 | //double tx2 = tx*tx; |
7480 | //double ty2 = ty*ty; |
7481 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaPhiX] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/ |
7482 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7483 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaPhiY] = -((cosa*(tx*cosa - ty*sina)*(u - x*cosa + y*sina))/ |
7484 | pow(1 + pow(tx*cosa - ty*sina,2),1.5)); |
7485 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaPhiZ] = (tx*ty*u*cos2a + (x + pow(ty,2)*x - tx*ty*y)*sina + |
7486 | cosa*(-(tx*ty*x) + y + pow(tx,2)*y + (tx - ty)*(tx + ty)*u*sina))/ |
7487 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7488 | |
7489 | // dDOCAW/dt0 |
7490 | double t0shift=4.;//ns |
7491 | double drift_shift = 0.0; |
7492 | if(USE_FDC_DRIFT_TIMES){ |
7493 | if (drift_time < 0.) drift_shift = drift; |
7494 | else drift_shift = (du>0.0?1.:-1.)*fdc_drift_distance(drift_time+t0shift,forward_traj[m].B); |
7495 | } |
7496 | alignmentDerivatives[FDCTrackD::dW_dt0]= (drift_shift-drift)/t0shift; |
7497 | |
7498 | // dDOCAW/dx |
7499 | alignmentDerivatives[FDCTrackD::dDOCAW_dx] = cosa/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
7500 | |
7501 | // dDOCAW/dy |
7502 | alignmentDerivatives[FDCTrackD::dDOCAW_dy] = -(sina/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
7503 | |
7504 | // dDOCAW/dtx |
7505 | alignmentDerivatives[FDCTrackD::dDOCAW_dtx] = (cosa*(tx*cosa - ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7506 | |
7507 | // dDOCAW/dty |
7508 | alignmentDerivatives[FDCTrackD::dDOCAW_dty] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/ |
7509 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7510 | |
7511 | // Then for the cathodes. The magnetic field correction now correlates the alignment constants for the wires and cathodes. |
7512 | |
7513 | //dDOCAC/ddeltax |
7514 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaX] = |
7515 | (-nr + (-nz + ty*cosa + tx*sina)*(tx*cosa - ty*sina))/(1 + pow(tx*cosa - ty*sina,2)); |
7516 | |
7517 | //dDOCAC/ddeltaz |
7518 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaZ] = |
7519 | nz + (-nz + (nr*tx + ty)*cosa + (tx - nr*ty)*sina)/(1 + pow(tx*cosa - ty*sina,2)); |
7520 | |
7521 | //dDOCAC/ddeltaPhiX |
7522 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaPhiX] = |
7523 | (-2*y*cosa*sina*(tx*cosa - ty*sina) - 2*x*pow(sina,2)*(tx*cosa - ty*sina) - |
7524 | (u - x*cosa + y*sina)*(-(nz*sina) + sina*(ty*cosa + tx*sina) - |
7525 | cosa*(tx*cosa - ty*sina)))/(1 + pow(tx*cosa - ty*sina,2)) + |
7526 | (2*sina*(tx*cosa - ty*sina)*(-((u - x*cosa + y*sina)* |
7527 | (nr + nz*(tx*cosa - ty*sina) - (ty*cosa + tx*sina)*(tx*cosa - ty*sina))) + |
7528 | y*cosa*(1 + pow(tx*cosa - ty*sina,2)) + x*sina*(1 + pow(tx*cosa - ty*sina,2))))/ |
7529 | pow(1 + pow(tx*cosa - ty*sina,2),2); |
7530 | |
7531 | |
7532 | //dDOCAC/ddeltaPhiY |
7533 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaPhiY] = (-2*y*pow(cosa,2)*(tx*cosa - ty*sina) - 2*x*cosa*sina*(tx*cosa - ty*sina) - |
7534 | (u - x*cosa + y*sina)*(-(nz*cosa) + cosa*(ty*cosa + tx*sina) + |
7535 | sina*(tx*cosa - ty*sina)))/(1 + pow(tx*cosa - ty*sina,2)) + |
7536 | (2*cosa*(tx*cosa - ty*sina)*(-((u - x*cosa + y*sina)* |
7537 | (nr + nz*(tx*cosa - ty*sina) - (ty*cosa + tx*sina)*(tx*cosa - ty*sina))) + |
7538 | y*cosa*(1 + pow(tx*cosa - ty*sina,2)) + x*sina*(1 + pow(tx*cosa - ty*sina,2))))/ |
7539 | pow(1 + pow(tx*cosa - ty*sina,2),2); |
7540 | |
7541 | //dDOCAC/ddeltaPhiZ |
7542 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaPhiZ] = (-2*(ty*cosa + tx*sina)*(tx*cosa - ty*sina)* |
7543 | (-((u - x*cosa + y*sina)*(nr + nz*(tx*cosa - ty*sina) - |
7544 | (ty*cosa + tx*sina)*(tx*cosa - ty*sina))) + |
7545 | y*cosa*(1 + pow(tx*cosa - ty*sina,2)) + x*sina*(1 + pow(tx*cosa - ty*sina,2))))/ |
7546 | pow(1 + pow(tx*cosa - ty*sina,2),2) + |
7547 | (2*y*cosa*(ty*cosa + tx*sina)*(tx*cosa - ty*sina) + |
7548 | 2*x*sina*(ty*cosa + tx*sina)*(tx*cosa - ty*sina) - |
7549 | (-(y*cosa) - x*sina)*(nr + nz*(tx*cosa - ty*sina) - |
7550 | (ty*cosa + tx*sina)*(tx*cosa - ty*sina)) - |
7551 | x*cosa*(1 + pow(tx*cosa - ty*sina,2)) + y*sina*(1 + pow(tx*cosa - ty*sina,2)) - |
7552 | (u - x*cosa + y*sina)*(nz*(ty*cosa + tx*sina) - pow(ty*cosa + tx*sina,2) - |
7553 | (tx*cosa - ty*sina)*(-(tx*cosa) + ty*sina)))/(1 + pow(tx*cosa - ty*sina,2)); |
7554 | |
7555 | //dDOCAC/dx |
7556 | alignmentDerivatives[FDCTrackD::dDOCAC_dx] = (cosa*(nr - tx*ty + nz*tx*cosa) + sina + ty*(ty - nz*cosa)*sina)/ |
7557 | (1 + pow(tx*cosa - ty*sina,2)); |
7558 | |
7559 | //dDOCAC/dy |
7560 | alignmentDerivatives[FDCTrackD::dDOCAC_dy] = ((1 + pow(tx,2))*cosa - (nr + tx*ty + nz*tx*cosa)*sina + nz*ty*pow(sina,2))/ |
7561 | (1 + pow(tx*cosa - ty*sina,2)); |
7562 | |
7563 | //dDOCAC/dtx |
7564 | alignmentDerivatives[FDCTrackD::dDOCAC_dtx] = ((u - x*cosa + y*sina)*(4*nr*tx - 2*ty*(pow(tx,2) + pow(ty,2)) + nz*(-4 + 3*pow(tx,2) + pow(ty,2))*cosa + |
7565 | 2*(2*nr*tx + ty*(2 - pow(tx,2) + pow(ty,2)))*cos2a + nz*(tx - ty)*(tx + ty)*cos3a - 2*nz*tx*ty*sina + |
7566 | 4*(tx - nr*ty + tx*pow(ty,2))*sin2a - 2*nz*tx*ty*sin3a))/ |
7567 | pow(2 + pow(tx,2) + pow(ty,2) + (tx - ty)*(tx + ty)*cos2a - 2*tx*ty*sin2a,2); |
7568 | |
7569 | //dDOCAC/dty |
7570 | alignmentDerivatives[FDCTrackD::dDOCAC_dty] = -(((u - x*cosa + y*sina)*(-2*(pow(tx,3) + 2*nr*ty + tx*pow(ty,2)) - 2*nz*tx*ty*cosa - |
7571 | 2*(2*tx + pow(tx,3) - 2*nr*ty - tx*pow(ty,2))*cos2a + 2*nz*tx*ty*cos3a + |
7572 | nz*(-4 + pow(tx,2) + 3*pow(ty,2))*sina + 4*(ty + tx*(nr + tx*ty))*sin2a + nz*(tx - ty)*(tx + ty)*sin3a)) |
7573 | /pow(2 + pow(tx,2) + pow(ty,2) + (tx - ty)*(tx + ty)*cos2a - 2*tx*ty*sin2a,2)); |
7574 | |
7575 | } |
7576 | |
7577 | if (DEBUG_LEVEL>19 && my_fdchits[id]->hit!=NULL__null){ |
7578 | jout << "Layer " << my_fdchits[id]->hit->wire->layer |
7579 | <<": t " << drift_time << " x "<< x << " y " << y |
7580 | << " coordinate along wire " << v << " resi_c " <<resi |
7581 | << " coordinate transverse to wire " << drift |
7582 | <<" resi_a " << resi_a |
7583 | <<endl; |
7584 | } |
7585 | |
7586 | double scale=1./sqrt(1.+tx*tx+ty*ty); |
7587 | double cosThetaRel=0.; |
7588 | if (my_fdchits[id]->hit!=NULL__null){ |
7589 | my_fdchits[id]->hit->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale)); |
7590 | } |
7591 | DTrackFitter::pull_t thisPull = pull_t(resi_a,sqrt(V(0,0)), |
7592 | forward_traj[m].s, |
7593 | fdc_updates[id].tdrift, |
7594 | fdc_updates[id].doca, |
7595 | NULL__null,my_fdchits[id]->hit, |
7596 | 0., |
7597 | forward_traj[m].z, |
7598 | cosThetaRel,0., |
7599 | resi,sqrt(V(1,1))); |
7600 | thisPull.left_right = left_right; |
7601 | thisPull.AddTrackDerivatives(alignmentDerivatives); |
7602 | forward_pulls.push_back(thisPull); |
7603 | } |
7604 | else{ |
7605 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
7606 | Ss=forward_traj[m].Skk+A*(Ss-S); |
7607 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
7608 | } |
7609 | |
7610 | } |
7611 | else{ |
7612 | unsigned int id=forward_traj[m].h_id-1000; |
7613 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7613<<" " << " Smoothing CDC ID " << id << endl; |
7614 | if (cdc_used_in_fit[id]&&my_cdchits[id]->status==good_hit){ |
7615 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7615<<" " << " Used in fit " << endl; |
7616 | A=cdc_updates[id].C*JT*C.InvertSym(); |
7617 | Ss=cdc_updates[id].S+A*(Ss-S); |
7618 | Cs=cdc_updates[id].C+A*(Cs-C)*A.Transpose(); |
7619 | if (!Cs.IsPosDef()){ |
7620 | if (DEBUG_LEVEL>1) |
7621 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7621<<" " << "Covariance Matrix not PosDef..." << endl; |
7622 | return VALUE_OUT_OF_RANGE; |
7623 | } |
7624 | if (!Ss.IsFinite()){ |
7625 | if (DEBUG_LEVEL>5) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7625<<" " << "Invalid values for smoothed parameters..." << endl; |
7626 | return VALUE_OUT_OF_RANGE; |
7627 | } |
7628 | |
7629 | // Fill in pulls information for cdc hits |
7630 | if(FillPullsVectorEntry(Ss,Cs,forward_traj[m],my_cdchits[id], |
7631 | cdc_updates[id],forward_pulls) != NOERROR) return VALUE_OUT_OF_RANGE; |
7632 | } |
7633 | else{ |
7634 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
7635 | Ss=forward_traj[m].Skk+A*(Ss-S); |
7636 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
7637 | } |
7638 | } |
7639 | } |
7640 | else{ |
7641 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
7642 | Ss=forward_traj[m].Skk+A*(Ss-S); |
7643 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
7644 | } |
7645 | |
7646 | S=forward_traj[m].Skk; |
7647 | C=forward_traj[m].Ckk; |
7648 | JT=forward_traj[m].J.Transpose(); |
7649 | } |
7650 | |
7651 | return NOERROR; |
7652 | } |
7653 | |
7654 | // at each step (going in the reverse direction to the filter) based on the |
7655 | // information from all the steps. |
7656 | jerror_t DTrackFitterKalmanSIMD::SmoothCentral(vector<pull_t>&cdc_pulls){ |
7657 | if (central_traj.size()<2) return RESOURCE_UNAVAILABLE; |
7658 | |
7659 | unsigned int max = central_traj.size()-1; |
7660 | DMatrix5x1 S=(central_traj[max].Skk); |
7661 | DMatrix5x5 C=(central_traj[max].Ckk); |
7662 | DMatrix5x5 JT=central_traj[max].J.Transpose(); |
7663 | DMatrix5x1 Ss=S; |
7664 | DMatrix5x5 Cs=C; |
7665 | DMatrix5x5 A,dC; |
7666 | |
7667 | if (DEBUG_LEVEL>1) { |
7668 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7668<<" " << " S C JT at start of smoothing " << endl; |
7669 | S.Print(); C.Print(); JT.Print(); |
7670 | } |
7671 | |
7672 | for (unsigned int m=max-1;m>0;m--){ |
7673 | if (central_traj[m].h_id>0){ |
7674 | unsigned int id=central_traj[m].h_id-1; |
7675 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7675<<" " << " Encountered Hit ID " << id << " At trajectory position " << m << "/" << max << endl; |
7676 | if (cdc_used_in_fit[id] && my_cdchits[id]->status == good_hit){ |
7677 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7677<<" " << " SmoothCentral CDC Hit ID " << id << " used in fit " << endl; |
7678 | |
7679 | A=cdc_updates[id].C*JT*C.InvertSym(); |
7680 | dC=Cs-C; |
7681 | Ss=cdc_updates[id].S+A*(Ss-S); |
7682 | Cs=cdc_updates[id].C+A*dC*A.Transpose(); |
7683 | |
7684 | if (!Ss.IsFinite()){ |
7685 | if (DEBUG_LEVEL>5) |
7686 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7686<<" " << "Invalid values for smoothed parameters..." << endl; |
7687 | return VALUE_OUT_OF_RANGE; |
7688 | } |
7689 | if (!Cs.IsPosDef()){ |
7690 | if (DEBUG_LEVEL>5){ |
7691 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7691<<" " << "Covariance Matrix not PosDef... Ckk dC A" << endl; |
7692 | cdc_updates[id].C.Print(); dC.Print(); A.Print(); |
7693 | } |
7694 | return VALUE_OUT_OF_RANGE; |
7695 | } |
7696 | |
7697 | // Get estimate for energy loss |
7698 | double q_over_p=Ss(state_q_over_pt)*cos(atan(Ss(state_tanl))); |
7699 | double dEdx=GetdEdx(q_over_p,central_traj[m].K_rho_Z_over_A, |
7700 | central_traj[m].rho_Z_over_A, |
7701 | central_traj[m].LnI,central_traj[m].Z); |
7702 | |
7703 | // Use Brent's algorithm to find doca to the wire |
7704 | DVector2 xy(central_traj[m].xy.X()-Ss(state_D)*sin(Ss(state_phi)), |
7705 | central_traj[m].xy.Y()+Ss(state_D)*cos(Ss(state_phi))); |
7706 | DVector2 old_xy=xy; |
7707 | DMatrix5x1 myS=Ss; |
7708 | double myds; |
7709 | DVector2 origin=my_cdchits[id]->origin; |
7710 | DVector2 dir=my_cdchits[id]->dir; |
7711 | double z0wire=my_cdchits[id]->z0wire; |
7712 | //BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy,z0wire,origin,dir,myS,myds); |
7713 | if(BrentCentral(dEdx,xy,z0wire,origin,dir,myS,myds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7714 | if(DEBUG_HISTS) alignDerivHists[0]->Fill(myds); |
7715 | DVector2 wirepos=origin+(myS(state_z)-z0wire)*dir; |
7716 | double cosstereo=my_cdchits[id]->cosstereo; |
7717 | DVector2 diff=xy-wirepos; |
7718 | // here we add a small number to avoid division by zero errors |
7719 | double d=cosstereo*diff.Mod()+EPS3.0e-8; |
7720 | |
7721 | // If we are doing the alignment, we need to numerically calculate the derivatives |
7722 | // wrt the wire origin, direction, and the track parameters. |
7723 | vector<double> alignmentDerivatives; |
7724 | if (ALIGNMENT_CENTRAL){ |
7725 | double dscut_min=0., dscut_max=1.; |
7726 | DVector3 wireDir = my_cdchits[id]->hit->wire->udir; |
7727 | double cosstereo_shifted; |
7728 | DMatrix5x1 alignS=Ss; // We will mess with this one |
7729 | double alignds; |
7730 | alignmentDerivatives.resize(12); |
7731 | alignmentDerivatives[CDCTrackD::dDdt0]=cdc_updates[id].dDdt0; |
7732 | // Wire position shift |
7733 | double wposShift=0.025; |
7734 | double wdirShift=0.00005; |
7735 | |
7736 | // Shift each track parameter value |
7737 | double shiftFraction=0.01; |
7738 | double shift_q_over_pt=shiftFraction*Ss(state_q_over_pt); |
7739 | double shift_phi=0.0001; |
7740 | double shift_tanl=shiftFraction*Ss(state_tanl); |
7741 | double shift_D=0.01; |
7742 | double shift_z=0.01; |
7743 | |
7744 | // Some data containers we don't need multiples of |
7745 | double z0_shifted; |
7746 | DVector2 shift, origin_shifted, dir_shifted, wirepos_shifted, diff_shifted, xy_shifted; |
7747 | |
7748 | // The DOCA for the shifted states == f(x+h) |
7749 | double d_dOriginX=0., d_dOriginY=0., d_dOriginZ=0.; |
7750 | double d_dDirX=0., d_dDirY=0., d_dDirZ=0.; |
7751 | double d_dS0=0., d_dS1=0., d_dS2=0., d_dS3=0., d_dS4=0.; |
7752 | // Let's do the wire shifts first |
7753 | |
7754 | //dOriginX |
7755 | alignS=Ss; |
7756 | alignds=0.; |
7757 | shift.Set(wposShift, 0.); |
7758 | origin_shifted=origin+shift; |
7759 | dir_shifted=dir; |
7760 | z0_shifted=z0wire; |
7761 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7762 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7763 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7764 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7765 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7766 | //if (BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7767 | // dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7768 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7769 | diff_shifted=xy_shifted-wirepos_shifted; |
7770 | d_dOriginX=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7771 | alignmentDerivatives[CDCTrackD::dDOCAdOriginX] = (d_dOriginX - d)/wposShift; |
7772 | if(DEBUG_HISTS){ |
7773 | alignDerivHists[12]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginX]); |
7774 | alignDerivHists[1]->Fill(alignds); |
7775 | brentCheckHists[1]->Fill(alignds,d_dOriginX); |
7776 | } |
7777 | |
7778 | //dOriginY |
7779 | alignS=Ss; |
7780 | alignds=0.; |
7781 | shift.Set(0.,wposShift); |
7782 | origin_shifted=origin+shift; |
7783 | dir_shifted=dir; |
7784 | z0_shifted=z0wire; |
7785 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7786 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7787 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7788 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7789 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7790 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7791 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7792 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7793 | diff_shifted=xy_shifted-wirepos_shifted; |
7794 | d_dOriginY=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7795 | alignmentDerivatives[CDCTrackD::dDOCAdOriginY] = (d_dOriginY - d)/wposShift; |
7796 | if(DEBUG_HISTS){ |
7797 | alignDerivHists[13]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginY]); |
7798 | alignDerivHists[2]->Fill(alignds); |
7799 | brentCheckHists[1]->Fill(alignds,d_dOriginY); |
7800 | } |
7801 | |
7802 | //dOriginZ |
7803 | alignS=Ss; |
7804 | alignds=0.; |
7805 | origin_shifted=origin; |
7806 | dir_shifted=dir; |
7807 | z0_shifted=z0wire+wposShift; |
7808 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7809 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7810 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7811 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7812 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7813 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7814 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7815 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7816 | diff_shifted=xy_shifted-wirepos_shifted; |
7817 | d_dOriginZ=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7818 | alignmentDerivatives[CDCTrackD::dDOCAdOriginZ] = (d_dOriginZ - d)/wposShift; |
7819 | if(DEBUG_HISTS){ |
7820 | alignDerivHists[14]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginZ]); |
7821 | alignDerivHists[3]->Fill(alignds); |
7822 | brentCheckHists[1]->Fill(alignds,d_dOriginZ); |
7823 | } |
7824 | |
7825 | //dDirX |
7826 | alignS=Ss; |
7827 | alignds=0.; |
7828 | shift.Set(wdirShift,0.); |
7829 | origin_shifted=origin; |
7830 | z0_shifted=z0wire; |
7831 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7832 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7833 | dir_shifted=dir+shift; |
7834 | cosstereo_shifted = cos((wireDir+DVector3(wdirShift,0.,0.)).Angle(DVector3(0.,0.,1.))); |
7835 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7836 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7837 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7838 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7839 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7840 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7841 | diff_shifted=xy_shifted-wirepos_shifted; |
7842 | d_dDirX=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
7843 | alignmentDerivatives[CDCTrackD::dDOCAdDirX] = (d_dDirX - d)/wdirShift; |
7844 | if(DEBUG_HISTS){ |
7845 | alignDerivHists[15]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirX]); |
7846 | alignDerivHists[4]->Fill(alignds); |
7847 | } |
7848 | |
7849 | //dDirY |
7850 | alignS=Ss; |
7851 | alignds=0.; |
7852 | shift.Set(0.,wdirShift); |
7853 | origin_shifted=origin; |
7854 | z0_shifted=z0wire; |
7855 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7856 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7857 | dir_shifted=dir+shift; |
7858 | cosstereo_shifted = cos((wireDir+DVector3(0.,wdirShift,0.)).Angle(DVector3(0.,0.,1.))); |
7859 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7860 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7861 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7862 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7863 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7864 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7865 | diff_shifted=xy_shifted-wirepos_shifted; |
7866 | d_dDirY=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
7867 | alignmentDerivatives[CDCTrackD::dDOCAdDirY] = (d_dDirY - d)/wdirShift; |
7868 | if(DEBUG_HISTS){ |
7869 | alignDerivHists[16]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirY]); |
7870 | alignDerivHists[5]->Fill(alignds); |
7871 | } |
7872 | |
7873 | //dDirZ |
7874 | alignS=Ss; |
7875 | alignds=0.; |
7876 | origin_shifted=origin; |
7877 | dir_shifted.Set(wireDir.X()/(wireDir.Z()+wdirShift), wireDir.Y()/(wireDir.Z()+wdirShift)); |
7878 | z0_shifted=z0wire; |
7879 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7880 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7881 | cosstereo_shifted = cos((wireDir+DVector3(0.,0.,wdirShift)).Angle(DVector3(0.,0.,1.))); |
7882 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7883 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7884 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7885 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7886 | // dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7887 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7888 | diff_shifted=xy_shifted-wirepos_shifted; |
7889 | d_dDirZ=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
7890 | alignmentDerivatives[CDCTrackD::dDOCAdDirZ] = (d_dDirZ - d)/wdirShift; |
7891 | if(DEBUG_HISTS){ |
7892 | alignDerivHists[17]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirZ]); |
7893 | alignDerivHists[6]->Fill(alignds); |
7894 | } |
7895 | |
7896 | // And now the derivatives wrt the track parameters |
7897 | //DMatrix5x1 trackShift(shift_q_over_pt, shift_phi, shift_tanl, shift_D, shift_z); |
7898 | |
7899 | DMatrix5x1 trackShiftS0(shift_q_over_pt, 0., 0., 0., 0.); |
7900 | DMatrix5x1 trackShiftS1(0., shift_phi, 0., 0., 0.); |
7901 | DMatrix5x1 trackShiftS2(0., 0., shift_tanl, 0., 0.); |
7902 | DMatrix5x1 trackShiftS3(0., 0., 0., shift_D, 0.); |
7903 | DMatrix5x1 trackShiftS4(0., 0., 0., 0., shift_z); |
7904 | |
7905 | // dS0 |
7906 | alignS=Ss+trackShiftS0; |
7907 | alignds=0.; |
7908 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7909 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7910 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7911 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7912 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7913 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
7914 | diff_shifted=xy_shifted-wirepos_shifted; |
7915 | d_dS0=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7916 | alignmentDerivatives[CDCTrackD::dDOCAdS0] = (d_dS0 - d)/shift_q_over_pt; |
7917 | if(DEBUG_HISTS){ |
7918 | alignDerivHists[18]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS0]); |
7919 | alignDerivHists[7]->Fill(alignds); |
7920 | } |
7921 | |
7922 | // dS1 |
7923 | alignS=Ss+trackShiftS1; |
7924 | alignds=0.; |
7925 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7926 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7927 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7928 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7929 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7930 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
7931 | diff_shifted=xy_shifted-wirepos_shifted; |
7932 | d_dS1=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7933 | alignmentDerivatives[CDCTrackD::dDOCAdS1] = (d_dS1 - d)/shift_phi; |
7934 | if(DEBUG_HISTS){ |
7935 | alignDerivHists[19]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS1]); |
7936 | alignDerivHists[8]->Fill(alignds); |
7937 | } |
7938 | |
7939 | // dS2 |
7940 | alignS=Ss+trackShiftS2; |
7941 | alignds=0.; |
7942 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7943 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7944 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7945 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7946 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7947 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
7948 | diff_shifted=xy_shifted-wirepos_shifted; |
7949 | d_dS2=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7950 | alignmentDerivatives[CDCTrackD::dDOCAdS2] = (d_dS2 - d)/shift_tanl; |
7951 | if(DEBUG_HISTS){ |
7952 | alignDerivHists[20]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS2]); |
7953 | alignDerivHists[9]->Fill(alignds); |
7954 | } |
7955 | |
7956 | // dS3 |
7957 | alignS=Ss+trackShiftS3; |
7958 | alignds=0.; |
7959 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7960 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7961 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7962 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7963 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7964 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
7965 | diff_shifted=xy_shifted-wirepos_shifted; |
7966 | d_dS3=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7967 | alignmentDerivatives[CDCTrackD::dDOCAdS3] = (d_dS3 - d)/shift_D; |
7968 | if(DEBUG_HISTS){ |
7969 | alignDerivHists[21]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS3]); |
7970 | alignDerivHists[10]->Fill(alignds); |
7971 | } |
7972 | |
7973 | // dS4 |
7974 | alignS=Ss+trackShiftS4; |
7975 | alignds=0.; |
7976 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7977 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7978 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7979 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7980 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7981 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
7982 | diff_shifted=xy_shifted-wirepos_shifted; |
7983 | d_dS4=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7984 | alignmentDerivatives[CDCTrackD::dDOCAdS4] = (d_dS4 - d)/shift_z; |
7985 | if(DEBUG_HISTS){ |
7986 | alignDerivHists[22]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS4]); |
7987 | alignDerivHists[11]->Fill(alignds); |
7988 | } |
7989 | } |
7990 | |
7991 | // Compute the Jacobian matrix |
7992 | // Find the field and gradient at (old_x,old_y,old_z) |
7993 | bfield->GetFieldAndGradient(old_xy.X(),old_xy.Y(),Ss(state_z), |
7994 | Bx,By,Bz, |
7995 | dBxdx,dBxdy,dBxdz,dBydx, |
7996 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
7997 | DMatrix5x5 Jc; |
7998 | StepJacobian(old_xy,xy-old_xy,myds,Ss,dEdx,Jc); |
7999 | |
8000 | // Projection matrix |
8001 | DMatrix5x1 H_T; |
8002 | double sinphi=sin(myS(state_phi)); |
8003 | double cosphi=cos(myS(state_phi)); |
8004 | double dx=diff.X(); |
8005 | double dy=diff.Y(); |
8006 | double cosstereo2_over_doca=cosstereo*cosstereo/d; |
8007 | H_T(state_D)=(dy*cosphi-dx*sinphi)*cosstereo2_over_doca; |
8008 | H_T(state_phi) |
8009 | =-myS(state_D)*cosstereo2_over_doca*(dx*cosphi+dy*sinphi); |
8010 | H_T(state_z)=-cosstereo2_over_doca*(dx*dir.X()+dy*dir.Y()); |
8011 | DMatrix1x5 H; |
8012 | H(state_D)=H_T(state_D); |
8013 | H(state_phi)=H_T(state_phi); |
8014 | H(state_z)=H_T(state_z); |
8015 | |
8016 | double Vhit=cdc_updates[id].variance; |
8017 | Cs=Jc*Cs*Jc.Transpose(); |
8018 | //double Vtrack = Cs.SandwichMultiply(Jc*H_T); |
8019 | double Vtrack=H*Cs*H_T; |
8020 | double VRes; |
8021 | |
8022 | bool skip_ring=(my_cdchits[id]->hit->wire->ring==RING_TO_SKIP); |
8023 | if (skip_ring) VRes = Vhit + Vtrack; |
8024 | else VRes = Vhit - Vtrack; |
8025 | |
8026 | if (DEBUG_LEVEL>1 && (!isfinite(VRes) || VRes < 0.0) ) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8026<<" " << " SmoothCentral Problem: VRes is " << VRes << " = " << Vhit << " - " << Vtrack << endl; |
8027 | |
8028 | double lambda=atan(Ss(state_tanl)); |
8029 | double sinl=sin(lambda); |
8030 | double cosl=cos(lambda); |
8031 | double cosThetaRel=my_cdchits[id]->hit->wire->udir.Dot(DVector3(cosphi*cosl, |
8032 | sinphi*cosl, |
8033 | sinl)); |
8034 | pull_t thisPull(cdc_updates[id].doca-d,sqrt(VRes), |
8035 | central_traj[m].s,cdc_updates[id].tdrift, |
8036 | d,my_cdchits[id]->hit,NULL__null, |
8037 | diff.Phi(),myS(state_z),cosThetaRel, |
8038 | cdc_updates[id].tcorr); |
8039 | |
8040 | thisPull.AddTrackDerivatives(alignmentDerivatives); |
8041 | cdc_pulls.push_back(thisPull); |
8042 | } |
8043 | else{ |
8044 | A=central_traj[m].Ckk*JT*C.InvertSym(); |
8045 | Ss=central_traj[m].Skk+A*(Ss-S); |
8046 | Cs=central_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8047 | } |
8048 | } |
8049 | else{ |
8050 | A=central_traj[m].Ckk*JT*C.InvertSym(); |
8051 | Ss=central_traj[m].Skk+A*(Ss-S); |
8052 | Cs=central_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8053 | } |
8054 | S=central_traj[m].Skk; |
8055 | C=central_traj[m].Ckk; |
8056 | JT=central_traj[m].J.Transpose(); |
8057 | } |
8058 | |
8059 | // ... last entries? |
8060 | // Don't really need since we should have already encountered all of the hits |
8061 | |
8062 | return NOERROR; |
8063 | |
8064 | } |
8065 | |
8066 | // Smoothing algorithm for the forward_traj_cdc trajectory. |
8067 | // Updates the state vector |
8068 | // at each step (going in the reverse direction to the filter) based on the |
8069 | // information from all the steps and outputs the state vector at the |
8070 | // outermost step. |
8071 | jerror_t DTrackFitterKalmanSIMD::SmoothForwardCDC(vector<pull_t>&cdc_pulls){ |
8072 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
8073 | |
8074 | unsigned int max=forward_traj.size()-1; |
8075 | DMatrix5x1 S=(forward_traj[max].Skk); |
8076 | DMatrix5x5 C=(forward_traj[max].Ckk); |
8077 | DMatrix5x5 JT=forward_traj[max].J.Transpose(); |
8078 | DMatrix5x1 Ss=S; |
8079 | DMatrix5x5 Cs=C; |
8080 | DMatrix5x5 A; |
8081 | for (unsigned int m=max-1;m>0;m--){ |
8082 | if (forward_traj[m].h_id>999){ |
8083 | unsigned int cdc_index=forward_traj[m].h_id-1000; |
8084 | if(cdc_used_in_fit[cdc_index] && my_cdchits[cdc_index]->status == good_hit){ |
8085 | if (DEBUG_LEVEL > 5) { |
8086 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8086<<" " << " Smoothing CDC index " << cdc_index << " ring " << my_cdchits[cdc_index]->hit->wire->ring |
8087 | << " straw " << my_cdchits[cdc_index]->hit->wire->straw << endl; |
8088 | } |
8089 | |
8090 | A=cdc_updates[cdc_index].C*JT*C.InvertSym(); |
8091 | Ss=cdc_updates[cdc_index].S+A*(Ss-S); |
8092 | if (!Ss.IsFinite()){ |
8093 | if (DEBUG_LEVEL>5) |
8094 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8094<<" " << "Invalid values for smoothed parameters..." << endl; |
8095 | return VALUE_OUT_OF_RANGE; |
8096 | } |
8097 | |
8098 | Cs=cdc_updates[cdc_index].C+A*(Cs-C)*A.Transpose(); |
8099 | |
8100 | if (!Cs.IsPosDef()){ |
8101 | if (DEBUG_LEVEL>5){ |
8102 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8102<<" " << "Covariance Matrix not Pos Def..." << endl; |
8103 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8103<<" " << " cdc_updates[cdc_index].C A C_ Cs " << endl; |
8104 | cdc_updates[cdc_index].C.Print(); |
8105 | A.Print(); |
8106 | C.Print(); |
8107 | Cs.Print(); |
8108 | } |
8109 | return VALUE_OUT_OF_RANGE; |
8110 | } |
8111 | if(FillPullsVectorEntry(Ss,Cs,forward_traj[m],my_cdchits[cdc_index], |
8112 | cdc_updates[cdc_index],cdc_pulls) != NOERROR) return VALUE_OUT_OF_RANGE; |
8113 | |
8114 | } |
8115 | else{ |
8116 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
8117 | Ss=forward_traj[m].Skk+A*(Ss-S); |
8118 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8119 | } |
8120 | } |
8121 | else{ |
8122 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
8123 | Ss=forward_traj[m].Skk+A*(Ss-S); |
8124 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8125 | } |
8126 | |
8127 | S=forward_traj[m].Skk; |
8128 | C=forward_traj[m].Ckk; |
8129 | JT=forward_traj[m].J.Transpose(); |
8130 | } |
8131 | |
8132 | return NOERROR; |
8133 | } |
8134 | |
8135 | // Fill the pulls vector with the best residual information using the smoothed |
8136 | // filter results. Uses Brent's algorithm to find the distance of closest |
8137 | // approach to the wire hit. |
8138 | jerror_t DTrackFitterKalmanSIMD::FillPullsVectorEntry(const DMatrix5x1 &Ss, |
8139 | const DMatrix5x5 &Cs, |
8140 | const DKalmanForwardTrajectory_t &traj,const DKalmanSIMDCDCHit_t *hit,const DKalmanUpdate_t &update, |
8141 | vector<pull_t>&my_pulls){ |
8142 | |
8143 | // Get estimate for energy loss |
8144 | double dEdx=GetdEdx(Ss(state_q_over_p),traj.K_rho_Z_over_A,traj.rho_Z_over_A, |
8145 | traj.LnI,traj.Z); |
8146 | |
8147 | // Use Brent's algorithm to find the doca to the wire |
8148 | DMatrix5x1 myS=Ss; |
8149 | DMatrix5x1 myS_temp=Ss; |
8150 | DMatrix5x5 myC=Cs; |
8151 | double mydz; |
8152 | double z=traj.z; |
8153 | DVector2 origin=hit->origin; |
8154 | DVector2 dir=hit->dir; |
8155 | double z0wire=hit->z0wire; |
8156 | if(BrentForward(z,dEdx,z0wire,origin,dir,myS,mydz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8157 | if(DEBUG_HISTS)alignDerivHists[23]->Fill(mydz); |
8158 | double new_z=z+mydz; |
8159 | DVector2 wirepos=origin+(new_z-z0wire)*dir; |
8160 | double cosstereo=hit->cosstereo; |
8161 | DVector2 xy(myS(state_x),myS(state_y)); |
8162 | |
8163 | DVector2 diff=xy-wirepos; |
8164 | double d=cosstereo*diff.Mod(); |
8165 | |
8166 | // If we are doing the alignment, we need to numerically calculate the derivatives |
8167 | // wrt the wire origin, direction, and the track parameters. |
8168 | vector<double> alignmentDerivatives; |
8169 | if (ALIGNMENT_FORWARD && hit->hit!=NULL__null){ |
8170 | double dzcut_min=0., dzcut_max=1.; |
8171 | DMatrix5x1 alignS=Ss; // We will mess with this one |
8172 | DVector3 wireDir = hit->hit->wire->udir; |
8173 | double cosstereo_shifted; |
8174 | double aligndz; |
8175 | alignmentDerivatives.resize(12); |
8176 | |
8177 | // Set t0 derivative |
8178 | alignmentDerivatives[CDCTrackD::dDdt0]=update.dDdt0; |
8179 | |
8180 | // Wire position shift |
8181 | double wposShift=0.025; |
8182 | double wdirShift=0.00005; |
8183 | |
8184 | // Shift each track parameter |
8185 | double shiftFraction=0.01; |
8186 | double shift_x=0.01; |
8187 | double shift_y=0.01; |
8188 | double shift_tx=shiftFraction*Ss(state_tx); |
8189 | double shift_ty=shiftFraction*Ss(state_ty);; |
8190 | double shift_q_over_p=shiftFraction*Ss(state_q_over_p); |
8191 | |
8192 | // Some data containers we don't need multiples of |
8193 | double z0_shifted, new_z_shifted; |
8194 | DVector2 shift, origin_shifted, dir_shifted, wirepos_shifted, diff_shifted, xy_shifted; |
8195 | |
8196 | // The DOCA for the shifted states == f(x+h) |
8197 | double d_dOriginX=0., d_dOriginY=0., d_dOriginZ=0.; |
8198 | double d_dDirX=0., d_dDirY=0., d_dDirZ=0.; |
8199 | double d_dS0=0., d_dS1=0., d_dS2=0., d_dS3=0., d_dS4=0.; |
8200 | // Let's do the wire shifts first |
8201 | |
8202 | //dOriginX |
8203 | alignS=Ss; |
8204 | aligndz=0.; |
8205 | shift.Set(wposShift, 0.); |
8206 | origin_shifted=origin+shift; |
8207 | dir_shifted=dir; |
8208 | z0_shifted=z0wire; |
8209 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8210 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8211 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8212 | new_z_shifted=z+aligndz; |
8213 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8214 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8215 | diff_shifted=xy_shifted-wirepos_shifted; |
8216 | d_dOriginX=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8217 | alignmentDerivatives[CDCTrackD::dDOCAdOriginX] = (d_dOriginX - d)/wposShift; |
8218 | if(DEBUG_HISTS){ |
8219 | alignDerivHists[24]->Fill(aligndz); |
8220 | alignDerivHists[35]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginX]); |
8221 | brentCheckHists[0]->Fill(aligndz,d_dOriginX); |
8222 | } |
8223 | |
8224 | //dOriginY |
8225 | alignS=Ss; |
8226 | aligndz=0.; |
8227 | shift.Set(0.,wposShift); |
8228 | origin_shifted=origin+shift; |
8229 | dir_shifted=dir; |
8230 | z0_shifted=z0wire; |
8231 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8232 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8233 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8234 | new_z_shifted=z+aligndz; |
8235 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8236 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8237 | diff_shifted=xy_shifted-wirepos_shifted; |
8238 | d_dOriginY=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8239 | alignmentDerivatives[CDCTrackD::dDOCAdOriginY] = (d_dOriginY - d)/wposShift; |
8240 | if(DEBUG_HISTS){ |
8241 | alignDerivHists[25]->Fill(aligndz); |
8242 | alignDerivHists[36]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginY]); |
8243 | brentCheckHists[0]->Fill(aligndz,d_dOriginY); |
8244 | } |
8245 | |
8246 | //dOriginZ |
8247 | alignS=Ss; |
8248 | aligndz=0.; |
8249 | origin_shifted=origin; |
8250 | dir_shifted=dir; |
8251 | z0_shifted=z0wire+wposShift; |
8252 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8253 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8254 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8255 | new_z_shifted=z+aligndz; |
8256 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8257 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8258 | diff_shifted=xy_shifted-wirepos_shifted; |
8259 | d_dOriginZ=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8260 | alignmentDerivatives[CDCTrackD::dDOCAdOriginZ] = (d_dOriginZ - d)/wposShift; |
8261 | if(DEBUG_HISTS){ |
8262 | alignDerivHists[26]->Fill(aligndz); |
8263 | alignDerivHists[37]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginZ]); |
8264 | brentCheckHists[0]->Fill(aligndz,d_dOriginZ); |
8265 | } |
8266 | |
8267 | //dDirX |
8268 | alignS=Ss; |
8269 | aligndz=0.; |
8270 | shift.Set(wdirShift,0.); |
8271 | origin_shifted=origin; |
8272 | z0_shifted=z0wire; |
8273 | dir_shifted=dir+shift; |
8274 | cosstereo_shifted = cos((wireDir+DVector3(wdirShift,0.,0.)).Angle(DVector3(0.,0.,1.))); |
8275 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8276 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8277 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8278 | new_z_shifted=z+aligndz; |
8279 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8280 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8281 | diff_shifted=xy_shifted-wirepos_shifted; |
8282 | d_dDirX=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
8283 | alignmentDerivatives[CDCTrackD::dDOCAdDirX] = (d_dDirX - d)/wdirShift; |
8284 | if(DEBUG_HISTS){ |
8285 | alignDerivHists[27]->Fill(aligndz); |
8286 | alignDerivHists[38]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirX]); |
8287 | } |
8288 | |
8289 | //dDirY |
8290 | alignS=Ss; |
8291 | aligndz=0.; |
8292 | shift.Set(0.,wdirShift); |
8293 | origin_shifted=origin; |
8294 | z0_shifted=z0wire; |
8295 | dir_shifted=dir+shift; |
8296 | cosstereo_shifted = cos((wireDir+DVector3(0.,wdirShift,0.)).Angle(DVector3(0.,0.,1.))); |
8297 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8298 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8299 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8300 | new_z_shifted=z+aligndz; |
8301 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8302 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8303 | diff_shifted=xy_shifted-wirepos_shifted; |
8304 | d_dDirY=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
8305 | alignmentDerivatives[CDCTrackD::dDOCAdDirY] = (d_dDirY - d)/wdirShift; |
8306 | if(DEBUG_HISTS){ |
8307 | alignDerivHists[28]->Fill(aligndz); |
8308 | alignDerivHists[39]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirY]); |
8309 | } |
8310 | |
8311 | //dDirZ - This is divided out in this code |
8312 | alignS=Ss; |
8313 | aligndz=0.; |
8314 | origin_shifted=origin; |
8315 | dir_shifted.Set(wireDir.X()/(wireDir.Z()+wdirShift), wireDir.Y()/(wireDir.Z()+wdirShift)); |
8316 | z0_shifted=z0wire; |
8317 | cosstereo_shifted = cos((wireDir+DVector3(0.,0.,wdirShift)).Angle(DVector3(0.,0.,1.))); |
8318 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8319 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8320 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8321 | new_z_shifted=z+aligndz; |
8322 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8323 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8324 | diff_shifted=xy_shifted-wirepos_shifted; |
8325 | d_dDirZ=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
8326 | alignmentDerivatives[CDCTrackD::dDOCAdDirZ] = (d_dDirZ - d)/wdirShift; |
8327 | if(DEBUG_HISTS){ |
8328 | alignDerivHists[29]->Fill(aligndz); |
8329 | alignDerivHists[40]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirZ]); |
8330 | } |
8331 | |
8332 | // And now the derivatives wrt the track parameters |
8333 | |
8334 | DMatrix5x1 trackShiftS0(shift_x, 0., 0., 0., 0.); |
8335 | DMatrix5x1 trackShiftS1(0., shift_y, 0., 0., 0.); |
8336 | DMatrix5x1 trackShiftS2(0., 0., shift_tx, 0., 0.); |
8337 | DMatrix5x1 trackShiftS3(0., 0., 0., shift_ty, 0.); |
8338 | DMatrix5x1 trackShiftS4(0., 0., 0., 0., shift_q_over_p); |
8339 | |
8340 | // dS0 |
8341 | alignS=Ss+trackShiftS0; |
8342 | aligndz=0.; |
8343 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8344 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8345 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8346 | new_z_shifted=z+aligndz; |
8347 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8348 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8349 | diff_shifted=xy_shifted-wirepos_shifted; |
8350 | d_dS0=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8351 | alignmentDerivatives[CDCTrackD::dDOCAdS0] = (d_dS0 - d)/shift_x; |
8352 | if(DEBUG_HISTS){ |
8353 | alignDerivHists[30]->Fill(aligndz); |
8354 | alignDerivHists[41]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS0]); |
8355 | } |
8356 | |
8357 | // dS1 |
8358 | alignS=Ss+trackShiftS1; |
8359 | aligndz=0.; |
8360 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8361 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8362 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8363 | new_z_shifted=z+aligndz; |
8364 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8365 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8366 | diff_shifted=xy_shifted-wirepos_shifted; |
8367 | d_dS1=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8368 | alignmentDerivatives[CDCTrackD::dDOCAdS1] = (d_dS1 - d)/shift_y; |
8369 | if(DEBUG_HISTS){ |
8370 | alignDerivHists[31]->Fill(aligndz); |
8371 | alignDerivHists[42]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS1]); |
8372 | } |
8373 | |
8374 | // dS2 |
8375 | alignS=Ss+trackShiftS2; |
8376 | aligndz=0.; |
8377 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8378 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8379 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8380 | new_z_shifted=z+aligndz; |
8381 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8382 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8383 | diff_shifted=xy_shifted-wirepos_shifted; |
8384 | d_dS2=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8385 | alignmentDerivatives[CDCTrackD::dDOCAdS2] = (d_dS2 - d)/shift_tx; |
8386 | if(DEBUG_HISTS){ |
8387 | alignDerivHists[32]->Fill(aligndz); |
8388 | alignDerivHists[43]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS2]); |
8389 | } |
8390 | |
8391 | // dS3 |
8392 | alignS=Ss+trackShiftS3; |
8393 | aligndz=0.; |
8394 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8395 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8396 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8397 | new_z_shifted=z+aligndz; |
8398 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8399 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8400 | diff_shifted=xy_shifted-wirepos_shifted; |
8401 | d_dS3=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8402 | alignmentDerivatives[CDCTrackD::dDOCAdS3] = (d_dS3 - d)/shift_ty; |
8403 | if(DEBUG_HISTS){ |
8404 | alignDerivHists[33]->Fill(aligndz); |
8405 | alignDerivHists[44]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS3]); |
8406 | } |
8407 | |
8408 | // dS4 |
8409 | alignS=Ss+trackShiftS4; |
8410 | aligndz=0.; |
8411 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8412 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8413 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8414 | new_z_shifted=z+aligndz; |
8415 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8416 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8417 | diff_shifted=xy_shifted-wirepos_shifted; |
8418 | d_dS4=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8419 | alignmentDerivatives[CDCTrackD::dDOCAdS4] = (d_dS4 - d)/shift_q_over_p; |
8420 | if(DEBUG_HISTS){ |
8421 | alignDerivHists[34]->Fill(aligndz); |
8422 | alignDerivHists[45]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS4]); |
8423 | } |
8424 | } |
8425 | |
8426 | // Find the field and gradient at (old_x,old_y,old_z) and compute the |
8427 | // Jacobian matrix for transforming from S to myS |
8428 | bfield->GetFieldAndGradient(Ss(state_x),Ss(state_y),z, |
8429 | Bx,By,Bz,dBxdx,dBxdy,dBxdz,dBydx, |
8430 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
8431 | DMatrix5x5 Jc; |
8432 | StepJacobian(z,new_z,Ss,dEdx,Jc); |
8433 | |
8434 | // Find the projection matrix |
8435 | DMatrix5x1 H_T; |
8436 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
8437 | H_T(state_x)=diff.X()*cosstereo2_over_d; |
8438 | H_T(state_y)=diff.Y()*cosstereo2_over_d; |
8439 | DMatrix1x5 H; |
8440 | H(state_x)=H_T(state_x); |
8441 | H(state_y)=H_T(state_y); |
8442 | |
8443 | // Find the variance for this hit |
8444 | |
8445 | bool skip_ring=(hit->hit->wire->ring==RING_TO_SKIP); |
8446 | |
8447 | double V=update.variance; |
8448 | myC=Jc*myC*Jc.Transpose(); |
8449 | if (skip_ring) V+=H*myC*H_T; |
8450 | else V-=H*myC*H_T; |
8451 | |
8452 | if (DEBUG_LEVEL>1 && (!isfinite(V) || V < 0.0) ) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8452<<" " << " Problem: V is " << V << endl; |
8453 | |
8454 | double tx=Ss(state_tx); |
8455 | double ty=Ss(state_ty); |
8456 | double scale=1./sqrt(1.+tx*tx+ty*ty); |
8457 | double cosThetaRel=hit->hit->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale)); |
8458 | |
8459 | pull_t thisPull(update.doca-d,sqrt(V),traj.s,update.tdrift,d,hit->hit, |
8460 | NULL__null,diff.Phi(),new_z,cosThetaRel,update.tcorr); |
8461 | thisPull.AddTrackDerivatives(alignmentDerivatives); |
8462 | my_pulls.push_back(thisPull); |
8463 | return NOERROR; |
8464 | } |
8465 | |
8466 | // Transform the 5x5 covariance matrix from the forward parametrization to the |
8467 | // central parametrization. |
8468 | void DTrackFitterKalmanSIMD::TransformCovariance(DMatrix5x5 &C){ |
8469 | DMatrix5x5 J; |
8470 | double tsquare=tx_*tx_+ty_*ty_; |
8471 | double cosl=cos(atan(tanl_)); |
8472 | double tanl2=tanl_*tanl_; |
8473 | double tanl3=tanl2*tanl_; |
8474 | double factor=1./sqrt(1.+tsquare); |
8475 | J(state_z,state_x)=tx_/tsquare; |
8476 | J(state_z,state_y)=ty_/tsquare; |
8477 | double diff=tx_*tx_-ty_*ty_; |
8478 | double frac=1./(tsquare*tsquare); |
8479 | J(state_z,state_tx)=-(x_*diff+2.*tx_*ty_*y_)*frac; |
8480 | J(state_z,state_ty)=-(2.*tx_*ty_*x_-y_*diff)*frac; |
8481 | J(state_tanl,state_tx)=-tx_*tanl3; |
8482 | J(state_tanl,state_ty)=-ty_*tanl3; |
8483 | J(state_q_over_pt,state_q_over_p)=1./cosl; |
8484 | J(state_q_over_pt,state_tx)=-q_over_p_*tx_*tanl3*factor; |
8485 | J(state_q_over_pt,state_ty)=-q_over_p_*ty_*tanl3*factor; |
8486 | J(state_phi,state_tx)=-ty_*tanl2; |
8487 | J(state_phi,state_ty)=tx_*tanl2; |
8488 | J(state_D,state_x)=x_/D_; |
8489 | J(state_D,state_y)=y_/D_; |
8490 | |
8491 | C=J*C*J.Transpose(); |
8492 | |
8493 | } |
8494 | |
8495 | jerror_t DTrackFitterKalmanSIMD::BrentForward(double z, double dedx, const double z0w, |
8496 | const DVector2 &origin, const DVector2 &dir, DMatrix5x1 &S, double &dz){ |
8497 | |
8498 | DVector2 wirepos=origin; |
8499 | wirepos+=(z-z0w)*dir; |
8500 | double dx=S(state_x)-wirepos.X(); |
8501 | double dy=S(state_y)-wirepos.Y(); |
8502 | double doca2 = dx*dx+dy*dy; |
8503 | |
8504 | if (BrentsAlgorithm(z,-mStepSizeZ,dedx,z0w,origin,dir,S,dz)!=NOERROR){ |
8505 | return VALUE_OUT_OF_RANGE; |
8506 | } |
8507 | |
8508 | double newz = z+dz; |
8509 | unsigned int maxSteps=5; |
8510 | unsigned int stepCounter=0; |
8511 | if (fabs(dz)<EPS31.e-2){ |
8512 | // doca |
8513 | double old_doca2=doca2; |
8514 | |
8515 | double ztemp=newz; |
8516 | newz=ztemp-mStepSizeZ; |
8517 | Step(ztemp,newz,dedx,S); |
8518 | // new wire position |
8519 | wirepos=origin; |
8520 | wirepos+=(newz-z0w)*dir; |
8521 | |
8522 | dx=S(state_x)-wirepos.X(); |
8523 | dy=S(state_y)-wirepos.Y(); |
8524 | doca2=dx*dx+dy*dy; |
8525 | ztemp=newz; |
8526 | |
8527 | while(doca2<old_doca2 && stepCounter<maxSteps){ |
8528 | newz=ztemp-mStepSizeZ; |
8529 | old_doca2=doca2; |
8530 | |
8531 | // Step to the new z position |
8532 | Step(ztemp,newz,dedx,S); |
8533 | stepCounter++; |
8534 | |
8535 | // find the new distance to the wire |
8536 | wirepos=origin; |
8537 | wirepos+=(newz-z0w)*dir; |
8538 | |
8539 | dx=S(state_x)-wirepos.X(); |
8540 | dy=S(state_y)-wirepos.Y(); |
8541 | doca2=dx*dx+dy*dy; |
8542 | |
8543 | ztemp=newz; |
8544 | } |
8545 | |
8546 | // Find the true doca |
8547 | double dz2=0.; |
8548 | if (BrentsAlgorithm(newz,-mStepSizeZ,dedx,z0w,origin,dir,S,dz2)!=NOERROR){ |
8549 | return VALUE_OUT_OF_RANGE; |
8550 | } |
8551 | newz=ztemp+dz2; |
8552 | |
8553 | // Change in z relative to where we started for this wire |
8554 | dz=newz-z; |
8555 | } |
8556 | else if (fabs(dz)>2.*mStepSizeZ-EPS31.e-2){ |
8557 | // whoops, looks like we didn't actually bracket the minimum |
8558 | // after all. Swim to make sure we pass the minimum doca. |
8559 | |
8560 | double ztemp=newz; |
8561 | // new wire position |
8562 | wirepos=origin; |
8563 | wirepos+=(ztemp-z0w)*dir; |
8564 | |
8565 | // doca |
8566 | double old_doca2=doca2; |
8567 | |
8568 | dx=S(state_x)-wirepos.X(); |
8569 | dy=S(state_y)-wirepos.Y(); |
8570 | doca2=dx*dx+dy*dy; |
8571 | |
8572 | while(doca2<old_doca2 && stepCounter<10*maxSteps){ |
8573 | newz=ztemp+mStepSizeZ; |
8574 | old_doca2=doca2; |
8575 | |
8576 | // Step to the new z position |
8577 | Step(ztemp,newz,dedx,S); |
8578 | stepCounter++; |
8579 | |
8580 | // find the new distance to the wire |
8581 | wirepos=origin; |
8582 | wirepos+=(newz-z0w)*dir; |
8583 | |
8584 | dx=S(state_x)-wirepos.X(); |
8585 | dy=S(state_y)-wirepos.Y(); |
8586 | doca2=dx*dx+dy*dy; |
8587 | |
8588 | ztemp=newz; |
8589 | } |
8590 | |
8591 | // Find the true doca |
8592 | double dz2=0.; |
8593 | if (BrentsAlgorithm(newz,mStepSizeZ,dedx,z0w,origin,dir,S,dz2)!=NOERROR){ |
8594 | return VALUE_OUT_OF_RANGE; |
8595 | } |
8596 | newz=ztemp+dz2; |
8597 | |
8598 | // Change in z relative to where we started for this wire |
8599 | dz=newz-z; |
8600 | } |
8601 | return NOERROR; |
8602 | } |
8603 | |
8604 | jerror_t DTrackFitterKalmanSIMD::BrentCentral(double dedx, DVector2 &xy, const double z0w, const DVector2 &origin, const DVector2 &dir, DMatrix5x1 &Sc, double &ds){ |
8605 | |
8606 | DVector2 wirexy=origin; |
8607 | wirexy+=(Sc(state_z)-z0w)*dir; |
8608 | |
8609 | // new doca |
8610 | double doca2=(xy-wirexy).Mod2(); |
8611 | double old_doca2=doca2; |
8612 | |
8613 | if (BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dedx,xy,z0w, |
8614 | origin,dir,Sc,ds)!=NOERROR){ |
8615 | return VALUE_OUT_OF_RANGE; |
8616 | } |
8617 | |
8618 | unsigned int maxSteps=3; |
8619 | unsigned int stepCounter=0; |
8620 | |
8621 | if (fabs(ds)<EPS31.e-2){ |
8622 | double my_ds=ds; |
8623 | old_doca2=doca2; |
8624 | Step(xy,-mStepSizeS,Sc,dedx); |
8625 | my_ds-=mStepSizeS; |
8626 | wirexy=origin; |
8627 | wirexy+=(Sc(state_z)-z0w)*dir; |
8628 | doca2=(xy-wirexy).Mod2(); |
8629 | while(doca2<old_doca2 && stepCounter<maxSteps){ |
8630 | old_doca2=doca2; |
8631 | // Bail if the transverse momentum has dropped below some minimum |
8632 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
8633 | return VALUE_OUT_OF_RANGE; |
8634 | } |
8635 | |
8636 | // Step through the field |
8637 | Step(xy,-mStepSizeS,Sc,dedx); |
8638 | stepCounter++; |
8639 | |
8640 | wirexy=origin; |
8641 | wirexy+=(Sc(state_z)-z0w)*dir; |
8642 | doca2=(xy-wirexy).Mod2(); |
8643 | |
8644 | my_ds-=mStepSizeS; |
8645 | } |
8646 | // Swim to the "true" doca |
8647 | double ds2=0.; |
8648 | if (BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dedx,xy,z0w, |
8649 | origin,dir,Sc,ds2)!=NOERROR){ |
8650 | return VALUE_OUT_OF_RANGE; |
8651 | } |
8652 | ds=my_ds+ds2; |
8653 | } |
8654 | else if (fabs(ds)>2*mStepSizeS-EPS31.e-2){ |
8655 | double my_ds=ds; |
8656 | |
8657 | // new wire position |
8658 | wirexy=origin; |
8659 | wirexy+=(Sc(state_z)-z0w)*dir; |
8660 | |
8661 | // doca |
8662 | old_doca2=doca2; |
8663 | doca2=(xy-wirexy).Mod2(); |
8664 | |
8665 | while(doca2<old_doca2 && stepCounter<maxSteps){ |
8666 | old_doca2=doca2; |
8667 | |
8668 | // Bail if the transverse momentum has dropped below some minimum |
8669 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
8670 | return VALUE_OUT_OF_RANGE; |
8671 | } |
8672 | |
8673 | // Step through the field |
8674 | Step(xy,mStepSizeS,Sc,dedx); |
8675 | stepCounter++; |
8676 | |
8677 | // Find the new distance to the wire |
8678 | wirexy=origin; |
8679 | wirexy+=(Sc(state_z)-z0w)*dir; |
8680 | doca2=(xy-wirexy).Mod2(); |
8681 | |
8682 | my_ds+=mStepSizeS; |
8683 | } |
8684 | // Swim to the "true" doca |
8685 | double ds2=0.; |
8686 | if (BrentsAlgorithm(mStepSizeS,mStepSizeS,dedx,xy,z0w, |
8687 | origin,dir,Sc,ds2)!=NOERROR){ |
8688 | return VALUE_OUT_OF_RANGE; |
8689 | } |
8690 | ds=my_ds+ds2; |
8691 | } |
8692 | return NOERROR; |
8693 | } |
8694 | |
8695 | // Find extrapolations to detectors outside of the tracking volume |
8696 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToOuterDetectors(const DMatrix5x1 &S0){ |
8697 | DMatrix5x1 S=S0; |
8698 | // Energy loss |
8699 | double dEdx=0.; |
8700 | |
8701 | // material properties |
8702 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
8703 | |
8704 | // Position variables |
8705 | double z=forward_traj[0].z; |
8706 | double newz=z,dz=0.; |
Value stored to 'newz' during its initialization is never read | |
8707 | |
8708 | // Current time and path length |
8709 | double t=forward_traj[0].t; |
8710 | double s=forward_traj[0].s; |
8711 | |
8712 | // Store the position and momentum at the exit to the tracking volume |
8713 | AddExtrapolation(SYS_NULL,z,S,t,s); |
8714 | |
8715 | // Loop to propagate track to outer detectors |
8716 | const double z_outer_max=1000.; |
8717 | const double x_max=130.; |
8718 | const double y_max=130.; |
8719 | const double fcal_radius_sq=120.47*120.47; |
8720 | bool hit_tof=false; |
8721 | bool hit_dirc=false; |
8722 | bool hit_fcal=false; |
8723 | bool got_fmwpc=(dFMWPCz_vec.size()>0)?true:false; |
8724 | unsigned int fmwpc_index=0; |
8725 | unsigned int trd_index=0; |
8726 | while (z>Z_MIN-100. && z<z_outer_max && fabs(S(state_x))<x_max |
8727 | && fabs(S(state_y))<y_max){ |
8728 | // Bail if the momentum has dropped below some minimum |
8729 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
8730 | if (DEBUG_LEVEL>2) |
8731 | { |
8732 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8732<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
8733 | << endl; |
8734 | } |
8735 | return VALUE_OUT_OF_RANGE; |
8736 | } |
8737 | |
8738 | // Check if we have passed into the BCAL |
8739 | double r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
8740 | if (r2>89.*89. && z<400.) return VALUE_OUT_OF_RANGE; |
8741 | if (r2>64.9*64.9 && r2<89.*89.){ |
8742 | if (extrapolations.at(SYS_BCAL).size()>299){ |
8743 | return VALUE_OUT_OF_RANGE; |
8744 | } |
8745 | if (z<406.){ |
8746 | AddExtrapolation(SYS_BCAL,z,S,t,s); |
8747 | } |
8748 | else if (extrapolations.at(SYS_BCAL).size()<5){ |
8749 | // There needs to be some steps inside the the volume of the BCAL for |
8750 | // the extrapolation to be useful. If this is not the case, clear |
8751 | // the extrapolation vector. |
8752 | extrapolations[SYS_BCAL].clear(); |
8753 | } |
8754 | } |
8755 | |
8756 | // Relationship between arc length and z |
8757 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx) |
8758 | +S(state_ty)*S(state_ty)); |
8759 | |
8760 | // get material properties from the Root Geometry |
8761 | DVector3 pos(S(state_x),S(state_y),z); |
8762 | DVector3 dir(S(state_tx),S(state_ty),1.); |
8763 | double s_to_boundary=0.; |
8764 | if (geom->FindMatKalman(pos,dir,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
8765 | last_material_map,&s_to_boundary) |
8766 | !=NOERROR){ |
8767 | if (DEBUG_LEVEL>0) |
8768 | { |
8769 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8769<<" " << "Material error in ExtrapolateForwardToOuterDetectors!"<< endl; |
8770 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8770<<" " << " Position (x,y,z)=("<<pos.x()<<","<<pos.y()<<","<<pos.z()<<")" |
8771 | <<endl; |
8772 | } |
8773 | return VALUE_OUT_OF_RANGE; |
8774 | } |
8775 | |
8776 | // Get dEdx for the upcoming step |
8777 | if (CORRECT_FOR_ELOSS){ |
8778 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
8779 | } |
8780 | |
8781 | // Adjust the step size |
8782 | double ds=mStepSizeS; |
8783 | if (fabs(dEdx)>EPS3.0e-8){ |
8784 | ds=DE_PER_STEP0.001/fabs(dEdx); |
8785 | } |
8786 | if (ds>mStepSizeS) ds=mStepSizeS; |
8787 | if (s_to_boundary<ds) ds=s_to_boundary; |
8788 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
8789 | if (ds<0.5 && z<406. && r2>65.*65.) ds=0.5; |
8790 | dz=ds*dz_ds; |
8791 | newz=z+dz; |
8792 | if (trd_index<dTRDz_vec.size() && newz>dTRDz_vec[trd_index]){ |
8793 | newz=dTRDz_vec[trd_index]+EPS3.0e-8; |
8794 | ds=(newz-z)/dz_ds; |
8795 | } |
8796 | if (hit_tof==false && newz>dTOFz){ |
8797 | newz=dTOFz+EPS3.0e-8; |
8798 | ds=(newz-z)/dz_ds; |
8799 | } |
8800 | if (hit_dirc==false && newz>dDIRCz){ |
8801 | newz=dDIRCz+EPS3.0e-8; |
8802 | ds=(newz-z)/dz_ds; |
8803 | } |
8804 | if (hit_fcal==false && newz>dFCALz){ |
8805 | newz=dFCALz+EPS3.0e-8; |
8806 | ds=(newz-z)/dz_ds; |
8807 | } |
8808 | if (fmwpc_index<dFMWPCz_vec.size()&&newz>dFMWPCz_vec[fmwpc_index]){ |
8809 | newz=dFMWPCz_vec[fmwpc_index]+EPS3.0e-8; |
8810 | ds=(newz-z)/dz_ds; |
8811 | } |
8812 | if (got_fmwpc&&newz>dCTOFz){ |
8813 | newz=dCTOFz+EPS3.0e-8; |
8814 | ds=(newz-z)/dz_ds; |
8815 | } |
8816 | s+=ds; |
8817 | |
8818 | // Flight time |
8819 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
8820 | double one_over_beta2=1.+mass2*q_over_p_sq; |
8821 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
8822 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
8823 | |
8824 | // Step through field |
8825 | Step(z,newz,dEdx,S); |
8826 | z=newz; |
8827 | |
8828 | if (trd_index<dTRDz_vec.size() && newz>dTRDz_vec[trd_index]){ |
8829 | AddExtrapolation(SYS_TRD,z,S,t,s); |
8830 | trd_index++; |
8831 | } |
8832 | if (hit_dirc==false && newz>dDIRCz){ |
8833 | hit_dirc=true; |
8834 | AddExtrapolation(SYS_DIRC,z,S,t,s); |
8835 | } |
8836 | if (hit_tof==false && newz>dTOFz){ |
8837 | hit_tof=true; |
8838 | AddExtrapolation(SYS_TOF,z,S,t,s); |
8839 | } |
8840 | if (hit_fcal==false && newz>dFCALz){ |
8841 | double r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
8842 | if (r2>fcal_radius_sq) return NOERROR; |
8843 | |
8844 | hit_fcal=true; |
8845 | AddExtrapolation(SYS_FCAL,z,S,t,s); |
8846 | |
8847 | // Propagate the track to the back of the FCAL block |
8848 | int num=int(45./dz); |
8849 | int m=0; |
8850 | for (;m<num;m++){ |
8851 | // propagate t and s to back of FCAL block |
8852 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
8853 | double one_over_beta2=1.+mass2*q_over_p_sq; |
8854 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
8855 | ds=dz*sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
8856 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
8857 | s+=ds; |
8858 | |
8859 | newz=z+dz; |
8860 | // Step through field |
8861 | Step(z,newz,dEdx,S); |
8862 | z=newz; |
8863 | |
8864 | r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
8865 | if (r2>fcal_radius_sq){ |
8866 | // Break out of the loop if the track exits out of the FCAL before |
8867 | // reaching the back end of the block |
8868 | break; |
8869 | } |
8870 | } |
8871 | if (m==num){ |
8872 | newz=dFCALzBack; |
8873 | |
8874 | // Propagate t and s to back of FCAL block |
8875 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
8876 | double one_over_beta2=1.+mass2*q_over_p_sq; |
8877 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
8878 | dz=newz-z; |
8879 | ds=dz*sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
8880 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
8881 | s+=ds; |
8882 | |
8883 | // Step through field |
8884 | Step(z,newz,dEdx,S); |
8885 | z=newz; |
8886 | } |
8887 | // add another extrapolation point at downstream end of FCAL |
8888 | AddExtrapolation(SYS_FCAL,z,S,t,s); |
8889 | // .. and exit if the muon detector is not installed |
8890 | if (got_fmwpc==false) return NOERROR; |
8891 | } |
8892 | // Deal with muon detector |
8893 | if (hit_fcal==true |
8894 | && (fabs(S(state_x))>dFMWPCsize || (fabs(S(state_y))>dFMWPCsize))){ |
8895 | return NOERROR; |
8896 | } |
8897 | if (fmwpc_index<dFMWPCz_vec.size() && newz>dFMWPCz_vec[fmwpc_index]){ |
8898 | AddExtrapolation(SYS_FMWPC,z,S,t,s); |
8899 | fmwpc_index++; |
8900 | } |
8901 | if (got_fmwpc && newz>dCTOFz){ |
8902 | AddExtrapolation(SYS_CTOF,z,S,t,s); |
8903 | return NOERROR; |
8904 | } |
8905 | } |
8906 | |
8907 | return NOERROR; |
8908 | } |
8909 | |
8910 | // Find extrapolations to detector layers within the tracking volume and |
8911 | // inward (toward the target). |
8912 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToInnerDetectors(){ |
8913 | // First deal with start counter. Only do this if the track has a chance |
8914 | // to intersect with the start counter volume. |
8915 | unsigned int inner_index=forward_traj.size()-1; |
8916 | unsigned int index_beyond_start_counter=inner_index; |
8917 | DMatrix5x1 S=forward_traj[inner_index].S; |
8918 | bool intersected_start_counter=false; |
8919 | if (sc_norm.empty()==false |
8920 | && S(state_x)*S(state_x)+S(state_y)*S(state_y)<SC_BARREL_R2 |
8921 | && forward_traj[inner_index].z<SC_END_NOSE_Z){ |
8922 | double d_old=1000.,d=1000.,z=0.; |
8923 | unsigned int index=0; |
8924 | for (unsigned int m=0;m<12;m++){ |
8925 | unsigned int k=inner_index; |
8926 | for (;k>1;k--){ |
8927 | S=forward_traj[k].S; |
8928 | z=forward_traj[k].z; |
8929 | |
8930 | double dphi=atan2(S(state_y),S(state_x))-SC_PHI_SECTOR1; |
8931 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
8932 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
8933 | if (index>29) index=0; |
8934 | d=sc_norm[index][m].Dot(DVector3(S(state_x),S(state_y),z) |
8935 | -sc_pos[index][m]); |
8936 | if (d*d_old<0){ // break if we cross the current plane |
8937 | if (m==0) index_beyond_start_counter=k; |
8938 | break; |
8939 | } |
8940 | d_old=d; |
8941 | } |
8942 | // if the z position would be beyond the current segment along z of |
8943 | // the start counter, move to the next plane |
8944 | if (z>sc_pos[index][m+1].z()&&m<11){ |
8945 | continue; |
8946 | } |
8947 | // allow for a little slop at the end of the nose |
8948 | else if (z<sc_pos[index][sc_pos[0].size()-1].z()+0.1){ |
8949 | // Hone in on intersection with the appropriate segment of the start |
8950 | // counter |
8951 | int count=0; |
8952 | DMatrix5x1 bestS=S; |
8953 | double dmin=d; |
8954 | double bestz=z; |
8955 | double t=forward_traj[k].t; |
8956 | double s=forward_traj[k].s; |
8957 | double bestt=t,bests=s; |
8958 | |
8959 | // Magnetic field |
8960 | bfield->GetField(S(state_x),S(state_y),z,Bx,By,Bz); |
8961 | |
8962 | while (fabs(d)>0.001 && count<20){ |
8963 | // track direction |
8964 | DVector3 phat(S(state_tx),S(state_ty),1); |
8965 | phat.SetMag(1.); |
8966 | |
8967 | // Step to the start counter plane |
8968 | double ds=d/sc_norm[index][m].Dot(phat); |
8969 | FastStep(z,-ds,0.,S); |
8970 | |
8971 | // Flight time |
8972 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
8973 | double one_over_beta2=1.+mass2*q_over_p_sq; |
8974 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
8975 | t-=ds*sqrt(one_over_beta2); // in units where c=1 |
8976 | s-=ds; |
8977 | |
8978 | // Find the index for the nearest start counter paddle |
8979 | double dphi=atan2(S(state_y),S(state_x))-SC_PHI_SECTOR1; |
8980 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
8981 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
8982 | |
8983 | // Find the new distance to the start counter (which could now be to |
8984 | // a plane in the one adjacent to the one before the step...) |
8985 | d=sc_norm[index][m].Dot(DVector3(S(state_x),S(state_y),z) |
8986 | -sc_pos[index][m]); |
8987 | if (fabs(d)<fabs(dmin)){ |
8988 | bestS=S; |
8989 | dmin=d; |
8990 | bestz=z; |
8991 | bests=s; |
8992 | bestt=t; |
8993 | } |
8994 | count++; |
8995 | } |
8996 | AddExtrapolation(SYS_START,bestz,bestS,bestt,bests); |
8997 | intersected_start_counter=true; |
8998 | break; |
8999 | } |
9000 | } |
9001 | } |
9002 | // Accumulate multiple-scattering terms for use in matching routines |
9003 | double s_theta_ms_sum=0.; |
9004 | double theta2ms_sum=0.; |
9005 | if (intersected_start_counter){ |
9006 | for (unsigned int k=inner_index;k>index_beyond_start_counter;k--){ |
9007 | double ds_theta_ms_sq=3.*fabs(forward_traj[k].Q(state_x,state_x)); |
9008 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9009 | double ds=forward_traj[k].s-forward_traj[k-1].s; |
9010 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9011 | } |
9012 | } |
9013 | |
9014 | // Deal with points within fiducial volume of chambers |
9015 | unsigned int fdc_plane=0; |
9016 | mT0Detector=SYS_NULL; |
9017 | mT0MinimumDriftTime=1e6; |
9018 | for (int k=intersected_start_counter?index_beyond_start_counter:inner_index;k>=0;k--){ |
9019 | double z=forward_traj[k].z; |
9020 | double t=forward_traj[k].t; |
9021 | double s=forward_traj[k].s; |
9022 | DMatrix5x1 S=forward_traj[k].S; |
9023 | |
9024 | // Find estimate for t0 using earliest drift time |
9025 | if (forward_traj[k].h_id>999){ |
9026 | unsigned int index=forward_traj[k].h_id-1000; |
9027 | double dt=my_cdchits[index]->tdrift-t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9028 | if (dt<mT0MinimumDriftTime){ |
9029 | mT0MinimumDriftTime=dt; |
9030 | mT0Detector=SYS_CDC; |
9031 | } |
9032 | } |
9033 | else if (forward_traj[k].h_id>0){ |
9034 | unsigned int index=forward_traj[k].h_id-1; |
9035 | double dt=my_fdchits[index]->t-t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9036 | if (dt<mT0MinimumDriftTime){ |
9037 | mT0MinimumDriftTime=dt; |
9038 | mT0Detector=SYS_FDC; |
9039 | } |
9040 | } |
9041 | |
9042 | //multiple scattering terms |
9043 | if (k>0){ |
9044 | double ds_theta_ms_sq=3.*fabs(forward_traj[k].Q(state_x,state_x)); |
9045 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9046 | double ds=forward_traj[k].s-forward_traj[k-1].s; |
9047 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9048 | } |
9049 | // Extrapolations in CDC region |
9050 | if (z<endplate_z_downstream){ |
9051 | AddExtrapolation(SYS_CDC,z,S,t,s,s_theta_ms_sum,theta2ms_sum); |
9052 | } |
9053 | else{ // extrapolations in FDC region |
9054 | // output step near wire plane |
9055 | if (z>fdc_z_wires[fdc_plane]-0.25){ |
9056 | double dz=z-fdc_z_wires[fdc_plane]; |
9057 | //printf("extrp dz %f\n",dz); |
9058 | if (fabs(dz)>EPS21.e-4){ |
9059 | Step(z,fdc_z_wires[fdc_plane],0.,S); |
9060 | } |
9061 | AddExtrapolation(SYS_FDC,fdc_z_wires[fdc_plane],S,t,s,s_theta_ms_sum, |
9062 | theta2ms_sum); |
9063 | |
9064 | if (fdc_plane==23){ |
9065 | return NOERROR; |
9066 | } |
9067 | |
9068 | fdc_plane++; |
9069 | } |
9070 | } |
9071 | } |
9072 | |
9073 | |
9074 | //-------------------------------------------------------------------------- |
9075 | // The following code continues the extrapolations to wire planes that were |
9076 | // not included in the forward trajectory |
9077 | //-------------------------------------------------------------------------- |
9078 | |
9079 | // Material properties |
9080 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
9081 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
9082 | |
9083 | // Energy loss |
9084 | double dEdx=0.; |
9085 | |
9086 | // multiple scattering matrix |
9087 | DMatrix5x5 Q; |
9088 | |
9089 | // Position variables |
9090 | S=forward_traj[0].S; |
9091 | double z=forward_traj[0].z,newz=z,dz=0.; |
9092 | |
9093 | // Current time and path length |
9094 | double t=forward_traj[0].t; |
9095 | double s=forward_traj[0].s; |
9096 | |
9097 | // Find intersection points to FDC planes beyond the exent of the forward |
9098 | // trajectory. |
9099 | while (z>Z_MIN-100. && z<fdc_z_wires[23]+1. && fdc_plane<24){ |
9100 | // Bail if the momentum has dropped below some minimum |
9101 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
9102 | if (DEBUG_LEVEL>2) |
9103 | { |
9104 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9104<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
9105 | << endl; |
9106 | } |
9107 | return VALUE_OUT_OF_RANGE; |
9108 | } |
9109 | |
9110 | // Current position |
9111 | DVector3 pos(S(state_x),S(state_y),z); |
9112 | if (pos.Perp()>50.) break; |
9113 | |
9114 | // get material properties from the Root Geometry |
9115 | DVector3 dir(S(state_tx),S(state_ty),1.); |
9116 | double s_to_boundary=0.; |
9117 | if (geom->FindMatKalman(pos,dir,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
9118 | chi2c_factor,chi2a_factor,chi2a_corr, |
9119 | last_material_map,&s_to_boundary) |
9120 | !=NOERROR){ |
9121 | if (DEBUG_LEVEL>0) |
9122 | { |
9123 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9123<<" " << "Material error in ExtrapolateForwardToOuterDetectors!"<< endl; |
9124 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9124<<" " << " Position (x,y,z)=("<<pos.x()<<","<<pos.y()<<","<<pos.z()<<")" |
9125 | <<endl; |
9126 | } |
9127 | return VALUE_OUT_OF_RANGE; |
9128 | } |
9129 | |
9130 | // Get dEdx for the upcoming step |
9131 | if (CORRECT_FOR_ELOSS){ |
9132 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
9133 | } |
9134 | |
9135 | // Relationship between arc length and z |
9136 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
9137 | |
9138 | // Adjust the step size |
9139 | double ds=mStepSizeS; |
9140 | if (fabs(dEdx)>EPS3.0e-8){ |
9141 | ds=DE_PER_STEP0.001/fabs(dEdx); |
9142 | } |
9143 | if (ds>mStepSizeS) ds=mStepSizeS; |
9144 | if (s_to_boundary<ds) ds=s_to_boundary; |
9145 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
9146 | dz=ds*dz_ds; |
9147 | newz=z+dz; |
9148 | |
9149 | bool got_fdc_hit=false; |
9150 | if (fdc_plane<24 && newz>fdc_z_wires[fdc_plane]){ |
9151 | newz=fdc_z_wires[fdc_plane]; |
9152 | ds=(newz-z)/dz_ds; |
9153 | got_fdc_hit=true; |
9154 | } |
9155 | s+=ds; |
9156 | |
9157 | // Flight time |
9158 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
9159 | double one_over_beta2=1.+mass2*q_over_p_sq; |
9160 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
9161 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
9162 | |
9163 | // Get the contribution to the covariance matrix due to multiple |
9164 | // scattering |
9165 | GetProcessNoise(z,ds,chi2c_factor,chi2a_factor,chi2a_corr,S,Q); |
9166 | double ds_theta_ms_sq=3.*fabs(Q(state_x,state_x)); |
9167 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9168 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9169 | |
9170 | // Step through field |
9171 | Step(z,newz,dEdx,S); |
9172 | z=newz; |
9173 | |
9174 | if (got_fdc_hit){ |
9175 | AddExtrapolation(SYS_FDC,z,S,s,t,s_theta_ms_sum,theta2ms_sum); |
9176 | fdc_plane++; |
9177 | } |
9178 | } |
9179 | |
9180 | return NOERROR; |
9181 | } |
9182 | |
9183 | // Routine to find intersections with surfaces useful at a later stage for track |
9184 | // matching |
9185 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateForwardToOtherDetectors(){ |
9186 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
9187 | //-------------------------------- |
9188 | // First swim to Start counter and CDC/FDC layers |
9189 | //-------------------------------- |
9190 | jerror_t error=ExtrapolateToInnerDetectors(); |
9191 | |
9192 | //-------------------------------- |
9193 | // Next swim to outer detectors... |
9194 | //-------------------------------- |
9195 | if (error==NOERROR){ |
9196 | return ExtrapolateToOuterDetectors(forward_traj[0].S); |
9197 | } |
9198 | |
9199 | return error; |
9200 | } |
9201 | |
9202 | // Routine to find intersections with surfaces useful at a later stage for track |
9203 | // matching |
9204 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateCentralToOtherDetectors(){ |
9205 | if (central_traj.size()<2) return RESOURCE_UNAVAILABLE; |
9206 | |
9207 | // First deal with start counter. Only do this if the track has a chance |
9208 | // to intersect with the start counter volume. |
9209 | unsigned int inner_index=central_traj.size()-1; |
9210 | unsigned int index_beyond_start_counter=inner_index; |
9211 | DVector2 xy=central_traj[inner_index].xy; |
9212 | DMatrix5x1 S=central_traj[inner_index].S; |
9213 | if (sc_norm.empty()==false |
9214 | &&xy.Mod2()<SC_BARREL_R2&& S(state_z)<SC_END_NOSE_Z){ |
9215 | double d_old=1000.,d=1000.,z=0.; |
9216 | unsigned int index=0; |
9217 | for (unsigned int m=0;m<12;m++){ |
9218 | unsigned int k=inner_index; |
9219 | for (;k>1;k--){ |
9220 | S=central_traj[k].S; |
9221 | z=S(state_z); |
9222 | xy=central_traj[k].xy; |
9223 | |
9224 | double dphi=xy.Phi()-SC_PHI_SECTOR1; |
9225 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
9226 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
9227 | if (index>29) index=0; |
9228 | //cout << "dphi " << dphi << " " << index << endl; |
9229 | |
9230 | d=sc_norm[index][m].Dot(DVector3(xy.X(),xy.Y(),z) |
9231 | -sc_pos[index][m]); |
9232 | |
9233 | if (d*d_old<0){ // break if we cross the current plane |
9234 | if (m==0) index_beyond_start_counter=k; |
9235 | break; |
9236 | } |
9237 | d_old=d; |
9238 | } |
9239 | // if the z position would be beyond the current segment along z of |
9240 | // the start counter, move to the next plane |
9241 | if (z>sc_pos[index][m+1].z()&&m<11){ |
9242 | continue; |
9243 | } |
9244 | // allow for a little slop at the end of the nose |
9245 | else if (z<sc_pos[index][sc_pos[0].size()-1].z()+0.1){ |
9246 | // Propagate the state and covariance through the field |
9247 | // using a straight-line approximation for each step to zero in on the |
9248 | // start counter paddle |
9249 | int count=0; |
9250 | DMatrix5x1 bestS=S; |
9251 | double dmin=d; |
9252 | DVector2 bestXY=central_traj[k].xy; |
9253 | double t=central_traj[k].t; |
9254 | double s=central_traj[k].s; |
9255 | double bests=s,bestt=t; |
9256 | // Magnetic field |
9257 | bfield->GetField(xy.X(),xy.Y(),S(state_z),Bx,By,Bz); |
9258 | |
9259 | while (fabs(d)>0.05 && count<20){ |
9260 | // track direction |
9261 | DVector3 phat(cos(S(state_phi)),sin(S(state_phi)),S(state_tanl)); |
9262 | phat.SetMag(1.); |
9263 | |
9264 | // path length increment |
9265 | double ds=d/sc_norm[index][m].Dot(phat); |
9266 | s-=ds; |
9267 | |
9268 | // Flight time |
9269 | double q_over_p=S(state_q_over_pt)*cos(atan(S(state_tanl))); |
9270 | double q_over_p_sq=q_over_p*q_over_p; |
9271 | double one_over_beta2=1.+mass2*q_over_p_sq; |
9272 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
9273 | t-=ds*sqrt(one_over_beta2); // in units where c=1 |
9274 | |
9275 | // Step along the trajectory using d to estimate path length |
9276 | FastStep(xy,-ds,0.,S); |
9277 | // Find the index for the nearest start counter paddle |
9278 | double dphi=xy.Phi()-SC_PHI_SECTOR1; |
9279 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
9280 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
9281 | if (index>29) index=0; |
9282 | |
9283 | // Find the new distance to the start counter (which could now be to |
9284 | // a plane in the one adjacent to the one before the step...) |
9285 | d=sc_norm[index][m].Dot(DVector3(xy.X(),xy.Y(),S(state_z)) |
9286 | -sc_pos[index][m]); |
9287 | if (fabs(d)<fabs(dmin)){ |
9288 | bestS=S; |
9289 | dmin=d; |
9290 | bestXY=xy; |
9291 | bests=s; |
9292 | bestt=t; |
9293 | } |
9294 | count++; |
9295 | } |
9296 | |
9297 | if (bestS(state_z)>sc_pos[0][0].z()-0.1){ |
9298 | AddExtrapolation(SYS_START,bestS,bestXY,bestt,bests); |
9299 | } |
9300 | break; |
9301 | } |
9302 | } |
9303 | } |
9304 | |
9305 | // Accumulate multiple-scattering terms for use in matching routines |
9306 | double s_theta_ms_sum=0.,theta2ms_sum=0.; |
9307 | for (unsigned int k=inner_index;k>index_beyond_start_counter;k--){ |
9308 | double ds_theta_ms_sq=3.*fabs(central_traj[k].Q(state_D,state_D)); |
9309 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9310 | double ds=central_traj[k].s-central_traj[k-1].s; |
9311 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9312 | } |
9313 | |
9314 | // Deal with points within fiducial volume of chambers |
9315 | mT0Detector=SYS_NULL; |
9316 | mT0MinimumDriftTime=1e6; |
9317 | for (int k=index_beyond_start_counter;k>=0;k--){ |
9318 | S=central_traj[k].S; |
9319 | xy=central_traj[k].xy; |
9320 | double t=central_traj[k].t; |
9321 | double s=central_traj[k].s; |
9322 | |
9323 | // Find estimate for t0 using earliest drift time |
9324 | if (central_traj[k].h_id>0){ |
9325 | unsigned int index=central_traj[k].h_id-1; |
9326 | double dt=my_cdchits[index]->tdrift-t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9327 | if (dt<mT0MinimumDriftTime){ |
9328 | mT0MinimumDriftTime=dt; |
9329 | mT0Detector=SYS_CDC; |
9330 | } |
9331 | } |
9332 | |
9333 | //multiple scattering terms |
9334 | if (k>0){ |
9335 | double ds_theta_ms_sq=3.*fabs(central_traj[k].Q(state_D,state_D)); |
9336 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9337 | double ds=central_traj[k].s-central_traj[k-1].s; |
9338 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9339 | } |
9340 | if (S(state_z)<endplate_z){ |
9341 | AddExtrapolation(SYS_CDC,S,xy,t,s,s_theta_ms_sum,theta2ms_sum); |
9342 | } |
9343 | } |
9344 | // Save the extrapolation at the exit of the tracking volume |
9345 | S=central_traj[0].S; |
9346 | xy=central_traj[0].xy; |
9347 | double t=central_traj[0].t; |
9348 | double s=central_traj[0].s; |
9349 | AddExtrapolation(SYS_NULL,S,xy,t,s); |
9350 | |
9351 | //------------------------------ |
9352 | // Next swim to outer detectors |
9353 | //------------------------------ |
9354 | // Position and step variables |
9355 | double r2=xy.Mod2(); |
9356 | double ds=mStepSizeS; // step along path in cm |
9357 | |
9358 | // Energy loss |
9359 | double dedx=0.; |
9360 | |
9361 | // Track propagation loop |
9362 | //if (false) |
9363 | while (S(state_z)>0. && S(state_z)<Z_MAX370.0 |
9364 | && r2<89.*89.){ |
9365 | // Bail if the transverse momentum has dropped below some minimum |
9366 | if (fabs(S(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
9367 | if (DEBUG_LEVEL>2) |
9368 | { |
9369 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9369<<" " << "Bailing: PT = " << 1./fabs(S(state_q_over_pt)) |
9370 | << endl; |
9371 | } |
9372 | return VALUE_OUT_OF_RANGE; |
9373 | } |
9374 | |
9375 | // get material properties from the Root Geometry |
9376 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
9377 | DVector3 pos3d(xy.X(),xy.Y(),S(state_z)); |
9378 | double s_to_boundary=0.; |
9379 | DVector3 dir(cos(S(state_phi)),sin(S(state_phi)),S(state_tanl)); |
9380 | if (geom->FindMatKalman(pos3d,dir,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
9381 | last_material_map,&s_to_boundary) |
9382 | !=NOERROR){ |
9383 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9383<<" " << "Material error in ExtrapolateToVertex! " << endl; |
9384 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9384<<" " << " Position (x,y,z)=("<<pos3d.x()<<","<<pos3d.y()<<"," |
9385 | << pos3d.z()<<")" |
9386 | <<endl; |
9387 | break; |
9388 | } |
9389 | |
9390 | // Get dEdx for the upcoming step |
9391 | double q_over_p=S(state_q_over_pt)*cos(atan(S(state_tanl))); |
9392 | if (CORRECT_FOR_ELOSS){ |
9393 | dedx=GetdEdx(q_over_p,K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
9394 | } |
9395 | // Adjust the step size |
9396 | if (fabs(dedx)>EPS3.0e-8){ |
9397 | ds=DE_PER_STEP0.001/fabs(dedx); |
9398 | } |
9399 | |
9400 | if (ds>mStepSizeS) ds=mStepSizeS; |
9401 | if (s_to_boundary<ds) ds=s_to_boundary; |
9402 | if (ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1; |
9403 | if (ds<0.5 && S(state_z)<400. && pos3d.Perp()>65.) ds=0.5; |
9404 | s+=ds; |
9405 | |
9406 | // Flight time |
9407 | double q_over_p_sq=q_over_p*q_over_p; |
9408 | double one_over_beta2=1.+mass2*q_over_p_sq; |
9409 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
9410 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
9411 | |
9412 | // Propagate the state through the field |
9413 | Step(xy,ds,S,dedx); |
9414 | |
9415 | r2=xy.Mod2(); |
9416 | // Check if we have passed into the BCAL |
9417 | if (r2>64.9*64.9){ |
9418 | if (extrapolations.at(SYS_BCAL).size()>299){ |
9419 | return VALUE_OUT_OF_RANGE; |
9420 | } |
9421 | if (S(state_z)<406.&&S(state_z)>17.0){ |
9422 | AddExtrapolation(SYS_BCAL,S,xy,t,s); |
9423 | } |
9424 | else if (extrapolations.at(SYS_BCAL).size()<5){ |
9425 | // There needs to be some steps inside the the volume of the BCAL for |
9426 | // the extrapolation to be useful. If this is not the case, clear |
9427 | // the extrolation vector. |
9428 | extrapolations[SYS_BCAL].clear(); |
9429 | } |
9430 | } |
9431 | } |
9432 | |
9433 | return NOERROR; |
9434 | } |
9435 | |
9436 | /*---------------------------------------------------------------------------*/ |
9437 | |
9438 | // Kalman engine for forward tracks, propagating from near the beam line to |
9439 | // the outermost hits (opposite to regular direction). |
9440 | kalman_error_t DTrackFitterKalmanSIMD::KalmanReverse(double fdc_anneal_factor, |
9441 | double cdc_anneal_factor, |
9442 | const DMatrix5x1 &Sstart, |
9443 | DMatrix5x5 &C, |
9444 | DMatrix5x1 &S, |
9445 | double &chisq, |
9446 | unsigned int &numdof){ |
9447 | DMatrix2x1 Mdiff; // difference between measurement and prediction |
9448 | DMatrix2x5 H; // Track projection matrix |
9449 | DMatrix5x2 H_T; // Transpose of track projection matrix |
9450 | DMatrix1x5 Hc; // Track projection matrix for cdc hits |
9451 | DMatrix5x1 Hc_T; // Transpose of track projection matrix for cdc hits |
9452 | DMatrix5x5 J; // State vector Jacobian matrix |
9453 | //DMatrix5x5 J_T; // transpose of this matrix |
9454 | DMatrix5x5 Q; // Process noise covariance matrix |
9455 | DMatrix5x2 K; // Kalman gain matrix |
9456 | DMatrix5x1 Kc; // Kalman gain matrix for cdc hits |
9457 | DMatrix2x2 V(0.0833,0.,0.,FDC_CATHODE_VARIANCE0.000225); // Measurement covariance matrix |
9458 | DMatrix5x1 S0,S0_; //State vector |
9459 | DMatrix5x5 Ctest; // Covariance matrix |
9460 | |
9461 | double Vc=0.0507; |
9462 | |
9463 | unsigned int cdc_index=0; |
9464 | unsigned int num_cdc_hits=my_cdchits.size(); |
9465 | bool more_cdc_measurements=(num_cdc_hits>0); |
9466 | double old_doca2=1e6; |
9467 | |
9468 | // Initialize chi squared |
9469 | chisq=0; |
9470 | |
9471 | // Initialize number of degrees of freedom |
9472 | numdof=0; |
9473 | |
9474 | // Cuts for pruning hits |
9475 | double fdc_chi2cut=NUM_FDC_SIGMA_CUT*NUM_FDC_SIGMA_CUT; |
9476 | double cdc_chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
9477 | |
9478 | // Vectors for cdc wires |
9479 | DVector2 origin,dir,wirepos; |
9480 | double z0w=0.; // origin in z for wire |
9481 | |
9482 | deque<DKalmanForwardTrajectory_t>::reverse_iterator rit = forward_traj.rbegin(); |
9483 | S0_=(*rit).S; |
9484 | S=Sstart; |
9485 | |
9486 | if (more_cdc_measurements){ |
9487 | origin=my_cdchits[0]->origin; |
9488 | dir=my_cdchits[0]->dir; |
9489 | z0w=my_cdchits[0]->z0wire; |
9490 | wirepos=origin+((*rit).z-z0w)*dir; |
9491 | } |
9492 | |
9493 | for (rit=forward_traj.rbegin()+1;rit!=forward_traj.rend();++rit){ |
9494 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
9495 | // from reference trajectory |
9496 | S0=(*rit).S; |
9497 | J=2.*I5x5-(*rit).J; // We only want to change the signs of the parts that depend on dz ... |
9498 | Q=(*rit).Q; |
9499 | |
9500 | // Check that the position is within the tracking volume! |
9501 | if (S(state_x)*S(state_x)+S(state_y)*S(state_y)>65.*65.){ |
9502 | return POSITION_OUT_OF_RANGE; |
9503 | } |
9504 | // Update the actual state vector and covariance matrix |
9505 | S=S0+J*(S-S0_); |
9506 | C=Q.AddSym(J*C*J.Transpose()); |
9507 | //C.Print(); |
9508 | |
9509 | // Save the current state of the reference trajectory |
9510 | S0_=S0; |
9511 | |
9512 | // Z position along the trajectory |
9513 | double z=(*rit).z; |
9514 | |
9515 | if (more_cdc_measurements){ |
9516 | // new wire position |
9517 | wirepos=origin; |
9518 | wirepos+=(z-z0w)*dir; |
9519 | |
9520 | // doca variables |
9521 | double dx=S(state_x)-wirepos.X(); |
9522 | double dy=S(state_y)-wirepos.Y(); |
9523 | double doca2=dx*dx+dy*dy; |
9524 | |
9525 | if (doca2>old_doca2){ |
9526 | if(my_cdchits[cdc_index]->status==good_hit){ |
9527 | find_doca_error_t swimmed_to_doca=DOCA_NO_BRENT; |
9528 | double newz=endplate_z; |
9529 | double dz=newz-z; |
9530 | // Sometimes the true doca would correspond to the case where the |
9531 | // wire would need to extend beyond the physical volume of the straw. |
9532 | // In this case, find the doca at the cdc endplate. |
9533 | if (z>endplate_z){ |
9534 | swimmed_to_doca=DOCA_ENDPLATE; |
9535 | SwimToEndplate(z,*rit,S); |
9536 | |
9537 | // wire position at the endplate |
9538 | wirepos=origin; |
9539 | wirepos+=(endplate_z-z0w)*dir; |
9540 | |
9541 | dx=S(state_x)-wirepos.X(); |
9542 | dy=S(state_y)-wirepos.Y(); |
9543 | } |
9544 | else{ |
9545 | // Find the true doca to the wire. If we had to use Brent's |
9546 | // algorithm, the routine returns USED_BRENT |
9547 | swimmed_to_doca=FindDoca(my_cdchits[cdc_index],*rit,mStepSizeZ, |
9548 | mStepSizeZ,S0,S,C,dx,dy,dz,true); |
9549 | if (swimmed_to_doca==BRENT_FAILED){ |
9550 | //_DBG_ << "Brent's algorithm failed" <<endl; |
9551 | return MOMENTUM_OUT_OF_RANGE; |
9552 | } |
9553 | |
9554 | newz=z+dz; |
9555 | } |
9556 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
9557 | double d=sqrt(dx*dx+dy*dy)*cosstereo+EPS3.0e-8; |
9558 | |
9559 | // Track projection |
9560 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
9561 | Hc_T(state_x)=dx*cosstereo2_over_d; |
9562 | Hc(state_x)=Hc_T(state_x); |
9563 | Hc_T(state_y)=dy*cosstereo2_over_d; |
9564 | Hc(state_y)=Hc_T(state_y); |
9565 | if (swimmed_to_doca==DOCA_NO_BRENT){ |
9566 | Hc_T(state_ty)=Hc_T(state_y)*dz; |
9567 | Hc(state_ty)=Hc_T(state_ty); |
9568 | Hc_T(state_tx)=Hc_T(state_x)*dz; |
9569 | Hc(state_tx)=Hc_T(state_tx); |
9570 | } |
9571 | else{ |
9572 | Hc_T(state_ty)=0.; |
9573 | Hc(state_ty)=0.; |
9574 | Hc_T(state_tx)=0.; |
9575 | Hc(state_tx)=0.; |
9576 | } |
9577 | // Find offset of wire with respect to the center of the |
9578 | // straw at this z position |
9579 | double delta=0.,dphi=0.; |
9580 | FindSag(dx,dy,newz-z0w,my_cdchits[cdc_index]->hit->wire,delta,dphi); |
9581 | |
9582 | // Find drift time and distance |
9583 | double tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
9584 | -(*rit).t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9585 | double tcorr=0.,dmeas=0.; |
9586 | double B=(*rit).B; |
9587 | ComputeCDCDrift(dphi,delta,tdrift,B,dmeas,Vc,tcorr); |
9588 | |
9589 | // Residual |
9590 | double res=dmeas-d; |
9591 | |
9592 | // inverse variance including prediction |
9593 | double Vproj=Hc*C*Hc_T; |
9594 | double InvV1=1./(Vc+Vproj); |
9595 | |
9596 | // Check if this hit is an outlier |
9597 | double chi2_hit=res*res*InvV1; |
9598 | if (chi2_hit<cdc_chi2cut){ |
9599 | // Compute KalmanSIMD gain matrix |
9600 | Kc=InvV1*(C*Hc_T); |
9601 | |
9602 | // Update state vector covariance matrix |
9603 | //C=C-K*(H*C); |
9604 | Ctest=C.SubSym(Kc*(Hc*C)); |
9605 | |
9606 | if (!Ctest.IsPosDef()){ |
9607 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9607<<" " << "Broken covariance matrix!" <<endl; |
9608 | return BROKEN_COVARIANCE_MATRIX; |
9609 | } |
9610 | |
9611 | if (tdrift >= CDC_T_DRIFT_MIN){ |
9612 | C=Ctest; |
9613 | S+=res*Kc; |
9614 | |
9615 | chisq+=(1.-Hc*Kc)*res*res/Vc; |
9616 | numdof++; |
9617 | } |
9618 | } |
9619 | |
9620 | // If we had to use Brent's algorithm to find the true doca or the |
9621 | // doca to the line corresponding to the wire is downstream of the |
9622 | // cdc endplate, we need to swim the state vector and covariance |
9623 | // matrix back to the appropriate position along the reference |
9624 | // trajectory. |
9625 | if (swimmed_to_doca!=DOCA_NO_BRENT){ |
9626 | double dedx=0.; |
9627 | if (CORRECT_FOR_ELOSS){ |
9628 | dedx=GetdEdx(S(state_q_over_p),(*rit).K_rho_Z_over_A, |
9629 | (*rit).rho_Z_over_A,(*rit).LnI,(*rit).Z); |
9630 | } |
9631 | StepBack(dedx,newz,z,S0,S,C); |
9632 | } |
9633 | } |
9634 | |
9635 | // new wire origin and direction |
9636 | if (cdc_index<num_cdc_hits-1){ |
9637 | cdc_index++; |
9638 | origin=my_cdchits[cdc_index]->origin; |
9639 | z0w=my_cdchits[cdc_index]->z0wire; |
9640 | dir=my_cdchits[cdc_index]->dir; |
9641 | } |
9642 | else more_cdc_measurements=false; |
9643 | |
9644 | // Update the wire position |
9645 | wirepos=origin+(z-z0w)*dir; |
9646 | |
9647 | // new doca |
9648 | dx=S(state_x)-wirepos.X(); |
9649 | dy=S(state_y)-wirepos.Y(); |
9650 | doca2=dx*dx+dy*dy; |
9651 | } |
9652 | old_doca2=doca2; |
9653 | } |
9654 | if ((*rit).h_id>0&&(*rit).h_id<1000){ |
9655 | unsigned int id=(*rit).h_id-1; |
9656 | |
9657 | // Variance in coordinate transverse to wire |
9658 | V(0,0)=my_fdchits[id]->uvar; |
9659 | |
9660 | // Variance in coordinate along wire |
9661 | V(1,1)=my_fdchits[id]->vvar; |
9662 | |
9663 | double upred=0,vpred=0.,doca=0.,cosalpha=0.,lorentz_factor=0.; |
9664 | FindDocaAndProjectionMatrix(my_fdchits[id],S,upred,vpred,doca,cosalpha, |
9665 | lorentz_factor,H_T); |
9666 | // Matrix transpose H_T -> H |
9667 | H=Transpose(H_T); |
9668 | |
9669 | |
9670 | DMatrix2x2 Vtemp=V+H*C*H_T; |
9671 | |
9672 | // Residual for coordinate along wire |
9673 | Mdiff(1)=my_fdchits[id]->vstrip-vpred-doca*lorentz_factor; |
9674 | |
9675 | // Residual for coordinate transverse to wire |
9676 | double drift_time=my_fdchits[id]->t-mT0-(*rit).t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9677 | if (my_fdchits[id]->hit!=NULL__null){ |
9678 | double drift=(doca>0.0?1.:-1.)*fdc_drift_distance(drift_time,(*rit).B); |
9679 | Mdiff(0)=drift-doca; |
9680 | |
9681 | // Variance in drift distance |
9682 | V(0,0)=fdc_drift_variance(drift_time); |
9683 | } |
9684 | else if (USE_TRD_DRIFT_TIMES&&my_fdchits[id]->status==trd_hit){ |
9685 | double drift =(doca>0.0?1.:-1.)*0.1*pow(drift_time/8./0.91,1./1.556); |
9686 | Mdiff(0)=drift-doca; |
9687 | |
9688 | // Variance in drift distance |
9689 | V(0,0)=0.05*0.05; |
9690 | } |
9691 | if ((*rit).num_hits==1){ |
9692 | // Add contribution of track covariance from state vector propagation |
9693 | // to measurement errors |
9694 | DMatrix2x2 Vtemp=V+H*C*H_T; |
9695 | double chi2_hit=Vtemp.Chi2(Mdiff); |
9696 | if (chi2_hit<fdc_chi2cut){ |
9697 | // Compute Kalman gain matrix |
9698 | DMatrix2x2 InvV=Vtemp.Invert(); |
9699 | DMatrix5x2 K=C*H_T*InvV; |
9700 | |
9701 | // Update the state vector |
9702 | S+=K*Mdiff; |
9703 | |
9704 | // Update state vector covariance matrix |
9705 | //C=C-K*(H*C); |
9706 | C=C.SubSym(K*(H*C)); |
9707 | |
9708 | if (DEBUG_LEVEL > 35) { |
9709 | jout << "S Update: " << endl; S.Print(); |
9710 | jout << "C Update: " << endl; C.Print(); |
9711 | } |
9712 | |
9713 | // Filtered residual and covariance of filtered residual |
9714 | DMatrix2x1 R=Mdiff-H*K*Mdiff; |
9715 | DMatrix2x2 RC=V-H*(C*H_T); |
9716 | |
9717 | // Update chi2 for this segment |
9718 | chisq+=RC.Chi2(R); |
9719 | |
9720 | if (DEBUG_LEVEL>30) |
9721 | { |
9722 | printf("hit %d p %5.2f dm %5.4f %5.4f sig %5.4f %5.4f chi2 %5.2f\n", |
9723 | id,1./S(state_q_over_p),Mdiff(0),Mdiff(1), |
9724 | sqrt(V(0,0)),sqrt(V(1,1)),RC.Chi2(R)); |
9725 | } |
9726 | |
9727 | numdof+=2; |
9728 | } |
9729 | } |
9730 | // Handle the case where there are multiple adjacent hits in the plane |
9731 | else { |
9732 | UpdateSandCMultiHit(*rit,upred,vpred,doca,cosalpha,lorentz_factor,V, |
9733 | Mdiff,H,H_T,S,C,fdc_chi2cut,false,chisq,numdof); |
9734 | } |
9735 | } |
9736 | |
9737 | //printf("chisq %f ndof %d prob %f\n",chisq,numdof,TMath::Prob(chisq,numdof)); |
9738 | } |
9739 | |
9740 | return FIT_SUCCEEDED; |
9741 | } |
9742 | |
9743 | // Finds the distance of closest approach between a CDC wire and the trajectory |
9744 | // of the track and updates the state vector and covariance matrix at this point. |
9745 | find_doca_error_t |
9746 | DTrackFitterKalmanSIMD::FindDoca(const DKalmanSIMDCDCHit_t *hit, |
9747 | const DKalmanForwardTrajectory_t &traj, |
9748 | double step1,double step2, |
9749 | DMatrix5x1 &S0,DMatrix5x1 &S, |
9750 | DMatrix5x5 &C, |
9751 | double &dx,double &dy,double &dz, |
9752 | bool do_reverse){ |
9753 | double z=traj.z,newz=z; |
9754 | DMatrix5x5 J; |
9755 | |
9756 | // wire position and direction |
9757 | DVector2 origin=hit->origin; |
9758 | double z0w=hit->z0wire; |
9759 | DVector2 dir=hit->dir; |
9760 | |
9761 | // energy loss |
9762 | double dedx=0.; |
9763 | |
9764 | // track direction variables |
9765 | double tx=S(state_tx); |
9766 | double ty=S(state_ty); |
9767 | double tanl=1./sqrt(tx*tx+ty*ty); |
9768 | double sinl=sin(atan(tanl)); |
9769 | |
9770 | // Wire direction variables |
9771 | double ux=dir.X(); |
9772 | double uy=dir.Y(); |
9773 | // Variables relating wire direction and track direction |
9774 | double my_ux=tx-ux; |
9775 | double my_uy=ty-uy; |
9776 | double denom=my_ux*my_ux+my_uy*my_uy; |
9777 | |
9778 | // variables for dealing with propagation of S and C if we |
9779 | // need to use Brent's algorithm to find the doca to the wire |
9780 | int num_steps=0; |
9781 | double my_dz=0.; |
9782 | |
9783 | // if the path length increment is small relative to the radius |
9784 | // of curvature, use a linear approximation to find dz |
9785 | bool do_brent=false; |
9786 | double sign=do_reverse?-1.:1.; |
9787 | double two_step=sign*(step1+step2); |
9788 | if (fabs(qBr2p0.003*S(state_q_over_p) |
9789 | *bfield->GetBz(S(state_x),S(state_y),z) |
9790 | *two_step/sinl)<0.05 |
9791 | && denom>EPS3.0e-8){ |
9792 | double dzw=z-z0w; |
9793 | dz=-((S(state_x)-origin.X()-ux*dzw)*my_ux |
9794 | +(S(state_y)-origin.Y()-uy*dzw)*my_uy)/denom; |
9795 | if (fabs(dz)>fabs(two_step) || sign*dz<0){ |
9796 | do_brent=true; |
9797 | } |
9798 | else{ |
9799 | newz=z+dz; |
9800 | // Check for exiting the straw |
9801 | if (newz>endplate_z){ |
9802 | dz=endplate_z-z; |
9803 | } |
9804 | } |
9805 | } |
9806 | else do_brent=true; |
9807 | if (do_brent){ |
9808 | if (CORRECT_FOR_ELOSS){ |
9809 | dedx=GetdEdx(S(state_q_over_p),traj.K_rho_Z_over_A,traj.rho_Z_over_A, |
9810 | traj.LnI,traj.Z); |
9811 | } |
9812 | |
9813 | // We have bracketed the minimum doca: use Brent's agorithm |
9814 | if (BrentForward(z,dedx,z0w,origin,dir,S,dz)!=NOERROR){ |
9815 | return BRENT_FAILED; |
9816 | } |
9817 | // Step the state and covariance through the field |
9818 | if (fabs(dz)>mStepSizeZ){ |
9819 | my_dz=(dz>0?1.0:-1.)*mStepSizeZ; |
9820 | num_steps=int(fabs(dz/my_dz)); |
9821 | double dz3=dz-num_steps*my_dz; |
9822 | |
9823 | double my_z=z; |
9824 | for (int m=0;m<num_steps;m++){ |
9825 | newz=my_z+my_dz; |
9826 | |
9827 | // propagate error matrix to z-position of hit |
9828 | StepJacobian(my_z,newz,S0,dedx,J); |
9829 | C=J*C*J.Transpose(); |
9830 | //C=C.SandwichMultiply(J); |
9831 | |
9832 | // Step reference trajectory by my_dz |
9833 | Step(my_z,newz,dedx,S0); |
9834 | |
9835 | my_z=newz; |
9836 | } |
9837 | |
9838 | newz=my_z+dz3; |
9839 | // propagate error matrix to z-position of hit |
9840 | StepJacobian(my_z,newz,S0,dedx,J); |
9841 | C=J*C*J.Transpose(); |
9842 | //C=C.SandwichMultiply(J); |
9843 | |
9844 | // Step reference trajectory by dz3 |
9845 | Step(my_z,newz,dedx,S0); |
9846 | } |
9847 | else{ |
9848 | newz = z + dz; |
9849 | |
9850 | // propagate error matrix to z-position of hit |
9851 | StepJacobian(z,newz,S0,dedx,J); |
9852 | C=J*C*J.Transpose(); |
9853 | //C=C.SandwichMultiply(J); |
9854 | |
9855 | // Step reference trajectory by dz |
9856 | Step(z,newz,dedx,S0); |
9857 | } |
9858 | } |
9859 | |
9860 | // Wire position at current z |
9861 | DVector2 wirepos=origin; |
9862 | wirepos+=(newz-z0w)*dir; |
9863 | |
9864 | double xw=wirepos.X(); |
9865 | double yw=wirepos.Y(); |
9866 | |
9867 | // predicted doca taking into account the orientation of the wire |
9868 | if (do_brent==false){ |
9869 | // In this case we did not have to swim to find the doca and |
9870 | // the transformation from the state vector space to the |
9871 | // measurement space is straight-forward. |
9872 | dy=S(state_y)+S(state_ty)*dz-yw; |
9873 | dx=S(state_x)+S(state_tx)*dz-xw; |
9874 | } |
9875 | else{ |
9876 | // In this case we swam the state vector to the position of the doca |
9877 | dy=S(state_y)-yw; |
9878 | dx=S(state_x)-xw; |
9879 | } |
9880 | |
9881 | if (do_brent) return USED_BRENT; |
9882 | return DOCA_NO_BRENT; |
9883 | } |
9884 | |
9885 | // Swim along a trajectory to the z-position z. |
9886 | void DTrackFitterKalmanSIMD::StepBack(double dedx,double newz,double z, |
9887 | DMatrix5x1 &S0,DMatrix5x1 &S, |
9888 | DMatrix5x5 &C){ |
9889 | double dz=newz-z; |
9890 | DMatrix5x5 J; |
9891 | int num_steps=int(fabs(dz/mStepSizeZ)); |
9892 | if (num_steps==0){ |
9893 | // Step C back to the z-position on the reference trajectory |
9894 | StepJacobian(newz,z,S0,dedx,J); |
9895 | C=J*C*J.Transpose(); |
9896 | //C=C.SandwichMultiply(J); |
9897 | |
9898 | // Step S to current position on the reference trajectory |
9899 | Step(newz,z,dedx,S); |
9900 | |
9901 | // Step S0 to current position on the reference trajectory |
9902 | Step(newz,z,dedx,S0); |
9903 | } |
9904 | else{ |
9905 | double my_z=newz; |
9906 | double my_dz=(dz>0.0?1.0:-1.)*mStepSizeZ; |
9907 | double dz3=dz-num_steps*my_dz; |
9908 | for (int m=0;m<num_steps;m++){ |
9909 | z=my_z-my_dz; |
9910 | |
9911 | // Step C along z |
9912 | StepJacobian(my_z,z,S0,dedx,J); |
9913 | C=J*C*J.Transpose(); |
9914 | //C=C.SandwichMultiply(J); |
9915 | |
9916 | // Step S along z |
9917 | Step(my_z,z,dedx,S); |
9918 | |
9919 | // Step S0 along z |
9920 | Step(my_z,z,dedx,S0); |
9921 | |
9922 | my_z=z; |
9923 | } |
9924 | z=my_z-dz3; |
9925 | |
9926 | // Step C back to the z-position on the reference trajectory |
9927 | StepJacobian(my_z,z,S0,dedx,J); |
9928 | C=J*C*J.Transpose(); |
9929 | //C=C.SandwichMultiply(J); |
9930 | |
9931 | // Step S to current position on the reference trajectory |
9932 | Step(my_z,z,dedx,S); |
9933 | |
9934 | // Step S0 to current position on the reference trajectory |
9935 | Step(my_z,z,dedx,S0); |
9936 | } |
9937 | } |
9938 | |
9939 | // Swim a track to the CDC endplate given starting z position |
9940 | void DTrackFitterKalmanSIMD::SwimToEndplate(double z, |
9941 | const DKalmanForwardTrajectory_t &traj, |
9942 | DMatrix5x1 &S){ |
9943 | double dedx=0.; |
9944 | if (CORRECT_FOR_ELOSS){ |
9945 | dedx=GetdEdx(S(state_q_over_p),traj.K_rho_Z_over_A,traj.rho_Z_over_A, |
9946 | traj.LnI,traj.Z); |
9947 | } |
9948 | double my_z=z; |
9949 | int num_steps=int(fabs((endplate_z-z)/mStepSizeZ)); |
9950 | for (int m=0;m<num_steps;m++){ |
9951 | my_z=z-mStepSizeZ; |
9952 | Step(z,my_z,dedx,S); |
9953 | z=my_z; |
9954 | } |
9955 | Step(z,endplate_z,dedx,S); |
9956 | } |
9957 | |
9958 | // Find the sag parameters (delta,dphi) for the given straw at the local z |
9959 | // position |
9960 | void DTrackFitterKalmanSIMD::FindSag(double dx,double dy, double zlocal, |
9961 | const DCDCWire *mywire,double &delta, |
9962 | double &dphi) const{ |
9963 | int ring_index=mywire->ring-1; |
9964 | int straw_index=mywire->straw-1; |
9965 | double phi_d=atan2(dy,dx); |
9966 | delta=max_sag[ring_index][straw_index]*(1.-zlocal*zlocal/5625.) |
9967 | *cos(phi_d + sag_phi_offset[ring_index][straw_index]); |
9968 | |
9969 | dphi=phi_d-mywire->origin.Phi(); |
9970 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
9971 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
9972 | if (mywire->origin.Y()<0) dphi*=-1.; |
9973 | } |
9974 | |
9975 | void DTrackFitterKalmanSIMD::FindDocaAndProjectionMatrix(const DKalmanSIMDFDCHit_t *hit, |
9976 | const DMatrix5x1 &S, |
9977 | double &upred, |
9978 | double &vpred, |
9979 | double &doca, |
9980 | double &cosalpha, |
9981 | double &lorentz_factor, |
9982 | DMatrix5x2 &H_T){ |
9983 | // Make the small alignment rotations |
9984 | // Use small-angle form. |
9985 | |
9986 | // Position and direction from state vector |
9987 | double x=S(state_x) + hit->phiZ*S(state_y); |
9988 | double y=S(state_y) - hit->phiZ*S(state_x); |
9989 | double tx =S(state_tx) + hit->phiZ*S(state_ty) - hit->phiY; |
9990 | double ty =S(state_ty) - hit->phiZ*S(state_tx) + hit->phiX; |
9991 | |
9992 | // Plane orientation and measurements |
9993 | double cosa=hit->cosa; |
9994 | double sina=hit->sina; |
9995 | double u=hit->uwire; |
9996 | |
9997 | // Projected coordinate along wire |
9998 | vpred=x*sina+y*cosa; |
9999 | |
10000 | // Direction tangent in the u-z plane |
10001 | double tu=tx*cosa-ty*sina; |
10002 | double tv=tx*sina+ty*cosa; |
10003 | double alpha=atan(tu); |
10004 | cosalpha=cos(alpha); |
10005 | double cosalpha2=cosalpha*cosalpha; |
10006 | double sinalpha=sin(alpha); |
10007 | |
10008 | // (signed) distance of closest approach to wire |
10009 | upred=x*cosa-y*sina; |
10010 | if (hit->status==gem_hit){ |
10011 | doca=upred-u; |
10012 | } |
10013 | else{ |
10014 | doca=(upred-u)*cosalpha; |
10015 | } |
10016 | |
10017 | // Correction for lorentz effect |
10018 | double nz=hit->nz; |
10019 | double nr=hit->nr; |
10020 | double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha; |
10021 | lorentz_factor=nz_sinalpha_plus_nr_cosalpha-tv*sinalpha; |
10022 | |
10023 | // To transform from (x,y) to (u,v), need to do a rotation: |
10024 | // u = x*cosa-y*sina |
10025 | // v = y*cosa+x*sina |
10026 | if (hit->status!=gem_hit){ |
10027 | H_T(state_x,1)=sina+cosa*cosalpha*lorentz_factor; |
10028 | H_T(state_y,1)=cosa-sina*cosalpha*lorentz_factor; |
10029 | |
10030 | double cos2_minus_sin2=cosalpha2-sinalpha*sinalpha; |
10031 | double fac=nz*cos2_minus_sin2-2.*nr*cosalpha*sinalpha; |
10032 | double doca_cosalpha=doca*cosalpha; |
10033 | double temp=doca_cosalpha*fac; |
10034 | H_T(state_tx,1)=cosa*temp-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2); |
10035 | H_T(state_ty,1)=-sina*temp-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2); |
10036 | |
10037 | H_T(state_x,0)=cosa*cosalpha; |
10038 | H_T(state_y,0)=-sina*cosalpha; |
10039 | |
10040 | double factor=doca*tu*cosalpha2; |
10041 | H_T(state_ty,0)=sina*factor; |
10042 | H_T(state_tx,0)=-cosa*factor; |
10043 | } |
10044 | else{ |
10045 | H_T(state_x,1)=sina; |
10046 | H_T(state_y,1)=cosa; |
10047 | H_T(state_x,0)=cosa; |
10048 | H_T(state_y,0)=-sina; |
10049 | } |
10050 | } |
10051 | |
10052 | // Update S and C using all the good adjacent hits in a particular FDC plane |
10053 | void DTrackFitterKalmanSIMD::UpdateSandCMultiHit(const DKalmanForwardTrajectory_t &traj, |
10054 | double upred,double vpred, |
10055 | double doca,double cosalpha, |
10056 | double lorentz_factor, |
10057 | DMatrix2x2 &V, |
10058 | DMatrix2x1 &Mdiff, |
10059 | DMatrix2x5 &H, |
10060 | const DMatrix5x2 &H_T, |
10061 | DMatrix5x1 &S,DMatrix5x5 &C, |
10062 | double fdc_chi2cut, |
10063 | bool skip_plane,double &chisq, |
10064 | unsigned int &numdof, |
10065 | double fdc_anneal_factor){ |
10066 | // If we do have multiple hits, then all of the hits within some |
10067 | // validation region are included with weights determined by how |
10068 | // close the hits are to the track projection of the state to the |
10069 | // "hit space". |
10070 | vector<DMatrix5x2> Klist; |
10071 | vector<DMatrix2x1> Mlist; |
10072 | vector<DMatrix2x5> Hlist; |
10073 | vector<DMatrix5x2> HTlist; |
10074 | vector<DMatrix2x2> Vlist; |
10075 | vector<double>probs; |
10076 | vector<unsigned int>used_ids; |
10077 | |
10078 | // use a Gaussian model for the probability for the hit |
10079 | DMatrix2x2 Vtemp=V+H*C*H_T; |
10080 | double chi2_hit=Vtemp.Chi2(Mdiff); |
10081 | double prob_hit=0.; |
10082 | |
10083 | // Add the first hit to the list, cutting out outliers |
10084 | unsigned int id=traj.h_id-1; |
10085 | if (chi2_hit<fdc_chi2cut && my_fdchits[id]->status==good_hit){ |
10086 | DMatrix2x2 InvV=Vtemp.Invert(); |
10087 | |
10088 | prob_hit=exp(-0.5*chi2_hit)/(M_TWO_PI6.28318530717958647692*sqrt(Vtemp.Determinant())); |
10089 | probs.push_back(prob_hit); |
10090 | Vlist.push_back(V); |
10091 | Hlist.push_back(H); |
10092 | HTlist.push_back(H_T); |
10093 | Mlist.push_back(Mdiff); |
10094 | Klist.push_back(C*H_T*InvV); // Kalman gain |
10095 | |
10096 | used_ids.push_back(id); |
10097 | fdc_used_in_fit[id]=true; |
10098 | } |
10099 | // loop over the remaining hits |
10100 | for (unsigned int m=1;m<traj.num_hits;m++){ |
10101 | unsigned int my_id=id-m; |
10102 | if (my_fdchits[my_id]->status==good_hit){ |
10103 | double u=my_fdchits[my_id]->uwire; |
10104 | double v=my_fdchits[my_id]->vstrip; |
10105 | |
10106 | // Doca to this wire |
10107 | double mydoca=(upred-u)*cosalpha; |
10108 | |
10109 | // variance for coordinate along the wire |
10110 | V(1,1)=my_fdchits[my_id]->vvar*fdc_anneal_factor; |
10111 | |
10112 | // Difference between measurement and projection |
10113 | Mdiff(1)=v-(vpred+mydoca*lorentz_factor); |
10114 | Mdiff(0)=-mydoca; |
10115 | if (fit_type==kTimeBased && USE_FDC_DRIFT_TIMES){ |
10116 | double drift_time=my_fdchits[my_id]->t-mT0-traj.t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
10117 | double sign=(doca>0.0)?1.:-1.; |
10118 | if (my_fdchits[my_id]->hit!=NULL__null){ |
10119 | double drift=sign*fdc_drift_distance(drift_time,traj.B); |
10120 | Mdiff(0)+=drift; |
10121 | |
10122 | // Variance in drift distance |
10123 | V(0,0)=fdc_drift_variance(drift_time)*fdc_anneal_factor; |
10124 | } |
10125 | else if (USE_TRD_DRIFT_TIMES&&my_fdchits[my_id]->status==trd_hit){ |
10126 | double drift = sign*0.1*pow(drift_time/8./0.91,1./1.556); |
10127 | Mdiff(0)+=drift; |
10128 | V(0,0)=0.05*0.05; |
10129 | } |
10130 | |
10131 | // H_T contains terms that are proportional to doca, so we only need |
10132 | // to scale the appropriate elements for the current hit. |
10133 | if (fabs(doca)<EPS3.0e-8){ |
10134 | doca=(doca>0)?EPS3.0e-8:-EPS3.0e-8; |
10135 | } |
10136 | double doca_ratio=mydoca/doca; |
10137 | DMatrix5x2 H_T_temp=H_T; |
10138 | H_T_temp(state_tx,1)*=doca_ratio; |
10139 | H_T_temp(state_ty,1)*=doca_ratio; |
10140 | H_T_temp(state_tx,0)*=doca_ratio; |
10141 | H_T_temp(state_ty,0)*=doca_ratio; |
10142 | H=Transpose(H_T_temp); |
10143 | |
10144 | // Update error matrix with state vector propagation covariance contrib. |
10145 | Vtemp=V+H*C*H_T_temp; |
10146 | // Cut out outliers and compute probability |
10147 | chi2_hit=Vtemp.Chi2(Mdiff); |
10148 | if (chi2_hit<fdc_chi2cut){ |
10149 | DMatrix2x2 InvV=Vtemp.Invert(); |
10150 | |
10151 | prob_hit=exp(-0.5*chi2_hit)/(M_TWO_PI6.28318530717958647692*sqrt(Vtemp.Determinant())); |
10152 | probs.push_back(prob_hit); |
10153 | Mlist.push_back(Mdiff); |
10154 | Vlist.push_back(V); |
10155 | Hlist.push_back(H); |
10156 | HTlist.push_back(H_T_temp); |
10157 | Klist.push_back(C*H_T_temp*InvV); |
10158 | |
10159 | used_ids.push_back(my_id); |
10160 | fdc_used_in_fit[my_id]=true; |
10161 | |
10162 | // Add some hit info to the update vector for this plane |
10163 | fdc_updates[my_id].tdrift=drift_time; |
10164 | fdc_updates[my_id].tcorr=fdc_updates[my_id].tdrift; |
10165 | fdc_updates[my_id].doca=mydoca; |
10166 | } |
10167 | } |
10168 | } // check that the hit is "good" |
10169 | } // loop over remaining hits |
10170 | if (probs.size()==0) return; |
10171 | |
10172 | double prob_tot=probs[0]; |
10173 | for (unsigned int m=1;m<probs.size();m++){ |
10174 | prob_tot+=probs[m]; |
10175 | } |
10176 | |
10177 | if (skip_plane==false){ |
10178 | DMatrix5x5 sum=I5x5; |
10179 | DMatrix5x5 sum2; |
10180 | for (unsigned int m=0;m<Klist.size();m++){ |
10181 | double my_prob=probs[m]/prob_tot; |
10182 | S+=my_prob*(Klist[m]*Mlist[m]); |
10183 | sum-=my_prob*(Klist[m]*Hlist[m]); |
10184 | sum2+=(my_prob*my_prob)*(Klist[m]*Vlist[m]*Transpose(Klist[m])); |
10185 | |
10186 | // Update chi2 |
10187 | DMatrix2x2 HK=Hlist[m]*Klist[m]; |
10188 | DMatrix2x1 R=Mlist[m]-HK*Mlist[m]; |
10189 | DMatrix2x2 RC=Vlist[m]-HK*Vlist[m]; |
10190 | chisq+=my_prob*RC.Chi2(R); |
10191 | |
10192 | unsigned int my_id=used_ids[m]; |
10193 | fdc_updates[my_id].V=RC; |
10194 | |
10195 | if (DEBUG_LEVEL > 25) |
10196 | { |
10197 | jout << " Adjusting state vector for FDC hit " << m << " with prob " << my_prob << " S:" << endl; |
10198 | S.Print(); |
10199 | } |
10200 | } |
10201 | // Update the covariance matrix C |
10202 | C=sum2.AddSym(sum*C*sum.Transpose()); |
10203 | |
10204 | if (DEBUG_LEVEL > 25) |
10205 | { jout << " C: " << endl; C.Print();} |
10206 | } |
10207 | |
10208 | // Save some information about the track at this plane in the update vector |
10209 | for (unsigned int m=0;m<Hlist.size();m++){ |
10210 | unsigned int my_id=used_ids[m]; |
10211 | fdc_updates[my_id].S=S; |
10212 | fdc_updates[my_id].C=C; |
10213 | if (skip_plane){ |
10214 | fdc_updates[my_id].V=Vlist[m]; |
10215 | } |
10216 | } |
10217 | // update number of degrees of freedom |
10218 | if (skip_plane==false){ |
10219 | numdof+=2; |
10220 | } |
10221 | } |
10222 | |
10223 | // Add extrapolation information for the given detector |
10224 | void DTrackFitterKalmanSIMD::AddExtrapolation(DetectorSystem_t detector, |
10225 | double z,const DMatrix5x1 &S, |
10226 | double t,double s, |
10227 | double s_theta_ms_sum, |
10228 | double theta2ms_sum){ |
10229 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
10230 | double tanl=1./sqrt(tsquare); |
10231 | double cosl=cos(atan(tanl)); |
10232 | double pt=cosl/fabs(S(state_q_over_p)); |
10233 | double phi=atan2(S(state_ty),S(state_tx)); |
10234 | DVector3 position(S(state_x),S(state_y),z); |
10235 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
10236 | extrapolations[detector].push_back(Extrapolation_t(position,momentum, |
10237 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s, |
10238 | s_theta_ms_sum, |
10239 | theta2ms_sum)); |
10240 | if (DEBUG_LEVEL>0){ |
10241 | cout << SystemName(detector) << ": s=" << s |
10242 | << " t=" << t*TIME_UNIT_CONVERSION3.33564095198152014e-02 << " (x,y,z)=(" |
10243 | << position.x() <<","<<position.y()<<","<<position.z()<<") (px,py,pz)=" |
10244 | << momentum.x() <<","<<momentum.y()<<","<<momentum.z()<<")" |
10245 | << endl; |
10246 | } |
10247 | } |
10248 | |
10249 | // Add extrapolation information for the given detector |
10250 | void DTrackFitterKalmanSIMD::AddExtrapolation(DetectorSystem_t detector, |
10251 | const DMatrix5x1 &S, |
10252 | const DVector2 &xy, |
10253 | double t,double s, |
10254 | double s_theta_ms_sum, |
10255 | double theta2ms_sum){ |
10256 | double tanl=S(state_tanl); |
10257 | double pt=1/fabs(S(state_q_over_pt)); |
10258 | double phi=S(state_phi); |
10259 | DVector3 position(xy.X(),xy.Y(),S(state_z)); |
10260 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
10261 | extrapolations[detector].push_back(Extrapolation_t(position,momentum, |
10262 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s, |
10263 | s_theta_ms_sum, |
10264 | theta2ms_sum)); |
10265 | if (DEBUG_LEVEL>0){ |
10266 | cout << SystemName(detector) << ": s=" << s |
10267 | << " t=" << t*TIME_UNIT_CONVERSION3.33564095198152014e-02 << " (x,y,z)=("<< position.x() |
10268 | <<","<<position.y()<<","<<position.z()<<") (px,py,pz)=" |
10269 | << momentum.x() <<","<<momentum.y()<<","<<momentum.z()<<")" |
10270 | << endl; |
10271 | } |
10272 | } |