File: | /home/sdobbs/work/clang/halld_recon/src/libraries/TRACKING/DTrackFitterKalmanSIMD.cc |
Warning: | line 8791, column 10 Value stored to 'newz' during its initialization is never read |
Press '?' to see keyboard shortcuts
Keyboard shortcuts:
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 | geom->GetCDCWires(cdcwires); |
333 | // geom->GetCDCRmid(cdc_rmid); // THIS ISN'T IMPLEMENTED!! |
334 | // extract the "mean" radius of each ring from the wire data |
335 | for(uint ring=0; ring<cdcwires.size(); ring++) |
336 | cdc_rmid.push_back( cdcwires[ring][0]->origin.Perp() ); |
337 | |
338 | // Outer detector geometry parameters |
339 | geom->GetFCALZ(dFCALz); |
340 | if (geom->GetDIRCZ(dDIRCz)==false) dDIRCz=1000.; |
341 | |
342 | vector<double>tof_face; |
343 | geom->Get("//section/composition/posXYZ[@volume='ForwardTOF']/@X_Y_Z", |
344 | tof_face); |
345 | vector<double>tof_plane; |
346 | geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='0']", tof_plane); |
347 | dTOFz=tof_face[2]+tof_plane[2]; |
348 | geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='1']", tof_plane); |
349 | dTOFz+=tof_face[2]+tof_plane[2]; |
350 | dTOFz*=0.5; // mid plane between tof planes |
351 | geom->GetTRDZ(dTRDz_vec); // TRD planes |
352 | |
353 | |
354 | // Get start counter geometry; |
355 | if (geom->GetStartCounterGeom(sc_pos, sc_norm)){ |
356 | // Create vector of direction vectors in scintillator planes |
357 | for (int i=0;i<30;i++){ |
358 | vector<DVector3>temp; |
359 | for (unsigned int j=0;j<sc_pos[i].size()-1;j++){ |
360 | double dx=sc_pos[i][j+1].x()-sc_pos[i][j].x(); |
361 | double dy=sc_pos[i][j+1].y()-sc_pos[i][j].y(); |
362 | double dz=sc_pos[i][j+1].z()-sc_pos[i][j].z(); |
363 | temp.push_back(DVector3(dx/dz,dy/dz,1.)); |
364 | } |
365 | sc_dir.push_back(temp); |
366 | } |
367 | SC_END_NOSE_Z=sc_pos[0][12].z(); |
368 | SC_BARREL_R2=sc_pos[0][0].Perp2(); |
369 | SC_PHI_SECTOR1=sc_pos[0][0].Phi(); |
370 | } |
371 | |
372 | // Get z positions of fdc wire planes |
373 | geom->GetFDCZ(fdc_z_wires); |
374 | // for now, assume the z extent of a package is the difference between the positions |
375 | // of the two wire planes. save half of this distance |
376 | fdc_package_size = (fdc_z_wires[1]-fdc_z_wires[0]) / 2.; |
377 | geom->GetFDCRmin(fdc_rmin_packages); |
378 | geom->GetFDCRmax(fdc_rmax); |
379 | |
380 | ADD_VERTEX_POINT=false; |
381 | gPARMS->SetDefaultParameter("KALMAN:ADD_VERTEX_POINT", ADD_VERTEX_POINT); |
382 | |
383 | THETA_CUT=60.0; |
384 | gPARMS->SetDefaultParameter("KALMAN:THETA_CUT", THETA_CUT); |
385 | |
386 | RING_TO_SKIP=0; |
387 | gPARMS->SetDefaultParameter("KALMAN:RING_TO_SKIP",RING_TO_SKIP); |
388 | |
389 | PLANE_TO_SKIP=0; |
390 | gPARMS->SetDefaultParameter("KALMAN:PLANE_TO_SKIP",PLANE_TO_SKIP); |
391 | |
392 | MINIMUM_HIT_FRACTION=0.7; |
393 | gPARMS->SetDefaultParameter("KALMAN:MINIMUM_HIT_FRACTION",MINIMUM_HIT_FRACTION); |
394 | MIN_HITS_FOR_REFIT=6; |
395 | gPARMS->SetDefaultParameter("KALMAN:MIN_HITS_FOR_REFIT", MIN_HITS_FOR_REFIT); |
396 | PHOTON_ENERGY_CUTOFF=0.125; |
397 | gPARMS->SetDefaultParameter("KALMAN:PHOTON_ENERGY_CUTOFF", |
398 | PHOTON_ENERGY_CUTOFF); |
399 | |
400 | USE_FDC_HITS=true; |
401 | gPARMS->SetDefaultParameter("TRKFIT:USE_FDC_HITS",USE_FDC_HITS); |
402 | USE_CDC_HITS=true; |
403 | gPARMS->SetDefaultParameter("TRKFIT:USE_CDC_HITS",USE_CDC_HITS); |
404 | USE_TRD_HITS=false; |
405 | gPARMS->SetDefaultParameter("TRKFIT:USE_TRD_HITS",USE_TRD_HITS); |
406 | USE_TRD_DRIFT_TIMES=true; |
407 | gPARMS->SetDefaultParameter("TRKFIT:USE_TRD_DRIFT_TIMES",USE_TRD_DRIFT_TIMES); |
408 | USE_GEM_HITS=false; |
409 | gPARMS->SetDefaultParameter("TRKFIT:USE_GEM_HITS",USE_GEM_HITS); |
410 | |
411 | |
412 | // Flag to enable calculation of alignment derivatives |
413 | ALIGNMENT=false; |
414 | gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT",ALIGNMENT); |
415 | |
416 | ALIGNMENT_FORWARD=false; |
417 | gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT_FORWARD",ALIGNMENT_FORWARD); |
418 | |
419 | ALIGNMENT_CENTRAL=false; |
420 | gPARMS->SetDefaultParameter("TRKFIT:ALIGNMENT_CENTRAL",ALIGNMENT_CENTRAL); |
421 | |
422 | if(ALIGNMENT){ALIGNMENT_FORWARD=true;ALIGNMENT_CENTRAL=true;} |
423 | |
424 | DEBUG_HISTS=false; |
425 | gPARMS->SetDefaultParameter("KALMAN:DEBUG_HISTS", DEBUG_HISTS); |
426 | |
427 | DEBUG_LEVEL=0; |
428 | gPARMS->SetDefaultParameter("KALMAN:DEBUG_LEVEL", DEBUG_LEVEL); |
429 | |
430 | USE_T0_FROM_WIRES=0; |
431 | gPARMS->SetDefaultParameter("KALMAN:USE_T0_FROM_WIRES", USE_T0_FROM_WIRES); |
432 | |
433 | ESTIMATE_T0_TB=false; |
434 | gPARMS->SetDefaultParameter("KALMAN:ESTIMATE_T0_TB",ESTIMATE_T0_TB); |
435 | |
436 | ENABLE_BOUNDARY_CHECK=true; |
437 | gPARMS->SetDefaultParameter("GEOM:ENABLE_BOUNDARY_CHECK", |
438 | ENABLE_BOUNDARY_CHECK); |
439 | |
440 | USE_MULS_COVARIANCE=true; |
441 | gPARMS->SetDefaultParameter("TRKFIT:USE_MULS_COVARIANCE", |
442 | USE_MULS_COVARIANCE); |
443 | |
444 | USE_PASS1_TIME_MODE=false; |
445 | gPARMS->SetDefaultParameter("KALMAN:USE_PASS1_TIME_MODE",USE_PASS1_TIME_MODE); |
446 | |
447 | USE_FDC_DRIFT_TIMES=true; |
448 | gPARMS->SetDefaultParameter("TRKFIT:USE_FDC_DRIFT_TIMES", |
449 | USE_FDC_DRIFT_TIMES); |
450 | |
451 | RECOVER_BROKEN_TRACKS=true; |
452 | gPARMS->SetDefaultParameter("KALMAN:RECOVER_BROKEN_TRACKS",RECOVER_BROKEN_TRACKS); |
453 | |
454 | NUM_CDC_SIGMA_CUT=5.0; |
455 | NUM_FDC_SIGMA_CUT=5.0; |
456 | gPARMS->SetDefaultParameter("KALMAN:NUM_CDC_SIGMA_CUT",NUM_CDC_SIGMA_CUT, |
457 | "maximum distance in number of sigmas away from projection to accept cdc hit"); |
458 | gPARMS->SetDefaultParameter("KALMAN:NUM_FDC_SIGMA_CUT",NUM_FDC_SIGMA_CUT, |
459 | "maximum distance in number of sigmas away from projection to accept fdc hit"); |
460 | |
461 | ANNEAL_SCALE=9.0; |
462 | ANNEAL_POW_CONST=1.5; |
463 | gPARMS->SetDefaultParameter("KALMAN:ANNEAL_SCALE",ANNEAL_SCALE, |
464 | "Scale factor for annealing"); |
465 | gPARMS->SetDefaultParameter("KALMAN:ANNEAL_POW_CONST",ANNEAL_POW_CONST, |
466 | "Annealing parameter"); |
467 | FORWARD_ANNEAL_SCALE=9.; |
468 | FORWARD_ANNEAL_POW_CONST=1.5; |
469 | gPARMS->SetDefaultParameter("KALMAN:FORWARD_ANNEAL_SCALE", |
470 | FORWARD_ANNEAL_SCALE, |
471 | "Scale factor for annealing"); |
472 | gPARMS->SetDefaultParameter("KALMAN:FORWARD_ANNEAL_POW_CONST", |
473 | FORWARD_ANNEAL_POW_CONST, |
474 | "Annealing parameter"); |
475 | |
476 | FORWARD_PARMS_COV=false; |
477 | gPARMS->SetDefaultParameter("KALMAN:FORWARD_PARMS_COV",FORWARD_PARMS_COV); |
478 | |
479 | CDC_VAR_SCALE_FACTOR=1.; |
480 | gPARMS->SetDefaultParameter("KALMAN:CDC_VAR_SCALE_FACTOR",CDC_VAR_SCALE_FACTOR); |
481 | CDC_T_DRIFT_MIN=-12.; // One f125 clock = 8 ns |
482 | gPARMS->SetDefaultParameter("KALMAN:CDC_T_DRIFT_MIN",CDC_T_DRIFT_MIN); |
483 | |
484 | MOLIERE_FRACTION=0.9; |
485 | gPARMS->SetDefaultParameter("KALMAN:MOLIERE_FRACTION",MOLIERE_FRACTION); |
486 | MS_SCALE_FACTOR=2.0; |
487 | gPARMS->SetDefaultParameter("KALMAN:MS_SCALE_FACTOR",MS_SCALE_FACTOR); |
488 | MOLIERE_RATIO1=0.5/(1.-MOLIERE_FRACTION); |
489 | MOLIERE_RATIO2=MS_SCALE_FACTOR*1.e-6/(1.+MOLIERE_FRACTION*MOLIERE_FRACTION); //scale_factor/(1+F*F) |
490 | |
491 | COVARIANCE_SCALE_FACTOR_CENTRAL=2.0; |
492 | gPARMS->SetDefaultParameter("KALMAN:COVARIANCE_SCALE_FACTOR_CENTRAL", |
493 | COVARIANCE_SCALE_FACTOR_CENTRAL); |
494 | |
495 | COVARIANCE_SCALE_FACTOR_FORWARD=2.0; |
496 | gPARMS->SetDefaultParameter("KALMAN:COVARIANCE_SCALE_FACTOR_FORWARD", |
497 | COVARIANCE_SCALE_FACTOR_FORWARD); |
498 | |
499 | WRITE_ML_TRAINING_OUTPUT=false; |
500 | gPARMS->SetDefaultParameter("KALMAN:WRITE_ML_TRAINING_OUTPUT", |
501 | WRITE_ML_TRAINING_OUTPUT); |
502 | |
503 | if (WRITE_ML_TRAINING_OUTPUT){ |
504 | mlfile.open("mltraining.dat"); |
505 | } |
506 | |
507 | |
508 | DApplication* dapp = dynamic_cast<DApplication*>(loop->GetJApplication()); |
509 | JCalibration *jcalib = dapp->GetJCalibration((loop->GetJEvent()).GetRunNumber()); |
510 | vector< map<string, double> > tvals; |
511 | cdc_drift_table.clear(); |
512 | if (jcalib->Get("CDC/cdc_drift_table", tvals)==false){ |
513 | for(unsigned int i=0; i<tvals.size(); i++){ |
514 | map<string, double> &row = tvals[i]; |
515 | cdc_drift_table.push_back(1000.*row["t"]); |
516 | } |
517 | } |
518 | else{ |
519 | jerr << " CDC time-to-distance table not available... bailing..." << endl; |
520 | exit(0); |
521 | } |
522 | |
523 | int straw_number[28]={42,42,54,54,66,66,80,80,93,93,106,106, |
524 | 123,123,135,135,146,146,158,158,170,170, |
525 | 182,182,197,197,209,209}; |
526 | max_sag.clear(); |
527 | sag_phi_offset.clear(); |
528 | int straw_count=0,ring_count=0; |
529 | if (jcalib->Get("CDC/sag_parameters", tvals)==false){ |
530 | vector<double>temp,temp2; |
531 | for(unsigned int i=0; i<tvals.size(); i++){ |
532 | map<string, double> &row = tvals[i]; |
533 | |
534 | temp.push_back(row["offset"]); |
535 | temp2.push_back(row["phi"]); |
536 | |
537 | straw_count++; |
538 | if (straw_count==straw_number[ring_count]){ |
539 | max_sag.push_back(temp); |
540 | sag_phi_offset.push_back(temp2); |
541 | temp.clear(); |
542 | temp2.clear(); |
543 | straw_count=0; |
544 | ring_count++; |
545 | } |
546 | } |
547 | } |
548 | |
549 | if (jcalib->Get("CDC/drift_parameters", tvals)==false){ |
550 | map<string, double> &row = tvals[0]; // long drift side |
551 | long_drift_func[0][0]=row["a1"]; |
552 | long_drift_func[0][1]=row["a2"]; |
553 | long_drift_func[0][2]=row["a3"]; |
554 | long_drift_func[1][0]=row["b1"]; |
555 | long_drift_func[1][1]=row["b2"]; |
556 | long_drift_func[1][2]=row["b3"]; |
557 | long_drift_func[2][0]=row["c1"]; |
558 | long_drift_func[2][1]=row["c2"]; |
559 | long_drift_func[2][2]=row["c3"]; |
560 | long_drift_Bscale_par1=row["B1"]; |
561 | long_drift_Bscale_par2=row["B2"]; |
562 | |
563 | row = tvals[1]; // short drift side |
564 | short_drift_func[0][0]=row["a1"]; |
565 | short_drift_func[0][1]=row["a2"]; |
566 | short_drift_func[0][2]=row["a3"]; |
567 | short_drift_func[1][0]=row["b1"]; |
568 | short_drift_func[1][1]=row["b2"]; |
569 | short_drift_func[1][2]=row["b3"]; |
570 | short_drift_func[2][0]=row["c1"]; |
571 | short_drift_func[2][1]=row["c2"]; |
572 | short_drift_func[2][2]=row["c3"]; |
573 | short_drift_Bscale_par1=row["B1"]; |
574 | short_drift_Bscale_par2=row["B2"]; |
575 | } |
576 | |
577 | map<string, double> cdc_drift_parms; |
578 | jcalib->Get("CDC/cdc_drift_parms", cdc_drift_parms); |
579 | CDC_DRIFT_BSCALE_PAR1 = cdc_drift_parms["bscale_par1"]; |
580 | CDC_DRIFT_BSCALE_PAR2 = cdc_drift_parms["bscale_par2"]; |
581 | |
582 | map<string, double> cdc_res_parms; |
583 | jcalib->Get("CDC/cdc_resolution_parms_v2", cdc_res_parms); |
584 | CDC_RES_PAR1 = cdc_res_parms["res_par1"]; |
585 | CDC_RES_PAR2 = cdc_res_parms["res_par2"]; |
586 | CDC_RES_PAR3 = cdc_res_parms["res_par3"]; |
587 | |
588 | // Parameters for correcting for deflection due to Lorentz force |
589 | map<string,double>lorentz_parms; |
590 | jcalib->Get("FDC/lorentz_deflection_parms",lorentz_parms); |
591 | LORENTZ_NR_PAR1=lorentz_parms["nr_par1"]; |
592 | LORENTZ_NR_PAR2=lorentz_parms["nr_par2"]; |
593 | LORENTZ_NZ_PAR1=lorentz_parms["nz_par1"]; |
594 | LORENTZ_NZ_PAR2=lorentz_parms["nz_par2"]; |
595 | |
596 | // Parameters for accounting for variation in drift distance from FDC |
597 | map<string,double>drift_res_parms; |
598 | jcalib->Get("FDC/drift_resolution_parms",drift_res_parms); |
599 | DRIFT_RES_PARMS[0]=drift_res_parms["p0"]; |
600 | DRIFT_RES_PARMS[1]=drift_res_parms["p1"]; |
601 | DRIFT_RES_PARMS[2]=drift_res_parms["p2"]; |
602 | map<string,double>drift_res_ext; |
603 | jcalib->Get("FDC/drift_resolution_ext",drift_res_ext); |
604 | DRIFT_RES_PARMS[3]=drift_res_ext["t_low"]; |
605 | DRIFT_RES_PARMS[4]=drift_res_ext["t_high"]; |
606 | DRIFT_RES_PARMS[5]=drift_res_ext["res_slope"]; |
607 | |
608 | // Time-to-distance function parameters for FDC |
609 | map<string,double>drift_func_parms; |
610 | jcalib->Get("FDC/drift_function_parms",drift_func_parms); |
611 | DRIFT_FUNC_PARMS[0]=drift_func_parms["p0"]; |
612 | DRIFT_FUNC_PARMS[1]=drift_func_parms["p1"]; |
613 | DRIFT_FUNC_PARMS[2]=drift_func_parms["p2"]; |
614 | DRIFT_FUNC_PARMS[3]=drift_func_parms["p3"]; |
615 | DRIFT_FUNC_PARMS[4]=1000.; |
616 | DRIFT_FUNC_PARMS[5]=0.; |
617 | map<string,double>drift_func_ext; |
618 | if (jcalib->Get("FDC/drift_function_ext",drift_func_ext)==false){ |
619 | DRIFT_FUNC_PARMS[4]=drift_func_ext["p4"]; |
620 | DRIFT_FUNC_PARMS[5]=drift_func_ext["p5"]; |
621 | } |
622 | // Factors for taking care of B-dependence of drift time for FDC |
623 | map<string, double> fdc_drift_parms; |
624 | jcalib->Get("FDC/fdc_drift_parms", fdc_drift_parms); |
625 | FDC_DRIFT_BSCALE_PAR1 = fdc_drift_parms["bscale_par1"]; |
626 | FDC_DRIFT_BSCALE_PAR2 = fdc_drift_parms["bscale_par2"]; |
627 | |
628 | |
629 | /* |
630 | if (jcalib->Get("FDC/fdc_drift2", tvals)==false){ |
631 | for(unsigned int i=0; i<tvals.size(); i++){ |
632 | map<string, float> &row = tvals[i]; |
633 | iter_float iter = row.begin(); |
634 | fdc_drift_table[i] = iter->second; |
635 | } |
636 | } |
637 | else{ |
638 | jerr << " FDC time-to-distance table not available... bailing..." << endl; |
639 | exit(0); |
640 | } |
641 | */ |
642 | |
643 | for (unsigned int i=0;i<5;i++)I5x5(i,i)=1.; |
644 | |
645 | |
646 | // center of the target |
647 | map<string, double> targetparms; |
648 | if (jcalib->Get("TARGET/target_parms",targetparms)==false){ |
649 | TARGET_Z = targetparms["TARGET_Z_POSITION"]; |
650 | } |
651 | else{ |
652 | geom->GetTargetZ(TARGET_Z); |
653 | } |
654 | if (ADD_VERTEX_POINT){ |
655 | gPARMS->SetDefaultParameter("KALMAN:VERTEX_POSITION",TARGET_Z); |
656 | } |
657 | |
658 | // Beam position and direction |
659 | map<string, double> beam_vals; |
660 | jcalib->Get("PHOTON_BEAM/beam_spot",beam_vals); |
661 | beam_center.Set(beam_vals["x"],beam_vals["y"]); |
662 | beam_dir.Set(beam_vals["dxdz"],beam_vals["dydz"]); |
663 | |
664 | if(print_messages) |
665 | jout << " Beam spot: x=" << beam_center.X() << " y=" << beam_center.Y() |
666 | << " z=" << beam_vals["z"] |
667 | << " dx/dz=" << beam_dir.X() << " dy/dz=" << beam_dir.Y() << endl; |
668 | beam_z0=beam_vals["z"]; |
669 | |
670 | // Inform user of some configuration settings |
671 | static bool config_printed = false; |
672 | if(!config_printed){ |
673 | config_printed = true; |
674 | stringstream ss; |
675 | ss << "vertex constraint: " ; |
676 | if(ADD_VERTEX_POINT){ |
677 | ss << "z = " << TARGET_Z << "cm" << endl; |
678 | }else{ |
679 | ss << "<none>" << endl; |
680 | } |
681 | jout << ss.str(); |
682 | } // config_printed |
683 | |
684 | if(DEBUG_HISTS){ |
685 | for (auto i=0; i < 46; i++){ |
686 | double min = -10., max=10.; |
687 | if(i%23<12) {min=-5; max=5;} |
688 | if(i<23)alignDerivHists[i]=new TH1I(Form("CentralDeriv%i",i),Form("CentralDeriv%i",i),200, min, max); |
689 | else alignDerivHists[i]=new TH1I(Form("ForwardDeriv%i",i%23),Form("ForwardDeriv%i",i%23),200, min, max); |
690 | } |
691 | brentCheckHists[0]=new TH2I("ForwardBrentCheck","DOCA vs ds", 100, -5., 5., 100, 0.0, 1.5); |
692 | brentCheckHists[1]=new TH2I("CentralBrentCheck","DOCA vs ds", 100, -5., 5., 100, 0.0, 1.5); |
693 | } |
694 | |
695 | dResourcePool_TMatrixFSym = std::make_shared<DResourcePool<TMatrixFSym>>(); |
696 | dResourcePool_TMatrixFSym->Set_ControlParams(20, 20, 50); |
697 | |
698 | my_fdchits.reserve(24); |
699 | my_cdchits.reserve(28); |
700 | fdc_updates.reserve(24); |
701 | cdc_updates.reserve(28); |
702 | cdc_used_in_fit.reserve(28); |
703 | fdc_used_in_fit.reserve(24); |
704 | } |
705 | |
706 | //----------------- |
707 | // ResetKalmanSIMD |
708 | //----------------- |
709 | void DTrackFitterKalmanSIMD::ResetKalmanSIMD(void) |
710 | { |
711 | last_material_map=0; |
712 | |
713 | for (unsigned int i=0;i<my_cdchits.size();i++){ |
714 | delete my_cdchits[i]; |
715 | } |
716 | for (unsigned int i=0;i<my_fdchits.size();i++){ |
717 | delete my_fdchits[i]; |
718 | } |
719 | central_traj.clear(); |
720 | forward_traj.clear(); |
721 | my_fdchits.clear(); |
722 | my_cdchits.clear(); |
723 | fdc_updates.clear(); |
724 | cdc_updates.clear(); |
725 | fdc_used_in_fit.clear(); |
726 | cdc_used_in_fit.clear(); |
727 | got_trd_gem_hits=false; |
728 | |
729 | cov.clear(); |
730 | fcov.clear(); |
731 | |
732 | len = 0.0; |
733 | ftime=0.0; |
734 | var_ftime=0.0; |
735 | x_=0.,y_=0.,tx_=0.,ty_=0.,q_over_p_ = 0.0; |
736 | z_=0.,phi_=0.,tanl_=0.,q_over_pt_ =0, D_= 0.0; |
737 | chisq_ = 0.0; |
738 | ndf_ = 0; |
739 | MASS=0.13957; |
740 | mass2=MASS*MASS; |
741 | Bx=By=0.; |
742 | Bz=2.; |
743 | dBxdx=0.,dBxdy=0.,dBxdz=0.,dBydx=0.,dBydy=0.,dBydy=0.,dBzdx=0.,dBzdy=0.,dBzdz=0.; |
744 | // Step sizes |
745 | mStepSizeS=2.0; |
746 | mStepSizeZ=2.0; |
747 | //mStepSizeZ=0.5; |
748 | |
749 | //if (fit_type==kTimeBased){ |
750 | // mStepSizeS=0.5; |
751 | // mStepSizeZ=0.5; |
752 | // } |
753 | |
754 | |
755 | mT0=0.,mT0MinimumDriftTime=1e6; |
756 | mVarT0=25.; |
757 | |
758 | mCDCInternalStepSize=0.5; |
759 | //mCDCInternalStepSize=1.0; |
760 | //mCentralStepSize=0.75; |
761 | mCentralStepSize=0.75; |
762 | |
763 | mT0Detector=SYS_CDC; |
764 | |
765 | IsHadron=true; |
766 | IsElectron=false; |
767 | IsPositron=false; |
768 | |
769 | PT_MIN=0.01; |
770 | Q_OVER_P_MAX=100.; |
771 | |
772 | // These variables provide the approximate location along the trajectory |
773 | // where there is an indication of a kink in the track |
774 | break_point_fdc_index=0; |
775 | break_point_cdc_index=0; |
776 | break_point_step_index=0; |
777 | |
778 | } |
779 | |
780 | //----------------- |
781 | // FitTrack |
782 | //----------------- |
783 | DTrackFitter::fit_status_t DTrackFitterKalmanSIMD::FitTrack(void) |
784 | { |
785 | // Reset member data and free an memory associated with the last fit, |
786 | // but some of which only for wire-based fits |
787 | ResetKalmanSIMD(); |
788 | |
789 | // Check that we have enough FDC and CDC hits to proceed |
790 | if (cdchits.size()==0 && fdchits.size()<4) return kFitNotDone; |
791 | if (cdchits.size()+fdchits.size() < 6) return kFitNotDone; |
792 | |
793 | // Copy hits from base class into structures specific to DTrackFitterKalmanSIMD |
794 | if (USE_CDC_HITS) |
795 | for(unsigned int i=0; i<cdchits.size(); i++)AddCDCHit(cdchits[i]); |
796 | if (USE_FDC_HITS) |
797 | for(unsigned int i=0; i<fdchits.size(); i++)AddFDCHit(fdchits[i]); |
798 | if (USE_TRD_HITS){ |
799 | for(unsigned int i=0; i<trdhits.size(); i++)AddTRDHit(trdhits[i]); |
800 | if (trdhits.size()>0){ |
801 | //_DBG_ << "Got TRD" <<endl; |
802 | got_trd_gem_hits=true; |
803 | } |
804 | } |
805 | if (USE_GEM_HITS){ |
806 | for(unsigned int i=0; i<gemhits.size(); i++)AddGEMHit(gemhits[i]); |
807 | if (gemhits.size()>0){ |
808 | //_DBG_ << " Got GEM" << endl; |
809 | got_trd_gem_hits=true; |
810 | } |
811 | } |
812 | |
813 | unsigned int num_good_cdchits=my_cdchits.size(); |
814 | unsigned int num_good_fdchits=my_fdchits.size(); |
815 | |
816 | // keep track of the range of detector elements that could be hit |
817 | // for calculating the number of expected hits later on |
818 | //int min_cdc_ring=-1, max_cdc_ring=-1; |
819 | |
820 | // Order the cdc hits by ring number |
821 | if (num_good_cdchits>0){ |
822 | stable_sort(my_cdchits.begin(),my_cdchits.end(),DKalmanSIMDCDCHit_cmp); |
823 | |
824 | //min_cdc_ring = my_cdchits[0]->hit->wire->ring; |
825 | //max_cdc_ring = my_cdchits[my_cdchits.size()-1]->hit->wire->ring; |
826 | |
827 | // Look for multiple hits on the same wire |
828 | for (unsigned int i=0;i<my_cdchits.size()-1;i++){ |
829 | if (my_cdchits[i]->hit->wire->ring==my_cdchits[i+1]->hit->wire->ring && |
830 | my_cdchits[i]->hit->wire->straw==my_cdchits[i+1]->hit->wire->straw){ |
831 | num_good_cdchits--; |
832 | if (my_cdchits[i]->tdrift<my_cdchits[i+1]->tdrift){ |
833 | my_cdchits[i+1]->status=late_hit; |
834 | } |
835 | else{ |
836 | my_cdchits[i]->status=late_hit; |
837 | } |
838 | } |
839 | } |
840 | |
841 | } |
842 | // Order the fdc hits by z |
843 | if (num_good_fdchits>0){ |
844 | stable_sort(my_fdchits.begin(),my_fdchits.end(),DKalmanSIMDFDCHit_cmp); |
845 | |
846 | // Look for multiple hits on the same wire |
847 | for (unsigned int i=0;i<my_fdchits.size()-1;i++){ |
848 | if (my_fdchits[i]->hit==NULL__null || my_fdchits[i+1]->hit==NULL__null) continue; |
849 | if (my_fdchits[i]->hit->wire->layer==my_fdchits[i+1]->hit->wire->layer && |
850 | my_fdchits[i]->hit->wire->wire==my_fdchits[i+1]->hit->wire->wire){ |
851 | num_good_fdchits--; |
852 | if (fabs(my_fdchits[i]->t-my_fdchits[i+1]->t)<EPS3.0e-8){ |
853 | double tsum_1=my_fdchits[i]->hit->t_u+my_fdchits[i]->hit->t_v; |
854 | double tsum_2=my_fdchits[i+1]->hit->t_u+my_fdchits[i+1]->hit->t_v; |
855 | if (tsum_1<tsum_2){ |
856 | my_fdchits[i+1]->status=late_hit; |
857 | } |
858 | else{ |
859 | my_fdchits[i]->status=late_hit; |
860 | } |
861 | } |
862 | else if (my_fdchits[i]->t<my_fdchits[i+1]->t){ |
863 | my_fdchits[i+1]->status=late_hit; |
864 | } |
865 | else{ |
866 | my_fdchits[i]->status=late_hit; |
867 | } |
868 | } |
869 | } |
870 | } |
871 | if (num_good_cdchits==0 && num_good_fdchits<4) return kFitNotDone; |
872 | if (num_good_cdchits+num_good_fdchits < 6) return kFitNotDone; |
873 | |
874 | // Create vectors of updates (from hits) to S and C |
875 | if (my_cdchits.size()>0){ |
876 | cdc_updates=vector<DKalmanUpdate_t>(my_cdchits.size()); |
877 | // Initialize vector to keep track of whether or not a hit is used in |
878 | // the fit |
879 | cdc_used_in_fit=vector<bool>(my_cdchits.size()); |
880 | } |
881 | if (my_fdchits.size()>0){ |
882 | fdc_updates=vector<DKalmanUpdate_t>(my_fdchits.size()); |
883 | // Initialize vector to keep track of whether or not a hit is used in |
884 | // the fit |
885 | fdc_used_in_fit=vector<bool>(my_fdchits.size()); |
886 | } |
887 | |
888 | // start time and variance |
889 | if (fit_type==kTimeBased){ |
890 | mT0=input_params.t0(); |
891 | switch(input_params.t0_detector()){ |
892 | case SYS_TOF: |
893 | mVarT0=0.01; |
894 | break; |
895 | case SYS_CDC: |
896 | mVarT0=7.5; |
897 | break; |
898 | case SYS_FDC: |
899 | mVarT0=7.5; |
900 | break; |
901 | case SYS_BCAL: |
902 | mVarT0=0.25; |
903 | break; |
904 | default: |
905 | mVarT0=0.09; |
906 | break; |
907 | } |
908 | } |
909 | |
910 | //_DBG_ << SystemName(input_params.t0_detector()) << " " << mT0 <<endl; |
911 | |
912 | //Set the mass |
913 | MASS=input_params.mass(); |
914 | mass2=MASS*MASS; |
915 | m_ratio=ELECTRON_MASS0.000511/MASS; |
916 | m_ratio_sq=m_ratio*m_ratio; |
917 | |
918 | // Is this particle an electron or positron? |
919 | if (MASS<0.001){ |
920 | IsHadron=false; |
921 | if (input_params.charge()<0.) IsElectron=true; |
922 | else IsPositron=true; |
923 | } |
924 | if (DEBUG_LEVEL>0) |
925 | { |
926 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<926<<" " << "------Starting " |
927 | <<(fit_type==kTimeBased?"Time-based":"Wire-based") |
928 | << " Fit with " << my_fdchits.size() << " FDC hits and " |
929 | << my_cdchits.size() << " CDC hits.-------" <<endl; |
930 | if (fit_type==kTimeBased){ |
931 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<931<<" " << " Using t0=" << mT0 << " from DET=" |
932 | << input_params.t0_detector() <<endl; |
933 | } |
934 | } |
935 | // Do the fit |
936 | jerror_t error = KalmanLoop(); |
937 | if (error!=NOERROR){ |
938 | if (DEBUG_LEVEL>0) |
939 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<939<<" " << "Fit failed with Error = " << error <<endl; |
940 | return kFitFailed; |
941 | } |
942 | |
943 | // Copy fit results into DTrackFitter base-class data members |
944 | DVector3 mom,pos; |
945 | GetPosition(pos); |
946 | GetMomentum(mom); |
947 | double charge = GetCharge(); |
948 | fit_params.setPosition(pos); |
949 | fit_params.setMomentum(mom); |
950 | fit_params.setTime(mT0MinimumDriftTime); |
951 | fit_params.setPID(IDTrack(charge, MASS)); |
952 | fit_params.setT0(mT0MinimumDriftTime,4.,mT0Detector); |
953 | |
954 | if (DEBUG_LEVEL>0){ |
955 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<955<<" " << "----- Pass: " |
956 | << (fit_type==kTimeBased?"Time-based ---":"Wire-based ---") |
957 | << " Mass: " << MASS |
958 | << " p=" << mom.Mag() |
959 | << " theta=" << 90.0-180./M_PI3.14159265358979323846*atan(tanl_) |
960 | << " vertex=(" << x_ << "," << y_ << "," << z_<<")" |
961 | << " chi2=" << chisq_ |
962 | <<endl; |
963 | if(DEBUG_LEVEL>3){ |
964 | //Dump pulls |
965 | for (unsigned int iPull = 0; iPull < pulls.size(); iPull++){ |
966 | if (pulls[iPull].cdc_hit != NULL__null){ |
967 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<967<<" " << " ring: " << pulls[iPull].cdc_hit->wire->ring |
968 | << " straw: " << pulls[iPull].cdc_hit->wire->straw |
969 | << " Residual: " << pulls[iPull].resi |
970 | << " Err: " << pulls[iPull].err |
971 | << " tdrift: " << pulls[iPull].tdrift |
972 | << " doca: " << pulls[iPull].d |
973 | << " docaphi: " << pulls[iPull].docaphi |
974 | << " z: " << pulls[iPull].z |
975 | << " cos(theta_rel): " << pulls[iPull].cosThetaRel |
976 | << " tcorr: " << pulls[iPull].tcorr |
977 | << endl; |
978 | } |
979 | } |
980 | } |
981 | } |
982 | |
983 | DMatrixDSym errMatrix(5); |
984 | // Fill the tracking error matrix and the one needed for kinematic fitting |
985 | if (fcov.size()!=0){ |
986 | // We MUST fill the entire matrix (not just upper right) even though |
987 | // this is a DMatrixDSym |
988 | for (unsigned int i=0;i<5;i++){ |
989 | for (unsigned int j=0;j<5;j++){ |
990 | errMatrix(i,j)=fcov[i][j]; |
991 | } |
992 | } |
993 | if (FORWARD_PARMS_COV){ |
994 | fit_params.setForwardParmFlag(true); |
995 | fit_params.setTrackingStateVector(x_,y_,tx_,ty_,q_over_p_); |
996 | |
997 | // Compute and fill the error matrix needed for kinematic fitting |
998 | fit_params.setErrorMatrix(Get7x7ErrorMatrixForward(errMatrix)); |
999 | } |
1000 | else { |
1001 | fit_params.setForwardParmFlag(false); |
1002 | fit_params.setTrackingStateVector(q_over_pt_,phi_,tanl_,D_,z_); |
1003 | |
1004 | // Compute and fill the error matrix needed for kinematic fitting |
1005 | fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix)); |
1006 | } |
1007 | } |
1008 | else if (cov.size()!=0){ |
1009 | fit_params.setForwardParmFlag(false); |
1010 | |
1011 | // We MUST fill the entire matrix (not just upper right) even though |
1012 | // this is a DMatrixDSym |
1013 | for (unsigned int i=0;i<5;i++){ |
1014 | for (unsigned int j=0;j<5;j++){ |
1015 | errMatrix(i,j)=cov[i][j]; |
1016 | } |
1017 | } |
1018 | fit_params.setTrackingStateVector(q_over_pt_,phi_,tanl_,D_,z_); |
1019 | |
1020 | // Compute and fill the error matrix needed for kinematic fitting |
1021 | fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix)); |
1022 | } |
1023 | auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource(); |
1024 | locTrackingCovarianceMatrix->ResizeTo(5, 5); |
1025 | for(unsigned int loc_i = 0; loc_i < 5; ++loc_i) |
1026 | { |
1027 | for(unsigned int loc_j = 0; loc_j < 5; ++loc_j) |
1028 | (*locTrackingCovarianceMatrix)(loc_i, loc_j) = errMatrix(loc_i, loc_j); |
1029 | |
1030 | } |
1031 | fit_params.setTrackingErrorMatrix(locTrackingCovarianceMatrix); |
1032 | this->chisq = GetChiSq(); |
1033 | this->Ndof = GetNDF(); |
1034 | fit_status = kFitSuccess; |
1035 | |
1036 | // figure out the number of expected hits for this track based on the final fit |
1037 | set<const DCDCWire *> expected_hit_straws; |
1038 | set<int> expected_hit_fdc_planes; |
1039 | |
1040 | for(uint i=0; i<extrapolations[SYS_CDC].size(); i++) { |
1041 | // figure out the radial position of the point to see which ring it's in |
1042 | double r = extrapolations[SYS_CDC][i].position.Perp(); |
1043 | uint ring=0; |
1044 | for(; ring<cdc_rmid.size(); ring++) { |
1045 | //_DBG_ << "Rs = " << r << " " << cdc_rmid[ring] << endl; |
1046 | if( (r<cdc_rmid[ring]-0.78) || (fabs(r-cdc_rmid[ring])<0.78) ) |
1047 | break; |
1048 | } |
1049 | if(ring == cdc_rmid.size()) ring--; |
1050 | //_DBG_ << "ring = " << ring << endl; |
1051 | //_DBG_ << "ring = " << ring << " stereo = " << cdcwires[ring][0]->stereo << endl; |
1052 | int best_straw=0; |
1053 | double best_dist_diff=fabs((extrapolations[SYS_CDC][i].position |
1054 | - cdcwires[ring][0]->origin).Mag()); |
1055 | // match based on straw center |
1056 | for(uint straw=1; straw<cdcwires[ring].size(); straw++) { |
1057 | DVector3 wire_position = cdcwires[ring][straw]->origin; // start with the nominal wire center |
1058 | // now take into account the z dependence due to the stereo angle |
1059 | double dz = extrapolations[SYS_CDC][i].position.Z() - cdcwires[ring][straw]->origin.Z(); |
1060 | double ds = dz*tan(cdcwires[ring][straw]->stereo); |
1061 | wire_position += DVector3(-ds*sin(cdcwires[ring][straw]->origin.Phi()), ds*cos(cdcwires[ring][straw]->origin.Phi()), dz); |
1062 | double diff = fabs((extrapolations[SYS_CDC][i].position |
1063 | - wire_position).Mag()); |
1064 | if( diff < best_dist_diff ) |
1065 | best_straw = straw; |
1066 | } |
1067 | |
1068 | expected_hit_straws.insert(cdcwires[ring][best_straw]); |
1069 | } |
1070 | |
1071 | for(uint i=0; i<extrapolations[SYS_FDC].size(); i++) { |
1072 | // check to make sure that the track goes through the sensitive region of the FDC |
1073 | // assume one hit per plane |
1074 | double z = extrapolations[SYS_FDC][i].position.Z(); |
1075 | double r = extrapolations[SYS_FDC][i].position.Perp(); |
1076 | |
1077 | // see if we're in the "sensitive area" of a package |
1078 | for(uint plane=0; plane<fdc_z_wires.size(); plane++) { |
1079 | int package = plane/6; |
1080 | if(fabs(z-fdc_z_wires[plane]) < fdc_package_size) { |
1081 | if( r<fdc_rmax && r>fdc_rmin_packages[package]) { |
1082 | expected_hit_fdc_planes.insert(plane); |
1083 | } |
1084 | break; // found the right plane |
1085 | } |
1086 | } |
1087 | } |
1088 | |
1089 | potential_cdc_hits_on_track = expected_hit_straws.size(); |
1090 | potential_fdc_hits_on_track = expected_hit_fdc_planes.size(); |
1091 | |
1092 | if(DEBUG_LEVEL>0) { |
1093 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<1093<<" " << " CDC hits/potential hits " << my_cdchits.size() << "/" << potential_cdc_hits_on_track |
1094 | << " FDC hits/potential hits " << my_fdchits.size() << "/" << potential_fdc_hits_on_track << endl; |
1095 | } |
1096 | |
1097 | //_DBG_ << "========= done!" << endl; |
1098 | |
1099 | return fit_status; |
1100 | } |
1101 | |
1102 | //----------------- |
1103 | // ChiSq |
1104 | //----------------- |
1105 | double DTrackFitterKalmanSIMD::ChiSq(fit_type_t fit_type, DReferenceTrajectory *rt, double *chisq_ptr, int *dof_ptr, vector<pull_t> *pulls_ptr) |
1106 | { |
1107 | // This simply returns whatever was left in for the chisq/NDF from the last fit. |
1108 | // Using a DReferenceTrajectory is not really appropriate here so the base class' |
1109 | // requirement of it should be reviewed. |
1110 | double chisq = GetChiSq(); |
1111 | unsigned int ndf = GetNDF(); |
1112 | |
1113 | if(chisq_ptr)*chisq_ptr = chisq; |
1114 | if(dof_ptr)*dof_ptr = int(ndf); |
1115 | if(pulls_ptr)*pulls_ptr = pulls; |
1116 | |
1117 | return chisq/double(ndf); |
1118 | } |
1119 | |
1120 | // Return the momentum at the distance of closest approach to the origin. |
1121 | inline void DTrackFitterKalmanSIMD::GetMomentum(DVector3 &mom){ |
1122 | double pt=1./fabs(q_over_pt_); |
1123 | mom.SetXYZ(pt*cos(phi_),pt*sin(phi_),pt*tanl_); |
1124 | } |
1125 | |
1126 | // Return the "vertex" position (position at which track crosses beam line) |
1127 | inline void DTrackFitterKalmanSIMD::GetPosition(DVector3 &pos){ |
1128 | DVector2 beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
1129 | pos.SetXYZ(x_+beam_pos.X(),y_+beam_pos.Y(),z_); |
1130 | } |
1131 | |
1132 | // Add GEM points |
1133 | void DTrackFitterKalmanSIMD::AddGEMHit(const DGEMPoint *gemhit){ |
1134 | DKalmanSIMDFDCHit_t *hit= new DKalmanSIMDFDCHit_t; |
1135 | |
1136 | hit->t=gemhit->time; |
1137 | hit->uwire=gemhit->x; |
1138 | hit->vstrip=gemhit->y; |
1139 | // From Justin (12/12/19): |
1140 | // DGEMPoint (GEM2 SRS, plane closest to the DIRC): |
1141 | // sigma_X = sigma_Y = 100 um |
1142 | hit->vvar=0.01*0.01; |
1143 | hit->uvar=hit->vvar; |
1144 | hit->z=gemhit->z; |
1145 | hit->cosa=1.; |
1146 | hit->sina=0.; |
1147 | hit->phiX=0.; |
1148 | hit->phiY=0.; |
1149 | hit->phiZ=0.; |
1150 | hit->nr=0.; |
1151 | hit->nz=0.; |
1152 | hit->status=gem_hit; |
1153 | hit->hit=NULL__null; |
1154 | |
1155 | my_fdchits.push_back(hit); |
1156 | } |
1157 | |
1158 | |
1159 | |
1160 | // Add TRD points |
1161 | void DTrackFitterKalmanSIMD::AddTRDHit(const DTRDPoint *trdhit){ |
1162 | DKalmanSIMDFDCHit_t *hit= new DKalmanSIMDFDCHit_t; |
1163 | |
1164 | hit->t=trdhit->time; |
1165 | hit->uwire=trdhit->x; |
1166 | hit->vstrip=trdhit->y; |
1167 | // From Justin (12/12/19): |
1168 | // sigma_X = 1 mm / sqrt(12) |
1169 | // sigma_Y = 300 um |
1170 | hit->vvar=0.03*0.03; |
1171 | hit->uvar=0.1*0.1/12.; |
1172 | hit->z=trdhit->z; |
1173 | hit->cosa=1.; |
1174 | hit->sina=0.; |
1175 | hit->phiX=0.; |
1176 | hit->phiY=0.; |
1177 | hit->phiZ=0.; |
1178 | hit->nr=0.; |
1179 | hit->nz=0.; |
1180 | hit->status=trd_hit; |
1181 | hit->hit=NULL__null; |
1182 | |
1183 | my_fdchits.push_back(hit); |
1184 | } |
1185 | |
1186 | // Add FDC hits |
1187 | void DTrackFitterKalmanSIMD::AddFDCHit(const DFDCPseudo *fdchit){ |
1188 | DKalmanSIMDFDCHit_t *hit= new DKalmanSIMDFDCHit_t; |
1189 | |
1190 | hit->t=fdchit->time; |
1191 | hit->uwire=fdchit->w; |
1192 | hit->vstrip=fdchit->s; |
1193 | hit->uvar=0.0833; |
1194 | hit->vvar=fdchit->ds*fdchit->ds; |
1195 | hit->z=fdchit->wire->origin.z(); |
1196 | hit->cosa=cos(fdchit->wire->angle); |
1197 | hit->sina=sin(fdchit->wire->angle); |
1198 | hit->phiX=fdchit->wire->angles.X(); |
1199 | hit->phiY=fdchit->wire->angles.Y(); |
1200 | hit->phiZ=fdchit->wire->angles.Z(); |
1201 | |
1202 | hit->nr=0.; |
1203 | hit->nz=0.; |
1204 | hit->dE=1e6*fdchit->dE; |
1205 | hit->hit=fdchit; |
1206 | hit->status=good_hit; |
1207 | |
1208 | my_fdchits.push_back(hit); |
1209 | } |
1210 | |
1211 | // Add CDC hits |
1212 | void DTrackFitterKalmanSIMD::AddCDCHit (const DCDCTrackHit *cdchit){ |
1213 | DKalmanSIMDCDCHit_t *hit= new DKalmanSIMDCDCHit_t; |
1214 | |
1215 | hit->hit=cdchit; |
1216 | hit->status=good_hit; |
1217 | hit->origin.Set(cdchit->wire->origin.x(),cdchit->wire->origin.y()); |
1218 | double one_over_uz=1./cdchit->wire->udir.z(); |
1219 | hit->dir.Set(one_over_uz*cdchit->wire->udir.x(), |
1220 | one_over_uz*cdchit->wire->udir.y()); |
1221 | hit->z0wire=cdchit->wire->origin.z(); |
1222 | hit->cosstereo=cos(cdchit->wire->stereo); |
1223 | hit->tdrift=cdchit->tdrift; |
1224 | my_cdchits.push_back(hit); |
1225 | } |
1226 | |
1227 | // Calculate the derivative of the state vector with respect to z |
1228 | jerror_t DTrackFitterKalmanSIMD::CalcDeriv(double z, |
1229 | const DMatrix5x1 &S, |
1230 | double dEdx, |
1231 | DMatrix5x1 &D){ |
1232 | double tx=S(state_tx),ty=S(state_ty); |
1233 | double q_over_p=S(state_q_over_p); |
1234 | |
1235 | // Turn off dEdx if the magnitude of the momentum drops below some cutoff |
1236 | if (fabs(q_over_p)>Q_OVER_P_MAX){ |
1237 | dEdx=0.; |
1238 | } |
1239 | // Try to keep the direction tangents from heading towards 90 degrees |
1240 | if (fabs(tx)>TAN_MAX10.) tx=TAN_MAX10.*(tx>0.0?1.:-1.); |
1241 | if (fabs(ty)>TAN_MAX10.) ty=TAN_MAX10.*(ty>0.0?1.:-1.); |
1242 | |
1243 | // useful combinations of terms |
1244 | double kq_over_p=qBr2p0.003*q_over_p; |
1245 | double tx2=tx*tx; |
1246 | double ty2=ty*ty; |
1247 | double txty=tx*ty; |
1248 | double one_plus_tx2=1.+tx2; |
1249 | double dsdz=sqrt(one_plus_tx2+ty2); |
1250 | double dtx_Bfac=ty*Bz+txty*Bx-one_plus_tx2*By; |
1251 | double dty_Bfac=Bx*(1.+ty2)-txty*By-tx*Bz; |
1252 | double kq_over_p_dsdz=kq_over_p*dsdz; |
1253 | |
1254 | // Derivative of S with respect to z |
1255 | D(state_x)=tx; |
1256 | D(state_y)=ty; |
1257 | D(state_tx)=kq_over_p_dsdz*dtx_Bfac; |
1258 | D(state_ty)=kq_over_p_dsdz*dty_Bfac; |
1259 | |
1260 | D(state_q_over_p)=0.; |
1261 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
1262 | double q_over_p_sq=q_over_p*q_over_p; |
1263 | double E=sqrt(1./q_over_p_sq+mass2); |
1264 | D(state_q_over_p)=-q_over_p_sq*q_over_p*E*dEdx*dsdz; |
1265 | } |
1266 | return NOERROR; |
1267 | } |
1268 | |
1269 | // Reference trajectory for forward tracks in CDC region |
1270 | // At each point we store the state vector and the Jacobian needed to get to |
1271 | //this state along z from the previous state. |
1272 | jerror_t DTrackFitterKalmanSIMD::SetCDCForwardReferenceTrajectory(DMatrix5x1 &S){ |
1273 | int i=0,forward_traj_length=forward_traj.size(); |
1274 | double z=z_; |
1275 | double r2=0.; |
1276 | bool stepped_to_boundary=false; |
1277 | |
1278 | // Magnetic field and gradient at beginning of trajectory |
1279 | //bfield->GetField(x_,y_,z_,Bx,By,Bz); |
1280 | bfield->GetFieldAndGradient(x_,y_,z_,Bx,By,Bz, |
1281 | dBxdx,dBxdy,dBxdz,dBydx, |
1282 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1283 | |
1284 | // Reset cdc status flags |
1285 | for (unsigned int i=0;i<my_cdchits.size();i++){ |
1286 | if (my_cdchits[i]->status!=late_hit) my_cdchits[i]->status=good_hit; |
1287 | } |
1288 | |
1289 | // Continue adding to the trajectory until we have reached the endplate |
1290 | // or the maximum radius |
1291 | while(z<endplate_z_downstream && z>cdc_origin[2] && |
1292 | r2<endplate_r2max && fabs(S(state_q_over_p))<Q_OVER_P_MAX |
1293 | && fabs(S(state_tx))<TAN_MAX10. |
1294 | && fabs(S(state_ty))<TAN_MAX10. |
1295 | ){ |
1296 | if (PropagateForwardCDC(forward_traj_length,i,z,r2,S,stepped_to_boundary) |
1297 | !=NOERROR) return UNRECOVERABLE_ERROR; |
1298 | } |
1299 | |
1300 | // Only use hits that would fall within the range of the reference trajectory |
1301 | /* |
1302 | for (unsigned int i=0;i<my_cdchits.size();i++){ |
1303 | DKalmanSIMDCDCHit_t *hit=my_cdchits[i]; |
1304 | double my_r2=(hit->origin+(z-hit->z0wire)*hit->dir).Mod2(); |
1305 | if (my_r2>r2) hit->status=bad_hit; |
1306 | } |
1307 | */ |
1308 | |
1309 | // If the current length of the trajectory deque is less than the previous |
1310 | // trajectory deque, remove the extra elements and shrink the deque |
1311 | if (i<(int)forward_traj.size()){ |
1312 | forward_traj_length=forward_traj.size(); |
1313 | for (int j=0;j<forward_traj_length-i;j++){ |
1314 | forward_traj.pop_front(); |
1315 | } |
1316 | } |
1317 | |
1318 | // return an error if there are not enough entries in the trajectory |
1319 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
1320 | |
1321 | if (DEBUG_LEVEL>20) |
1322 | { |
1323 | cout << "--- Forward cdc trajectory ---" <<endl; |
1324 | for (unsigned int m=0;m<forward_traj.size();m++){ |
1325 | // DMatrix5x1 S=*(forward_traj[m].S); |
1326 | DMatrix5x1 S=(forward_traj[m].S); |
1327 | double tx=S(state_tx),ty=S(state_ty); |
1328 | double phi=atan2(ty,tx); |
1329 | double cosphi=cos(phi); |
1330 | double sinphi=sin(phi); |
1331 | double p=fabs(1./S(state_q_over_p)); |
1332 | double tanl=1./sqrt(tx*tx+ty*ty); |
1333 | double sinl=sin(atan(tanl)); |
1334 | double cosl=cos(atan(tanl)); |
1335 | cout |
1336 | << setiosflags(ios::fixed)<< "pos: " << setprecision(4) |
1337 | << forward_traj[m].S(state_x) << ", " |
1338 | << forward_traj[m].S(state_y) << ", " |
1339 | << forward_traj[m].z << " mom: " |
1340 | << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", " |
1341 | << p*sinl << " -> " << p |
1342 | <<" s: " << setprecision(3) |
1343 | << forward_traj[m].s |
1344 | <<" t: " << setprecision(3) |
1345 | << forward_traj[m].t/SPEED_OF_LIGHT29.9792458 |
1346 | <<" B: " << forward_traj[m].B |
1347 | << endl; |
1348 | } |
1349 | } |
1350 | |
1351 | // Current state vector |
1352 | S=forward_traj[0].S; |
1353 | |
1354 | // position at the end of the swim |
1355 | x_=forward_traj[0].S(state_x); |
1356 | y_=forward_traj[0].S(state_y); |
1357 | z_=forward_traj[0].z; |
1358 | |
1359 | return NOERROR; |
1360 | } |
1361 | |
1362 | // Routine that extracts the state vector propagation part out of the reference |
1363 | // trajectory loop |
1364 | jerror_t DTrackFitterKalmanSIMD::PropagateForwardCDC(int length,int &index, |
1365 | double &z,double &r2, |
1366 | DMatrix5x1 &S, |
1367 | bool &stepped_to_boundary){ |
1368 | DMatrix5x5 J,Q; |
1369 | DKalmanForwardTrajectory_t temp; |
1370 | int my_i=0; |
1371 | temp.h_id=0; |
1372 | temp.num_hits=0; |
1373 | double dEdx=0.; |
1374 | double s_to_boundary=1e6; |
1375 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
1376 | |
1377 | // current position |
1378 | DVector3 pos(S(state_x),S(state_y),z); |
1379 | temp.z=z; |
1380 | // squared radius |
1381 | r2=pos.Perp2(); |
1382 | |
1383 | temp.s=len; |
1384 | temp.t=ftime; |
1385 | temp.S=S; |
1386 | |
1387 | // Kinematic variables |
1388 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
1389 | double one_over_beta2=1.+mass2*q_over_p_sq; |
1390 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
1391 | |
1392 | // get material properties from the Root Geometry |
1393 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
1394 | DVector3 mom(S(state_tx),S(state_ty),1.); |
1395 | if(geom->FindMatKalman(pos,mom,temp.K_rho_Z_over_A, |
1396 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1397 | temp.chi2c_factor,temp.chi2a_factor, |
1398 | temp.chi2a_corr, |
1399 | last_material_map, |
1400 | &s_to_boundary)!=NOERROR){ |
1401 | return UNRECOVERABLE_ERROR; |
1402 | } |
1403 | } |
1404 | else |
1405 | { |
1406 | if(geom->FindMatKalman(pos,temp.K_rho_Z_over_A, |
1407 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1408 | temp.chi2c_factor,temp.chi2a_factor, |
1409 | temp.chi2a_corr, |
1410 | last_material_map)!=NOERROR){ |
1411 | return UNRECOVERABLE_ERROR; |
1412 | } |
1413 | } |
1414 | |
1415 | // Get dEdx for the upcoming step |
1416 | if (CORRECT_FOR_ELOSS){ |
1417 | dEdx=GetdEdx(S(state_q_over_p),temp.K_rho_Z_over_A,temp.rho_Z_over_A, |
1418 | temp.LnI,temp.Z); |
1419 | } |
1420 | |
1421 | index++; |
1422 | if (index<=length){ |
1423 | my_i=length-index; |
1424 | forward_traj[my_i].s=temp.s; |
1425 | forward_traj[my_i].t=temp.t; |
1426 | forward_traj[my_i].h_id=temp.h_id; |
1427 | forward_traj[my_i].num_hits=0; |
1428 | forward_traj[my_i].z=temp.z; |
1429 | forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A; |
1430 | forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A; |
1431 | forward_traj[my_i].LnI=temp.LnI; |
1432 | forward_traj[my_i].Z=temp.Z; |
1433 | forward_traj[my_i].S=S; |
1434 | } |
1435 | |
1436 | // Determine the step size based on energy loss |
1437 | //double step=mStepSizeS*dz_ds; |
1438 | double max_step_size |
1439 | =(z<endplate_z&& r2>endplate_r2min)?mCDCInternalStepSize:mStepSizeS; |
1440 | double ds=mStepSizeS; |
1441 | if (z<endplate_z && r2<endplate_r2max && z>cdc_origin[2]){ |
1442 | if (!stepped_to_boundary){ |
1443 | stepped_to_boundary=false; |
1444 | if (fabs(dEdx)>EPS3.0e-8){ |
1445 | ds=DE_PER_STEP0.001/fabs(dEdx); |
1446 | } |
1447 | if (ds>max_step_size) ds=max_step_size; |
1448 | if (s_to_boundary<ds){ |
1449 | ds=s_to_boundary+EPS31.e-2; |
1450 | stepped_to_boundary=true; |
1451 | } |
1452 | if(ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1; |
1453 | } |
1454 | else{ |
1455 | ds=MIN_STEP_SIZE0.1; |
1456 | stepped_to_boundary=false; |
1457 | } |
1458 | } |
1459 | double newz=z+ds*dz_ds; // new z position |
1460 | |
1461 | // Store magnetic field |
1462 | temp.B=sqrt(Bx*Bx+By*By+Bz*Bz); |
1463 | |
1464 | // Step through field |
1465 | ds=FasterStep(z,newz,dEdx,S); |
1466 | |
1467 | // update path length |
1468 | len+=fabs(ds); |
1469 | |
1470 | // Update flight time |
1471 | ftime+=ds*sqrt(one_over_beta2);// in units where c=1 |
1472 | |
1473 | // Get the contribution to the covariance matrix due to multiple |
1474 | // scattering |
1475 | GetProcessNoise(z,ds,temp.chi2c_factor,temp.chi2a_factor,temp.chi2a_corr, |
1476 | temp.S,Q); |
1477 | |
1478 | // Energy loss straggling |
1479 | if (CORRECT_FOR_ELOSS){ |
1480 | double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A); |
1481 | Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2; |
1482 | } |
1483 | |
1484 | // Compute the Jacobian matrix and its transpose |
1485 | StepJacobian(newz,z,S,dEdx,J); |
1486 | |
1487 | // update the trajectory |
1488 | if (index<=length){ |
1489 | forward_traj[my_i].B=temp.B; |
1490 | forward_traj[my_i].Q=Q; |
1491 | forward_traj[my_i].J=J; |
1492 | } |
1493 | else{ |
1494 | temp.Q=Q; |
1495 | temp.J=J; |
1496 | temp.Ckk=Zero5x5; |
1497 | temp.Skk=Zero5x1; |
1498 | forward_traj.push_front(temp); |
1499 | } |
1500 | |
1501 | //update z |
1502 | z=newz; |
1503 | |
1504 | return NOERROR; |
1505 | } |
1506 | |
1507 | // Routine that extracts the state vector propagation part out of the reference |
1508 | // trajectory loop |
1509 | jerror_t DTrackFitterKalmanSIMD::PropagateCentral(int length, int &index, |
1510 | DVector2 &my_xy, |
1511 | double &var_t_factor, |
1512 | DMatrix5x1 &Sc, |
1513 | bool &stepped_to_boundary){ |
1514 | DKalmanCentralTrajectory_t temp; |
1515 | DMatrix5x5 J; // State vector Jacobian matrix |
1516 | DMatrix5x5 Q; // Process noise covariance matrix |
1517 | |
1518 | //Initialize some variables needed later |
1519 | double dEdx=0.; |
1520 | double s_to_boundary=1e6; |
1521 | int my_i=0; |
1522 | // Kinematic variables |
1523 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
1524 | double q_over_p_sq=q_over_p*q_over_p; |
1525 | double one_over_beta2=1.+mass2*q_over_p_sq; |
1526 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
1527 | |
1528 | // Reset D to zero |
1529 | Sc(state_D)=0.; |
1530 | |
1531 | temp.xy=my_xy; |
1532 | temp.s=len; |
1533 | temp.t=ftime; |
1534 | temp.h_id=0; |
1535 | temp.S=Sc; |
1536 | |
1537 | // Store magnitude of magnetic field |
1538 | temp.B=sqrt(Bx*Bx+By*By+Bz*Bz); |
1539 | |
1540 | // get material properties from the Root Geometry |
1541 | DVector3 pos3d(my_xy.X(),my_xy.Y(),Sc(state_z)); |
1542 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
1543 | DVector3 mom(cos(Sc(state_phi)),sin(Sc(state_phi)),Sc(state_tanl)); |
1544 | if(geom->FindMatKalman(pos3d,mom,temp.K_rho_Z_over_A, |
1545 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1546 | temp.chi2c_factor,temp.chi2a_factor, |
1547 | temp.chi2a_corr, |
1548 | last_material_map, |
1549 | &s_to_boundary) |
1550 | !=NOERROR){ |
1551 | return UNRECOVERABLE_ERROR; |
1552 | } |
1553 | } |
1554 | else if(geom->FindMatKalman(pos3d,temp.K_rho_Z_over_A, |
1555 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1556 | temp.chi2c_factor,temp.chi2a_factor, |
1557 | temp.chi2a_corr, |
1558 | last_material_map)!=NOERROR){ |
1559 | return UNRECOVERABLE_ERROR; |
1560 | } |
1561 | |
1562 | if (CORRECT_FOR_ELOSS){ |
1563 | dEdx=GetdEdx(q_over_p,temp.K_rho_Z_over_A,temp.rho_Z_over_A,temp.LnI, |
1564 | temp.Z); |
1565 | } |
1566 | |
1567 | // If the deque already exists, update it |
1568 | index++; |
1569 | if (index<=length){ |
1570 | my_i=length-index; |
1571 | central_traj[my_i].B=temp.B; |
1572 | central_traj[my_i].s=temp.s; |
1573 | central_traj[my_i].t=temp.t; |
1574 | central_traj[my_i].h_id=0; |
1575 | central_traj[my_i].xy=temp.xy; |
1576 | central_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A; |
1577 | central_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A; |
1578 | central_traj[my_i].LnI=temp.LnI; |
1579 | central_traj[my_i].Z=temp.Z; |
1580 | central_traj[my_i].S=Sc; |
1581 | } |
1582 | |
1583 | // Adjust the step size |
1584 | double step_size=mStepSizeS; |
1585 | if (stepped_to_boundary){ |
1586 | step_size=MIN_STEP_SIZE0.1; |
1587 | stepped_to_boundary=false; |
1588 | } |
1589 | else{ |
1590 | if (fabs(dEdx)>EPS3.0e-8){ |
1591 | step_size=DE_PER_STEP0.001/fabs(dEdx); |
1592 | } |
1593 | if(step_size>mStepSizeS) step_size=mStepSizeS; |
1594 | if (s_to_boundary<step_size){ |
1595 | step_size=s_to_boundary+EPS31.e-2; |
1596 | stepped_to_boundary=true; |
1597 | } |
1598 | if(step_size<MIN_STEP_SIZE0.1)step_size=MIN_STEP_SIZE0.1; |
1599 | } |
1600 | double r2=my_xy.Mod2(); |
1601 | if (r2>endplate_r2min |
1602 | && step_size>mCDCInternalStepSize) step_size=mCDCInternalStepSize; |
1603 | // Propagate the state through the field |
1604 | FasterStep(my_xy,step_size,Sc,dEdx); |
1605 | |
1606 | // update path length |
1607 | len+=step_size; |
1608 | |
1609 | // Update flight time |
1610 | double dt=step_size*sqrt(one_over_beta2); // in units of c=1 |
1611 | double one_minus_beta2=1.-1./one_over_beta2; |
1612 | ftime+=dt; |
1613 | var_ftime+=dt*dt*one_minus_beta2*one_minus_beta2*0.0004; |
1614 | var_t_factor=dt*dt*one_minus_beta2*one_minus_beta2; |
1615 | |
1616 | //printf("t %f sigt %f\n",TIME_UNIT_CONVERSION*ftime,TIME_UNIT_CONVERSION*sqrt(var_ftime)); |
1617 | |
1618 | // Multiple scattering |
1619 | GetProcessNoiseCentral(step_size,temp.chi2c_factor,temp.chi2a_factor, |
1620 | temp.chi2a_corr,temp.S,Q); |
1621 | |
1622 | // Energy loss straggling in the approximation of thick absorbers |
1623 | if (CORRECT_FOR_ELOSS){ |
1624 | double varE |
1625 | =GetEnergyVariance(step_size,one_over_beta2,temp.K_rho_Z_over_A); |
1626 | Q(state_q_over_pt,state_q_over_pt) |
1627 | +=varE*temp.S(state_q_over_pt)*temp.S(state_q_over_pt)*one_over_beta2 |
1628 | *q_over_p_sq; |
1629 | } |
1630 | |
1631 | // B-field and gradient at current (x,y,z) |
1632 | bfield->GetFieldAndGradient(my_xy.X(),my_xy.Y(),Sc(state_z),Bx,By,Bz, |
1633 | dBxdx,dBxdy,dBxdz,dBydx, |
1634 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1635 | |
1636 | // Compute the Jacobian matrix and its transpose |
1637 | StepJacobian(my_xy,temp.xy-my_xy,-step_size,Sc,dEdx,J); |
1638 | |
1639 | // Update the trajectory info |
1640 | if (index<=length){ |
1641 | central_traj[my_i].Q=Q; |
1642 | central_traj[my_i].J=J; |
1643 | } |
1644 | else{ |
1645 | temp.Q=Q; |
1646 | temp.J=J; |
1647 | temp.Ckk=Zero5x5; |
1648 | temp.Skk=Zero5x1; |
1649 | central_traj.push_front(temp); |
1650 | } |
1651 | |
1652 | return NOERROR; |
1653 | } |
1654 | |
1655 | |
1656 | |
1657 | // Reference trajectory for central tracks |
1658 | // At each point we store the state vector and the Jacobian needed to get to this state |
1659 | // along s from the previous state. |
1660 | // The tricky part is that we swim out from the target to find Sc and pos along the trajectory |
1661 | // but we need the Jacobians for the opposite direction, because the filter proceeds from |
1662 | // the outer hits toward the target. |
1663 | jerror_t DTrackFitterKalmanSIMD::SetCDCReferenceTrajectory(const DVector2 &xy, |
1664 | DMatrix5x1 &Sc){ |
1665 | bool stepped_to_boundary=false; |
1666 | int i=0,central_traj_length=central_traj.size(); |
1667 | // factor for scaling momentum resolution to propagate variance in flight |
1668 | // time |
1669 | double var_t_factor=0; |
1670 | |
1671 | // Magnetic field and gradient at beginning of trajectory |
1672 | //bfield->GetField(x_,y_,z_,Bx,By,Bz); |
1673 | bfield->GetFieldAndGradient(x_,y_,z_,Bx,By,Bz, |
1674 | dBxdx,dBxdy,dBxdz,dBydx, |
1675 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1676 | |
1677 | // Copy of initial position in xy |
1678 | DVector2 my_xy=xy; |
1679 | double r2=xy.Mod2(),z=z_; |
1680 | |
1681 | // Reset cdc status flags |
1682 | for (unsigned int j=0;j<my_cdchits.size();j++){ |
1683 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
1684 | } |
1685 | |
1686 | // Continue adding to the trajectory until we have reached the endplate |
1687 | // or the maximum radius |
1688 | while(z<endplate_z && z>=Z_MIN-100. && r2<endplate_r2max |
1689 | && fabs(Sc(state_q_over_pt))<Q_OVER_PT_MAX100. |
1690 | ){ |
1691 | if (PropagateCentral(central_traj_length,i,my_xy,var_t_factor,Sc,stepped_to_boundary) |
1692 | !=NOERROR) return UNRECOVERABLE_ERROR; |
1693 | z=Sc(state_z); |
1694 | r2=my_xy.Mod2(); |
1695 | } |
1696 | |
1697 | // If the current length of the trajectory deque is less than the previous |
1698 | // trajectory deque, remove the extra elements and shrink the deque |
1699 | if (i<(int)central_traj.size()){ |
1700 | int central_traj_length=central_traj.size(); |
1701 | for (int j=0;j<central_traj_length-i;j++){ |
1702 | central_traj.pop_front(); |
1703 | } |
1704 | } |
1705 | |
1706 | // Only use hits that would fall within the range of the reference trajectory |
1707 | /*for (unsigned int j=0;j<my_cdchits.size();j++){ |
1708 | DKalmanSIMDCDCHit_t *hit=my_cdchits[j]; |
1709 | double my_r2=(hit->origin+(z-hit->z0wire)*hit->dir).Mod2(); |
1710 | if (my_r2>r2) hit->status=bad_hit; |
1711 | } |
1712 | */ |
1713 | |
1714 | // return an error if there are not enough entries in the trajectory |
1715 | if (central_traj.size()<2) return RESOURCE_UNAVAILABLE; |
1716 | |
1717 | if (DEBUG_LEVEL>20) |
1718 | { |
1719 | cout << "---------" << central_traj.size() <<" entries------" <<endl; |
1720 | for (unsigned int m=0;m<central_traj.size();m++){ |
1721 | DMatrix5x1 S=central_traj[m].S; |
1722 | double cosphi=cos(S(state_phi)); |
1723 | double sinphi=sin(S(state_phi)); |
1724 | double pt=fabs(1./S(state_q_over_pt)); |
1725 | double tanl=S(state_tanl); |
1726 | |
1727 | cout |
1728 | << m << "::" |
1729 | << setiosflags(ios::fixed)<< "pos: " << setprecision(4) |
1730 | << central_traj[m].xy.X() << ", " |
1731 | << central_traj[m].xy.Y() << ", " |
1732 | << central_traj[m].S(state_z) << " mom: " |
1733 | << pt*cosphi << ", " << pt*sinphi << ", " |
1734 | << pt*tanl << " -> " << pt/cos(atan(tanl)) |
1735 | <<" s: " << setprecision(3) |
1736 | << central_traj[m].s |
1737 | <<" t: " << setprecision(3) |
1738 | << central_traj[m].t/SPEED_OF_LIGHT29.9792458 |
1739 | <<" B: " << central_traj[m].B |
1740 | << endl; |
1741 | } |
1742 | } |
1743 | |
1744 | // State at end of swim |
1745 | Sc=central_traj[0].S; |
1746 | |
1747 | return NOERROR; |
1748 | } |
1749 | |
1750 | // Routine that extracts the state vector propagation part out of the reference |
1751 | // trajectory loop |
1752 | jerror_t DTrackFitterKalmanSIMD::PropagateForward(int length,int &i, |
1753 | double &z,double zhit, |
1754 | DMatrix5x1 &S, bool &done, |
1755 | bool &stepped_to_boundary, |
1756 | bool &stepped_to_endplate){ |
1757 | DMatrix5x5 J,Q; |
1758 | DKalmanForwardTrajectory_t temp; |
1759 | |
1760 | // Initialize some variables |
1761 | temp.h_id=0; |
1762 | temp.num_hits=0; |
1763 | int my_i=0; |
1764 | double s_to_boundary=1e6; |
1765 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
1766 | |
1767 | // current position |
1768 | DVector3 pos(S(state_x),S(state_y),z); |
1769 | double r2=pos.Perp2(); |
1770 | |
1771 | temp.s=len; |
1772 | temp.t=ftime; |
1773 | temp.z=z; |
1774 | temp.S=S; |
1775 | |
1776 | // Kinematic variables |
1777 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
1778 | double one_over_beta2=1.+mass2*q_over_p_sq; |
1779 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
1780 | |
1781 | // get material properties from the Root Geometry |
1782 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
1783 | DVector3 mom(S(state_tx),S(state_ty),1.); |
1784 | if (geom->FindMatKalman(pos,mom,temp.K_rho_Z_over_A, |
1785 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1786 | temp.chi2c_factor,temp.chi2a_factor, |
1787 | temp.chi2a_corr, |
1788 | last_material_map, |
1789 | &s_to_boundary) |
1790 | !=NOERROR){ |
1791 | return UNRECOVERABLE_ERROR; |
1792 | } |
1793 | } |
1794 | else |
1795 | { |
1796 | if (geom->FindMatKalman(pos,temp.K_rho_Z_over_A, |
1797 | temp.rho_Z_over_A,temp.LnI,temp.Z, |
1798 | temp.chi2c_factor,temp.chi2a_factor, |
1799 | temp.chi2a_corr, |
1800 | last_material_map)!=NOERROR){ |
1801 | return UNRECOVERABLE_ERROR; |
1802 | } |
1803 | } |
1804 | |
1805 | // Get dEdx for the upcoming step |
1806 | double dEdx=0.; |
1807 | if (CORRECT_FOR_ELOSS){ |
1808 | dEdx=GetdEdx(S(state_q_over_p),temp.K_rho_Z_over_A, |
1809 | temp.rho_Z_over_A,temp.LnI,temp.Z); |
1810 | } |
1811 | i++; |
1812 | my_i=length-i; |
1813 | if (i<=length){ |
1814 | forward_traj[my_i].s=temp.s; |
1815 | forward_traj[my_i].t=temp.t; |
1816 | forward_traj[my_i].h_id=temp.h_id; |
1817 | forward_traj[my_i].num_hits=temp.num_hits; |
1818 | forward_traj[my_i].z=temp.z; |
1819 | forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A; |
1820 | forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A; |
1821 | forward_traj[my_i].LnI=temp.LnI; |
1822 | forward_traj[my_i].S=S; |
1823 | } |
1824 | else{ |
1825 | temp.S=S; |
1826 | } |
1827 | |
1828 | // Determine the step size based on energy loss |
1829 | // step=mStepSizeS*dz_ds; |
1830 | double max_step_size |
1831 | =(z<endplate_z&& r2>endplate_r2min)?mCentralStepSize:mStepSizeS; |
1832 | double ds=mStepSizeS; |
1833 | if (z>cdc_origin[2]){ |
1834 | if (!stepped_to_boundary){ |
1835 | stepped_to_boundary=false; |
1836 | if (fabs(dEdx)>EPS3.0e-8){ |
1837 | ds=DE_PER_STEP0.001/fabs(dEdx); |
1838 | } |
1839 | if (ds>max_step_size) ds=max_step_size; |
1840 | if (s_to_boundary<ds){ |
1841 | ds=s_to_boundary+EPS31.e-2; |
1842 | stepped_to_boundary=true; |
1843 | } |
1844 | if(ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1; |
1845 | |
1846 | } |
1847 | else{ |
1848 | ds=MIN_STEP_SIZE0.1; |
1849 | stepped_to_boundary=false; |
1850 | } |
1851 | } |
1852 | |
1853 | double dz=stepped_to_endplate ? endplate_dz : ds*dz_ds; |
1854 | double newz=z+dz; // new z position |
1855 | // Check if we are stepping through the CDC endplate |
1856 | if (newz>endplate_z && z<endplate_z){ |
1857 | // _DBG_ << endl; |
1858 | newz=endplate_z+EPS31.e-2; |
1859 | stepped_to_endplate=true; |
1860 | } |
1861 | |
1862 | // Check if we are about to step to one of the wire planes |
1863 | done=false; |
1864 | if (newz>zhit){ |
1865 | newz=zhit; |
1866 | done=true; |
1867 | } |
1868 | |
1869 | // Store magnitude of magnetic field |
1870 | temp.B=sqrt(Bx*Bx+By*By+Bz*Bz); |
1871 | |
1872 | // Step through field |
1873 | ds=FasterStep(z,newz,dEdx,S); |
1874 | |
1875 | // update path length |
1876 | len+=ds; |
1877 | |
1878 | // update flight time |
1879 | ftime+=ds*sqrt(one_over_beta2); // in units where c=1 |
1880 | |
1881 | // Get the contribution to the covariance matrix due to multiple |
1882 | // scattering |
1883 | GetProcessNoise(z,ds,temp.chi2c_factor,temp.chi2a_factor,temp.chi2a_corr, |
1884 | temp.S,Q); |
1885 | |
1886 | // Energy loss straggling |
1887 | if (CORRECT_FOR_ELOSS){ |
1888 | double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A); |
1889 | Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2; |
1890 | } |
1891 | |
1892 | // Compute the Jacobian matrix and its transpose |
1893 | StepJacobian(newz,z,S,dEdx,J); |
1894 | |
1895 | // update the trajectory data |
1896 | if (i<=length){ |
1897 | forward_traj[my_i].B=temp.B; |
1898 | forward_traj[my_i].Q=Q; |
1899 | forward_traj[my_i].J=J; |
1900 | } |
1901 | else{ |
1902 | temp.Q=Q; |
1903 | temp.J=J; |
1904 | temp.Ckk=Zero5x5; |
1905 | temp.Skk=Zero5x1; |
1906 | forward_traj.push_front(temp); |
1907 | } |
1908 | |
1909 | // update z |
1910 | z=newz; |
1911 | |
1912 | return NOERROR; |
1913 | } |
1914 | |
1915 | // Reference trajectory for trajectories with hits in the forward direction |
1916 | // At each point we store the state vector and the Jacobian needed to get to this state |
1917 | // along z from the previous state. |
1918 | jerror_t DTrackFitterKalmanSIMD::SetReferenceTrajectory(DMatrix5x1 &S){ |
1919 | |
1920 | // Magnetic field and gradient at beginning of trajectory |
1921 | //bfield->GetField(x_,y_,z_,Bx,By,Bz); |
1922 | bfield->GetFieldAndGradient(x_,y_,z_,Bx,By,Bz, |
1923 | dBxdx,dBxdy,dBxdz,dBydx, |
1924 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
1925 | |
1926 | // progress in z from hit to hit |
1927 | double z=z_; |
1928 | int i=0; |
1929 | |
1930 | int forward_traj_length=forward_traj.size(); |
1931 | // loop over the fdc hits |
1932 | double zhit=0.,old_zhit=0.; |
1933 | bool stepped_to_boundary=false; |
1934 | bool stepped_to_endplate=false; |
1935 | unsigned int m=0; |
1936 | double z_max=400.; |
1937 | double r2max=50.*50.; |
1938 | if (got_trd_gem_hits){ |
1939 | z_max=600.; |
1940 | r2max=100.*100.; |
1941 | } |
1942 | for (m=0;m<my_fdchits.size();m++){ |
1943 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX |
1944 | || fabs(S(state_tx))>TAN_MAX10. |
1945 | || fabs(S(state_ty))>TAN_MAX10. |
1946 | || S(state_x)*S(state_x)+S(state_y)*S(state_y)>r2max |
1947 | || z>z_max || z<Z_MIN-100. |
1948 | ){ |
1949 | break; |
1950 | } |
1951 | |
1952 | zhit=my_fdchits[m]->z; |
1953 | if (fabs(old_zhit-zhit)>EPS3.0e-8){ |
1954 | bool done=false; |
1955 | while (!done){ |
1956 | if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX |
1957 | || fabs(S(state_tx))>TAN_MAX10. |
1958 | || fabs(S(state_ty))>TAN_MAX10. |
1959 | || S(state_x)*S(state_x)+S(state_y)*S(state_y)>r2max |
1960 | || z>z_max || z< Z_MIN-100. |
1961 | ){ |
1962 | break; |
1963 | } |
1964 | |
1965 | if (PropagateForward(forward_traj_length,i,z,zhit,S,done, |
1966 | stepped_to_boundary,stepped_to_endplate) |
1967 | !=NOERROR) |
1968 | return UNRECOVERABLE_ERROR; |
1969 | } |
1970 | } |
1971 | old_zhit=zhit; |
1972 | } |
1973 | |
1974 | // If m<2 then no useable FDC hits survived the check on the magnitude on the |
1975 | // momentum |
1976 | if (m<2) return UNRECOVERABLE_ERROR; |
1977 | |
1978 | // Make sure the reference trajectory goes one step beyond the most |
1979 | // downstream hit plane |
1980 | if (m==my_fdchits.size()){ |
1981 | bool done=false; |
1982 | if (PropagateForward(forward_traj_length,i,z,z_max,S,done, |
1983 | stepped_to_boundary,stepped_to_endplate) |
1984 | !=NOERROR) |
1985 | return UNRECOVERABLE_ERROR; |
1986 | if (PropagateForward(forward_traj_length,i,z,z_max,S,done, |
1987 | stepped_to_boundary,stepped_to_endplate) |
1988 | !=NOERROR) |
1989 | return UNRECOVERABLE_ERROR; |
1990 | } |
1991 | |
1992 | // Shrink the deque if the new trajectory has less points in it than the |
1993 | // old trajectory |
1994 | if (i<(int)forward_traj.size()){ |
1995 | int mylen=forward_traj.size(); |
1996 | //_DBG_ << "Shrinking: " << mylen << " to " << i << endl; |
1997 | for (int j=0;j<mylen-i;j++){ |
1998 | forward_traj.pop_front(); |
1999 | } |
2000 | // _DBG_ << " Now " << forward_traj.size() << endl; |
2001 | } |
2002 | |
2003 | // If we lopped off some hits on the downstream end, truncate the trajectory to |
2004 | // the point in z just beyond the last valid hit |
2005 | unsigned int my_id=my_fdchits.size(); |
2006 | if (m<my_id){ |
2007 | if (zhit<z) my_id=m; |
2008 | else my_id=m-1; |
2009 | zhit=my_fdchits[my_id-1]->z; |
2010 | //_DBG_ << "Shrinking: " << forward_traj.size()<< endl; |
2011 | for (;;){ |
2012 | z=forward_traj[0].z; |
2013 | if (z<zhit+EPS21.e-4) break; |
2014 | forward_traj.pop_front(); |
2015 | } |
2016 | //_DBG_ << " Now " << forward_traj.size() << endl; |
2017 | |
2018 | // Temporory structure keeping state and trajectory information |
2019 | DKalmanForwardTrajectory_t temp; |
2020 | temp.h_id=0; |
2021 | temp.num_hits=0; |
2022 | temp.B=0.; |
2023 | temp.K_rho_Z_over_A=temp.rho_Z_over_A=temp.LnI=0.; |
2024 | temp.Q=Zero5x5; |
2025 | |
2026 | // last S vector |
2027 | S=forward_traj[0].S; |
2028 | |
2029 | // Step just beyond the last hit |
2030 | double newz=z+0.01; |
2031 | double ds=Step(z,newz,0.,S); // ignore energy loss for this small step |
2032 | temp.s=forward_traj[0].s+ds; |
2033 | temp.z=newz; |
2034 | temp.S=S; |
2035 | |
2036 | // Flight time |
2037 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
2038 | double one_over_beta2=1.+mass2*q_over_p_sq; |
2039 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
2040 | temp.t=forward_traj[0].t+ds*sqrt(one_over_beta2); // in units where c=1 |
2041 | |
2042 | // Covariance and state vector needed for smoothing code |
2043 | temp.Ckk=Zero5x5; |
2044 | temp.Skk=Zero5x1; |
2045 | |
2046 | // Jacobian matrices |
2047 | temp.J=I5x5; |
2048 | |
2049 | forward_traj.push_front(temp); |
2050 | } |
2051 | |
2052 | // return an error if there are not enough entries in the trajectory |
2053 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
2054 | |
2055 | // Fill in Lorentz deflection parameters |
2056 | for (unsigned int m=0;m<forward_traj.size();m++){ |
2057 | if (my_id>0){ |
2058 | unsigned int hit_id=my_id-1; |
2059 | double z=forward_traj[m].z; |
2060 | if (fabs(z-my_fdchits[hit_id]->z)<EPS21.e-4){ |
2061 | forward_traj[m].h_id=my_id; |
2062 | |
2063 | if (my_fdchits[hit_id]->hit!=NULL__null){ |
2064 | // Get the magnetic field at this position along the trajectory |
2065 | bfield->GetField(forward_traj[m].S(state_x),forward_traj[m].S(state_y), |
2066 | z,Bx,By,Bz); |
2067 | double Br=sqrt(Bx*Bx+By*By); |
2068 | |
2069 | // Angle between B and wire |
2070 | double my_phi=0.; |
2071 | if (Br>0.) my_phi=acos((Bx*my_fdchits[hit_id]->sina |
2072 | +By*my_fdchits[hit_id]->cosa)/Br); |
2073 | /* |
2074 | lorentz_def->GetLorentzCorrectionParameters(forward_traj[m].pos.x(), |
2075 | forward_traj[m].pos.y(), |
2076 | forward_traj[m].pos.z(), |
2077 | tanz,tanr); |
2078 | my_fdchits[hit_id]->nr=tanr; |
2079 | my_fdchits[hit_id]->nz=tanz; |
2080 | */ |
2081 | |
2082 | my_fdchits[hit_id]->nr=LORENTZ_NR_PAR1*Bz*(1.+LORENTZ_NR_PAR2*Br); |
2083 | my_fdchits[hit_id]->nz=(LORENTZ_NZ_PAR1+LORENTZ_NZ_PAR2*Bz)*(Br*cos(my_phi)); |
2084 | } |
2085 | |
2086 | my_id--; |
2087 | |
2088 | unsigned int num=1; |
2089 | while (hit_id>0 |
2090 | && fabs(my_fdchits[hit_id]->z-my_fdchits[hit_id-1]->z)<EPS3.0e-8){ |
2091 | hit_id=my_id-1; |
2092 | num++; |
2093 | my_id--; |
2094 | } |
2095 | forward_traj[m].num_hits=num; |
2096 | } |
2097 | |
2098 | } |
2099 | } |
2100 | |
2101 | if (DEBUG_LEVEL>20) |
2102 | { |
2103 | cout << "--- Forward fdc trajectory ---" <<endl; |
2104 | for (unsigned int m=0;m<forward_traj.size();m++){ |
2105 | DMatrix5x1 S=(forward_traj[m].S); |
2106 | double tx=S(state_tx),ty=S(state_ty); |
2107 | double phi=atan2(ty,tx); |
2108 | double cosphi=cos(phi); |
2109 | double sinphi=sin(phi); |
2110 | double p=fabs(1./S(state_q_over_p)); |
2111 | double tanl=1./sqrt(tx*tx+ty*ty); |
2112 | double sinl=sin(atan(tanl)); |
2113 | double cosl=cos(atan(tanl)); |
2114 | cout |
2115 | << setiosflags(ios::fixed)<< "pos: " << setprecision(4) |
2116 | << forward_traj[m].S(state_x) << ", " |
2117 | << forward_traj[m].S(state_y) << ", " |
2118 | << forward_traj[m].z << " mom: " |
2119 | << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", " |
2120 | << p*sinl << " -> " << p |
2121 | <<" s: " << setprecision(3) |
2122 | << forward_traj[m].s |
2123 | <<" t: " << setprecision(3) |
2124 | << forward_traj[m].t/SPEED_OF_LIGHT29.9792458 |
2125 | <<" id: " << forward_traj[m].h_id |
2126 | << endl; |
2127 | } |
2128 | } |
2129 | |
2130 | |
2131 | // position at the end of the swim |
2132 | z_=z; |
2133 | x_=S(state_x); |
2134 | y_=S(state_y); |
2135 | |
2136 | return NOERROR; |
2137 | } |
2138 | |
2139 | // Step the state vector through the field from oldz to newz. |
2140 | // Uses the 4th-order Runga-Kutte algorithm. |
2141 | double DTrackFitterKalmanSIMD::Step(double oldz,double newz, double dEdx, |
2142 | DMatrix5x1 &S){ |
2143 | double delta_z=newz-oldz; |
2144 | if (fabs(delta_z)<EPS3.0e-8) return 0.; // skip if the step is too small |
2145 | |
2146 | // Direction tangents |
2147 | double tx=S(state_tx); |
2148 | double ty=S(state_ty); |
2149 | double ds=sqrt(1.+tx*tx+ty*ty)*delta_z; |
2150 | |
2151 | double delta_z_over_2=0.5*delta_z; |
2152 | double midz=oldz+delta_z_over_2; |
2153 | DMatrix5x1 D1,D2,D3,D4; |
2154 | |
2155 | //B-field and gradient at at (x,y,z) |
2156 | bfield->GetFieldAndGradient(S(state_x),S(state_y),oldz,Bx,By,Bz, |
2157 | dBxdx,dBxdy,dBxdz,dBydx, |
2158 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2159 | double Bx0=Bx,By0=By,Bz0=Bz; |
2160 | |
2161 | // Calculate the derivative and propagate the state to the next point |
2162 | CalcDeriv(oldz,S,dEdx,D1); |
2163 | DMatrix5x1 S1=S+delta_z_over_2*D1; |
2164 | |
2165 | // Calculate the field at the first intermediate point |
2166 | double dx=S1(state_x)-S(state_x); |
2167 | double dy=S1(state_y)-S(state_y); |
2168 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2169 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2170 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2171 | |
2172 | // Calculate the derivative and propagate the state to the next point |
2173 | CalcDeriv(midz,S1,dEdx,D2); |
2174 | S1=S+delta_z_over_2*D2; |
2175 | |
2176 | // Calculate the field at the second intermediate point |
2177 | dx=S1(state_x)-S(state_x); |
2178 | dy=S1(state_y)-S(state_y); |
2179 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2180 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2181 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2182 | |
2183 | // Calculate the derivative and propagate the state to the next point |
2184 | CalcDeriv(midz,S1,dEdx,D3); |
2185 | S1=S+delta_z*D3; |
2186 | |
2187 | // Calculate the field at the final point |
2188 | dx=S1(state_x)-S(state_x); |
2189 | dy=S1(state_y)-S(state_y); |
2190 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z; |
2191 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z; |
2192 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z; |
2193 | |
2194 | // Final derivative |
2195 | CalcDeriv(newz,S1,dEdx,D4); |
2196 | |
2197 | // S+=delta_z*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2198 | double dz_over_6=delta_z*ONE_SIXTH0.16666666666666667; |
2199 | double dz_over_3=delta_z*ONE_THIRD0.33333333333333333; |
2200 | S+=dz_over_6*D1; |
2201 | S+=dz_over_3*D2; |
2202 | S+=dz_over_3*D3; |
2203 | S+=dz_over_6*D4; |
2204 | |
2205 | // Don't let the magnitude of the momentum drop below some cutoff |
2206 | //if (fabs(S(state_q_over_p))>Q_OVER_P_MAX) |
2207 | // S(state_q_over_p)=Q_OVER_P_MAX*(S(state_q_over_p)>0.0?1.:-1.); |
2208 | // Try to keep the direction tangents from heading towards 90 degrees |
2209 | //if (fabs(S(state_tx))>TAN_MAX) |
2210 | // S(state_tx)=TAN_MAX*(S(state_tx)>0.0?1.:-1.); |
2211 | //if (fabs(S(state_ty))>TAN_MAX) |
2212 | // S(state_ty)=TAN_MAX*(S(state_ty)>0.0?1.:-1.); |
2213 | |
2214 | return ds; |
2215 | } |
2216 | // Step the state vector through the field from oldz to newz. |
2217 | // Uses the 4th-order Runga-Kutte algorithm. |
2218 | // Uses the gradient to compute the field at the intermediate and last |
2219 | // points. |
2220 | double DTrackFitterKalmanSIMD::FasterStep(double oldz,double newz, double dEdx, |
2221 | DMatrix5x1 &S){ |
2222 | double delta_z=newz-oldz; |
2223 | if (fabs(delta_z)<EPS3.0e-8) return 0.; // skip if the step is too small |
2224 | |
2225 | // Direction tangents |
2226 | double tx=S(state_tx); |
2227 | double ty=S(state_ty); |
2228 | double ds=sqrt(1.+tx*tx+ty*ty)*delta_z; |
2229 | |
2230 | double delta_z_over_2=0.5*delta_z; |
2231 | double midz=oldz+delta_z_over_2; |
2232 | DMatrix5x1 D1,D2,D3,D4; |
2233 | double Bx0=Bx,By0=By,Bz0=Bz; |
2234 | |
2235 | // The magnetic field at the beginning of the step is assumed to be |
2236 | // obtained at the end of the previous step through StepJacobian |
2237 | |
2238 | // Calculate the derivative and propagate the state to the next point |
2239 | CalcDeriv(oldz,S,dEdx,D1); |
2240 | DMatrix5x1 S1=S+delta_z_over_2*D1; |
2241 | |
2242 | // Calculate the field at the first intermediate point |
2243 | double dx=S1(state_x)-S(state_x); |
2244 | double dy=S1(state_y)-S(state_y); |
2245 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2246 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2247 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2248 | |
2249 | // Calculate the derivative and propagate the state to the next point |
2250 | CalcDeriv(midz,S1,dEdx,D2); |
2251 | S1=S+delta_z_over_2*D2; |
2252 | |
2253 | // Calculate the field at the second intermediate point |
2254 | dx=S1(state_x)-S(state_x); |
2255 | dy=S1(state_y)-S(state_y); |
2256 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z_over_2; |
2257 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z_over_2; |
2258 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z_over_2; |
2259 | |
2260 | // Calculate the derivative and propagate the state to the next point |
2261 | CalcDeriv(midz,S1,dEdx,D3); |
2262 | S1=S+delta_z*D3; |
2263 | |
2264 | // Calculate the field at the final point |
2265 | dx=S1(state_x)-S(state_x); |
2266 | dy=S1(state_y)-S(state_y); |
2267 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*delta_z; |
2268 | By=By0+dBydx*dx+dBydy*dy+dBydz*delta_z; |
2269 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*delta_z; |
2270 | |
2271 | // Final derivative |
2272 | CalcDeriv(newz,S1,dEdx,D4); |
2273 | |
2274 | // S+=delta_z*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2275 | double dz_over_6=delta_z*ONE_SIXTH0.16666666666666667; |
2276 | double dz_over_3=delta_z*ONE_THIRD0.33333333333333333; |
2277 | S+=dz_over_6*D1; |
2278 | S+=dz_over_3*D2; |
2279 | S+=dz_over_3*D3; |
2280 | S+=dz_over_6*D4; |
2281 | |
2282 | // Don't let the magnitude of the momentum drop below some cutoff |
2283 | //if (fabs(S(state_q_over_p))>Q_OVER_P_MAX) |
2284 | // S(state_q_over_p)=Q_OVER_P_MAX*(S(state_q_over_p)>0.0?1.:-1.); |
2285 | // Try to keep the direction tangents from heading towards 90 degrees |
2286 | //if (fabs(S(state_tx))>TAN_MAX) |
2287 | // S(state_tx)=TAN_MAX*(S(state_tx)>0.0?1.:-1.); |
2288 | //if (fabs(S(state_ty))>TAN_MAX) |
2289 | // S(state_ty)=TAN_MAX*(S(state_ty)>0.0?1.:-1.); |
2290 | |
2291 | return ds; |
2292 | } |
2293 | |
2294 | |
2295 | |
2296 | // Compute the Jacobian matrix for the forward parametrization. |
2297 | jerror_t DTrackFitterKalmanSIMD::StepJacobian(double oldz,double newz, |
2298 | const DMatrix5x1 &S, |
2299 | double dEdx,DMatrix5x5 &J){ |
2300 | // Initialize the Jacobian matrix |
2301 | //J.Zero(); |
2302 | //for (int i=0;i<5;i++) J(i,i)=1.; |
2303 | J=I5x5; |
2304 | |
2305 | // Step in z |
2306 | double delta_z=newz-oldz; |
2307 | if (fabs(delta_z)<EPS3.0e-8) return NOERROR; //skip if the step is too small |
2308 | |
2309 | // Current values of state vector variables |
2310 | double x=S(state_x), y=S(state_y),tx=S(state_tx),ty=S(state_ty); |
2311 | double q_over_p=S(state_q_over_p); |
2312 | |
2313 | //B-field and field gradient at (x,y,z) |
2314 | //if (get_field) |
2315 | bfield->GetFieldAndGradient(x,y,oldz,Bx,By,Bz,dBxdx,dBxdy, |
2316 | dBxdz,dBydx,dBydy, |
2317 | dBydz,dBzdx,dBzdy,dBzdz); |
2318 | |
2319 | // Don't let the magnitude of the momentum drop below some cutoff |
2320 | if (fabs(q_over_p)>Q_OVER_P_MAX){ |
2321 | q_over_p=Q_OVER_P_MAX*(q_over_p>0.0?1.:-1.); |
2322 | dEdx=0.; |
2323 | } |
2324 | // Try to keep the direction tangents from heading towards 90 degrees |
2325 | if (fabs(tx)>TAN_MAX10.) tx=TAN_MAX10.*(tx>0.0?1.:-1.); |
2326 | if (fabs(ty)>TAN_MAX10.) ty=TAN_MAX10.*(ty>0.0?1.:-1.); |
2327 | // useful combinations of terms |
2328 | double kq_over_p=qBr2p0.003*q_over_p; |
2329 | double tx2=tx*tx; |
2330 | double twotx2=2.*tx2; |
2331 | double ty2=ty*ty; |
2332 | double twoty2=2.*ty2; |
2333 | double txty=tx*ty; |
2334 | double one_plus_tx2=1.+tx2; |
2335 | double one_plus_ty2=1.+ty2; |
2336 | double one_plus_twotx2_plus_ty2=one_plus_ty2+twotx2; |
2337 | double one_plus_twoty2_plus_tx2=one_plus_tx2+twoty2; |
2338 | double dsdz=sqrt(1.+tx2+ty2); |
2339 | double ds=dsdz*delta_z; |
2340 | double kds=qBr2p0.003*ds; |
2341 | double kqdz_over_p_over_dsdz=kq_over_p*delta_z/dsdz; |
2342 | double kq_over_p_ds=kq_over_p*ds; |
2343 | double dtx_Bdep=ty*Bz+txty*Bx-one_plus_tx2*By; |
2344 | double dty_Bdep=Bx*one_plus_ty2-txty*By-tx*Bz; |
2345 | double Bxty=Bx*ty; |
2346 | double Bytx=By*tx; |
2347 | double Bztxty=Bz*txty; |
2348 | double Byty=By*ty; |
2349 | double Bxtx=Bx*tx; |
2350 | |
2351 | // Jacobian |
2352 | J(state_x,state_tx)=J(state_y,state_ty)=delta_z; |
2353 | J(state_tx,state_q_over_p)=kds*dtx_Bdep; |
2354 | J(state_ty,state_q_over_p)=kds*dty_Bdep; |
2355 | J(state_tx,state_tx)+=kqdz_over_p_over_dsdz*(Bxty*(one_plus_twotx2_plus_ty2) |
2356 | -Bytx*(3.*one_plus_tx2+twoty2) |
2357 | +Bztxty); |
2358 | J(state_tx,state_x)=kq_over_p_ds*(ty*dBzdx+txty*dBxdx-one_plus_tx2*dBydx); |
2359 | J(state_ty,state_ty)+=kqdz_over_p_over_dsdz*(Bxty*(3.*one_plus_ty2+twotx2) |
2360 | -Bytx*(one_plus_twoty2_plus_tx2) |
2361 | -Bztxty); |
2362 | J(state_ty,state_y)= kq_over_p_ds*(one_plus_ty2*dBxdy-txty*dBydy-tx*dBzdy); |
2363 | J(state_tx,state_ty)=kqdz_over_p_over_dsdz |
2364 | *((Bxtx+Bz)*(one_plus_twoty2_plus_tx2)-Byty*one_plus_tx2); |
2365 | J(state_tx,state_y)= kq_over_p_ds*(tx*dBzdy+txty*dBxdy-one_plus_tx2*dBydy); |
2366 | J(state_ty,state_tx)=-kqdz_over_p_over_dsdz*((Byty+Bz)*(one_plus_twotx2_plus_ty2) |
2367 | -Bxtx*one_plus_ty2); |
2368 | J(state_ty,state_x)=kq_over_p_ds*(one_plus_ty2*dBxdx-txty*dBydx-tx*dBzdx); |
2369 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2370 | double one_over_p_sq=q_over_p*q_over_p; |
2371 | double E=sqrt(1./one_over_p_sq+mass2); |
2372 | J(state_q_over_p,state_q_over_p)-=dEdx*ds/E*(2.+3.*mass2*one_over_p_sq); |
2373 | double temp=-(q_over_p*one_over_p_sq/dsdz)*E*dEdx*delta_z; |
2374 | J(state_q_over_p,state_tx)=tx*temp; |
2375 | J(state_q_over_p,state_ty)=ty*temp; |
2376 | } |
2377 | |
2378 | return NOERROR; |
2379 | } |
2380 | |
2381 | // Calculate the derivative for the alternate set of parameters {q/pT, phi, |
2382 | // tan(lambda),D,z} |
2383 | jerror_t DTrackFitterKalmanSIMD::CalcDeriv(DVector2 &dpos,const DMatrix5x1 &S, |
2384 | double dEdx,DMatrix5x1 &D1){ |
2385 | //Direction at current point |
2386 | double tanl=S(state_tanl); |
2387 | // Don't let tanl exceed some maximum |
2388 | if (fabs(tanl)>TAN_MAX10.) tanl=TAN_MAX10.*(tanl>0.0?1.:-1.); |
2389 | |
2390 | double phi=S(state_phi); |
2391 | double cosphi=cos(phi); |
2392 | double sinphi=sin(phi); |
2393 | double lambda=atan(tanl); |
2394 | double sinl=sin(lambda); |
2395 | double cosl=cos(lambda); |
2396 | // Other parameters |
2397 | double q_over_pt=S(state_q_over_pt); |
2398 | double pt=fabs(1./q_over_pt); |
2399 | |
2400 | // Turn off dEdx if the pt drops below some minimum |
2401 | if (pt<PT_MIN) { |
2402 | dEdx=0.; |
2403 | } |
2404 | double kq_over_pt=qBr2p0.003*q_over_pt; |
2405 | |
2406 | // Derivative of S with respect to s |
2407 | double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi; |
2408 | D1(state_q_over_pt) |
2409 | =kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2410 | double one_over_cosl=1./cosl; |
2411 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2412 | double p=pt*one_over_cosl; |
2413 | double p_sq=p*p; |
2414 | double E=sqrt(p_sq+mass2); |
2415 | D1(state_q_over_pt)-=q_over_pt*E*dEdx/p_sq; |
2416 | } |
2417 | // D1(state_phi) |
2418 | // =kq_over_pt*(Bx*cosphi*sinl+By*sinphi*sinl-Bz*cosl); |
2419 | D1(state_phi)=kq_over_pt*((Bx*cosphi+By*sinphi)*sinl-Bz*cosl); |
2420 | D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2421 | D1(state_z)=sinl; |
2422 | |
2423 | // New direction |
2424 | dpos.Set(cosl*cosphi,cosl*sinphi); |
2425 | |
2426 | return NOERROR; |
2427 | } |
2428 | |
2429 | // Calculate the derivative and Jacobian matrices for the alternate set of |
2430 | // parameters {q/pT, phi, tan(lambda),D,z} |
2431 | jerror_t DTrackFitterKalmanSIMD::CalcDerivAndJacobian(const DVector2 &xy, |
2432 | DVector2 &dxy, |
2433 | const DMatrix5x1 &S, |
2434 | double dEdx, |
2435 | DMatrix5x5 &J1, |
2436 | DMatrix5x1 &D1){ |
2437 | //Direction at current point |
2438 | double tanl=S(state_tanl); |
2439 | // Don't let tanl exceed some maximum |
2440 | if (fabs(tanl)>TAN_MAX10.) tanl=TAN_MAX10.*(tanl>0.0?1.:-1.); |
2441 | |
2442 | double phi=S(state_phi); |
2443 | double cosphi=cos(phi); |
2444 | double sinphi=sin(phi); |
2445 | double lambda=atan(tanl); |
2446 | double sinl=sin(lambda); |
2447 | double cosl=cos(lambda); |
2448 | double cosl2=cosl*cosl; |
2449 | double cosl3=cosl*cosl2; |
2450 | double one_over_cosl=1./cosl; |
2451 | // Other parameters |
2452 | double q_over_pt=S(state_q_over_pt); |
2453 | double pt=fabs(1./q_over_pt); |
2454 | double q=pt*q_over_pt; |
2455 | |
2456 | // Turn off dEdx if pt drops below some minimum |
2457 | if (pt<PT_MIN) { |
2458 | dEdx=0.; |
2459 | } |
2460 | double kq_over_pt=qBr2p0.003*q_over_pt; |
2461 | |
2462 | // B-field and gradient at (x,y,z) |
2463 | bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2464 | dBxdx,dBxdy,dBxdz,dBydx, |
2465 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2466 | |
2467 | // Derivative of S with respect to s |
2468 | double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi; |
2469 | double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi; |
2470 | D1(state_q_over_pt)=kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2471 | D1(state_phi)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl); |
2472 | D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2473 | D1(state_z)=sinl; |
2474 | |
2475 | // New direction |
2476 | dxy.Set(cosl*cosphi,cosl*sinphi); |
2477 | |
2478 | // Jacobian matrix elements |
2479 | J1(state_phi,state_phi)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2480 | J1(state_phi,state_q_over_pt) |
2481 | =qBr2p0.003*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl); |
2482 | J1(state_phi,state_tanl)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*cosl |
2483 | +Bz*sinl)*cosl2; |
2484 | J1(state_phi,state_z) |
2485 | =kq_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl); |
2486 | |
2487 | J1(state_tanl,state_phi)=-kq_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl; |
2488 | J1(state_tanl,state_q_over_pt)=qBr2p0.003*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2489 | J1(state_tanl,state_tanl)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2490 | J1(state_tanl,state_z)=kq_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl; |
2491 | J1(state_q_over_pt,state_phi) |
2492 | =-kq_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi; |
2493 | J1(state_q_over_pt,state_q_over_pt) |
2494 | =2.*kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2495 | J1(state_q_over_pt,state_tanl) |
2496 | =kq_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi; |
2497 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2498 | double p=pt*one_over_cosl; |
2499 | double p_sq=p*p; |
2500 | double m2_over_p2=mass2/p_sq; |
2501 | double E=sqrt(p_sq+mass2); |
2502 | |
2503 | D1(state_q_over_pt)-=q_over_pt*E/p_sq*dEdx; |
2504 | J1(state_q_over_pt,state_q_over_pt)-=dEdx*(2.+3.*m2_over_p2)/E; |
2505 | J1(state_q_over_pt,state_tanl)+=q*dEdx*sinl*(1.+2.*m2_over_p2)/(p*E); |
2506 | } |
2507 | J1(state_q_over_pt,state_z) |
2508 | =kq_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi); |
2509 | J1(state_z,state_tanl)=cosl3; |
2510 | |
2511 | return NOERROR; |
2512 | } |
2513 | |
2514 | // Step the state and the covariance matrix through the field |
2515 | jerror_t DTrackFitterKalmanSIMD::StepStateAndCovariance(DVector2 &xy, |
2516 | double ds, |
2517 | double dEdx, |
2518 | DMatrix5x1 &S, |
2519 | DMatrix5x5 &J, |
2520 | DMatrix5x5 &C){ |
2521 | //Initialize the Jacobian matrix |
2522 | J=I5x5; |
2523 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2524 | |
2525 | // B-field and gradient at current (x,y,z) |
2526 | bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2527 | dBxdx,dBxdy,dBxdz,dBydx, |
2528 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2529 | double Bx0=Bx,By0=By,Bz0=Bz; |
2530 | |
2531 | // Matrices for intermediate steps |
2532 | DMatrix5x1 D1,D2,D3,D4; |
2533 | DMatrix5x1 S1; |
2534 | DMatrix5x5 J1; |
2535 | DVector2 dxy1,dxy2,dxy3,dxy4; |
2536 | double ds_2=0.5*ds; |
2537 | |
2538 | // Find the derivative at the first point, propagate the state to the |
2539 | // first intermediate point and start filling the Jacobian matrix |
2540 | CalcDerivAndJacobian(xy,dxy1,S,dEdx,J1,D1); |
2541 | S1=S+ds_2*D1; |
2542 | |
2543 | // Calculate the field at the first intermediate point |
2544 | double dz=S1(state_z)-S(state_z); |
2545 | double dx=ds_2*dxy1.X(); |
2546 | double dy=ds_2*dxy1.Y(); |
2547 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2548 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2549 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2550 | |
2551 | // Calculate the derivative and propagate the state to the next point |
2552 | CalcDeriv(dxy2,S1,dEdx,D2); |
2553 | S1=S+ds_2*D2; |
2554 | |
2555 | // Calculate the field at the second intermediate point |
2556 | dz=S1(state_z)-S(state_z); |
2557 | dx=ds_2*dxy2.X(); |
2558 | dy=ds_2*dxy2.Y(); |
2559 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2560 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2561 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2562 | |
2563 | // Calculate the derivative and propagate the state to the next point |
2564 | CalcDeriv(dxy3,S1,dEdx,D3); |
2565 | S1=S+ds*D3; |
2566 | |
2567 | // Calculate the field at the final point |
2568 | dz=S1(state_z)-S(state_z); |
2569 | dx=ds*dxy3.X(); |
2570 | dy=ds*dxy3.Y(); |
2571 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2572 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2573 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2574 | |
2575 | // Final derivative |
2576 | CalcDeriv(dxy4,S1,dEdx,D4); |
2577 | |
2578 | // Position vector increment |
2579 | //DVector3 dpos |
2580 | // =ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4); |
2581 | double ds_over_6=ds*ONE_SIXTH0.16666666666666667; |
2582 | double ds_over_3=ds*ONE_THIRD0.33333333333333333; |
2583 | DVector2 dxy=ds_over_6*dxy1; |
2584 | dxy+=ds_over_3*dxy2; |
2585 | dxy+=ds_over_3*dxy3; |
2586 | dxy+=ds_over_6*dxy4; |
2587 | |
2588 | // New Jacobian matrix |
2589 | J+=ds*J1; |
2590 | |
2591 | // Deal with changes in D |
2592 | double B=sqrt(Bx0*Bx0+By0*By0+Bz0*Bz0); |
2593 | //double qrc_old=qpt/(qBr2p*Bz_); |
2594 | double qpt=1./S(state_q_over_pt); |
2595 | double q=(qpt>0.)?1.:-1.; |
2596 | double qrc_old=qpt/(qBr2p0.003*B); |
2597 | double sinphi=sin(S(state_phi)); |
2598 | double cosphi=cos(S(state_phi)); |
2599 | double qrc_plus_D=S(state_D)+qrc_old; |
2600 | dx=dxy.X(); |
2601 | dy=dxy.Y(); |
2602 | double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi; |
2603 | double rc=sqrt(dxy.Mod2() |
2604 | +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi) |
2605 | +qrc_plus_D*qrc_plus_D); |
2606 | double q_over_rc=q/rc; |
2607 | |
2608 | J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D); |
2609 | J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.); |
2610 | J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi); |
2611 | |
2612 | // New xy vector |
2613 | xy+=dxy; |
2614 | |
2615 | // New state vector |
2616 | //S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2617 | S+=ds_over_6*D1; |
2618 | S+=ds_over_3*D2; |
2619 | S+=ds_over_3*D3; |
2620 | S+=ds_over_6*D4; |
2621 | |
2622 | // Don't let the pt drop below some minimum |
2623 | //if (fabs(1./S(state_q_over_pt))<PT_MIN) { |
2624 | // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.); |
2625 | // } |
2626 | // Don't let tanl exceed some maximum |
2627 | if (fabs(S(state_tanl))>TAN_MAX10.){ |
2628 | S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0.0?1.:-1.); |
2629 | } |
2630 | |
2631 | // New covariance matrix |
2632 | // C=J C J^T |
2633 | //C=C.SandwichMultiply(J); |
2634 | C=J*C*J.Transpose(); |
2635 | |
2636 | return NOERROR; |
2637 | } |
2638 | |
2639 | // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z} |
2640 | // Uses the gradient to compute the field at the intermediate and last |
2641 | // points. |
2642 | jerror_t DTrackFitterKalmanSIMD::FasterStep(DVector2 &xy,double ds, |
2643 | DMatrix5x1 &S, |
2644 | double dEdx){ |
2645 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2646 | |
2647 | // Matrices for intermediate steps |
2648 | DMatrix5x1 D1,D2,D3,D4; |
2649 | DMatrix5x1 S1; |
2650 | DVector2 dxy1,dxy2,dxy3,dxy4; |
2651 | double ds_2=0.5*ds; |
2652 | double Bx0=Bx,By0=By,Bz0=Bz; |
2653 | |
2654 | // The magnetic field at the beginning of the step is assumed to be |
2655 | // obtained at the end of the previous step through StepJacobian |
2656 | |
2657 | // Calculate the derivative and propagate the state to the next point |
2658 | CalcDeriv(dxy1,S,dEdx,D1); |
2659 | S1=S+ds_2*D1; |
2660 | |
2661 | // Calculate the field at the first intermediate point |
2662 | double dz=S1(state_z)-S(state_z); |
2663 | double dx=ds_2*dxy1.X(); |
2664 | double dy=ds_2*dxy1.Y(); |
2665 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2666 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2667 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2668 | |
2669 | // Calculate the derivative and propagate the state to the next point |
2670 | CalcDeriv(dxy2,S1,dEdx,D2); |
2671 | S1=S+ds_2*D2; |
2672 | |
2673 | // Calculate the field at the second intermediate point |
2674 | dz=S1(state_z)-S(state_z); |
2675 | dx=ds_2*dxy2.X(); |
2676 | dy=ds_2*dxy2.Y(); |
2677 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2678 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2679 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2680 | |
2681 | // Calculate the derivative and propagate the state to the next point |
2682 | CalcDeriv(dxy3,S1,dEdx,D3); |
2683 | S1=S+ds*D3; |
2684 | |
2685 | // Calculate the field at the final point |
2686 | dz=S1(state_z)-S(state_z); |
2687 | dx=ds*dxy3.X(); |
2688 | dy=ds*dxy3.Y(); |
2689 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2690 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2691 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2692 | |
2693 | // Final derivative |
2694 | CalcDeriv(dxy4,S1,dEdx,D4); |
2695 | |
2696 | // New state vector |
2697 | // S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2698 | double ds_over_6=ds*ONE_SIXTH0.16666666666666667; |
2699 | double ds_over_3=ds*ONE_THIRD0.33333333333333333; |
2700 | S+=ds_over_6*D1; |
2701 | S+=ds_over_3*D2; |
2702 | S+=ds_over_3*D3; |
2703 | S+=ds_over_6*D4; |
2704 | |
2705 | // New position |
2706 | //pos+=ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4); |
2707 | xy+=ds_over_6*dxy1; |
2708 | xy+=ds_over_3*dxy2; |
2709 | xy+=ds_over_3*dxy3; |
2710 | xy+=ds_over_6*dxy4; |
2711 | |
2712 | // Don't let the pt drop below some minimum |
2713 | //if (fabs(1./S(state_q_over_pt))<PT_MIN) { |
2714 | // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.); |
2715 | //} |
2716 | // Don't let tanl exceed some maximum |
2717 | if (fabs(S(state_tanl))>TAN_MAX10.){ |
2718 | S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0.0?1.:-1.); |
2719 | } |
2720 | |
2721 | return NOERROR; |
2722 | } |
2723 | |
2724 | // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z} |
2725 | jerror_t DTrackFitterKalmanSIMD::Step(DVector2 &xy,double ds, |
2726 | DMatrix5x1 &S, |
2727 | double dEdx){ |
2728 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2729 | |
2730 | // B-field and gradient at current (x,y,z) |
2731 | bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2732 | dBxdx,dBxdy,dBxdz,dBydx, |
2733 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2734 | double Bx0=Bx,By0=By,Bz0=Bz; |
2735 | |
2736 | // Matrices for intermediate steps |
2737 | DMatrix5x1 D1,D2,D3,D4; |
2738 | DMatrix5x1 S1; |
2739 | DVector2 dxy1,dxy2,dxy3,dxy4; |
2740 | double ds_2=0.5*ds; |
2741 | |
2742 | // Find the derivative at the first point, propagate the state to the |
2743 | // first intermediate point |
2744 | CalcDeriv(dxy1,S,dEdx,D1); |
2745 | S1=S+ds_2*D1; |
2746 | |
2747 | // Calculate the field at the first intermediate point |
2748 | double dz=S1(state_z)-S(state_z); |
2749 | double dx=ds_2*dxy1.X(); |
2750 | double dy=ds_2*dxy1.Y(); |
2751 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2752 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2753 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2754 | |
2755 | // Calculate the derivative and propagate the state to the next point |
2756 | CalcDeriv(dxy2,S1,dEdx,D2); |
2757 | S1=S+ds_2*D2; |
2758 | |
2759 | // Calculate the field at the second intermediate point |
2760 | dz=S1(state_z)-S(state_z); |
2761 | dx=ds_2*dxy2.X(); |
2762 | dy=ds_2*dxy2.Y(); |
2763 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2764 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2765 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2766 | |
2767 | // Calculate the derivative and propagate the state to the next point |
2768 | CalcDeriv(dxy3,S1,dEdx,D3); |
2769 | S1=S+ds*D3; |
2770 | |
2771 | // Calculate the field at the final point |
2772 | dz=S1(state_z)-S(state_z); |
2773 | dx=ds*dxy3.X(); |
2774 | dy=ds*dxy3.Y(); |
2775 | Bx=Bx0+dBxdx*dx+dBxdy*dy+dBxdz*dz; |
2776 | By=By0+dBydx*dx+dBydy*dy+dBydz*dz; |
2777 | Bz=Bz0+dBzdx*dx+dBzdy*dy+dBzdz*dz; |
2778 | |
2779 | // Final derivative |
2780 | CalcDeriv(dxy4,S1,dEdx,D4); |
2781 | |
2782 | // New state vector |
2783 | // S+=ds*(ONE_SIXTH*D1+ONE_THIRD*D2+ONE_THIRD*D3+ONE_SIXTH*D4); |
2784 | double ds_over_6=ds*ONE_SIXTH0.16666666666666667; |
2785 | double ds_over_3=ds*ONE_THIRD0.33333333333333333; |
2786 | S+=ds_over_6*D1; |
2787 | S+=ds_over_3*D2; |
2788 | S+=ds_over_3*D3; |
2789 | S+=ds_over_6*D4; |
2790 | |
2791 | // New position |
2792 | //pos+=ds*(ONE_SIXTH*dpos1+ONE_THIRD*dpos2+ONE_THIRD*dpos3+ONE_SIXTH*dpos4); |
2793 | xy+=ds_over_6*dxy1; |
2794 | xy+=ds_over_3*dxy2; |
2795 | xy+=ds_over_3*dxy3; |
2796 | xy+=ds_over_6*dxy4; |
2797 | |
2798 | // Don't let the pt drop below some minimum |
2799 | //if (fabs(1./S(state_q_over_pt))<PT_MIN) { |
2800 | // S(state_q_over_pt)=(1./PT_MIN)*(S(state_q_over_pt)>0.0?1.:-1.); |
2801 | //} |
2802 | // Don't let tanl exceed some maximum |
2803 | if (fabs(S(state_tanl))>TAN_MAX10.){ |
2804 | S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0.0?1.:-1.); |
2805 | } |
2806 | |
2807 | return NOERROR; |
2808 | } |
2809 | |
2810 | // Assuming that the magnetic field is constant over the step, use a helical |
2811 | // model to step directly to the next point along the trajectory. |
2812 | void DTrackFitterKalmanSIMD::FastStep(double &z,double ds, double dEdx, |
2813 | DMatrix5x1 &S){ |
2814 | |
2815 | // Compute convenience terms involving Bx, By, Bz |
2816 | double one_over_p=fabs(S(state_q_over_p)); |
2817 | double p=1./one_over_p; |
2818 | double tx=S(state_tx),ty=S(state_ty); |
2819 | double denom=sqrt(1.+tx*tx+ty*ty); |
2820 | double px=p*tx/denom; |
2821 | double py=p*ty/denom; |
2822 | double pz=p/denom; |
2823 | double q=S(state_q_over_p)>0?1.:-1.; |
2824 | double k_q=qBr2p0.003*q; |
2825 | double ds_over_p=ds*one_over_p; |
2826 | double factor=k_q*(0.25*ds_over_p); |
2827 | double Ax=factor*Bx,Ay=factor*By,Az=factor*Bz; |
2828 | double Ax2=Ax*Ax,Ay2=Ay*Ay,Az2=Az*Az; |
2829 | double AxAy=Ax*Ay,AxAz=Ax*Az,AyAz=Ay*Az; |
2830 | double one_plus_Ax2=1.+Ax2; |
2831 | double scale=ds_over_p/(one_plus_Ax2+Ay2+Az2); |
2832 | |
2833 | // Compute new position |
2834 | double dx=scale*(px*one_plus_Ax2+py*(AxAy+Az)+pz*(AxAz-Ay)); |
2835 | double dy=scale*(px*(AxAy-Az)+py*(1.+Ay2)+pz*(AyAz+Ax)); |
2836 | double dz=scale*(px*(AxAz+Ay)+py*(AyAz-Ax)+pz*(1.+Az2)); |
2837 | S(state_x)+=dx; |
2838 | S(state_y)+=dy; |
2839 | z+=dz; |
2840 | |
2841 | // Compute new momentum |
2842 | px+=k_q*(Bz*dy-By*dz); |
2843 | py+=k_q*(Bx*dz-Bz*dx); |
2844 | pz+=k_q*(By*dx-Bx*dy); |
2845 | S(state_tx)=px/pz; |
2846 | S(state_ty)=py/pz; |
2847 | if (fabs(dEdx)>EPS3.0e-8){ |
2848 | double one_over_p_sq=one_over_p*one_over_p; |
2849 | double E=sqrt(1./one_over_p_sq+mass2); |
2850 | S(state_q_over_p)-=S(state_q_over_p)*one_over_p_sq*E*dEdx*ds; |
2851 | } |
2852 | } |
2853 | // Assuming that the magnetic field is constant over the step, use a helical |
2854 | // model to step directly to the next point along the trajectory. |
2855 | void DTrackFitterKalmanSIMD::FastStep(DVector2 &xy,double ds, double dEdx, |
2856 | DMatrix5x1 &S){ |
2857 | |
2858 | // Compute convenience terms involving Bx, By, Bz |
2859 | double pt=fabs(1./S(state_q_over_pt)); |
2860 | double one_over_p=cos(atan(S(state_tanl)))/pt; |
2861 | double px=pt*cos(S(state_phi)); |
2862 | double py=pt*sin(S(state_phi)); |
2863 | double pz=pt*S(state_tanl); |
2864 | double q=S(state_q_over_pt)>0?1.:-1.; |
2865 | double k_q=qBr2p0.003*q; |
2866 | double ds_over_p=ds*one_over_p; |
2867 | double factor=k_q*(0.25*ds_over_p); |
2868 | double Ax=factor*Bx,Ay=factor*By,Az=factor*Bz; |
2869 | double Ax2=Ax*Ax,Ay2=Ay*Ay,Az2=Az*Az; |
2870 | double AxAy=Ax*Ay,AxAz=Ax*Az,AyAz=Ay*Az; |
2871 | double one_plus_Ax2=1.+Ax2; |
2872 | double scale=ds_over_p/(one_plus_Ax2+Ay2+Az2); |
2873 | |
2874 | // Compute new position |
2875 | double dx=scale*(px*one_plus_Ax2+py*(AxAy+Az)+pz*(AxAz-Ay)); |
2876 | double dy=scale*(px*(AxAy-Az)+py*(1.+Ay2)+pz*(AyAz+Ax)); |
2877 | double dz=scale*(px*(AxAz+Ay)+py*(AyAz-Ax)+pz*(1.+Az2)); |
2878 | xy.Set(xy.X()+dx,xy.Y()+dy); |
2879 | S(state_z)+=dz; |
2880 | |
2881 | // Compute new momentum |
2882 | px+=k_q*(Bz*dy-By*dz); |
2883 | py+=k_q*(Bx*dz-Bz*dx); |
2884 | pz+=k_q*(By*dx-Bx*dy); |
2885 | pt=sqrt(px*px+py*py); |
2886 | S(state_q_over_pt)=q/pt; |
2887 | S(state_phi)=atan2(py,px); |
2888 | S(state_tanl)=pz/pt; |
2889 | if (fabs(dEdx)>EPS3.0e-8){ |
2890 | double one_over_p_sq=one_over_p*one_over_p; |
2891 | double E=sqrt(1./one_over_p_sq+mass2); |
2892 | S(state_q_over_p)-=S(state_q_over_pt)*one_over_p_sq*E*dEdx*ds; |
2893 | } |
2894 | } |
2895 | |
2896 | // Calculate the jacobian matrix for the alternate parameter set |
2897 | // {q/pT,phi,tanl(lambda),D,z} |
2898 | jerror_t DTrackFitterKalmanSIMD::StepJacobian(const DVector2 &xy, |
2899 | const DVector2 &dxy, |
2900 | double ds,const DMatrix5x1 &S, |
2901 | double dEdx,DMatrix5x5 &J){ |
2902 | // Initialize the Jacobian matrix |
2903 | //J.Zero(); |
2904 | //for (int i=0;i<5;i++) J(i,i)=1.; |
2905 | J=I5x5; |
2906 | |
2907 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
2908 | // B-field and gradient at current (x,y,z) |
2909 | //bfield->GetFieldAndGradient(xy.X(),xy.Y(),S(state_z),Bx,By,Bz, |
2910 | //dBxdx,dBxdy,dBxdz,dBydx, |
2911 | //dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
2912 | |
2913 | // Charge |
2914 | double q=(S(state_q_over_pt)>0.0)?1.:-1.; |
2915 | |
2916 | //kinematic quantities |
2917 | double q_over_pt=S(state_q_over_pt); |
2918 | double sinphi=sin(S(state_phi)); |
2919 | double cosphi=cos(S(state_phi)); |
2920 | double D=S(state_D); |
2921 | double lambda=atan(S(state_tanl)); |
2922 | double sinl=sin(lambda); |
2923 | double cosl=cos(lambda); |
2924 | double cosl2=cosl*cosl; |
2925 | double cosl3=cosl*cosl2; |
2926 | double one_over_cosl=1./cosl; |
2927 | double pt=fabs(1./q_over_pt); |
2928 | |
2929 | // Turn off dEdx if pt drops below some minimum |
2930 | if (pt<PT_MIN) { |
2931 | dEdx=0.; |
2932 | } |
2933 | double kds=qBr2p0.003*ds; |
2934 | double kq_ds_over_pt=kds*q_over_pt; |
2935 | double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi; |
2936 | double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi; |
2937 | |
2938 | // Jacobian matrix elements |
2939 | J(state_phi,state_phi)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2940 | J(state_phi,state_q_over_pt)=kds*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl); |
2941 | J(state_phi,state_tanl)=kq_ds_over_pt*(By_sinphi_plus_Bx_cosphi*cosl |
2942 | +Bz*sinl)*cosl2; |
2943 | J(state_phi,state_z) |
2944 | =kq_ds_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl); |
2945 | |
2946 | J(state_tanl,state_phi)=-kq_ds_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl; |
2947 | J(state_tanl,state_q_over_pt)=kds*By_cosphi_minus_Bx_sinphi*one_over_cosl; |
2948 | J(state_tanl,state_tanl)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2949 | J(state_tanl,state_z)=kq_ds_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl; |
2950 | J(state_q_over_pt,state_phi) |
2951 | =-kq_ds_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi; |
2952 | J(state_q_over_pt,state_q_over_pt) |
2953 | +=2.*kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi; |
2954 | J(state_q_over_pt,state_tanl) |
2955 | =kq_ds_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi; |
2956 | if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){ |
2957 | double p=pt*one_over_cosl; |
2958 | double p_sq=p*p; |
2959 | double m2_over_p2=mass2/p_sq; |
2960 | double E=sqrt(p_sq+mass2); |
2961 | double dE_over_E=dEdx*ds/E; |
2962 | |
2963 | J(state_q_over_pt,state_q_over_pt)-=dE_over_E*(2.+3.*m2_over_p2); |
2964 | J(state_q_over_pt,state_tanl)+=q*dE_over_E*sinl*(1.+2.*m2_over_p2)/p; |
2965 | } |
2966 | J(state_q_over_pt,state_z) |
2967 | =kq_ds_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi); |
2968 | J(state_z,state_tanl)=cosl3*ds; |
2969 | |
2970 | // Deal with changes in D |
2971 | double B=sqrt(Bx*Bx+By*By+Bz*Bz); |
2972 | //double qrc_old=qpt/(qBr2p*fabs(Bz)); |
2973 | double qpt=FactorForSenseOfRotation/q_over_pt; |
2974 | double qrc_old=qpt/(qBr2p0.003*B); |
2975 | double qrc_plus_D=D+qrc_old; |
2976 | double dx=dxy.X(); |
2977 | double dy=dxy.Y(); |
2978 | double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi; |
2979 | double rc=sqrt(dxy.Mod2() |
2980 | +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi) |
2981 | +qrc_plus_D*qrc_plus_D); |
2982 | double q_over_rc=FactorForSenseOfRotation*q/rc; |
2983 | |
2984 | J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D); |
2985 | J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.); |
2986 | J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi); |
2987 | |
2988 | return NOERROR; |
2989 | } |
2990 | |
2991 | |
2992 | |
2993 | |
2994 | // Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z} |
2995 | jerror_t DTrackFitterKalmanSIMD::StepJacobian(const DVector2 &xy, |
2996 | double ds,const DMatrix5x1 &S, |
2997 | double dEdx,DMatrix5x5 &J){ |
2998 | // Initialize the Jacobian matrix |
2999 | //J.Zero(); |
3000 | //for (int i=0;i<5;i++) J(i,i)=1.; |
3001 | J=I5x5; |
3002 | |
3003 | if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small |
3004 | |
3005 | // Matrices for intermediate steps |
3006 | DMatrix5x5 J1; |
3007 | DMatrix5x1 D1; |
3008 | DVector2 dxy1; |
3009 | |
3010 | // charge |
3011 | double q=(S(state_q_over_pt)>0.0)?1.:-1.; |
3012 | q*=FactorForSenseOfRotation; |
3013 | |
3014 | //kinematic quantities |
3015 | double qpt=1./S(state_q_over_pt) * FactorForSenseOfRotation; |
3016 | double sinphi=sin(S(state_phi)); |
3017 | double cosphi=cos(S(state_phi)); |
3018 | double D=S(state_D); |
3019 | |
3020 | CalcDerivAndJacobian(xy,dxy1,S,dEdx,J1,D1); |
3021 | // double Bz_=fabs(Bz); // needed for computing D |
3022 | |
3023 | // New Jacobian matrix |
3024 | J+=ds*J1; |
3025 | |
3026 | // change in position |
3027 | DVector2 dxy =ds*dxy1; |
3028 | |
3029 | // Deal with changes in D |
3030 | double B=sqrt(Bx*Bx+By*By+Bz*Bz); |
3031 | //double qrc_old=qpt/(qBr2p*Bz_); |
3032 | double qrc_old=qpt/(qBr2p0.003*B); |
3033 | double qrc_plus_D=D+qrc_old; |
3034 | double dx=dxy.X(); |
3035 | double dy=dxy.Y(); |
3036 | double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi; |
3037 | double rc=sqrt(dxy.Mod2() |
3038 | +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi) |
3039 | +qrc_plus_D*qrc_plus_D); |
3040 | double q_over_rc=q/rc; |
3041 | |
3042 | J(state_D,state_D)=q_over_rc*(dx_sinphi_minus_dy_cosphi+qrc_plus_D); |
3043 | J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.); |
3044 | J(state_D,state_phi)=q_over_rc*qrc_plus_D*(dx*cosphi+dy*sinphi); |
3045 | |
3046 | return NOERROR; |
3047 | } |
3048 | |
3049 | // Compute contributions to the covariance matrix due to multiple scattering |
3050 | // using the Lynch/Dahl empirical formulas |
3051 | jerror_t DTrackFitterKalmanSIMD::GetProcessNoiseCentral(double ds, |
3052 | double chi2c_factor, |
3053 | double chi2a_factor, |
3054 | double chi2a_corr, |
3055 | const DMatrix5x1 &Sc, |
3056 | DMatrix5x5 &Q){ |
3057 | Q.Zero(); |
3058 | //return NOERROR; |
3059 | if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS3.0e-8){ |
3060 | double tanl=Sc(state_tanl); |
3061 | double tanl2=tanl*tanl; |
3062 | double one_plus_tanl2=1.+tanl2; |
3063 | double one_over_pt=fabs(Sc(state_q_over_pt)); |
3064 | double my_ds=fabs(ds); |
3065 | double my_ds_2=0.5*my_ds; |
3066 | |
3067 | Q(state_phi,state_phi)=one_plus_tanl2; |
3068 | Q(state_tanl,state_tanl)=one_plus_tanl2*one_plus_tanl2; |
3069 | Q(state_q_over_pt,state_q_over_pt)=one_over_pt*one_over_pt*tanl2; |
3070 | Q(state_q_over_pt,state_tanl)=Q(state_tanl,state_q_over_pt) |
3071 | =Sc(state_q_over_pt)*tanl*one_plus_tanl2; |
3072 | Q(state_D,state_D)=ONE_THIRD0.33333333333333333*ds*ds; |
3073 | |
3074 | // I am not sure the following is correct... |
3075 | double sign_D=(Sc(state_D)>0.0)?1.:-1.; |
3076 | Q(state_D,state_phi)=Q(state_phi,state_D)=sign_D*my_ds_2*sqrt(one_plus_tanl2); |
3077 | Q(state_D,state_tanl)=Q(state_tanl,state_D)=sign_D*my_ds_2*one_plus_tanl2; |
3078 | 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); |
3079 | |
3080 | double one_over_p_sq=one_over_pt*one_over_pt/one_plus_tanl2; |
3081 | double one_over_beta2=1.+mass2*one_over_p_sq; |
3082 | double chi2c_p_sq=chi2c_factor*my_ds*one_over_beta2; |
3083 | double chi2a_p_sq=chi2a_factor*(1.+chi2a_corr*one_over_beta2); |
3084 | // F=Fraction of Moliere distribution to be taken into account |
3085 | // nu=0.5*chi2c/(chi2a*(1.-F)); |
3086 | double nu=MOLIERE_RATIO1*chi2c_p_sq/chi2a_p_sq; |
3087 | double one_plus_nu=1.+nu; |
3088 | // sig2_ms=chi2c*1e-6/(1.+F*F)*((one_plus_nu)/nu*log(one_plus_nu)-1.); |
3089 | double sig2_ms=chi2c_p_sq*one_over_p_sq*MOLIERE_RATIO2 |
3090 | *(one_plus_nu/nu*log(one_plus_nu)-1.); |
3091 | |
3092 | Q*=sig2_ms; |
3093 | } |
3094 | |
3095 | return NOERROR; |
3096 | } |
3097 | |
3098 | // Compute contributions to the covariance matrix due to multiple scattering |
3099 | // using the Lynch/Dahl empirical formulas |
3100 | jerror_t DTrackFitterKalmanSIMD::GetProcessNoise(double z, double ds, |
3101 | double chi2c_factor, |
3102 | double chi2a_factor, |
3103 | double chi2a_corr, |
3104 | const DMatrix5x1 &S, |
3105 | DMatrix5x5 &Q){ |
3106 | |
3107 | Q.Zero(); |
3108 | //return NOERROR; |
3109 | if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS3.0e-8){ |
3110 | double tx=S(state_tx),ty=S(state_ty); |
3111 | double one_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
3112 | double my_ds=fabs(ds); |
3113 | double my_ds_2=0.5*my_ds; |
3114 | double tx2=tx*tx; |
3115 | double ty2=ty*ty; |
3116 | double one_plus_tx2=1.+tx2; |
3117 | double one_plus_ty2=1.+ty2; |
3118 | double tsquare=tx2+ty2; |
3119 | double one_plus_tsquare=1.+tsquare; |
3120 | |
3121 | Q(state_tx,state_tx)=one_plus_tx2*one_plus_tsquare; |
3122 | Q(state_ty,state_ty)=one_plus_ty2*one_plus_tsquare; |
3123 | Q(state_tx,state_ty)=Q(state_ty,state_tx)=tx*ty*one_plus_tsquare; |
3124 | |
3125 | Q(state_x,state_x)=ONE_THIRD0.33333333333333333*ds*ds; |
3126 | Q(state_y,state_y)=Q(state_x,state_x); |
3127 | Q(state_y,state_ty)=Q(state_ty,state_y) |
3128 | = my_ds_2*sqrt(one_plus_tsquare*one_plus_ty2); |
3129 | Q(state_x,state_tx)=Q(state_tx,state_x) |
3130 | = my_ds_2*sqrt(one_plus_tsquare*one_plus_tx2); |
3131 | |
3132 | double one_over_beta2=1.+one_over_p_sq*mass2; |
3133 | double chi2c_p_sq=chi2c_factor*my_ds*one_over_beta2; |
3134 | double chi2a_p_sq=chi2a_factor*(1.+chi2a_corr*one_over_beta2); |
3135 | // F=MOLIERE_FRACTION =Fraction of Moliere distribution to be taken into account |
3136 | // nu=0.5*chi2c/(chi2a*(1.-F)); |
3137 | double nu=MOLIERE_RATIO1*chi2c_p_sq/chi2a_p_sq; |
3138 | double one_plus_nu=1.+nu; |
3139 | double sig2_ms=chi2c_p_sq*one_over_p_sq*MOLIERE_RATIO2 |
3140 | *(one_plus_nu/nu*log(one_plus_nu)-1.); |
3141 | |
3142 | // printf("fac %f %f %f\n",chi2c_factor,chi2a_factor,chi2a_corr); |
3143 | //printf("omega %f\n",chi2c/chi2a); |
3144 | |
3145 | |
3146 | Q*=sig2_ms; |
3147 | } |
3148 | |
3149 | return NOERROR; |
3150 | } |
3151 | |
3152 | // Calculate the energy loss per unit length given properties of the material |
3153 | // through which a particle of momentum p is passing |
3154 | double DTrackFitterKalmanSIMD::GetdEdx(double q_over_p,double K_rho_Z_over_A, |
3155 | double rho_Z_over_A,double LnI,double Z){ |
3156 | if (rho_Z_over_A<=0.) return 0.; |
3157 | //return 0.; |
3158 | |
3159 | double p=fabs(1./q_over_p); |
3160 | double betagamma=p/MASS; |
3161 | double betagamma2=betagamma*betagamma; |
3162 | double gamma2=1.+betagamma2; |
3163 | double beta2=betagamma2/gamma2; |
3164 | if (beta2<EPS3.0e-8) beta2=EPS3.0e-8; |
3165 | |
3166 | // density effect |
3167 | double delta=CalcDensityEffect(betagamma,rho_Z_over_A,LnI); |
3168 | |
3169 | double dEdx=0.; |
3170 | // For particles heavier than electrons: |
3171 | if (IsHadron){ |
3172 | double two_Me_betagamma_sq=two_m_e*betagamma2; |
3173 | double Tmax |
3174 | =two_Me_betagamma_sq/(1.+2.*sqrt(gamma2)*m_ratio+m_ratio_sq); |
3175 | |
3176 | dEdx=K_rho_Z_over_A/beta2*(-log(two_Me_betagamma_sq*Tmax) |
3177 | +2.*(LnI + beta2)+delta); |
3178 | } |
3179 | else{ |
3180 | // relativistic kinetic energy in units of M_e c^2 |
3181 | double tau=sqrt(gamma2)-1.; |
3182 | double tau_sq=tau*tau; |
3183 | double tau_plus_1=tau+1.; |
3184 | double tau_plus_2=tau+2.; |
3185 | double tau_plus_2_sq=tau_plus_2*tau_plus_2; |
3186 | double f=0.; // function that depends on tau; see Leo (2nd ed.), p. 38. |
3187 | if (IsElectron){ |
3188 | f=1.-beta2+(0.125*tau_sq-(2.*tau+1.)*log(2.))/(tau_plus_1*tau_plus_1); |
3189 | } |
3190 | else{ |
3191 | f=2.*log(2.)-(beta2/12.)*(23.+14./tau_plus_2+10./tau_plus_2_sq |
3192 | +4./(tau_plus_2*tau_plus_2_sq)); |
3193 | } |
3194 | |
3195 | // collision loss (Leo eq. 2.66) |
3196 | double dEdx_coll |
3197 | =-K_rho_Z_over_A/beta2*(log(0.5*tau_sq*tau_plus_2*m_e_sq)-LnI+f-delta); |
3198 | |
3199 | // radiation loss (Leo eqs. 2.74, 2.76 with Z^2 -> Z(Z+1) |
3200 | double a=Z*ALPHA1./137.; |
3201 | double a2=a*a; |
3202 | double a4=a2*a2; |
3203 | double epsilon=1.-PHOTON_ENERGY_CUTOFF; |
3204 | double epsilon2=epsilon*epsilon; |
3205 | double f_Z=a2*(1./(1.+a2)+0.20206-0.0369*a2+0.0083*a4-0.002*a2*a4); |
3206 | // The expression below is the integral of the photon energy weighted |
3207 | // by the bremsstrahlung cross section up to a maximum photon energy |
3208 | // expressed as a fraction of the incident electron energy. |
3209 | double dEdx_rad=-K_rho_Z_over_A*tau_plus_1*(2.*ALPHA1./137./M_PI3.14159265358979323846)*(Z+1.) |
3210 | *((log(183.*pow(Z,-1./3.))-f_Z) |
3211 | *(1.-epsilon-(1./3.)*(epsilon2-epsilon*epsilon2)) |
3212 | +1./18.*(1.-epsilon2)); |
3213 | |
3214 | |
3215 | // dEdx_rad=0.; |
3216 | |
3217 | dEdx=dEdx_coll+dEdx_rad; |
3218 | } |
3219 | |
3220 | return dEdx; |
3221 | } |
3222 | |
3223 | // Calculate the variance in the energy loss in a Gaussian approximation. |
3224 | // The standard deviation of the energy loss distribution is |
3225 | // var=0.1535*density*(Z/A)*x*(1-0.5*beta^2)*Tmax [MeV] |
3226 | // where Tmax is the maximum energy transfer. |
3227 | // (derived from Leo (2nd ed.), eq. 2.100. Note that I think there is a typo |
3228 | // in this formula in the text...) |
3229 | double DTrackFitterKalmanSIMD::GetEnergyVariance(double ds, |
3230 | double one_over_beta2, |
3231 | double K_rho_Z_over_A){ |
3232 | if (K_rho_Z_over_A<=0.) return 0.; |
3233 | |
3234 | double betagamma2=1./(one_over_beta2-1.); |
3235 | double gamma2=betagamma2*one_over_beta2; |
3236 | double two_Me_betagamma_sq=two_m_e*betagamma2; |
3237 | double Tmax=two_Me_betagamma_sq/(1.+2.*sqrt(gamma2)*m_ratio+m_ratio_sq); |
3238 | double var=K_rho_Z_over_A*one_over_beta2*fabs(ds)*Tmax*(1.-0.5/one_over_beta2); |
3239 | return var; |
3240 | } |
3241 | |
3242 | // Interface routine for Kalman filter |
3243 | jerror_t DTrackFitterKalmanSIMD::KalmanLoop(void){ |
3244 | if (z_<Z_MIN-100.) return VALUE_OUT_OF_RANGE; |
3245 | |
3246 | // Vector to store the list of hits used in the fit for the forward parametrization |
3247 | vector<const DCDCTrackHit*>forward_cdc_used_in_fit; |
3248 | |
3249 | // State vector and initial guess for covariance matrix |
3250 | DMatrix5x1 S0; |
3251 | DMatrix5x5 C0; |
3252 | |
3253 | chisq_=-1.; |
3254 | |
3255 | // Angle with respect to beam line |
3256 | double theta_deg=(180/M_PI3.14159265358979323846)*input_params.momentum().Theta(); |
3257 | //double theta_deg_sq=theta_deg*theta_deg; |
3258 | double tanl0=tanl_=tan(M_PI_21.57079632679489661923-input_params.momentum().Theta()); |
3259 | |
3260 | // Azimuthal angle |
3261 | double phi0=phi_=input_params.momentum().Phi(); |
3262 | |
3263 | // Guess for momentum error |
3264 | double dpt_over_pt=0.1; |
3265 | /* |
3266 | if (theta_deg<15){ |
3267 | dpt_over_pt=0.107-0.0178*theta_deg+0.000966*theta_deg_sq; |
3268 | } |
3269 | else { |
3270 | dpt_over_pt=0.0288+0.00579*theta_deg-2.77e-5*theta_deg_sq; |
3271 | } |
3272 | */ |
3273 | /* |
3274 | if (theta_deg<28.){ |
3275 | theta_deg=28.; |
3276 | theta_deg_sq=theta_deg*theta_deg; |
3277 | } |
3278 | else if (theta_deg>125.){ |
3279 | theta_deg=125.; |
3280 | theta_deg_sq=theta_deg*theta_deg; |
3281 | } |
3282 | */ |
3283 | double sig_lambda=0.02; |
3284 | double dp_over_p_sq |
3285 | =dpt_over_pt*dpt_over_pt+tanl_*tanl_*sig_lambda*sig_lambda; |
3286 | |
3287 | // Input charge |
3288 | double q=input_params.charge(); |
3289 | |
3290 | // Input momentum |
3291 | DVector3 pvec=input_params.momentum(); |
3292 | double p_mag=pvec.Mag(); |
3293 | double px=pvec.x(); |
3294 | double py=pvec.y(); |
3295 | double pz=pvec.z(); |
3296 | double q_over_p0=q_over_p_=q/p_mag; |
3297 | double q_over_pt0=q_over_pt_=q/pvec.Perp(); |
3298 | |
3299 | // Initial position |
3300 | double x0=x_=input_params.position().x(); |
3301 | double y0=y_=input_params.position().y(); |
3302 | double z0=z_=input_params.position().z(); |
3303 | |
3304 | if (fit_type==kWireBased && theta_deg>10.){ |
3305 | double Bz=fabs(bfield->GetBz(x0,y0,z0)); |
3306 | double sperp=25.; // empirical guess |
3307 | if (my_fdchits.size()>0 && my_fdchits[0]->hit!=NULL__null){ |
3308 | double my_z=my_fdchits[0]->z; |
3309 | double my_x=my_fdchits[0]->hit->xy.X(); |
3310 | double my_y=my_fdchits[0]->hit->xy.Y(); |
3311 | Bz+=fabs(bfield->GetBz(my_x,my_y,my_z)); |
3312 | Bz*=0.5; // crude average |
3313 | sperp=(my_z-z0)/tanl_; |
3314 | } |
3315 | double twokappa=qBr2p0.003*Bz*q_over_pt0*FactorForSenseOfRotation; |
3316 | double one_over_2k=1./twokappa; |
3317 | if (my_fdchits.size()==0){ |
3318 | for (unsigned int i=my_cdchits.size()-1;i>1;i--){ |
3319 | // Use outermost axial straw to estimate a resonable arc length |
3320 | if (my_cdchits[i]->hit->is_stereo==false){ |
3321 | double tworc=2.*fabs(one_over_2k); |
3322 | double ratio=(my_cdchits[i]->hit->wire->origin |
3323 | -input_params.position()).Perp()/tworc; |
3324 | sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_21.57079632679489661923; |
3325 | if (sperp<25.) sperp=25.; |
3326 | break; |
3327 | } |
3328 | } |
3329 | } |
3330 | double twoks=twokappa*sperp; |
3331 | double cosphi=cos(phi0); |
3332 | double sinphi=sin(phi0); |
3333 | double sin2ks=sin(twoks); |
3334 | double cos2ks=cos(twoks); |
3335 | double one_minus_cos2ks=1.-cos2ks; |
3336 | double myx=x0+one_over_2k*(cosphi*sin2ks-sinphi*one_minus_cos2ks); |
3337 | double myy=y0+one_over_2k*(sinphi*sin2ks+cosphi*one_minus_cos2ks); |
3338 | double mypx=px*cos2ks-py*sin2ks; |
3339 | double mypy=py*cos2ks+px*sin2ks; |
3340 | double myphi=atan2(mypy,mypx); |
3341 | phi0=phi_=myphi; |
3342 | px=mypx; |
3343 | py=mypy; |
3344 | x0=x_=myx; |
3345 | y0=y_=myy; |
3346 | z0+=tanl_*sperp; |
3347 | z_=z0; |
3348 | } |
3349 | |
3350 | // Check integrity of input parameters |
3351 | if (!isfinite(x0) || !isfinite(y0) || !isfinite(q_over_p0)){ |
3352 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3352<<" " << "Invalid input parameters!" <<endl; |
3353 | return UNRECOVERABLE_ERROR; |
3354 | } |
3355 | |
3356 | // Initial direction tangents |
3357 | double tx0=tx_=px/pz; |
3358 | double ty0=ty_=py/pz; |
3359 | double one_plus_tsquare=1.+tx_*tx_+ty_*ty_; |
3360 | |
3361 | // deal with hits in FDC |
3362 | double fdc_prob=0.,fdc_chisq=-1.; |
3363 | unsigned int fdc_ndf=0; |
3364 | if (my_fdchits.size()>0 |
3365 | && // Make sure that these parameters are valid for forward-going tracks |
3366 | (isfinite(tx0) && isfinite(ty0)) |
3367 | ){ |
3368 | if (DEBUG_LEVEL>0){ |
3369 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3369<<" " << "Using forward parameterization." <<endl; |
3370 | } |
3371 | |
3372 | // Initial guess for the state vector |
3373 | S0(state_x)=x_; |
3374 | S0(state_y)=y_; |
3375 | S0(state_tx)=tx_; |
3376 | S0(state_ty)=ty_; |
3377 | S0(state_q_over_p)=q_over_p_; |
3378 | |
3379 | // Initial guess for forward representation covariance matrix |
3380 | C0(state_x,state_x)=2.0; |
3381 | C0(state_y,state_y)=2.0; |
3382 | double temp=sig_lambda*one_plus_tsquare; |
3383 | C0(state_tx,state_tx)=C0(state_ty,state_ty)=temp*temp; |
3384 | C0(state_q_over_p,state_q_over_p)=dp_over_p_sq*q_over_p_*q_over_p_; |
3385 | C0*=COVARIANCE_SCALE_FACTOR_FORWARD; |
3386 | |
3387 | if (my_cdchits.size()>0){ |
3388 | mCDCInternalStepSize=0.25; |
3389 | } |
3390 | |
3391 | // The position from the track candidate is reported just outside the |
3392 | // start counter for tracks containing cdc hits. Propagate to the distance |
3393 | // of closest approach to the beam line |
3394 | if (fit_type==kWireBased) ExtrapolateToVertex(S0); |
3395 | |
3396 | kalman_error_t error=ForwardFit(S0,C0); |
3397 | if (error==FIT_SUCCEEDED) return NOERROR; |
3398 | if ((error==FIT_FAILED || ndf_==0) && my_cdchits.size()<6){ |
3399 | return UNRECOVERABLE_ERROR; |
3400 | } |
3401 | |
3402 | fdc_prob=TMath::Prob(chisq_,ndf_); |
3403 | fdc_ndf=ndf_; |
3404 | fdc_chisq=chisq_; |
3405 | } |
3406 | |
3407 | // Deal with hits in the CDC |
3408 | if (my_cdchits.size()>5){ |
3409 | kalman_error_t error=FIT_NOT_DONE; |
3410 | kalman_error_t cdc_error=FIT_NOT_DONE; |
3411 | |
3412 | // Save the current state of the extrapolation vector if it exists |
3413 | map<DetectorSystem_t,vector<Extrapolation_t> >saved_extrapolations; |
3414 | if (!extrapolations.empty()){ |
3415 | saved_extrapolations=extrapolations; |
3416 | ClearExtrapolations(); |
3417 | } |
3418 | bool save_IsSmoothed=IsSmoothed; |
3419 | |
3420 | // Chi-squared, degrees of freedom, and probability |
3421 | double forward_prob=0.; |
3422 | double chisq_forward=-1.; |
3423 | unsigned int ndof_forward=0; |
3424 | |
3425 | // Parameters at "vertex" |
3426 | double phi=phi_,q_over_pt=q_over_pt_,tanl=tanl_,x=x_,y=y_,z=z_; |
3427 | vector< vector <double> > fcov_save; |
3428 | vector<pull_t>pulls_save; |
3429 | pulls_save.assign(pulls.begin(),pulls.end()); |
3430 | if (!fcov.empty()){ |
3431 | fcov_save.assign(fcov.begin(),fcov.end()); |
3432 | } |
3433 | if (my_fdchits.size()>0){ |
3434 | if (error==INVALID_FIT) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3434<<" "<< "Invalid fit " << fcov.size() << " " << fdc_ndf <<endl; |
3435 | } |
3436 | |
3437 | // Use forward parameters for CDC-only tracks with theta<THETA_CUT degrees |
3438 | if (theta_deg<THETA_CUT){ |
3439 | if (DEBUG_LEVEL>0){ |
3440 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3440<<" " << "Using forward parameterization." <<endl; |
3441 | } |
3442 | |
3443 | // Step size |
3444 | mStepSizeS=mCentralStepSize; |
3445 | |
3446 | // Initialize the state vector |
3447 | S0(state_x)=x_=x0; |
3448 | S0(state_y)=y_=y0; |
3449 | S0(state_tx)=tx_=tx0; |
3450 | S0(state_ty)=ty_=ty0; |
3451 | S0(state_q_over_p)=q_over_p_=q_over_p0; |
3452 | z_=z0; |
3453 | |
3454 | // Initial guess for forward representation covariance matrix |
3455 | double temp=sig_lambda*one_plus_tsquare; |
3456 | C0(state_x,state_x)=2.0; |
3457 | C0(state_y,state_y)=2.0; |
3458 | C0(state_tx,state_tx)=C0(state_ty,state_ty)=temp*temp; |
3459 | C0(state_q_over_p,state_q_over_p)=dp_over_p_sq*q_over_p_*q_over_p_; |
3460 | C0*=COVARIANCE_SCALE_FACTOR_FORWARD; |
3461 | |
3462 | //C0*=1.+1./tsquare; |
3463 | |
3464 | // The position from the track candidate is reported just outside the |
3465 | // start counter for tracks containing cdc hits. Propagate to the |
3466 | // distance of closest approach to the beam line |
3467 | if (fit_type==kWireBased) ExtrapolateToVertex(S0); |
3468 | |
3469 | error=ForwardCDCFit(S0,C0); |
3470 | if (error==FIT_SUCCEEDED) return NOERROR; |
3471 | |
3472 | // Find the CL of the fit |
3473 | forward_prob=TMath::Prob(chisq_,ndf_); |
3474 | if (my_fdchits.size()>0){ |
3475 | if (fdc_ndf==0 || forward_prob>fdc_prob){ |
3476 | // We did not end up using the fdc hits after all... |
3477 | fdchits_used_in_fit.clear(); |
3478 | } |
3479 | else{ |
3480 | chisq_=fdc_chisq; |
3481 | ndf_=fdc_ndf; |
3482 | x_=x; |
3483 | y_=y; |
3484 | z_=z; |
3485 | phi_=phi; |
3486 | tanl_=tanl; |
3487 | q_over_pt_=q_over_pt; |
3488 | if (!fcov_save.empty()){ |
3489 | fcov.assign(fcov_save.begin(),fcov_save.end()); |
3490 | } |
3491 | if (!saved_extrapolations.empty()){ |
3492 | extrapolations=saved_extrapolations; |
3493 | } |
3494 | IsSmoothed=save_IsSmoothed; |
3495 | pulls.assign(pulls_save.begin(),pulls_save.end()); |
3496 | |
3497 | // _DBG_ << endl; |
3498 | return NOERROR; |
3499 | } |
3500 | |
3501 | // Save the best values for the parameters and chi2 for now |
3502 | chisq_forward=chisq_; |
3503 | ndof_forward=ndf_; |
3504 | x=x_; |
3505 | y=y_; |
3506 | z=z_; |
3507 | phi=phi_; |
3508 | tanl=tanl_; |
3509 | q_over_pt=q_over_pt_; |
3510 | fcov_save.assign(fcov.begin(),fcov.end()); |
3511 | pulls_save.assign(pulls.begin(),pulls.end()); |
3512 | save_IsSmoothed=IsSmoothed; |
3513 | if (!extrapolations.empty()){ |
3514 | saved_extrapolations=extrapolations; |
3515 | ClearExtrapolations(); |
3516 | } |
3517 | |
3518 | // Save the list of hits used in the fit |
3519 | forward_cdc_used_in_fit.assign(cdchits_used_in_fit.begin(),cdchits_used_in_fit.end()); |
3520 | |
3521 | } |
3522 | } |
3523 | |
3524 | // Attempt to fit the track using the central parametrization. |
3525 | if (DEBUG_LEVEL>0){ |
3526 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3526<<" " << "Using central parameterization." <<endl; |
3527 | } |
3528 | |
3529 | // Step size |
3530 | mStepSizeS=mCentralStepSize; |
3531 | |
3532 | // Initialize the state vector |
3533 | S0(state_q_over_pt)=q_over_pt_=q_over_pt0; |
3534 | S0(state_phi)=phi_=phi0; |
3535 | S0(state_tanl)=tanl_=tanl0; |
3536 | S0(state_z)=z_=z0; |
3537 | S0(state_D)=0.; |
3538 | |
3539 | // Initialize the covariance matrix |
3540 | double dz=1.0; |
3541 | C0(state_z,state_z)=dz*dz; |
3542 | C0(state_q_over_pt,state_q_over_pt) |
3543 | =q_over_pt_*q_over_pt_*dpt_over_pt*dpt_over_pt; |
3544 | double dphi=0.02; |
3545 | C0(state_phi,state_phi)=dphi*dphi; |
3546 | C0(state_D,state_D)=1.0; |
3547 | double tanl2=tanl_*tanl_; |
3548 | double one_plus_tanl2=1.+tanl2; |
3549 | C0(state_tanl,state_tanl)=(one_plus_tanl2)*(one_plus_tanl2) |
3550 | *sig_lambda*sig_lambda; |
3551 | C0*=COVARIANCE_SCALE_FACTOR_CENTRAL; |
3552 | |
3553 | //if (theta_deg>90.) C0*=1.+5.*tanl2; |
3554 | //else C0*=1.+5.*tanl2*tanl2; |
3555 | |
3556 | mCentralStepSize=0.4; |
3557 | mCDCInternalStepSize=0.2; |
3558 | |
3559 | // The position from the track candidate is reported just outside the |
3560 | // start counter for tracks containing cdc hits. Propagate to the |
3561 | // distance of closest approach to the beam line |
3562 | DVector2 xy(x0,y0); |
3563 | if (fit_type==kWireBased){ |
3564 | ExtrapolateToVertex(xy,S0); |
3565 | } |
3566 | |
3567 | cdc_error=CentralFit(xy,S0,C0); |
3568 | if (cdc_error==FIT_SUCCEEDED){ |
3569 | // if the result of the fit using the forward parameterization succeeded |
3570 | // but the chi2 was too high, it still may provide the best estimate |
3571 | // for the track parameters... |
3572 | double central_prob=TMath::Prob(chisq_,ndf_); |
3573 | |
3574 | if (central_prob<forward_prob){ |
3575 | phi_=phi; |
3576 | q_over_pt_=q_over_pt; |
3577 | tanl_=tanl; |
3578 | x_=x; |
3579 | y_=y; |
3580 | z_=z; |
3581 | chisq_=chisq_forward; |
3582 | ndf_= ndof_forward; |
3583 | fcov.assign(fcov_save.begin(),fcov_save.end()); |
3584 | pulls.assign(pulls_save.begin(),pulls_save.end()); |
3585 | IsSmoothed=save_IsSmoothed; |
3586 | if (!saved_extrapolations.empty()){ |
3587 | extrapolations=saved_extrapolations; |
3588 | } |
3589 | |
3590 | cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end()); |
3591 | |
3592 | // We did not end up using any fdc hits... |
3593 | fdchits_used_in_fit.clear(); |
3594 | |
3595 | } |
3596 | return NOERROR; |
3597 | |
3598 | } |
3599 | // otherwise if the fit using the forward parametrization worked, return that |
3600 | else if (error!=FIT_FAILED){ |
3601 | phi_=phi; |
3602 | q_over_pt_=q_over_pt; |
3603 | tanl_=tanl; |
3604 | x_=x; |
3605 | y_=y; |
3606 | z_=z; |
3607 | chisq_=chisq_forward; |
3608 | ndf_= ndof_forward; |
3609 | |
3610 | if (!saved_extrapolations.empty()){ |
3611 | extrapolations=saved_extrapolations; |
3612 | } |
3613 | IsSmoothed=save_IsSmoothed; |
3614 | fcov.assign(fcov_save.begin(),fcov_save.end()); |
3615 | pulls.assign(pulls_save.begin(),pulls_save.end()); |
3616 | cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end()); |
3617 | |
3618 | // We did not end up using any fdc hits... |
3619 | fdchits_used_in_fit.clear(); |
3620 | |
3621 | return NOERROR; |
3622 | } |
3623 | else return UNRECOVERABLE_ERROR; |
3624 | } |
3625 | |
3626 | if (ndf_==0) return UNRECOVERABLE_ERROR; |
3627 | |
3628 | return NOERROR; |
3629 | } |
3630 | |
3631 | #define ITMAX20 20 |
3632 | #define CGOLD0.3819660 0.3819660 |
3633 | #define ZEPS1.0e-10 1.0e-10 |
3634 | #define SHFT(a,b,c,d)(a)=(b);(b)=(c);(c)=(d); (a)=(b);(b)=(c);(c)=(d); |
3635 | #define SIGN(a,b)((b)>=0.0?fabs(a):-fabs(a)) ((b)>=0.0?fabs(a):-fabs(a)) |
3636 | // Routine for finding the minimum of a function bracketed between two values |
3637 | // (see Numerical Recipes in C, pp. 404-405). |
3638 | jerror_t DTrackFitterKalmanSIMD::BrentsAlgorithm(double ds1,double ds2, |
3639 | double dedx,DVector2 &pos, |
3640 | const double z0wire, |
3641 | const DVector2 &origin, |
3642 | const DVector2 &dir, |
3643 | DMatrix5x1 &Sc, |
3644 | double &ds_out){ |
3645 | double d=0.; |
3646 | double e=0.0; // will be distance moved on step before last |
3647 | double ax=0.; |
3648 | double bx=-ds1; |
3649 | double cx=-ds1-ds2; |
3650 | |
3651 | double a=(ax<cx?ax:cx); |
3652 | double b=(ax>cx?ax:cx); |
3653 | double x=bx,w=bx,v=bx; |
3654 | |
3655 | // printf("ds1 %f ds2 %f\n",ds1,ds2); |
3656 | // Initialize return step size |
3657 | ds_out=0.; |
3658 | |
3659 | // Save the starting position |
3660 | // DVector3 pos0=pos; |
3661 | // DMatrix S0(Sc); |
3662 | |
3663 | // Step to intermediate point |
3664 | Step(pos,x,Sc,dedx); |
3665 | // Bail if the transverse momentum has dropped below some minimum |
3666 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3667 | if (DEBUG_LEVEL>2) |
3668 | { |
3669 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3669<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3670 | << endl; |
3671 | } |
3672 | return VALUE_OUT_OF_RANGE; |
3673 | } |
3674 | |
3675 | DVector2 wirepos=origin+(Sc(state_z)-z0wire)*dir; |
3676 | double u_old=x; |
3677 | double u=0.; |
3678 | |
3679 | // initialization |
3680 | double fw=(pos-wirepos).Mod2(); |
3681 | double fv=fw,fx=fw; |
3682 | |
3683 | // main loop |
3684 | for (unsigned int iter=1;iter<=ITMAX20;iter++){ |
3685 | double xm=0.5*(a+b); |
3686 | double tol1=EPS21.e-4*fabs(x)+ZEPS1.0e-10; |
3687 | double tol2=2.0*tol1; |
3688 | |
3689 | if (fabs(x-xm)<=(tol2-0.5*(b-a))){ |
3690 | if (Sc(state_z)<=cdc_origin[2]){ |
3691 | unsigned int iter2=0; |
3692 | double ds_temp=0.; |
3693 | while (fabs(Sc(state_z)-cdc_origin[2])>EPS21.e-4 && iter2<20){ |
3694 | u=x-(cdc_origin[2]-Sc(state_z))*sin(atan(Sc(state_tanl))); |
3695 | x=u; |
3696 | ds_temp+=u_old-u; |
3697 | // Bail if the transverse momentum has dropped below some minimum |
3698 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3699 | if (DEBUG_LEVEL>2) |
3700 | { |
3701 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3701<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3702 | << endl; |
3703 | } |
3704 | return VALUE_OUT_OF_RANGE; |
3705 | } |
3706 | |
3707 | // Function evaluation |
3708 | Step(pos,u_old-u,Sc,dedx); |
3709 | u_old=u; |
3710 | iter2++; |
3711 | } |
3712 | //printf("new z %f ds %f \n",pos.z(),x); |
3713 | ds_out=ds_temp; |
3714 | return NOERROR; |
3715 | } |
3716 | else if (Sc(state_z)>=endplate_z){ |
3717 | unsigned int iter2=0; |
3718 | double ds_temp=0.; |
3719 | while (fabs(Sc(state_z)-endplate_z)>EPS21.e-4 && iter2<20){ |
3720 | u=x-(endplate_z-Sc(state_z))*sin(atan(Sc(state_tanl))); |
3721 | x=u; |
3722 | ds_temp+=u_old-u; |
3723 | |
3724 | // Bail if the transverse momentum has dropped below some minimum |
3725 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3726 | if (DEBUG_LEVEL>2) |
3727 | { |
3728 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3728<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3729 | << endl; |
3730 | } |
3731 | return VALUE_OUT_OF_RANGE; |
3732 | } |
3733 | |
3734 | // Function evaluation |
3735 | Step(pos,u_old-u,Sc,dedx); |
3736 | u_old=u; |
3737 | iter2++; |
3738 | } |
3739 | //printf("new z %f ds %f \n",pos.z(),x); |
3740 | ds_out=ds_temp; |
3741 | return NOERROR; |
3742 | } |
3743 | ds_out=cx-x; |
3744 | return NOERROR; |
3745 | } |
3746 | // trial parabolic fit |
3747 | if (fabs(e)>tol1){ |
3748 | double x_minus_w=x-w; |
3749 | double x_minus_v=x-v; |
3750 | double r=x_minus_w*(fx-fv); |
3751 | double q=x_minus_v*(fx-fw); |
3752 | double p=x_minus_v*q-x_minus_w*r; |
3753 | q=2.0*(q-r); |
3754 | if (q>0.0) p=-p; |
3755 | q=fabs(q); |
3756 | double etemp=e; |
3757 | e=d; |
3758 | if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x)) |
3759 | // fall back on the Golden Section technique |
3760 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3761 | else{ |
3762 | // parabolic step |
3763 | d=p/q; |
3764 | u=x+d; |
3765 | if (u-a<tol2 || b-u <tol2) |
3766 | d=SIGN(tol1,xm-x)((xm-x)>=0.0?fabs(tol1):-fabs(tol1)); |
3767 | } |
3768 | } else{ |
3769 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3770 | } |
3771 | u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d)((d)>=0.0?fabs(tol1):-fabs(tol1))); |
3772 | |
3773 | // Bail if the transverse momentum has dropped below some minimum |
3774 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
3775 | if (DEBUG_LEVEL>2) |
3776 | { |
3777 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3777<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
3778 | << endl; |
3779 | } |
3780 | return VALUE_OUT_OF_RANGE; |
3781 | } |
3782 | |
3783 | // Function evaluation |
3784 | Step(pos,u_old-u,Sc,dedx); |
3785 | u_old=u; |
3786 | |
3787 | wirepos=origin; |
3788 | wirepos+=(Sc(state_z)-z0wire)*dir; |
3789 | double fu=(pos-wirepos).Mod2(); |
3790 | |
3791 | //cout << "Brent: z="<<Sc(state_z) << " d="<<sqrt(fu) << endl; |
3792 | |
3793 | if (fu<=fx){ |
3794 | if (u>=x) a=x; else b=x; |
3795 | SHFT(v,w,x,u)(v)=(w);(w)=(x);(x)=(u);; |
3796 | SHFT(fv,fw,fx,fu)(fv)=(fw);(fw)=(fx);(fx)=(fu);; |
3797 | } |
3798 | else { |
3799 | if (u<x) a=u; else b=u; |
3800 | if (fu<=fw || w==x){ |
3801 | v=w; |
3802 | w=u; |
3803 | fv=fw; |
3804 | fw=fu; |
3805 | } |
3806 | else if (fu<=fv || v==x || v==w){ |
3807 | v=u; |
3808 | fv=fu; |
3809 | } |
3810 | } |
3811 | } |
3812 | ds_out=cx-x; |
3813 | |
3814 | return NOERROR; |
3815 | } |
3816 | |
3817 | // Routine for finding the minimum of a function bracketed between two values |
3818 | // (see Numerical Recipes in C, pp. 404-405). |
3819 | jerror_t DTrackFitterKalmanSIMD::BrentsAlgorithm(double z,double dz, |
3820 | double dedx, |
3821 | const double z0wire, |
3822 | const DVector2 &origin, |
3823 | const DVector2 &dir, |
3824 | DMatrix5x1 &S, |
3825 | double &dz_out){ |
3826 | double d=0.,u=0.; |
3827 | double e=0.0; // will be distance moved on step before last |
3828 | double ax=0.; |
3829 | double bx=-dz; |
3830 | double cx=-2.*dz; |
3831 | |
3832 | double a=(ax<cx?ax:cx); |
3833 | double b=(ax>cx?ax:cx); |
3834 | double x=bx,w=bx,v=bx; |
3835 | |
3836 | // Initialize dz_out |
3837 | dz_out=0.; |
3838 | |
3839 | // Step to intermediate point |
3840 | double z_new=z+x; |
3841 | Step(z,z_new,dedx,S); |
3842 | // Bail if the momentum has dropped below some minimum |
3843 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
3844 | if (DEBUG_LEVEL>2) |
3845 | { |
3846 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3846<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
3847 | << endl; |
3848 | } |
3849 | return VALUE_OUT_OF_RANGE; |
3850 | } |
3851 | |
3852 | double dz0wire=z-z0wire; |
3853 | DVector2 wirepos=origin+(dz0wire+x)*dir; |
3854 | DVector2 pos(S(state_x),S(state_y)); |
3855 | double z_old=z_new; |
3856 | |
3857 | // initialization |
3858 | double fw=(pos-wirepos).Mod2(); |
3859 | double fv=fw; |
3860 | double fx=fw; |
3861 | |
3862 | // main loop |
3863 | for (unsigned int iter=1;iter<=ITMAX20;iter++){ |
3864 | double xm=0.5*(a+b); |
3865 | double tol1=EPS21.e-4*fabs(x)+ZEPS1.0e-10; |
3866 | double tol2=2.0*tol1; |
3867 | if (fabs(x-xm)<=(tol2-0.5*(b-a))){ |
3868 | if (z_new>=endplate_z){ |
3869 | x=endplate_z-z_new; |
3870 | |
3871 | // Bail if the momentum has dropped below some minimum |
3872 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
3873 | if (DEBUG_LEVEL>2) |
3874 | { |
3875 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3875<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
3876 | << endl; |
3877 | } |
3878 | return VALUE_OUT_OF_RANGE; |
3879 | } |
3880 | if (!isfinite(S(state_x)) || !isfinite(S(state_y))){ |
3881 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3881<<" " <<endl; |
3882 | return VALUE_OUT_OF_RANGE; |
3883 | } |
3884 | Step(z_new,endplate_z,dedx,S); |
3885 | } |
3886 | dz_out=x; |
3887 | return NOERROR; |
3888 | } |
3889 | // trial parabolic fit |
3890 | if (fabs(e)>tol1){ |
3891 | double x_minus_w=x-w; |
3892 | double x_minus_v=x-v; |
3893 | double r=x_minus_w*(fx-fv); |
3894 | double q=x_minus_v*(fx-fw); |
3895 | double p=x_minus_v*q-x_minus_w*r; |
3896 | q=2.0*(q-r); |
3897 | if (q>0.0) p=-p; |
3898 | q=fabs(q); |
3899 | double etemp=e; |
3900 | e=d; |
3901 | if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x)) |
3902 | // fall back on the Golden Section technique |
3903 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3904 | else{ |
3905 | // parabolic step |
3906 | d=p/q; |
3907 | u=x+d; |
3908 | if (u-a<tol2 || b-u <tol2) |
3909 | d=SIGN(tol1,xm-x)((xm-x)>=0.0?fabs(tol1):-fabs(tol1)); |
3910 | } |
3911 | } else{ |
3912 | d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x)); |
3913 | } |
3914 | u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d)((d)>=0.0?fabs(tol1):-fabs(tol1))); |
3915 | |
3916 | // Function evaluation |
3917 | //S=S0; |
3918 | z_new=z+u; |
3919 | // Bail if the momentum has dropped below some minimum |
3920 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
3921 | if (DEBUG_LEVEL>2) |
3922 | { |
3923 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<3923<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
3924 | << endl; |
3925 | } |
3926 | return VALUE_OUT_OF_RANGE; |
3927 | } |
3928 | |
3929 | Step(z_old,z_new,dedx,S); |
3930 | z_old=z_new; |
3931 | |
3932 | wirepos=origin; |
3933 | wirepos+=(dz0wire+u)*dir; |
3934 | pos.Set(S(state_x),S(state_y)); |
3935 | double fu=(pos-wirepos).Mod2(); |
3936 | |
3937 | // _DBG_ << "Brent: z="<< z+u << " d^2="<<fu << endl; |
3938 | |
3939 | if (fu<=fx){ |
3940 | if (u>=x) a=x; else b=x; |
3941 | SHFT(v,w,x,u)(v)=(w);(w)=(x);(x)=(u);; |
3942 | SHFT(fv,fw,fx,fu)(fv)=(fw);(fw)=(fx);(fx)=(fu);; |
3943 | } |
3944 | else { |
3945 | if (u<x) a=u; else b=u; |
3946 | if (fu<=fw || w==x){ |
3947 | v=w; |
3948 | w=u; |
3949 | fv=fw; |
3950 | fw=fu; |
3951 | } |
3952 | else if (fu<=fv || v==x || v==w){ |
3953 | v=u; |
3954 | fv=fu; |
3955 | } |
3956 | } |
3957 | } |
3958 | dz_out=x; |
3959 | return NOERROR; |
3960 | } |
3961 | |
3962 | // Kalman engine for Central tracks; updates the position on the trajectory |
3963 | // after the last hit (closest to the target) is added |
3964 | kalman_error_t DTrackFitterKalmanSIMD::KalmanCentral(double anneal_factor, |
3965 | DMatrix5x1 &Sc, |
3966 | DMatrix5x5 &Cc, |
3967 | DVector2 &xy,double &chisq, |
3968 | unsigned int &my_ndf |
3969 | ){ |
3970 | DMatrix1x5 H; // Track projection matrix |
3971 | DMatrix5x1 H_T; // Transpose of track projection matrix |
3972 | DMatrix5x5 J; // State vector Jacobian matrix |
3973 | DMatrix5x5 Q; // Process noise covariance matrix |
3974 | DMatrix5x1 K; // KalmanSIMD gain matrix |
3975 | DMatrix5x5 Ctest; // covariance matrix |
3976 | // double V=0.2028; //1.56*1.56/12.; // Measurement variance |
3977 | double V=0.0507; |
3978 | double InvV; // inverse of variance |
3979 | //DMatrix5x1 dS; // perturbation in state vector |
3980 | DMatrix5x1 S0,S0_; // state vector |
3981 | |
3982 | // set the used_in_fit flags to false for cdc hits |
3983 | unsigned int num_cdc=cdc_used_in_fit.size(); |
3984 | for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false; |
3985 | for (unsigned int i=0;i<central_traj.size();i++){ |
3986 | central_traj[i].h_id=0; |
3987 | } |
3988 | |
3989 | // Initialize the chi2 for this part of the track |
3990 | chisq=0.; |
3991 | my_ndf=0; |
3992 | |
3993 | double chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
3994 | |
3995 | // path length increment |
3996 | double ds2=0.; |
3997 | |
3998 | //printf(">>>>>>>>>>>>>>>>\n"); |
3999 | |
4000 | // beginning position |
4001 | double phi=Sc(state_phi); |
4002 | xy.Set(central_traj[break_point_step_index].xy.X()-Sc(state_D)*sin(phi), |
4003 | central_traj[break_point_step_index].xy.Y()+Sc(state_D)*cos(phi)); |
4004 | |
4005 | // Wire origin and direction |
4006 | // unsigned int cdc_index=my_cdchits.size()-1; |
4007 | unsigned int cdc_index=break_point_cdc_index; |
4008 | if (break_point_cdc_index<num_cdc-1){ |
4009 | num_cdc=break_point_cdc_index+1; |
4010 | } |
4011 | |
4012 | if (num_cdc<MIN_HITS_FOR_REFIT) chi2cut=BIG1.0e8; |
4013 | |
4014 | // Wire origin and direction |
4015 | DVector2 origin=my_cdchits[cdc_index]->origin; |
4016 | double z0w=my_cdchits[cdc_index]->z0wire; |
4017 | DVector2 dir=my_cdchits[cdc_index]->dir; |
4018 | DVector2 wirexy=origin+(Sc(state_z)-z0w)*dir; |
4019 | |
4020 | // Save the starting values for C and S in the deque |
4021 | central_traj[break_point_step_index].Skk=Sc; |
4022 | central_traj[break_point_step_index].Ckk=Cc; |
4023 | |
4024 | // doca variables |
4025 | double doca2,old_doca2=(xy-wirexy).Mod2(); |
4026 | |
4027 | // energy loss |
4028 | double dedx=0.; |
4029 | |
4030 | // Boolean for flagging when we are done with measurements |
4031 | bool more_measurements=true; |
4032 | |
4033 | // Initialize S0_ and perform the loop over the trajectory |
4034 | S0_=central_traj[break_point_step_index].S; |
4035 | |
4036 | for (unsigned int k=break_point_step_index+1;k<central_traj.size();k++){ |
4037 | unsigned int k_minus_1=k-1; |
4038 | |
4039 | // Check that C matrix is positive definite |
4040 | if (!Cc.IsPosDef()){ |
4041 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4041<<" " << "Broken covariance matrix!" <<endl; |
4042 | return BROKEN_COVARIANCE_MATRIX; |
4043 | } |
4044 | |
4045 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
4046 | // from reference trajectory |
4047 | S0=central_traj[k].S; |
4048 | J=central_traj[k].J; |
4049 | Q=central_traj[k].Q; |
4050 | |
4051 | //Q.Print(); |
4052 | //J.Print(); |
4053 | |
4054 | // State S is perturbation about a seed S0 |
4055 | //dS=Sc-S0_; |
4056 | //dS.Zero(); |
4057 | |
4058 | // Update the actual state vector and covariance matrix |
4059 | Sc=S0+J*(Sc-S0_); |
4060 | // Cc=J*(Cc*JT)+Q; |
4061 | // Cc=Q.AddSym(Cc.SandwichMultiply(J)); |
4062 | Cc=Q.AddSym(J*Cc*J.Transpose()); |
4063 | |
4064 | // Save the current state and covariance matrix in the deque |
4065 | if (fit_type==kTimeBased){ |
4066 | central_traj[k].Skk=Sc; |
4067 | central_traj[k].Ckk=Cc; |
4068 | } |
4069 | |
4070 | // update position based on new doca to reference trajectory |
4071 | xy.Set(central_traj[k].xy.X()-Sc(state_D)*sin(Sc(state_phi)), |
4072 | central_traj[k].xy.Y()+Sc(state_D)*cos(Sc(state_phi))); |
4073 | |
4074 | // Bail if the position is grossly outside of the tracking volume |
4075 | if (xy.Mod2()>R2_MAX4225.0 || Sc(state_z)<Z_MIN-100. || Sc(state_z)>Z_MAX370.0){ |
4076 | if (DEBUG_LEVEL>2) |
4077 | { |
4078 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4078<<" "<< "Went outside of tracking volume at z="<<Sc(state_z) |
4079 | << " r="<<xy.Mod()<<endl; |
4080 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4080<<" " << " Break indexes: " << break_point_cdc_index <<"," |
4081 | << break_point_step_index << endl; |
4082 | } |
4083 | return POSITION_OUT_OF_RANGE; |
4084 | } |
4085 | // Bail if the transverse momentum has dropped below some minimum |
4086 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
4087 | if (DEBUG_LEVEL>2) |
4088 | { |
4089 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4089<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
4090 | << " at step " << k |
4091 | << endl; |
4092 | } |
4093 | return MOMENTUM_OUT_OF_RANGE; |
4094 | } |
4095 | |
4096 | |
4097 | // Save the current state of the reference trajectory |
4098 | S0_=S0; |
4099 | |
4100 | // new wire position |
4101 | wirexy=origin; |
4102 | wirexy+=(Sc(state_z)-z0w)*dir; |
4103 | |
4104 | // new doca |
4105 | doca2=(xy-wirexy).Mod2(); |
4106 | |
4107 | // Check if the doca is no longer decreasing |
4108 | if (more_measurements && (doca2>old_doca2 && Sc(state_z)>cdc_origin[2])){ |
4109 | if (my_cdchits[cdc_index]->status==good_hit){ |
4110 | if (DEBUG_LEVEL>9) { |
4111 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4111<<" " << " Good Hit Ring " << my_cdchits[cdc_index]->hit->wire->ring << " Straw " << my_cdchits[cdc_index]->hit->wire->straw << endl; |
4112 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4112<<" " << " doca " << sqrt(doca2) << endl; |
4113 | } |
4114 | |
4115 | // Save values at end of current step |
4116 | DVector2 xy0=central_traj[k].xy; |
4117 | |
4118 | // Variables for the computation of D at the doca to the wire |
4119 | double D=Sc(state_D); |
4120 | double q=(Sc(state_q_over_pt)>0.0)?1.:-1.; |
4121 | |
4122 | q*=FactorForSenseOfRotation; |
4123 | |
4124 | double qpt=1./Sc(state_q_over_pt) * FactorForSenseOfRotation; |
4125 | double sinphi=sin(Sc(state_phi)); |
4126 | double cosphi=cos(Sc(state_phi)); |
4127 | //double qrc_old=qpt/fabs(qBr2p*bfield->GetBz(pos.x(),pos.y(),pos.z())); |
4128 | double qrc_old=qpt/fabs(qBr2p0.003*central_traj[k].B); |
4129 | double qrc_plus_D=D+qrc_old; |
4130 | |
4131 | // wire direction variable |
4132 | double ux=dir.X(); |
4133 | double uy=dir.Y(); |
4134 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
4135 | // Variables relating wire direction and track direction |
4136 | //double my_ux=ux*sinl-cosl*cosphi; |
4137 | //double my_uy=uy*sinl-cosl*sinphi; |
4138 | //double denom=my_ux*my_ux+my_uy*my_uy; |
4139 | // distance variables |
4140 | DVector2 diff,dxy1; |
4141 | |
4142 | // use Brent's algorithm to find the poca to the wire |
4143 | // See Numerical Recipes in C, pp 404-405 |
4144 | |
4145 | // dEdx for current position along trajectory |
4146 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
4147 | if (CORRECT_FOR_ELOSS){ |
4148 | dedx=GetdEdx(q_over_p, central_traj[k].K_rho_Z_over_A, |
4149 | central_traj[k].rho_Z_over_A, |
4150 | central_traj[k].LnI,central_traj[k].Z); |
4151 | } |
4152 | |
4153 | if (BrentCentral(dedx,xy,z0w,origin,dir,Sc,ds2)!=NOERROR) return MOMENTUM_OUT_OF_RANGE; |
4154 | |
4155 | //Step along the reference trajectory and compute the new covariance matrix |
4156 | StepStateAndCovariance(xy0,ds2,dedx,S0,J,Cc); |
4157 | |
4158 | // Compute the value of D (signed distance to the reference trajectory) |
4159 | // at the doca to the wire |
4160 | dxy1=xy0-central_traj[k].xy; |
4161 | double rc=sqrt(dxy1.Mod2() |
4162 | +2.*qrc_plus_D*(dxy1.X()*sinphi-dxy1.Y()*cosphi) |
4163 | +qrc_plus_D*qrc_plus_D); |
4164 | Sc(state_D)=q*rc-qrc_old; |
4165 | |
4166 | // wire position |
4167 | wirexy=origin; |
4168 | wirexy+=(Sc(state_z)-z0w)*dir; |
4169 | diff=xy-wirexy; |
4170 | |
4171 | // prediction for measurement |
4172 | double doca=diff.Mod()+EPS3.0e-8; |
4173 | double prediction=doca*cosstereo; |
4174 | |
4175 | // Measurement |
4176 | double measurement=0.39,tdrift=0.,tcorr=0.,dDdt0=0.; |
4177 | if (fit_type==kTimeBased || USE_PASS1_TIME_MODE){ |
4178 | // Find offset of wire with respect to the center of the |
4179 | // straw at this z position |
4180 | const DCDCWire *mywire=my_cdchits[cdc_index]->hit->wire; |
4181 | int ring_index=mywire->ring-1; |
4182 | int straw_index=mywire->straw-1; |
4183 | double dz=Sc(state_z)-z0w; |
4184 | double phi_d=diff.Phi(); |
4185 | double delta |
4186 | =max_sag[ring_index][straw_index]*(1.-dz*dz/5625.) |
4187 | *cos(phi_d + sag_phi_offset[ring_index][straw_index]); |
4188 | double dphi=phi_d-mywire->origin.Phi(); |
4189 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
4190 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
4191 | if (mywire->origin.Y()<0) dphi*=-1.; |
4192 | |
4193 | tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
4194 | -central_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
4195 | double B=central_traj[k_minus_1].B; |
4196 | ComputeCDCDrift(dphi,delta,tdrift,B,measurement,V,tcorr); |
4197 | V*=anneal_factor; |
4198 | if (ALIGNMENT_CENTRAL){ |
4199 | double myV=0.; |
4200 | double mytcorr=0.; |
4201 | double d_shifted; |
4202 | double dt=2.0; |
4203 | ComputeCDCDrift(dphi,delta,tdrift+dt,B,d_shifted,myV,mytcorr); |
4204 | dDdt0=(d_shifted-measurement)/dt; |
4205 | } |
4206 | |
4207 | //_DBG_ << tcorr << " " << dphi << " " << dm << endl; |
4208 | |
4209 | } |
4210 | |
4211 | // Projection matrix |
4212 | sinphi=sin(Sc(state_phi)); |
4213 | cosphi=cos(Sc(state_phi)); |
4214 | double dx=diff.X(); |
4215 | double dy=diff.Y(); |
4216 | double cosstereo_over_doca=cosstereo/doca; |
4217 | H_T(state_D)=(dy*cosphi-dx*sinphi)*cosstereo_over_doca; |
4218 | H_T(state_phi) |
4219 | =-Sc(state_D)*cosstereo_over_doca*(dx*cosphi+dy*sinphi); |
4220 | H_T(state_z)=-cosstereo_over_doca*(dx*ux+dy*uy); |
4221 | H(state_tanl)=0.; |
4222 | H_T(state_tanl)=0.; |
4223 | H(state_D)=H_T(state_D); |
4224 | H(state_z)=H_T(state_z); |
4225 | H(state_phi)=H_T(state_phi); |
4226 | |
4227 | // Difference and inverse of variance |
4228 | //InvV=1./(V+H*(Cc*H_T)); |
4229 | //double Vproj=Cc.SandwichMultiply(H_T); |
4230 | double Vproj=H*Cc*H_T; |
4231 | InvV=1./(V+Vproj); |
4232 | double dm=measurement-prediction; |
4233 | |
4234 | if (InvV<0.){ |
4235 | if (DEBUG_LEVEL>1) |
4236 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4236<<" " << k <<" "<< central_traj.size()-1<<" Negative variance??? " << V << " " << H*(Cc*H_T) <<endl; |
4237 | |
4238 | break_point_cdc_index=(3*num_cdc)/4; |
4239 | return NEGATIVE_VARIANCE; |
4240 | } |
4241 | /* |
4242 | if (fabs(cosstereo)<1.){ |
4243 | 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); |
4244 | } |
4245 | */ |
4246 | |
4247 | // Check how far this hit is from the expected position |
4248 | double chi2check=dm*dm*InvV; |
4249 | if (DEBUG_LEVEL>9) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4249<<" " << " Prediction " << prediction << " Measurement " << measurement << " Chi2 " << chi2check << endl; |
4250 | if (chi2check<chi2cut) |
4251 | { |
4252 | if (DEBUG_LEVEL>9) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4252<<" " << " Passed Chi^2 check Ring " << my_cdchits[cdc_index]->hit->wire->ring << " Straw " << my_cdchits[cdc_index]->hit->wire->straw << endl; |
4253 | |
4254 | // Compute Kalman gain matrix |
4255 | K=InvV*(Cc*H_T); |
4256 | |
4257 | // Update state vector covariance matrix |
4258 | //Cc=Cc-(K*(H*Cc)); |
4259 | Ctest=Cc.SubSym(K*(H*Cc)); |
4260 | // Joseph form |
4261 | // C = (I-KH)C(I-KH)^T + KVK^T |
4262 | //Ctest=Cc.SandwichMultiply(I5x5-K*H)+V*MultiplyTranspose(K); |
4263 | // Check that Ctest is positive definite |
4264 | if (!Ctest.IsPosDef()){ |
4265 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4265<<" " << "Broken covariance matrix!" <<endl; |
4266 | return BROKEN_COVARIANCE_MATRIX; |
4267 | } |
4268 | bool skip_ring |
4269 | =(my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP); |
4270 | //Update covariance matrix and state vector |
4271 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
4272 | Cc=Ctest; |
4273 | Sc+=dm*K; |
4274 | } |
4275 | |
4276 | // Mark point on ref trajectory with a hit id for the straw |
4277 | central_traj[k].h_id=cdc_index+1; |
4278 | if (DEBUG_LEVEL>9) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4278<<" " << " Marked Trajectory central_traj[k].h_id=cdc_index+1 (k cdc_index)" << k << " " << cdc_index << endl; |
4279 | |
4280 | // Save some updated information for this hit |
4281 | double scale=(skip_ring)?1.:(1.-H*K); |
4282 | cdc_updates[cdc_index].tcorr=tcorr; |
4283 | cdc_updates[cdc_index].tdrift=tdrift; |
4284 | cdc_updates[cdc_index].doca=measurement; |
4285 | cdc_updates[cdc_index].variance=V; |
4286 | cdc_updates[cdc_index].dDdt0=dDdt0; |
4287 | cdc_used_in_fit[cdc_index]=true; |
4288 | if (tdrift < CDC_T_DRIFT_MIN) cdc_used_in_fit[cdc_index]=false; |
4289 | |
4290 | // Update chi2 for this hit |
4291 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
4292 | chisq+=scale*dm*dm/V; |
4293 | my_ndf++; |
4294 | } |
4295 | if (DEBUG_LEVEL>10) |
4296 | cout |
4297 | << "ring " << my_cdchits[cdc_index]->hit->wire->ring |
4298 | << " t " << my_cdchits[cdc_index]->hit->tdrift |
4299 | << " Dm-Dpred " << dm |
4300 | << " chi2 " << (1.-H*K)*dm*dm/V |
4301 | << endl; |
4302 | |
4303 | break_point_cdc_index=cdc_index; |
4304 | break_point_step_index=k_minus_1; |
4305 | |
4306 | //else printf("Negative variance!!!\n"); |
4307 | |
4308 | |
4309 | } |
4310 | |
4311 | // Move back to the right step along the reference trajectory. |
4312 | StepStateAndCovariance(xy,-ds2,dedx,Sc,J,Cc); |
4313 | |
4314 | // Save state and covariance matrix to update vector |
4315 | cdc_updates[cdc_index].S=Sc; |
4316 | cdc_updates[cdc_index].C=Cc; |
4317 | |
4318 | //Sc.Print(); |
4319 | //Cc.Print(); |
4320 | |
4321 | // update position on current trajectory based on corrected doca to |
4322 | // reference trajectory |
4323 | xy.Set(central_traj[k].xy.X()-Sc(state_D)*sin(Sc(state_phi)), |
4324 | central_traj[k].xy.Y()+Sc(state_D)*cos(Sc(state_phi))); |
4325 | |
4326 | } |
4327 | |
4328 | // new wire origin and direction |
4329 | if (cdc_index>0){ |
4330 | cdc_index--; |
4331 | origin=my_cdchits[cdc_index]->origin; |
4332 | z0w=my_cdchits[cdc_index]->z0wire; |
4333 | dir=my_cdchits[cdc_index]->dir; |
4334 | } |
4335 | else{ |
4336 | more_measurements=false; |
4337 | } |
4338 | |
4339 | // Update the wire position |
4340 | wirexy=origin+(Sc(state_z)-z0w)*dir; |
4341 | |
4342 | //s+=ds2; |
4343 | // new doca |
4344 | doca2=(xy-wirexy).Mod2(); |
4345 | } |
4346 | |
4347 | old_doca2=doca2; |
4348 | |
4349 | } |
4350 | |
4351 | // If there are not enough degrees of freedom, something went wrong... |
4352 | if (my_ndf<6){ |
4353 | chisq=-1.; |
4354 | my_ndf=0; |
4355 | return PRUNED_TOO_MANY_HITS; |
4356 | } |
4357 | else my_ndf-=5; |
4358 | |
4359 | // Check if the momentum is unphysically large |
4360 | double p=cos(atan(Sc(state_tanl)))/fabs(Sc(state_q_over_pt)); |
4361 | if (p>12.0){ |
4362 | if (DEBUG_LEVEL>2) |
4363 | { |
4364 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4364<<" " << "Unphysical momentum: P = " << p <<endl; |
4365 | } |
4366 | return MOMENTUM_OUT_OF_RANGE; |
4367 | } |
4368 | |
4369 | // Check if we have a kink in the track or threw away too many cdc hits |
4370 | if (num_cdc>=MIN_HITS_FOR_REFIT){ |
4371 | if (break_point_cdc_index>1){ |
4372 | if (break_point_cdc_index<num_cdc/2){ |
4373 | break_point_cdc_index=(3*num_cdc)/4; |
4374 | } |
4375 | return BREAK_POINT_FOUND; |
4376 | } |
4377 | |
4378 | unsigned int num_good=0; |
4379 | for (unsigned int j=0;j<num_cdc;j++){ |
4380 | if (cdc_used_in_fit[j]) num_good++; |
4381 | } |
4382 | if (double(num_good)/double(num_cdc)<MINIMUM_HIT_FRACTION){ |
4383 | return PRUNED_TOO_MANY_HITS; |
4384 | } |
4385 | } |
4386 | |
4387 | return FIT_SUCCEEDED; |
4388 | } |
4389 | |
4390 | // Kalman engine for forward tracks |
4391 | kalman_error_t DTrackFitterKalmanSIMD::KalmanForward(double fdc_anneal_factor, |
4392 | double cdc_anneal_factor, |
4393 | DMatrix5x1 &S, |
4394 | DMatrix5x5 &C, |
4395 | double &chisq, |
4396 | unsigned int &numdof){ |
4397 | DMatrix2x1 Mdiff; // difference between measurement and prediction |
4398 | DMatrix2x5 H; // Track projection matrix |
4399 | DMatrix5x2 H_T; // Transpose of track projection matrix |
4400 | DMatrix1x5 Hc; // Track projection matrix for cdc hits |
4401 | DMatrix5x1 Hc_T; // Transpose of track projection matrix for cdc hits |
4402 | DMatrix5x5 J; // State vector Jacobian matrix |
4403 | //DMatrix5x5 J_T; // transpose of this matrix |
4404 | DMatrix5x5 Q; // Process noise covariance matrix |
4405 | DMatrix5x2 K; // Kalman gain matrix |
4406 | DMatrix5x1 Kc; // Kalman gain matrix for cdc hits |
4407 | DMatrix2x2 V(0.0833,0.,0.,FDC_CATHODE_VARIANCE0.000225); // Measurement covariance matrix |
4408 | DMatrix2x1 R; // Filtered residual |
4409 | DMatrix2x2 RC; // Covariance of filtered residual |
4410 | DMatrix5x1 S0,S0_; //State vector |
4411 | DMatrix5x5 Ctest; // Covariance matrix |
4412 | DMatrix2x2 InvV; // Inverse of error matrix |
4413 | |
4414 | double Vc=0.0507; |
4415 | |
4416 | // Vectors for cdc wires |
4417 | DVector2 origin,dir,wirepos; |
4418 | double z0w=0.; // origin in z for wire |
4419 | |
4420 | // Set used_in_fit flags to false for fdc and cdc hits |
4421 | unsigned int num_cdc=cdc_used_in_fit.size(); |
4422 | unsigned int num_fdc=fdc_used_in_fit.size(); |
4423 | for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false; |
4424 | for (unsigned int i=0;i<num_fdc;i++) fdc_used_in_fit[i]=false; |
4425 | for (unsigned int i=0;i<forward_traj.size();i++){ |
4426 | if (forward_traj[i].h_id>999) |
4427 | forward_traj[i].h_id=0; |
4428 | } |
4429 | |
4430 | // Save the starting values for C and S in the deque |
4431 | forward_traj[break_point_step_index].Skk=S; |
4432 | forward_traj[break_point_step_index].Ckk=C; |
4433 | |
4434 | // Initialize chi squared |
4435 | chisq=0; |
4436 | |
4437 | // Initialize number of degrees of freedom |
4438 | numdof=0; |
4439 | |
4440 | double fdc_chi2cut=NUM_FDC_SIGMA_CUT*NUM_FDC_SIGMA_CUT; |
4441 | double cdc_chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
4442 | |
4443 | unsigned int num_fdc_hits=break_point_fdc_index+1; |
4444 | unsigned int max_num_fdc_used_in_fit=num_fdc_hits; |
4445 | unsigned int num_cdc_hits=my_cdchits.size(); |
4446 | unsigned int cdc_index=0; |
4447 | if (num_cdc_hits>0) cdc_index=num_cdc_hits-1; |
4448 | bool more_cdc_measurements=(num_cdc_hits>0); |
4449 | double old_doca2=1e6; |
4450 | |
4451 | if (num_fdc_hits+num_cdc_hits<MIN_HITS_FOR_REFIT){ |
4452 | cdc_chi2cut=BIG1.0e8; |
4453 | fdc_chi2cut=BIG1.0e8; |
4454 | } |
4455 | |
4456 | if (more_cdc_measurements){ |
4457 | origin=my_cdchits[cdc_index]->origin; |
4458 | dir=my_cdchits[cdc_index]->dir; |
4459 | z0w=my_cdchits[cdc_index]->z0wire; |
4460 | wirepos=origin+(forward_traj[break_point_step_index].z-z0w)*dir; |
4461 | } |
4462 | |
4463 | S0_=(forward_traj[break_point_step_index].S); |
4464 | |
4465 | if (DEBUG_LEVEL > 25){ |
4466 | jout << "Entering DTrackFitterKalmanSIMD::KalmanForward ================================" << endl; |
4467 | jout << " We have the following starting parameters for our fit. S = State vector, C = Covariance matrix" << endl; |
4468 | S.Print(); |
4469 | C.Print(); |
4470 | jout << setprecision(6); |
4471 | jout << " There are " << num_cdc << " CDC Updates and " << num_fdc << " FDC Updates on this track" << endl; |
4472 | jout << " There are " << num_cdc_hits << " CDC Hits and " << num_fdc_hits << " FDC Hits on this track" << endl; |
4473 | jout << " With NUM_FDC_SIGMA_CUT = " << NUM_FDC_SIGMA_CUT << " and NUM_CDC_SIGMA_CUT = " << NUM_CDC_SIGMA_CUT << endl; |
4474 | jout << " fdc_anneal_factor = " << fdc_anneal_factor << " cdc_anneal_factor = " << cdc_anneal_factor << endl; |
4475 | jout << " yields fdc_chi2cut = " << fdc_chi2cut << " cdc_chi2cut = " << cdc_chi2cut << endl; |
4476 | jout << " Starting from break_point_step_index " << break_point_step_index << endl; |
4477 | jout << " S0_ which is the state vector of the reference trajectory at the break point step:" << endl; |
4478 | S0_.Print(); |
4479 | jout << " ===== Beginning pass over reference trajectory ======== " << endl; |
4480 | } |
4481 | |
4482 | for (unsigned int k=break_point_step_index+1;k<forward_traj.size();k++){ |
4483 | unsigned int k_minus_1=k-1; |
4484 | |
4485 | // Check that C matrix is positive definite |
4486 | if (!C.IsPosDef()){ |
4487 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4487<<" " << "Broken covariance matrix!" <<endl; |
4488 | return BROKEN_COVARIANCE_MATRIX; |
4489 | } |
4490 | |
4491 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
4492 | // from reference trajectory |
4493 | S0=(forward_traj[k].S); |
4494 | J=(forward_traj[k].J); |
4495 | Q=(forward_traj[k].Q); |
4496 | |
4497 | // State S is perturbation about a seed S0 |
4498 | //dS=S-S0_; |
4499 | |
4500 | // Update the actual state vector and covariance matrix |
4501 | S=S0+J*(S-S0_); |
4502 | |
4503 | // Bail if the momentum has dropped below some minimum |
4504 | if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX){ |
4505 | if (DEBUG_LEVEL>2) |
4506 | { |
4507 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4507<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; |
4508 | } |
4509 | break_point_fdc_index=(3*num_fdc)/4; |
4510 | return MOMENTUM_OUT_OF_RANGE; |
4511 | } |
4512 | |
4513 | |
4514 | //C=J*(C*J_T)+Q; |
4515 | //C=Q.AddSym(C.SandwichMultiply(J)); |
4516 | C=Q.AddSym(J*C*J.Transpose()); |
4517 | |
4518 | // Save the current state and covariance matrix in the deque |
4519 | forward_traj[k].Skk=S; |
4520 | forward_traj[k].Ckk=C; |
4521 | |
4522 | // Save the current state of the reference trajectory |
4523 | S0_=S0; |
4524 | |
4525 | // Z position along the trajectory |
4526 | double z=forward_traj[k].z; |
4527 | |
4528 | if (DEBUG_LEVEL > 25){ |
4529 | jout << " At reference trajectory index " << k << " at z=" << z << endl; |
4530 | jout << " The State vector from the reference trajectory" << endl; |
4531 | S0.Print(); |
4532 | jout << " The Jacobian matrix " << endl; |
4533 | J.Print(); |
4534 | jout << " The Q matrix "<< endl; |
4535 | Q.Print(); |
4536 | jout << " The updated State vector S=S0+J*(S-S0_)" << endl; |
4537 | S.Print(); |
4538 | jout << " The updated Covariance matrix C=J*(C*J_T)+Q;" << endl; |
4539 | C.Print(); |
4540 | } |
4541 | |
4542 | // Add the hit |
4543 | if (num_fdc_hits>0){ |
4544 | if (forward_traj[k].h_id>0 && forward_traj[k].h_id<1000){ |
4545 | unsigned int id=forward_traj[k].h_id-1; |
4546 | // Check if this is a plane we want to skip in the fit (we still want |
4547 | // to store track and hit info at this plane, however). |
4548 | bool skip_plane=(my_fdchits[id]->hit!=NULL__null |
4549 | &&my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP); |
4550 | double upred=0,vpred=0.,doca=0.,cosalpha=0.,lorentz_factor=0.; |
4551 | FindDocaAndProjectionMatrix(my_fdchits[id],S,upred,vpred,doca,cosalpha, |
4552 | lorentz_factor,H_T); |
4553 | // Matrix transpose H_T -> H |
4554 | H=Transpose(H_T); |
4555 | |
4556 | // Variance in coordinate transverse to wire |
4557 | V(0,0)=my_fdchits[id]->uvar; |
4558 | if (my_fdchits[id]->hit==NULL__null&&my_fdchits[id]->status!=trd_hit){ |
4559 | V(0,0)*=fdc_anneal_factor; |
4560 | } |
4561 | |
4562 | // Variance in coordinate along wire |
4563 | V(1,1)=my_fdchits[id]->vvar*fdc_anneal_factor; |
4564 | |
4565 | // Residual for coordinate along wire |
4566 | Mdiff(1)=my_fdchits[id]->vstrip-vpred-doca*lorentz_factor; |
4567 | |
4568 | // Residual for coordinate transverse to wire |
4569 | Mdiff(0)=-doca; |
4570 | double drift_time=my_fdchits[id]->t-mT0-forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
4571 | if (fit_type==kTimeBased && USE_FDC_DRIFT_TIMES){ |
4572 | if (my_fdchits[id]->hit!=NULL__null){ |
4573 | double drift=(doca>0.0?1.:-1.) |
4574 | *fdc_drift_distance(drift_time,forward_traj[k].B); |
4575 | Mdiff(0)+=drift; |
4576 | |
4577 | // Variance in drift distance |
4578 | V(0,0)=fdc_drift_variance(drift_time)*fdc_anneal_factor; |
4579 | } |
4580 | else if (USE_TRD_DRIFT_TIMES&&my_fdchits[id]->status==trd_hit){ |
4581 | double drift =(doca>0.0?1.:-1.)*0.1*pow(drift_time/8./0.91,1./1.556); |
4582 | Mdiff(0)+=drift; |
4583 | |
4584 | // Variance in drift distance |
4585 | V(0,0)=0.05*0.05*fdc_anneal_factor; |
4586 | } |
4587 | } |
4588 | // Check to see if we have multiple hits in the same plane |
4589 | if (!ALIGNMENT_FORWARD && forward_traj[k].num_hits>1){ |
4590 | UpdateSandCMultiHit(forward_traj[k],upred,vpred,doca,cosalpha, |
4591 | lorentz_factor,V,Mdiff,H,H_T,S,C, |
4592 | fdc_chi2cut,skip_plane,chisq,numdof, |
4593 | fdc_anneal_factor); |
4594 | } |
4595 | else{ |
4596 | if (DEBUG_LEVEL > 25) jout << " == There is a single FDC hit on this plane" << endl; |
4597 | |
4598 | // Variance for this hit |
4599 | DMatrix2x2 Vtemp=V+H*C*H_T; |
4600 | InvV=Vtemp.Invert(); |
4601 | |
4602 | // Check if this hit is an outlier |
4603 | double chi2_hit=Vtemp.Chi2(Mdiff); |
4604 | if (chi2_hit<fdc_chi2cut){ |
4605 | // Compute Kalman gain matrix |
4606 | K=C*H_T*InvV; |
4607 | |
4608 | if (skip_plane==false){ |
4609 | // Update the state vector |
4610 | S+=K*Mdiff; |
4611 | |
4612 | // Update state vector covariance matrix |
4613 | //C=C-K*(H*C); |
4614 | C=C.SubSym(K*(H*C)); |
4615 | |
4616 | if (DEBUG_LEVEL > 25) { |
4617 | jout << "S Update: " << endl; S.Print(); |
4618 | jout << "C Uodate: " << endl; C.Print(); |
4619 | } |
4620 | } |
4621 | |
4622 | // Store the "improved" values for the state vector and covariance |
4623 | fdc_updates[id].S=S; |
4624 | fdc_updates[id].C=C; |
4625 | fdc_updates[id].tdrift=drift_time; |
4626 | fdc_updates[id].tcorr=fdc_updates[id].tdrift; // temporary! |
4627 | fdc_updates[id].doca=doca; |
4628 | fdc_used_in_fit[id]=true; |
4629 | |
4630 | if (skip_plane==false){ |
4631 | // Filtered residual and covariance of filtered residual |
4632 | R=Mdiff-H*K*Mdiff; |
4633 | RC=V-H*(C*H_T); |
4634 | |
4635 | fdc_updates[id].V=RC; |
4636 | |
4637 | // Update chi2 for this segment |
4638 | chisq+=RC.Chi2(R); |
4639 | |
4640 | // update number of degrees of freedom |
4641 | numdof+=2; |
4642 | |
4643 | if (DEBUG_LEVEL>20) |
4644 | { |
4645 | printf("hit %d p %5.2f t %f dm %5.2f sig %f chi2 %5.2f z %5.2f\n", |
4646 | id,1./S(state_q_over_p),fdc_updates[id].tdrift,Mdiff(1), |
4647 | sqrt(V(1,1)),RC.Chi2(R), |
4648 | forward_traj[k].z); |
4649 | |
4650 | } |
4651 | } |
4652 | else{ |
4653 | fdc_updates[id].V=V; |
4654 | } |
4655 | |
4656 | break_point_fdc_index=id; |
4657 | break_point_step_index=k; |
4658 | } |
4659 | } |
4660 | if (num_fdc_hits>=forward_traj[k].num_hits) |
4661 | num_fdc_hits-=forward_traj[k].num_hits; |
4662 | } |
4663 | } |
4664 | else if (more_cdc_measurements /* && z<endplate_z*/){ |
4665 | // new wire position |
4666 | wirepos=origin; |
4667 | wirepos+=(z-z0w)*dir; |
4668 | |
4669 | // doca variables |
4670 | double dx=S(state_x)-wirepos.X(); |
4671 | double dy=S(state_y)-wirepos.Y(); |
4672 | double doca2=dx*dx+dy*dy; |
4673 | |
4674 | // Check if the doca is no longer decreasing |
4675 | if (doca2>old_doca2){ |
4676 | if(my_cdchits[cdc_index]->status==good_hit){ |
4677 | find_doca_error_t swimmed_to_doca=DOCA_NO_BRENT; |
4678 | double newz=endplate_z; |
4679 | double dz=newz-z; |
4680 | // Sometimes the true doca would correspond to the case where the |
4681 | // wire would need to extend beyond the physical volume of the straw. |
4682 | // In this case, find the doca at the cdc endplate. |
4683 | if (z>endplate_z){ |
4684 | swimmed_to_doca=DOCA_ENDPLATE; |
4685 | SwimToEndplate(z,forward_traj[k],S); |
4686 | |
4687 | // wire position at the endplate |
4688 | wirepos=origin; |
4689 | wirepos+=(endplate_z-z0w)*dir; |
4690 | |
4691 | dx=S(state_x)-wirepos.X(); |
4692 | dy=S(state_y)-wirepos.Y(); |
4693 | } |
4694 | else{ |
4695 | // Find the true doca to the wire. If we had to use Brent's |
4696 | // algorithm, the routine returns true. |
4697 | double step1=mStepSizeZ; |
4698 | double step2=mStepSizeZ; |
4699 | if (k>=2){ |
4700 | step1=-forward_traj[k].z+forward_traj[k_minus_1].z; |
4701 | step2=-forward_traj[k_minus_1].z+forward_traj[k-2].z; |
4702 | } |
4703 | swimmed_to_doca=FindDoca(my_cdchits[cdc_index],forward_traj[k], |
4704 | step1,step2,S0,S,C,dx,dy,dz); |
4705 | if (swimmed_to_doca==BRENT_FAILED){ |
4706 | //break_point_fdc_index=(3*num_fdc)/4; |
4707 | return MOMENTUM_OUT_OF_RANGE; |
4708 | } |
4709 | |
4710 | newz=forward_traj[k].z+dz; |
4711 | } |
4712 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
4713 | double d=sqrt(dx*dx+dy*dy)*cosstereo+EPS3.0e-8; |
4714 | |
4715 | // Track projection |
4716 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
4717 | Hc_T(state_x)=dx*cosstereo2_over_d; |
4718 | Hc(state_x)=Hc_T(state_x); |
4719 | Hc_T(state_y)=dy*cosstereo2_over_d; |
4720 | Hc(state_y)=Hc_T(state_y); |
4721 | if (swimmed_to_doca==DOCA_NO_BRENT){ |
4722 | Hc_T(state_ty)=Hc_T(state_y)*dz; |
4723 | Hc(state_ty)=Hc_T(state_ty); |
4724 | Hc_T(state_tx)=Hc_T(state_x)*dz; |
4725 | Hc(state_tx)=Hc_T(state_tx); |
4726 | } |
4727 | else{ |
4728 | Hc_T(state_ty)=0.; |
4729 | Hc(state_ty)=0.; |
4730 | Hc_T(state_tx)=0.; |
4731 | Hc(state_tx)=0.; |
4732 | } |
4733 | |
4734 | //H.Print(); |
4735 | |
4736 | // The next measurement |
4737 | double dm=0.39,tdrift=0.,tcorr=0.,dDdt0=0.; |
4738 | if (fit_type==kTimeBased){ |
4739 | // Find offset of wire with respect to the center of the |
4740 | // straw at this z position |
4741 | double delta=0,dphi=0.; |
4742 | FindSag(dx,dy,newz-z0w,my_cdchits[cdc_index]->hit->wire,delta,dphi); |
4743 | |
4744 | // Find drift time and distance |
4745 | tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
4746 | -forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
4747 | double B=forward_traj[k_minus_1].B; |
4748 | ComputeCDCDrift(dphi,delta,tdrift,B,dm,Vc,tcorr); |
4749 | Vc*=cdc_anneal_factor; |
4750 | if (ALIGNMENT_FORWARD){ |
4751 | double myV=0.; |
4752 | double mytcorr=0.; |
4753 | double d_shifted; |
4754 | double dt=5.0; |
4755 | // Dont compute this for negative drift times |
4756 | if (tdrift < 0.) d_shifted = dm; |
4757 | else ComputeCDCDrift(dphi,delta,tdrift+dt,B,d_shifted,myV,mytcorr); |
4758 | dDdt0=(d_shifted-dm)/dt; |
4759 | } |
4760 | |
4761 | if (max_num_fdc_used_in_fit>4) |
4762 | { |
4763 | Vc*=CDC_VAR_SCALE_FACTOR; //de-weight CDC hits |
4764 | } |
4765 | //_DBG_ << "t " << tdrift << " d " << d << " delta " << delta << " dphi " << atan2(dy,dx)-mywire->origin.Phi() << endl; |
4766 | |
4767 | //_DBG_ << tcorr << " " << dphi << " " << dm << endl; |
4768 | } |
4769 | |
4770 | // Residual |
4771 | double res=dm-d; |
4772 | |
4773 | // inverse variance including prediction |
4774 | //double InvV1=1./(Vc+H*(C*H_T)); |
4775 | //double Vproj=C.SandwichMultiply(Hc_T); |
4776 | double Vproj=Hc*C*Hc_T; |
4777 | double InvV1=1./(Vc+Vproj); |
4778 | if (InvV1<0.){ |
4779 | if (DEBUG_LEVEL>0) |
4780 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4780<<" " << "Negative variance???" << endl; |
4781 | return NEGATIVE_VARIANCE; |
4782 | } |
4783 | |
4784 | // Check if this hit is an outlier |
4785 | double chi2_hit=res*res*InvV1; |
4786 | if (chi2_hit<cdc_chi2cut){ |
4787 | // Compute KalmanSIMD gain matrix |
4788 | Kc=InvV1*(C*Hc_T); |
4789 | |
4790 | // Update state vector covariance matrix |
4791 | //C=C-K*(H*C); |
4792 | Ctest=C.SubSym(Kc*(Hc*C)); |
4793 | //Ctest=C.SandwichMultiply(I5x5-K*H)+Vc*MultiplyTranspose(K); |
4794 | // Check that Ctest is positive definite |
4795 | if (!Ctest.IsPosDef()){ |
4796 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4796<<" " << "Broken covariance matrix!" <<endl; |
4797 | return BROKEN_COVARIANCE_MATRIX; |
4798 | } |
4799 | bool skip_ring |
4800 | =(my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP); |
4801 | // update covariance matrix and state vector |
4802 | if (my_cdchits[cdc_index]->hit->wire->ring!=RING_TO_SKIP && tdrift >= CDC_T_DRIFT_MIN){ |
4803 | C=Ctest; |
4804 | S+=res*Kc; |
4805 | |
4806 | if (DEBUG_LEVEL > 25) { |
4807 | jout << " == Adding CDC Hit in Ring " << my_cdchits[cdc_index]->hit->wire->ring << endl; |
4808 | jout << " New S: " << endl; S.Print(); |
4809 | jout << " New C: " << endl; C.Print(); |
4810 | } |
4811 | } |
4812 | |
4813 | // Flag the place along the reference trajectory with hit id |
4814 | forward_traj[k].h_id=1000+cdc_index; |
4815 | |
4816 | // Store updated info related to this hit |
4817 | double scale=(skip_ring)?1.:(1.-Hc*Kc); |
4818 | cdc_updates[cdc_index].tdrift=tdrift; |
4819 | cdc_updates[cdc_index].tcorr=tcorr; |
4820 | cdc_updates[cdc_index].variance=Vc; |
4821 | cdc_updates[cdc_index].doca=dm; |
4822 | cdc_updates[cdc_index].dDdt0=dDdt0; |
4823 | cdc_used_in_fit[cdc_index]=true; |
4824 | if(tdrift < CDC_T_DRIFT_MIN){ |
4825 | //_DBG_ << tdrift << endl; |
4826 | cdc_used_in_fit[cdc_index]=false; |
4827 | } |
4828 | |
4829 | // Update chi2 and number of degrees of freedom for this hit |
4830 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
4831 | chisq+=scale*res*res/Vc; |
4832 | numdof++; |
4833 | } |
4834 | |
4835 | if (DEBUG_LEVEL>10) |
4836 | jout << "Ring " << my_cdchits[cdc_index]->hit->wire->ring |
4837 | << " Straw " << my_cdchits[cdc_index]->hit->wire->straw |
4838 | << " Pred " << d << " Meas " << dm |
4839 | << " Sigma meas " << sqrt(Vc) |
4840 | << " t " << tcorr |
4841 | << " Chi2 " << (1.-Hc*Kc)*res*res/Vc << endl; |
4842 | |
4843 | break_point_cdc_index=cdc_index; |
4844 | break_point_step_index=k_minus_1; |
4845 | |
4846 | } |
4847 | |
4848 | // If we had to use Brent's algorithm to find the true doca or the |
4849 | // doca to the line corresponding to the wire is downstream of the |
4850 | // cdc endplate, we need to swim the state vector and covariance |
4851 | // matrix back to the appropriate position along the reference |
4852 | // trajectory. |
4853 | if (swimmed_to_doca!=DOCA_NO_BRENT){ |
4854 | double dedx=0.; |
4855 | if (CORRECT_FOR_ELOSS){ |
4856 | dedx=GetdEdx(S(state_q_over_p), |
4857 | forward_traj[k].K_rho_Z_over_A, |
4858 | forward_traj[k].rho_Z_over_A, |
4859 | forward_traj[k].LnI,forward_traj[k].Z); |
4860 | } |
4861 | StepBack(dedx,newz,forward_traj[k].z,S0,S,C); |
4862 | } |
4863 | |
4864 | cdc_updates[cdc_index].S=S; |
4865 | cdc_updates[cdc_index].C=C; |
4866 | } |
4867 | |
4868 | // new wire origin and direction |
4869 | if (cdc_index>0){ |
4870 | cdc_index--; |
4871 | origin=my_cdchits[cdc_index]->origin; |
4872 | z0w=my_cdchits[cdc_index]->z0wire; |
4873 | dir=my_cdchits[cdc_index]->dir; |
4874 | } |
4875 | else more_cdc_measurements=false; |
4876 | |
4877 | // Update the wire position |
4878 | wirepos=origin+(z-z0w)*dir; |
4879 | |
4880 | // new doca |
4881 | dx=S(state_x)-wirepos.X(); |
4882 | dy=S(state_y)-wirepos.Y(); |
4883 | doca2=dx*dx+dy*dy; |
4884 | } |
4885 | old_doca2=doca2; |
4886 | } |
4887 | } |
4888 | // Save final z position |
4889 | z_=forward_traj[forward_traj.size()-1].z; |
4890 | |
4891 | // The following code segment addes a fake point at a well-defined z position |
4892 | // that would correspond to a thin foil target. It should not be turned on |
4893 | // for an extended target. |
4894 | if (ADD_VERTEX_POINT){ |
4895 | double dz_to_target=TARGET_Z-z_; |
4896 | double my_dz=mStepSizeZ*(dz_to_target>0?1.:-1.); |
4897 | int num_steps=int(fabs(dz_to_target/my_dz)); |
4898 | |
4899 | for (int k=0;k<num_steps;k++){ |
4900 | double newz=z_+my_dz; |
4901 | // Step C along z |
4902 | StepJacobian(z_,newz,S,0.,J); |
4903 | C=J*C*J.Transpose(); |
4904 | //C=C.SandwichMultiply(J); |
4905 | |
4906 | // Step S along z |
4907 | Step(z_,newz,0.,S); |
4908 | |
4909 | z_=newz; |
4910 | } |
4911 | |
4912 | // Step C along z |
4913 | StepJacobian(z_,TARGET_Z,S,0.,J); |
4914 | C=J*C*J.Transpose(); |
4915 | //C=C.SandwichMultiply(J); |
4916 | |
4917 | // Step S along z |
4918 | Step(z_,TARGET_Z,0.,S); |
4919 | |
4920 | z_=TARGET_Z; |
4921 | |
4922 | // predicted doca taking into account the orientation of the wire |
4923 | double dy=S(state_y); |
4924 | double dx=S(state_x); |
4925 | double d=sqrt(dx*dx+dy*dy); |
4926 | |
4927 | // Track projection |
4928 | double one_over_d=1./d; |
4929 | Hc_T(state_x)=dx*one_over_d; |
4930 | Hc(state_x)=Hc_T(state_x); |
4931 | Hc_T(state_y)=dy*one_over_d; |
4932 | Hc(state_y)=Hc_T(state_y); |
4933 | |
4934 | // Variance of target point |
4935 | // Variance is for average beam spot size assuming triangular distribution |
4936 | // out to 2.2 mm from the beam line. |
4937 | // sigma_r = 2.2 mm/ sqrt(18) |
4938 | Vc=0.002689; |
4939 | |
4940 | // inverse variance including prediction |
4941 | double InvV1=1./(Vc+Hc*(C*Hc_T)); |
4942 | //double InvV1=1./(Vc+C.SandwichMultiply(H_T)); |
4943 | if (InvV1<0.){ |
4944 | if (DEBUG_LEVEL>0) |
4945 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<4945<<" " << "Negative variance???" << endl; |
4946 | return NEGATIVE_VARIANCE; |
4947 | } |
4948 | // Compute KalmanSIMD gain matrix |
4949 | Kc=InvV1*(C*Hc_T); |
4950 | |
4951 | // Update the state vector with the target point |
4952 | // "Measurement" is average of expected beam spot size |
4953 | double res=0.1466666667-d; |
4954 | S+=res*Kc; |
4955 | // Update state vector covariance matrix |
4956 | //C=C-K*(H*C); |
4957 | C=C.SubSym(Kc*(Hc*C)); |
4958 | |
4959 | // Update chi2 for this segment |
4960 | chisq+=(1.-Hc*Kc)*res*res/Vc; |
4961 | numdof++; |
4962 | } |
4963 | |
4964 | // Check that there were enough hits to make this a valid fit |
4965 | if (numdof<6){ |
4966 | chisq=-1.; |
4967 | numdof=0; |
4968 | |
4969 | if (num_cdc==0){ |
4970 | unsigned int new_index=(3*num_fdc)/4; |
4971 | break_point_fdc_index=(new_index>=MIN_HITS_FOR_REFIT)?new_index:(MIN_HITS_FOR_REFIT-1); |
4972 | } |
4973 | else{ |
4974 | unsigned int new_index=(3*num_fdc)/4; |
4975 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
4976 | break_point_fdc_index=new_index; |
4977 | } |
4978 | else{ |
4979 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
4980 | } |
4981 | } |
4982 | return PRUNED_TOO_MANY_HITS; |
4983 | } |
4984 | |
4985 | // chisq*=anneal_factor; |
4986 | numdof-=5; |
4987 | |
4988 | // Final positions in x and y for this leg |
4989 | x_=S(state_x); |
4990 | y_=S(state_y); |
4991 | |
4992 | if (DEBUG_LEVEL>1){ |
4993 | cout << "Position after forward filter: " << x_ << ", " << y_ << ", " << z_ <<endl; |
4994 | cout << "Momentum " << 1./S(state_q_over_p) <<endl; |
4995 | } |
4996 | |
4997 | if (!S.IsFinite()) return FIT_FAILED; |
4998 | |
4999 | // Check if we have a kink in the track or threw away too many hits |
5000 | if (num_cdc>0 && break_point_fdc_index>0 && break_point_cdc_index>2){ |
5001 | if (break_point_fdc_index+num_cdc<MIN_HITS_FOR_REFIT){ |
5002 | //_DBG_ << endl; |
5003 | unsigned int new_index=(3*num_fdc)/4; |
5004 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
5005 | break_point_fdc_index=new_index; |
5006 | } |
5007 | else{ |
5008 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
5009 | } |
5010 | } |
5011 | return BREAK_POINT_FOUND; |
5012 | } |
5013 | if (num_cdc==0 && break_point_fdc_index>2){ |
5014 | //_DBG_ << endl; |
5015 | if (break_point_fdc_index<num_fdc/2){ |
5016 | break_point_fdc_index=(3*num_fdc)/4; |
5017 | } |
5018 | if (break_point_fdc_index<MIN_HITS_FOR_REFIT-1){ |
5019 | break_point_fdc_index=MIN_HITS_FOR_REFIT-1; |
5020 | } |
5021 | return BREAK_POINT_FOUND; |
5022 | } |
5023 | if (num_cdc>5 && break_point_cdc_index>2){ |
5024 | //_DBG_ << endl; |
5025 | unsigned int new_index=3*(num_fdc)/4; |
5026 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
5027 | break_point_fdc_index=new_index; |
5028 | } |
5029 | else{ |
5030 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
5031 | } |
5032 | return BREAK_POINT_FOUND; |
5033 | } |
5034 | unsigned int num_good=0; |
5035 | unsigned int num_hits=num_cdc+max_num_fdc_used_in_fit; |
5036 | for (unsigned int j=0;j<num_cdc;j++){ |
5037 | if (cdc_used_in_fit[j]) num_good++; |
5038 | } |
5039 | for (unsigned int j=0;j<num_fdc;j++){ |
5040 | if (fdc_used_in_fit[j]) num_good++; |
5041 | } |
5042 | if (double(num_good)/double(num_hits)<MINIMUM_HIT_FRACTION){ |
5043 | //_DBG_ <<endl; |
5044 | if (num_cdc==0){ |
5045 | unsigned int new_index=(3*num_fdc)/4; |
5046 | break_point_fdc_index=(new_index>=MIN_HITS_FOR_REFIT)?new_index:(MIN_HITS_FOR_REFIT-1); |
5047 | } |
5048 | else{ |
5049 | unsigned int new_index=(3*num_fdc)/4; |
5050 | if (new_index+num_cdc>=MIN_HITS_FOR_REFIT){ |
5051 | break_point_fdc_index=new_index; |
5052 | } |
5053 | else{ |
5054 | break_point_fdc_index=MIN_HITS_FOR_REFIT-num_cdc; |
5055 | } |
5056 | } |
5057 | return PRUNED_TOO_MANY_HITS; |
5058 | } |
5059 | |
5060 | return FIT_SUCCEEDED; |
5061 | } |
5062 | |
5063 | |
5064 | |
5065 | // Kalman engine for forward tracks -- this routine adds CDC hits |
5066 | kalman_error_t DTrackFitterKalmanSIMD::KalmanForwardCDC(double anneal, |
5067 | DMatrix5x1 &S, |
5068 | DMatrix5x5 &C, |
5069 | double &chisq, |
5070 | unsigned int &numdof){ |
5071 | DMatrix1x5 H; // Track projection matrix |
5072 | DMatrix5x1 H_T; // Transpose of track projection matrix |
5073 | DMatrix5x5 J; // State vector Jacobian matrix |
5074 | //DMatrix5x5 J_T; // transpose of this matrix |
5075 | DMatrix5x5 Q; // Process noise covariance matrix |
5076 | DMatrix5x1 K; // KalmanSIMD gain matrix |
5077 | DMatrix5x1 S0,S0_,Stest; //State vector |
5078 | DMatrix5x5 Ctest; // covariance matrix |
5079 | //DMatrix5x1 dS; // perturbation in state vector |
5080 | double V=0.0507; |
5081 | |
5082 | // set used_in_fit flags to false for cdc hits |
5083 | unsigned int num_cdc=cdc_used_in_fit.size(); |
5084 | for (unsigned int i=0;i<num_cdc;i++) cdc_used_in_fit[i]=false; |
5085 | for (unsigned int i=0;i<forward_traj.size();i++){ |
5086 | forward_traj[i].h_id=0; |
5087 | } |
5088 | |
5089 | // initialize chi2 info |
5090 | chisq=0.; |
5091 | numdof=0; |
5092 | |
5093 | double chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
5094 | |
5095 | // Save the starting values for C and S in the deque |
5096 | forward_traj[break_point_step_index].Skk=S; |
5097 | forward_traj[break_point_step_index].Ckk=C; |
5098 | |
5099 | // z-position |
5100 | double z=forward_traj[break_point_step_index].z; |
5101 | |
5102 | // wire information |
5103 | unsigned int cdc_index=break_point_cdc_index; |
5104 | if (cdc_index<num_cdc-1){ |
5105 | num_cdc=cdc_index+1; |
5106 | } |
5107 | |
5108 | if (num_cdc<MIN_HITS_FOR_REFIT) chi2cut=BIG1.0e8; |
5109 | |
5110 | DVector2 origin=my_cdchits[cdc_index]->origin; |
5111 | double z0w=my_cdchits[cdc_index]->z0wire; |
5112 | DVector2 dir=my_cdchits[cdc_index]->dir; |
5113 | DVector2 wirepos=origin+(z-z0w)*dir; |
5114 | bool more_measurements=true; |
5115 | |
5116 | // doca variables |
5117 | double dx=S(state_x)-wirepos.X(); |
5118 | double dy=S(state_y)-wirepos.Y(); |
5119 | double doca2=0.,old_doca2=dx*dx+dy*dy; |
5120 | |
5121 | // loop over entries in the trajectory |
5122 | S0_=(forward_traj[break_point_step_index].S); |
5123 | for (unsigned int k=break_point_step_index+1;k<forward_traj.size()/*-1*/;k++){ |
5124 | unsigned int k_minus_1=k-1; |
5125 | |
5126 | // Check that C matrix is positive definite |
5127 | if (!C.IsPosDef()){ |
5128 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5128<<" " << "Broken covariance matrix!" <<endl; |
5129 | return BROKEN_COVARIANCE_MATRIX; |
5130 | } |
5131 | |
5132 | z=forward_traj[k].z; |
5133 | |
5134 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
5135 | // from reference trajectory |
5136 | S0=(forward_traj[k].S); |
5137 | J=(forward_traj[k].J); |
5138 | Q=(forward_traj[k].Q); |
5139 | |
5140 | // Update the actual state vector and covariance matrix |
5141 | S=S0+J*(S-S0_); |
5142 | |
5143 | // Bail if the position is grossly outside of the tracking volume |
5144 | if (S(state_x)*S(state_x)+S(state_y)*S(state_y)>R2_MAX4225.0){ |
5145 | if (DEBUG_LEVEL>2) |
5146 | { |
5147 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5147<<" "<< "Went outside of tracking volume at x=" << S(state_x) |
5148 | << " y=" << S(state_y) <<" z="<<z<<endl; |
5149 | } |
5150 | return POSITION_OUT_OF_RANGE; |
5151 | } |
5152 | // Bail if the momentum has dropped below some minimum |
5153 | if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX){ |
5154 | if (DEBUG_LEVEL>2) |
5155 | { |
5156 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5156<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl; |
5157 | } |
5158 | return MOMENTUM_OUT_OF_RANGE; |
5159 | } |
5160 | |
5161 | //C=J*(C*J_T)+Q; |
5162 | C=Q.AddSym(J*C*J.Transpose()); |
5163 | //C=Q.AddSym(C.SandwichMultiply(J)); |
5164 | |
5165 | // Save the current state of the reference trajectory |
5166 | S0_=S0; |
5167 | |
5168 | // new wire position |
5169 | wirepos=origin; |
5170 | wirepos+=(z-z0w)*dir; |
5171 | |
5172 | // new doca |
5173 | dx=S(state_x)-wirepos.X(); |
5174 | dy=S(state_y)-wirepos.Y(); |
5175 | doca2=dx*dx+dy*dy; |
5176 | |
5177 | // Save the current state and covariance matrix in the deque |
5178 | if (fit_type==kTimeBased){ |
5179 | forward_traj[k].Skk=S; |
5180 | forward_traj[k].Ckk=C; |
5181 | } |
5182 | |
5183 | // Check if the doca is no longer decreasing |
5184 | if (more_measurements && doca2>old_doca2/* && z<endplate_z_downstream*/){ |
5185 | if (my_cdchits[cdc_index]->status==good_hit){ |
5186 | find_doca_error_t swimmed_to_doca=DOCA_NO_BRENT; |
5187 | double newz=endplate_z; |
5188 | double dz=newz-z; |
5189 | // Sometimes the true doca would correspond to the case where the |
5190 | // wire would need to extend beyond the physical volume of the straw. |
5191 | // In this case, find the doca at the cdc endplate. |
5192 | if (z>endplate_z){ |
5193 | swimmed_to_doca=DOCA_ENDPLATE; |
5194 | SwimToEndplate(z,forward_traj[k],S); |
5195 | |
5196 | // wire position at the endplate |
5197 | wirepos=origin; |
5198 | wirepos+=(endplate_z-z0w)*dir; |
5199 | |
5200 | dx=S(state_x)-wirepos.X(); |
5201 | dy=S(state_y)-wirepos.Y(); |
5202 | } |
5203 | else{ |
5204 | // Find the true doca to the wire. If we had to use Brent's |
5205 | // algorithm, the routine returns USED_BRENT |
5206 | double step1=mStepSizeZ; |
5207 | double step2=mStepSizeZ; |
5208 | if (k>=2){ |
5209 | step1=-forward_traj[k].z+forward_traj[k_minus_1].z; |
5210 | step2=-forward_traj[k_minus_1].z+forward_traj[k-2].z; |
5211 | } |
5212 | swimmed_to_doca=FindDoca(my_cdchits[cdc_index],forward_traj[k], |
5213 | step1,step2,S0,S,C,dx,dy,dz); |
5214 | if (swimmed_to_doca==BRENT_FAILED){ |
5215 | break_point_cdc_index=(3*num_cdc)/4; |
5216 | return MOMENTUM_OUT_OF_RANGE; |
5217 | } |
5218 | newz=forward_traj[k].z+dz; |
5219 | } |
5220 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
5221 | double d=sqrt(dx*dx+dy*dy)*cosstereo+EPS3.0e-8; |
5222 | |
5223 | // Track projection |
5224 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
5225 | H_T(state_x)=dx*cosstereo2_over_d; |
5226 | H(state_x)=H_T(state_x); |
5227 | H_T(state_y)=dy*cosstereo2_over_d; |
5228 | H(state_y)=H_T(state_y); |
5229 | if (swimmed_to_doca==DOCA_NO_BRENT){ |
5230 | H_T(state_ty)=H_T(state_y)*dz; |
5231 | H(state_ty)=H_T(state_ty); |
5232 | H_T(state_tx)=H_T(state_x)*dz; |
5233 | H(state_tx)=H_T(state_tx); |
5234 | } |
5235 | else{ |
5236 | H_T(state_ty)=0.; |
5237 | H(state_ty)=0.; |
5238 | H_T(state_tx)=0.; |
5239 | H(state_tx)=0.; |
5240 | } |
5241 | |
5242 | // The next measurement |
5243 | double dm=0.39,tdrift=0.,tcorr=0.,dDdt0=0.; |
5244 | if (fit_type==kTimeBased || USE_PASS1_TIME_MODE){ |
5245 | // Find offset of wire with respect to the center of the |
5246 | // straw at this z position |
5247 | double delta=0,dphi=0.; |
5248 | FindSag(dx,dy,newz-z0w,my_cdchits[cdc_index]->hit->wire,delta,dphi); |
5249 | // Find drift time and distance |
5250 | tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
5251 | -forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
5252 | double B=forward_traj[k_minus_1].B; |
5253 | ComputeCDCDrift(dphi,delta,tdrift,B,dm,V,tcorr); |
5254 | V*=anneal; |
5255 | if (ALIGNMENT_FORWARD){ |
5256 | double myV=0.; |
5257 | double mytcorr=0.; |
5258 | double d_shifted; |
5259 | double dt=2.0; |
5260 | if (tdrift < 0.) d_shifted = dm; |
5261 | else ComputeCDCDrift(dphi,delta,tdrift+dt,B,d_shifted,myV,mytcorr); |
5262 | dDdt0=(d_shifted-dm)/dt; |
5263 | } |
5264 | //_DBG_ << tcorr << " " << dphi << " " << dm << endl; |
5265 | |
5266 | } |
5267 | // residual |
5268 | double res=dm-d; |
5269 | |
5270 | // inverse of variance including prediction |
5271 | double Vproj=H*C*H_T; |
5272 | double InvV=1./(V+Vproj); |
5273 | if (InvV<0.){ |
5274 | if (DEBUG_LEVEL>0) |
5275 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5275<<" " << "Negative variance???" << endl; |
5276 | break_point_cdc_index=(3*num_cdc)/4; |
5277 | return NEGATIVE_VARIANCE; |
5278 | } |
5279 | |
5280 | // Check how far this hit is from the expected position |
5281 | double chi2check=res*res*InvV; |
5282 | if (chi2check<chi2cut){ |
5283 | // Compute KalmanSIMD gain matrix |
5284 | K=InvV*(C*H_T); |
5285 | |
5286 | // Update state vector covariance matrix |
5287 | Ctest=C.SubSym(K*(H*C)); |
5288 | // Joseph form |
5289 | // C = (I-KH)C(I-KH)^T + KVK^T |
5290 | //Ctest=C.SandwichMultiply(I5x5-K*H)+V*MultiplyTranspose(K); |
5291 | // Check that Ctest is positive definite |
5292 | if (!Ctest.IsPosDef()){ |
5293 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5293<<" " << "Broken covariance matrix!" <<endl; |
5294 | return BROKEN_COVARIANCE_MATRIX; |
5295 | } |
5296 | |
5297 | bool skip_ring |
5298 | =(my_cdchits[cdc_index]->hit->wire->ring==RING_TO_SKIP); |
5299 | // update covariance matrix and state vector |
5300 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
5301 | C=Ctest; |
5302 | S+=res*K; |
5303 | } |
5304 | // Mark point on ref trajectory with a hit id for the straw |
5305 | forward_traj[k].h_id=cdc_index+1000; |
5306 | |
5307 | // Store some updated values related to the hit |
5308 | double scale=(skip_ring)?1.:(1.-H*K); |
5309 | cdc_updates[cdc_index].tcorr=tcorr; |
5310 | cdc_updates[cdc_index].tdrift=tdrift; |
5311 | cdc_updates[cdc_index].doca=dm; |
5312 | cdc_updates[cdc_index].variance=V; |
5313 | cdc_updates[cdc_index].dDdt0=dDdt0; |
5314 | cdc_used_in_fit[cdc_index]=true; |
5315 | if(tdrift < CDC_T_DRIFT_MIN) cdc_used_in_fit[cdc_index]=false; |
5316 | |
5317 | // Update chi2 for this segment |
5318 | if (skip_ring==false && tdrift >= CDC_T_DRIFT_MIN){ |
5319 | chisq+=scale*res*res/V; |
5320 | numdof++; |
5321 | } |
5322 | break_point_cdc_index=cdc_index; |
5323 | break_point_step_index=k_minus_1; |
5324 | |
5325 | if (DEBUG_LEVEL>9) |
5326 | printf("Ring %d straw %d pred %f meas %f chi2 %f useBrent %i \n", |
5327 | my_cdchits[cdc_index]->hit->wire->ring, |
5328 | my_cdchits[cdc_index]->hit->wire->straw, |
5329 | d,dm,(1.-H*K)*res*res/V,swimmed_to_doca); |
5330 | |
5331 | } |
5332 | |
5333 | // If we had to use Brent's algorithm to find the true doca or the |
5334 | // doca to the line corresponding to the wire is downstream of the |
5335 | // cdc endplate, we need to swim the state vector and covariance |
5336 | // matrix back to the appropriate position along the reference |
5337 | // trajectory. |
5338 | if (swimmed_to_doca!=DOCA_NO_BRENT){ |
5339 | double dedx=0.; |
5340 | if (CORRECT_FOR_ELOSS){ |
5341 | dedx=GetdEdx(S(state_q_over_p), |
5342 | forward_traj[k].K_rho_Z_over_A, |
5343 | forward_traj[k].rho_Z_over_A, |
5344 | forward_traj[k].LnI,forward_traj[k].Z); |
5345 | } |
5346 | StepBack(dedx,newz,forward_traj[k].z,S0,S,C); |
5347 | } |
5348 | |
5349 | cdc_updates[cdc_index].S=S; |
5350 | cdc_updates[cdc_index].C=C; |
5351 | } |
5352 | |
5353 | // new wire origin and direction |
5354 | if (cdc_index>0){ |
5355 | cdc_index--; |
5356 | origin=my_cdchits[cdc_index]->origin; |
5357 | z0w=my_cdchits[cdc_index]->z0wire; |
5358 | dir=my_cdchits[cdc_index]->dir; |
5359 | } |
5360 | else{ |
5361 | more_measurements=false; |
5362 | } |
5363 | |
5364 | // Update the wire position |
5365 | wirepos=origin; |
5366 | wirepos+=(z-z0w)*dir; |
5367 | |
5368 | // new doca |
5369 | dx=S(state_x)-wirepos.X(); |
5370 | dy=S(state_y)-wirepos.Y(); |
5371 | doca2=dx*dx+dy*dy; |
5372 | } |
5373 | old_doca2=doca2; |
5374 | } |
5375 | |
5376 | // Check that there were enough hits to make this a valid fit |
5377 | if (numdof<6){ |
5378 | chisq=-1.; |
5379 | numdof=0; |
5380 | return PRUNED_TOO_MANY_HITS; |
5381 | } |
5382 | numdof-=5; |
5383 | |
5384 | // Final position for this leg |
5385 | x_=S(state_x); |
5386 | y_=S(state_y); |
5387 | z_=forward_traj[forward_traj.size()-1].z; |
5388 | |
5389 | if (!S.IsFinite()) return FIT_FAILED; |
5390 | |
5391 | // Check if the momentum is unphysically large |
5392 | if (1./fabs(S(state_q_over_p))>12.0){ |
5393 | if (DEBUG_LEVEL>2) |
5394 | { |
5395 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5395<<" " << "Unphysical momentum: P = " << 1./fabs(S(state_q_over_p)) |
5396 | <<endl; |
5397 | } |
5398 | return MOMENTUM_OUT_OF_RANGE; |
5399 | } |
5400 | |
5401 | // Check if we have a kink in the track or threw away too many cdc hits |
5402 | if (num_cdc>=MIN_HITS_FOR_REFIT){ |
5403 | if (break_point_cdc_index>1){ |
5404 | if (break_point_cdc_index<num_cdc/2){ |
5405 | break_point_cdc_index=(3*num_cdc)/4; |
5406 | } |
5407 | return BREAK_POINT_FOUND; |
5408 | } |
5409 | |
5410 | unsigned int num_good=0; |
5411 | for (unsigned int j=0;j<num_cdc;j++){ |
5412 | if (cdc_used_in_fit[j]) num_good++; |
5413 | } |
5414 | if (double(num_good)/double(num_cdc)<MINIMUM_HIT_FRACTION){ |
5415 | return PRUNED_TOO_MANY_HITS; |
5416 | } |
5417 | } |
5418 | |
5419 | return FIT_SUCCEEDED; |
5420 | } |
5421 | |
5422 | // Extrapolate to the point along z of closest approach to the beam line using |
5423 | // the forward track state vector parameterization. Converts to the central |
5424 | // track representation at the end. |
5425 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DMatrix5x1 &S, |
5426 | DMatrix5x5 &C){ |
5427 | DMatrix5x5 J; // Jacobian matrix |
5428 | DMatrix5x5 Q; // multiple scattering matrix |
5429 | DMatrix5x1 S1(S); // copy of S |
5430 | |
5431 | // position variables |
5432 | double z=z_,newz=z_; |
5433 | |
5434 | DVector2 beam_pos=beam_center+(z-beam_z0)*beam_dir; |
5435 | double r2_old=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5436 | double dz_old=0.; |
5437 | double dEdx=0.; |
5438 | double sign=1.; |
5439 | |
5440 | // material properties |
5441 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
5442 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5443 | |
5444 | // if (fit_type==kTimeBased)printf("------Extrapolating\n"); |
5445 | |
5446 | // printf("-----------\n"); |
5447 | // Current position |
5448 | DVector3 pos(S(state_x),S(state_y),z); |
5449 | |
5450 | // get material properties from the Root Geometry |
5451 | if (geom->FindMatKalman(pos,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5452 | chi2c_factor,chi2a_factor,chi2a_corr, |
5453 | last_material_map) |
5454 | !=NOERROR){ |
5455 | if (DEBUG_LEVEL>1) |
5456 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5456<<" " << "Material error in ExtrapolateToVertex at (x,y,z)=(" |
5457 | << pos.X() <<"," << pos.y()<<","<< pos.z()<<")"<< endl; |
5458 | return UNRECOVERABLE_ERROR; |
5459 | } |
5460 | |
5461 | // Adjust the step size |
5462 | double ds_dz=sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
5463 | double dz=-mStepSizeS/ds_dz; |
5464 | if (fabs(dEdx)>EPS3.0e-8){ |
5465 | dz=(-1.)*DE_PER_STEP0.001/fabs(dEdx)/ds_dz; |
5466 | } |
5467 | if(fabs(dz)>mStepSizeZ) dz=-mStepSizeZ; |
5468 | if(fabs(dz)<MIN_STEP_SIZE0.1)dz=-MIN_STEP_SIZE0.1; |
5469 | |
5470 | // Get dEdx for the upcoming step |
5471 | if (CORRECT_FOR_ELOSS){ |
5472 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5473 | } |
5474 | |
5475 | |
5476 | double ztest=endplate_z; |
5477 | if (my_fdchits.size()>0){ |
5478 | ztest =my_fdchits[0]->z-1.; |
5479 | } |
5480 | if (z<ztest) |
5481 | { |
5482 | // Check direction of propagation |
5483 | DMatrix5x1 S2(S); // second copy of S |
5484 | |
5485 | // Step test states through the field and compute squared radii |
5486 | Step(z,z-dz,dEdx,S1); |
5487 | // Bail if the momentum has dropped below some minimum |
5488 | if (fabs(S1(state_q_over_p))>Q_OVER_P_MAX){ |
5489 | if (DEBUG_LEVEL>2) |
5490 | { |
5491 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5491<<" " << "Bailing: P = " << 1./fabs(S1(state_q_over_p)) |
5492 | << endl; |
5493 | } |
5494 | return UNRECOVERABLE_ERROR; |
5495 | } |
5496 | beam_pos=beam_center+(z-dz-beam_z0)*beam_dir; |
5497 | double r2minus=(DVector2(S1(state_x),S1(state_y))-beam_pos).Mod2(); |
5498 | |
5499 | Step(z,z+dz,dEdx,S2); |
5500 | // Bail if the momentum has dropped below some minimum |
5501 | if (fabs(S2(state_q_over_p))>Q_OVER_P_MAX){ |
5502 | if (DEBUG_LEVEL>2) |
5503 | { |
5504 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5504<<" " << "Bailing: P = " << 1./fabs(S2(state_q_over_p)) |
5505 | << endl; |
5506 | } |
5507 | return UNRECOVERABLE_ERROR; |
5508 | } |
5509 | beam_pos=beam_center+(z+dz-beam_z0)*beam_dir; |
5510 | double r2plus=(DVector2(S2(state_x),S2(state_y))-beam_pos).Mod2(); |
5511 | // Check to see if we have already bracketed the minimum |
5512 | if (r2plus>r2_old && r2minus>r2_old){ |
5513 | newz=z+dz; |
5514 | double dz2=0.; |
5515 | if (BrentsAlgorithm(newz,dz,dEdx,0.,beam_center,beam_dir,S2,dz2)!=NOERROR){ |
5516 | if (DEBUG_LEVEL>2) |
5517 | { |
5518 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5518<<" " << "Bailing: P = " << 1./fabs(S2(state_q_over_p)) |
5519 | << endl; |
5520 | } |
5521 | return UNRECOVERABLE_ERROR; |
5522 | } |
5523 | z_=newz+dz2; |
5524 | |
5525 | // Compute the Jacobian matrix |
5526 | StepJacobian(z,z_,S,dEdx,J); |
5527 | |
5528 | // Propagate the covariance matrix |
5529 | C=J*C*J.Transpose(); |
5530 | //C=C.SandwichMultiply(J); |
5531 | |
5532 | // Step to the position of the doca |
5533 | Step(z,z_,dEdx,S); |
5534 | |
5535 | // update internal variables |
5536 | x_=S(state_x); |
5537 | y_=S(state_y); |
5538 | |
5539 | return NOERROR; |
5540 | } |
5541 | |
5542 | // Find direction to propagate toward minimum doca |
5543 | if (r2minus<r2_old && r2_old<r2plus){ |
5544 | newz=z-dz; |
5545 | |
5546 | // Compute the Jacobian matrix |
5547 | StepJacobian(z,newz,S,dEdx,J); |
5548 | |
5549 | // Propagate the covariance matrix |
5550 | C=J*C*J.Transpose(); |
5551 | //C=C.SandwichMultiply(J); |
5552 | |
5553 | S2=S; |
5554 | S=S1; |
5555 | S1=S2; |
5556 | dz*=-1.; |
5557 | sign=-1.; |
5558 | dz_old=dz; |
5559 | r2_old=r2minus; |
5560 | z=z+dz; |
5561 | } |
5562 | if (r2minus>r2_old && r2_old>r2plus){ |
5563 | newz=z+dz; |
5564 | |
5565 | // Compute the Jacobian matrix |
5566 | StepJacobian(z,newz,S,dEdx,J); |
5567 | |
5568 | // Propagate the covariance matrix |
5569 | C=J*C*J.Transpose(); |
5570 | //C=C.SandwichMultiply(J); |
5571 | |
5572 | S1=S; |
5573 | S=S2; |
5574 | dz_old=dz; |
5575 | r2_old=r2plus; |
5576 | z=z+dz; |
5577 | } |
5578 | } |
5579 | |
5580 | double r2=r2_old; |
5581 | while (z>Z_MIN-100. && r2<R2_MAX4225.0 && z<ztest && r2>EPS3.0e-8){ |
5582 | // Bail if the momentum has dropped below some minimum |
5583 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
5584 | if (DEBUG_LEVEL>2) |
5585 | { |
5586 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5586<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
5587 | << endl; |
5588 | } |
5589 | return UNRECOVERABLE_ERROR; |
5590 | } |
5591 | |
5592 | // Relationship between arc length and z |
5593 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
5594 | |
5595 | // get material properties from the Root Geometry |
5596 | pos.SetXYZ(S(state_x),S(state_y),z); |
5597 | double s_to_boundary=1.e6; |
5598 | if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){ |
5599 | DVector3 mom(S(state_tx),S(state_ty),1.); |
5600 | if (geom->FindMatKalman(pos,mom,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5601 | chi2c_factor,chi2a_factor,chi2a_corr, |
5602 | last_material_map,&s_to_boundary) |
5603 | !=NOERROR){ |
5604 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5604<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5605 | return UNRECOVERABLE_ERROR; |
5606 | } |
5607 | } |
5608 | else{ |
5609 | if (geom->FindMatKalman(pos,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5610 | chi2c_factor,chi2a_factor,chi2a_corr, |
5611 | last_material_map) |
5612 | !=NOERROR){ |
5613 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5613<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5614 | break; |
5615 | } |
5616 | } |
5617 | |
5618 | // Get dEdx for the upcoming step |
5619 | if (CORRECT_FOR_ELOSS){ |
5620 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5621 | } |
5622 | |
5623 | // Adjust the step size |
5624 | //dz=-sign*mStepSizeS*dz_ds; |
5625 | double ds=mStepSizeS; |
5626 | if (fabs(dEdx)>EPS3.0e-8){ |
5627 | ds=DE_PER_STEP0.001/fabs(dEdx); |
5628 | } |
5629 | /* |
5630 | if(fabs(dz)>mStepSizeZ) dz=-sign*mStepSizeZ; |
5631 | if (fabs(dz)<z_to_boundary) dz=-sign*z_to_boundary; |
5632 | if(fabs(dz)<MIN_STEP_SIZE)dz=-sign*MIN_STEP_SIZE; |
5633 | */ |
5634 | if (ds>mStepSizeS) ds=mStepSizeS; |
5635 | if (ds>s_to_boundary) ds=s_to_boundary; |
5636 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
5637 | dz=-sign*ds*dz_ds; |
5638 | |
5639 | // Get the contribution to the covariance matrix due to multiple |
5640 | // scattering |
5641 | GetProcessNoise(z,ds,chi2c_factor,chi2a_factor,chi2a_corr,S,Q); |
5642 | |
5643 | if (CORRECT_FOR_ELOSS){ |
5644 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
5645 | double one_over_beta2=1.+mass2*q_over_p_sq; |
5646 | double varE=GetEnergyVariance(ds,one_over_beta2,K_rho_Z_over_A); |
5647 | Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2; |
5648 | } |
5649 | |
5650 | |
5651 | newz=z+dz; |
5652 | // Compute the Jacobian matrix |
5653 | StepJacobian(z,newz,S,dEdx,J); |
5654 | |
5655 | // Propagate the covariance matrix |
5656 | C=Q.AddSym(J*C*J.Transpose()); |
5657 | //C=Q.AddSym(C.SandwichMultiply(J)); |
5658 | |
5659 | // Step through field |
5660 | Step(z,newz,dEdx,S); |
5661 | |
5662 | // Check if we passed the minimum doca to the beam line |
5663 | beam_pos=beam_center+(newz-beam_z0)*beam_dir; |
5664 | r2=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5665 | //r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
5666 | if (r2>r2_old){ |
5667 | double two_step=dz+dz_old; |
5668 | |
5669 | // Find the increment/decrement in z to get to the minimum doca to the |
5670 | // beam line |
5671 | S1=S; |
5672 | if (BrentsAlgorithm(newz,0.5*two_step,dEdx,0.,beam_center,beam_dir,S,dz)!=NOERROR){ |
5673 | //_DBG_<<endl; |
5674 | return UNRECOVERABLE_ERROR; |
5675 | } |
5676 | |
5677 | // Compute the Jacobian matrix |
5678 | z_=newz+dz; |
5679 | StepJacobian(newz,z_,S1,dEdx,J); |
5680 | |
5681 | // Propagate the covariance matrix |
5682 | //C=J*C*J.Transpose()+(dz/(newz-z))*Q; |
5683 | //C=((dz/newz-z)*Q).AddSym(C.SandwichMultiply(J)); |
5684 | //C=C.SandwichMultiply(J); |
5685 | C=J*C*J.Transpose(); |
5686 | |
5687 | // update internal variables |
5688 | x_=S(state_x); |
5689 | y_=S(state_y); |
5690 | |
5691 | return NOERROR; |
5692 | } |
5693 | r2_old=r2; |
5694 | dz_old=dz; |
5695 | S1=S; |
5696 | z=newz; |
5697 | } |
5698 | // update internal variables |
5699 | x_=S(state_x); |
5700 | y_=S(state_y); |
5701 | z_=newz; |
5702 | |
5703 | return NOERROR; |
5704 | } |
5705 | |
5706 | |
5707 | // Extrapolate to the point along z of closest approach to the beam line using |
5708 | // the forward track state vector parameterization. |
5709 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DMatrix5x1 &S){ |
5710 | DMatrix5x5 J; // Jacobian matrix |
5711 | DMatrix5x1 S1(S); // copy of S |
5712 | |
5713 | // position variables |
5714 | double z=z_,newz=z_; |
5715 | DVector2 beam_pos=beam_center+(z-beam_z0)*beam_dir; |
5716 | double r2_old=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5717 | double dz_old=0.; |
5718 | double dEdx=0.; |
5719 | |
5720 | // Direction and origin for beam line |
5721 | DVector2 dir; |
5722 | DVector2 origin; |
5723 | |
5724 | // material properties |
5725 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
5726 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5727 | DVector3 pos; // current position along trajectory |
5728 | |
5729 | double r2=r2_old; |
5730 | while (z>Z_MIN-100. && r2<R2_MAX4225.0 && z<Z_MAX370.0 && r2>EPS3.0e-8){ |
5731 | // Bail if the momentum has dropped below some minimum |
5732 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
5733 | if (DEBUG_LEVEL>2) |
5734 | { |
5735 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5735<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
5736 | << endl; |
5737 | } |
5738 | return UNRECOVERABLE_ERROR; |
5739 | } |
5740 | |
5741 | // Relationship between arc length and z |
5742 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
5743 | |
5744 | // get material properties from the Root Geometry |
5745 | pos.SetXYZ(S(state_x),S(state_y),z); |
5746 | if (geom->FindMatKalman(pos,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5747 | chi2c_factor,chi2a_factor,chi2a_corr, |
5748 | last_material_map) |
5749 | !=NOERROR){ |
5750 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5750<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5751 | break; |
5752 | } |
5753 | |
5754 | // Get dEdx for the upcoming step |
5755 | if (CORRECT_FOR_ELOSS){ |
5756 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5757 | } |
5758 | |
5759 | // Adjust the step size |
5760 | double ds=mStepSizeS; |
5761 | if (fabs(dEdx)>EPS3.0e-8){ |
5762 | ds=DE_PER_STEP0.001/fabs(dEdx); |
5763 | } |
5764 | if (ds>mStepSizeS) ds=mStepSizeS; |
5765 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
5766 | double dz=-ds*dz_ds; |
5767 | |
5768 | newz=z+dz; |
5769 | |
5770 | |
5771 | // Step through field |
5772 | Step(z,newz,dEdx,S); |
5773 | |
5774 | // Check if we passed the minimum doca to the beam line |
5775 | beam_pos=beam_center+(newz-beam_z0)*beam_dir; |
5776 | r2=(DVector2(S(state_x),S(state_y))-beam_pos).Mod2(); |
5777 | |
5778 | if (r2>r2_old && newz<endplate_z){ |
5779 | double two_step=dz+dz_old; |
5780 | |
5781 | // Find the increment/decrement in z to get to the minimum doca to the |
5782 | // beam line |
5783 | if (BrentsAlgorithm(newz,0.5*two_step,dEdx,0.,beam_center,beam_dir,S,dz)!=NOERROR){ |
5784 | return UNRECOVERABLE_ERROR; |
5785 | } |
5786 | // update internal variables |
5787 | x_=S(state_x); |
5788 | y_=S(state_y); |
5789 | z_=newz+dz; |
5790 | |
5791 | return NOERROR; |
5792 | } |
5793 | r2_old=r2; |
5794 | dz_old=dz; |
5795 | z=newz; |
5796 | } |
5797 | |
5798 | // If we extrapolate beyond the fiducial volume of the detector before |
5799 | // finding the doca, abandon the extrapolation... |
5800 | if (newz<Z_MIN-100.){ |
5801 | //_DBG_ << "Extrapolated z = " << newz << endl; |
5802 | // Restore old state vector |
5803 | S=S1; |
5804 | return VALUE_OUT_OF_RANGE; |
5805 | } |
5806 | |
5807 | // update internal variables |
5808 | x_=S(state_x); |
5809 | y_=S(state_y); |
5810 | z_=newz; |
5811 | |
5812 | |
5813 | return NOERROR; |
5814 | } |
5815 | |
5816 | |
5817 | |
5818 | |
5819 | // Propagate track to point of distance of closest approach to origin |
5820 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DVector2 &xy, |
5821 | DMatrix5x1 &Sc,DMatrix5x5 &Cc){ |
5822 | DMatrix5x5 Jc=I5x5; //Jacobian matrix |
5823 | DMatrix5x5 Q; // multiple scattering matrix |
5824 | |
5825 | // Position and step variables |
5826 | DVector2 beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
5827 | double r2=(xy-beam_pos).Mod2(); |
5828 | double ds=-mStepSizeS; // step along path in cm |
5829 | double r2_old=r2; |
5830 | |
5831 | // Energy loss |
5832 | double dedx=0.; |
5833 | |
5834 | // Check direction of propagation |
5835 | DMatrix5x1 S0; |
5836 | S0=Sc; |
5837 | DVector2 xy0=xy; |
5838 | DVector2 xy1=xy; |
5839 | Step(xy0,ds,S0,dedx); |
5840 | // Bail if the transverse momentum has dropped below some minimum |
5841 | if (fabs(S0(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
5842 | if (DEBUG_LEVEL>2) |
5843 | { |
5844 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5844<<" " << "Bailing: PT = " << 1./fabs(S0(state_q_over_pt)) |
5845 | << endl; |
5846 | } |
5847 | return UNRECOVERABLE_ERROR; |
5848 | } |
5849 | beam_pos=beam_center+(S0(state_z)-beam_z0)*beam_dir; |
5850 | r2=(xy0-beam_pos).Mod2(); |
5851 | if (r2>r2_old) ds*=-1.; |
5852 | double ds_old=ds; |
5853 | |
5854 | // if (fit_type==kTimeBased)printf("------Extrapolating\n"); |
5855 | |
5856 | if (central_traj.size()==0){ |
5857 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5857<<" " << "Central trajectory size==0!" << endl; |
5858 | return UNRECOVERABLE_ERROR; |
5859 | } |
5860 | |
5861 | // D is now on the actual trajectory itself |
5862 | Sc(state_D)=0.; |
5863 | |
5864 | // Track propagation loop |
5865 | while (Sc(state_z)>Z_MIN-100. && Sc(state_z)<Z_MAX370.0 |
5866 | && r2<R2_MAX4225.0){ |
5867 | // Bail if the transverse momentum has dropped below some minimum |
5868 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
5869 | if (DEBUG_LEVEL>2) |
5870 | { |
5871 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5871<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt)) |
5872 | << endl; |
5873 | } |
5874 | return UNRECOVERABLE_ERROR; |
5875 | } |
5876 | |
5877 | // get material properties from the Root Geometry |
5878 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
5879 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5880 | DVector3 pos3d(xy.X(),xy.Y(),Sc(state_z)); |
5881 | if (geom->FindMatKalman(pos3d,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5882 | chi2c_factor,chi2a_factor,chi2a_corr, |
5883 | last_material_map) |
5884 | !=NOERROR){ |
5885 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5885<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5886 | break; |
5887 | } |
5888 | |
5889 | // Get dEdx for the upcoming step |
5890 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
5891 | if (CORRECT_FOR_ELOSS){ |
5892 | dedx=-GetdEdx(q_over_p,K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
5893 | } |
5894 | // Adjust the step size |
5895 | double sign=(ds>0.0)?1.:-1.; |
5896 | if (fabs(dedx)>EPS3.0e-8){ |
5897 | ds=sign*DE_PER_STEP0.001/fabs(dedx); |
5898 | } |
5899 | if(fabs(ds)>mStepSizeS) ds=sign*mStepSizeS; |
5900 | if(fabs(ds)<MIN_STEP_SIZE0.1)ds=sign*MIN_STEP_SIZE0.1; |
5901 | |
5902 | // Multiple scattering |
5903 | GetProcessNoiseCentral(ds,chi2c_factor,chi2a_factor,chi2a_corr,Sc,Q); |
5904 | |
5905 | if (CORRECT_FOR_ELOSS){ |
5906 | double q_over_p_sq=q_over_p*q_over_p; |
5907 | double one_over_beta2=1.+mass2*q_over_p*q_over_p; |
5908 | double varE=GetEnergyVariance(ds,one_over_beta2,K_rho_Z_over_A); |
5909 | Q(state_q_over_pt,state_q_over_pt) |
5910 | +=varE*Sc(state_q_over_pt)*Sc(state_q_over_pt)*one_over_beta2 |
5911 | *q_over_p_sq; |
5912 | } |
5913 | |
5914 | // Propagate the state and covariance through the field |
5915 | S0=Sc; |
5916 | DVector2 old_xy=xy; |
5917 | StepStateAndCovariance(xy,ds,dedx,Sc,Jc,Cc); |
5918 | |
5919 | // Add contribution due to multiple scattering |
5920 | Cc=(sign*Q).AddSym(Cc); |
5921 | |
5922 | beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
5923 | r2=(xy-beam_pos).Mod2(); |
5924 | //printf("r %f r_old %f \n",sqrt(r2),sqrt(r2_old)); |
5925 | if (r2>r2_old) { |
5926 | // We've passed the true minimum; backtrack to find the "vertex" |
5927 | // position |
5928 | double my_ds=0.; |
5929 | if (BrentsAlgorithm(ds,ds_old,dedx,xy,0.,beam_center,beam_dir,Sc,my_ds)!=NOERROR){ |
5930 | //_DBG_ <<endl; |
5931 | return UNRECOVERABLE_ERROR; |
5932 | } |
5933 | //printf ("Brent min r %f\n",xy.Mod()); |
5934 | |
5935 | // Find the field and gradient at (old_x,old_y,old_z) |
5936 | bfield->GetFieldAndGradient(old_xy.X(),old_xy.Y(),S0(state_z),Bx,By,Bz, |
5937 | dBxdx,dBxdy,dBxdz,dBydx, |
5938 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
5939 | |
5940 | // Compute the Jacobian matrix |
5941 | my_ds-=ds_old; |
5942 | StepJacobian(old_xy,xy-old_xy,my_ds,S0,dedx,Jc); |
5943 | |
5944 | // Propagate the covariance matrix |
5945 | //Cc=Jc*Cc*Jc.Transpose()+(my_ds/ds_old)*Q; |
5946 | //Cc=((my_ds/ds_old)*Q).AddSym(Cc.SandwichMultiply(Jc)); |
5947 | Cc=((my_ds/ds_old)*Q).AddSym(Jc*Cc*Jc.Transpose()); |
5948 | |
5949 | break; |
5950 | } |
5951 | r2_old=r2; |
5952 | ds_old=ds; |
5953 | } |
5954 | |
5955 | return NOERROR; |
5956 | } |
5957 | |
5958 | // Propagate track to point of distance of closest approach to origin |
5959 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DVector2 &xy, |
5960 | DMatrix5x1 &Sc){ |
5961 | // Save un-extroplated quantities |
5962 | DMatrix5x1 S0(Sc); |
5963 | DVector2 xy0(xy); |
5964 | |
5965 | // Initialize the beam position = center of target, and the direction |
5966 | DVector2 origin; |
5967 | DVector2 dir; |
5968 | |
5969 | // Position and step variables |
5970 | DVector2 beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
5971 | double r2=(xy-beam_pos).Mod2(); |
5972 | double ds=-mStepSizeS; // step along path in cm |
5973 | double r2_old=r2; |
5974 | |
5975 | // Energy loss |
5976 | double dedx=0.; |
5977 | |
5978 | // Check direction of propagation |
5979 | DMatrix5x1 S1=Sc; |
5980 | DVector2 xy1=xy; |
5981 | Step(xy1,ds,S1,dedx); |
5982 | beam_pos=beam_center+(S1(state_z)-beam_z0)*beam_dir; |
5983 | r2=(xy1-beam_pos).Mod2(); |
5984 | if (r2>r2_old) ds*=-1.; |
5985 | double ds_old=ds; |
5986 | |
5987 | // Track propagation loop |
5988 | while (Sc(state_z)>Z_MIN-100. && Sc(state_z)<Z_MAX370.0 |
5989 | && r2<R2_MAX4225.0){ |
5990 | // get material properties from the Root Geometry |
5991 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0; |
5992 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
5993 | DVector3 pos3d(xy.X(),xy.Y(),Sc(state_z)); |
5994 | if (geom->FindMatKalman(pos3d,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
5995 | chi2c_factor,chi2a_factor,chi2a_corr, |
5996 | last_material_map) |
5997 | !=NOERROR){ |
5998 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<5998<<" " << "Material error in ExtrapolateToVertex! " << endl; |
5999 | break; |
6000 | } |
6001 | |
6002 | // Get dEdx for the upcoming step |
6003 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
6004 | if (CORRECT_FOR_ELOSS){ |
6005 | dedx=GetdEdx(q_over_p,K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
6006 | } |
6007 | // Adjust the step size |
6008 | double sign=(ds>0.0)?1.:-1.; |
6009 | if (fabs(dedx)>EPS3.0e-8){ |
6010 | ds=sign*DE_PER_STEP0.001/fabs(dedx); |
6011 | } |
6012 | if(fabs(ds)>mStepSizeS) ds=sign*mStepSizeS; |
6013 | if(fabs(ds)<MIN_STEP_SIZE0.1)ds=sign*MIN_STEP_SIZE0.1; |
6014 | |
6015 | // Propagate the state through the field |
6016 | Step(xy,ds,Sc,dedx); |
6017 | |
6018 | beam_pos=beam_center+(Sc(state_z)-beam_z0)*beam_dir; |
6019 | r2=(xy-beam_pos).Mod2(); |
6020 | //printf("r %f r_old %f \n",r,r_old); |
6021 | if (r2>r2_old) { |
6022 | // We've passed the true minimum; backtrack to find the "vertex" |
6023 | // position |
6024 | double my_ds=0.; |
6025 | BrentsAlgorithm(ds,ds_old,dedx,xy,0.,beam_center,beam_dir,Sc,my_ds); |
6026 | //printf ("Brent min r %f\n",pos.Perp()); |
6027 | break; |
6028 | } |
6029 | r2_old=r2; |
6030 | ds_old=ds; |
6031 | } |
6032 | |
6033 | // If we extrapolate beyond the fiducial volume of the detector before |
6034 | // finding the doca, abandon the extrapolation... |
6035 | if (Sc(state_z)<Z_MIN-100.){ |
6036 | //_DBG_ << "Extrapolated z = " << Sc(state_z) << endl; |
6037 | // Restore un-extrapolated values |
6038 | Sc=S0; |
6039 | xy=xy0; |
6040 | return VALUE_OUT_OF_RANGE; |
6041 | } |
6042 | |
6043 | return NOERROR; |
6044 | } |
6045 | |
6046 | |
6047 | |
6048 | |
6049 | // Transform the 5x5 tracking error matrix into a 7x7 error matrix in cartesian |
6050 | // coordinates |
6051 | shared_ptr<TMatrixFSym> DTrackFitterKalmanSIMD::Get7x7ErrorMatrixForward(DMatrixDSym C){ |
6052 | auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); |
6053 | C7x7->ResizeTo(7, 7); |
6054 | DMatrix J(7,5); |
6055 | |
6056 | double p=1./fabs(q_over_p_); |
6057 | double tanl=1./sqrt(tx_*tx_+ty_*ty_); |
6058 | double tanl2=tanl*tanl; |
6059 | double lambda=atan(tanl); |
6060 | double sinl=sin(lambda); |
6061 | double sinl3=sinl*sinl*sinl; |
6062 | |
6063 | J(state_X,state_x)=J(state_Y,state_y)=1.; |
6064 | J(state_Pz,state_ty)=-p*ty_*sinl3; |
6065 | J(state_Pz,state_tx)=-p*tx_*sinl3; |
6066 | J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3; |
6067 | J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3; |
6068 | J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3; |
6069 | J(state_Pz,state_q_over_p)=-p*sinl/q_over_p_; |
6070 | J(state_Px,state_q_over_p)=tx_*J(state_Pz,state_q_over_p); |
6071 | J(state_Py,state_q_over_p)=ty_*J(state_Pz,state_q_over_p); |
6072 | J(state_Z,state_x)=-tx_*tanl2; |
6073 | J(state_Z,state_y)=-ty_*tanl2; |
6074 | double diff=tx_*tx_-ty_*ty_; |
6075 | double frac=tanl2*tanl2; |
6076 | J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac; |
6077 | J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac; |
6078 | |
6079 | // C'= JCJ^T |
6080 | *C7x7=C.Similarity(J); |
6081 | |
6082 | return C7x7; |
6083 | } |
6084 | |
6085 | |
6086 | |
6087 | // Transform the 5x5 tracking error matrix into a 7x7 error matrix in cartesian |
6088 | // coordinates |
6089 | shared_ptr<TMatrixFSym> DTrackFitterKalmanSIMD::Get7x7ErrorMatrix(DMatrixDSym C){ |
6090 | auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); |
6091 | C7x7->ResizeTo(7, 7); |
6092 | DMatrix J(7,5); |
6093 | //double cosl=cos(atan(tanl_)); |
6094 | double pt=1./fabs(q_over_pt_); |
6095 | //double p=pt/cosl; |
6096 | // double p_sq=p*p; |
6097 | // double E=sqrt(mass2+p_sq); |
6098 | double pt_sq=1./(q_over_pt_*q_over_pt_); |
6099 | double cosphi=cos(phi_); |
6100 | double sinphi=sin(phi_); |
6101 | double q=(q_over_pt_>0.0)?1.:-1.; |
6102 | |
6103 | J(state_Px,state_q_over_pt)=-q*pt_sq*cosphi; |
6104 | J(state_Px,state_phi)=-pt*sinphi; |
6105 | |
6106 | J(state_Py,state_q_over_pt)=-q*pt_sq*sinphi; |
6107 | J(state_Py,state_phi)=pt*cosphi; |
6108 | |
6109 | J(state_Pz,state_q_over_pt)=-q*pt_sq*tanl_; |
6110 | J(state_Pz,state_tanl)=pt; |
6111 | |
6112 | J(state_X,state_phi)=-D_*cosphi; |
6113 | J(state_X,state_D)=-sinphi; |
6114 | |
6115 | J(state_Y,state_phi)=-D_*sinphi; |
6116 | J(state_Y,state_D)=cosphi; |
6117 | |
6118 | J(state_Z,state_z)=1.; |
6119 | |
6120 | // C'= JCJ^T |
6121 | *C7x7=C.Similarity(J); |
6122 | // C7x7->Print(); |
6123 | // _DBG_ << " " << C7x7->operator()(3,3) << " " << C7x7->operator()(4,4) << endl; |
6124 | |
6125 | return C7x7; |
6126 | } |
6127 | |
6128 | // Track recovery for Central tracks |
6129 | //----------------------------------- |
6130 | // This code attempts to recover tracks that are "broken". Sometimes the fit fails because too many hits were pruned |
6131 | // such that the number of surviving hits is less than the minimum number of degrees of freedom for a five-parameter fit. |
6132 | // 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 |
6133 | // be valid (i.e., make sense in terms of the number of degrees of freedom for the number of parameters (5) we are trying |
6134 | // to determine), we throw away a number of hits near the target because the projected trajectory deviates too far away from |
6135 | // the actual hits. This may be an indication of a kink in the trajectory. This condition is flagged as BREAK_POINT_FOUND. |
6136 | // Sometimes the fit may succeed and no break point is found, but the pruning threw out a large number of hits. This |
6137 | // condition is flagged as PRUNED_TOO_MANY_HITS. The recovery code proceeds by truncating the reference trajectory to |
6138 | // to a point just after the hit where a break point was found and all hits are ignored beyond this point subject to a |
6139 | // minimum number of hits set by MIN_HITS_FOR_REFIT. The recovery code always attempts to use the hits closest to the |
6140 | // target. The code is allowed to iterate; with each iteration the trajectory and list of useable hits is further truncated. |
6141 | kalman_error_t DTrackFitterKalmanSIMD::RecoverBrokenTracks(double anneal_factor, |
6142 | DMatrix5x1 &S, |
6143 | DMatrix5x5 &C, |
6144 | const DMatrix5x5 &C0, |
6145 | DVector2 &xy, |
6146 | double &chisq, |
6147 | unsigned int &numdof){ |
6148 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6148<<" " << "Attempting to recover broken track ... " <<endl; |
6149 | |
6150 | // Initialize degrees of freedom and chi^2 |
6151 | double refit_chisq=-1.; |
6152 | unsigned int refit_ndf=0; |
6153 | // State vector and covariance matrix |
6154 | DMatrix5x5 C1; |
6155 | DMatrix5x1 S1; |
6156 | // Position vector |
6157 | DVector2 refit_xy; |
6158 | |
6159 | // save the status of the hits used in the fit |
6160 | unsigned int num_hits=cdc_used_in_fit.size(); |
6161 | vector<bool>old_cdc_used_status(num_hits); |
6162 | for (unsigned int j=0;j<num_hits;j++){ |
6163 | old_cdc_used_status[j]=cdc_used_in_fit[j]; |
6164 | } |
6165 | |
6166 | // Truncate the reference trajectory to just beyond the break point (or the minimum number of hits) |
6167 | unsigned int min_cdc_index_for_refit=MIN_HITS_FOR_REFIT-1; |
6168 | //if (break_point_cdc_index<num_hits/2) |
6169 | // break_point_cdc_index=num_hits/2; |
6170 | if (break_point_cdc_index<min_cdc_index_for_refit){ |
6171 | break_point_cdc_index=min_cdc_index_for_refit; |
6172 | } |
6173 | // Next determine where we need to truncate the trajectory |
6174 | DVector2 origin=my_cdchits[break_point_cdc_index]->origin; |
6175 | DVector2 dir=my_cdchits[break_point_cdc_index]->dir; |
6176 | double z0=my_cdchits[break_point_cdc_index]->z0wire; |
6177 | unsigned int k=0; |
6178 | for (k=central_traj.size()-1;k>1;k--){ |
6179 | double r2=central_traj[k].xy.Mod2(); |
6180 | double r2next=central_traj[k-1].xy.Mod2(); |
6181 | double r2_cdc=(origin+(central_traj[k].S(state_z)-z0)*dir).Mod2(); |
6182 | if (r2next>r2 && r2>r2_cdc) break; |
6183 | } |
6184 | break_point_step_index=k; |
6185 | |
6186 | if (break_point_step_index==central_traj.size()-1){ |
6187 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6187<<" " << "Invalid reference trajectory in track recovery..." << endl; |
6188 | return FIT_FAILED; |
6189 | } |
6190 | |
6191 | kalman_error_t refit_error=FIT_NOT_DONE; |
6192 | unsigned int old_cdc_index=break_point_cdc_index; |
6193 | unsigned int old_step_index=break_point_step_index; |
6194 | unsigned int refit_iter=0; |
6195 | unsigned int num_cdchits=my_cdchits.size(); |
6196 | while (break_point_cdc_index>4 && break_point_step_index>0 |
6197 | && break_point_step_index<central_traj.size()){ |
6198 | refit_iter++; |
6199 | |
6200 | // Flag the cdc hits within the radius of the break point cdc index |
6201 | // as good, the rest as bad. |
6202 | for (unsigned int j=0;j<=break_point_cdc_index;j++){ |
6203 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6204 | } |
6205 | for (unsigned int j=break_point_cdc_index+1;j<num_cdchits;j++){ |
6206 | my_cdchits[j]->status=bad_hit; |
6207 | } |
6208 | |
6209 | // Now refit with the truncated trajectory and list of hits |
6210 | //C1=C0; |
6211 | //C1=4.0*C0; |
6212 | C1=1.0*C0; |
6213 | S1=central_traj[break_point_step_index].S; |
6214 | refit_chisq=-1.; |
6215 | refit_ndf=0; |
6216 | refit_error=KalmanCentral(anneal_factor,S1,C1,refit_xy, |
6217 | refit_chisq,refit_ndf); |
6218 | if (refit_error==FIT_SUCCEEDED |
6219 | || (refit_error==BREAK_POINT_FOUND |
6220 | && break_point_cdc_index==1 |
6221 | ) |
6222 | //|| refit_error==PRUNED_TOO_MANY_HITS |
6223 | ){ |
6224 | C=C1; |
6225 | S=S1; |
6226 | xy=refit_xy; |
6227 | chisq=refit_chisq; |
6228 | numdof=refit_ndf; |
6229 | |
6230 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6230<<" " << "Fit recovery succeeded..." << endl; |
6231 | return FIT_SUCCEEDED; |
6232 | } |
6233 | |
6234 | break_point_cdc_index=old_cdc_index-refit_iter; |
6235 | break_point_step_index=old_step_index-refit_iter; |
6236 | } |
6237 | |
6238 | // If the refit did not work, restore the old list hits used in the fit |
6239 | // before the track recovery was attempted. |
6240 | for (unsigned int k=0;k<num_hits;k++){ |
6241 | cdc_used_in_fit[k]=old_cdc_used_status[k]; |
6242 | } |
6243 | |
6244 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6244<<" " << "Fit recovery failed..." << endl; |
6245 | return FIT_FAILED; |
6246 | } |
6247 | |
6248 | // Track recovery for forward tracks |
6249 | //----------------------------------- |
6250 | // This code attempts to recover tracks that are "broken". Sometimes the fit fails because too many hits were pruned |
6251 | // such that the number of surviving hits is less than the minimum number of degrees of freedom for a five-parameter fit. |
6252 | // 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 |
6253 | // be valid (i.e., make sense in terms of the number of degrees of freedom for the number of parameters (5) we are trying |
6254 | // to determine), we throw away a number of hits near the target because the projected trajectory deviates too far away from |
6255 | // the actual hits. This may be an indication of a kink in the trajectory. This condition is flagged as BREAK_POINT_FOUND. |
6256 | // Sometimes the fit may succeed and no break point is found, but the pruning threw out a large number of hits. This |
6257 | // condition is flagged as PRUNED_TOO_MANY_HITS. The recovery code proceeds by truncating the reference trajectory to |
6258 | // to a point just after the hit where a break point was found and all hits are ignored beyond this point subject to a |
6259 | // minimum number of hits. The recovery code always attempts to use the hits closest to the target. The code is allowed to |
6260 | // iterate; with each iteration the trajectory and list of useable hits is further truncated. |
6261 | kalman_error_t DTrackFitterKalmanSIMD::RecoverBrokenTracks(double anneal_factor, |
6262 | DMatrix5x1 &S, |
6263 | DMatrix5x5 &C, |
6264 | const DMatrix5x5 &C0, |
6265 | double &chisq, |
6266 | unsigned int &numdof |
6267 | ){ |
6268 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6268<<" " << "Attempting to recover broken track ... " <<endl; |
6269 | |
6270 | unsigned int num_cdchits=my_cdchits.size(); |
6271 | unsigned int num_fdchits=my_fdchits.size(); |
6272 | |
6273 | // Initialize degrees of freedom and chi^2 |
6274 | double refit_chisq=-1.; |
6275 | unsigned int refit_ndf=0; |
6276 | // State vector and covariance matrix |
6277 | DMatrix5x5 C1; |
6278 | DMatrix5x1 S1; |
6279 | |
6280 | // save the status of the hits used in the fit |
6281 | vector<bool>old_cdc_used_status(num_cdchits); |
6282 | vector<bool>old_fdc_used_status(num_fdchits); |
6283 | for (unsigned int j=0;j<num_fdchits;j++){ |
6284 | old_fdc_used_status[j]=fdc_used_in_fit[j]; |
6285 | } |
6286 | for (unsigned int j=0;j<num_cdchits;j++){ |
6287 | old_cdc_used_status[j]=cdc_used_in_fit[j]; |
6288 | } |
6289 | |
6290 | unsigned int min_cdc_index=MIN_HITS_FOR_REFIT-1; |
6291 | if (my_fdchits.size()>0){ |
6292 | if (break_point_cdc_index<5){ |
6293 | break_point_cdc_index=0; |
6294 | min_cdc_index=5; |
6295 | } |
6296 | } |
6297 | /*else{ |
6298 | unsigned int half_num_cdchits=num_cdchits/2; |
6299 | if (break_point_cdc_index<half_num_cdchits |
6300 | && half_num_cdchits>min_cdc_index) |
6301 | break_point_cdc_index=half_num_cdchits; |
6302 | } |
6303 | */ |
6304 | if (break_point_cdc_index<min_cdc_index){ |
6305 | break_point_cdc_index=min_cdc_index; |
6306 | } |
6307 | |
6308 | // Find the index at which to truncate the reference trajectory |
6309 | DVector2 origin=my_cdchits[break_point_cdc_index]->origin; |
6310 | DVector2 dir=my_cdchits[break_point_cdc_index]->dir; |
6311 | double z0=my_cdchits[break_point_cdc_index]->z0wire; |
6312 | unsigned int k=forward_traj.size()-1; |
6313 | for (;k>1;k--){ |
6314 | DMatrix5x1 S1=forward_traj[k].S; |
6315 | double x1=S1(state_x); |
6316 | double y1=S1(state_y); |
6317 | double r2=x1*x1+y1*y1; |
6318 | DMatrix5x1 S2=forward_traj[k-1].S; |
6319 | double x2=S2(state_x); |
6320 | double y2=S2(state_y); |
6321 | double r2next=x2*x2+y2*y2; |
6322 | double r2cdc=(origin+(forward_traj[k].z-z0)*dir).Mod2(); |
6323 | |
6324 | if (r2next>r2 && r2>r2cdc) break; |
6325 | } |
6326 | break_point_step_index=k; |
6327 | |
6328 | if (break_point_step_index==forward_traj.size()-1){ |
6329 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6329<<" " << "Invalid reference trajectory in track recovery..." << endl; |
6330 | return FIT_FAILED; |
6331 | } |
6332 | |
6333 | // Attemp to refit the track using the abreviated list of hits and the truncated |
6334 | // reference trajectory. Iterates if the fit fails. |
6335 | kalman_error_t refit_error=FIT_NOT_DONE; |
6336 | unsigned int old_cdc_index=break_point_cdc_index; |
6337 | unsigned int old_step_index=break_point_step_index; |
6338 | unsigned int refit_iter=0; |
6339 | while (break_point_cdc_index>4 && break_point_step_index>0 |
6340 | && break_point_step_index<forward_traj.size()){ |
6341 | refit_iter++; |
6342 | |
6343 | // Flag the cdc hits within the radius of the break point cdc index |
6344 | // as good, the rest as bad. |
6345 | for (unsigned int j=0;j<=break_point_cdc_index;j++){ |
6346 | if (my_cdchits[j]->status!=late_hit) my_cdchits[j]->status=good_hit; |
6347 | } |
6348 | for (unsigned int j=break_point_cdc_index+1;j<num_cdchits;j++){ |
6349 | my_cdchits[j]->status=bad_hit; |
6350 | } |
6351 | |
6352 | // Re-initialize the state vector, covariance, chisq and number of degrees of freedom |
6353 | //C1=4.0*C0; |
6354 | C1=1.0*C0; |
6355 | S1=forward_traj[break_point_step_index].S; |
6356 | refit_chisq=-1.; |
6357 | refit_ndf=0; |
6358 | // Now refit with the truncated trajectory and list of hits |
6359 | refit_error=KalmanForwardCDC(anneal_factor,S1,C1,refit_chisq,refit_ndf); |
6360 | if (refit_error==FIT_SUCCEEDED |
6361 | || (refit_error==BREAK_POINT_FOUND |
6362 | && break_point_cdc_index==1 |
6363 | ) |
6364 | //|| refit_error==PRUNED_TOO_MANY_HITS |
6365 | ){ |
6366 | C=C1; |
6367 | S=S1; |
6368 | chisq=refit_chisq; |
6369 | numdof=refit_ndf; |
6370 | return FIT_SUCCEEDED; |
6371 | } |
6372 | break_point_cdc_index=old_cdc_index-refit_iter; |
6373 | break_point_step_index=old_step_index-refit_iter; |
6374 | } |
6375 | // If the refit did not work, restore the old list hits used in the fit |
6376 | // before the track recovery was attempted. |
6377 | for (unsigned int k=0;k<num_cdchits;k++){ |
6378 | cdc_used_in_fit[k]=old_cdc_used_status[k]; |
6379 | } |
6380 | for (unsigned int k=0;k<num_fdchits;k++){ |
6381 | fdc_used_in_fit[k]=old_fdc_used_status[k]; |
6382 | } |
6383 | |
6384 | return FIT_FAILED; |
6385 | } |
6386 | |
6387 | |
6388 | // Track recovery for forward-going tracks with hits in the FDC |
6389 | kalman_error_t |
6390 | DTrackFitterKalmanSIMD::RecoverBrokenForwardTracks(double fdc_anneal, |
6391 | double cdc_anneal, |
6392 | DMatrix5x1 &S, |
6393 | DMatrix5x5 &C, |
6394 | const DMatrix5x5 &C0, |
6395 | double &chisq, |
6396 | unsigned int &numdof){ |
6397 | if (DEBUG_LEVEL>1) |
6398 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6398<<" " << "Attempting to recover broken track ... " <<endl; |
6399 | unsigned int num_cdchits=my_cdchits.size(); |
6400 | unsigned int num_fdchits=fdc_updates.size(); |
6401 | |
6402 | // Initialize degrees of freedom and chi^2 |
6403 | double refit_chisq=-1.; |
6404 | unsigned int refit_ndf=0; |
6405 | // State vector and covariance matrix |
6406 | DMatrix5x5 C1; |
6407 | DMatrix5x1 S1; |
6408 | |
6409 | // save the status of the hits used in the fit |
6410 | vector<int>old_cdc_used_status(num_cdchits); |
6411 | vector<int>old_fdc_used_status(num_fdchits); |
6412 | for (unsigned int j=0;j<num_fdchits;j++){ |
6413 | old_fdc_used_status[j]=fdc_used_in_fit[j]; |
6414 | } |
6415 | for (unsigned int j=0;j<num_cdchits;j++){ |
6416 | old_cdc_used_status[j]=cdc_used_in_fit[j]; |
6417 | } |
6418 | |
6419 | // Truncate the trajectory |
6420 | double zhit=my_fdchits[break_point_fdc_index]->z; |
6421 | unsigned int k=0; |
6422 | for (;k<forward_traj.size();k++){ |
6423 | double z=forward_traj[k].z; |
6424 | if (z<zhit) break; |
6425 | } |
6426 | for (unsigned int j=0;j<=break_point_fdc_index;j++){ |
6427 | my_fdchits[j]->status=good_hit; |
6428 | } |
6429 | for (unsigned int j=break_point_fdc_index+1;j<num_fdchits;j++){ |
6430 | my_fdchits[j]->status=bad_hit; |
6431 | } |
6432 | |
6433 | if (k==forward_traj.size()) return FIT_NOT_DONE; |
6434 | |
6435 | break_point_step_index=k; |
6436 | |
6437 | // Attemp to refit the track using the abreviated list of hits and the truncated |
6438 | // reference trajectory. |
6439 | kalman_error_t refit_error=FIT_NOT_DONE; |
6440 | int refit_iter=0; |
6441 | unsigned int break_id=break_point_fdc_index; |
6442 | while (break_id+num_cdchits>=MIN_HITS_FOR_REFIT && break_id>0 |
6443 | && break_point_step_index<forward_traj.size() |
6444 | && break_point_step_index>1 |
6445 | && refit_iter<10){ |
6446 | refit_iter++; |
6447 | |
6448 | // Mark the hits as bad if they are not included |
6449 | if (break_id >= 0){ |
6450 | for (unsigned int j=0;j<num_cdchits;j++){ |
6451 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6452 | } |
6453 | for (unsigned int j=0;j<=break_id;j++){ |
6454 | my_fdchits[j]->status=good_hit; |
6455 | } |
6456 | for (unsigned int j=break_id+1;j<num_fdchits;j++){ |
6457 | my_fdchits[j]->status=bad_hit; |
6458 | } |
6459 | } |
6460 | else{ |
6461 | // BreakID should always be 0 or positive, so this should never happen, but could be investigated in the future. |
6462 | for (unsigned int j=0;j<num_cdchits+break_id;j++){ |
6463 | if (my_cdchits[j]->status!=late_hit) my_cdchits[j]->status=good_hit; |
6464 | } |
6465 | for (unsigned int j=num_cdchits+break_id;j<num_cdchits;j++){ |
6466 | my_cdchits[j]->status=bad_hit; |
6467 | } |
6468 | for (unsigned int j=0;j<num_fdchits;j++){ |
6469 | my_fdchits[j]->status=bad_hit; |
6470 | } |
6471 | } |
6472 | |
6473 | // Re-initialize the state vector, covariance, chisq and number of degrees of freedom |
6474 | //C1=4.0*C0; |
6475 | C1=1.0*C0; |
6476 | S1=forward_traj[break_point_step_index].S; |
6477 | refit_chisq=-1.; |
6478 | refit_ndf=0; |
6479 | |
6480 | // Now refit with the truncated trajectory and list of hits |
6481 | refit_error=KalmanForward(fdc_anneal,cdc_anneal,S1,C1,refit_chisq,refit_ndf); |
6482 | if (refit_error==FIT_SUCCEEDED |
6483 | //|| (refit_error==PRUNED_TOO_MANY_HITS) |
6484 | ){ |
6485 | C=C1; |
6486 | S=S1; |
6487 | chisq=refit_chisq; |
6488 | numdof=refit_ndf; |
6489 | |
6490 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6490<<" " << "Refit succeeded" << endl; |
6491 | return FIT_SUCCEEDED; |
6492 | } |
6493 | // Truncate the trajectory |
6494 | if (break_id>0) break_id--; |
6495 | else break; |
6496 | zhit=my_fdchits[break_id]->z; |
6497 | k=0; |
6498 | for (;k<forward_traj.size();k++){ |
6499 | double z=forward_traj[k].z; |
6500 | if (z<zhit) break; |
6501 | } |
6502 | break_point_step_index=k; |
6503 | |
6504 | } |
6505 | |
6506 | // If the refit did not work, restore the old list hits used in the fit |
6507 | // before the track recovery was attempted. |
6508 | for (unsigned int k=0;k<num_cdchits;k++){ |
6509 | cdc_used_in_fit[k]=old_cdc_used_status[k]; |
6510 | } |
6511 | for (unsigned int k=0;k<num_fdchits;k++){ |
6512 | fdc_used_in_fit[k]=old_fdc_used_status[k]; |
6513 | } |
6514 | |
6515 | return FIT_FAILED; |
6516 | } |
6517 | |
6518 | |
6519 | |
6520 | // Routine to fit hits in the FDC and the CDC using the forward parametrization |
6521 | kalman_error_t DTrackFitterKalmanSIMD::ForwardFit(const DMatrix5x1 &S0,const DMatrix5x5 &C0){ |
6522 | unsigned int num_cdchits=my_cdchits.size(); |
6523 | unsigned int num_fdchits=my_fdchits.size(); |
6524 | unsigned int max_fdc_index=num_fdchits-1; |
6525 | |
6526 | // Vectors to keep track of updated state vectors and covariance matrices (after |
6527 | // adding the hit information) |
6528 | vector<bool>last_fdc_used_in_fit(num_fdchits); |
6529 | vector<bool>last_cdc_used_in_fit(num_cdchits); |
6530 | vector<pull_t>forward_pulls; |
6531 | vector<pull_t>last_forward_pulls; |
6532 | |
6533 | // Charge |
6534 | // double q=input_params.charge(); |
6535 | |
6536 | // Covariance matrix and state vector |
6537 | DMatrix5x5 C; |
6538 | DMatrix5x1 S=S0; |
6539 | |
6540 | // Create matrices to store results from previous iteration |
6541 | DMatrix5x1 Slast(S); |
6542 | DMatrix5x5 Clast(C0); |
6543 | // last z position |
6544 | double last_z=z_; |
6545 | |
6546 | double fdc_anneal=FORWARD_ANNEAL_SCALE+1.; // variable for scaling cut for hit pruning |
6547 | double cdc_anneal=ANNEAL_SCALE+1.; // variable for scaling cut for hit pruning |
6548 | |
6549 | // Chi-squared and degrees of freedom |
6550 | double chisq=-1.,chisq_forward=-1.; |
6551 | unsigned int my_ndf=0; |
6552 | unsigned int last_ndf=1; |
6553 | kalman_error_t error=FIT_NOT_DONE,last_error=FIT_NOT_DONE; |
6554 | |
6555 | // Iterate over reference trajectories |
6556 | for (int iter=0; |
6557 | iter<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20); |
6558 | iter++) { |
6559 | // These variables provide the approximate location along the trajectory |
6560 | // where there is an indication of a kink in the track |
6561 | break_point_fdc_index=max_fdc_index; |
6562 | break_point_cdc_index=0; |
6563 | break_point_step_index=0; |
6564 | |
6565 | // Reset material map index |
6566 | last_material_map=0; |
6567 | |
6568 | // Abort if momentum is too low |
6569 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX) break; |
6570 | |
6571 | // Initialize path length variable and flight time |
6572 | len=0; |
6573 | ftime=0.; |
6574 | var_ftime=0.; |
6575 | |
6576 | // Scale cut for pruning hits according to the iteration number |
6577 | fdc_anneal=(iter<MIN_ITER3)?(FORWARD_ANNEAL_SCALE/pow(FORWARD_ANNEAL_POW_CONST,iter)+1.):1.; |
6578 | cdc_anneal=(iter<MIN_ITER3)?(ANNEAL_SCALE/pow(ANNEAL_POW_CONST,iter)+1.):1.; |
6579 | |
6580 | // Swim through the field out to the most upstream FDC hit |
6581 | jerror_t ref_track_error=SetReferenceTrajectory(S); |
6582 | if (ref_track_error!=NOERROR){ |
6583 | if (iter==0) return FIT_FAILED; |
6584 | break; |
6585 | } |
6586 | |
6587 | // Reset the status of the cdc hits |
6588 | for (unsigned int j=0;j<num_cdchits;j++){ |
6589 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6590 | } |
6591 | |
6592 | // perform the kalman filter |
6593 | C=C0; |
6594 | bool did_second_refit=false; |
6595 | error=KalmanForward(fdc_anneal,cdc_anneal,S,C,chisq,my_ndf); |
6596 | if (DEBUG_LEVEL>1){ |
6597 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6597<<" " << "Iter: " << iter+1 << " Chi2=" << chisq << " Ndf=" << my_ndf << " Error code: " << error << endl; |
6598 | } |
6599 | // Try to recover tracks that failed the first attempt at fitting by |
6600 | // cutting outer hits |
6601 | if (error!=FIT_SUCCEEDED && RECOVER_BROKEN_TRACKS |
6602 | && num_fdchits>2 // some minimum to make this worthwhile... |
6603 | && break_point_fdc_index<num_fdchits |
6604 | && break_point_fdc_index+num_cdchits>=MIN_HITS_FOR_REFIT |
6605 | && forward_traj.size()>2*MIN_HITS_FOR_REFIT // avoid small track stubs |
6606 | ){ |
6607 | DMatrix5x5 Ctemp=C; |
6608 | DMatrix5x1 Stemp=S; |
6609 | unsigned int temp_ndf=my_ndf; |
6610 | double temp_chi2=chisq; |
6611 | double x=x_,y=y_,z=z_; |
6612 | |
6613 | kalman_error_t refit_error=RecoverBrokenForwardTracks(fdc_anneal, |
6614 | cdc_anneal, |
6615 | S,C,C0,chisq, |
6616 | my_ndf); |
6617 | if (fit_type==kTimeBased && refit_error!=FIT_SUCCEEDED){ |
6618 | fdc_anneal=1000.; |
6619 | cdc_anneal=1000.; |
6620 | refit_error=RecoverBrokenForwardTracks(fdc_anneal, |
6621 | cdc_anneal, |
6622 | S,C,C0,chisq, |
6623 | my_ndf); |
6624 | //chisq=1e6; |
6625 | did_second_refit=true; |
6626 | } |
6627 | if (refit_error!=FIT_SUCCEEDED){ |
6628 | if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){ |
6629 | C=Ctemp; |
6630 | S=Stemp; |
6631 | my_ndf=temp_ndf; |
6632 | chisq=temp_chi2; |
6633 | x_=x,y_=y,z_=z; |
6634 | |
6635 | if (num_cdchits<6) error=FIT_SUCCEEDED; |
6636 | } |
6637 | else error=FIT_FAILED; |
6638 | } |
6639 | else error=FIT_SUCCEEDED; |
6640 | } |
6641 | if ((error==POSITION_OUT_OF_RANGE || error==MOMENTUM_OUT_OF_RANGE) |
6642 | && iter==0){ |
6643 | return FIT_FAILED; |
6644 | } |
6645 | if (error==FIT_FAILED || error==INVALID_FIT || my_ndf==0){ |
6646 | if (iter==0) return FIT_FAILED; // first iteration failed |
6647 | break; |
6648 | } |
6649 | |
6650 | if (iter>MIN_ITER3 && did_second_refit==false){ |
6651 | double new_reduced_chisq=chisq/my_ndf; |
6652 | double old_reduced_chisq=chisq_forward/last_ndf; |
6653 | double new_prob=TMath::Prob(chisq,my_ndf); |
6654 | double old_prob=TMath::Prob(chisq_forward,last_ndf); |
6655 | if (new_prob<old_prob |
6656 | || fabs(new_reduced_chisq-old_reduced_chisq)<CHISQ_DELTA0.01){ |
6657 | break; |
6658 | } |
6659 | } |
6660 | |
6661 | chisq_forward=chisq; |
6662 | last_ndf=my_ndf; |
6663 | last_error=error; |
6664 | Slast=S; |
6665 | Clast=C; |
6666 | last_z=z_; |
6667 | |
6668 | last_fdc_used_in_fit=fdc_used_in_fit; |
6669 | last_cdc_used_in_fit=cdc_used_in_fit; |
6670 | } //iteration |
6671 | |
6672 | IsSmoothed=false; |
6673 | if(fit_type==kTimeBased){ |
6674 | forward_pulls.clear(); |
6675 | if (SmoothForward(forward_pulls) == NOERROR){ |
6676 | IsSmoothed = true; |
6677 | pulls.assign(forward_pulls.begin(),forward_pulls.end()); |
6678 | } |
6679 | } |
6680 | |
6681 | // total chisq and ndf |
6682 | chisq_=chisq_forward; |
6683 | ndf_=last_ndf; |
6684 | |
6685 | // output lists of hits used in the fit and fill pull vector |
6686 | cdchits_used_in_fit.clear(); |
6687 | for (unsigned int m=0;m<last_cdc_used_in_fit.size();m++){ |
6688 | if (last_cdc_used_in_fit[m]){ |
6689 | cdchits_used_in_fit.push_back(my_cdchits[m]->hit); |
6690 | } |
6691 | } |
6692 | fdchits_used_in_fit.clear(); |
6693 | for (unsigned int m=0;m<last_fdc_used_in_fit.size();m++){ |
6694 | if (last_fdc_used_in_fit[m] && my_fdchits[m]->hit!=NULL__null){ |
6695 | fdchits_used_in_fit.push_back(my_fdchits[m]->hit); |
6696 | } |
6697 | } |
6698 | // fill pull vector |
6699 | //pulls.assign(last_forward_pulls.begin(),last_forward_pulls.end()); |
6700 | |
6701 | // fill vector of extrapolations |
6702 | ClearExtrapolations(); |
6703 | if (forward_traj.size()>1){ |
6704 | ExtrapolateToInnerDetectors(); |
6705 | if (fit_type==kTimeBased){ |
6706 | double reverse_chisq=1e16,reverse_chisq_old=1e16; |
6707 | unsigned int reverse_ndf=0,reverse_ndf_old=0; |
6708 | |
6709 | // Run the Kalman filter in the reverse direction, to get the best guess |
6710 | // for the state vector at the last FDC point on the track |
6711 | DMatrix5x5 CReverse=C; |
6712 | DMatrix5x1 SReverse=S,SDownstream,SBest; |
6713 | kalman_error_t reverse_error=FIT_NOT_DONE; |
6714 | for (int iter=0;iter<20;iter++){ |
6715 | reverse_chisq_old=reverse_chisq; |
6716 | reverse_ndf_old=reverse_ndf; |
6717 | SBest=SDownstream; |
6718 | reverse_error=KalmanReverse(fdc_anneal,cdc_anneal,SReverse,CReverse, |
6719 | SDownstream,reverse_chisq,reverse_ndf); |
6720 | if (reverse_error!=FIT_SUCCEEDED) break; |
6721 | |
6722 | SReverse=SDownstream; |
6723 | for (unsigned int k=0;k<forward_traj.size()-1;k++){ |
6724 | // Get dEdx for the upcoming step |
6725 | double dEdx=0.; |
6726 | if (CORRECT_FOR_ELOSS){ |
6727 | dEdx=GetdEdx(SReverse(state_q_over_p), |
6728 | forward_traj[k].K_rho_Z_over_A, |
6729 | forward_traj[k].rho_Z_over_A, |
6730 | forward_traj[k].LnI,forward_traj[k].Z); |
6731 | } |
6732 | // Step through field |
6733 | DMatrix5x5 J; |
6734 | double z=forward_traj[k].z; |
6735 | double newz=forward_traj[k+1].z; |
6736 | StepJacobian(z,newz,SReverse,dEdx,J); |
6737 | Step(z,newz,dEdx,SReverse); |
6738 | |
6739 | CReverse=forward_traj[k].Q.AddSym(J*CReverse*J.Transpose()); |
6740 | } |
6741 | |
6742 | double reduced_chisq=reverse_chisq/double(reverse_ndf); |
6743 | double reduced_chisq_old=reverse_chisq_old/double(reverse_ndf_old); |
6744 | if (reduced_chisq>reduced_chisq_old |
6745 | || fabs(reduced_chisq-reduced_chisq_old)<0.01) break; |
6746 | } |
6747 | |
6748 | if (reverse_error!=FIT_SUCCEEDED){ |
6749 | ExtrapolateToOuterDetectors(forward_traj[0].S); |
6750 | } |
6751 | else{ |
6752 | ExtrapolateToOuterDetectors(SBest); |
6753 | } |
6754 | } |
6755 | else{ |
6756 | ExtrapolateToOuterDetectors(forward_traj[0].S); |
6757 | } |
6758 | if (extrapolations.at(SYS_BCAL).size()==1){ |
6759 | // There needs to be some steps inside the the volume of the BCAL for |
6760 | // the extrapolation to be useful. If this is not the case, clear |
6761 | // the extrolation vector. |
6762 | extrapolations[SYS_BCAL].clear(); |
6763 | } |
6764 | } |
6765 | // Extrapolate to the point of closest approach to the beam line |
6766 | z_=last_z; |
6767 | DVector2 beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
6768 | double dx=Slast(state_x)-beam_pos.X(); |
6769 | double dy=Slast(state_y)-beam_pos.Y(); |
6770 | bool extrapolated=false; |
6771 | if (sqrt(dx*dx+dy*dy)>EPS21.e-4){ |
6772 | DMatrix5x5 Ctemp=Clast; |
6773 | DMatrix5x1 Stemp=Slast; |
6774 | double ztemp=z_; |
6775 | if (ExtrapolateToVertex(Stemp,Ctemp)==NOERROR){ |
6776 | Clast=Ctemp; |
6777 | Slast=Stemp; |
6778 | extrapolated=true; |
6779 | } |
6780 | else{ |
6781 | //_DBG_ << endl; |
6782 | z_=ztemp; |
6783 | } |
6784 | } |
6785 | |
6786 | // Final momentum, positions and tangents |
6787 | x_=Slast(state_x), y_=Slast(state_y); |
6788 | tx_=Slast(state_tx),ty_=Slast(state_ty); |
6789 | q_over_p_=Slast(state_q_over_p); |
6790 | |
6791 | // Convert from forward rep. to central rep. |
6792 | double tsquare=tx_*tx_+ty_*ty_; |
6793 | tanl_=1./sqrt(tsquare); |
6794 | double cosl=cos(atan(tanl_)); |
6795 | q_over_pt_=q_over_p_/cosl; |
6796 | phi_=atan2(ty_,tx_); |
6797 | if (FORWARD_PARMS_COV==false){ |
6798 | if (extrapolated){ |
6799 | beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
6800 | dx=x_-beam_pos.X(); |
6801 | dy=y_-beam_pos.Y(); |
6802 | } |
6803 | D_=sqrt(dx*dx+dy*dy)+EPS3.0e-8; |
6804 | x_ = dx; y_ = dy; |
6805 | double cosphi=cos(phi_); |
6806 | double sinphi=sin(phi_); |
6807 | if ((dx>0.0 && sinphi>0.0) || (dy<0.0 && cosphi>0.0) |
6808 | || (dy>0.0 && cosphi<0.0) || (dx<0.0 && sinphi<0.0)) D_*=-1.; |
6809 | TransformCovariance(Clast); |
6810 | } |
6811 | // Covariance matrix |
6812 | vector<double>dummy; |
6813 | for (unsigned int i=0;i<5;i++){ |
6814 | dummy.clear(); |
6815 | for(unsigned int j=0;j<5;j++){ |
6816 | dummy.push_back(Clast(i,j)); |
6817 | } |
6818 | fcov.push_back(dummy); |
6819 | } |
6820 | |
6821 | return last_error; |
6822 | } |
6823 | |
6824 | // Routine to fit hits in the CDC using the forward parametrization |
6825 | kalman_error_t DTrackFitterKalmanSIMD::ForwardCDCFit(const DMatrix5x1 &S0,const DMatrix5x5 &C0){ |
6826 | unsigned int num_cdchits=my_cdchits.size(); |
6827 | unsigned int max_cdc_index=num_cdchits-1; |
6828 | unsigned int min_cdc_index_for_refit=MIN_HITS_FOR_REFIT-1; |
6829 | |
6830 | // Charge |
6831 | // double q=input_params.charge(); |
6832 | |
6833 | // Covariance matrix and state vector |
6834 | DMatrix5x5 C; |
6835 | DMatrix5x1 S=S0; |
6836 | |
6837 | // Create matrices to store results from previous iteration |
6838 | DMatrix5x1 Slast(S); |
6839 | DMatrix5x5 Clast(C0); |
6840 | |
6841 | // Vectors to keep track of updated state vectors and covariance matrices (after |
6842 | // adding the hit information) |
6843 | vector<pull_t>cdc_pulls; |
6844 | vector<pull_t>last_cdc_pulls; |
6845 | vector<bool>last_cdc_used_in_fit; |
6846 | |
6847 | double anneal_factor=ANNEAL_SCALE+1.; |
6848 | kalman_error_t error=FIT_NOT_DONE,last_error=FIT_NOT_DONE; |
6849 | |
6850 | // Chi-squared and degrees of freedom |
6851 | double chisq=-1.,chisq_forward=-1.; |
6852 | unsigned int my_ndf=0; |
6853 | unsigned int last_ndf=1; |
6854 | // last z position |
6855 | double zlast=0.; |
6856 | // unsigned int last_break_point_index=0,last_break_point_step_index=0; |
6857 | |
6858 | // Iterate over reference trajectories |
6859 | for (int iter2=0; |
6860 | iter2<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20); |
6861 | iter2++){ |
6862 | if (DEBUG_LEVEL>1){ |
6863 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6863<<" " <<"-------- iteration " << iter2+1 << " -----------" <<endl; |
6864 | } |
6865 | |
6866 | // These variables provide the approximate location along the trajectory |
6867 | // where there is an indication of a kink in the track |
6868 | break_point_cdc_index=max_cdc_index; |
6869 | break_point_step_index=0; |
6870 | |
6871 | // Reset material map index |
6872 | last_material_map=0; |
6873 | |
6874 | // Abort if momentum is too low |
6875 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
6876 | //printf("Too low momentum? %f\n",1/S(state_q_over_p)); |
6877 | break; |
6878 | } |
6879 | |
6880 | // Scale cut for pruning hits according to the iteration number |
6881 | anneal_factor=(iter2<MIN_ITER3)?(ANNEAL_SCALE/pow(ANNEAL_POW_CONST,iter2)+1.):1.; |
6882 | |
6883 | // Initialize path length variable and flight time |
6884 | len=0; |
6885 | ftime=0.; |
6886 | var_ftime=0.; |
6887 | |
6888 | // Swim to create the reference trajectory |
6889 | jerror_t ref_track_error=SetCDCForwardReferenceTrajectory(S); |
6890 | if (ref_track_error!=NOERROR){ |
6891 | if (iter2==0) return FIT_FAILED; |
6892 | break; |
6893 | } |
6894 | |
6895 | // Reset the status of the cdc hits |
6896 | for (unsigned int j=0;j<num_cdchits;j++){ |
6897 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
6898 | } |
6899 | |
6900 | // perform the filter |
6901 | C=C0; |
6902 | bool did_second_refit=false; |
6903 | error=KalmanForwardCDC(anneal_factor,S,C,chisq,my_ndf); |
6904 | |
6905 | // Try to recover tracks that failed the first attempt at fitting by |
6906 | // cutting outer hits |
6907 | if (error!=FIT_SUCCEEDED && RECOVER_BROKEN_TRACKS |
6908 | && num_cdchits>=MIN_HITS_FOR_REFIT){ |
6909 | DMatrix5x5 Ctemp=C; |
6910 | DMatrix5x1 Stemp=S; |
6911 | unsigned int temp_ndf=my_ndf; |
6912 | double temp_chi2=chisq; |
6913 | double x=x_,y=y_,z=z_; |
6914 | |
6915 | if (error==MOMENTUM_OUT_OF_RANGE){ |
6916 | //_DBG_ << "Momentum out of range" <<endl; |
6917 | unsigned int new_index=(3*max_cdc_index)/4; |
6918 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
6919 | } |
6920 | |
6921 | if (error==BROKEN_COVARIANCE_MATRIX){ |
6922 | break_point_cdc_index=min_cdc_index_for_refit; |
6923 | //_DBG_ << "Bad Cov" <<endl; |
6924 | } |
6925 | if (error==POSITION_OUT_OF_RANGE){ |
6926 | //_DBG_ << "Bad position" << endl; |
6927 | unsigned int new_index=(max_cdc_index)/2; |
6928 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
6929 | } |
6930 | if (error==PRUNED_TOO_MANY_HITS){ |
6931 | // _DBG_ << "Prune" << endl; |
6932 | unsigned int new_index=(3*max_cdc_index)/4; |
6933 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
6934 | // anneal_factor*=10.; |
6935 | } |
6936 | |
6937 | kalman_error_t refit_error=RecoverBrokenTracks(anneal_factor,S,C,C0,chisq,my_ndf); |
6938 | if (fit_type==kTimeBased && refit_error!=FIT_SUCCEEDED){ |
6939 | anneal_factor=1000.; |
6940 | refit_error=RecoverBrokenTracks(anneal_factor,S,C,C0,chisq,my_ndf); |
6941 | //chisq=1e6; |
6942 | did_second_refit=true; |
6943 | } |
6944 | |
6945 | if (refit_error!=FIT_SUCCEEDED){ |
6946 | if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){ |
6947 | C=Ctemp; |
6948 | S=Stemp; |
6949 | my_ndf=temp_ndf; |
6950 | chisq=temp_chi2; |
6951 | x_=x,y_=y,z_=z; |
6952 | |
6953 | // error=FIT_SUCCEEDED; |
6954 | } |
6955 | else error=FIT_FAILED; |
6956 | } |
6957 | else error=FIT_SUCCEEDED; |
6958 | } |
6959 | if ((error==POSITION_OUT_OF_RANGE || error==MOMENTUM_OUT_OF_RANGE) |
6960 | && iter2==0){ |
6961 | return FIT_FAILED; |
6962 | } |
6963 | if (error==FIT_FAILED || error==INVALID_FIT || my_ndf==0){ |
6964 | if (iter2==0) return error; |
6965 | break; |
6966 | } |
6967 | |
6968 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<6968<<" " << "--> Chisq " << chisq << " NDF " |
6969 | << my_ndf |
6970 | << " Prob: " << TMath::Prob(chisq,my_ndf) |
6971 | << endl; |
6972 | |
6973 | if (iter2>MIN_ITER3 && did_second_refit==false){ |
6974 | double new_reduced_chisq=chisq/my_ndf; |
6975 | double old_reduced_chisq=chisq_forward/last_ndf; |
6976 | double new_prob=TMath::Prob(chisq,my_ndf); |
6977 | double old_prob=TMath::Prob(chisq_forward,last_ndf); |
6978 | if (new_prob<old_prob |
6979 | || fabs(new_reduced_chisq-old_reduced_chisq)<CHISQ_DELTA0.01) break; |
6980 | } |
6981 | |
6982 | chisq_forward=chisq; |
6983 | Slast=S; |
6984 | Clast=C; |
6985 | last_error=error; |
6986 | last_ndf=my_ndf; |
6987 | zlast=z_; |
6988 | |
6989 | last_cdc_used_in_fit=cdc_used_in_fit; |
6990 | } //iteration |
6991 | |
6992 | // Run the smoother |
6993 | IsSmoothed=false; |
6994 | if(fit_type==kTimeBased){ |
6995 | cdc_pulls.clear(); |
6996 | if (SmoothForwardCDC(cdc_pulls) == NOERROR){ |
6997 | IsSmoothed = true; |
6998 | pulls.assign(cdc_pulls.begin(),cdc_pulls.end()); |
6999 | } |
7000 | } |
7001 | |
7002 | // total chisq and ndf |
7003 | chisq_=chisq_forward; |
7004 | ndf_=last_ndf; |
7005 | |
7006 | // output lists of hits used in the fit and fill the pull vector |
7007 | cdchits_used_in_fit.clear(); |
7008 | for (unsigned int m=0;m<last_cdc_used_in_fit.size();m++){ |
7009 | if (last_cdc_used_in_fit[m]){ |
7010 | cdchits_used_in_fit.push_back(my_cdchits[m]->hit); |
7011 | } |
7012 | } |
7013 | // output pulls vector |
7014 | //pulls.assign(last_cdc_pulls.begin(),last_cdc_pulls.end()); |
7015 | |
7016 | // Fill extrapolation vector |
7017 | ClearExtrapolations(); |
7018 | if (forward_traj.size()>1){ |
7019 | if (fit_type==kWireBased){ |
7020 | ExtrapolateForwardToOtherDetectors(); |
7021 | } |
7022 | else{ |
7023 | ExtrapolateToInnerDetectors(); |
7024 | |
7025 | double reverse_chisq=1e16,reverse_chisq_old=1e16; |
7026 | unsigned int reverse_ndf=0,reverse_ndf_old=0; |
7027 | |
7028 | // Run the Kalman filter in the reverse direction, to get the best guess |
7029 | // for the state vector at the last FDC point on the track |
7030 | DMatrix5x5 CReverse=C; |
7031 | DMatrix5x1 SReverse=S,SDownstream,SBest; |
7032 | kalman_error_t reverse_error=FIT_NOT_DONE; |
7033 | for (int iter=0;iter<20;iter++){ |
7034 | reverse_chisq_old=reverse_chisq; |
7035 | reverse_ndf_old=reverse_ndf; |
7036 | SBest=SDownstream; |
7037 | reverse_error=KalmanReverse(0.,anneal_factor,SReverse,CReverse, |
7038 | SDownstream,reverse_chisq,reverse_ndf); |
7039 | if (reverse_error!=FIT_SUCCEEDED) break; |
7040 | |
7041 | SReverse=SDownstream; |
7042 | for (unsigned int k=0;k<forward_traj.size()-1;k++){ |
7043 | // Get dEdx for the upcoming step |
7044 | double dEdx=0.; |
7045 | if (CORRECT_FOR_ELOSS){ |
7046 | dEdx=GetdEdx(SReverse(state_q_over_p), |
7047 | forward_traj[k].K_rho_Z_over_A, |
7048 | forward_traj[k].rho_Z_over_A, |
7049 | forward_traj[k].LnI,forward_traj[k].Z); |
7050 | } |
7051 | // Step through field |
7052 | DMatrix5x5 J; |
7053 | double z=forward_traj[k].z; |
7054 | double newz=forward_traj[k+1].z; |
7055 | StepJacobian(z,newz,SReverse,dEdx,J); |
7056 | Step(z,newz,dEdx,SReverse); |
7057 | |
7058 | CReverse=forward_traj[k].Q.AddSym(J*CReverse*J.Transpose()); |
7059 | } |
7060 | |
7061 | double reduced_chisq=reverse_chisq/double(reverse_ndf); |
7062 | double reduced_chisq_old=reverse_chisq_old/double(reverse_ndf_old); |
7063 | if (reduced_chisq>reduced_chisq_old |
7064 | || fabs(reduced_chisq-reduced_chisq_old)<0.01) break; |
7065 | } |
7066 | if (reverse_error!=FIT_SUCCEEDED){ |
7067 | ExtrapolateToOuterDetectors(forward_traj[0].S); |
7068 | } |
7069 | else{ |
7070 | ExtrapolateToOuterDetectors(SBest); |
7071 | } |
7072 | } |
7073 | } |
7074 | if (extrapolations.at(SYS_BCAL).size()==1){ |
7075 | // There needs to be some steps inside the the volume of the BCAL for |
7076 | // the extrapolation to be useful. If this is not the case, clear |
7077 | // the extrolation vector. |
7078 | extrapolations[SYS_BCAL].clear(); |
7079 | } |
7080 | |
7081 | // Extrapolate to the point of closest approach to the beam line |
7082 | z_=zlast; |
7083 | DVector2 beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
7084 | double dx=Slast(state_x)-beam_pos.X(); |
7085 | double dy=Slast(state_y)-beam_pos.Y(); |
7086 | bool extrapolated=false; |
7087 | if (sqrt(dx*dx+dy*dy)>EPS21.e-4){ |
7088 | if (ExtrapolateToVertex(Slast,Clast)!=NOERROR) return EXTRAPOLATION_FAILED; |
7089 | extrapolated=true; |
7090 | } |
7091 | |
7092 | // Final momentum, positions and tangents |
7093 | x_=Slast(state_x), y_=Slast(state_y); |
7094 | tx_=Slast(state_tx),ty_=Slast(state_ty); |
7095 | q_over_p_=Slast(state_q_over_p); |
7096 | |
7097 | // Convert from forward rep. to central rep. |
7098 | double tsquare=tx_*tx_+ty_*ty_; |
7099 | tanl_=1./sqrt(tsquare); |
7100 | double cosl=cos(atan(tanl_)); |
7101 | q_over_pt_=q_over_p_/cosl; |
7102 | phi_=atan2(ty_,tx_); |
7103 | if (FORWARD_PARMS_COV==false){ |
7104 | if (extrapolated){ |
7105 | beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
7106 | dx=x_-beam_pos.X(); |
7107 | dy=y_-beam_pos.Y(); |
7108 | } |
7109 | D_=sqrt(dx*dx+dy*dy)+EPS3.0e-8; |
7110 | x_ = dx; y_ = dy; |
7111 | double cosphi=cos(phi_); |
7112 | double sinphi=sin(phi_); |
7113 | if ((dx>0.0 && sinphi>0.0) || (dy<0.0 && cosphi>0.0) |
7114 | || (dy>0.0 && cosphi<0.0) || (dx<0.0 && sinphi<0.0)) D_*=-1.; |
7115 | TransformCovariance(Clast); |
7116 | } |
7117 | // Covariance matrix |
7118 | vector<double>dummy; |
7119 | // ... forward parameterization |
7120 | fcov.clear(); |
7121 | for (unsigned int i=0;i<5;i++){ |
7122 | dummy.clear(); |
7123 | for(unsigned int j=0;j<5;j++){ |
7124 | dummy.push_back(Clast(i,j)); |
7125 | } |
7126 | fcov.push_back(dummy); |
7127 | } |
7128 | |
7129 | return last_error; |
7130 | } |
7131 | |
7132 | // Routine to fit hits in the CDC using the central parametrization |
7133 | kalman_error_t DTrackFitterKalmanSIMD::CentralFit(const DVector2 &startpos, |
7134 | const DMatrix5x1 &S0, |
7135 | const DMatrix5x5 &C0){ |
7136 | // Initial position in x and y |
7137 | DVector2 pos(startpos); |
7138 | |
7139 | // Charge |
7140 | // double q=input_params.charge(); |
7141 | |
7142 | // Covariance matrix and state vector |
7143 | DMatrix5x5 Cc; |
7144 | DMatrix5x1 Sc=S0; |
7145 | |
7146 | // Variables to store values from previous iterations |
7147 | DMatrix5x1 Sclast(Sc); |
7148 | DMatrix5x5 Cclast(C0); |
7149 | DVector2 last_pos=pos; |
7150 | |
7151 | unsigned int num_cdchits=my_cdchits.size(); |
7152 | unsigned int max_cdc_index=num_cdchits-1; |
7153 | unsigned int min_cdc_index_for_refit=MIN_HITS_FOR_REFIT-1; |
7154 | |
7155 | // Vectors to keep track of updated state vectors and covariance matrices (after |
7156 | // adding the hit information) |
7157 | vector<pull_t>last_cdc_pulls; |
7158 | vector<pull_t>cdc_pulls; |
7159 | vector<bool>last_cdc_used_in_fit(num_cdchits); |
7160 | |
7161 | double anneal_factor=ANNEAL_SCALE+1.; // variable for scaling cut for hit pruning |
7162 | |
7163 | //Initialization of chisq, ndf, and error status |
7164 | double chisq_iter=-1.,chisq=-1.; |
7165 | unsigned int my_ndf=0; |
7166 | ndf_=0.; |
7167 | unsigned int last_ndf=1; |
7168 | kalman_error_t error=FIT_NOT_DONE,last_error=FIT_NOT_DONE; |
7169 | |
7170 | // Iterate over reference trajectories |
7171 | int iter2=0; |
7172 | for (;iter2<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20); |
7173 | iter2++){ |
7174 | if (DEBUG_LEVEL>1){ |
7175 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7175<<" " <<"-------- iteration " << iter2+1 << " -----------" <<endl; |
7176 | } |
7177 | |
7178 | // These variables provide the approximate location along the trajectory |
7179 | // where there is an indication of a kink in the track |
7180 | break_point_cdc_index=max_cdc_index; |
7181 | break_point_step_index=0; |
7182 | |
7183 | // Reset material map index |
7184 | last_material_map=0; |
7185 | |
7186 | // Break out of loop if p is too small |
7187 | double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl))); |
7188 | if (fabs(q_over_p)>Q_OVER_P_MAX) break; |
7189 | |
7190 | // Initialize path length variable and flight time |
7191 | len=0.; |
7192 | ftime=0.; |
7193 | var_ftime=0.; |
7194 | |
7195 | // Scale cut for pruning hits according to the iteration number |
7196 | anneal_factor=(iter2<MIN_ITER3)?(ANNEAL_SCALE/pow(ANNEAL_POW_CONST,iter2)+1.):1.; |
7197 | |
7198 | // Initialize trajectory deque |
7199 | jerror_t ref_track_error=SetCDCReferenceTrajectory(last_pos,Sc); |
7200 | if (ref_track_error!=NOERROR){ |
7201 | if (iter2==0) return FIT_FAILED; |
7202 | break; |
7203 | } |
7204 | |
7205 | // Reset the status of the cdc hits |
7206 | for (unsigned int j=0;j<num_cdchits;j++){ |
7207 | if (my_cdchits[j]->status!=late_hit)my_cdchits[j]->status=good_hit; |
7208 | } |
7209 | |
7210 | // perform the fit |
7211 | Cc=C0; |
7212 | bool did_second_refit=false; |
7213 | error=KalmanCentral(anneal_factor,Sc,Cc,pos,chisq,my_ndf); |
7214 | |
7215 | // Try to recover tracks that failed the first attempt at fitting by |
7216 | // throwing away outer hits. |
7217 | if (error!=FIT_SUCCEEDED && RECOVER_BROKEN_TRACKS |
7218 | && num_cdchits>=MIN_HITS_FOR_REFIT){ |
7219 | DVector2 temp_pos=pos; |
7220 | DMatrix5x1 Stemp=Sc; |
7221 | DMatrix5x5 Ctemp=Cc; |
7222 | unsigned int temp_ndf=my_ndf; |
7223 | double temp_chi2=chisq; |
7224 | |
7225 | if (error==MOMENTUM_OUT_OF_RANGE){ |
7226 | //_DBG_ << "Momentum out of range" <<endl; |
7227 | unsigned int new_index=(3*max_cdc_index)/4; |
7228 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
7229 | } |
7230 | |
7231 | if (error==BROKEN_COVARIANCE_MATRIX){ |
7232 | break_point_cdc_index=min_cdc_index_for_refit; |
7233 | //_DBG_ << "Bad Cov" <<endl; |
7234 | } |
7235 | if (error==POSITION_OUT_OF_RANGE){ |
7236 | //_DBG_ << "Bad position" << endl; |
7237 | unsigned int new_index=(max_cdc_index)/2; |
7238 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
7239 | } |
7240 | if (error==PRUNED_TOO_MANY_HITS){ |
7241 | unsigned int new_index=(3*max_cdc_index)/4; |
7242 | break_point_cdc_index=(new_index>min_cdc_index_for_refit)?new_index:min_cdc_index_for_refit; |
7243 | //anneal_factor*=10.; |
7244 | //_DBG_ << "Prune" << endl; |
7245 | } |
7246 | |
7247 | kalman_error_t refit_error=RecoverBrokenTracks(anneal_factor,Sc,Cc,C0,pos,chisq,my_ndf); |
7248 | if (fit_type==kTimeBased && refit_error!=FIT_SUCCEEDED){ |
7249 | anneal_factor=1000.; |
7250 | refit_error=RecoverBrokenTracks(anneal_factor,Sc,Cc,C0,pos,chisq,my_ndf); |
7251 | //chisq=1e6; |
7252 | did_second_refit=true; |
7253 | } |
7254 | |
7255 | if (refit_error!=FIT_SUCCEEDED){ |
7256 | //_DBG_ << error << endl; |
7257 | if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){ |
7258 | Cc=Ctemp; |
7259 | Sc=Stemp; |
7260 | my_ndf=temp_ndf; |
7261 | chisq=temp_chi2; |
7262 | pos=temp_pos; |
7263 | if (DEBUG_LEVEL > 1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7263<<" " << " Refit did not succeed, but restoring old values" << endl; |
7264 | |
7265 | //error=FIT_SUCCEEDED; |
7266 | } |
7267 | } |
7268 | else error=FIT_SUCCEEDED; |
7269 | } |
7270 | if ((error==POSITION_OUT_OF_RANGE || error==MOMENTUM_OUT_OF_RANGE) |
7271 | && iter2==0){ |
7272 | return FIT_FAILED; |
7273 | } |
7274 | if (error==FIT_FAILED || error==INVALID_FIT || my_ndf==0){ |
7275 | if (iter2==0) return error; |
7276 | break; |
7277 | } |
7278 | |
7279 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7279<<" " << "--> Chisq " << chisq << " Ndof " << my_ndf |
7280 | << " Prob: " << TMath::Prob(chisq,my_ndf) |
7281 | << endl; |
7282 | |
7283 | if (iter2>MIN_ITER3 && did_second_refit==false){ |
7284 | double new_reduced_chisq=chisq/my_ndf; |
7285 | double old_reduced_chisq=chisq_iter/last_ndf; |
7286 | double new_prob=TMath::Prob(chisq,my_ndf); |
7287 | double old_prob=TMath::Prob(chisq_iter,last_ndf); |
7288 | if (new_prob<old_prob |
7289 | || fabs(new_reduced_chisq-old_reduced_chisq)<CHISQ_DELTA0.01) break; |
7290 | } |
7291 | |
7292 | // Save the current state vector and covariance matrix |
7293 | Cclast=Cc; |
7294 | Sclast=Sc; |
7295 | last_pos=pos; |
7296 | chisq_iter=chisq; |
7297 | last_ndf=my_ndf; |
7298 | last_error=error; |
7299 | |
7300 | last_cdc_used_in_fit=cdc_used_in_fit; |
7301 | } |
7302 | |
7303 | // Run smoother and fill pulls vector |
7304 | IsSmoothed=false; |
7305 | if(fit_type==kTimeBased){ |
7306 | cdc_pulls.clear(); |
7307 | if (SmoothCentral(cdc_pulls) == NOERROR){ |
7308 | IsSmoothed = true; |
7309 | pulls.assign(cdc_pulls.begin(),cdc_pulls.end()); |
7310 | } |
7311 | } |
7312 | |
7313 | // Fill extrapolations vector |
7314 | ClearExtrapolations(); |
7315 | ExtrapolateCentralToOtherDetectors(); |
7316 | if (extrapolations.at(SYS_BCAL).size()==1){ |
7317 | // There needs to be some steps inside the the volume of the BCAL for |
7318 | // the extrapolation to be useful. If this is not the case, clear |
7319 | // the extrolation vector. |
7320 | extrapolations[SYS_BCAL].clear(); |
7321 | } |
7322 | |
7323 | // Extrapolate to the point of closest approach to the beam line |
7324 | DVector2 beam_pos=beam_center+(Sclast(state_z)-beam_z0)*beam_dir; |
7325 | bool extrapolated=false; |
7326 | if ((last_pos-beam_pos).Mod()>EPS21.e-4){ // in cm |
7327 | if (ExtrapolateToVertex(last_pos,Sclast,Cclast)!=NOERROR) return EXTRAPOLATION_FAILED; |
7328 | extrapolated=true; |
7329 | } |
7330 | |
7331 | // output lists of hits used in the fit |
7332 | cdchits_used_in_fit.clear(); |
7333 | for (unsigned int m=0;m<last_cdc_used_in_fit.size();m++){ |
7334 | if (last_cdc_used_in_fit[m]){ |
7335 | cdchits_used_in_fit.push_back(my_cdchits[m]->hit); |
7336 | } |
7337 | } |
7338 | // output the pull information |
7339 | //pulls.assign(last_cdc_pulls.begin(),last_cdc_pulls.end()); |
7340 | |
7341 | // Track Parameters at "vertex" |
7342 | phi_=Sclast(state_phi); |
7343 | q_over_pt_=Sclast(state_q_over_pt); |
7344 | tanl_=Sclast(state_tanl); |
7345 | x_=last_pos.X(); |
7346 | y_=last_pos.Y(); |
7347 | z_=Sclast(state_z); |
7348 | // Find the (signed) distance of closest approach to the beam line |
7349 | if (extrapolated){ |
7350 | beam_pos=beam_center+(z_-beam_z0)*beam_dir; |
7351 | } |
7352 | double dx=x_-beam_pos.X(); |
7353 | double dy=y_-beam_pos.Y(); |
7354 | D_=sqrt(dx*dx+dy*dy)+EPS3.0e-8; |
7355 | x_ = dx; y_ = dy; |
7356 | double cosphi=cos(phi_); |
7357 | double sinphi=sin(phi_); |
7358 | if ((dx>0.0 && sinphi>0.0) || (dy <0.0 && cosphi>0.0) |
7359 | || (dy>0.0 && cosphi<0.0) || (dx<0.0 && sinphi<0.0)) D_*=-1.; |
7360 | // Rotate covariance matrix to coordinate system centered on x=0,y=0 in the |
7361 | // lab |
7362 | DMatrix5x5 Jc=I5x5; |
7363 | Jc(state_D,state_D)=(dy*cosphi-dx*sinphi)/D_; |
7364 | //Cclast=Cclast.SandwichMultiply(Jc); |
7365 | Cclast=Jc*Cclast*Jc.Transpose(); |
7366 | |
7367 | if (!isfinite(x_) || !isfinite(y_) || !isfinite(z_) || !isfinite(phi_) |
7368 | || !isfinite(q_over_pt_) || !isfinite(tanl_)){ |
7369 | if (DEBUG_LEVEL>0){ |
7370 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7370<<" " << "At least one parameter is NaN or +-inf!!" <<endl; |
7371 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7371<<" " << "x " << x_ << " y " << y_ << " z " << z_ << " phi " << phi_ |
7372 | << " q/pt " << q_over_pt_ << " tanl " << tanl_ << endl; |
7373 | } |
7374 | return INVALID_FIT; |
7375 | } |
7376 | |
7377 | // Covariance matrix at vertex |
7378 | fcov.clear(); |
7379 | vector<double>dummy; |
7380 | for (unsigned int i=0;i<5;i++){ |
7381 | dummy.clear(); |
7382 | for(unsigned int j=0;j<5;j++){ |
7383 | dummy.push_back(Cclast(i,j)); |
7384 | } |
7385 | cov.push_back(dummy); |
7386 | } |
7387 | |
7388 | // total chisq and ndf |
7389 | chisq_=chisq_iter; |
7390 | ndf_=last_ndf; |
7391 | //printf("NDof %d\n",ndf); |
7392 | |
7393 | return last_error; |
7394 | } |
7395 | |
7396 | // Smoothing algorithm for the forward trajectory. Updates the state vector |
7397 | // at each step (going in the reverse direction to the filter) based on the |
7398 | // information from all the steps and outputs the state vector at the |
7399 | // outermost step. |
7400 | jerror_t DTrackFitterKalmanSIMD::SmoothForward(vector<pull_t>&forward_pulls){ |
7401 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
7402 | |
7403 | unsigned int max=forward_traj.size()-1; |
7404 | DMatrix5x1 S=(forward_traj[max].Skk); |
7405 | DMatrix5x5 C=(forward_traj[max].Ckk); |
7406 | DMatrix5x5 JT=forward_traj[max].J.Transpose(); |
7407 | DMatrix5x1 Ss=S; |
7408 | DMatrix5x5 Cs=C; |
7409 | DMatrix5x5 A,dC; |
7410 | |
7411 | if (DEBUG_LEVEL>19){ |
7412 | jout << "---- Smoothed residuals ----" <<endl; |
7413 | jout << setprecision(4); |
7414 | } |
7415 | |
7416 | for (unsigned int m=max-1;m>0;m--){ |
7417 | |
7418 | if (WRITE_ML_TRAINING_OUTPUT){ |
7419 | mlfile << forward_traj[m].z; |
7420 | for (unsigned int k=0;k<5;k++){ |
7421 | mlfile << " " << Ss(k); |
7422 | } |
7423 | for (unsigned int k=0;k<5;k++){ |
7424 | mlfile << " " << Cs(k,k); |
7425 | for (unsigned int j=k+1;j<5;j++){ |
7426 | mlfile <<" " << Cs(k,j); |
7427 | } |
7428 | } |
7429 | mlfile << endl; |
7430 | } |
7431 | |
7432 | if (forward_traj[m].h_id>0){ |
7433 | if (forward_traj[m].h_id<1000){ |
7434 | unsigned int id=forward_traj[m].h_id-1; |
7435 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7435<<" " << " Smoothing FDC ID " << id << endl; |
7436 | if (fdc_used_in_fit[id] && my_fdchits[id]->status==good_hit){ |
7437 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7437<<" " << " Used in fit " << endl; |
7438 | A=fdc_updates[id].C*JT*C.InvertSym(); |
7439 | Ss=fdc_updates[id].S+A*(Ss-S); |
7440 | |
7441 | if (!Ss.IsFinite()){ |
7442 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7442<<" " << "Invalid values for smoothed parameters..." << endl; |
7443 | return VALUE_OUT_OF_RANGE; |
7444 | } |
7445 | dC=A*(Cs-C)*A.Transpose(); |
7446 | Cs=fdc_updates[id].C+dC; |
7447 | if (!Cs.IsPosDef()){ |
7448 | if (DEBUG_LEVEL>1) |
7449 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7449<<" " << "Covariance Matrix not PosDef..." << endl; |
7450 | return VALUE_OUT_OF_RANGE; |
7451 | } |
7452 | |
7453 | // Position and direction from state vector with small angle |
7454 | // alignment correction |
7455 | double x=Ss(state_x) + my_fdchits[id]->phiZ*Ss(state_y); |
7456 | double y=Ss(state_y) - my_fdchits[id]->phiZ*Ss(state_x); |
7457 | double tx=Ss(state_tx)+ my_fdchits[id]->phiZ*Ss(state_ty) |
7458 | - my_fdchits[id]->phiY; |
7459 | double ty=Ss(state_ty) - my_fdchits[id]->phiZ*Ss(state_tx) |
7460 | + my_fdchits[id]->phiX; |
7461 | |
7462 | double cosa=my_fdchits[id]->cosa; |
7463 | double sina=my_fdchits[id]->sina; |
7464 | double u=my_fdchits[id]->uwire; |
7465 | double v=my_fdchits[id]->vstrip; |
7466 | |
7467 | // Projected position along the wire without doca-dependent corrections |
7468 | double vpred_uncorrected=x*sina+y*cosa; |
7469 | |
7470 | // Projected position in the plane of the wires transverse to the wires |
7471 | double upred=x*cosa-y*sina; |
7472 | |
7473 | // Direction tangent in the u-z plane |
7474 | double tu=tx*cosa-ty*sina; |
7475 | double one_plus_tu2=1.+tu*tu; |
7476 | double alpha=atan(tu); |
7477 | double cosalpha=cos(alpha); |
7478 | //double cosalpha2=cosalpha*cosalpha; |
7479 | double sinalpha=sin(alpha); |
7480 | |
7481 | // (signed) distance of closest approach to wire |
7482 | double du=upred-u; |
7483 | double doca=du*cosalpha; |
7484 | |
7485 | // Correction for lorentz effect |
7486 | double nz=my_fdchits[id]->nz; |
7487 | double nr=my_fdchits[id]->nr; |
7488 | double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha; |
7489 | |
7490 | // Difference between measurement and projection |
7491 | double tv=tx*sina+ty*cosa; |
7492 | double resi=v-(vpred_uncorrected+doca*(nz_sinalpha_plus_nr_cosalpha |
7493 | -tv*sinalpha)); |
7494 | double drift_time=my_fdchits[id]->t-mT0 |
7495 | -forward_traj[m].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
7496 | double drift = 0.0; |
7497 | int left_right = -999; |
7498 | if (USE_FDC_DRIFT_TIMES) { |
7499 | drift = (du > 0.0 ? 1.0 : -1.0) * fdc_drift_distance(drift_time, forward_traj[m].B); |
7500 | left_right = (du > 0.0 ? +1 : -1); |
7501 | } |
7502 | |
7503 | double resi_a = drift - doca; |
7504 | |
7505 | // Variance from filter step |
7506 | // This V is really "R" in Fruhwirths notation, in the case that the track is used in the fit. |
7507 | DMatrix2x2 V=fdc_updates[id].V; |
7508 | // Compute projection matrix and find the variance for the residual |
7509 | DMatrix5x2 H_T; |
7510 | double temp2=nz_sinalpha_plus_nr_cosalpha-tv*sinalpha; |
7511 | H_T(state_x,1)=sina+cosa*cosalpha*temp2; |
7512 | H_T(state_y,1)=cosa-sina*cosalpha*temp2; |
7513 | |
7514 | double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha; |
7515 | double fac=nz*cos2_minus_sin2-2.*nr*cosalpha*sinalpha; |
7516 | double doca_cosalpha=doca*cosalpha; |
7517 | double temp=doca_cosalpha*fac; |
7518 | H_T(state_tx,1)=cosa*temp |
7519 | -doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2) |
7520 | ; |
7521 | H_T(state_ty,1)=-sina*temp |
7522 | -doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2) |
7523 | ; |
7524 | |
7525 | H_T(state_x,0)=cosa*cosalpha; |
7526 | H_T(state_y,0)=-sina*cosalpha; |
7527 | double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2; |
7528 | H_T(state_ty,0)=sina*factor; |
7529 | H_T(state_tx,0)=-cosa*factor; |
7530 | |
7531 | // Matrix transpose H_T -> H |
7532 | DMatrix2x5 H=Transpose(H_T); |
7533 | |
7534 | if (my_fdchits[id]->hit!=NULL__null |
7535 | && my_fdchits[id]->hit->wire->layer==PLANE_TO_SKIP){ |
7536 | //V+=Cs.SandwichMultiply(H_T); |
7537 | V=V+H*Cs*H_T; |
7538 | } |
7539 | else{ |
7540 | //V-=dC.SandwichMultiply(H_T); |
7541 | V=V-H*dC*H_T; |
7542 | } |
7543 | |
7544 | |
7545 | vector<double> alignmentDerivatives; |
7546 | if (ALIGNMENT_FORWARD && my_fdchits[id]->hit!=NULL__null){ |
7547 | alignmentDerivatives.resize(FDCTrackD::size); |
7548 | // Let's get the alignment derivatives |
7549 | |
7550 | // Things are assumed to be linear near the wire, derivatives can be determined analytically. |
7551 | // First for the wires |
7552 | |
7553 | //dDOCAW/ddeltax |
7554 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaX] = -(1/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
7555 | |
7556 | //dDOCAW/ddeltaz |
7557 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaZ] = (tx*cosa - ty*sina)/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
7558 | |
7559 | //dDOCAW/ddeltaPhiX |
7560 | double cos2a = cos(2.*my_fdchits[id]->hit->wire->angle); |
7561 | double sin2a = sin(2.*my_fdchits[id]->hit->wire->angle); |
7562 | double cos3a = cos(3.*my_fdchits[id]->hit->wire->angle); |
7563 | double sin3a = sin(3.*my_fdchits[id]->hit->wire->angle); |
7564 | //double tx2 = tx*tx; |
7565 | //double ty2 = ty*ty; |
7566 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaPhiX] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/ |
7567 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7568 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaPhiY] = -((cosa*(tx*cosa - ty*sina)*(u - x*cosa + y*sina))/ |
7569 | pow(1 + pow(tx*cosa - ty*sina,2),1.5)); |
7570 | alignmentDerivatives[FDCTrackD::dDOCAW_dDeltaPhiZ] = (tx*ty*u*cos2a + (x + pow(ty,2)*x - tx*ty*y)*sina + |
7571 | cosa*(-(tx*ty*x) + y + pow(tx,2)*y + (tx - ty)*(tx + ty)*u*sina))/ |
7572 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7573 | |
7574 | // dDOCAW/dt0 |
7575 | double t0shift=4.;//ns |
7576 | double drift_shift = 0.0; |
7577 | if(USE_FDC_DRIFT_TIMES){ |
7578 | if (drift_time < 0.) drift_shift = drift; |
7579 | else drift_shift = (du>0.0?1.:-1.)*fdc_drift_distance(drift_time+t0shift,forward_traj[m].B); |
7580 | } |
7581 | alignmentDerivatives[FDCTrackD::dW_dt0]= (drift_shift-drift)/t0shift; |
7582 | |
7583 | // dDOCAW/dx |
7584 | alignmentDerivatives[FDCTrackD::dDOCAW_dx] = cosa/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
7585 | |
7586 | // dDOCAW/dy |
7587 | alignmentDerivatives[FDCTrackD::dDOCAW_dy] = -(sina/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
7588 | |
7589 | // dDOCAW/dtx |
7590 | alignmentDerivatives[FDCTrackD::dDOCAW_dtx] = (cosa*(tx*cosa - ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7591 | |
7592 | // dDOCAW/dty |
7593 | alignmentDerivatives[FDCTrackD::dDOCAW_dty] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/ |
7594 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
7595 | |
7596 | // Then for the cathodes. The magnetic field correction now correlates the alignment constants for the wires and cathodes. |
7597 | |
7598 | //dDOCAC/ddeltax |
7599 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaX] = |
7600 | (-nr + (-nz + ty*cosa + tx*sina)*(tx*cosa - ty*sina))/(1 + pow(tx*cosa - ty*sina,2)); |
7601 | |
7602 | //dDOCAC/ddeltaz |
7603 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaZ] = |
7604 | nz + (-nz + (nr*tx + ty)*cosa + (tx - nr*ty)*sina)/(1 + pow(tx*cosa - ty*sina,2)); |
7605 | |
7606 | //dDOCAC/ddeltaPhiX |
7607 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaPhiX] = |
7608 | (-2*y*cosa*sina*(tx*cosa - ty*sina) - 2*x*pow(sina,2)*(tx*cosa - ty*sina) - |
7609 | (u - x*cosa + y*sina)*(-(nz*sina) + sina*(ty*cosa + tx*sina) - |
7610 | cosa*(tx*cosa - ty*sina)))/(1 + pow(tx*cosa - ty*sina,2)) + |
7611 | (2*sina*(tx*cosa - ty*sina)*(-((u - x*cosa + y*sina)* |
7612 | (nr + nz*(tx*cosa - ty*sina) - (ty*cosa + tx*sina)*(tx*cosa - ty*sina))) + |
7613 | y*cosa*(1 + pow(tx*cosa - ty*sina,2)) + x*sina*(1 + pow(tx*cosa - ty*sina,2))))/ |
7614 | pow(1 + pow(tx*cosa - ty*sina,2),2); |
7615 | |
7616 | |
7617 | //dDOCAC/ddeltaPhiY |
7618 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaPhiY] = (-2*y*pow(cosa,2)*(tx*cosa - ty*sina) - 2*x*cosa*sina*(tx*cosa - ty*sina) - |
7619 | (u - x*cosa + y*sina)*(-(nz*cosa) + cosa*(ty*cosa + tx*sina) + |
7620 | sina*(tx*cosa - ty*sina)))/(1 + pow(tx*cosa - ty*sina,2)) + |
7621 | (2*cosa*(tx*cosa - ty*sina)*(-((u - x*cosa + y*sina)* |
7622 | (nr + nz*(tx*cosa - ty*sina) - (ty*cosa + tx*sina)*(tx*cosa - ty*sina))) + |
7623 | y*cosa*(1 + pow(tx*cosa - ty*sina,2)) + x*sina*(1 + pow(tx*cosa - ty*sina,2))))/ |
7624 | pow(1 + pow(tx*cosa - ty*sina,2),2); |
7625 | |
7626 | //dDOCAC/ddeltaPhiZ |
7627 | alignmentDerivatives[FDCTrackD::dDOCAC_dDeltaPhiZ] = (-2*(ty*cosa + tx*sina)*(tx*cosa - ty*sina)* |
7628 | (-((u - x*cosa + y*sina)*(nr + nz*(tx*cosa - ty*sina) - |
7629 | (ty*cosa + tx*sina)*(tx*cosa - ty*sina))) + |
7630 | y*cosa*(1 + pow(tx*cosa - ty*sina,2)) + x*sina*(1 + pow(tx*cosa - ty*sina,2))))/ |
7631 | pow(1 + pow(tx*cosa - ty*sina,2),2) + |
7632 | (2*y*cosa*(ty*cosa + tx*sina)*(tx*cosa - ty*sina) + |
7633 | 2*x*sina*(ty*cosa + tx*sina)*(tx*cosa - ty*sina) - |
7634 | (-(y*cosa) - x*sina)*(nr + nz*(tx*cosa - ty*sina) - |
7635 | (ty*cosa + tx*sina)*(tx*cosa - ty*sina)) - |
7636 | x*cosa*(1 + pow(tx*cosa - ty*sina,2)) + y*sina*(1 + pow(tx*cosa - ty*sina,2)) - |
7637 | (u - x*cosa + y*sina)*(nz*(ty*cosa + tx*sina) - pow(ty*cosa + tx*sina,2) - |
7638 | (tx*cosa - ty*sina)*(-(tx*cosa) + ty*sina)))/(1 + pow(tx*cosa - ty*sina,2)); |
7639 | |
7640 | //dDOCAC/dx |
7641 | alignmentDerivatives[FDCTrackD::dDOCAC_dx] = (cosa*(nr - tx*ty + nz*tx*cosa) + sina + ty*(ty - nz*cosa)*sina)/ |
7642 | (1 + pow(tx*cosa - ty*sina,2)); |
7643 | |
7644 | //dDOCAC/dy |
7645 | alignmentDerivatives[FDCTrackD::dDOCAC_dy] = ((1 + pow(tx,2))*cosa - (nr + tx*ty + nz*tx*cosa)*sina + nz*ty*pow(sina,2))/ |
7646 | (1 + pow(tx*cosa - ty*sina,2)); |
7647 | |
7648 | //dDOCAC/dtx |
7649 | 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 + |
7650 | 2*(2*nr*tx + ty*(2 - pow(tx,2) + pow(ty,2)))*cos2a + nz*(tx - ty)*(tx + ty)*cos3a - 2*nz*tx*ty*sina + |
7651 | 4*(tx - nr*ty + tx*pow(ty,2))*sin2a - 2*nz*tx*ty*sin3a))/ |
7652 | pow(2 + pow(tx,2) + pow(ty,2) + (tx - ty)*(tx + ty)*cos2a - 2*tx*ty*sin2a,2); |
7653 | |
7654 | //dDOCAC/dty |
7655 | 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 - |
7656 | 2*(2*tx + pow(tx,3) - 2*nr*ty - tx*pow(ty,2))*cos2a + 2*nz*tx*ty*cos3a + |
7657 | nz*(-4 + pow(tx,2) + 3*pow(ty,2))*sina + 4*(ty + tx*(nr + tx*ty))*sin2a + nz*(tx - ty)*(tx + ty)*sin3a)) |
7658 | /pow(2 + pow(tx,2) + pow(ty,2) + (tx - ty)*(tx + ty)*cos2a - 2*tx*ty*sin2a,2)); |
7659 | |
7660 | } |
7661 | |
7662 | if (DEBUG_LEVEL>19 && my_fdchits[id]->hit!=NULL__null){ |
7663 | jout << "Layer " << my_fdchits[id]->hit->wire->layer |
7664 | <<": t " << drift_time << " x "<< x << " y " << y |
7665 | << " coordinate along wire " << v << " resi_c " <<resi |
7666 | << " coordinate transverse to wire " << drift |
7667 | <<" resi_a " << resi_a |
7668 | <<endl; |
7669 | } |
7670 | |
7671 | double scale=1./sqrt(1.+tx*tx+ty*ty); |
7672 | double cosThetaRel=0.; |
7673 | if (my_fdchits[id]->hit!=NULL__null){ |
7674 | my_fdchits[id]->hit->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale)); |
7675 | } |
7676 | DTrackFitter::pull_t thisPull = pull_t(resi_a,sqrt(V(0,0)), |
7677 | forward_traj[m].s, |
7678 | fdc_updates[id].tdrift, |
7679 | fdc_updates[id].doca, |
7680 | NULL__null,my_fdchits[id]->hit, |
7681 | 0., |
7682 | forward_traj[m].z, |
7683 | cosThetaRel,0., |
7684 | resi,sqrt(V(1,1))); |
7685 | thisPull.left_right = left_right; |
7686 | thisPull.AddTrackDerivatives(alignmentDerivatives); |
7687 | forward_pulls.push_back(thisPull); |
7688 | } |
7689 | else{ |
7690 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
7691 | Ss=forward_traj[m].Skk+A*(Ss-S); |
7692 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
7693 | } |
7694 | |
7695 | } |
7696 | else{ |
7697 | unsigned int id=forward_traj[m].h_id-1000; |
7698 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7698<<" " << " Smoothing CDC ID " << id << endl; |
7699 | if (cdc_used_in_fit[id]&&my_cdchits[id]->status==good_hit){ |
7700 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7700<<" " << " Used in fit " << endl; |
7701 | A=cdc_updates[id].C*JT*C.InvertSym(); |
7702 | Ss=cdc_updates[id].S+A*(Ss-S); |
7703 | Cs=cdc_updates[id].C+A*(Cs-C)*A.Transpose(); |
7704 | if (!Cs.IsPosDef()){ |
7705 | if (DEBUG_LEVEL>1) |
7706 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7706<<" " << "Covariance Matrix not PosDef..." << endl; |
7707 | return VALUE_OUT_OF_RANGE; |
7708 | } |
7709 | if (!Ss.IsFinite()){ |
7710 | if (DEBUG_LEVEL>5) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7710<<" " << "Invalid values for smoothed parameters..." << endl; |
7711 | return VALUE_OUT_OF_RANGE; |
7712 | } |
7713 | |
7714 | // Fill in pulls information for cdc hits |
7715 | if(FillPullsVectorEntry(Ss,Cs,forward_traj[m],my_cdchits[id], |
7716 | cdc_updates[id],forward_pulls) != NOERROR) return VALUE_OUT_OF_RANGE; |
7717 | } |
7718 | else{ |
7719 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
7720 | Ss=forward_traj[m].Skk+A*(Ss-S); |
7721 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
7722 | } |
7723 | } |
7724 | } |
7725 | else{ |
7726 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
7727 | Ss=forward_traj[m].Skk+A*(Ss-S); |
7728 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
7729 | } |
7730 | |
7731 | S=forward_traj[m].Skk; |
7732 | C=forward_traj[m].Ckk; |
7733 | JT=forward_traj[m].J.Transpose(); |
7734 | } |
7735 | |
7736 | return NOERROR; |
7737 | } |
7738 | |
7739 | // at each step (going in the reverse direction to the filter) based on the |
7740 | // information from all the steps. |
7741 | jerror_t DTrackFitterKalmanSIMD::SmoothCentral(vector<pull_t>&cdc_pulls){ |
7742 | if (central_traj.size()<2) return RESOURCE_UNAVAILABLE; |
7743 | |
7744 | unsigned int max = central_traj.size()-1; |
7745 | DMatrix5x1 S=(central_traj[max].Skk); |
7746 | DMatrix5x5 C=(central_traj[max].Ckk); |
7747 | DMatrix5x5 JT=central_traj[max].J.Transpose(); |
7748 | DMatrix5x1 Ss=S; |
7749 | DMatrix5x5 Cs=C; |
7750 | DMatrix5x5 A,dC; |
7751 | |
7752 | if (DEBUG_LEVEL>1) { |
7753 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7753<<" " << " S C JT at start of smoothing " << endl; |
7754 | S.Print(); C.Print(); JT.Print(); |
7755 | } |
7756 | |
7757 | for (unsigned int m=max-1;m>0;m--){ |
7758 | if (central_traj[m].h_id>0){ |
7759 | unsigned int id=central_traj[m].h_id-1; |
7760 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7760<<" " << " Encountered Hit ID " << id << " At trajectory position " << m << "/" << max << endl; |
7761 | if (cdc_used_in_fit[id] && my_cdchits[id]->status == good_hit){ |
7762 | if (DEBUG_LEVEL>1) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7762<<" " << " SmoothCentral CDC Hit ID " << id << " used in fit " << endl; |
7763 | |
7764 | A=cdc_updates[id].C*JT*C.InvertSym(); |
7765 | dC=Cs-C; |
7766 | Ss=cdc_updates[id].S+A*(Ss-S); |
7767 | Cs=cdc_updates[id].C+A*dC*A.Transpose(); |
7768 | |
7769 | if (!Ss.IsFinite()){ |
7770 | if (DEBUG_LEVEL>5) |
7771 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7771<<" " << "Invalid values for smoothed parameters..." << endl; |
7772 | return VALUE_OUT_OF_RANGE; |
7773 | } |
7774 | if (!Cs.IsPosDef()){ |
7775 | if (DEBUG_LEVEL>5){ |
7776 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<7776<<" " << "Covariance Matrix not PosDef... Ckk dC A" << endl; |
7777 | cdc_updates[id].C.Print(); dC.Print(); A.Print(); |
7778 | } |
7779 | return VALUE_OUT_OF_RANGE; |
7780 | } |
7781 | |
7782 | // Get estimate for energy loss |
7783 | double q_over_p=Ss(state_q_over_pt)*cos(atan(Ss(state_tanl))); |
7784 | double dEdx=GetdEdx(q_over_p,central_traj[m].K_rho_Z_over_A, |
7785 | central_traj[m].rho_Z_over_A, |
7786 | central_traj[m].LnI,central_traj[m].Z); |
7787 | |
7788 | // Use Brent's algorithm to find doca to the wire |
7789 | DVector2 xy(central_traj[m].xy.X()-Ss(state_D)*sin(Ss(state_phi)), |
7790 | central_traj[m].xy.Y()+Ss(state_D)*cos(Ss(state_phi))); |
7791 | DVector2 old_xy=xy; |
7792 | DMatrix5x1 myS=Ss; |
7793 | double myds; |
7794 | DVector2 origin=my_cdchits[id]->origin; |
7795 | DVector2 dir=my_cdchits[id]->dir; |
7796 | double z0wire=my_cdchits[id]->z0wire; |
7797 | //BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy,z0wire,origin,dir,myS,myds); |
7798 | if(BrentCentral(dEdx,xy,z0wire,origin,dir,myS,myds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7799 | if(DEBUG_HISTS) alignDerivHists[0]->Fill(myds); |
7800 | DVector2 wirepos=origin+(myS(state_z)-z0wire)*dir; |
7801 | double cosstereo=my_cdchits[id]->cosstereo; |
7802 | DVector2 diff=xy-wirepos; |
7803 | // here we add a small number to avoid division by zero errors |
7804 | double d=cosstereo*diff.Mod()+EPS3.0e-8; |
7805 | |
7806 | // If we are doing the alignment, we need to numerically calculate the derivatives |
7807 | // wrt the wire origin, direction, and the track parameters. |
7808 | vector<double> alignmentDerivatives; |
7809 | if (ALIGNMENT_CENTRAL){ |
7810 | double dscut_min=0., dscut_max=1.; |
7811 | DVector3 wireDir = my_cdchits[id]->hit->wire->udir; |
7812 | double cosstereo_shifted; |
7813 | DMatrix5x1 alignS=Ss; // We will mess with this one |
7814 | double alignds; |
7815 | alignmentDerivatives.resize(12); |
7816 | alignmentDerivatives[CDCTrackD::dDdt0]=cdc_updates[id].dDdt0; |
7817 | // Wire position shift |
7818 | double wposShift=0.025; |
7819 | double wdirShift=0.00005; |
7820 | |
7821 | // Shift each track parameter value |
7822 | double shiftFraction=0.01; |
7823 | double shift_q_over_pt=shiftFraction*Ss(state_q_over_pt); |
7824 | double shift_phi=0.0001; |
7825 | double shift_tanl=shiftFraction*Ss(state_tanl); |
7826 | double shift_D=0.01; |
7827 | double shift_z=0.01; |
7828 | |
7829 | // Some data containers we don't need multiples of |
7830 | double z0_shifted; |
7831 | DVector2 shift, origin_shifted, dir_shifted, wirepos_shifted, diff_shifted, xy_shifted; |
7832 | |
7833 | // The DOCA for the shifted states == f(x+h) |
7834 | double d_dOriginX=0., d_dOriginY=0., d_dOriginZ=0.; |
7835 | double d_dDirX=0., d_dDirY=0., d_dDirZ=0.; |
7836 | double d_dS0=0., d_dS1=0., d_dS2=0., d_dS3=0., d_dS4=0.; |
7837 | // Let's do the wire shifts first |
7838 | |
7839 | //dOriginX |
7840 | alignS=Ss; |
7841 | alignds=0.; |
7842 | shift.Set(wposShift, 0.); |
7843 | origin_shifted=origin+shift; |
7844 | dir_shifted=dir; |
7845 | z0_shifted=z0wire; |
7846 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7847 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7848 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7849 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7850 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7851 | //if (BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7852 | // dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7853 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7854 | diff_shifted=xy_shifted-wirepos_shifted; |
7855 | d_dOriginX=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7856 | alignmentDerivatives[CDCTrackD::dDOCAdOriginX] = (d_dOriginX - d)/wposShift; |
7857 | if(DEBUG_HISTS){ |
7858 | alignDerivHists[12]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginX]); |
7859 | alignDerivHists[1]->Fill(alignds); |
7860 | brentCheckHists[1]->Fill(alignds,d_dOriginX); |
7861 | } |
7862 | |
7863 | //dOriginY |
7864 | alignS=Ss; |
7865 | alignds=0.; |
7866 | shift.Set(0.,wposShift); |
7867 | origin_shifted=origin+shift; |
7868 | dir_shifted=dir; |
7869 | z0_shifted=z0wire; |
7870 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7871 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7872 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7873 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7874 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7875 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7876 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7877 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7878 | diff_shifted=xy_shifted-wirepos_shifted; |
7879 | d_dOriginY=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7880 | alignmentDerivatives[CDCTrackD::dDOCAdOriginY] = (d_dOriginY - d)/wposShift; |
7881 | if(DEBUG_HISTS){ |
7882 | alignDerivHists[13]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginY]); |
7883 | alignDerivHists[2]->Fill(alignds); |
7884 | brentCheckHists[1]->Fill(alignds,d_dOriginY); |
7885 | } |
7886 | |
7887 | //dOriginZ |
7888 | alignS=Ss; |
7889 | alignds=0.; |
7890 | origin_shifted=origin; |
7891 | dir_shifted=dir; |
7892 | z0_shifted=z0wire+wposShift; |
7893 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7894 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7895 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7896 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7897 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7898 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7899 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7900 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7901 | diff_shifted=xy_shifted-wirepos_shifted; |
7902 | d_dOriginZ=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
7903 | alignmentDerivatives[CDCTrackD::dDOCAdOriginZ] = (d_dOriginZ - d)/wposShift; |
7904 | if(DEBUG_HISTS){ |
7905 | alignDerivHists[14]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginZ]); |
7906 | alignDerivHists[3]->Fill(alignds); |
7907 | brentCheckHists[1]->Fill(alignds,d_dOriginZ); |
7908 | } |
7909 | |
7910 | //dDirX |
7911 | alignS=Ss; |
7912 | alignds=0.; |
7913 | shift.Set(wdirShift,0.); |
7914 | origin_shifted=origin; |
7915 | z0_shifted=z0wire; |
7916 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7917 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7918 | dir_shifted=dir+shift; |
7919 | cosstereo_shifted = cos((wireDir+DVector3(wdirShift,0.,0.)).Angle(DVector3(0.,0.,1.))); |
7920 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7921 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7922 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7923 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7924 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7925 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7926 | diff_shifted=xy_shifted-wirepos_shifted; |
7927 | d_dDirX=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
7928 | alignmentDerivatives[CDCTrackD::dDOCAdDirX] = (d_dDirX - d)/wdirShift; |
7929 | if(DEBUG_HISTS){ |
7930 | alignDerivHists[15]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirX]); |
7931 | alignDerivHists[4]->Fill(alignds); |
7932 | } |
7933 | |
7934 | //dDirY |
7935 | alignS=Ss; |
7936 | alignds=0.; |
7937 | shift.Set(0.,wdirShift); |
7938 | origin_shifted=origin; |
7939 | z0_shifted=z0wire; |
7940 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7941 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7942 | dir_shifted=dir+shift; |
7943 | cosstereo_shifted = cos((wireDir+DVector3(0.,wdirShift,0.)).Angle(DVector3(0.,0.,1.))); |
7944 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7945 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7946 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7947 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7948 | // dir_shifted,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7949 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7950 | diff_shifted=xy_shifted-wirepos_shifted; |
7951 | d_dDirY=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
7952 | alignmentDerivatives[CDCTrackD::dDOCAdDirY] = (d_dDirY - d)/wdirShift; |
7953 | if(DEBUG_HISTS){ |
7954 | alignDerivHists[16]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirY]); |
7955 | alignDerivHists[5]->Fill(alignds); |
7956 | } |
7957 | |
7958 | //dDirZ |
7959 | alignS=Ss; |
7960 | alignds=0.; |
7961 | origin_shifted=origin; |
7962 | dir_shifted.Set(wireDir.X()/(wireDir.Z()+wdirShift), wireDir.Y()/(wireDir.Z()+wdirShift)); |
7963 | z0_shifted=z0wire; |
7964 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7965 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7966 | cosstereo_shifted = cos((wireDir+DVector3(0.,0.,wdirShift)).Angle(DVector3(0.,0.,1.))); |
7967 | if (BrentCentral(dEdx,xy_shifted,z0_shifted,origin_shifted, |
7968 | dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7969 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7970 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0_shifted,origin_shifted, |
7971 | // dir_shifted,alignS,alignds)!=NOERROR) return VALUE_OUT_OF_RANGE; |
7972 | wirepos_shifted=origin_shifted+(alignS(state_z)-z0_shifted)*dir_shifted; |
7973 | diff_shifted=xy_shifted-wirepos_shifted; |
7974 | d_dDirZ=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
7975 | alignmentDerivatives[CDCTrackD::dDOCAdDirZ] = (d_dDirZ - d)/wdirShift; |
7976 | if(DEBUG_HISTS){ |
7977 | alignDerivHists[17]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirZ]); |
7978 | alignDerivHists[6]->Fill(alignds); |
7979 | } |
7980 | |
7981 | // And now the derivatives wrt the track parameters |
7982 | //DMatrix5x1 trackShift(shift_q_over_pt, shift_phi, shift_tanl, shift_D, shift_z); |
7983 | |
7984 | DMatrix5x1 trackShiftS0(shift_q_over_pt, 0., 0., 0., 0.); |
7985 | DMatrix5x1 trackShiftS1(0., shift_phi, 0., 0., 0.); |
7986 | DMatrix5x1 trackShiftS2(0., 0., shift_tanl, 0., 0.); |
7987 | DMatrix5x1 trackShiftS3(0., 0., 0., shift_D, 0.); |
7988 | DMatrix5x1 trackShiftS4(0., 0., 0., 0., shift_z); |
7989 | |
7990 | // dS0 |
7991 | alignS=Ss+trackShiftS0; |
7992 | alignds=0.; |
7993 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
7994 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
7995 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7996 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
7997 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
7998 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
7999 | diff_shifted=xy_shifted-wirepos_shifted; |
8000 | d_dS0=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8001 | alignmentDerivatives[CDCTrackD::dDOCAdS0] = (d_dS0 - d)/shift_q_over_pt; |
8002 | if(DEBUG_HISTS){ |
8003 | alignDerivHists[18]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS0]); |
8004 | alignDerivHists[7]->Fill(alignds); |
8005 | } |
8006 | |
8007 | // dS1 |
8008 | alignS=Ss+trackShiftS1; |
8009 | alignds=0.; |
8010 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
8011 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
8012 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8013 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
8014 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8015 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
8016 | diff_shifted=xy_shifted-wirepos_shifted; |
8017 | d_dS1=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8018 | alignmentDerivatives[CDCTrackD::dDOCAdS1] = (d_dS1 - d)/shift_phi; |
8019 | if(DEBUG_HISTS){ |
8020 | alignDerivHists[19]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS1]); |
8021 | alignDerivHists[8]->Fill(alignds); |
8022 | } |
8023 | |
8024 | // dS2 |
8025 | alignS=Ss+trackShiftS2; |
8026 | alignds=0.; |
8027 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
8028 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
8029 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8030 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
8031 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8032 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
8033 | diff_shifted=xy_shifted-wirepos_shifted; |
8034 | d_dS2=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8035 | alignmentDerivatives[CDCTrackD::dDOCAdS2] = (d_dS2 - d)/shift_tanl; |
8036 | if(DEBUG_HISTS){ |
8037 | alignDerivHists[20]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS2]); |
8038 | alignDerivHists[9]->Fill(alignds); |
8039 | } |
8040 | |
8041 | // dS3 |
8042 | alignS=Ss+trackShiftS3; |
8043 | alignds=0.; |
8044 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
8045 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
8046 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8047 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
8048 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8049 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
8050 | diff_shifted=xy_shifted-wirepos_shifted; |
8051 | d_dS3=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8052 | alignmentDerivatives[CDCTrackD::dDOCAdS3] = (d_dS3 - d)/shift_D; |
8053 | if(DEBUG_HISTS){ |
8054 | alignDerivHists[21]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS3]); |
8055 | alignDerivHists[10]->Fill(alignds); |
8056 | } |
8057 | |
8058 | // dS4 |
8059 | alignS=Ss+trackShiftS4; |
8060 | alignds=0.; |
8061 | xy_shifted.Set(central_traj[m].xy.X()-alignS(state_D)*sin(alignS(state_phi)), |
8062 | central_traj[m].xy.Y()+alignS(state_D)*cos(alignS(state_phi))); |
8063 | if(BrentCentral(dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8064 | if (alignds < dscut_min || alignds > dscut_max) return VALUE_OUT_OF_RANGE; |
8065 | //if(BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dEdx,xy_shifted,z0wire,origin,dir,alignS,alignds) != NOERROR) return VALUE_OUT_OF_RANGE; |
8066 | wirepos_shifted=origin+(alignS(state_z)-z0wire)*dir; |
8067 | diff_shifted=xy_shifted-wirepos_shifted; |
8068 | d_dS4=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8069 | alignmentDerivatives[CDCTrackD::dDOCAdS4] = (d_dS4 - d)/shift_z; |
8070 | if(DEBUG_HISTS){ |
8071 | alignDerivHists[22]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS4]); |
8072 | alignDerivHists[11]->Fill(alignds); |
8073 | } |
8074 | } |
8075 | |
8076 | // Compute the Jacobian matrix |
8077 | // Find the field and gradient at (old_x,old_y,old_z) |
8078 | bfield->GetFieldAndGradient(old_xy.X(),old_xy.Y(),Ss(state_z), |
8079 | Bx,By,Bz, |
8080 | dBxdx,dBxdy,dBxdz,dBydx, |
8081 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
8082 | DMatrix5x5 Jc; |
8083 | StepJacobian(old_xy,xy-old_xy,myds,Ss,dEdx,Jc); |
8084 | |
8085 | // Projection matrix |
8086 | DMatrix5x1 H_T; |
8087 | double sinphi=sin(myS(state_phi)); |
8088 | double cosphi=cos(myS(state_phi)); |
8089 | double dx=diff.X(); |
8090 | double dy=diff.Y(); |
8091 | double cosstereo2_over_doca=cosstereo*cosstereo/d; |
8092 | H_T(state_D)=(dy*cosphi-dx*sinphi)*cosstereo2_over_doca; |
8093 | H_T(state_phi) |
8094 | =-myS(state_D)*cosstereo2_over_doca*(dx*cosphi+dy*sinphi); |
8095 | H_T(state_z)=-cosstereo2_over_doca*(dx*dir.X()+dy*dir.Y()); |
8096 | DMatrix1x5 H; |
8097 | H(state_D)=H_T(state_D); |
8098 | H(state_phi)=H_T(state_phi); |
8099 | H(state_z)=H_T(state_z); |
8100 | |
8101 | double Vhit=cdc_updates[id].variance; |
8102 | Cs=Jc*Cs*Jc.Transpose(); |
8103 | //double Vtrack = Cs.SandwichMultiply(Jc*H_T); |
8104 | double Vtrack=H*Cs*H_T; |
8105 | double VRes; |
8106 | |
8107 | bool skip_ring=(my_cdchits[id]->hit->wire->ring==RING_TO_SKIP); |
8108 | if (skip_ring) VRes = Vhit + Vtrack; |
8109 | else VRes = Vhit - Vtrack; |
8110 | |
8111 | if (DEBUG_LEVEL>1 && (!isfinite(VRes) || VRes < 0.0) ) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8111<<" " << " SmoothCentral Problem: VRes is " << VRes << " = " << Vhit << " - " << Vtrack << endl; |
8112 | |
8113 | double lambda=atan(Ss(state_tanl)); |
8114 | double sinl=sin(lambda); |
8115 | double cosl=cos(lambda); |
8116 | double cosThetaRel=my_cdchits[id]->hit->wire->udir.Dot(DVector3(cosphi*cosl, |
8117 | sinphi*cosl, |
8118 | sinl)); |
8119 | pull_t thisPull(cdc_updates[id].doca-d,sqrt(VRes), |
8120 | central_traj[m].s,cdc_updates[id].tdrift, |
8121 | d,my_cdchits[id]->hit,NULL__null, |
8122 | diff.Phi(),myS(state_z),cosThetaRel, |
8123 | cdc_updates[id].tcorr); |
8124 | |
8125 | thisPull.AddTrackDerivatives(alignmentDerivatives); |
8126 | cdc_pulls.push_back(thisPull); |
8127 | } |
8128 | else{ |
8129 | A=central_traj[m].Ckk*JT*C.InvertSym(); |
8130 | Ss=central_traj[m].Skk+A*(Ss-S); |
8131 | Cs=central_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8132 | } |
8133 | } |
8134 | else{ |
8135 | A=central_traj[m].Ckk*JT*C.InvertSym(); |
8136 | Ss=central_traj[m].Skk+A*(Ss-S); |
8137 | Cs=central_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8138 | } |
8139 | S=central_traj[m].Skk; |
8140 | C=central_traj[m].Ckk; |
8141 | JT=central_traj[m].J.Transpose(); |
8142 | } |
8143 | |
8144 | // ... last entries? |
8145 | // Don't really need since we should have already encountered all of the hits |
8146 | |
8147 | return NOERROR; |
8148 | |
8149 | } |
8150 | |
8151 | // Smoothing algorithm for the forward_traj_cdc trajectory. |
8152 | // Updates the state vector |
8153 | // at each step (going in the reverse direction to the filter) based on the |
8154 | // information from all the steps and outputs the state vector at the |
8155 | // outermost step. |
8156 | jerror_t DTrackFitterKalmanSIMD::SmoothForwardCDC(vector<pull_t>&cdc_pulls){ |
8157 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
8158 | |
8159 | unsigned int max=forward_traj.size()-1; |
8160 | DMatrix5x1 S=(forward_traj[max].Skk); |
8161 | DMatrix5x5 C=(forward_traj[max].Ckk); |
8162 | DMatrix5x5 JT=forward_traj[max].J.Transpose(); |
8163 | DMatrix5x1 Ss=S; |
8164 | DMatrix5x5 Cs=C; |
8165 | DMatrix5x5 A; |
8166 | for (unsigned int m=max-1;m>0;m--){ |
8167 | if (forward_traj[m].h_id>999){ |
8168 | unsigned int cdc_index=forward_traj[m].h_id-1000; |
8169 | if(cdc_used_in_fit[cdc_index] && my_cdchits[cdc_index]->status == good_hit){ |
8170 | if (DEBUG_LEVEL > 5) { |
8171 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8171<<" " << " Smoothing CDC index " << cdc_index << " ring " << my_cdchits[cdc_index]->hit->wire->ring |
8172 | << " straw " << my_cdchits[cdc_index]->hit->wire->straw << endl; |
8173 | } |
8174 | |
8175 | A=cdc_updates[cdc_index].C*JT*C.InvertSym(); |
8176 | Ss=cdc_updates[cdc_index].S+A*(Ss-S); |
8177 | if (!Ss.IsFinite()){ |
8178 | if (DEBUG_LEVEL>5) |
8179 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8179<<" " << "Invalid values for smoothed parameters..." << endl; |
8180 | return VALUE_OUT_OF_RANGE; |
8181 | } |
8182 | |
8183 | Cs=cdc_updates[cdc_index].C+A*(Cs-C)*A.Transpose(); |
8184 | |
8185 | if (!Cs.IsPosDef()){ |
8186 | if (DEBUG_LEVEL>5){ |
8187 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8187<<" " << "Covariance Matrix not Pos Def..." << endl; |
8188 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8188<<" " << " cdc_updates[cdc_index].C A C_ Cs " << endl; |
8189 | cdc_updates[cdc_index].C.Print(); |
8190 | A.Print(); |
8191 | C.Print(); |
8192 | Cs.Print(); |
8193 | } |
8194 | return VALUE_OUT_OF_RANGE; |
8195 | } |
8196 | if(FillPullsVectorEntry(Ss,Cs,forward_traj[m],my_cdchits[cdc_index], |
8197 | cdc_updates[cdc_index],cdc_pulls) != NOERROR) return VALUE_OUT_OF_RANGE; |
8198 | |
8199 | } |
8200 | else{ |
8201 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
8202 | Ss=forward_traj[m].Skk+A*(Ss-S); |
8203 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8204 | } |
8205 | } |
8206 | else{ |
8207 | A=forward_traj[m].Ckk*JT*C.InvertSym(); |
8208 | Ss=forward_traj[m].Skk+A*(Ss-S); |
8209 | Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose(); |
8210 | } |
8211 | |
8212 | S=forward_traj[m].Skk; |
8213 | C=forward_traj[m].Ckk; |
8214 | JT=forward_traj[m].J.Transpose(); |
8215 | } |
8216 | |
8217 | return NOERROR; |
8218 | } |
8219 | |
8220 | // Fill the pulls vector with the best residual information using the smoothed |
8221 | // filter results. Uses Brent's algorithm to find the distance of closest |
8222 | // approach to the wire hit. |
8223 | jerror_t DTrackFitterKalmanSIMD::FillPullsVectorEntry(const DMatrix5x1 &Ss, |
8224 | const DMatrix5x5 &Cs, |
8225 | const DKalmanForwardTrajectory_t &traj,const DKalmanSIMDCDCHit_t *hit,const DKalmanUpdate_t &update, |
8226 | vector<pull_t>&my_pulls){ |
8227 | |
8228 | // Get estimate for energy loss |
8229 | double dEdx=GetdEdx(Ss(state_q_over_p),traj.K_rho_Z_over_A,traj.rho_Z_over_A, |
8230 | traj.LnI,traj.Z); |
8231 | |
8232 | // Use Brent's algorithm to find the doca to the wire |
8233 | DMatrix5x1 myS=Ss; |
8234 | DMatrix5x1 myS_temp=Ss; |
8235 | DMatrix5x5 myC=Cs; |
8236 | double mydz; |
8237 | double z=traj.z; |
8238 | DVector2 origin=hit->origin; |
8239 | DVector2 dir=hit->dir; |
8240 | double z0wire=hit->z0wire; |
8241 | if(BrentForward(z,dEdx,z0wire,origin,dir,myS,mydz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8242 | if(DEBUG_HISTS)alignDerivHists[23]->Fill(mydz); |
8243 | double new_z=z+mydz; |
8244 | DVector2 wirepos=origin+(new_z-z0wire)*dir; |
8245 | double cosstereo=hit->cosstereo; |
8246 | DVector2 xy(myS(state_x),myS(state_y)); |
8247 | |
8248 | DVector2 diff=xy-wirepos; |
8249 | double d=cosstereo*diff.Mod(); |
8250 | |
8251 | // If we are doing the alignment, we need to numerically calculate the derivatives |
8252 | // wrt the wire origin, direction, and the track parameters. |
8253 | vector<double> alignmentDerivatives; |
8254 | if (ALIGNMENT_FORWARD && hit->hit!=NULL__null){ |
8255 | double dzcut_min=0., dzcut_max=1.; |
8256 | DMatrix5x1 alignS=Ss; // We will mess with this one |
8257 | DVector3 wireDir = hit->hit->wire->udir; |
8258 | double cosstereo_shifted; |
8259 | double aligndz; |
8260 | alignmentDerivatives.resize(12); |
8261 | |
8262 | // Set t0 derivative |
8263 | alignmentDerivatives[CDCTrackD::dDdt0]=update.dDdt0; |
8264 | |
8265 | // Wire position shift |
8266 | double wposShift=0.025; |
8267 | double wdirShift=0.00005; |
8268 | |
8269 | // Shift each track parameter |
8270 | double shiftFraction=0.01; |
8271 | double shift_x=0.01; |
8272 | double shift_y=0.01; |
8273 | double shift_tx=shiftFraction*Ss(state_tx); |
8274 | double shift_ty=shiftFraction*Ss(state_ty);; |
8275 | double shift_q_over_p=shiftFraction*Ss(state_q_over_p); |
8276 | |
8277 | // Some data containers we don't need multiples of |
8278 | double z0_shifted, new_z_shifted; |
8279 | DVector2 shift, origin_shifted, dir_shifted, wirepos_shifted, diff_shifted, xy_shifted; |
8280 | |
8281 | // The DOCA for the shifted states == f(x+h) |
8282 | double d_dOriginX=0., d_dOriginY=0., d_dOriginZ=0.; |
8283 | double d_dDirX=0., d_dDirY=0., d_dDirZ=0.; |
8284 | double d_dS0=0., d_dS1=0., d_dS2=0., d_dS3=0., d_dS4=0.; |
8285 | // Let's do the wire shifts first |
8286 | |
8287 | //dOriginX |
8288 | alignS=Ss; |
8289 | aligndz=0.; |
8290 | shift.Set(wposShift, 0.); |
8291 | origin_shifted=origin+shift; |
8292 | dir_shifted=dir; |
8293 | z0_shifted=z0wire; |
8294 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8295 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8296 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8297 | new_z_shifted=z+aligndz; |
8298 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8299 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8300 | diff_shifted=xy_shifted-wirepos_shifted; |
8301 | d_dOriginX=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8302 | alignmentDerivatives[CDCTrackD::dDOCAdOriginX] = (d_dOriginX - d)/wposShift; |
8303 | if(DEBUG_HISTS){ |
8304 | alignDerivHists[24]->Fill(aligndz); |
8305 | alignDerivHists[35]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginX]); |
8306 | brentCheckHists[0]->Fill(aligndz,d_dOriginX); |
8307 | } |
8308 | |
8309 | //dOriginY |
8310 | alignS=Ss; |
8311 | aligndz=0.; |
8312 | shift.Set(0.,wposShift); |
8313 | origin_shifted=origin+shift; |
8314 | dir_shifted=dir; |
8315 | z0_shifted=z0wire; |
8316 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8317 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8318 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8319 | new_z_shifted=z+aligndz; |
8320 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8321 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8322 | diff_shifted=xy_shifted-wirepos_shifted; |
8323 | d_dOriginY=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8324 | alignmentDerivatives[CDCTrackD::dDOCAdOriginY] = (d_dOriginY - d)/wposShift; |
8325 | if(DEBUG_HISTS){ |
8326 | alignDerivHists[25]->Fill(aligndz); |
8327 | alignDerivHists[36]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginY]); |
8328 | brentCheckHists[0]->Fill(aligndz,d_dOriginY); |
8329 | } |
8330 | |
8331 | //dOriginZ |
8332 | alignS=Ss; |
8333 | aligndz=0.; |
8334 | origin_shifted=origin; |
8335 | dir_shifted=dir; |
8336 | z0_shifted=z0wire+wposShift; |
8337 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8338 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8339 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8340 | new_z_shifted=z+aligndz; |
8341 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8342 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8343 | diff_shifted=xy_shifted-wirepos_shifted; |
8344 | d_dOriginZ=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8345 | alignmentDerivatives[CDCTrackD::dDOCAdOriginZ] = (d_dOriginZ - d)/wposShift; |
8346 | if(DEBUG_HISTS){ |
8347 | alignDerivHists[26]->Fill(aligndz); |
8348 | alignDerivHists[37]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdOriginZ]); |
8349 | brentCheckHists[0]->Fill(aligndz,d_dOriginZ); |
8350 | } |
8351 | |
8352 | //dDirX |
8353 | alignS=Ss; |
8354 | aligndz=0.; |
8355 | shift.Set(wdirShift,0.); |
8356 | origin_shifted=origin; |
8357 | z0_shifted=z0wire; |
8358 | dir_shifted=dir+shift; |
8359 | cosstereo_shifted = cos((wireDir+DVector3(wdirShift,0.,0.)).Angle(DVector3(0.,0.,1.))); |
8360 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,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,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8363 | new_z_shifted=z+aligndz; |
8364 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8365 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8366 | diff_shifted=xy_shifted-wirepos_shifted; |
8367 | d_dDirX=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
8368 | alignmentDerivatives[CDCTrackD::dDOCAdDirX] = (d_dDirX - d)/wdirShift; |
8369 | if(DEBUG_HISTS){ |
8370 | alignDerivHists[27]->Fill(aligndz); |
8371 | alignDerivHists[38]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirX]); |
8372 | } |
8373 | |
8374 | //dDirY |
8375 | alignS=Ss; |
8376 | aligndz=0.; |
8377 | shift.Set(0.,wdirShift); |
8378 | origin_shifted=origin; |
8379 | z0_shifted=z0wire; |
8380 | dir_shifted=dir+shift; |
8381 | cosstereo_shifted = cos((wireDir+DVector3(0.,wdirShift,0.)).Angle(DVector3(0.,0.,1.))); |
8382 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8383 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8384 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8385 | new_z_shifted=z+aligndz; |
8386 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8387 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8388 | diff_shifted=xy_shifted-wirepos_shifted; |
8389 | d_dDirY=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
8390 | alignmentDerivatives[CDCTrackD::dDOCAdDirY] = (d_dDirY - d)/wdirShift; |
8391 | if(DEBUG_HISTS){ |
8392 | alignDerivHists[28]->Fill(aligndz); |
8393 | alignDerivHists[39]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirY]); |
8394 | } |
8395 | |
8396 | //dDirZ - This is divided out in this code |
8397 | alignS=Ss; |
8398 | aligndz=0.; |
8399 | origin_shifted=origin; |
8400 | dir_shifted.Set(wireDir.X()/(wireDir.Z()+wdirShift), wireDir.Y()/(wireDir.Z()+wdirShift)); |
8401 | z0_shifted=z0wire; |
8402 | cosstereo_shifted = cos((wireDir+DVector3(0.,0.,wdirShift)).Angle(DVector3(0.,0.,1.))); |
8403 | if(BrentForward(z,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8404 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8405 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0_shifted,origin_shifted,dir_shifted,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8406 | new_z_shifted=z+aligndz; |
8407 | wirepos_shifted=origin_shifted+(new_z_shifted-z0_shifted)*dir_shifted; |
8408 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8409 | diff_shifted=xy_shifted-wirepos_shifted; |
8410 | d_dDirZ=cosstereo_shifted*diff_shifted.Mod()+EPS3.0e-8; |
8411 | alignmentDerivatives[CDCTrackD::dDOCAdDirZ] = (d_dDirZ - d)/wdirShift; |
8412 | if(DEBUG_HISTS){ |
8413 | alignDerivHists[29]->Fill(aligndz); |
8414 | alignDerivHists[40]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdDirZ]); |
8415 | } |
8416 | |
8417 | // And now the derivatives wrt the track parameters |
8418 | |
8419 | DMatrix5x1 trackShiftS0(shift_x, 0., 0., 0., 0.); |
8420 | DMatrix5x1 trackShiftS1(0., shift_y, 0., 0., 0.); |
8421 | DMatrix5x1 trackShiftS2(0., 0., shift_tx, 0., 0.); |
8422 | DMatrix5x1 trackShiftS3(0., 0., 0., shift_ty, 0.); |
8423 | DMatrix5x1 trackShiftS4(0., 0., 0., 0., shift_q_over_p); |
8424 | |
8425 | // dS0 |
8426 | alignS=Ss+trackShiftS0; |
8427 | aligndz=0.; |
8428 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8429 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8430 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8431 | new_z_shifted=z+aligndz; |
8432 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8433 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8434 | diff_shifted=xy_shifted-wirepos_shifted; |
8435 | d_dS0=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8436 | alignmentDerivatives[CDCTrackD::dDOCAdS0] = (d_dS0 - d)/shift_x; |
8437 | if(DEBUG_HISTS){ |
8438 | alignDerivHists[30]->Fill(aligndz); |
8439 | alignDerivHists[41]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS0]); |
8440 | } |
8441 | |
8442 | // dS1 |
8443 | alignS=Ss+trackShiftS1; |
8444 | aligndz=0.; |
8445 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8446 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8447 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8448 | new_z_shifted=z+aligndz; |
8449 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8450 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8451 | diff_shifted=xy_shifted-wirepos_shifted; |
8452 | d_dS1=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8453 | alignmentDerivatives[CDCTrackD::dDOCAdS1] = (d_dS1 - d)/shift_y; |
8454 | if(DEBUG_HISTS){ |
8455 | alignDerivHists[31]->Fill(aligndz); |
8456 | alignDerivHists[42]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS1]); |
8457 | } |
8458 | |
8459 | // dS2 |
8460 | alignS=Ss+trackShiftS2; |
8461 | aligndz=0.; |
8462 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8463 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8464 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8465 | new_z_shifted=z+aligndz; |
8466 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8467 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8468 | diff_shifted=xy_shifted-wirepos_shifted; |
8469 | d_dS2=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8470 | alignmentDerivatives[CDCTrackD::dDOCAdS2] = (d_dS2 - d)/shift_tx; |
8471 | if(DEBUG_HISTS){ |
8472 | alignDerivHists[32]->Fill(aligndz); |
8473 | alignDerivHists[43]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS2]); |
8474 | } |
8475 | |
8476 | // dS3 |
8477 | alignS=Ss+trackShiftS3; |
8478 | aligndz=0.; |
8479 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8480 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8481 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8482 | new_z_shifted=z+aligndz; |
8483 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8484 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8485 | diff_shifted=xy_shifted-wirepos_shifted; |
8486 | d_dS3=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8487 | alignmentDerivatives[CDCTrackD::dDOCAdS3] = (d_dS3 - d)/shift_ty; |
8488 | if(DEBUG_HISTS){ |
8489 | alignDerivHists[33]->Fill(aligndz); |
8490 | alignDerivHists[44]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS3]); |
8491 | } |
8492 | |
8493 | // dS4 |
8494 | alignS=Ss+trackShiftS4; |
8495 | aligndz=0.; |
8496 | if(BrentForward(z,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8497 | if(aligndz < dzcut_min || aligndz > dzcut_max) return VALUE_OUT_OF_RANGE; |
8498 | //if(BrentsAlgorithm(z,-mStepSizeZ,dEdx,z0wire,origin,dir,alignS,aligndz) != NOERROR) return VALUE_OUT_OF_RANGE; |
8499 | new_z_shifted=z+aligndz; |
8500 | wirepos_shifted=origin+(new_z_shifted-z0wire)*dir; |
8501 | xy_shifted.Set(alignS(state_x),alignS(state_y)); |
8502 | diff_shifted=xy_shifted-wirepos_shifted; |
8503 | d_dS4=cosstereo*diff_shifted.Mod()+EPS3.0e-8; |
8504 | alignmentDerivatives[CDCTrackD::dDOCAdS4] = (d_dS4 - d)/shift_q_over_p; |
8505 | if(DEBUG_HISTS){ |
8506 | alignDerivHists[34]->Fill(aligndz); |
8507 | alignDerivHists[45]->Fill(alignmentDerivatives[CDCTrackD::dDOCAdS4]); |
8508 | } |
8509 | } |
8510 | |
8511 | // Find the field and gradient at (old_x,old_y,old_z) and compute the |
8512 | // Jacobian matrix for transforming from S to myS |
8513 | bfield->GetFieldAndGradient(Ss(state_x),Ss(state_y),z, |
8514 | Bx,By,Bz,dBxdx,dBxdy,dBxdz,dBydx, |
8515 | dBydy,dBydz,dBzdx,dBzdy,dBzdz); |
8516 | DMatrix5x5 Jc; |
8517 | StepJacobian(z,new_z,Ss,dEdx,Jc); |
8518 | |
8519 | // Find the projection matrix |
8520 | DMatrix5x1 H_T; |
8521 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
8522 | H_T(state_x)=diff.X()*cosstereo2_over_d; |
8523 | H_T(state_y)=diff.Y()*cosstereo2_over_d; |
8524 | DMatrix1x5 H; |
8525 | H(state_x)=H_T(state_x); |
8526 | H(state_y)=H_T(state_y); |
8527 | |
8528 | // Find the variance for this hit |
8529 | |
8530 | bool skip_ring=(hit->hit->wire->ring==RING_TO_SKIP); |
8531 | |
8532 | double V=update.variance; |
8533 | myC=Jc*myC*Jc.Transpose(); |
8534 | if (skip_ring) V+=H*myC*H_T; |
8535 | else V-=H*myC*H_T; |
8536 | |
8537 | if (DEBUG_LEVEL>1 && (!isfinite(V) || V < 0.0) ) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8537<<" " << " Problem: V is " << V << endl; |
8538 | |
8539 | double tx=Ss(state_tx); |
8540 | double ty=Ss(state_ty); |
8541 | double scale=1./sqrt(1.+tx*tx+ty*ty); |
8542 | double cosThetaRel=hit->hit->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale)); |
8543 | |
8544 | pull_t thisPull(update.doca-d,sqrt(V),traj.s,update.tdrift,d,hit->hit, |
8545 | NULL__null,diff.Phi(),new_z,cosThetaRel,update.tcorr); |
8546 | thisPull.AddTrackDerivatives(alignmentDerivatives); |
8547 | my_pulls.push_back(thisPull); |
8548 | return NOERROR; |
8549 | } |
8550 | |
8551 | // Transform the 5x5 covariance matrix from the forward parametrization to the |
8552 | // central parametrization. |
8553 | void DTrackFitterKalmanSIMD::TransformCovariance(DMatrix5x5 &C){ |
8554 | DMatrix5x5 J; |
8555 | double tsquare=tx_*tx_+ty_*ty_; |
8556 | double cosl=cos(atan(tanl_)); |
8557 | double tanl2=tanl_*tanl_; |
8558 | double tanl3=tanl2*tanl_; |
8559 | double factor=1./sqrt(1.+tsquare); |
8560 | J(state_z,state_x)=tx_/tsquare; |
8561 | J(state_z,state_y)=ty_/tsquare; |
8562 | double diff=tx_*tx_-ty_*ty_; |
8563 | double frac=1./(tsquare*tsquare); |
8564 | J(state_z,state_tx)=-(x_*diff+2.*tx_*ty_*y_)*frac; |
8565 | J(state_z,state_ty)=-(2.*tx_*ty_*x_-y_*diff)*frac; |
8566 | J(state_tanl,state_tx)=-tx_*tanl3; |
8567 | J(state_tanl,state_ty)=-ty_*tanl3; |
8568 | J(state_q_over_pt,state_q_over_p)=1./cosl; |
8569 | J(state_q_over_pt,state_tx)=-q_over_p_*tx_*tanl3*factor; |
8570 | J(state_q_over_pt,state_ty)=-q_over_p_*ty_*tanl3*factor; |
8571 | J(state_phi,state_tx)=-ty_*tanl2; |
8572 | J(state_phi,state_ty)=tx_*tanl2; |
8573 | J(state_D,state_x)=x_/D_; |
8574 | J(state_D,state_y)=y_/D_; |
8575 | |
8576 | C=J*C*J.Transpose(); |
8577 | |
8578 | } |
8579 | |
8580 | jerror_t DTrackFitterKalmanSIMD::BrentForward(double z, double dedx, const double z0w, |
8581 | const DVector2 &origin, const DVector2 &dir, DMatrix5x1 &S, double &dz){ |
8582 | |
8583 | DVector2 wirepos=origin; |
8584 | wirepos+=(z-z0w)*dir; |
8585 | double dx=S(state_x)-wirepos.X(); |
8586 | double dy=S(state_y)-wirepos.Y(); |
8587 | double doca2 = dx*dx+dy*dy; |
8588 | |
8589 | if (BrentsAlgorithm(z,-mStepSizeZ,dedx,z0w,origin,dir,S,dz)!=NOERROR){ |
8590 | return VALUE_OUT_OF_RANGE; |
8591 | } |
8592 | |
8593 | double newz = z+dz; |
8594 | unsigned int maxSteps=5; |
8595 | unsigned int stepCounter=0; |
8596 | if (fabs(dz)<EPS31.e-2){ |
8597 | // doca |
8598 | double old_doca2=doca2; |
8599 | |
8600 | double ztemp=newz; |
8601 | newz=ztemp-mStepSizeZ; |
8602 | Step(ztemp,newz,dedx,S); |
8603 | // new wire position |
8604 | wirepos=origin; |
8605 | wirepos+=(newz-z0w)*dir; |
8606 | |
8607 | dx=S(state_x)-wirepos.X(); |
8608 | dy=S(state_y)-wirepos.Y(); |
8609 | doca2=dx*dx+dy*dy; |
8610 | ztemp=newz; |
8611 | |
8612 | while(doca2<old_doca2 && stepCounter<maxSteps){ |
8613 | newz=ztemp-mStepSizeZ; |
8614 | old_doca2=doca2; |
8615 | |
8616 | // Step to the new z position |
8617 | Step(ztemp,newz,dedx,S); |
8618 | stepCounter++; |
8619 | |
8620 | // find the new distance to the wire |
8621 | wirepos=origin; |
8622 | wirepos+=(newz-z0w)*dir; |
8623 | |
8624 | dx=S(state_x)-wirepos.X(); |
8625 | dy=S(state_y)-wirepos.Y(); |
8626 | doca2=dx*dx+dy*dy; |
8627 | |
8628 | ztemp=newz; |
8629 | } |
8630 | |
8631 | // Find the true doca |
8632 | double dz2=0.; |
8633 | if (BrentsAlgorithm(newz,-mStepSizeZ,dedx,z0w,origin,dir,S,dz2)!=NOERROR){ |
8634 | return VALUE_OUT_OF_RANGE; |
8635 | } |
8636 | newz=ztemp+dz2; |
8637 | |
8638 | // Change in z relative to where we started for this wire |
8639 | dz=newz-z; |
8640 | } |
8641 | else if (fabs(dz)>2.*mStepSizeZ-EPS31.e-2){ |
8642 | // whoops, looks like we didn't actually bracket the minimum |
8643 | // after all. Swim to make sure we pass the minimum doca. |
8644 | |
8645 | double ztemp=newz; |
8646 | // new wire position |
8647 | wirepos=origin; |
8648 | wirepos+=(ztemp-z0w)*dir; |
8649 | |
8650 | // doca |
8651 | double old_doca2=doca2; |
8652 | |
8653 | dx=S(state_x)-wirepos.X(); |
8654 | dy=S(state_y)-wirepos.Y(); |
8655 | doca2=dx*dx+dy*dy; |
8656 | |
8657 | while(doca2<old_doca2 && stepCounter<10*maxSteps){ |
8658 | newz=ztemp+mStepSizeZ; |
8659 | old_doca2=doca2; |
8660 | |
8661 | // Step to the new z position |
8662 | Step(ztemp,newz,dedx,S); |
8663 | stepCounter++; |
8664 | |
8665 | // find the new distance to the wire |
8666 | wirepos=origin; |
8667 | wirepos+=(newz-z0w)*dir; |
8668 | |
8669 | dx=S(state_x)-wirepos.X(); |
8670 | dy=S(state_y)-wirepos.Y(); |
8671 | doca2=dx*dx+dy*dy; |
8672 | |
8673 | ztemp=newz; |
8674 | } |
8675 | |
8676 | // Find the true doca |
8677 | double dz2=0.; |
8678 | if (BrentsAlgorithm(newz,mStepSizeZ,dedx,z0w,origin,dir,S,dz2)!=NOERROR){ |
8679 | return VALUE_OUT_OF_RANGE; |
8680 | } |
8681 | newz=ztemp+dz2; |
8682 | |
8683 | // Change in z relative to where we started for this wire |
8684 | dz=newz-z; |
8685 | } |
8686 | return NOERROR; |
8687 | } |
8688 | |
8689 | jerror_t DTrackFitterKalmanSIMD::BrentCentral(double dedx, DVector2 &xy, const double z0w, const DVector2 &origin, const DVector2 &dir, DMatrix5x1 &Sc, double &ds){ |
8690 | |
8691 | DVector2 wirexy=origin; |
8692 | wirexy+=(Sc(state_z)-z0w)*dir; |
8693 | |
8694 | // new doca |
8695 | double doca2=(xy-wirexy).Mod2(); |
8696 | double old_doca2=doca2; |
8697 | |
8698 | if (BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dedx,xy,z0w, |
8699 | origin,dir,Sc,ds)!=NOERROR){ |
8700 | return VALUE_OUT_OF_RANGE; |
8701 | } |
8702 | |
8703 | unsigned int maxSteps=3; |
8704 | unsigned int stepCounter=0; |
8705 | |
8706 | if (fabs(ds)<EPS31.e-2){ |
8707 | double my_ds=ds; |
8708 | old_doca2=doca2; |
8709 | Step(xy,-mStepSizeS,Sc,dedx); |
8710 | my_ds-=mStepSizeS; |
8711 | wirexy=origin; |
8712 | wirexy+=(Sc(state_z)-z0w)*dir; |
8713 | doca2=(xy-wirexy).Mod2(); |
8714 | while(doca2<old_doca2 && stepCounter<maxSteps){ |
8715 | old_doca2=doca2; |
8716 | // Bail if the transverse momentum has dropped below some minimum |
8717 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
8718 | return VALUE_OUT_OF_RANGE; |
8719 | } |
8720 | |
8721 | // Step through the field |
8722 | Step(xy,-mStepSizeS,Sc,dedx); |
8723 | stepCounter++; |
8724 | |
8725 | wirexy=origin; |
8726 | wirexy+=(Sc(state_z)-z0w)*dir; |
8727 | doca2=(xy-wirexy).Mod2(); |
8728 | |
8729 | my_ds-=mStepSizeS; |
8730 | } |
8731 | // Swim to the "true" doca |
8732 | double ds2=0.; |
8733 | if (BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dedx,xy,z0w, |
8734 | origin,dir,Sc,ds2)!=NOERROR){ |
8735 | return VALUE_OUT_OF_RANGE; |
8736 | } |
8737 | ds=my_ds+ds2; |
8738 | } |
8739 | else if (fabs(ds)>2*mStepSizeS-EPS31.e-2){ |
8740 | double my_ds=ds; |
8741 | |
8742 | // new wire position |
8743 | wirexy=origin; |
8744 | wirexy+=(Sc(state_z)-z0w)*dir; |
8745 | |
8746 | // doca |
8747 | old_doca2=doca2; |
8748 | doca2=(xy-wirexy).Mod2(); |
8749 | |
8750 | while(doca2<old_doca2 && stepCounter<maxSteps){ |
8751 | old_doca2=doca2; |
8752 | |
8753 | // Bail if the transverse momentum has dropped below some minimum |
8754 | if (fabs(Sc(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
8755 | return VALUE_OUT_OF_RANGE; |
8756 | } |
8757 | |
8758 | // Step through the field |
8759 | Step(xy,mStepSizeS,Sc,dedx); |
8760 | stepCounter++; |
8761 | |
8762 | // Find the new distance to the wire |
8763 | wirexy=origin; |
8764 | wirexy+=(Sc(state_z)-z0w)*dir; |
8765 | doca2=(xy-wirexy).Mod2(); |
8766 | |
8767 | my_ds+=mStepSizeS; |
8768 | } |
8769 | // Swim to the "true" doca |
8770 | double ds2=0.; |
8771 | if (BrentsAlgorithm(mStepSizeS,mStepSizeS,dedx,xy,z0w, |
8772 | origin,dir,Sc,ds2)!=NOERROR){ |
8773 | return VALUE_OUT_OF_RANGE; |
8774 | } |
8775 | ds=my_ds+ds2; |
8776 | } |
8777 | return NOERROR; |
8778 | } |
8779 | |
8780 | // Find extrapolations to detectors outside of the tracking volume |
8781 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToOuterDetectors(const DMatrix5x1 &S0){ |
8782 | DMatrix5x1 S=S0; |
8783 | // Energy loss |
8784 | double dEdx=0.; |
8785 | |
8786 | // material properties |
8787 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
8788 | |
8789 | // Position variables |
8790 | double z=forward_traj[0].z; |
8791 | double newz=z,dz=0.; |
Value stored to 'newz' during its initialization is never read | |
8792 | |
8793 | // Current time and path length |
8794 | double t=forward_traj[0].t; |
8795 | double s=forward_traj[0].s; |
8796 | |
8797 | // Store the position and momentum at the exit to the tracking volume |
8798 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
8799 | double tanl=1./sqrt(tsquare); |
8800 | double cosl=cos(atan(tanl)); |
8801 | double pt=cosl/fabs(S(state_q_over_p)); |
8802 | double phi=atan2(S(state_ty),S(state_tx)); |
8803 | DVector3 position(S(state_x),S(state_y),z); |
8804 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
8805 | extrapolations[SYS_NULL].push_back(Extrapolation_t(position,momentum, |
8806 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02, |
8807 | s)); |
8808 | |
8809 | // Loop to propagate track to outer detectors |
8810 | const double z_outer_max=650.; |
8811 | const double x_max=130.; |
8812 | const double y_max=130.; |
8813 | bool hit_tof=false; |
8814 | bool hit_dirc=false; |
8815 | unsigned int trd_index=0; |
8816 | while (z>Z_MIN-100. && z<z_outer_max && fabs(S(state_x))<x_max |
8817 | && fabs(S(state_y))<y_max){ |
8818 | // Bail if the momentum has dropped below some minimum |
8819 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
8820 | if (DEBUG_LEVEL>2) |
8821 | { |
8822 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8822<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
8823 | << endl; |
8824 | } |
8825 | return VALUE_OUT_OF_RANGE; |
8826 | } |
8827 | |
8828 | // Check if we have passed into the BCAL |
8829 | double r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
8830 | if (r2>89.*89. && z<400.) return VALUE_OUT_OF_RANGE; |
8831 | if (r2>64.9*64.9 && r2<89.*89.){ |
8832 | if (extrapolations.at(SYS_BCAL).size()>299){ |
8833 | return VALUE_OUT_OF_RANGE; |
8834 | } |
8835 | if (z<406.){ |
8836 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
8837 | double tanl=1./sqrt(tsquare); |
8838 | double cosl=cos(atan(tanl)); |
8839 | double pt=cosl/fabs(S(state_q_over_p)); |
8840 | double phi=atan2(S(state_ty),S(state_tx)); |
8841 | DVector3 position(S(state_x),S(state_y),z); |
8842 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
8843 | extrapolations[SYS_BCAL].push_back(Extrapolation_t(position,momentum, |
8844 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
8845 | } |
8846 | else if (extrapolations.at(SYS_BCAL).size()<5){ |
8847 | // There needs to be some steps inside the the volume of the BCAL for |
8848 | // the extrapolation to be useful. If this is not the case, clear |
8849 | // the extrapolation vector. |
8850 | extrapolations[SYS_BCAL].clear(); |
8851 | } |
8852 | } |
8853 | |
8854 | // Relationship between arc length and z |
8855 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx) |
8856 | +S(state_ty)*S(state_ty)); |
8857 | |
8858 | // get material properties from the Root Geometry |
8859 | DVector3 pos(S(state_x),S(state_y),z); |
8860 | DVector3 dir(S(state_tx),S(state_ty),1.); |
8861 | double s_to_boundary=0.; |
8862 | if (geom->FindMatKalman(pos,dir,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
8863 | last_material_map,&s_to_boundary) |
8864 | !=NOERROR){ |
8865 | if (DEBUG_LEVEL>0) |
8866 | { |
8867 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8867<<" " << "Material error in ExtrapolateForwardToOuterDetectors!"<< endl; |
8868 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<8868<<" " << " Position (x,y,z)=("<<pos.x()<<","<<pos.y()<<","<<pos.z()<<")" |
8869 | <<endl; |
8870 | } |
8871 | return VALUE_OUT_OF_RANGE; |
8872 | } |
8873 | |
8874 | // Get dEdx for the upcoming step |
8875 | if (CORRECT_FOR_ELOSS){ |
8876 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
8877 | } |
8878 | |
8879 | // Adjust the step size |
8880 | double ds=mStepSizeS; |
8881 | if (fabs(dEdx)>EPS3.0e-8){ |
8882 | ds=DE_PER_STEP0.001/fabs(dEdx); |
8883 | } |
8884 | if (ds>mStepSizeS) ds=mStepSizeS; |
8885 | if (s_to_boundary<ds) ds=s_to_boundary; |
8886 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
8887 | if (ds<0.5 && z<406. && r2>65.*65.) ds=0.5; |
8888 | dz=ds*dz_ds; |
8889 | newz=z+dz; |
8890 | if (trd_index<dTRDz_vec.size() && newz>dTRDz_vec[trd_index]){ |
8891 | newz=dTRDz_vec[trd_index]+EPS3.0e-8; |
8892 | ds=(newz-z)/dz_ds; |
8893 | } |
8894 | if (hit_tof==false && newz>dTOFz){ |
8895 | newz=dTOFz+EPS3.0e-8; |
8896 | ds=(newz-z)/dz_ds; |
8897 | } |
8898 | if (hit_dirc==false && newz>dDIRCz){ |
8899 | newz=dDIRCz+EPS3.0e-8; |
8900 | ds=(newz-z)/dz_ds; |
8901 | } |
8902 | if (newz>dFCALz){ |
8903 | newz=dFCALz+EPS3.0e-8; |
8904 | ds=(newz-z)/dz_ds; |
8905 | } |
8906 | s+=ds; |
8907 | |
8908 | // Flight time |
8909 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
8910 | double one_over_beta2=1.+mass2*q_over_p_sq; |
8911 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
8912 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
8913 | |
8914 | // Step through field |
8915 | Step(z,newz,dEdx,S); |
8916 | z=newz; |
8917 | |
8918 | if (trd_index<dTRDz_vec.size() && newz>dTRDz_vec[trd_index]){ |
8919 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
8920 | double tanl=1./sqrt(tsquare); |
8921 | double cosl=cos(atan(tanl)); |
8922 | double pt=cosl/fabs(S(state_q_over_p)); |
8923 | double phi=atan2(S(state_ty),S(state_tx)); |
8924 | DVector3 position(S(state_x),S(state_y),z); |
8925 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
8926 | //position.Print(); |
8927 | extrapolations[SYS_TRD].push_back(Extrapolation_t(position,momentum, |
8928 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
8929 | trd_index++; |
8930 | } |
8931 | if (hit_dirc==false && newz>dDIRCz){ |
8932 | hit_dirc=true; |
8933 | |
8934 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
8935 | double tanl=1./sqrt(tsquare); |
8936 | double cosl=cos(atan(tanl)); |
8937 | double pt=cosl/fabs(S(state_q_over_p)); |
8938 | double phi=atan2(S(state_ty),S(state_tx)); |
8939 | DVector3 position(S(state_x),S(state_y),z); |
8940 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
8941 | extrapolations[SYS_DIRC].push_back(Extrapolation_t(position,momentum, |
8942 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
8943 | } |
8944 | if (hit_tof==false && newz>dTOFz){ |
8945 | hit_tof=true; |
8946 | |
8947 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
8948 | double tanl=1./sqrt(tsquare); |
8949 | double cosl=cos(atan(tanl)); |
8950 | double pt=cosl/fabs(S(state_q_over_p)); |
8951 | double phi=atan2(S(state_ty),S(state_tx)); |
8952 | DVector3 position(S(state_x),S(state_y),z); |
8953 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
8954 | extrapolations[SYS_TOF].push_back(Extrapolation_t(position,momentum, |
8955 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
8956 | } |
8957 | if (newz>dFCALz){ |
8958 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
8959 | double tanl=1./sqrt(tsquare); |
8960 | double cosl=cos(atan(tanl)); |
8961 | double pt=cosl/fabs(S(state_q_over_p)); |
8962 | double phi=atan2(S(state_ty),S(state_tx)); |
8963 | DVector3 position(S(state_x),S(state_y),z); |
8964 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
8965 | extrapolations[SYS_FCAL].push_back(Extrapolation_t(position,momentum, |
8966 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
8967 | |
8968 | // add another extrapolation point at downstream end of FCAL |
8969 | double zend=newz+45.; |
8970 | Step(newz,zend,dEdx,S); |
8971 | ds=(zend-newz)/dz_ds; |
8972 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
8973 | s+=ds; |
8974 | tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
8975 | tanl=1./sqrt(tsquare); |
8976 | cosl=cos(atan(tanl)); |
8977 | pt=cosl/fabs(S(state_q_over_p)); |
8978 | phi=atan2(S(state_ty),S(state_tx)); |
8979 | position.SetXYZ(S(state_x),S(state_y),zend); |
8980 | momentum.SetXYZ(pt*cos(phi),pt*sin(phi),pt*tanl); |
8981 | extrapolations[SYS_FCAL].push_back(Extrapolation_t(position,momentum, |
8982 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
8983 | |
8984 | return NOERROR; |
8985 | } |
8986 | } |
8987 | return NOERROR; |
8988 | } |
8989 | |
8990 | // Find extrapolations to detector layers within the tracking volume and |
8991 | // inward (toward the target). |
8992 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateToInnerDetectors(){ |
8993 | // First deal with start counter. Only do this if the track has a chance |
8994 | // to intersect with the start counter volume. |
8995 | unsigned int inner_index=forward_traj.size()-1; |
8996 | unsigned int index_beyond_start_counter=inner_index; |
8997 | DMatrix5x1 S=forward_traj[inner_index].S; |
8998 | bool intersected_start_counter=false; |
8999 | if (sc_norm.empty()==false |
9000 | && S(state_x)*S(state_x)+S(state_y)*S(state_y)<SC_BARREL_R2 |
9001 | && forward_traj[inner_index].z<SC_END_NOSE_Z){ |
9002 | double d_old=1000.,d=1000.,z=0.; |
9003 | unsigned int index=0; |
9004 | for (unsigned int m=0;m<12;m++){ |
9005 | unsigned int k=inner_index; |
9006 | for (;k>1;k--){ |
9007 | S=forward_traj[k].S; |
9008 | z=forward_traj[k].z; |
9009 | |
9010 | double dphi=atan2(S(state_y),S(state_x))-SC_PHI_SECTOR1; |
9011 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
9012 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
9013 | if (index>29) index=0; |
9014 | d=sc_norm[index][m].Dot(DVector3(S(state_x),S(state_y),z) |
9015 | -sc_pos[index][m]); |
9016 | if (d*d_old<0){ // break if we cross the current plane |
9017 | if (m==0) index_beyond_start_counter=k; |
9018 | break; |
9019 | } |
9020 | d_old=d; |
9021 | } |
9022 | // if the z position would be beyond the current segment along z of |
9023 | // the start counter, move to the next plane |
9024 | if (z>sc_pos[index][m+1].z()&&m<11){ |
9025 | continue; |
9026 | } |
9027 | // allow for a little slop at the end of the nose |
9028 | else if (z<sc_pos[index][sc_pos[0].size()-1].z()+0.1){ |
9029 | // Hone in on intersection with the appropriate segment of the start |
9030 | // counter |
9031 | int count=0; |
9032 | DMatrix5x1 bestS=S; |
9033 | double dmin=d; |
9034 | double bestz=z; |
9035 | double t=forward_traj[k].t; |
9036 | double s=forward_traj[k].s; |
9037 | // Magnetic field |
9038 | bfield->GetField(S(state_x),S(state_y),z,Bx,By,Bz); |
9039 | |
9040 | while (fabs(d)>0.001 && count<20){ |
9041 | // track direction |
9042 | DVector3 phat(S(state_tx),S(state_ty),1); |
9043 | phat.SetMag(1.); |
9044 | |
9045 | // Step to the start counter plane |
9046 | double ds=d/sc_norm[index][m].Dot(phat); |
9047 | FastStep(z,-ds,0.,S); |
9048 | |
9049 | // Flight time |
9050 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
9051 | double one_over_beta2=1.+mass2*q_over_p_sq; |
9052 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
9053 | t-=ds*sqrt(one_over_beta2); // in units where c=1 |
9054 | s-=ds; |
9055 | |
9056 | // Find the index for the nearest start counter paddle |
9057 | double dphi=atan2(S(state_y),S(state_x))-SC_PHI_SECTOR1; |
9058 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
9059 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
9060 | |
9061 | // Find the new distance to the start counter (which could now be to |
9062 | // a plane in the one adjacent to the one before the step...) |
9063 | d=sc_norm[index][m].Dot(DVector3(S(state_x),S(state_y),z) |
9064 | -sc_pos[index][m]); |
9065 | if (fabs(d)<fabs(dmin)){ |
9066 | bestS=S; |
9067 | dmin=d; |
9068 | bestz=z; |
9069 | } |
9070 | count++; |
9071 | } |
9072 | |
9073 | // New position and momentum |
9074 | double tsquare=bestS(state_tx)*bestS(state_tx) |
9075 | +bestS(state_ty)*bestS(state_ty); |
9076 | double tanl=1./sqrt(tsquare); |
9077 | double cosl=cos(atan(tanl)); |
9078 | double pt=cosl/fabs(bestS(state_q_over_p)); |
9079 | double phi=atan2(bestS(state_ty),bestS(state_tx)); |
9080 | DVector3 position(bestS(state_x),bestS(state_y),bestz); |
9081 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
9082 | extrapolations[SYS_START].push_back(Extrapolation_t(position,momentum, |
9083 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
9084 | |
9085 | //printf("forward track:\n"); |
9086 | //position.Print(); |
9087 | intersected_start_counter=true; |
9088 | break; |
9089 | } |
9090 | } |
9091 | } |
9092 | // Accumulate multiple-scattering terms for use in matching routines |
9093 | double s_theta_ms_sum=0.; |
9094 | double theta2ms_sum=0.; |
9095 | if (intersected_start_counter){ |
9096 | for (unsigned int k=inner_index;k>index_beyond_start_counter;k--){ |
9097 | double ds_theta_ms_sq=3.*fabs(forward_traj[k].Q(state_x,state_x)); |
9098 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9099 | double ds=forward_traj[k].s-forward_traj[k-1].s; |
9100 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9101 | } |
9102 | } |
9103 | |
9104 | // Deal with points within fiducial volume of chambers |
9105 | unsigned int fdc_plane=0; |
9106 | mT0Detector=SYS_NULL; |
9107 | mT0MinimumDriftTime=1e6; |
9108 | for (int k=intersected_start_counter?index_beyond_start_counter:inner_index;k>=0;k--){ |
9109 | double z=forward_traj[k].z; |
9110 | double t=forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9111 | double s=forward_traj[k].s; |
9112 | DMatrix5x1 S=forward_traj[k].S; |
9113 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
9114 | double tanl=1./sqrt(tsquare); |
9115 | double cosl=cos(atan(tanl)); |
9116 | double pt=cosl/fabs(S(state_q_over_p)); |
9117 | double phi=atan2(S(state_ty),S(state_tx)); |
9118 | |
9119 | // Find estimate for t0 using earliest drift time |
9120 | if (forward_traj[k].h_id>999){ |
9121 | unsigned int index=forward_traj[k].h_id-1000; |
9122 | double dt=my_cdchits[index]->tdrift-t; |
9123 | if (dt<mT0MinimumDriftTime){ |
9124 | mT0MinimumDriftTime=dt; |
9125 | mT0Detector=SYS_CDC; |
9126 | } |
9127 | } |
9128 | else if (forward_traj[k].h_id>0){ |
9129 | unsigned int index=forward_traj[k].h_id-1; |
9130 | double dt=my_fdchits[index]->t-t; |
9131 | if (dt<mT0MinimumDriftTime){ |
9132 | mT0MinimumDriftTime=dt; |
9133 | mT0Detector=SYS_FDC; |
9134 | } |
9135 | } |
9136 | |
9137 | //multiple scattering terms |
9138 | if (k>0){ |
9139 | double ds_theta_ms_sq=3.*fabs(forward_traj[k].Q(state_x,state_x)); |
9140 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9141 | double ds=forward_traj[k].s-forward_traj[k-1].s; |
9142 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9143 | } |
9144 | // Extrapolations in CDC region |
9145 | if (z<endplate_z_downstream){ |
9146 | DVector3 position(S(state_x),S(state_y),z); |
9147 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
9148 | extrapolations[SYS_CDC].push_back(Extrapolation_t(position,momentum, |
9149 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s, |
9150 | s_theta_ms_sum,theta2ms_sum)); |
9151 | |
9152 | } |
9153 | else{ // extrapolations in FDC region |
9154 | // output step near wire plane |
9155 | if (z>fdc_z_wires[fdc_plane]-0.25){ |
9156 | double dz=z-fdc_z_wires[fdc_plane]; |
9157 | //printf("extrp dz %f\n",dz); |
9158 | if (fabs(dz)>EPS21.e-4){ |
9159 | Step(z,fdc_z_wires[fdc_plane],0.,S); |
9160 | tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
9161 | tanl=1./sqrt(tsquare); |
9162 | cosl=cos(atan(tanl)); |
9163 | pt=cosl/fabs(S(state_q_over_p)); |
9164 | phi=atan2(S(state_ty),S(state_tx)); |
9165 | } |
9166 | DVector3 position(S(state_x),S(state_y),fdc_z_wires[fdc_plane]); |
9167 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
9168 | extrapolations[SYS_FDC].push_back(Extrapolation_t(position,momentum, |
9169 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s, |
9170 | s_theta_ms_sum,theta2ms_sum)); |
9171 | if (fdc_plane==23){ |
9172 | return NOERROR; |
9173 | } |
9174 | |
9175 | fdc_plane++; |
9176 | } |
9177 | } |
9178 | } |
9179 | |
9180 | |
9181 | //-------------------------------------------------------------------------- |
9182 | // The following code continues the extrapolations to wire planes that were |
9183 | // not included in the forward trajectory |
9184 | //-------------------------------------------------------------------------- |
9185 | |
9186 | // Material properties |
9187 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
9188 | double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.; |
9189 | |
9190 | // Energy loss |
9191 | double dEdx=0.; |
9192 | |
9193 | // multiple scattering matrix |
9194 | DMatrix5x5 Q; |
9195 | |
9196 | // Position variables |
9197 | S=forward_traj[0].S; |
9198 | double z=forward_traj[0].z,newz=z,dz=0.; |
9199 | |
9200 | // Current time and path length |
9201 | double t=forward_traj[0].t; |
9202 | double s=forward_traj[0].s; |
9203 | |
9204 | // Find intersection points to FDC planes beyond the exent of the forward |
9205 | // trajectory. |
9206 | while (z>Z_MIN-100. && z<fdc_z_wires[23]+1. && fdc_plane<24){ |
9207 | // Bail if the momentum has dropped below some minimum |
9208 | if (fabs(S(state_q_over_p))>Q_OVER_P_MAX){ |
9209 | if (DEBUG_LEVEL>2) |
9210 | { |
9211 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9211<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) |
9212 | << endl; |
9213 | } |
9214 | return VALUE_OUT_OF_RANGE; |
9215 | } |
9216 | |
9217 | // Current position |
9218 | DVector3 pos(S(state_x),S(state_y),z); |
9219 | if (pos.Perp()>50.) break; |
9220 | |
9221 | // get material properties from the Root Geometry |
9222 | DVector3 dir(S(state_tx),S(state_ty),1.); |
9223 | double s_to_boundary=0.; |
9224 | if (geom->FindMatKalman(pos,dir,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
9225 | chi2c_factor,chi2a_factor,chi2a_corr, |
9226 | last_material_map,&s_to_boundary) |
9227 | !=NOERROR){ |
9228 | if (DEBUG_LEVEL>0) |
9229 | { |
9230 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9230<<" " << "Material error in ExtrapolateForwardToOuterDetectors!"<< endl; |
9231 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9231<<" " << " Position (x,y,z)=("<<pos.x()<<","<<pos.y()<<","<<pos.z()<<")" |
9232 | <<endl; |
9233 | } |
9234 | return VALUE_OUT_OF_RANGE; |
9235 | } |
9236 | |
9237 | // Get dEdx for the upcoming step |
9238 | if (CORRECT_FOR_ELOSS){ |
9239 | dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
9240 | } |
9241 | |
9242 | // Relationship between arc length and z |
9243 | double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
9244 | |
9245 | // Adjust the step size |
9246 | double ds=mStepSizeS; |
9247 | if (fabs(dEdx)>EPS3.0e-8){ |
9248 | ds=DE_PER_STEP0.001/fabs(dEdx); |
9249 | } |
9250 | if (ds>mStepSizeS) ds=mStepSizeS; |
9251 | if (s_to_boundary<ds) ds=s_to_boundary; |
9252 | if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1; |
9253 | dz=ds*dz_ds; |
9254 | newz=z+dz; |
9255 | |
9256 | bool got_fdc_hit=false; |
9257 | if (fdc_plane<24 && newz>fdc_z_wires[fdc_plane]){ |
9258 | newz=fdc_z_wires[fdc_plane]; |
9259 | ds=(newz-z)/dz_ds; |
9260 | got_fdc_hit=true; |
9261 | } |
9262 | s+=ds; |
9263 | |
9264 | // Flight time |
9265 | double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p); |
9266 | double one_over_beta2=1.+mass2*q_over_p_sq; |
9267 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
9268 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
9269 | |
9270 | // Get the contribution to the covariance matrix due to multiple |
9271 | // scattering |
9272 | GetProcessNoise(z,ds,chi2c_factor,chi2a_factor,chi2a_corr,S,Q); |
9273 | double ds_theta_ms_sq=3.*fabs(Q(state_x,state_x)); |
9274 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9275 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9276 | |
9277 | // Step through field |
9278 | Step(z,newz,dEdx,S); |
9279 | z=newz; |
9280 | |
9281 | if (got_fdc_hit){ |
9282 | double tsquare=S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty); |
9283 | double tanl=1./sqrt(tsquare); |
9284 | double cosl=cos(atan(tanl)); |
9285 | double pt=cosl/fabs(S(state_q_over_p)); |
9286 | double phi=atan2(S(state_ty),S(state_tx)); |
9287 | DVector3 position(S(state_x),S(state_y),z); |
9288 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
9289 | extrapolations[SYS_FDC].push_back(Extrapolation_t(position,momentum, |
9290 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s, |
9291 | s_theta_ms_sum,theta2ms_sum)); |
9292 | |
9293 | fdc_plane++; |
9294 | } |
9295 | } |
9296 | |
9297 | return NOERROR; |
9298 | } |
9299 | |
9300 | // Routine to find intersections with surfaces useful at a later stage for track |
9301 | // matching |
9302 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateForwardToOtherDetectors(){ |
9303 | if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE; |
9304 | //-------------------------------- |
9305 | // First swim to Start counter and CDC/FDC layers |
9306 | //-------------------------------- |
9307 | jerror_t error=ExtrapolateToInnerDetectors(); |
9308 | |
9309 | //-------------------------------- |
9310 | // Next swim to outer detectors... |
9311 | //-------------------------------- |
9312 | if (error==NOERROR){ |
9313 | return ExtrapolateToOuterDetectors(forward_traj[0].S); |
9314 | } |
9315 | |
9316 | return error; |
9317 | } |
9318 | |
9319 | // Routine to find intersections with surfaces useful at a later stage for track |
9320 | // matching |
9321 | jerror_t DTrackFitterKalmanSIMD::ExtrapolateCentralToOtherDetectors(){ |
9322 | if (central_traj.size()<2) return RESOURCE_UNAVAILABLE; |
9323 | |
9324 | // First deal with start counter. Only do this if the track has a chance |
9325 | // to intersect with the start counter volume. |
9326 | unsigned int inner_index=central_traj.size()-1; |
9327 | unsigned int index_beyond_start_counter=inner_index; |
9328 | DVector2 xy=central_traj[inner_index].xy; |
9329 | DMatrix5x1 S=central_traj[inner_index].S; |
9330 | if (sc_norm.empty()==false |
9331 | &&xy.Mod2()<SC_BARREL_R2&& S(state_z)<SC_END_NOSE_Z){ |
9332 | double d_old=1000.,d=1000.,z=0.; |
9333 | unsigned int index=0; |
9334 | for (unsigned int m=0;m<12;m++){ |
9335 | unsigned int k=inner_index; |
9336 | for (;k>1;k--){ |
9337 | S=central_traj[k].S; |
9338 | z=S(state_z); |
9339 | xy=central_traj[k].xy; |
9340 | |
9341 | double dphi=xy.Phi()-SC_PHI_SECTOR1; |
9342 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
9343 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
9344 | if (index>29) index=0; |
9345 | //cout << "dphi " << dphi << " " << index << endl; |
9346 | |
9347 | d=sc_norm[index][m].Dot(DVector3(xy.X(),xy.Y(),z) |
9348 | -sc_pos[index][m]); |
9349 | |
9350 | if (d*d_old<0){ // break if we cross the current plane |
9351 | if (m==0) index_beyond_start_counter=k; |
9352 | break; |
9353 | } |
9354 | d_old=d; |
9355 | } |
9356 | // if the z position would be beyond the current segment along z of |
9357 | // the start counter, move to the next plane |
9358 | if (z>sc_pos[index][m+1].z()&&m<11){ |
9359 | continue; |
9360 | } |
9361 | // allow for a little slop at the end of the nose |
9362 | else if (z<sc_pos[index][sc_pos[0].size()-1].z()+0.1){ |
9363 | // Propagate the state and covariance through the field |
9364 | // using a straight-line approximation for each step to zero in on the |
9365 | // start counter paddle |
9366 | int count=0; |
9367 | DMatrix5x1 bestS=S; |
9368 | double dmin=d; |
9369 | DVector2 bestXY=central_traj[k].xy; |
9370 | double t=central_traj[k].t; |
9371 | double s=central_traj[k].s; |
9372 | // Magnetic field |
9373 | bfield->GetField(xy.X(),xy.Y(),S(state_z),Bx,By,Bz); |
9374 | |
9375 | while (fabs(d)>0.05 && count<20){ |
9376 | // track direction |
9377 | DVector3 phat(cos(S(state_phi)),sin(S(state_phi)),S(state_tanl)); |
9378 | phat.SetMag(1.); |
9379 | |
9380 | // path length increment |
9381 | double ds=d/sc_norm[index][m].Dot(phat); |
9382 | s-=ds; |
9383 | |
9384 | // Flight time |
9385 | double q_over_p=S(state_q_over_pt)*cos(atan(S(state_tanl))); |
9386 | double q_over_p_sq=q_over_p*q_over_p; |
9387 | double one_over_beta2=1.+mass2*q_over_p_sq; |
9388 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
9389 | t-=ds*sqrt(one_over_beta2); // in units where c=1 |
9390 | |
9391 | // Step along the trajectory using d to estimate path length |
9392 | FastStep(xy,-ds,0.,S); |
9393 | // Find the index for the nearest start counter paddle |
9394 | double dphi=xy.Phi()-SC_PHI_SECTOR1; |
9395 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
9396 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
9397 | if (index>29) index=0; |
9398 | |
9399 | // Find the new distance to the start counter (which could now be to |
9400 | // a plane in the one adjacent to the one before the step...) |
9401 | d=sc_norm[index][m].Dot(DVector3(xy.X(),xy.Y(),S(state_z)) |
9402 | -sc_pos[index][m]); |
9403 | if (fabs(d)<fabs(dmin)){ |
9404 | bestS=S; |
9405 | dmin=d; |
9406 | bestXY=xy; |
9407 | } |
9408 | count++; |
9409 | } |
9410 | |
9411 | if (bestS(state_z)>sc_pos[0][0].z()-0.1){ |
9412 | double tanl=bestS(state_tanl); |
9413 | double pt=1/fabs(bestS(state_q_over_pt)); |
9414 | double phi=bestS(state_phi); |
9415 | DVector3 position(bestXY.X(),bestXY.Y(),bestS(state_z)); |
9416 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
9417 | extrapolations[SYS_START].push_back(Extrapolation_t(position,momentum, |
9418 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
9419 | //printf("Central track:\n"); |
9420 | //position.Print(); |
9421 | } |
9422 | break; |
9423 | } |
9424 | } |
9425 | } |
9426 | |
9427 | // Accumulate multiple-scattering terms for use in matching routines |
9428 | double s_theta_ms_sum=0.,theta2ms_sum=0.; |
9429 | for (unsigned int k=inner_index;k>index_beyond_start_counter;k--){ |
9430 | double ds_theta_ms_sq=3.*fabs(central_traj[k].Q(state_D,state_D)); |
9431 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9432 | double ds=central_traj[k].s-central_traj[k-1].s; |
9433 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9434 | } |
9435 | |
9436 | // Deal with points within fiducial volume of chambers |
9437 | mT0Detector=SYS_NULL; |
9438 | mT0MinimumDriftTime=1e6; |
9439 | for (int k=index_beyond_start_counter;k>=0;k--){ |
9440 | S=central_traj[k].S; |
9441 | xy=central_traj[k].xy; |
9442 | double t=central_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02; // convert to ns |
9443 | double s=central_traj[k].s; |
9444 | double tanl=S(state_tanl); |
9445 | double pt=1/fabs(S(state_q_over_pt)); |
9446 | double phi=S(state_phi); |
9447 | |
9448 | // Find estimate for t0 using earliest drift time |
9449 | if (central_traj[k].h_id>0){ |
9450 | unsigned int index=central_traj[k].h_id-1; |
9451 | double dt=my_cdchits[index]->tdrift-t; |
9452 | if (dt<mT0MinimumDriftTime){ |
9453 | mT0MinimumDriftTime=dt; |
9454 | mT0Detector=SYS_CDC; |
9455 | } |
9456 | } |
9457 | |
9458 | //multiple scattering terms |
9459 | if (k>0){ |
9460 | double ds_theta_ms_sq=3.*fabs(central_traj[k].Q(state_D,state_D)); |
9461 | s_theta_ms_sum+=sqrt(ds_theta_ms_sq); |
9462 | double ds=central_traj[k].s-central_traj[k-1].s; |
9463 | theta2ms_sum+=ds_theta_ms_sq/(ds*ds); |
9464 | } |
9465 | if (S(state_z)<endplate_z){ |
9466 | DVector3 position(xy.X(),xy.Y(),S(state_z)); |
9467 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
9468 | extrapolations[SYS_CDC].push_back(Extrapolation_t(position,momentum,t,s, |
9469 | s_theta_ms_sum,theta2ms_sum)); |
9470 | |
9471 | } |
9472 | } |
9473 | // Save the extrapolatoin at the exit of the tracking volume |
9474 | S=central_traj[0].S; |
9475 | xy=central_traj[0].xy; |
9476 | double t=central_traj[0].t; |
9477 | double s=central_traj[0].s; |
9478 | double tanl=S(state_tanl); |
9479 | double pt=1/fabs(S(state_q_over_pt)); |
9480 | double phi=S(state_phi); |
9481 | DVector3 position(xy.X(),xy.Y(),S(state_z)); |
9482 | DVector3 momentum(pt*cos(phi),pt*sin(phi),pt*tanl); |
9483 | extrapolations[SYS_NULL].push_back(Extrapolation_t(position,momentum, |
9484 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02 |
9485 | ,s)); |
9486 | |
9487 | //------------------------------ |
9488 | // Next swim to outer detectors |
9489 | //------------------------------ |
9490 | // Position and step variables |
9491 | double r2=xy.Mod2(); |
9492 | double ds=mStepSizeS; // step along path in cm |
9493 | |
9494 | // Energy loss |
9495 | double dedx=0.; |
9496 | |
9497 | // Track propagation loop |
9498 | //if (false) |
9499 | while (S(state_z)>0. && S(state_z)<Z_MAX370.0 |
9500 | && r2<89.*89.){ |
9501 | // Bail if the transverse momentum has dropped below some minimum |
9502 | if (fabs(S(state_q_over_pt))>Q_OVER_PT_MAX100.){ |
9503 | if (DEBUG_LEVEL>2) |
9504 | { |
9505 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9505<<" " << "Bailing: PT = " << 1./fabs(S(state_q_over_pt)) |
9506 | << endl; |
9507 | } |
9508 | return VALUE_OUT_OF_RANGE; |
9509 | } |
9510 | |
9511 | // get material properties from the Root Geometry |
9512 | double rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.,Z=0.; |
9513 | DVector3 pos3d(xy.X(),xy.Y(),S(state_z)); |
9514 | double s_to_boundary=0.; |
9515 | DVector3 dir(cos(S(state_phi)),sin(S(state_phi)),S(state_tanl)); |
9516 | if (geom->FindMatKalman(pos3d,dir,K_rho_Z_over_A,rho_Z_over_A,LnI,Z, |
9517 | last_material_map,&s_to_boundary) |
9518 | !=NOERROR){ |
9519 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9519<<" " << "Material error in ExtrapolateToVertex! " << endl; |
9520 | _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9520<<" " << " Position (x,y,z)=("<<pos3d.x()<<","<<pos3d.y()<<"," |
9521 | << pos3d.z()<<")" |
9522 | <<endl; |
9523 | break; |
9524 | } |
9525 | |
9526 | // Get dEdx for the upcoming step |
9527 | double q_over_p=S(state_q_over_pt)*cos(atan(S(state_tanl))); |
9528 | if (CORRECT_FOR_ELOSS){ |
9529 | dedx=GetdEdx(q_over_p,K_rho_Z_over_A,rho_Z_over_A,LnI,Z); |
9530 | } |
9531 | // Adjust the step size |
9532 | if (fabs(dedx)>EPS3.0e-8){ |
9533 | ds=DE_PER_STEP0.001/fabs(dedx); |
9534 | } |
9535 | |
9536 | if (ds>mStepSizeS) ds=mStepSizeS; |
9537 | if (s_to_boundary<ds) ds=s_to_boundary; |
9538 | if (ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1; |
9539 | if (ds<0.5 && S(state_z)<400. && pos3d.Perp()>65.) ds=0.5; |
9540 | s+=ds; |
9541 | |
9542 | // Flight time |
9543 | double q_over_p_sq=q_over_p*q_over_p; |
9544 | double one_over_beta2=1.+mass2*q_over_p_sq; |
9545 | if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8; |
9546 | t+=ds*sqrt(one_over_beta2); // in units where c=1 |
9547 | |
9548 | // Propagate the state through the field |
9549 | Step(xy,ds,S,dedx); |
9550 | |
9551 | r2=xy.Mod2(); |
9552 | // Check if we have passed into the BCAL |
9553 | if (r2>64.9*64.9){ |
9554 | if (extrapolations.at(SYS_BCAL).size()>299){ |
9555 | return VALUE_OUT_OF_RANGE; |
9556 | } |
9557 | if (S(state_z)<406.&&S(state_z)>17.0){ |
9558 | tanl=S(state_tanl); |
9559 | pt=1/fabs(S(state_q_over_pt)); |
9560 | phi=S(state_phi); |
9561 | position.SetXYZ(xy.X(),xy.Y(),S(state_z)); |
9562 | momentum.SetXYZ(pt*cos(phi),pt*sin(phi),pt*tanl); |
9563 | extrapolations[SYS_BCAL].push_back(Extrapolation_t(position,momentum, |
9564 | t*TIME_UNIT_CONVERSION3.33564095198152014e-02,s)); |
9565 | } |
9566 | else if (extrapolations.at(SYS_BCAL).size()<5){ |
9567 | // There needs to be some steps inside the the volume of the BCAL for |
9568 | // the extrapolation to be useful. If this is not the case, clear |
9569 | // the extrolation vector. |
9570 | extrapolations[SYS_BCAL].clear(); |
9571 | } |
9572 | } |
9573 | } |
9574 | |
9575 | return NOERROR; |
9576 | } |
9577 | |
9578 | /*---------------------------------------------------------------------------*/ |
9579 | |
9580 | // Kalman engine for forward tracks, propagating from near the beam line to |
9581 | // the outermost hits (opposite to regular direction). |
9582 | kalman_error_t DTrackFitterKalmanSIMD::KalmanReverse(double fdc_anneal_factor, |
9583 | double cdc_anneal_factor, |
9584 | const DMatrix5x1 &Sstart, |
9585 | DMatrix5x5 &C, |
9586 | DMatrix5x1 &S, |
9587 | double &chisq, |
9588 | unsigned int &numdof){ |
9589 | DMatrix2x1 Mdiff; // difference between measurement and prediction |
9590 | DMatrix2x5 H; // Track projection matrix |
9591 | DMatrix5x2 H_T; // Transpose of track projection matrix |
9592 | DMatrix1x5 Hc; // Track projection matrix for cdc hits |
9593 | DMatrix5x1 Hc_T; // Transpose of track projection matrix for cdc hits |
9594 | DMatrix5x5 J; // State vector Jacobian matrix |
9595 | //DMatrix5x5 J_T; // transpose of this matrix |
9596 | DMatrix5x5 Q; // Process noise covariance matrix |
9597 | DMatrix5x2 K; // Kalman gain matrix |
9598 | DMatrix5x1 Kc; // Kalman gain matrix for cdc hits |
9599 | DMatrix2x2 V(0.0833,0.,0.,FDC_CATHODE_VARIANCE0.000225); // Measurement covariance matrix |
9600 | DMatrix5x1 S0,S0_; //State vector |
9601 | DMatrix5x5 Ctest; // Covariance matrix |
9602 | |
9603 | double Vc=0.0507; |
9604 | |
9605 | unsigned int cdc_index=0; |
9606 | unsigned int num_cdc_hits=my_cdchits.size(); |
9607 | bool more_cdc_measurements=(num_cdc_hits>0); |
9608 | double old_doca2=1e6; |
9609 | |
9610 | // Initialize chi squared |
9611 | chisq=0; |
9612 | |
9613 | // Initialize number of degrees of freedom |
9614 | numdof=0; |
9615 | |
9616 | // Cuts for pruning hits |
9617 | double fdc_chi2cut=NUM_FDC_SIGMA_CUT*NUM_FDC_SIGMA_CUT; |
9618 | double cdc_chi2cut=NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT; |
9619 | |
9620 | // Vectors for cdc wires |
9621 | DVector2 origin,dir,wirepos; |
9622 | double z0w=0.; // origin in z for wire |
9623 | |
9624 | deque<DKalmanForwardTrajectory_t>::reverse_iterator rit = forward_traj.rbegin(); |
9625 | S0_=(*rit).S; |
9626 | S=Sstart; |
9627 | |
9628 | if (more_cdc_measurements){ |
9629 | origin=my_cdchits[0]->origin; |
9630 | dir=my_cdchits[0]->dir; |
9631 | z0w=my_cdchits[0]->z0wire; |
9632 | wirepos=origin+((*rit).z-z0w)*dir; |
9633 | } |
9634 | |
9635 | for (rit=forward_traj.rbegin()+1;rit!=forward_traj.rend();++rit){ |
9636 | // Get the state vector, jacobian matrix, and multiple scattering matrix |
9637 | // from reference trajectory |
9638 | S0=(*rit).S; |
9639 | J=2.*I5x5-(*rit).J; // We only want to change the signs of the parts that depend on dz ... |
9640 | Q=(*rit).Q; |
9641 | |
9642 | // Check that the position is within the tracking volume! |
9643 | if (S(state_x)*S(state_x)+S(state_y)*S(state_y)>65.*65.){ |
9644 | return POSITION_OUT_OF_RANGE; |
9645 | } |
9646 | // Update the actual state vector and covariance matrix |
9647 | S=S0+J*(S-S0_); |
9648 | C=Q.AddSym(J*C*J.Transpose()); |
9649 | //C.Print(); |
9650 | |
9651 | // Save the current state of the reference trajectory |
9652 | S0_=S0; |
9653 | |
9654 | // Z position along the trajectory |
9655 | double z=(*rit).z; |
9656 | |
9657 | if (more_cdc_measurements){ |
9658 | // new wire position |
9659 | wirepos=origin; |
9660 | wirepos+=(z-z0w)*dir; |
9661 | |
9662 | // doca variables |
9663 | double dx=S(state_x)-wirepos.X(); |
9664 | double dy=S(state_y)-wirepos.Y(); |
9665 | double doca2=dx*dx+dy*dy; |
9666 | |
9667 | if (doca2>old_doca2){ |
9668 | if(my_cdchits[cdc_index]->status==good_hit){ |
9669 | find_doca_error_t swimmed_to_doca=DOCA_NO_BRENT; |
9670 | double newz=endplate_z; |
9671 | double dz=newz-z; |
9672 | // Sometimes the true doca would correspond to the case where the |
9673 | // wire would need to extend beyond the physical volume of the straw. |
9674 | // In this case, find the doca at the cdc endplate. |
9675 | if (z>endplate_z){ |
9676 | swimmed_to_doca=DOCA_ENDPLATE; |
9677 | SwimToEndplate(z,*rit,S); |
9678 | |
9679 | // wire position at the endplate |
9680 | wirepos=origin; |
9681 | wirepos+=(endplate_z-z0w)*dir; |
9682 | |
9683 | dx=S(state_x)-wirepos.X(); |
9684 | dy=S(state_y)-wirepos.Y(); |
9685 | } |
9686 | else{ |
9687 | // Find the true doca to the wire. If we had to use Brent's |
9688 | // algorithm, the routine returns USED_BRENT |
9689 | swimmed_to_doca=FindDoca(my_cdchits[cdc_index],*rit,mStepSizeZ, |
9690 | mStepSizeZ,S0,S,C,dx,dy,dz,true); |
9691 | if (swimmed_to_doca==BRENT_FAILED){ |
9692 | //_DBG_ << "Brent's algorithm failed" <<endl; |
9693 | return MOMENTUM_OUT_OF_RANGE; |
9694 | } |
9695 | |
9696 | newz=z+dz; |
9697 | } |
9698 | double cosstereo=my_cdchits[cdc_index]->cosstereo; |
9699 | double d=sqrt(dx*dx+dy*dy)*cosstereo+EPS3.0e-8; |
9700 | |
9701 | // Track projection |
9702 | double cosstereo2_over_d=cosstereo*cosstereo/d; |
9703 | Hc_T(state_x)=dx*cosstereo2_over_d; |
9704 | Hc(state_x)=Hc_T(state_x); |
9705 | Hc_T(state_y)=dy*cosstereo2_over_d; |
9706 | Hc(state_y)=Hc_T(state_y); |
9707 | if (swimmed_to_doca==DOCA_NO_BRENT){ |
9708 | Hc_T(state_ty)=Hc_T(state_y)*dz; |
9709 | Hc(state_ty)=Hc_T(state_ty); |
9710 | Hc_T(state_tx)=Hc_T(state_x)*dz; |
9711 | Hc(state_tx)=Hc_T(state_tx); |
9712 | } |
9713 | else{ |
9714 | Hc_T(state_ty)=0.; |
9715 | Hc(state_ty)=0.; |
9716 | Hc_T(state_tx)=0.; |
9717 | Hc(state_tx)=0.; |
9718 | } |
9719 | // Find offset of wire with respect to the center of the |
9720 | // straw at this z position |
9721 | double delta=0.,dphi=0.; |
9722 | FindSag(dx,dy,newz-z0w,my_cdchits[cdc_index]->hit->wire,delta,dphi); |
9723 | |
9724 | // Find drift time and distance |
9725 | double tdrift=my_cdchits[cdc_index]->tdrift-mT0 |
9726 | -(*rit).t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9727 | double tcorr=0.,dmeas=0.; |
9728 | double B=(*rit).B; |
9729 | ComputeCDCDrift(dphi,delta,tdrift,B,dmeas,Vc,tcorr); |
9730 | |
9731 | // Residual |
9732 | double res=dmeas-d; |
9733 | |
9734 | // inverse variance including prediction |
9735 | double Vproj=Hc*C*Hc_T; |
9736 | double InvV1=1./(Vc+Vproj); |
9737 | |
9738 | // Check if this hit is an outlier |
9739 | double chi2_hit=res*res*InvV1; |
9740 | if (chi2_hit<cdc_chi2cut){ |
9741 | // Compute KalmanSIMD gain matrix |
9742 | Kc=InvV1*(C*Hc_T); |
9743 | |
9744 | // Update state vector covariance matrix |
9745 | //C=C-K*(H*C); |
9746 | Ctest=C.SubSym(Kc*(Hc*C)); |
9747 | |
9748 | if (!Ctest.IsPosDef()){ |
9749 | if (DEBUG_LEVEL>0) _DBG_std::cerr<<"libraries/TRACKING/DTrackFitterKalmanSIMD.cc" <<":"<<9749<<" " << "Broken covariance matrix!" <<endl; |
9750 | } |
9751 | |
9752 | if (tdrift >= CDC_T_DRIFT_MIN){ |
9753 | C=Ctest; |
9754 | S+=res*Kc; |
9755 | |
9756 | chisq+=(1.-Hc*Kc)*res*res/Vc; |
9757 | numdof++; |
9758 | } |
9759 | } |
9760 | |
9761 | // If we had to use Brent's algorithm to find the true doca or the |
9762 | // doca to the line corresponding to the wire is downstream of the |
9763 | // cdc endplate, we need to swim the state vector and covariance |
9764 | // matrix back to the appropriate position along the reference |
9765 | // trajectory. |
9766 | if (swimmed_to_doca!=DOCA_NO_BRENT){ |
9767 | double dedx=0.; |
9768 | if (CORRECT_FOR_ELOSS){ |
9769 | dedx=GetdEdx(S(state_q_over_p),(*rit).K_rho_Z_over_A, |
9770 | (*rit).rho_Z_over_A,(*rit).LnI,(*rit).Z); |
9771 | } |
9772 | StepBack(dedx,newz,z,S0,S,C); |
9773 | } |
9774 | } |
9775 | |
9776 | // new wire origin and direction |
9777 | if (cdc_index<num_cdc_hits-1){ |
9778 | cdc_index++; |
9779 | origin=my_cdchits[cdc_index]->origin; |
9780 | z0w=my_cdchits[cdc_index]->z0wire; |
9781 | dir=my_cdchits[cdc_index]->dir; |
9782 | } |
9783 | else more_cdc_measurements=false; |
9784 | |
9785 | // Update the wire position |
9786 | wirepos=origin+(z-z0w)*dir; |
9787 | |
9788 | // new doca |
9789 | dx=S(state_x)-wirepos.X(); |
9790 | dy=S(state_y)-wirepos.Y(); |
9791 | doca2=dx*dx+dy*dy; |
9792 | } |
9793 | old_doca2=doca2; |
9794 | } |
9795 | if ((*rit).h_id>0&&(*rit).h_id<1000){ |
9796 | unsigned int id=(*rit).h_id-1; |
9797 | |
9798 | // Variance in coordinate transverse to wire |
9799 | V(0,0)=my_fdchits[id]->uvar; |
9800 | |
9801 | // Variance in coordinate along wire |
9802 | V(1,1)=my_fdchits[id]->vvar; |
9803 | |
9804 | double upred=0,vpred=0.,doca=0.,cosalpha=0.,lorentz_factor=0.; |
9805 | FindDocaAndProjectionMatrix(my_fdchits[id],S,upred,vpred,doca,cosalpha, |
9806 | lorentz_factor,H_T); |
9807 | // Matrix transpose H_T -> H |
9808 | H=Transpose(H_T); |
9809 | |
9810 | |
9811 | DMatrix2x2 Vtemp=V+H*C*H_T; |
9812 | |
9813 | // Residual for coordinate along wire |
9814 | Mdiff(1)=my_fdchits[id]->vstrip-vpred-doca*lorentz_factor; |
9815 | |
9816 | // Residual for coordinate transverse to wire |
9817 | double drift_time=my_fdchits[id]->t-mT0-(*rit).t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
9818 | if (my_fdchits[id]->hit!=NULL__null){ |
9819 | double drift=(doca>0.0?1.:-1.)*fdc_drift_distance(drift_time,(*rit).B); |
9820 | Mdiff(0)=drift-doca; |
9821 | |
9822 | // Variance in drift distance |
9823 | V(0,0)=fdc_drift_variance(drift_time); |
9824 | } |
9825 | else if (USE_TRD_DRIFT_TIMES&&my_fdchits[id]->status==trd_hit){ |
9826 | double drift =(doca>0.0?1.:-1.)*0.1*pow(drift_time/8./0.91,1./1.556); |
9827 | Mdiff(0)=drift-doca; |
9828 | |
9829 | // Variance in drift distance |
9830 | V(0,0)=0.05*0.05; |
9831 | } |
9832 | if ((*rit).num_hits==1){ |
9833 | // Add contribution of track covariance from state vector propagation |
9834 | // to measurement errors |
9835 | DMatrix2x2 Vtemp=V+H*C*H_T; |
9836 | double chi2_hit=Vtemp.Chi2(Mdiff); |
9837 | if (chi2_hit<fdc_chi2cut){ |
9838 | // Compute Kalman gain matrix |
9839 | DMatrix2x2 InvV=Vtemp.Invert(); |
9840 | DMatrix5x2 K=C*H_T*InvV; |
9841 | |
9842 | // Update the state vector |
9843 | S+=K*Mdiff; |
9844 | |
9845 | // Update state vector covariance matrix |
9846 | //C=C-K*(H*C); |
9847 | C=C.SubSym(K*(H*C)); |
9848 | |
9849 | if (DEBUG_LEVEL > 35) { |
9850 | jout << "S Update: " << endl; S.Print(); |
9851 | jout << "C Update: " << endl; C.Print(); |
9852 | } |
9853 | |
9854 | // Filtered residual and covariance of filtered residual |
9855 | DMatrix2x1 R=Mdiff-H*K*Mdiff; |
9856 | DMatrix2x2 RC=V-H*(C*H_T); |
9857 | |
9858 | // Update chi2 for this segment |
9859 | chisq+=RC.Chi2(R); |
9860 | |
9861 | if (DEBUG_LEVEL>30) |
9862 | { |
9863 | printf("hit %d p %5.2f dm %5.4f %5.4f sig %5.4f %5.4f chi2 %5.2f\n", |
9864 | id,1./S(state_q_over_p),Mdiff(0),Mdiff(1), |
9865 | sqrt(V(0,0)),sqrt(V(1,1)),RC.Chi2(R)); |
9866 | } |
9867 | |
9868 | numdof+=2; |
9869 | } |
9870 | } |
9871 | // Handle the case where there are multiple adjacent hits in the plane |
9872 | else { |
9873 | UpdateSandCMultiHit(*rit,upred,vpred,doca,cosalpha,lorentz_factor,V, |
9874 | Mdiff,H,H_T,S,C,fdc_chi2cut,false,chisq,numdof); |
9875 | } |
9876 | } |
9877 | |
9878 | //printf("chisq %f ndof %d prob %f\n",chisq,numdof,TMath::Prob(chisq,numdof)); |
9879 | } |
9880 | |
9881 | return FIT_SUCCEEDED; |
9882 | } |
9883 | |
9884 | // Finds the distance of closest approach between a CDC wire and the trajectory |
9885 | // of the track and updates the state vector and covariance matrix at this point. |
9886 | find_doca_error_t |
9887 | DTrackFitterKalmanSIMD::FindDoca(const DKalmanSIMDCDCHit_t *hit, |
9888 | const DKalmanForwardTrajectory_t &traj, |
9889 | double step1,double step2, |
9890 | DMatrix5x1 &S0,DMatrix5x1 &S, |
9891 | DMatrix5x5 &C, |
9892 | double &dx,double &dy,double &dz, |
9893 | bool do_reverse){ |
9894 | double z=traj.z,newz=z; |
9895 | DMatrix5x5 J; |
9896 | |
9897 | // wire position and direction |
9898 | DVector2 origin=hit->origin; |
9899 | double z0w=hit->z0wire; |
9900 | DVector2 dir=hit->dir; |
9901 | |
9902 | // energy loss |
9903 | double dedx=0.; |
9904 | |
9905 | // track direction variables |
9906 | double tx=S(state_tx); |
9907 | double ty=S(state_ty); |
9908 | double tanl=1./sqrt(tx*tx+ty*ty); |
9909 | double sinl=sin(atan(tanl)); |
9910 | |
9911 | // Wire direction variables |
9912 | double ux=dir.X(); |
9913 | double uy=dir.Y(); |
9914 | // Variables relating wire direction and track direction |
9915 | double my_ux=tx-ux; |
9916 | double my_uy=ty-uy; |
9917 | double denom=my_ux*my_ux+my_uy*my_uy; |
9918 | |
9919 | // variables for dealing with propagation of S and C if we |
9920 | // need to use Brent's algorithm to find the doca to the wire |
9921 | int num_steps=0; |
9922 | double my_dz=0.; |
9923 | |
9924 | // if the path length increment is small relative to the radius |
9925 | // of curvature, use a linear approximation to find dz |
9926 | bool do_brent=false; |
9927 | double sign=do_reverse?-1.:1.; |
9928 | double two_step=sign*(step1+step2); |
9929 | if (fabs(qBr2p0.003*S(state_q_over_p) |
9930 | *bfield->GetBz(S(state_x),S(state_y),z) |
9931 | *two_step/sinl)<0.05 |
9932 | && denom>EPS3.0e-8){ |
9933 | double dzw=z-z0w; |
9934 | dz=-((S(state_x)-origin.X()-ux*dzw)*my_ux |
9935 | +(S(state_y)-origin.Y()-uy*dzw)*my_uy)/denom; |
9936 | if (fabs(dz)>fabs(two_step) || sign*dz<0){ |
9937 | do_brent=true; |
9938 | } |
9939 | else{ |
9940 | newz=z+dz; |
9941 | // Check for exiting the straw |
9942 | if (newz>endplate_z){ |
9943 | dz=endplate_z-z; |
9944 | } |
9945 | } |
9946 | } |
9947 | else do_brent=true; |
9948 | if (do_brent){ |
9949 | if (CORRECT_FOR_ELOSS){ |
9950 | dedx=GetdEdx(S(state_q_over_p),traj.K_rho_Z_over_A,traj.rho_Z_over_A, |
9951 | traj.LnI,traj.Z); |
9952 | } |
9953 | |
9954 | // We have bracketed the minimum doca: use Brent's agorithm |
9955 | if (BrentForward(z,dedx,z0w,origin,dir,S,dz)!=NOERROR){ |
9956 | return BRENT_FAILED; |
9957 | } |
9958 | // Step the state and covariance through the field |
9959 | if (fabs(dz)>mStepSizeZ){ |
9960 | my_dz=(dz>0?1.0:-1.)*mStepSizeZ; |
9961 | num_steps=int(fabs(dz/my_dz)); |
9962 | double dz3=dz-num_steps*my_dz; |
9963 | |
9964 | double my_z=z; |
9965 | for (int m=0;m<num_steps;m++){ |
9966 | newz=my_z+my_dz; |
9967 | |
9968 | // propagate error matrix to z-position of hit |
9969 | StepJacobian(my_z,newz,S0,dedx,J); |
9970 | C=J*C*J.Transpose(); |
9971 | //C=C.SandwichMultiply(J); |
9972 | |
9973 | // Step reference trajectory by my_dz |
9974 | Step(my_z,newz,dedx,S0); |
9975 | |
9976 | my_z=newz; |
9977 | } |
9978 | |
9979 | newz=my_z+dz3; |
9980 | // propagate error matrix to z-position of hit |
9981 | StepJacobian(my_z,newz,S0,dedx,J); |
9982 | C=J*C*J.Transpose(); |
9983 | //C=C.SandwichMultiply(J); |
9984 | |
9985 | // Step reference trajectory by dz3 |
9986 | Step(my_z,newz,dedx,S0); |
9987 | } |
9988 | else{ |
9989 | newz = z + dz; |
9990 | |
9991 | // propagate error matrix to z-position of hit |
9992 | StepJacobian(z,newz,S0,dedx,J); |
9993 | C=J*C*J.Transpose(); |
9994 | //C=C.SandwichMultiply(J); |
9995 | |
9996 | // Step reference trajectory by dz |
9997 | Step(z,newz,dedx,S0); |
9998 | } |
9999 | } |
10000 | |
10001 | // Wire position at current z |
10002 | DVector2 wirepos=origin; |
10003 | wirepos+=(newz-z0w)*dir; |
10004 | |
10005 | double xw=wirepos.X(); |
10006 | double yw=wirepos.Y(); |
10007 | |
10008 | // predicted doca taking into account the orientation of the wire |
10009 | if (do_brent==false){ |
10010 | // In this case we did not have to swim to find the doca and |
10011 | // the transformation from the state vector space to the |
10012 | // measurement space is straight-forward. |
10013 | dy=S(state_y)+S(state_ty)*dz-yw; |
10014 | dx=S(state_x)+S(state_tx)*dz-xw; |
10015 | } |
10016 | else{ |
10017 | // In this case we swam the state vector to the position of the doca |
10018 | dy=S(state_y)-yw; |
10019 | dx=S(state_x)-xw; |
10020 | } |
10021 | |
10022 | if (do_brent) return USED_BRENT; |
10023 | return DOCA_NO_BRENT; |
10024 | } |
10025 | |
10026 | // Swim along a trajectory to the z-position z. |
10027 | void DTrackFitterKalmanSIMD::StepBack(double dedx,double newz,double z, |
10028 | DMatrix5x1 &S0,DMatrix5x1 &S, |
10029 | DMatrix5x5 &C){ |
10030 | double dz=newz-z; |
10031 | DMatrix5x5 J; |
10032 | int num_steps=int(fabs(dz/mStepSizeZ)); |
10033 | if (num_steps==0){ |
10034 | // Step C back to the z-position on the reference trajectory |
10035 | StepJacobian(newz,z,S0,dedx,J); |
10036 | C=J*C*J.Transpose(); |
10037 | //C=C.SandwichMultiply(J); |
10038 | |
10039 | // Step S to current position on the reference trajectory |
10040 | Step(newz,z,dedx,S); |
10041 | |
10042 | // Step S0 to current position on the reference trajectory |
10043 | Step(newz,z,dedx,S0); |
10044 | } |
10045 | else{ |
10046 | double my_z=newz; |
10047 | double my_dz=(dz>0.0?1.0:-1.)*mStepSizeZ; |
10048 | double dz3=dz-num_steps*my_dz; |
10049 | for (int m=0;m<num_steps;m++){ |
10050 | z=my_z-my_dz; |
10051 | |
10052 | // Step C along z |
10053 | StepJacobian(my_z,z,S0,dedx,J); |
10054 | C=J*C*J.Transpose(); |
10055 | //C=C.SandwichMultiply(J); |
10056 | |
10057 | // Step S along z |
10058 | Step(my_z,z,dedx,S); |
10059 | |
10060 | // Step S0 along z |
10061 | Step(my_z,z,dedx,S0); |
10062 | |
10063 | my_z=z; |
10064 | } |
10065 | z=my_z-dz3; |
10066 | |
10067 | // Step C back to the z-position on the reference trajectory |
10068 | StepJacobian(my_z,z,S0,dedx,J); |
10069 | C=J*C*J.Transpose(); |
10070 | //C=C.SandwichMultiply(J); |
10071 | |
10072 | // Step S to current position on the reference trajectory |
10073 | Step(my_z,z,dedx,S); |
10074 | |
10075 | // Step S0 to current position on the reference trajectory |
10076 | Step(my_z,z,dedx,S0); |
10077 | } |
10078 | } |
10079 | |
10080 | // Swim a track to the CDC endplate given starting z position |
10081 | void DTrackFitterKalmanSIMD::SwimToEndplate(double z, |
10082 | const DKalmanForwardTrajectory_t &traj, |
10083 | DMatrix5x1 &S){ |
10084 | double dedx=0.; |
10085 | if (CORRECT_FOR_ELOSS){ |
10086 | dedx=GetdEdx(S(state_q_over_p),traj.K_rho_Z_over_A,traj.rho_Z_over_A, |
10087 | traj.LnI,traj.Z); |
10088 | } |
10089 | double my_z=z; |
10090 | int num_steps=int(fabs((endplate_z-z)/mStepSizeZ)); |
10091 | for (int m=0;m<num_steps;m++){ |
10092 | my_z=z-mStepSizeZ; |
10093 | Step(z,my_z,dedx,S); |
10094 | z=my_z; |
10095 | } |
10096 | Step(z,endplate_z,dedx,S); |
10097 | } |
10098 | |
10099 | // Find the sag parameters (delta,dphi) for the given straw at the local z |
10100 | // position |
10101 | void DTrackFitterKalmanSIMD::FindSag(double dx,double dy, double zlocal, |
10102 | const DCDCWire *mywire,double &delta, |
10103 | double &dphi) const{ |
10104 | int ring_index=mywire->ring-1; |
10105 | int straw_index=mywire->straw-1; |
10106 | double phi_d=atan2(dy,dx); |
10107 | delta=max_sag[ring_index][straw_index]*(1.-zlocal*zlocal/5625.) |
10108 | *cos(phi_d + sag_phi_offset[ring_index][straw_index]); |
10109 | |
10110 | dphi=phi_d-mywire->origin.Phi(); |
10111 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
10112 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
10113 | if (mywire->origin.Y()<0) dphi*=-1.; |
10114 | } |
10115 | |
10116 | void DTrackFitterKalmanSIMD::FindDocaAndProjectionMatrix(const DKalmanSIMDFDCHit_t *hit, |
10117 | const DMatrix5x1 &S, |
10118 | double &upred, |
10119 | double &vpred, |
10120 | double &doca, |
10121 | double &cosalpha, |
10122 | double &lorentz_factor, |
10123 | DMatrix5x2 &H_T){ |
10124 | // Make the small alignment rotations |
10125 | // Use small-angle form. |
10126 | |
10127 | // Position and direction from state vector |
10128 | double x=S(state_x) + hit->phiZ*S(state_y); |
10129 | double y=S(state_y) - hit->phiZ*S(state_x); |
10130 | double tx =S(state_tx) + hit->phiZ*S(state_ty) - hit->phiY; |
10131 | double ty =S(state_ty) - hit->phiZ*S(state_tx) + hit->phiX; |
10132 | |
10133 | // Plane orientation and measurements |
10134 | double cosa=hit->cosa; |
10135 | double sina=hit->sina; |
10136 | double u=hit->uwire; |
10137 | |
10138 | // Projected coordinate along wire |
10139 | vpred=x*sina+y*cosa; |
10140 | |
10141 | // Direction tangent in the u-z plane |
10142 | double tu=tx*cosa-ty*sina; |
10143 | double tv=tx*sina+ty*cosa; |
10144 | double alpha=atan(tu); |
10145 | cosalpha=cos(alpha); |
10146 | double cosalpha2=cosalpha*cosalpha; |
10147 | double sinalpha=sin(alpha); |
10148 | |
10149 | // (signed) distance of closest approach to wire |
10150 | upred=x*cosa-y*sina; |
10151 | if (hit->status==gem_hit){ |
10152 | doca=upred-u; |
10153 | } |
10154 | else{ |
10155 | doca=(upred-u)*cosalpha; |
10156 | } |
10157 | |
10158 | // Correction for lorentz effect |
10159 | double nz=hit->nz; |
10160 | double nr=hit->nr; |
10161 | double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha; |
10162 | lorentz_factor=nz_sinalpha_plus_nr_cosalpha-tv*sinalpha; |
10163 | |
10164 | // To transform from (x,y) to (u,v), need to do a rotation: |
10165 | // u = x*cosa-y*sina |
10166 | // v = y*cosa+x*sina |
10167 | if (hit->status!=gem_hit){ |
10168 | H_T(state_x,1)=sina+cosa*cosalpha*lorentz_factor; |
10169 | H_T(state_y,1)=cosa-sina*cosalpha*lorentz_factor; |
10170 | |
10171 | double cos2_minus_sin2=cosalpha2-sinalpha*sinalpha; |
10172 | double fac=nz*cos2_minus_sin2-2.*nr*cosalpha*sinalpha; |
10173 | double doca_cosalpha=doca*cosalpha; |
10174 | double temp=doca_cosalpha*fac; |
10175 | H_T(state_tx,1)=cosa*temp-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2); |
10176 | H_T(state_ty,1)=-sina*temp-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2); |
10177 | |
10178 | H_T(state_x,0)=cosa*cosalpha; |
10179 | H_T(state_y,0)=-sina*cosalpha; |
10180 | |
10181 | double factor=doca*tu*cosalpha2; |
10182 | H_T(state_ty,0)=sina*factor; |
10183 | H_T(state_tx,0)=-cosa*factor; |
10184 | } |
10185 | else{ |
10186 | H_T(state_x,1)=sina; |
10187 | H_T(state_y,1)=cosa; |
10188 | H_T(state_x,0)=cosa; |
10189 | H_T(state_y,0)=-sina; |
10190 | } |
10191 | } |
10192 | |
10193 | // Update S and C using all the good adjacent hits in a particular FDC plane |
10194 | void DTrackFitterKalmanSIMD::UpdateSandCMultiHit(const DKalmanForwardTrajectory_t &traj, |
10195 | double upred,double vpred, |
10196 | double doca,double cosalpha, |
10197 | double lorentz_factor, |
10198 | DMatrix2x2 &V, |
10199 | DMatrix2x1 &Mdiff, |
10200 | DMatrix2x5 &H, |
10201 | const DMatrix5x2 &H_T, |
10202 | DMatrix5x1 &S,DMatrix5x5 &C, |
10203 | double fdc_chi2cut, |
10204 | bool skip_plane,double &chisq, |
10205 | unsigned int &numdof, |
10206 | double fdc_anneal_factor){ |
10207 | // If we do have multiple hits, then all of the hits within some |
10208 | // validation region are included with weights determined by how |
10209 | // close the hits are to the track projection of the state to the |
10210 | // "hit space". |
10211 | vector<DMatrix5x2> Klist; |
10212 | vector<DMatrix2x1> Mlist; |
10213 | vector<DMatrix2x5> Hlist; |
10214 | vector<DMatrix5x2> HTlist; |
10215 | vector<DMatrix2x2> Vlist; |
10216 | vector<double>probs; |
10217 | vector<unsigned int>used_ids; |
10218 | |
10219 | // use a Gaussian model for the probability for the hit |
10220 | DMatrix2x2 Vtemp=V+H*C*H_T; |
10221 | double chi2_hit=Vtemp.Chi2(Mdiff); |
10222 | double prob_hit=0.; |
10223 | |
10224 | // Add the first hit to the list, cutting out outliers |
10225 | unsigned int id=traj.h_id-1; |
10226 | if (chi2_hit<fdc_chi2cut && my_fdchits[id]->status==good_hit){ |
10227 | DMatrix2x2 InvV=Vtemp.Invert(); |
10228 | |
10229 | prob_hit=exp(-0.5*chi2_hit)/(M_TWO_PI6.28318530717958647692*sqrt(Vtemp.Determinant())); |
10230 | probs.push_back(prob_hit); |
10231 | Vlist.push_back(V); |
10232 | Hlist.push_back(H); |
10233 | HTlist.push_back(H_T); |
10234 | Mlist.push_back(Mdiff); |
10235 | Klist.push_back(C*H_T*InvV); // Kalman gain |
10236 | |
10237 | used_ids.push_back(id); |
10238 | fdc_used_in_fit[id]=true; |
10239 | } |
10240 | // loop over the remaining hits |
10241 | for (unsigned int m=1;m<traj.num_hits;m++){ |
10242 | unsigned int my_id=id-m; |
10243 | if (my_fdchits[my_id]->status==good_hit){ |
10244 | double u=my_fdchits[my_id]->uwire; |
10245 | double v=my_fdchits[my_id]->vstrip; |
10246 | |
10247 | // Doca to this wire |
10248 | double mydoca=(upred-u)*cosalpha; |
10249 | |
10250 | // variance for coordinate along the wire |
10251 | V(1,1)=my_fdchits[my_id]->vvar*fdc_anneal_factor; |
10252 | |
10253 | // Difference between measurement and projection |
10254 | Mdiff(1)=v-(vpred+mydoca*lorentz_factor); |
10255 | Mdiff(0)=-mydoca; |
10256 | if (fit_type==kTimeBased && USE_FDC_DRIFT_TIMES){ |
10257 | double drift_time=my_fdchits[my_id]->t-mT0-traj.t*TIME_UNIT_CONVERSION3.33564095198152014e-02; |
10258 | double sign=(doca>0.0)?1.:-1.; |
10259 | if (my_fdchits[my_id]->hit!=NULL__null){ |
10260 | double drift=sign*fdc_drift_distance(drift_time,traj.B); |
10261 | Mdiff(0)+=drift; |
10262 | |
10263 | // Variance in drift distance |
10264 | V(0,0)=fdc_drift_variance(drift_time)*fdc_anneal_factor; |
10265 | } |
10266 | else if (USE_TRD_DRIFT_TIMES&&my_fdchits[my_id]->status==trd_hit){ |
10267 | double drift = sign*0.1*pow(drift_time/8./0.91,1./1.556); |
10268 | Mdiff(0)+=drift; |
10269 | V(0,0)=0.05*0.05; |
10270 | } |
10271 | |
10272 | // H_T contains terms that are proportional to doca, so we only need |
10273 | // to scale the appropriate elements for the current hit. |
10274 | if (fabs(doca)<EPS3.0e-8){ |
10275 | doca=(doca>0)?EPS3.0e-8:-EPS3.0e-8; |
10276 | } |
10277 | double doca_ratio=mydoca/doca; |
10278 | DMatrix5x2 H_T_temp=H_T; |
10279 | H_T_temp(state_tx,1)*=doca_ratio; |
10280 | H_T_temp(state_ty,1)*=doca_ratio; |
10281 | H_T_temp(state_tx,0)*=doca_ratio; |
10282 | H_T_temp(state_ty,0)*=doca_ratio; |
10283 | H=Transpose(H_T_temp); |
10284 | |
10285 | // Update error matrix with state vector propagation covariance contrib. |
10286 | Vtemp=V+H*C*H_T_temp; |
10287 | // Cut out outliers and compute probability |
10288 | chi2_hit=Vtemp.Chi2(Mdiff); |
10289 | if (chi2_hit<fdc_chi2cut){ |
10290 | DMatrix2x2 InvV=Vtemp.Invert(); |
10291 | |
10292 | prob_hit=exp(-0.5*chi2_hit)/(M_TWO_PI6.28318530717958647692*sqrt(Vtemp.Determinant())); |
10293 | probs.push_back(prob_hit); |
10294 | Mlist.push_back(Mdiff); |
10295 | Vlist.push_back(V); |
10296 | Hlist.push_back(H); |
10297 | HTlist.push_back(H_T_temp); |
10298 | Klist.push_back(C*H_T_temp*InvV); |
10299 | |
10300 | used_ids.push_back(my_id); |
10301 | fdc_used_in_fit[my_id]=true; |
10302 | |
10303 | // Add some hit info to the update vector for this plane |
10304 | fdc_updates[my_id].tdrift=drift_time; |
10305 | fdc_updates[my_id].tcorr=fdc_updates[my_id].tdrift; |
10306 | fdc_updates[my_id].doca=mydoca; |
10307 | } |
10308 | } |
10309 | } // check that the hit is "good" |
10310 | } // loop over remaining hits |
10311 | if (probs.size()==0) return; |
10312 | |
10313 | double prob_tot=probs[0]; |
10314 | for (unsigned int m=1;m<probs.size();m++){ |
10315 | prob_tot+=probs[m]; |
10316 | } |
10317 | |
10318 | if (skip_plane==false){ |
10319 | DMatrix5x5 sum=I5x5; |
10320 | DMatrix5x5 sum2; |
10321 | for (unsigned int m=0;m<Klist.size();m++){ |
10322 | double my_prob=probs[m]/prob_tot; |
10323 | S+=my_prob*(Klist[m]*Mlist[m]); |
10324 | sum-=my_prob*(Klist[m]*Hlist[m]); |
10325 | sum2+=(my_prob*my_prob)*(Klist[m]*Vlist[m]*Transpose(Klist[m])); |
10326 | |
10327 | // Update chi2 |
10328 | DMatrix2x2 HK=Hlist[m]*Klist[m]; |
10329 | DMatrix2x1 R=Mlist[m]-HK*Mlist[m]; |
10330 | DMatrix2x2 RC=Vlist[m]-HK*Vlist[m]; |
10331 | chisq+=my_prob*RC.Chi2(R); |
10332 | |
10333 | unsigned int my_id=used_ids[m]; |
10334 | fdc_updates[my_id].V=RC; |
10335 | |
10336 | if (DEBUG_LEVEL > 25) |
10337 | { |
10338 | jout << " Adjusting state vector for FDC hit " << m << " with prob " << my_prob << " S:" << endl; |
10339 | S.Print(); |
10340 | } |
10341 | } |
10342 | // Update the covariance matrix C |
10343 | C=sum2.AddSym(sum*C*sum.Transpose()); |
10344 | |
10345 | if (DEBUG_LEVEL > 25) |
10346 | { jout << " C: " << endl; C.Print();} |
10347 | } |
10348 | |
10349 | // Save some information about the track at this plane in the update vector |
10350 | for (unsigned int m=0;m<Hlist.size();m++){ |
10351 | unsigned int my_id=used_ids[m]; |
10352 | fdc_updates[my_id].S=S; |
10353 | fdc_updates[my_id].C=C; |
10354 | if (skip_plane){ |
10355 | fdc_updates[my_id].V=Vlist[m]; |
10356 | } |
10357 | } |
10358 | // update number of degrees of freedom |
10359 | if (skip_plane==false){ |
10360 | numdof+=2; |
10361 | } |
10362 | } |