Bug Summary

File:libraries/TRACKING/DTrackFitterKalmanSIMD.cc
Location:line 5217, column 5
Description:Value stored to 'ds' is never read

Annotated Source Code

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
13#include <TH2F.h>
14#include <TROOT.h>
15#include <TMath.h>
16#include <DMatrix.h>
17
18#include <iomanip>
19#include <math.h>
20
21#define MAX_TB_PASSES20 20
22#define MAX_WB_PASSES20 20
23#define MIN_PROTON_P0.3 0.3
24#define MIN_PION_P0.08 0.08
25#define MAX_P12.0 12.0
26
27#define NaNstd::numeric_limits<double>::quiet_NaN() std::numeric_limits<double>::quiet_NaN()
28
29// Local boolean routines for sorting
30//bool static DKalmanSIMDHit_cmp(DKalmanSIMDHit_t *a, DKalmanSIMDHit_t *b){
31// return a->z<b->z;
32//}
33
34inline bool static DKalmanSIMDFDCHit_cmp(DKalmanSIMDFDCHit_t *a, DKalmanSIMDFDCHit_t *b){
35 if (fabs(a->z-b->z)<EPS3.0e-8) return(a->t<b->t);
36
37 return a->z<b->z;
38}
39inline bool static DKalmanSIMDCDCHit_cmp(DKalmanSIMDCDCHit_t *a, DKalmanSIMDCDCHit_t *b){
40 if (a==NULL__null || b==NULL__null){
41 cout << "Null pointer in CDC hit list??" << endl;
42 return false;
43 }
44 if(b->hit->wire->ring == a->hit->wire->ring){
45 return b->hit->wire->straw < a->hit->wire->straw;
46 }
47
48 return (b->hit->wire->ring>a->hit->wire->ring);
49}
50
51
52// Locate a position in array xx given x
53void DTrackFitterKalmanSIMD::locate(const double *xx,int n,double x,int *j){
54 int ju,jm,jl;
55 int ascnd;
56
57 jl=-1;
58 ju=n;
59 ascnd=(xx[n-1]>=xx[0]);
60 while(ju-jl>1){
61 jm=(ju+jl)>>1;
62 if ( (x>=xx[jm])==ascnd)
63 jl=jm;
64 else
65 ju=jm;
66 }
67 if (x==xx[0]) *j=0;
68 else if (x==xx[n-1]) *j=n-2;
69 else *j=jl;
70}
71
72
73
74// Variance for position along wire
75double DTrackFitterKalmanSIMD::fdc_y_variance(double tanl,double x,double dE){
76 //double sigma=0.0395/dE;
77 double sigma=2.6795e-4*FDC_CATHODE_SIGMA/dE;
78 //sigma*=(1+fabs(x));
79 // sigma*=(1.144e-3*cosh(11.62*x)+0.65)/0.6555;
80 //sigma*=1.16;
81 sigma=0.031/dE+0.007;
82
83 return sigma*sigma;
84}
85
86// Crude approximation for the variance in drift distance due to smearing
87double fdc_drift_variance(double t){
88 //return FDC_ANODE_VARIANCE;
89 if (t<1) t=1;
90 double par[4]={6.051e-3,-1.118e-5,-1.658e-6,2.036e-8};
91 double sigma=8.993e-3/(t+0.001);
92 for (int i=0;i<4;i++) sigma+=par[i]*pow(t,i);
93 sigma*=1.1;
94
95 return sigma*sigma;
96}
97
98// Smearing function derived from fitting residuals
99double DTrackFitterKalmanSIMD::cdc_variance(double B,double tanl,double t){
100 //return CDC_VARIANCE;
101 if (t<0.0) t=0.0;
102 double sigma=0.127/(t+1.96)+4.66e-3+4.67e-6*t;
103 sigma*=0.76*(1-0.0513*tanl-0.133*tanl*tanl+0.09*tanl*tanl*tanl);
104 return sigma*sigma;
105}
106
107double DTrackFitterKalmanSIMD::cdc_forward_variance(double B,double tanl,double t){
108 if (t<0.0) t=0.0;
109 double sigma=0.127/(t+1.96)+4.66e-3+4.67e-6*t;
110 sigma*=0.75*(1.125-0.05*tanl);
111 return sigma*sigma;
112}
113
114#define CDC_T0_OFFSET16.5 16.5 //17.0
115// Interpolate on a table to convert time to distance for the cdc
116double DTrackFitterKalmanSIMD::cdc_drift_distance(double t,double B){
117 //if (t<0.) return 0.;
118 double time=t*CDC_DRIFT_B_SCALE
119 /(CDC_DRIFT_B_SCALE_PAR1602.0+CDC_DRIFT_B_SCALE_PAR258.22*B);
120 int id=int((time+CDC_T0_OFFSET16.5)/2.);
121 if (id<0) id=0;
122 if (id>398) id=398;
123 double d=cdc_drift_table[id];
124 //_DBG_ << time<<" "<< id <<" "<< d <<endl;
125
126 if (id!=398){
127 double frac=0.5*(time+CDC_T0_OFFSET16.5-2.*double(id));
128 double dd=cdc_drift_table[id+1]-cdc_drift_table[id];
129 d+=frac*dd;
130 }
131
132 return d;
133}
134
135#define FDC_T0_OFFSET17.6 17.6
136// Interpolate on a table to convert time to distance for the fdc
137double DTrackFitterKalmanSIMD::fdc_drift_distance(double t,double Bz){
138 double a=93.31,b=4.614,Bref=2.143;
139 t*=(a+b*Bref)/(a+b*Bz);
140 int id=int((t+FDC_T0_OFFSET17.6)/2.);
141 if (id<0) id=0;
142 if (id>138) id=138;
143 double d=fdc_drift_table[id];
144 if (id!=138){
145 double frac=0.5*(t+FDC_T0_OFFSET17.6-2.*double(id));
146 double dd=fdc_drift_table[id+1]-fdc_drift_table[id];
147 d+=frac*dd;
148 }
149
150 return d;
151}
152
153
154DTrackFitterKalmanSIMD::DTrackFitterKalmanSIMD(JEventLoop *loop):DTrackFitter(loop){
155 // Get the position of the CDC downstream endplate from DGeometry
156 geom->GetCDCEndplate(endplate_z,endplate_dz,endplate_rmin,endplate_rmax);
157 endplate_z-=0.5*endplate_dz;
158
159 // Beginning of the cdc
160 vector<double>cdc_center;
161 vector<double>cdc_upstream_endplate_pos;
162 vector<double>cdc_endplate_dim;
163 geom->Get("//posXYZ[@volume='CentralDC'/@X_Y_Z",cdc_origin);
164 geom->Get("//posXYZ[@volume='centralDC_option-1']/@X_Y_Z",cdc_center);
165 geom->Get("//posXYZ[@volume='CDPU']/@X_Y_Z",cdc_upstream_endplate_pos);
166 geom->Get("//tubs[@name='CDPU']/@Rio_Z",cdc_endplate_dim);
167 cdc_origin[2]+=cdc_center[2]+cdc_upstream_endplate_pos[2]
168 +0.5*cdc_endplate_dim[2];
169
170 DEBUG_HISTS=false;
171 gPARMS->SetDefaultParameter("KALMAN:DEBUG_HISTS", DEBUG_HISTS);
172
173 DEBUG_LEVEL=0;
174 gPARMS->SetDefaultParameter("KALMAN:DEBUG_LEVEL", DEBUG_LEVEL);
175
176 USE_T0_FROM_WIRES=0;
177 gPARMS->SetDefaultParameter("KALMAN:USE_T0_FROM_WIRES", USE_T0_FROM_WIRES);
178
179 ESTIMATE_T0_TB=false;
180 gPARMS->SetDefaultParameter("KALMAN:ESTIMATE_T0_TB",ESTIMATE_T0_TB);
181
182 ENABLE_BOUNDARY_CHECK=true;
183 gPARMS->SetDefaultParameter("GEOM:ENABLE_BOUNDARY_CHECK",
184 ENABLE_BOUNDARY_CHECK);
185
186 USE_MULS_COVARIANCE=true;
187 gPARMS->SetDefaultParameter("TRKFIT:USE_MULS_COVARIANCE",
188 USE_MULS_COVARIANCE);
189
190 RECOVER_BROKEN_TRACKS=true;
191 gPARMS->SetDefaultParameter("KALMAN:RECOVER_BROKEN_TRACKS",RECOVER_BROKEN_TRACKS);
192
193 MIN_FIT_P = 0.050; // GeV
194 gPARMS->SetDefaultParameter("TRKFIT:MIN_FIT_P", MIN_FIT_P, "Minimum fit momentum in GeV/c for fit to be considered successful");
195
196 NUM_CDC_SIGMA_CUT=3.5;
197 NUM_FDC_SIGMA_CUT=3.5;
198 gPARMS->SetDefaultParameter("KALMAN:NUM_CDC_SIGMA_CUT",NUM_CDC_SIGMA_CUT,
199 "maximum distance in number of sigmas away from projection to accept cdc hit");
200 gPARMS->SetDefaultParameter("KALMAN:NUM_FDC_SIGMA_CUT",NUM_FDC_SIGMA_CUT,
201 "maximum distance in number of sigmas away from projection to accept fdc hit");
202
203 FORWARD_PARMS_COV=false;
204 gPARMS->SetDefaultParameter("KALMAN:FORWARD_PARMS_COV",FORWARD_PARMS_COV);
205
206 DApplication* dapp = dynamic_cast<DApplication*>(loop->GetJApplication());
207
208 if(DEBUG_HISTS){
209 dapp->Lock();
210
211 Hstepsize=(TH2F*)gROOT->FindObject("Hstepsize");
212 if (!Hstepsize){
213 Hstepsize=new TH2F("Hstepsize","step size numerator",
214 362,0,362,130,0,65);
215 Hstepsize->SetXTitle("z (cm)");
216 Hstepsize->SetYTitle("r (cm)");
217 }
218 HstepsizeDenom=(TH2F*)gROOT->FindObject("HstepsizeDenom");
219 if (!HstepsizeDenom){
220 HstepsizeDenom=new TH2F("HstepsizeDenom","step size denominator",
221 362,0,362,130,0,65);
222 HstepsizeDenom->SetXTitle("z (cm)");
223 HstepsizeDenom->SetYTitle("r (cm)");
224 }
225
226 cdc_residuals=(TH2F*)gROOT->FindObject("cdc_residuals");
227 if (!cdc_residuals){
228 cdc_residuals=new TH2F("cdc_residuals","residuals vs ring",
229 30,0.5,30.5,1000,-5,5);
230 cdc_residuals->SetXTitle("ring number");
231 cdc_residuals->SetYTitle("#Deltad (cm)");
232 }
233
234 fdc_xresiduals=(TH2F*)gROOT->FindObject("fdc_xresiduals");
235 if (!fdc_xresiduals){
236 fdc_xresiduals=new TH2F("fdc_xresiduals","x residuals vs z",
237 200,170.,370.,1000,-5,5.);
238 fdc_xresiduals->SetXTitle("z (cm)");
239 fdc_xresiduals->SetYTitle("#Deltax (cm)");
240 }
241 fdc_yresiduals=(TH2F*)gROOT->FindObject("fdc_yresiduals");
242 if (!fdc_yresiduals){
243 fdc_yresiduals=new TH2F("fdc_yresiduals","y residuals vs z",
244 200,170.,370.,1000,-5,5.);
245 fdc_yresiduals->SetXTitle("z (cm)");
246 fdc_yresiduals->SetYTitle("#Deltay (cm)");
247 }
248 thetay_vs_thetax=(TH2F*)gROOT->FindObject("thetay_vs_thetax");
249 if (!thetay_vs_thetax){
250 thetay_vs_thetax=new TH2F("thetay_vs_thetax","#thetay vs. #thetax",
251 360,-90.,90.,360,-90,90.);
252 thetay_vs_thetax->SetXTitle("z (cm)");
253 thetay_vs_thetax->SetYTitle("#Deltay (cm)");
254 }
255
256
257 fdc_t0=(TH2F*)gROOT->FindObject("fdc_t0");
258 if (!fdc_t0){
259 fdc_t0=new TH2F("fdc_t0","t0 estimate from tracks vs momentum",100,0,7,200,-50,50);
260 }
261 fdc_t0_timebased=(TH2F*)gROOT->FindObject("fdc_t0_timebased");
262 if (!fdc_t0_timebased){
263 fdc_t0_timebased=new TH2F("fdc_t0_timebased","time-based t0 estimate from tracks vs momentum",100,0,7,200,-50,50);
264 }
265 fdc_t0_vs_theta=(TH2F*)gROOT->FindObject("fdc_t0_vs_theta");
266 if (!fdc_t0_vs_theta){
267 fdc_t0_vs_theta=new TH2F("fdc_t0_vs_theta","t0 estimate from tracks vs. #theta",140,0,140,200,-50,50);
268 }
269 fdc_t0_timebased_vs_theta=(TH2F*)gROOT->FindObject("fdc_t0_timebased_vs_theta");
270 if (!fdc_t0_timebased_vs_theta){
271 fdc_t0_timebased_vs_theta=new TH2F("fdc_t0_timebased_vs_theta","t0_timebased estimate from tracks vs. #theta",140,0,140,200,-50,50);
272 }
273 cdc_drift=(TH2F*)gROOT->FindObject("cdc_drift");
274 if (!cdc_drift){
275 cdc_drift=new TH2F("cdc_drift","cdc drift distance vs time",400,-20,780.,
276 100,0.0,1.0);
277 }
278 cdc_time_vs_d=(TH2F*)gROOT->FindObject("cdc_time_vs_d");
279 if (!cdc_time_vs_d){
280 cdc_time_vs_d=new TH2F("cdc_time_vs_d","cdc drift time vs distance",100,0,0.8,
281 400,-20,780.);
282 }
283
284
285 cdc_res=(TH2F*)gROOT->FindObject("cdc_res");
286 if (!cdc_res){
287 cdc_res=new TH2F("cdc_res","cdc #deltad vs time",200,-20,780,
288 100,-0.1,0.1);
289 }
290 cdc_res_vs_tanl=(TH2F*)gROOT->FindObject("cdc_res_vs_tanl");
291 if (!cdc_res_vs_tanl){
292 cdc_res_vs_tanl=new TH2F("cdc_res_vs_tanl","cdc #deltad vs #theta",
293 100,-5,5,
294 200,-0.1,0.1);
295 }
296 cdc_res_vs_dE=(TH2F*)gROOT->FindObject("cdc_res_vs_dE");
297 if (!cdc_res_vs_dE){
298 cdc_res_vs_dE=new TH2F("cdc_res_vs_dE","cdc #deltad vs #DeltaE",
299 100,0,10e-5,
300 200,-0.1,0.1);
301 }
302 cdc_res_vs_B=(TH2F*)gROOT->FindObject("cdc_res_vs_B");
303 if (!cdc_res_vs_B){
304 cdc_res_vs_B=new TH2F("cdc_res_vs_B","cdc #deltad vs B",
305 100,1.55,2.15,
306 200,-0.1,0.1);
307 }
308 cdc_drift_vs_B=(TH2F*)gROOT->FindObject("cdc_drift_vs_B");
309 if (!cdc_drift_vs_B){
310 cdc_drift_vs_B=new TH2F("cdc_drift_vs_B","cdc #deltad vs B",
311 100,1.55,2.15,
312 200,0,800.0);
313 }
314 cdc_drift_forward=(TH2F*)gROOT->FindObject("cdc_drift_forward");
315 if (!cdc_drift_forward){
316 cdc_drift_forward=new TH2F("cdc_drift_forward","cdc drift distance vs time",400,-20,780.,
317 100,0.0,1.0);
318 }
319 cdc_res_forward=(TH2F*)gROOT->FindObject("cdc_res_forward");
320 if (!cdc_res_forward){
321 cdc_res_forward=new TH2F("cdc_res_forward","cdc #deltad vs time",400,-20,780,
322 200,-0.1,0.1);
323 }
324 fdc_drift=(TH2F*)gROOT->FindObject("fdc_drift");
325 if (!fdc_drift){
326 fdc_drift=new TH2F("fdc_drift","fdc drift distance vs time",200,-20,380.,
327 100,0.0,1.0);
328 }
329 fdc_time_vs_d=(TH2F*)gROOT->FindObject("fdc_time_vs_d");
330 if (!fdc_time_vs_d){
331 fdc_time_vs_d=new TH2F("fdc_time_vs_d","fdc drift time vs distance",100,0,0.5,
332 200,-20,380.);
333 }
334 fdc_drift_vs_B=(TH2F*)gROOT->FindObject("fdc_drift_vs_B");
335 if (!fdc_drift_vs_B){
336 fdc_drift_vs_B=new TH2F("fdc_drift_vs_B","fdc t vs B",
337 100,1.55,2.35,
338 200,50,250.0);
339 }
340
341
342 fdc_yres=(TH2F*)gROOT->FindObject("fdc_yres");
343 if (!fdc_yres){
344 fdc_yres=new TH2F("fdc_yres",
345 "fdc yres",2304,0.5,2304.5,
346 100,-5,5);
347 }
348 fdc_yres_vs_tanl=(TH2F*)gROOT->FindObject("fdc_yres_vs_tanl");
349 if (!fdc_yres_vs_tanl){
350 fdc_yres_vs_tanl=new TH2F("fdc_yres_vs_tanl",
351 "fdc yres vs tanl",60,0,60.0,
352 50,-5.,5.);
353 }
354
355 fdc_xres=(TH2F*)gROOT->FindObject("fdc_xres");
356 if (!fdc_xres){
357 fdc_xres=new TH2F("fdc_xres",
358 "fdc xres vs tdrift",200,-20,380,
359 200,-0.1,0.1);
360 }
361 dapp->Unlock();
362 }
363
364
365 JCalibration *jcalib = dapp->GetJCalibration(0); // need run number here
366 typedef map<string,float>::iterator iter_float;
367 vector< map<string, float> > tvals;
368 if (jcalib->Get("CDC/cdc_drift", tvals)==false){
369 for(unsigned int i=0; i<tvals.size(); i++){
370 map<string, float> &row = tvals[i];
371 iter_float iter = row.begin();
372 cdc_drift_table[i] = iter->second;
373 //_DBG_ << i <<" "<< iter->second << endl;
374
375 }
376 }
377 else{
378 jerr << " CDC time-to-distance table not available... bailing..." << endl;
379 exit(0);
380 }
381
382 if (jcalib->Get("FDC/fdc_drift2", tvals)==false){
383 for(unsigned int i=0; i<tvals.size(); i++){
384 map<string, float> &row = tvals[i];
385 iter_float iter = row.begin();
386 fdc_drift_table[i] = iter->second;
387 }
388 }
389 else{
390 jerr << " FDC time-to-distance table not available... bailing..." << endl;
391 exit(0);
392 }
393
394 FDC_CATHODE_SIGMA=0.;
395 map<string, double> fdcparms;
396 jcalib->Get("FDC/fdc_parms", fdcparms);
397 FDC_CATHODE_SIGMA = fdcparms["FDC_CATHODE_SIGMA"];
398
399 double Bref=1.83;
400 CDC_DRIFT_B_SCALE=CDC_DRIFT_B_SCALE_PAR1602.0+CDC_DRIFT_B_SCALE_PAR258.22*Bref;
401
402 Bref=2.27;
403 FDC_DRIFT_B_SCALE=FDC_DRIFT_B_SCALE_PAR156.03+FDC_DRIFT_B_SCALE_PAR29.122*Bref;
404
405 for (unsigned int i=0;i<5;i++)I5x5(i,i)=1.;
406}
407
408//-----------------
409// ResetKalmanSIMD
410//-----------------
411void DTrackFitterKalmanSIMD::ResetKalmanSIMD(void)
412{
413 last_material_map=0;
414
415 for (unsigned int i=0;i<my_cdchits.size();i++){
416 delete my_cdchits[i];
417 }
418 for (unsigned int i=0;i<my_fdchits.size();i++){
419 delete my_fdchits[i];
420 }
421 central_traj.clear();
422 forward_traj.clear();
423 my_fdchits.clear();
424 my_cdchits.clear();
425 fdc_updates.clear();
426 cdc_updates.clear();
427
428 cov.clear();
429 fcov.clear();
430 pulls.clear();
431
432 len = 0.0;
433 ftime=0.0;
434 x_=0.,y_=0.,tx_=0.,ty_=0.,q_over_p_ = 0.0;
435 z_=0.,phi_=0.,tanl_=0.,q_over_pt_ =0, D_= 0.0;
436 chisq_ = 0.0;
437 ndf_ = 0;
438 MASS=0.13957;
439 mass2=MASS*MASS;
440 Bx=By=0.;
441 Bz=-2.;
442 dBxdx=0.,dBxdy=0.,dBxdz=0.,dBydx=0.,dBydy=0.,dBydy=0.,dBzdx=0.,dBzdy=0.,dBzdz=0.;
443 // Step sizes
444 mStepSizeS=2.0;
445 mStepSizeZ=2.0;
446 /*
447 if (fit_type==kTimeBased){
448 mStepSizeS=0.5;
449 mStepSizeZ=0.5;
450 }
451 */
452 last_smooth=false;
453 mT0=0.,mT0MinimumDriftTime=0.,mT0Average=0.;
454 mInvVarT0=0.;
455 mVarT0=0.;
456
457 mCDCInternalStepSize=0.8;
458
459 mMinDriftTime=1000.;
460 mMinDriftID=0;
461
462}
463
464//-----------------
465// FitTrack
466//-----------------
467DTrackFitter::fit_status_t DTrackFitterKalmanSIMD::FitTrack(void)
468{
469 // Reset member data and free an memory associated with the last fit,
470 // but some of which only for wire-based fits
471 ResetKalmanSIMD();
472
473 // Check that we have enough FDC and CDC hits to proceed
474 if (cdchits.size()+fdchits.size()<6) return kFitFailed;
475
476 // Copy hits from base class into structures specific to DTrackFitterKalmanSIMD
477 if (cdchits.size()>=MIN_CDC_HITS2)
478 for(unsigned int i=0; i<cdchits.size(); i++)AddCDCHit(cdchits[i]);
479 if (fdchits.size()>=MIN_FDC_HITS2)
480 for(unsigned int i=0; i<fdchits.size(); i++)AddFDCHit(fdchits[i]);
481
482 if(my_cdchits.size()+my_fdchits.size()<6) return kFitFailed;
483
484 // Create vectors of updates (from hits) to S and C
485 if (my_cdchits.size()>0) cdc_updates=vector<DKalmanUpdate_t>(my_cdchits.size());
486 if (my_fdchits.size()>0) fdc_updates=vector<DKalmanUpdate_t>(my_fdchits.size());
487
488 // Order the cdc hits by ring number
489 if (my_cdchits.size()>0){
490 sort(my_cdchits.begin(),my_cdchits.end(),DKalmanSIMDCDCHit_cmp);
491
492 // For 2 adjacent hits in a single ring, swap hits from the default
493 // ordering according to the phi values relative to the phi of the
494 // innermost hit.
495 /*
496 if (my_cdchits.size()>1){
497 double phi0=my_cdchits[0]->hit->wire->origin.Phi();
498 for (unsigned int i=0;i<my_cdchits.size()-1;i++){
499 if (my_cdchits[i]->hit->wire->ring
500 ==my_cdchits[i+1]->hit->wire->ring){
501 double phi1=my_cdchits[i]->hit->wire->origin.Phi();
502 double phi2=my_cdchits[i+1]->hit->wire->origin.Phi();
503 if (fabs(phi1-phi0)>fabs(phi2-phi0)){
504 DKalmanSIMDCDCHit_t a=*my_cdchits[i];
505 DKalmanSIMDCDCHit_t b=*my_cdchits[i+1];
506 *my_cdchits[i]=b;
507 *my_cdchits[i+1]=a;
508 }
509 if (my_cdchits[i]->hit->wire->straw==my_cdchits[i+1]->hit->wire->straw)
510 printf("two hits\n");
511 printf("Swapping\n");
512 // my_cdchits[i+1]->status=1;
513 }
514 }
515 }
516 */
517 }
518 // Order the fdc hits by z
519 if (my_fdchits.size()>0){
520 sort(my_fdchits.begin(),my_fdchits.end(),DKalmanSIMDFDCHit_cmp);
521 }
522
523 // start time and variance
524 mT0=NaNstd::numeric_limits<double>::quiet_NaN();
525 switch(input_params.t0_detector()){
526 case SYS_TOF:
527 mVarT0=0.01;
528 break;
529 case SYS_CDC:
530 mVarT0=25.;
531 break;
532 case SYS_FDC:
533 mVarT0=100.;
534 break;
535 case SYS_BCAL:
536 mVarT0=0.25;
537 break;
538 default:
539 mVarT0=0.09;
540 break;
541 }
542
543 if (fit_type==kTimeBased){
544 mT0=input_params.t0();
545 }
546
547 //Set the mass
548 MASS=input_params.mass();
549 mass2=MASS*MASS;
550 m_ratio=ELECTRON_MASS0.000511/MASS;
551 m_ratio_sq=m_ratio*m_ratio;
552
553 if (DEBUG_LEVEL>0){
554 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
554<<" " << "------Starting "
555 <<(fit_type==kTimeBased?"Time-based":"Wire-based")
556 << " Fit with " << my_fdchits.size() << " FDC hits and "
557 << my_cdchits.size() << " CDC hits.-------" <<endl;
558 if (fit_type==kTimeBased){
559 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
559<<" " << " Using t0=" << mT0 << " from DET="
560 << input_params.t0_detector() <<endl;
561 }
562 }
563 // Do the fit
564 jerror_t error = KalmanLoop();
565 if (error!=NOERROR){
566 if (DEBUG_LEVEL>0)
567 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
567<<" " << "Fit failed with Error = " << error <<endl;
568 return kFitFailed;
569 }
570
571 // Copy fit results into DTrackFitter base-class data members
572 DVector3 mom,pos;
573 GetPosition(pos);
574 GetMomentum(mom);
575 double charge = GetCharge();
576 fit_params.setPosition(pos);
577 fit_params.setMomentum(mom);
578 fit_params.setCharge(charge);
579 fit_params.setMass(MASS);
580
581 if (DEBUG_LEVEL>0)
582 cout << "----- Pass: "
583 << (fit_type==kTimeBased?"Time-based ---":"Wire-based ---")
584 << " Mass: " << MASS
585 << " p=" << mom.Mag()
586 << " theta=" << 90.0-180./M_PI3.14159265358979323846*atan(tanl_)
587 << " vertex=(" << x_ << "," << y_ << "," << z_<<")"
588 << " chi2=" << chisq_
589 <<endl;
590
591
592 // Start time (t0) estimate
593 double my_t0=-1000.;
594 if (mInvVarT0>0){
595 my_t0=mT0Average;
596 fit_params.setT0(my_t0,1./sqrt(mInvVarT0),
597 my_fdchits.size()>0?SYS_FDC:SYS_CDC);
598 }
599 else{
600 my_t0=mT0MinimumDriftTime;
601 fit_params.setT0(my_t0,10.,(mMinDriftID<1000)?SYS_FDC:SYS_CDC);
602 }
603 if (DEBUG_HISTS){
604 double my_p=mom.Mag();
605 double my_theta=mom.Theta()*180./M_PI3.14159265358979323846;
606 if (fit_type==kWireBased){
607 fdc_t0->Fill(my_p,my_t0);
608 fdc_t0_vs_theta->Fill(my_theta,my_t0);
609 }
610 else{
611 fdc_t0_timebased->Fill(my_p,my_t0);
612 fdc_t0_timebased_vs_theta->Fill(my_theta,my_t0);
613 }
614 }
615
616 DMatrixDSym errMatrix(5);
617 // Fill the tracking error matrix and the one needed for kinematic fitting
618 if (FORWARD_PARMS_COV && fcov.size()!=0){
619 fit_params.setForwardParmFlag(true);
620
621 // We MUST fill the entire matrix (not just upper right) even though
622 // this is a DMatrixDSym
623 for (unsigned int i=0;i<5;i++){
624 for (unsigned int j=0;j<5;j++){
625 errMatrix(i,j)=fcov[i][j];
626 }
627 }
628 fit_params.setTrackingStateVector(x_,y_,tx_,ty_,q_over_p_);
629
630 // Compute and fill the error matrix needed for kinematic fitting
631 fit_params.setErrorMatrix(Get7x7ErrorMatrixForward(errMatrix));
632 }
633 else{
634 fit_params.setForwardParmFlag(false);
635
636 // We MUST fill the entire matrix (not just upper right) even though
637 // this is a DMatrixDSym
638 for (unsigned int i=0;i<5;i++){
639 for (unsigned int j=0;j<5;j++){
640 errMatrix(i,j)=cov[i][j];
641 }
642 }
643 fit_params.setTrackingStateVector(q_over_pt_,phi_,tanl_,D_,z_);
644
645 // Compute and fill the error matrix needed for kinematic fitting
646 fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix));
647 }
648 fit_params.setTrackingErrorMatrix(errMatrix);
649 this->chisq = GetChiSq();
650 this->Ndof = GetNDF();
651 fit_status = kFitSuccess;
652
653 // Check that the momentum is above some minimal amount. If
654 // not, return that the fit failed.
655 if(fit_params.momentum().Mag() < MIN_FIT_P)fit_status = kFitFailed;
656
657
658 // Debug histograms for hit residuals
659 if (DEBUG_HISTS && fit_type==kTimeBased){
660 if (fdc_yresiduals){
661 for (unsigned int i=0;i<my_fdchits.size();i++){
662 if (my_fdchits[i]->yres<1000 && my_fdchits[i]->xres<1000){
663 if (fdc_yresiduals)fdc_yresiduals->Fill(my_fdchits[i]->z,
664 my_fdchits[i]->yres
665 /my_fdchits[i]->ysig);
666 if (fdc_xresiduals)fdc_xresiduals->Fill(my_fdchits[i]->z,
667 my_fdchits[i]->xres/
668 my_fdchits[i]->xsig);
669 }
670 }
671 }
672 }
673
674
675 //printf("========= done!\n");
676
677 return fit_status;
678}
679
680//-----------------
681// ChiSq
682//-----------------
683double DTrackFitterKalmanSIMD::ChiSq(fit_type_t fit_type, DReferenceTrajectory *rt, double *chisq_ptr, int *dof_ptr, vector<pull_t> *pulls_ptr)
684{
685 // This simply returns whatever was left in for the chisq/NDF from the last fit.
686 // Using a DReferenceTrajectory is not really appropriate here so the base class'
687 // requirement of it should be reviewed.
688 double chisq = GetChiSq();
689 unsigned int ndf = GetNDF();
690
691 if(chisq_ptr)*chisq_ptr = chisq;
692 if(dof_ptr)*dof_ptr = int(ndf);
693 if(pulls_ptr)*pulls_ptr = pulls;
694
695 return chisq/double(ndf);
696}
697
698// Initialize the state vector
699jerror_t DTrackFitterKalmanSIMD::SetSeed(double q,DVector3 pos, DVector3 mom){
700 if (!isfinite(pos.Mag()) || !isfinite(mom.Mag())){
701 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
701<<" " << "Invalid seed data." <<endl;
702 return UNRECOVERABLE_ERROR;
703 }
704 if (mom.Mag()<MIN_FIT_P){
705 mom.SetMag(MIN_FIT_P);
706 }
707 else if (MASS>0.9 && mom.Mag()<MIN_PROTON_P0.3){
708 mom.SetMag(MIN_PROTON_P0.3);
709 }
710 else if (MASS<0.9 && mom.Mag()<MIN_FIT_P){
711 mom.SetMag(MIN_PION_P0.08);
712 }
713 if (mom.Mag()>MAX_P12.0){
714 mom.SetMag(MAX_P12.0);
715 }
716
717 // Forward parameterization
718 x_=pos.x();
719 y_=pos.y();
720 z_=pos.z();
721 tx_= mom.x()/mom.z();
722 ty_= mom.y()/mom.z();
723 q_over_p_=q/mom.Mag();
724
725 // Central parameterization
726 phi_=mom.Phi();
727 tanl_=tan(M_PI_21.57079632679489661923-mom.Theta());
728 q_over_pt_=q/mom.Perp();
729
730 return NOERROR;
731}
732
733// Return the momentum at the distance of closest approach to the origin.
734inline void DTrackFitterKalmanSIMD::GetMomentum(DVector3 &mom){
735 double pt=1./fabs(q_over_pt_);
736 mom.SetXYZ(pt*cos(phi_),pt*sin(phi_),pt*tanl_);
737}
738
739// Return the "vertex" position (position at which track crosses beam line)
740inline void DTrackFitterKalmanSIMD::GetPosition(DVector3 &pos){
741 pos.SetXYZ(x_,y_,z_);
742}
743
744// Add FDC hits
745jerror_t DTrackFitterKalmanSIMD::AddFDCHit(const DFDCPseudo *fdchit){
746 DKalmanSIMDFDCHit_t *hit= new DKalmanSIMDFDCHit_t;
747
748 hit->package=fdchit->wire->layer/6;
749 hit->t=fdchit->time;
750 hit->uwire=fdchit->w;
751 hit->vstrip=fdchit->s;
752 hit->z=fdchit->wire->origin.z();
753 hit->cosa=fdchit->wire->udir.y();
754 hit->sina=fdchit->wire->udir.x();
755 hit->nr=0.;
756 hit->nz=0.;
757 hit->dE=1e6*fdchit->dE;
758 hit->xres=hit->yres=1000.;
759 hit->hit=fdchit;
760
761 my_fdchits.push_back(hit);
762
763 if (hit->t<mMinDriftTime){
764 mMinDriftTime=hit->t;
765 mMinDriftID=my_fdchits.size();
766 }
767
768 return NOERROR;
769}
770
771// Add CDC hits
772jerror_t DTrackFitterKalmanSIMD::AddCDCHit (const DCDCTrackHit *cdchit){
773 DKalmanSIMDCDCHit_t *hit= new DKalmanSIMDCDCHit_t;
774
775 hit->hit=cdchit;
776 hit->status=good_hit;
777 hit->residual=1000.;
778 my_cdchits.push_back(hit);
779
780 if (hit->hit->tdrift<mMinDriftTime){
781 mMinDriftTime=hit->hit->tdrift;
782 mMinDriftID=my_cdchits.size()+999;
783 }
784
785 return NOERROR;
786}
787
788// Calculate the derivative of the state vector with respect to z
789jerror_t DTrackFitterKalmanSIMD::CalcDeriv(double z,double dz,
790 const DMatrix5x1 &S,
791 double dEdx,
792 DMatrix5x1 &D){
793 double x=S(state_x), y=S(state_y),tx=S(state_tx),ty=S(state_ty);
794 double q_over_p=S(state_q_over_p);
795
796 //B-field at (x,y,z)
797 bfield->GetField(x,y,z,Bx,By,Bz);
798
799 // Don't let the magnitude of the momentum drop below some cutoff
800 if (fabs(q_over_p)>Q_OVER_P_MAX100.){
801 q_over_p=Q_OVER_P_MAX100.*(q_over_p>0?1.:-1.);
802 dEdx=0.;
803 }
804 // Try to keep the direction tangents from heading towards 90 degrees
805 if (fabs(tx)>TAN_MAX10.) tx=TAN_MAX10.*(tx>0?1.:-1.);
806 if (fabs(ty)>TAN_MAX10.) ty=TAN_MAX10.*(ty>0?1.:-1.);
807
808 // useful combinations of terms
809 double kq_over_p=qBr2p0.003*q_over_p;
810 double tx2=tx*tx;
811 double ty2=ty*ty;
812 double txty=tx*ty;
813 double one_plus_tx2=1.+tx2;
814 double dsdz=sqrt(one_plus_tx2+ty2);
815 double dtx_Bfac=ty*Bz+txty*Bx-one_plus_tx2*By;
816 double dty_Bfac=Bx*(1.+ty2)-txty*By-tx*Bz;
817 double kq_over_p_dsdz=kq_over_p*dsdz;
818 // double kq_over_p_ds=0.5*dz*kq_over_p_dsdz;
819
820 // Derivative of S with respect to z
821 D(state_x)=tx;
822 D(state_y)=ty;
823 D(state_tx)=kq_over_p_dsdz*dtx_Bfac;
824 D(state_ty)=kq_over_p_dsdz*dty_Bfac;
825
826 D(state_q_over_p)=0.;
827 if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){
828 double q_over_p_sq=q_over_p*q_over_p;
829 double E=sqrt(1./q_over_p_sq+mass2);
830 D(state_q_over_p)=-q_over_p_sq*q_over_p*E*dEdx*dsdz;
831 }
832 return NOERROR;
833}
834
835// Reference trajectory for forward tracks in CDC region
836// At each point we store the state vector and the Jacobian needed to get to
837//this state along z from the previous state.
838jerror_t DTrackFitterKalmanSIMD::SetCDCForwardReferenceTrajectory(DMatrix5x1 &S){
839 int i=0,forward_traj_length=forward_traj.size();
840 double z=z_;
841 double r=0.;
842 bool stepped_to_boundary=false;
843
844 // Coordinates for outermost cdc hit
845 unsigned int id=my_cdchits.size()-1;
846 DVector3 origin=my_cdchits[id]->hit->wire->origin;
847 DVector3 dir=my_cdchits[id]->hit->wire->udir;
848 double uz=dir.z();
849 double z0=origin.z();
850 double rmax=R_MAX65.0;
851
852 // Magnetic field at beginning of trajectory
853 bfield->GetField(x_,y_,z_,Bx,By,Bz);
854
855 // Reset cdc status flags
856 for (unsigned int i=0;i<my_cdchits.size();i++){
857 my_cdchits[i]->status=good_hit;
858 }
859
860 // Continue adding to the trajectory until we have reached the endplate
861 // or the maximum radius
862 while(z<endplate_z &&
863 r<rmax && fabs(S(state_q_over_p))<Q_OVER_P_MAX100.
864 && z>cdc_origin[2]){
865 if (PropagateForwardCDC(forward_traj_length,i,z,r,S,stepped_to_boundary)
866 !=NOERROR) return UNRECOVERABLE_ERROR;
867 rmax=(origin+((z-z0)/uz)*dir).Perp()+DELTA_R1.0;
868 }
869
870 // Only use hits that would fall within the range of the reference trajectory
871 for (unsigned int i=0;i<my_cdchits.size();i++){
872 DVector3 origin=my_cdchits[i]->hit->wire->origin;
873 double z0=origin.z();
874 DVector3 dir=my_cdchits[i]->hit->wire->udir;
875 double uz=dir.z();
876 double my_r=(origin+((z-z0)/uz)*dir).Perp();
877 if (my_r>r) my_cdchits[i]->status=bad_hit;
878 }
879
880 // If the current length of the trajectory deque is less than the previous
881 // trajectory deque, remove the extra elements and shrink the deque
882 if (i<(int)forward_traj.size()){
883 forward_traj_length=forward_traj.size();
884 for (int j=0;j<forward_traj_length-i;j++){
885 forward_traj.pop_front();
886 }
887 }
888
889 // return an error if there are still no entries in the trajectory
890 if (forward_traj.size()==0) return RESOURCE_UNAVAILABLE;
891
892 if (DEBUG_LEVEL>10)
893 {
894 cout << "--- Forward cdc trajectory ---" <<endl;
895 for (unsigned int m=0;m<forward_traj.size();m++){
896 // DMatrix5x1 S=*(forward_traj[m].S);
897 DMatrix5x1 S=(forward_traj[m].S);
898 double tx=S(state_tx),ty=S(state_ty);
899 double phi=atan2(ty,tx);
900 double cosphi=cos(phi);
901 double sinphi=sin(phi);
902 double p=fabs(1./S(state_q_over_p));
903 double tanl=1./sqrt(tx*tx+ty*ty);
904 double sinl=sin(atan(tanl));
905 double cosl=cos(atan(tanl));
906 cout
907 << setiosflags(ios::fixed)<< "pos: " << setprecision(4)
908 << forward_traj[m].pos.x() << ", "
909 << forward_traj[m].pos.y() << ", "
910 << forward_traj[m].pos.z() << " mom: "
911 << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", "
912 << p*sinl << " -> " << p
913 <<" s: " << setprecision(3)
914 << forward_traj[m].s
915 <<" t: " << setprecision(3)
916 << forward_traj[m].t
917 << endl;
918 }
919 }
920
921 // Current state vector
922 S=forward_traj[0].S;
923
924 // position at the end of the swim
925 z_=forward_traj[0].pos.Z();
926 x_=forward_traj[0].pos.X();
927 y_=forward_traj[0].pos.Y();
928
929 return NOERROR;
930}
931
932// Routine that extracts the state vector propagation part out of the reference
933// trajectory loop
934jerror_t DTrackFitterKalmanSIMD::PropagateForwardCDC(int length,int &index,
935 double &z,double &r,
936 DMatrix5x1 &S,
937 bool &stepped_to_boundary){
938 DMatrix5x5 J,Q;
939 DKalmanSIMDState_t temp;
940 int my_i=0;
941 temp.h_id=0;
942 double dEdx=0.;
943 double s_to_boundary=1e6;
944 double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
945
946 // State at current position
947 temp.pos.SetXYZ(S(state_x),S(state_y),z);
948 // radius of hit
949 r=temp.pos.Perp();
950
951 temp.s=len;
952 temp.t=ftime;
953 temp.Z=temp.K_rho_Z_over_A=temp.rho_Z_over_A=temp.LnI=0.; //initialize
954 temp.chi2c_factor=temp.chi2a_factor=temp.chi2a_corr=0.;
955
956 //if (r<r_outer_hit)
957 if (z<endplate_z && r<endplate_rmax && z>cdc_origin[2])
958 {
959 // get material properties from the Root Geometry
960 if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){
961 DVector3 mom(S(state_tx),S(state_ty),1.);
962 if(geom->FindMatKalman(temp.pos,mom,temp.Z,temp.K_rho_Z_over_A,
963 temp.rho_Z_over_A,temp.LnI,
964 temp.chi2c_factor,temp.chi2a_factor,
965 temp.chi2a_corr,
966 last_material_map,
967 &s_to_boundary)!=NOERROR){
968 return UNRECOVERABLE_ERROR;
969 }
970 }
971 else
972 {
973 if(geom->FindMatKalman(temp.pos,temp.Z,temp.K_rho_Z_over_A,
974 temp.rho_Z_over_A,temp.LnI,
975 temp.chi2c_factor,temp.chi2a_factor,
976 temp.chi2a_corr,
977 last_material_map)!=NOERROR){
978 return UNRECOVERABLE_ERROR;
979 }
980 }
981
982 // Get dEdx for the upcoming step
983 if (CORRECT_FOR_ELOSS){
984 dEdx=GetdEdx(S(state_q_over_p),temp.K_rho_Z_over_A,temp.rho_Z_over_A,
985 temp.LnI);
986 }
987 }
988 index++;
989 if (index<=length){
990 my_i=length-index;
991 forward_traj[my_i].s=temp.s;
992 forward_traj[my_i].t=temp.t;
993 forward_traj[my_i].h_id=temp.h_id;
994 forward_traj[my_i].pos=temp.pos;
995 forward_traj[my_i].Z=temp.Z;
996 forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A;
997 forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A;
998 forward_traj[my_i].LnI=temp.LnI;
999 forward_traj[my_i].S=S;
1000 }
1001 else{
1002 temp.S=S;
1003 }
1004
1005 // Determine the step size based on energy loss
1006 //double step=mStepSizeS*dz_ds;
1007 double ds=mStepSizeS;
1008 if (z<endplate_z && r<endplate_rmax && z>cdc_origin[2]){
1009 if (!stepped_to_boundary){
1010 stepped_to_boundary=false;
1011 if (fabs(dEdx)>EPS3.0e-8){
1012 ds=(fit_type==kWireBased?DE_PER_STEP_WIRE_BASED0.0005:DE_PER_STEP_TIME_BASED0.0005)
1013 /fabs(dEdx);
1014 }
1015 /*
1016 if (fabs(dBzdz)>EPS){
1017 double my_step_size_B=BFIELD_FRAC*fabs(Bz/dBzdz)/dz_ds;
1018 if (my_step_size_B<ds){
1019 ds=my_step_size_B;
1020 }
1021 }
1022 */
1023 if(ds>mStepSizeS) ds=mStepSizeS;
1024 double my_r=sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y));
1025 double cdc_step_size_cut=mCDCInternalStepSize
1026 *(1.+0.25/(S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)));
1027 if (ds>cdc_step_size_cut && my_r>endplate_rmin
1028 && my_r<endplate_rmax && z<endplate_z)
1029 ds=cdc_step_size_cut;
1030 if (s_to_boundary<ds){
1031 ds=s_to_boundary;
1032 stepped_to_boundary=true;
1033 }
1034 if(ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1;
1035 }
1036 else{
1037 ds=MIN_STEP_SIZE0.1;
1038 stepped_to_boundary=false;
1039 }
1040 }
1041
1042 if (DEBUG_HISTS && fit_type==kTimeBased){
1043 if (Hstepsize && HstepsizeDenom){
1044 Hstepsize->Fill(z,sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y))
1045 ,ds);
1046 HstepsizeDenom->Fill(z,sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y)));
1047 }
1048 }
1049 double newz=z+ds*dz_ds; // new z position
1050
1051 // Deal with the CDC endplate
1052 //if (newz>endplate_z){
1053 // step=endplate_z-z+0.01;
1054 // newz=endplate_z+0.01;
1055 // }
1056
1057 // Step through field
1058 ds=Step(z,newz,dEdx,S);
1059 len+=fabs(ds);
1060
1061 double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
1062 double one_over_beta2=1.+mass2*q_over_p_sq;
1063 if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8;
1064 ftime+=ds*sqrt(one_over_beta2);// in units where c=1
1065
1066 // Get the contribution to the covariance matrix due to multiple
1067 // scattering
1068 GetProcessNoise(ds,temp.chi2c_factor,temp.chi2a_factor,temp.chi2a_corr,S,Q);
1069
1070 // Energy loss straggling
1071 if (CORRECT_FOR_ELOSS){
1072 double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A);
1073 Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2;
1074 /*
1075 if (Q(state_q_over_p,state_q_over_p)>1.) {
1076 printf("Greater than 1? %f %f %f\n",varE,
1077 Q(state_q_over_p,state_q_over_p),S(state_q_over_p));
1078 Q(state_q_over_p,state_q_over_p)=1.;
1079 }
1080 */
1081 }
1082
1083 // Compute the Jacobian matrix and its transpose
1084 StepJacobian(newz,z,S,dEdx,J);
1085
1086 // update the trajectory
1087 if (index<=length){
1088 // In the bore of the magnet the off-axis components are small
1089 forward_traj[my_i].B=sqrt(Bx*Bx+By*By+Bz*Bz);
1090 forward_traj[my_i].Q=Q;
1091 forward_traj[my_i].J=J;
1092 forward_traj[my_i].JT=J.Transpose();
1093 }
1094 else{
1095 temp.B=sqrt(Bx*Bx+By*By+Bz*Bz);
1096 temp.Q=Q;
1097 temp.J=J;
1098 temp.JT=J.Transpose();
1099 temp.Ckk=Zero5x5;
1100 temp.Skk=Zero5x1;
1101 forward_traj.push_front(temp);
1102 }
1103
1104 //update z
1105 z=newz;
1106
1107 return NOERROR;
1108}
1109
1110// Reference trajectory for central tracks
1111// At each point we store the state vector and the Jacobian needed to get to this state
1112// along s from the previous state.
1113// The tricky part is that we swim out from the target to find Sc and pos along the trajectory
1114// but we need the Jacobians for the opposite direction, because the filter proceeds from
1115// the outer hits toward the target.
1116jerror_t DTrackFitterKalmanSIMD::SetCDCReferenceTrajectory(DVector3 pos,
1117 DMatrix5x1 &Sc){
1118 DKalmanSIMDState_t temp;
1119 DMatrix5x5 J; // State vector Jacobian matrix
1120 DMatrix5x5 Q; // Process noise covariance matrix
1121
1122 // Magnetic field at beginning of trajectory
1123 bfield->GetField(x_,y_,z_,Bx,By,Bz);
1124
1125 // Position, step, radius, etc. variables
1126 DVector3 oldpos;
1127 double dedx=0;
1128 double one_over_beta2=1.,varE=0.,q_over_p=1.,q_over_p_sq=1.;
1129 len=0.;
1130 int i=0;
1131 double t=0.;
1132 double step_size=MIN_STEP_SIZE0.1;
1133 double s_to_boundary=1000.;
1134
1135 // Coordinates for outermost cdc hit
1136 unsigned int id=my_cdchits.size()-1;
1137 DVector3 origin=my_cdchits[id]->hit->wire->origin;
1138 DVector3 dir=my_cdchits[id]->hit->wire->udir;
1139 bool stepped_to_boundary=false;
1140 double uz=dir.z();
1141 double z0=origin.z();
1142 double rmax=R_MAX65.0,r=0;
1143
1144 // Reset cdc status flags
1145 for (unsigned int j=0;j<my_cdchits.size();j++){
1146 my_cdchits[j]->status=good_hit;
1147 }
1148
1149 if (central_traj.size()>0){ // reuse existing deque
1150 // Reset D to zero
1151 Sc(state_D)=0.;
1152
1153 for (int m=central_traj.size()-1;m>=0;m--){
1154 i++;
1155 central_traj[m].s=len;
1156 central_traj[m].t=t;
1157 central_traj[m].pos=pos;
1158 central_traj[m].h_id=0;
1159 central_traj[m].S=Sc;
1160 central_traj[m].S(state_D)=0.; // make sure D=0.
1161
1162 // update path length and flight time
1163 len+=step_size;
1164 q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
1165 //q_over_p_sq=q_over_p*q_over_p;
1166 //t+=step_size*sqrt(1.+mass2*q_over_p_sq)/SPEED_OF_LIGHT;
1167
1168 // Initialize dEdx
1169 dedx=0.;
1170
1171 // Adjust the step size
1172 step_size=mStepSizeS;
1173 if (pos.z()<endplate_z && pos.Perp()<endplate_rmax
1174 && pos.z()>cdc_origin[2]){
1175 // get material properties from the Root Geometry
1176 if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){
1177 DVector3 mom(cos(Sc(state_phi)),sin(Sc(state_phi)),Sc(state_tanl));
1178 if(geom->FindMatKalman(pos,mom,central_traj[m].Z,
1179 central_traj[m].K_rho_Z_over_A,
1180 central_traj[m].rho_Z_over_A,
1181 central_traj[m].LnI,
1182 central_traj[m].chi2c_factor,
1183 central_traj[m].chi2a_factor,
1184 central_traj[m].chi2a_corr,
1185 last_material_map,
1186 &s_to_boundary)!=NOERROR){
1187 return UNRECOVERABLE_ERROR;
1188 }
1189 }
1190 else
1191 {
1192 if(geom->FindMatKalman(pos,central_traj[m].Z,
1193 central_traj[m].K_rho_Z_over_A,
1194 central_traj[m].rho_Z_over_A,
1195 central_traj[m].LnI,
1196 central_traj[m].chi2c_factor,
1197 central_traj[m].chi2a_factor,
1198 central_traj[m].chi2a_corr,
1199 last_material_map)!=NOERROR){
1200 return UNRECOVERABLE_ERROR;
1201 }
1202 }
1203 // Get dEdx for this step
1204 if (CORRECT_FOR_ELOSS){
1205 dedx=GetdEdx(q_over_p,central_traj[m].K_rho_Z_over_A,
1206 central_traj[m].rho_Z_over_A,central_traj[m].LnI);
1207 }
1208
1209 if (!stepped_to_boundary){
1210 stepped_to_boundary=false;
1211 if (fabs(dedx)>EPS3.0e-8){
1212 step_size=
1213 (fit_type==kWireBased?DE_PER_STEP_WIRE_BASED0.0005:DE_PER_STEP_TIME_BASED0.0005)
1214 /fabs(dedx);
1215 }
1216 double my_r=pos.Perp();
1217 /*
1218 if (fabs(dBzdz)>EPS){
1219 double my_step_size_B=BFIELD_FRAC*fabs(Bz/dBzdz/sin(atan(Sc(state_tanl))));
1220 if (my_step_size_B<step_size){
1221 step_size=my_step_size_B;
1222 }
1223 }
1224 */
1225 if(step_size>mStepSizeS) step_size=mStepSizeS;
1226
1227 double cdc_step_size_cut
1228 =mCDCInternalStepSize*(1.+0.25*Sc(state_tanl)*Sc(state_tanl));
1229 if (step_size>cdc_step_size_cut
1230 && my_r>endplate_rmin
1231 && my_r<endplate_rmax
1232 && pos.Z()>cdc_origin[2])
1233 step_size=cdc_step_size_cut;
1234
1235 if (s_to_boundary<step_size){
1236 step_size=s_to_boundary;
1237 stepped_to_boundary=true;
1238 }
1239 if(step_size<MIN_STEP_SIZE0.1)step_size=MIN_STEP_SIZE0.1;
1240 }
1241 else{
1242 step_size=MIN_STEP_SIZE0.1;
1243 stepped_to_boundary=false;
1244 }
1245 }
1246
1247 if (DEBUG_HISTS && fit_type==kTimeBased){
1248 if (Hstepsize && HstepsizeDenom){
1249 Hstepsize->Fill(pos.z(),pos.Perp(),step_size);
1250 HstepsizeDenom->Fill(pos.z(),pos.Perp());
1251 }
1252 }
1253
1254 // Propagate the state through the field
1255 FixedStep(pos,step_size,Sc,dedx,central_traj[m].B);
1256
1257
1258 // Break out of the loop if we would swim out of the fiducial volume
1259 rmax=(origin+((Sc(state_z)-z0)/uz)*dir).Perp()+DELTA_R1.0;
1260 r=pos.Perp();
1261 if (r>rmax || pos.z()<Z_MIN0. || pos.z()>endplate_z
1262 || fabs(1./Sc(state_q_over_pt))<PT_MIN0.01)
1263 break;
1264
1265 // update flight time
1266 q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
1267 q_over_p_sq=q_over_p*q_over_p;
1268 one_over_beta2=1.+mass2*q_over_p_sq;
1269 if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8;
1270 t+=step_size*sqrt(one_over_beta2); // in units of c=1
1271
1272 // Multiple scattering
1273 GetProcessNoiseCentral(step_size,central_traj[m].chi2c_factor,
1274 central_traj[m].chi2a_factor,
1275 central_traj[m].chi2a_corr,Sc,Q);
1276
1277 // Energy loss straggling
1278 if (CORRECT_FOR_ELOSS){
1279 varE=GetEnergyVariance(step_size,one_over_beta2,
1280 central_traj[m].K_rho_Z_over_A);
1281 Q(state_q_over_pt,state_q_over_pt)
1282 +=varE*Sc(state_q_over_pt)*Sc(state_q_over_pt)*one_over_beta2
1283 *q_over_p_sq;
1284
1285 //if (Q(state_q_over_pt,state_q_over_pt)>1.)
1286 // Q(state_q_over_pt,state_q_over_pt)=1.;
1287 }
1288
1289 // Compute the Jacobian matrix for back-tracking towards target
1290 StepJacobian(pos,central_traj[m].pos-pos,-step_size,Sc,dedx,J);
1291
1292 // Fill the deque with the Jacobian and Process Noise matrices
1293 central_traj[m].J=J;
1294 central_traj[m].Q=Q;
1295 central_traj[m].JT=J.Transpose();
1296 }
1297 }
1298 // If the current length of the trajectory deque is less than the previous
1299 // trajectory deque, remove the extra elements and shrink the deque
1300 if (i<(int)central_traj.size()){
1301 int central_traj_length=central_traj.size();
1302 for (int j=0;j<central_traj_length-i;j++){
1303 central_traj.pop_front();
1304 }
1305 }
1306 else{
1307 // Swim out
1308 r=pos.Perp();
1309 while(r<rmax && pos.z()<endplate_z && pos.z()>Z_MIN0. && len<MAX_PATH_LENGTH500.
1310 && fabs(1./Sc(state_q_over_pt))>PT_MIN0.01){
1311 // Reset D to zero
1312 Sc(state_D)=0.;
1313
1314 // store old position and Z-component of the magnetic field
1315 oldpos=pos;
1316
1317 temp.pos=pos;
1318 temp.s=len;
1319 temp.t=t;
1320 temp.h_id=0;
1321 temp.K_rho_Z_over_A=0.,temp.rho_Z_over_A=0.,temp.Z=0.,temp.LnI=0.; //initialize
1322 temp.chi2c_factor=0.,temp.chi2a_factor=0.,temp.chi2a_corr=0.;
1323
1324 // update path length and flight time
1325 len+=step_size;
1326 q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
1327 q_over_p_sq=q_over_p*q_over_p;
1328 //t+=step_size*sqrt(1.+mass2*q_over_p_sq)/SPEED_OF_LIGHT;
1329
1330 // New state vector
1331 temp.S=Sc;
1332
1333 // Initialize dEdx
1334 dedx=0.;
1335
1336 // Adjust the step size
1337 step_size=mStepSizeS;
1338 if (pos.z()<endplate_z && pos.Perp()<endplate_rmax
1339 && pos.z()>cdc_origin[2]){
1340 // get material properties from the Root Geometry
1341 if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){
1342 DVector3 mom(cos(Sc(state_phi)),sin(Sc(state_phi)),Sc(state_tanl));
1343 if(geom->FindMatKalman(pos,mom,temp.Z,temp.K_rho_Z_over_A,
1344 temp.rho_Z_over_A,temp.LnI,
1345 temp.chi2c_factor,temp.chi2a_factor,
1346 temp.chi2a_corr,
1347 last_material_map,
1348 &s_to_boundary)
1349 !=NOERROR){
1350 return UNRECOVERABLE_ERROR;
1351 }
1352 }
1353 else
1354 {
1355 if(geom->FindMatKalman(pos,temp.Z,temp.K_rho_Z_over_A,
1356 temp.rho_Z_over_A,temp.LnI,
1357 temp.chi2c_factor,temp.chi2a_factor,
1358 temp.chi2a_corr,
1359 last_material_map)!=NOERROR){
1360 return UNRECOVERABLE_ERROR;
1361 }
1362 }
1363 if (CORRECT_FOR_ELOSS){
1364 dedx=GetdEdx(q_over_p,temp.K_rho_Z_over_A,temp.rho_Z_over_A,temp.LnI);
1365 }
1366 if (!stepped_to_boundary){
1367 stepped_to_boundary=false;
1368
1369 if (fabs(dedx)>EPS3.0e-8){
1370 step_size=
1371 (fit_type==kWireBased?DE_PER_STEP_WIRE_BASED0.0005:DE_PER_STEP_TIME_BASED0.0005)
1372 /fabs(dedx);
1373 }
1374 /*
1375 if (fabs(dBzdz)>EPS){
1376 double my_step_size_B=BFIELD_FRAC*fabs(Bz/dBzdz/sin(atan(Sc(state_tanl))));
1377 if (my_step_size_B<step_size){
1378 step_size=my_step_size_B;
1379 }
1380 }
1381 */
1382 if(step_size>mStepSizeS) step_size=mStepSizeS;
1383 double my_r=pos.Perp();
1384
1385 double cdc_step_size_cut
1386 =mCDCInternalStepSize*(1.+0.25*Sc(state_tanl)*Sc(state_tanl));
1387 if (step_size>cdc_step_size_cut && my_r>endplate_rmin
1388 && my_r<endplate_rmax
1389 && pos.Z()>cdc_origin[2])
1390 step_size=cdc_step_size_cut;
1391 if (s_to_boundary<step_size){
1392 step_size=s_to_boundary;
1393 stepped_to_boundary=true;
1394 }
1395 if(step_size<MIN_STEP_SIZE0.1)step_size=MIN_STEP_SIZE0.1;
1396 }
1397 else{
1398 step_size=MIN_STEP_SIZE0.1;
1399 stepped_to_boundary=false;
1400 }
1401 }
1402 if (DEBUG_HISTS && fit_type==kTimeBased){
1403 if (Hstepsize && HstepsizeDenom){
1404 Hstepsize->Fill(pos.z(),pos.Perp(),step_size);
1405 HstepsizeDenom->Fill(pos.z(),pos.Perp());
1406 }
1407 }
1408
1409 // Propagate the state through the field
1410 FixedStep(pos,step_size,Sc,dedx,temp.B);
1411
1412 // Update flight time
1413 q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
1414 q_over_p_sq=q_over_p*q_over_p;
1415 one_over_beta2=1.+mass2*q_over_p_sq;
1416 if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8;
1417 t+=step_size*sqrt(one_over_beta2); // in units of c=1
1418
1419 // Multiple scattering
1420 GetProcessNoiseCentral(step_size,temp.chi2c_factor,temp.chi2a_factor,
1421 temp.chi2a_corr,Sc,Q);
1422
1423 // Energy loss straggling in the approximation of thick absorbers
1424 if (CORRECT_FOR_ELOSS){
1425 varE=GetEnergyVariance(step_size,one_over_beta2,temp.K_rho_Z_over_A);
1426 Q(state_q_over_pt,state_q_over_pt)
1427 +=varE*Sc(state_q_over_pt)*Sc(state_q_over_pt)*one_over_beta2
1428 *q_over_p_sq;
1429
1430 //if (Q(state_q_over_pt,state_q_over_pt)>1.)
1431 // Q(state_q_over_pt,state_q_over_pt)=1.;
1432 }
1433
1434 // Compute the Jacobian matrix and its transpose
1435 StepJacobian(pos,temp.pos-pos,-step_size,Sc,dedx,J);
1436
1437 // update the radius relative to the beam line
1438 r=pos.Perp();
1439 rmax=(origin+((Sc(state_z)-z0)/uz)*dir).Perp()+DELTA_R1.0;
1440
1441 // Update the trajectory info
1442 temp.Q=Q;
1443 temp.J=J;
1444 temp.JT=J.Transpose();
1445 temp.Ckk=Zero5x5;
1446 temp.Skk=Zero5x1;
1447 central_traj.push_front(temp);
1448 }
1449 }
1450
1451 // Only use hits that would fall within the range of the reference trajectory
1452 for (unsigned int j=0;j<my_cdchits.size();j++){
1453 DVector3 origin=my_cdchits[j]->hit->wire->origin;
1454 double z0=origin.z();
1455 DVector3 dir=my_cdchits[j]->hit->wire->udir;
1456 double uz=dir.z();
1457 double my_r=(origin+((Sc(state_z)-z0)/uz)*dir).Perp();
1458 if (my_r>r) my_cdchits[j]->status=bad_hit;
1459 }
1460
1461
1462 // return an error if there are still no entries in the trajectory
1463 if (central_traj.size()==0) return RESOURCE_UNAVAILABLE;
1464
1465 if (DEBUG_LEVEL>10)
1466 {
1467 cout << "---------" << central_traj.size() <<" entries------" <<endl;
1468 for (unsigned int m=0;m<central_traj.size();m++){
1469 DMatrix5x1 S=central_traj[m].S;
1470 double cosphi=cos(S(state_phi));
1471 double sinphi=sin(S(state_phi));
1472 double pt=fabs(1./S(state_q_over_pt));
1473 double tanl=S(state_tanl);
1474
1475 cout
1476 << m << "::"
1477 << setiosflags(ios::fixed)<< "pos: " << setprecision(4)
1478 << central_traj[m].pos.x() << ", "
1479 << central_traj[m].pos.y() << ", "
1480 << central_traj[m].pos.z() << " mom: "
1481 << pt*cosphi << ", " << pt*sinphi << ", "
1482 << pt*tanl << " -> " << pt/cos(atan(tanl))
1483 <<" s: " << setprecision(3)
1484 << central_traj[m].s
1485 <<" t: " << setprecision(3)
1486 << central_traj[m].t
1487 << endl;
1488 }
1489 }
1490
1491 // State at end of swim
1492 Sc=central_traj[0].S;
1493
1494 return NOERROR;
1495}
1496
1497// Routine that extracts the state vector propagation part out of the reference
1498// trajectory loop
1499jerror_t DTrackFitterKalmanSIMD::PropagateForward(int length,int &i,
1500 double &z,double zhit,
1501 DMatrix5x1 &S, bool &done,
1502 bool &stepped_to_boundary){
1503 DMatrix5x5 J,Q,JT;
1504 DKalmanSIMDState_t temp;
1505
1506 // Initialize some variables
1507 temp.h_id=0;
1508 int my_i=0;
1509 double s_to_boundary=1e6;
1510 double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
1511
1512 temp.s=len;
1513 temp.t=ftime;
1514 temp.pos.SetXYZ(S(state_x),S(state_y),z);
1515 temp.K_rho_Z_over_A=temp.rho_Z_over_A=temp.Z=temp.LnI=0.; //initialize
1516 temp.chi2c_factor=temp.chi2a_factor=temp.chi2a_corr=0.;
1517
1518 // get material properties from the Root Geometry
1519 if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){
1520 DVector3 mom(S(state_tx),S(state_ty),1.);
1521 if (geom->FindMatKalman(temp.pos,mom,temp.Z,temp.K_rho_Z_over_A,
1522 temp.rho_Z_over_A,temp.LnI,
1523 temp.chi2c_factor,temp.chi2a_factor,
1524 temp.chi2a_corr,
1525 last_material_map,
1526 &s_to_boundary)
1527 !=NOERROR){
1528 return UNRECOVERABLE_ERROR;
1529 }
1530 }
1531 else
1532 {
1533 if (geom->FindMatKalman(temp.pos,temp.Z,temp.K_rho_Z_over_A,
1534 temp.rho_Z_over_A,temp.LnI,
1535 temp.chi2c_factor,temp.chi2a_factor,
1536 temp.chi2a_corr,
1537 last_material_map)!=NOERROR){
1538 return UNRECOVERABLE_ERROR;
1539 }
1540 }
1541 // Get dEdx for the upcoming step
1542 double dEdx=0.;
1543 if (CORRECT_FOR_ELOSS){
1544 dEdx=GetdEdx(S(state_q_over_p),temp.K_rho_Z_over_A,
1545 temp.rho_Z_over_A,temp.LnI);
1546 }
1547 i++;
1548 my_i=length-i;
1549 if (i<=length){
1550 forward_traj[my_i].s=temp.s;
1551 forward_traj[my_i].t=temp.t;
1552 forward_traj[my_i].h_id=temp.h_id;
1553 forward_traj[my_i].pos=temp.pos;
1554 forward_traj[my_i].Z=temp.Z;
1555 forward_traj[my_i].rho_Z_over_A=temp.rho_Z_over_A;
1556 forward_traj[my_i].K_rho_Z_over_A=temp.K_rho_Z_over_A;
1557 forward_traj[my_i].LnI=temp.LnI;
1558 forward_traj[my_i].S=S;
1559 }
1560 else{
1561 temp.S=S;
1562 }
1563
1564 // Determine the step size based on energy loss
1565 // step=mStepSizeS*dz_ds;
1566 double ds=mStepSizeS;
1567 if (z>cdc_origin[2]){
1568 if (!stepped_to_boundary){
1569 stepped_to_boundary=false;
1570 if (fabs(dEdx)>EPS3.0e-8){
1571 ds=(fit_type==kWireBased?DE_PER_STEP_WIRE_BASED0.0005:DE_PER_STEP_TIME_BASED0.0005)
1572 /fabs(dEdx);
1573 }
1574 /*
1575 if (fabs(dBzdz)>EPS){
1576 double my_step_size_B=BFIELD_FRAC*fabs(Bz/dBzdz)/dz_ds;
1577 if (my_step_size_B<ds){
1578 ds=my_step_size_B;
1579 }
1580 }
1581 */
1582 if(ds>mStepSizeS) ds=mStepSizeS;
1583
1584 //double my_r=sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y));
1585
1586 // Reduce the step size inside the FDC packages
1587 /*
1588 if (fabs(z-zhit)<1.0) ds=FDC_INTERNAL_STEP_SIZE;
1589 else if (ds>mCDCInternalStepSize && my_r>endplate_rmin
1590 && my_r<R_MAX && z<endplate_z)
1591 ds=mCDCInternalStepSize;
1592 */
1593 if (s_to_boundary<ds){
1594 ds=s_to_boundary;
1595 stepped_to_boundary=true;
1596 }
1597 if(ds<MIN_STEP_SIZE0.1)ds=MIN_STEP_SIZE0.1;
1598 }
1599 else{
1600 ds=MIN_STEP_SIZE0.1;
1601 stepped_to_boundary=false;
1602 }
1603 }
1604
1605 if (DEBUG_HISTS && fit_type==kTimeBased){
1606 if (Hstepsize && HstepsizeDenom){
1607 Hstepsize->Fill(z,sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y)),
1608 ds);
1609 HstepsizeDenom->Fill(z,sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y)));
1610 }
1611 }
1612 double newz=z+ds*dz_ds; // new z position
1613
1614 // Check if we are about to step to one of the wire planes
1615 done=false;
1616 if (newz>zhit){
1617 newz=zhit;
1618 done=true;
1619 }
1620
1621 // Step through field
1622 ds=Step(z,newz,dEdx,S);
1623 len+=ds;
1624
1625 double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
1626 double one_over_beta2=1.+mass2*q_over_p_sq;
1627 if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8;
1628 ftime+=ds*sqrt(one_over_beta2); // in units where c=1
1629
1630 // Get the contribution to the covariance matrix due to multiple
1631 // scattering
1632 GetProcessNoise(ds,temp.chi2c_factor,temp.chi2a_factor,temp.chi2a_corr,S,Q);
1633
1634 // Energy loss straggling
1635 if (CORRECT_FOR_ELOSS){
1636 double varE=GetEnergyVariance(ds,one_over_beta2,temp.K_rho_Z_over_A);
1637 Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2;
1638
1639 //if (Q(state_q_over_pt,state_q_over_pt)>1.)
1640 //Q(state_q_over_pt,state_q_over_pt)=1.;
1641 }
1642
1643 // Compute the Jacobian matrix and its transpose
1644 StepJacobian(newz,z,S,dEdx,J);
1645
1646 // update the trajectory data
1647 if (i<=length){
1648 // In the bore of magnet the off-axis components of B are small
1649 forward_traj[my_i].B=sqrt(Bx*Bx+By*By+Bz*Bz);
1650 forward_traj[my_i].Q=Q;
1651 forward_traj[my_i].J=J;
1652 forward_traj[my_i].JT=J.Transpose();
1653 }
1654 else{
1655 temp.B=sqrt(Bx*Bx+By*By+Bz*Bz);
1656 temp.Q=Q;
1657 temp.J=J;
1658 temp.JT=J.Transpose();
1659 temp.Ckk=Zero5x5;
1660 temp.Skk=Zero5x1;
1661 forward_traj.push_front(temp);
1662 }
1663
1664 // update z
1665 z=newz;
1666
1667 return NOERROR;
1668}
1669
1670// Reference trajectory for trajectories with hits in the forward direction
1671// At each point we store the state vector and the Jacobian needed to get to this state
1672// along z from the previous state.
1673jerror_t DTrackFitterKalmanSIMD::SetReferenceTrajectory(DMatrix5x1 &S){
1674
1675 // Magnetic field at beginning of trajectory
1676 bfield->GetField(x_,y_,z_,Bx,By,Bz);
1677
1678 // progress in z from hit to hit
1679 double z=z_;
1680 int i=0;
1681 int forward_traj_length=forward_traj.size();
1682 // loop over the fdc hits
1683 double zhit=0.,old_zhit=0.;
1684 bool stepped_to_boundary=false;
1685 unsigned int m=0;
1686 for (m=0;m<my_fdchits.size();m++){
1687 if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX100.){
1688 break;
1689 }
1690
1691 zhit=my_fdchits[m]->z;
1692 if (fabs(old_zhit-zhit)>EPS3.0e-8){
1693 bool done=false;
1694 while (!done){
1695 if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX100.){
1696 break;
1697 }
1698
1699 if (PropagateForward(forward_traj_length,i,z,zhit,S,done,
1700 stepped_to_boundary)
1701 !=NOERROR)
1702 return UNRECOVERABLE_ERROR;
1703 }
1704 }
1705 old_zhit=zhit;
1706 }
1707
1708 // If m<2 then no useable FDC hits survived the check on the magnitude on the
1709 // momentum
1710 if (m<2) return UNRECOVERABLE_ERROR;
1711
1712 // Make sure the reference trajectory goes one step beyond the most
1713 // downstream hit plane
1714 if (m==my_fdchits.size()){
1715 bool done=false;
1716 if (PropagateForward(forward_traj_length,i,z,400.,S,done,
1717 stepped_to_boundary)
1718 !=NOERROR)
1719 return UNRECOVERABLE_ERROR;
1720 if (PropagateForward(forward_traj_length,i,z,400.,S,done,
1721 stepped_to_boundary)
1722 !=NOERROR)
1723 return UNRECOVERABLE_ERROR;
1724 }
1725
1726 // Shrink the deque if the new trajectory has less points in it than the
1727 // old trajectory
1728 if (i<(int)forward_traj.size()){
1729 int mylen=forward_traj.size();
1730 for (int j=0;j<mylen-i;j++){
1731 forward_traj.pop_front();
1732 }
1733 }
1734
1735 // If we lopped off some hits on the downstream end, truncate the trajectory to
1736 // the point in z just beyond the last valid hit
1737 unsigned int my_id=my_fdchits.size();
1738 if (m<my_id){
1739 if (zhit<z) my_id=m;
1740 else my_id=m-1;
1741 zhit=my_fdchits[my_id-1]->z;
1742 for (;;){
1743 z=forward_traj[0].pos.z();
1744 if (z<zhit+EPS21.e-4) break;
1745 forward_traj.pop_front();
1746 }
1747
1748 // Temporory structure keeping state and trajectory information
1749 DKalmanSIMDState_t temp;
1750 temp.h_id=0;
1751 temp.B=0.;
1752 temp.Z=temp.K_rho_Z_over_A=temp.rho_Z_over_A=temp.LnI=0.;
1753 temp.Q=Zero5x5;
1754
1755 // last S vector
1756 S=forward_traj[0].S;
1757
1758 // Step just beyond the last hit
1759 double newz=z+0.01;
1760 double ds=Step(z,newz,0.,S); // ignore energy loss for this small step
1761 temp.s=forward_traj[0].s+ds;
1762 temp.pos.SetXYZ(S(state_x),S(state_y),newz);
1763 temp.S=S;
1764
1765 // Flight time
1766 double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
1767 double one_over_beta2=1.+mass2*q_over_p_sq;
1768 if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8;
1769 temp.t=forward_traj[0].t+ds*sqrt(one_over_beta2); // in units where c=1
1770
1771 // Covariance and state vector needed for smoothing code
1772 temp.Ckk=Zero5x5;
1773 temp.Skk=Zero5x1;
1774
1775 // Jacobian matrices
1776 temp.JT=temp.J=I5x5;
1777
1778 forward_traj.push_front(temp);
1779 }
1780
1781 // Fill in Lorentz deflection parameters
1782 for (unsigned int m=0;m<forward_traj.size();m++){
1783 if (my_id>0){
1784 unsigned int hit_id=my_id-1;
1785 double z=forward_traj[m].pos.z();
1786 if (fabs(z-my_fdchits[hit_id]->z)<EPS21.e-4){
1787 forward_traj[m].h_id=my_id;
1788
1789 // Get the magnetic field at this position along the trajectory
1790 bfield->GetField(forward_traj[m].pos.x(),forward_traj[m].pos.y(),z,
1791 Bx,By,Bz);
1792 double Br=sqrt(Bx*Bx+By*By);
1793
1794 // Angle between B and wire
1795 double my_phi=0.;
1796 if (Br>0.) my_phi=acos((Bx*my_fdchits[hit_id]->sina
1797 +By*my_fdchits[hit_id]->cosa)/Br);
1798 /*
1799 lorentz_def->GetLorentzCorrectionParameters(forward_traj[m].pos.x(),
1800 forward_traj[m].pos.y(),
1801 forward_traj[m].pos.z(),
1802 tanz,tanr);
1803 my_fdchits[hit_id]->nr=tanr;
1804 my_fdchits[hit_id]->nz=tanz;
1805 */
1806
1807 my_fdchits[hit_id]->nr=1.05*0.1458*Bz*(1.-0.048*Br);
1808 my_fdchits[hit_id]->nz=1.05*(0.1717+0.01227*Bz)*(Br*cos(my_phi));
1809
1810 my_id--;
1811
1812 unsigned int num=1;
1813 while (hit_id>0
1814 && fabs(my_fdchits[hit_id]->z-my_fdchits[hit_id-1]->z)<EPS3.0e-8){
1815 hit_id=my_id-1;
1816 num++;
1817 my_id--;
1818 }
1819 forward_traj[m].num_hits=num;
1820 }
1821
1822 }
1823 }
1824
1825
1826 if (DEBUG_LEVEL>10)
1827 {
1828 cout << "--- Forward fdc trajectory ---" <<endl;
1829 for (unsigned int m=0;m<forward_traj.size();m++){
1830 DMatrix5x1 S=(forward_traj[m].S);
1831 double tx=S(state_tx),ty=S(state_ty);
1832 double phi=atan2(ty,tx);
1833 double cosphi=cos(phi);
1834 double sinphi=sin(phi);
1835 double p=fabs(1./S(state_q_over_p));
1836 double tanl=1./sqrt(tx*tx+ty*ty);
1837 double sinl=sin(atan(tanl));
1838 double cosl=cos(atan(tanl));
1839 cout
1840 << setiosflags(ios::fixed)<< "pos: " << setprecision(4)
1841 << forward_traj[m].pos.x() << ", "
1842 << forward_traj[m].pos.y() << ", "
1843 << forward_traj[m].pos.z() << " mom: "
1844 << p*cosphi*cosl<< ", " << p*sinphi*cosl << ", "
1845 << p*sinl << " -> " << p
1846 <<" s: " << setprecision(3)
1847 << forward_traj[m].s
1848 <<" t: " << setprecision(3)
1849 << forward_traj[m].t
1850 <<" id: " << forward_traj[m].h_id
1851 << endl;
1852 }
1853 }
1854
1855
1856 // position at the end of the swim
1857 z_=z;
1858 x_=S(state_x);
1859 y_=S(state_y);
1860
1861 return NOERROR;
1862}
1863
1864// Step the state vector through the field from oldz to newz.
1865// Uses the 4th-order Runga-Kutte algorithm.
1866double DTrackFitterKalmanSIMD::Step(double oldz,double newz, double dEdx,
1867 DMatrix5x1 &S){
1868 double delta_z=newz-oldz;
1869 if (fabs(delta_z)<EPS3.0e-8) return 0.; // skip if the step is too small
1870
1871 // Direction tangents
1872 double tx=S(state_tx);
1873 double ty=S(state_ty);
1874 double ds=sqrt(1.+tx*tx+ty*ty)*delta_z;
1875
1876 double delta_z_over_2=0.5*delta_z;
1877 double midz=oldz+delta_z_over_2;
1878 DMatrix5x1 D1,D2,D3,D4;
1879
1880 CalcDeriv(oldz,delta_z,S,dEdx,D1);
1881 CalcDeriv(midz,delta_z_over_2,S+delta_z_over_2*D1,dEdx,D2);
1882 CalcDeriv(midz,delta_z_over_2,S+delta_z_over_2*D2,dEdx,D3);
1883 CalcDeriv(newz,delta_z,S+delta_z*D3,dEdx,D4);
1884
1885 S+=delta_z*(ONE_SIXTH0.16666666666666667*D1+ONE_THIRD0.33333333333333333*D2+ONE_THIRD0.33333333333333333*D3+ONE_SIXTH0.16666666666666667*D4);
1886
1887 // Don't let the magnitude of the momentum drop below some cutoff
1888 if (fabs(S(state_q_over_p))>Q_OVER_P_MAX100.)
1889 S(state_q_over_p)=Q_OVER_P_MAX100.*(S(state_q_over_p)>0?1.:-1.);
1890 // Try to keep the direction tangents from heading towards 90 degrees
1891 if (fabs(S(state_tx))>TAN_MAX10.)
1892 S(state_tx)=TAN_MAX10.*(S(state_tx)>0?1.:-1.);
1893 if (fabs(S(state_ty))>TAN_MAX10.)
1894 S(state_ty)=TAN_MAX10.*(S(state_ty)>0?1.:-1.);
1895
1896 return ds;
1897}
1898
1899// Compute the Jacobian matrix for the forward parametrization.
1900jerror_t DTrackFitterKalmanSIMD::StepJacobian(double oldz,double newz,
1901 const DMatrix5x1 &S,
1902 double dEdx,DMatrix5x5 &J){
1903 // Initialize the Jacobian matrix
1904 //J.Zero();
1905 //for (int i=0;i<5;i++) J(i,i)=1.;
1906 J=I5x5;
1907
1908 // Step in z
1909 double delta_z=newz-oldz;
1910 if (fabs(delta_z)<EPS3.0e-8) return NOERROR; //skip if the step is too small
1911
1912 // Current values of state vector variables
1913 double x=S(state_x), y=S(state_y),tx=S(state_tx),ty=S(state_ty);
1914 double q_over_p=S(state_q_over_p);
1915
1916 //B-field and field gradient at (x,y,z)
1917 //if (get_field)
1918 bfield->GetFieldAndGradient(x,y,oldz,Bx,By,Bz,dBxdx,dBxdy,
1919 dBxdz,dBydx,dBydy,
1920 dBydz,dBzdx,dBzdy,dBzdz);
1921
1922 // Don't let the magnitude of the momentum drop below some cutoff
1923 if (fabs(q_over_p)>Q_OVER_P_MAX100.){
1924 q_over_p=Q_OVER_P_MAX100.*(q_over_p>0?1.:-1.);
1925 dEdx=0.;
1926 }
1927 // Try to keep the direction tangents from heading towards 90 degrees
1928 if (fabs(tx)>TAN_MAX10.) tx=TAN_MAX10.*(tx>0?1.:-1.);
1929 if (fabs(ty)>TAN_MAX10.) ty=TAN_MAX10.*(ty>0?1.:-1.);
1930 // useful combinations of terms
1931 double kq_over_p=qBr2p0.003*q_over_p;
1932 double tx2=tx*tx;
1933 double twotx2=2.*tx2;
1934 double ty2=ty*ty;
1935 double twoty2=2.*ty2;
1936 double txty=tx*ty;
1937 double one_plus_tx2=1.+tx2;
1938 double one_plus_ty2=1.+ty2;
1939 double one_plus_twotx2_plus_ty2=one_plus_ty2+twotx2;
1940 double one_plus_twoty2_plus_tx2=one_plus_tx2+twoty2;
1941 double dsdz=sqrt(1.+tx2+ty2);
1942 double ds=dsdz*delta_z;
1943 double kds=qBr2p0.003*ds;
1944 double kqdz_over_p_over_dsdz=kq_over_p*delta_z/dsdz;
1945 double kq_over_p_ds=kq_over_p*ds;
1946 double dtx_Bdep=ty*Bz+txty*Bx-one_plus_tx2*By;
1947 double dty_Bdep=Bx*one_plus_ty2-txty*By-tx*Bz;
1948 double Bxty=Bx*ty;
1949 double Bytx=By*tx;
1950 double Bztxty=Bz*txty;
1951 double Byty=By*ty;
1952 double Bxtx=Bx*tx;
1953
1954 // Jacobian
1955 J(state_x,state_tx)=J(state_y,state_ty)=delta_z;
1956 J(state_tx,state_q_over_p)=kds*dtx_Bdep;
1957 J(state_ty,state_q_over_p)=kds*dty_Bdep;
1958 J(state_tx,state_tx)+=kqdz_over_p_over_dsdz*(Bxty*(one_plus_twotx2_plus_ty2)
1959 -Bytx*(3.*one_plus_tx2+twoty2)
1960 +Bztxty);
1961 J(state_tx,state_x)=kq_over_p_ds*(ty*dBzdx+txty*dBxdx-one_plus_tx2*dBydx);
1962 J(state_ty,state_ty)+=kqdz_over_p_over_dsdz*(Bxty*(3.*one_plus_ty2+twotx2)
1963 -Bytx*(one_plus_twoty2_plus_tx2)
1964 -Bztxty);
1965 J(state_ty,state_y)= kq_over_p_ds*(one_plus_ty2*dBxdy-txty*dBydy-tx*dBzdy);
1966 J(state_tx,state_ty)=kqdz_over_p_over_dsdz
1967 *((Bxtx+Bz)*(one_plus_twoty2_plus_tx2)-Byty*one_plus_tx2);
1968 J(state_tx,state_y)= kq_over_p_ds*(tx*dBzdy+txty*dBxdy-one_plus_tx2*dBydy);
1969 J(state_ty,state_tx)=-kqdz_over_p_over_dsdz*((Byty+Bz)*(one_plus_twotx2_plus_ty2)
1970 -Bxtx*one_plus_ty2);
1971 J(state_ty,state_x)=kq_over_p_ds*(one_plus_ty2*dBxdx-txty*dBydx-tx*dBzdx);
1972 if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){
1973 double one_over_p_sq=q_over_p*q_over_p;
1974 double E=sqrt(1./one_over_p_sq+mass2);
1975 J(state_q_over_p,state_q_over_p)-=dEdx*ds/E*(2.+3.*mass2*one_over_p_sq);
1976 double temp=-(q_over_p*one_over_p_sq/dsdz)*E*dEdx*delta_z;
1977 J(state_q_over_p,state_tx)=tx*temp;
1978 J(state_q_over_p,state_ty)=ty*temp;
1979 }
1980
1981 return NOERROR;
1982}
1983
1984// Calculate the derivative for the alternate set of parameters {q/pT, phi,
1985// tan(lambda),D,z}
1986jerror_t DTrackFitterKalmanSIMD::CalcDeriv(double ds,const DVector3 &pos,
1987 DVector3 &dpos,const DMatrix5x1 &S,
1988 double dEdx,DMatrix5x1 &D1){
1989 //Direction at current point
1990 double tanl=S(state_tanl);
1991 // Don't let tanl exceed some maximum
1992 if (fabs(tanl)>TAN_MAX10.) tanl=TAN_MAX10.*(tanl>0?1.:-1.);
1993
1994 double phi=S(state_phi);
1995 double cosphi=cos(phi);
1996 double sinphi=sin(phi);
1997 double lambda=atan(tanl);
1998 double sinl=sin(lambda);
1999 double cosl=cos(lambda);
2000 // Other parameters
2001 double q_over_pt=S(state_q_over_pt);
2002 double pt=fabs(1./q_over_pt);
2003
2004 // Don't let the pt drop below some minimum
2005 if (pt<PT_MIN0.01) {
2006 pt=PT_MIN0.01;
2007 q_over_pt=(1./PT_MIN0.01)*(q_over_pt>0?1.:-1.);
2008 dEdx=0.;
2009 }
2010 double kq_over_pt=qBr2p0.003*q_over_pt;
2011
2012 // Derivative of S with respect to s
2013 double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi;
2014 D1(state_q_over_pt)
2015 =kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2016 double one_over_cosl=1./cosl;
2017 if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){
2018 double p=pt*one_over_cosl;
2019 double p_sq=p*p;
2020 double E=sqrt(p_sq+mass2);
2021 D1(state_q_over_pt)+=-q_over_pt*E/p_sq*dEdx;
2022 }
2023 D1(state_phi)
2024 =kq_over_pt*(Bx*cosphi*sinl+By*sinphi*sinl-Bz*cosl);
2025 D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2026 D1(state_z)=sinl;
2027
2028 // New direction
2029 dpos.SetXYZ(cosl*cosphi,cosl*sinphi,sinl);
2030
2031 return NOERROR;
2032}
2033
2034// Calculate the derivative and Jacobian matrices for the alternate set of
2035// parameters {q/pT, phi, tan(lambda),D,z}
2036jerror_t DTrackFitterKalmanSIMD::CalcDerivAndJacobian(double ds,
2037 const DVector3 &pos,
2038 DVector3 &dpos,
2039 const DMatrix5x1 &S,
2040 double dEdx,
2041 DMatrix5x5 &J1,
2042 DMatrix5x1 &D1){
2043 //Direction at current point
2044 double tanl=S(state_tanl);
2045 // Don't let tanl exceed some maximum
2046 if (fabs(tanl)>TAN_MAX10.) tanl=TAN_MAX10.*(tanl>0?1.:-1.);
2047
2048 double phi=S(state_phi);
2049 double cosphi=cos(phi);
2050 double sinphi=sin(phi);
2051 double lambda=atan(tanl);
2052 double sinl=sin(lambda);
2053 double cosl=cos(lambda);
2054 double cosl2=cosl*cosl;
2055 double cosl3=cosl*cosl2;
2056 double one_over_cosl=1./cosl;
2057 // Other parameters
2058 double q_over_pt=S(state_q_over_pt);
2059 double pt=fabs(1./q_over_pt);
2060 double q=pt*q_over_pt;
2061
2062 // Don't let the pt drop below some minimum
2063 if (pt<PT_MIN0.01) {
2064 pt=PT_MIN0.01;
2065 q_over_pt=q/PT_MIN0.01;
2066 dEdx=0.;
2067 }
2068 double kq_over_pt=qBr2p0.003*q_over_pt;
2069
2070 // B-field and gradient at (x,y,z)
2071 bfield->GetFieldAndGradient(pos.x(),pos.y(),pos.z(),Bx,By,Bz,
2072 dBxdx,dBxdy,dBxdz,dBydx,
2073 dBydy,dBydz,dBzdx,dBzdy,dBzdz);
2074
2075 // Derivative of S with respect to s
2076 double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi;
2077 double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi;
2078 D1(state_q_over_pt)=kq_over_pt*q_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2079 D1(state_phi)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl);
2080 D1(state_tanl)=kq_over_pt*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2081 D1(state_z)=sinl;
2082
2083 // New direction
2084 dpos.SetXYZ(cosl*cosphi,cosl*sinphi,sinl);
2085
2086 // Jacobian matrix elements
2087 J1(state_phi,state_phi)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2088 J1(state_phi,state_q_over_pt)
2089 =qBr2p0.003*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl);
2090 J1(state_phi,state_tanl)=kq_over_pt*(By_sinphi_plus_Bx_cosphi*cosl
2091 +Bz*sinl)*cosl2;
2092 J1(state_phi,state_z)
2093 =kq_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl);
2094
2095 J1(state_tanl,state_phi)=-kq_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl;
2096 J1(state_tanl,state_q_over_pt)=qBr2p0.003*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2097 J1(state_tanl,state_tanl)=kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2098 J1(state_tanl,state_z)=kq_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl;
2099 J1(state_q_over_pt,state_phi)
2100 =-kq_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi;
2101 J1(state_q_over_pt,state_q_over_pt)
2102 =2.*kq_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2103 J1(state_q_over_pt,state_tanl)
2104 =kq_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi;
2105 if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){
2106 double p=pt*one_over_cosl;
2107 double p_sq=p*p;
2108 double m2_over_p2=mass2/p_sq;
2109 double E=sqrt(p_sq+mass2);
2110
2111 D1(state_q_over_pt)+=-q_over_pt*E/p_sq*dEdx;
2112 J1(state_q_over_pt,state_q_over_pt)+=-dEdx*(2.+3.*m2_over_p2)/E;
2113 J1(state_q_over_pt,state_tanl)+=q*dEdx*sinl*(1.+2.*m2_over_p2)/(p*E);
2114 }
2115 J1(state_q_over_pt,state_z)
2116 =kq_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi);
2117 J1(state_z,state_tanl)=cosl3;
2118
2119 return NOERROR;
2120}
2121
2122// Convert between the forward parameter set {x,y,tx,ty,q/p} and the central
2123// parameter set {q/pT,phi,tan(lambda),D,z}
2124jerror_t DTrackFitterKalmanSIMD::ConvertStateVectorAndCovariance(double z,
2125 const DMatrix5x1 &S,
2126 DMatrix5x1 &Sc,
2127 const DMatrix5x5 &C,
2128 DMatrix5x5 &Cc){
2129 //double x=S(state_x),y=S(state_y);
2130 //double tx=S(state_tx),ty=S(state_ty),q_over_p=S(state_q_over_p);
2131 // Copy over to the class variables
2132 x_=S(state_x), y_=S(state_y);
2133 tx_=S(state_tx),ty_=S(state_ty);
2134 q_over_p_=S(state_q_over_p);
2135 double tsquare=tx_*tx_+ty_*ty_;
2136 double tanl=1./sqrt(tsquare);
2137 double cosl=cos(atan(tanl));
2138 Sc(state_q_over_pt)=q_over_p_/cosl;
2139 Sc(state_phi)=atan2(ty_,tx_);
2140 Sc(state_tanl)=tanl;
2141 Sc(state_D)=sqrt(x_*x_+y_*y_);
2142 Sc(state_z)=z;
2143
2144 // D is a signed quantity
2145 double cosphi=cos(Sc(state_phi));
2146 double sinphi=sin(Sc(state_phi));
2147 if ((x_>0 && sinphi>0) || (y_ <0 && cosphi>0) || (y_>0 && cosphi<0)
2148 || (x_<0 && sinphi<0)) Sc(state_D)*=-1.;
2149
2150 // Rotate the covariance matrix from forward parameter space to central
2151 // parameter space
2152 DMatrix5x5 J;
2153
2154 double tanl2=tanl*tanl;
2155 double tanl3=tanl2*tanl;
2156 double factor=1./sqrt(1.+tsquare);
2157 J(state_z,state_x)=-tx_/tsquare;
2158 J(state_z,state_y)=-ty_/tsquare;
2159 double diff=tx_*tx_-ty_*ty_;
2160 double frac=1./(tsquare*tsquare);
2161 J(state_z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac;
2162 J(state_z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac;
2163 J(state_tanl,state_tx)=-tx_*tanl3;
2164 J(state_tanl,state_ty)=-ty_*tanl3;
2165 J(state_q_over_pt,state_q_over_p)=1./cosl;
2166 J(state_q_over_pt,state_tx)=-q_over_p_*tx_*tanl3*factor;
2167 J(state_q_over_pt,state_ty)=-q_over_p_*ty_*tanl3*factor;
2168 J(state_phi,state_tx)=-ty_*tanl2;
2169 J(state_phi,state_ty)=tx_*tanl2;
2170 J(state_D,state_x)=x_/Sc(state_D);
2171 J(state_D,state_y)=y_/Sc(state_D);
2172
2173 Cc=J*C*J.Transpose();
2174
2175 return NOERROR;
2176}
2177
2178
2179jerror_t DTrackFitterKalmanSIMD::StepStateAndCovariance(DVector3 &pos,double ds,
2180 double dEdx,DMatrix5x1 &S,
2181 DMatrix5x5 &J,
2182 DMatrix5x5 &C){
2183 if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small
2184 // Magnetic field
2185 bfield->GetField(pos.x(),pos.y(),pos.z(),Bx,By,Bz);
2186
2187 // Matrices for intermediate steps
2188 DMatrix5x1 D1,D2,D3,D4;
2189 DMatrix5x1 S1;
2190 DVector3 dpos1,dpos2,dpos3,dpos4;
2191 double ds_2=0.5*ds;
2192
2193 CalcDeriv(0.,pos,dpos1,S,dEdx,D1);
2194
2195 DVector3 mypos=pos+ds_2*dpos1;
2196 bfield->GetField(mypos.x(),mypos.y(),mypos.z(),Bx,By,Bz);
2197 S1=S+ds_2*D1;
2198
2199 CalcDeriv(ds_2,mypos,dpos2,S1,dEdx,D2);
2200
2201 mypos=pos+ds_2*dpos2;
2202 bfield->GetField(mypos.x(),mypos.y(),mypos.z(),Bx,By,Bz);
2203 S1=S+ds_2*D2;
2204
2205 CalcDeriv(ds_2,mypos,dpos3,S1,dEdx,D3);
2206
2207 mypos=pos+ds*dpos3;
2208 bfield->GetField(mypos.x(),mypos.y(),mypos.z(),Bx,By,Bz);
2209 S1=S+ds*D3;
2210
2211 CalcDeriv(ds,mypos,dpos4,S1,dEdx,D4);
2212
2213 // Position vector increment
2214 DVector3 dpos
2215 =ds*(ONE_SIXTH0.16666666666666667*dpos1+ONE_THIRD0.33333333333333333*dpos2+ONE_THIRD0.33333333333333333*dpos3+ONE_SIXTH0.16666666666666667*dpos4);
2216
2217 // Compute the Jacobian matrix
2218 StepJacobian(pos,dpos,ds,S,dEdx,J);
2219
2220 // New position vector
2221 pos+=dpos;
2222
2223 // New state vector
2224 S+=ds*(ONE_SIXTH0.16666666666666667*D1+ONE_THIRD0.33333333333333333*D2+ONE_THIRD0.33333333333333333*D3+ONE_SIXTH0.16666666666666667*D4);
2225
2226 // Don't let the pt drop below some minimum
2227 if (fabs(1./S(state_q_over_pt))<PT_MIN0.01) {
2228 S(state_q_over_pt)=(1./PT_MIN0.01)*(S(state_q_over_pt)>0?1.:-1.);
2229 }
2230 // Don't let tanl exceed some maximum
2231 if (fabs(S(state_tanl))>TAN_MAX10.){
2232 S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0?1.:-1.);
2233 }
2234
2235 // New covariance matrix
2236 // C=J C J^T
2237 C=C.SandwichMultiply(J);
2238
2239 return NOERROR;
2240}
2241
2242
2243
2244// Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z}
2245jerror_t DTrackFitterKalmanSIMD::FixedStep(DVector3 &pos,double ds,
2246 DMatrix5x1 &S,
2247 double dEdx){
2248 double B=0.;
2249 FixedStep(pos,ds,S,dEdx,B);
2250 return NOERROR;
2251}
2252
2253// Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z}
2254jerror_t DTrackFitterKalmanSIMD::FixedStep(DVector3 &pos,double ds,
2255 DMatrix5x1 &S,
2256 double dEdx,double &B){
2257 // Magnetic field
2258 bfield->GetField(pos.x(),pos.y(),pos.z(),Bx,By,Bz);
2259 B=sqrt(Bx*Bx+By*By+Bz*Bz);
2260
2261 if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small
2262
2263 // Matrices for intermediate steps
2264 DMatrix5x1 D1,D2,D3,D4;
2265 DMatrix5x1 S1;
2266 DVector3 dpos1,dpos2,dpos3,dpos4;
2267 double ds_2=0.5*ds;
2268
2269 CalcDeriv(0.,pos,dpos1,S,dEdx,D1);
2270
2271 DVector3 mypos=pos+ds_2*dpos1;
2272 bfield->GetField(mypos.x(),mypos.y(),mypos.z(),Bx,By,Bz);
2273 S1=S+ds_2*D1;
2274
2275 CalcDeriv(ds_2,mypos,dpos2,S1,dEdx,D2);
2276
2277 mypos=pos+ds_2*dpos2;
2278 bfield->GetField(mypos.x(),mypos.y(),mypos.z(),Bx,By,Bz);
2279 S1=S+ds_2*D2;
2280
2281 CalcDeriv(ds_2,mypos,dpos3,S1,dEdx,D3);
2282
2283 mypos=pos+ds*dpos3;
2284 bfield->GetField(mypos.x(),mypos.y(),mypos.z(),Bx,By,Bz);
2285 S1=S+ds*D3;
2286
2287 CalcDeriv(ds,mypos,dpos4,S1,dEdx,D4);
2288
2289 // New state vector
2290 S+=ds*(ONE_SIXTH0.16666666666666667*D1+ONE_THIRD0.33333333333333333*D2+ONE_THIRD0.33333333333333333*D3+ONE_SIXTH0.16666666666666667*D4);
2291
2292 // New position
2293 pos+=ds*(ONE_SIXTH0.16666666666666667*dpos1+ONE_THIRD0.33333333333333333*dpos2+ONE_THIRD0.33333333333333333*dpos3+ONE_SIXTH0.16666666666666667*dpos4);
2294
2295 // Don't let the pt drop below some minimum
2296 if (fabs(1./S(state_q_over_pt))<PT_MIN0.01) {
2297 S(state_q_over_pt)=(1./PT_MIN0.01)*(S(state_q_over_pt)>0?1.:-1.);
2298 }
2299 // Don't let tanl exceed some maximum
2300 if (fabs(S(state_tanl))>TAN_MAX10.){
2301 S(state_tanl)=TAN_MAX10.*(S(state_tanl)>0?1.:-1.);
2302 }
2303
2304 return NOERROR;
2305}
2306
2307
2308// Calculate the jacobian matrix for the alternate parameter set
2309// {q/pT,phi,tanl(lambda),D,z}
2310jerror_t DTrackFitterKalmanSIMD::StepJacobian(const DVector3 &pos,
2311 const DVector3 &dpos,
2312 double ds,const DMatrix5x1 &S,
2313 double dEdx,DMatrix5x5 &J){
2314 // Initialize the Jacobian matrix
2315 //J.Zero();
2316 //for (int i=0;i<5;i++) J(i,i)=1.;
2317 J=I5x5;
2318
2319 if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small
2320
2321 // charge
2322 double q=(S(state_q_over_pt)>0)?1.:-1.;
2323
2324 //kinematic quantities
2325 double qpt=1./S(state_q_over_pt);
2326 double sinphi=sin(S(state_phi));
2327 double cosphi=cos(S(state_phi));
2328 double D=S(state_D);
2329 double lambda=atan(S(state_tanl));
2330 double sinl=sin(lambda);
2331 double cosl=cos(lambda);
2332 double cosl2=cosl*cosl;
2333 double cosl3=cosl*cosl2;
2334 double one_over_cosl=1./cosl;
2335 // Other parameters
2336 double q_over_pt=S(state_q_over_pt);
2337 double pt=fabs(1./q_over_pt);
2338
2339 // Don't let the pt drop below some minimum
2340 if (pt<PT_MIN0.01) {
2341 pt=PT_MIN0.01;
2342 q_over_pt=q/PT_MIN0.01;
2343 dEdx=0.;
2344 }
2345 double kds=qBr2p0.003*ds;
2346 double kq_ds_over_pt=kds*q_over_pt;
2347
2348 // B-field and gradient at (x,y,z)
2349 bfield->GetFieldAndGradient(pos.x(),pos.y(),pos.z(),Bx,By,Bz,
2350 dBxdx,dBxdy,dBxdz,dBydx,
2351 dBydy,dBydz,dBzdx,dBzdy,dBzdz);
2352
2353 double By_cosphi_minus_Bx_sinphi=By*cosphi-Bx*sinphi;
2354 double By_sinphi_plus_Bx_cosphi=By*sinphi+Bx*cosphi;
2355
2356 // Jacobian matrix elements
2357 J(state_phi,state_phi)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2358 J(state_phi,state_q_over_pt)=kds*(By_sinphi_plus_Bx_cosphi*sinl-Bz*cosl);
2359 J(state_phi,state_tanl)=kq_ds_over_pt*(By_sinphi_plus_Bx_cosphi*cosl
2360 +Bz*sinl)*cosl2;
2361 J(state_phi,state_z)
2362 =kq_ds_over_pt*(dBxdz*cosphi*sinl+dBydz*sinphi*sinl-dBzdz*cosl);
2363
2364 J(state_tanl,state_phi)=-kq_ds_over_pt*By_sinphi_plus_Bx_cosphi*one_over_cosl;
2365 J(state_tanl,state_q_over_pt)=kds*By_cosphi_minus_Bx_sinphi*one_over_cosl;
2366 J(state_tanl,state_tanl)+=kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2367 J(state_tanl,state_z)=kq_ds_over_pt*(dBydz*cosphi-dBxdz*sinphi)*one_over_cosl;
2368 J(state_q_over_pt,state_phi)
2369 =-kq_ds_over_pt*q_over_pt*sinl*By_sinphi_plus_Bx_cosphi;
2370 J(state_q_over_pt,state_q_over_pt)
2371 +=2.*kq_ds_over_pt*sinl*By_cosphi_minus_Bx_sinphi;
2372 J(state_q_over_pt,state_tanl)
2373 =kq_ds_over_pt*q_over_pt*cosl3*By_cosphi_minus_Bx_sinphi;
2374 if (CORRECT_FOR_ELOSS && fabs(dEdx)>EPS3.0e-8){
2375 double p=pt*one_over_cosl;
2376 double p_sq=p*p;
2377 double m2_over_p2=mass2/p_sq;
2378 double E=sqrt(p_sq+mass2);
2379 double dE_over_E=dEdx*ds/E;
2380
2381 J(state_q_over_pt,state_q_over_pt)+=-dE_over_E*(2.+3.*m2_over_p2);
2382 J(state_q_over_pt,state_tanl)+=q*dE_over_E*sinl*(1.+2.*m2_over_p2)/p;
2383 }
2384 J(state_q_over_pt,state_z)
2385 =kq_ds_over_pt*q_over_pt*sinl*(dBydz*cosphi-dBxdz*sinphi);
2386 J(state_z,state_tanl)=cosl3*ds;
2387
2388 // Deal with changes in D
2389 double B=sqrt(Bx*Bx+By*By+Bz*Bz);
2390 //double qrc_old=qpt/(qBr2p*fabs(Bz));
2391 double qrc_old=qpt/(qBr2p0.003*B);
2392 double qrc_plus_D=D+qrc_old;
2393 double dx=dpos.x();
2394 double dy=dpos.y();
2395 double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi;
2396 double rc=sqrt(dpos.Perp2()
2397 +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi)
2398 +qrc_plus_D*qrc_plus_D);
2399
2400 J(state_D,state_D)=q*(dx_sinphi_minus_dy_cosphi+qrc_plus_D)/rc;
2401 J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.);
2402 J(state_D,state_phi)=q*qrc_plus_D*(dx*cosphi+dy*sinphi)/rc;
2403
2404 return NOERROR;
2405}
2406
2407
2408
2409
2410// Runga-Kutte for alternate parameter set {q/pT,phi,tanl(lambda),D,z}
2411jerror_t DTrackFitterKalmanSIMD::StepJacobian(const DVector3 &pos,
2412 double ds,const DMatrix5x1 &S,
2413 double dEdx,DMatrix5x5 &J){
2414 // Initialize the Jacobian matrix
2415 //J.Zero();
2416 //for (int i=0;i<5;i++) J(i,i)=1.;
2417 J=I5x5;
2418
2419 if (fabs(ds)<EPS3.0e-8) return NOERROR; // break out if ds is too small
2420
2421 // Matrices for intermediate steps
2422 DMatrix5x5 J1;
2423 DMatrix5x1 D1;
2424 DVector3 dpos1;
2425
2426 // charge
2427 double q=(S(state_q_over_pt)>0)?1.:-1.;
2428
2429 //kinematic quantities
2430 double qpt=1./S(state_q_over_pt);
2431 double sinphi=sin(S(state_phi));
2432 double cosphi=cos(S(state_phi));
2433 double D=S(state_D);
2434
2435 CalcDerivAndJacobian(0.,pos,dpos1,S,dEdx,J1,D1);
2436 // double Bz_=fabs(Bz); // needed for computing D
2437
2438 // New Jacobian matrix
2439 J+=ds*J1;
2440
2441 // change in position
2442 DVector3 dpos =ds*dpos1;
2443
2444 // Deal with changes in D
2445 double B=sqrt(Bx*Bx+By*By+Bz*Bz);
2446 //double qrc_old=qpt/(qBr2p*Bz_);
2447 double qrc_old=qpt/(qBr2p0.003*B);
2448 double qrc_plus_D=D+qrc_old;
2449 double dx=dpos.x();
2450 double dy=dpos.y();
2451 double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi;
2452 double rc=sqrt(dpos.Perp2()
2453 +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi)
2454 +qrc_plus_D*qrc_plus_D);
2455
2456 J(state_D,state_D)=q*(dx_sinphi_minus_dy_cosphi+qrc_plus_D)/rc;
2457 J(state_D,state_q_over_pt)=qpt*qrc_old*(J(state_D,state_D)-1.);
2458 J(state_D,state_phi)=q*qrc_plus_D*(dx*cosphi+dy*sinphi)/rc;
2459
2460 return NOERROR;
2461}
2462
2463// Compute contributions to the covariance matrix due to multiple scattering
2464// using the Lynch/Dahl empirical formulas
2465jerror_t DTrackFitterKalmanSIMD::GetProcessNoiseCentral(double ds,
2466 double chi2c_factor,
2467 double chi2a_factor,
2468 double chi2a_corr,
2469 const DMatrix5x1 &Sc,
2470 DMatrix5x5 &Q){
2471 Q.Zero();
2472 //return NOERROR;
2473 if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS3.0e-8){
2474 double tanl=Sc(state_tanl);
2475 double tanl2=tanl*tanl;
2476 double one_plus_tanl2=1.+tanl2;
2477 double one_over_pt=fabs(Sc(state_q_over_pt));
2478 double my_ds=fabs(ds);
2479 double my_ds_2=0.5*my_ds;
2480
2481 Q(state_phi,state_phi)=one_plus_tanl2;
2482 Q(state_tanl,state_tanl)=one_plus_tanl2*one_plus_tanl2;
2483 Q(state_q_over_pt,state_q_over_pt)=one_over_pt*one_over_pt*tanl2;
2484 Q(state_q_over_pt,state_tanl)=Q(state_tanl,state_q_over_pt)
2485 =Sc(state_q_over_pt)*tanl*one_plus_tanl2;
2486 Q(state_D,state_D)=ONE_THIRD0.33333333333333333*ds*ds;
2487
2488 // I am not sure the following is correct...
2489 double sign_D=(Sc(state_D)>0)?1.:-1.;
2490 Q(state_D,state_phi)=Q(state_phi,state_D)=sign_D*my_ds_2*sqrt(one_plus_tanl2);
2491 Q(state_D,state_tanl)=Q(state_tanl,state_D)=sign_D*my_ds_2*one_plus_tanl2;
2492 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);
2493
2494 double one_over_p_sq=one_over_pt*one_over_pt/one_plus_tanl2;
2495 double one_over_beta2=1.+mass2*one_over_p_sq;
2496 double chi2c=chi2c_factor*my_ds*one_over_beta2*one_over_p_sq;
2497 double chi2a=chi2a_factor*(1.+chi2a_corr*one_over_beta2)*one_over_p_sq;
2498 // F=Fraction of Moliere distribution to be taken into account
2499 // nu=0.5*chi2c/(chi2a*(1.-F));
2500 double nu=MOLIERE_RATIO15.0*chi2c/chi2a;
2501 double one_plus_nu=1.+nu;
2502 // sig2_ms=chi2c*1e-6/(1.+F*F)*((one_plus_nu)/nu*log(one_plus_nu)-1.);
2503 double sig2_ms=chi2c*MOLIERE_RATIO311.0e-7*(one_plus_nu/nu*log(one_plus_nu)-1.);
2504
2505 Q=sig2_ms*Q;
2506 }
2507
2508 return NOERROR;
2509}
2510
2511// Compute contributions to the covariance matrix due to multiple scattering
2512// using the Lynch/Dahl empirical formulas
2513jerror_t DTrackFitterKalmanSIMD::GetProcessNoise(double ds,
2514 double chi2c_factor,
2515 double chi2a_factor,
2516 double chi2a_corr,
2517 const DMatrix5x1 &S,
2518 DMatrix5x5 &Q){
2519
2520 Q.Zero();
2521 //return NOERROR;
2522 if (USE_MULS_COVARIANCE && chi2c_factor>0. && fabs(ds)>EPS3.0e-8){
2523 double tx=S(state_tx),ty=S(state_ty);
2524 double one_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
2525 double my_ds=fabs(ds);
2526 double my_ds_2=0.5*my_ds;
2527 double tx2=tx*tx;
2528 double ty2=ty*ty;
2529 double one_plus_tx2=1.+tx2;
2530 double one_plus_ty2=1.+ty2;
2531 double tsquare=tx2+ty2;
2532 double one_plus_tsquare=1.+tsquare;
2533
2534 Q(state_tx,state_tx)=one_plus_tx2*one_plus_tsquare;
2535 Q(state_ty,state_ty)=one_plus_ty2*one_plus_tsquare;
2536 Q(state_tx,state_ty)=Q(state_ty,state_tx)=tx*ty*one_plus_tsquare;
2537
2538 Q(state_x,state_x)=ONE_THIRD0.33333333333333333*ds*ds;
2539 Q(state_y,state_y)=Q(state_x,state_x);
2540 Q(state_y,state_ty)=Q(state_ty,state_y)
2541 = my_ds_2*sqrt(one_plus_tsquare*one_plus_ty2);
2542 Q(state_x,state_tx)=Q(state_tx,state_x)
2543 = my_ds_2*sqrt(one_plus_tsquare*one_plus_tx2);
2544
2545 double one_over_beta2=1.+one_over_p_sq*mass2;
2546 double chi2c=chi2c_factor*my_ds*one_over_beta2*one_over_p_sq;
2547 double chi2a=chi2a_factor*(1.+chi2a_corr*one_over_beta2)*one_over_p_sq;
2548 // F=MOLIERE_FRACTION =Fraction of Moliere distribution to be taken into account
2549 // nu=0.5*chi2c/(chi2a*(1.-F));
2550 double nu=MOLIERE_RATIO15.0*chi2c/chi2a;
2551 double one_plus_nu=1.+nu;
2552 double sig2_ms=chi2c*MOLIERE_RATIO211.0e-7*(one_plus_nu/nu*log(one_plus_nu)-1.);
2553
2554
2555 // printf("fac %f %f %f\n",chi2c_factor,chi2a_factor,chi2a_corr);
2556 //printf("omega %f\n",chi2c/chi2a);
2557
2558
2559 Q=sig2_ms*Q;
2560 }
2561
2562 return NOERROR;
2563}
2564
2565// Calculate the energy loss per unit length given properties of the material
2566// through which a particle of momentum p is passing
2567double DTrackFitterKalmanSIMD::GetdEdx(double q_over_p,double K_rho_Z_over_A,
2568 double rho_Z_over_A,double LnI){
2569 if (rho_Z_over_A<=0.) return 0.;
2570 //return 0.;
2571
2572 double p=fabs(1./q_over_p);
2573 double betagamma=p/MASS;
2574 double betagamma2=betagamma*betagamma;
2575 double gamma2=1.+betagamma2;
2576 double beta2=betagamma2/gamma2;
2577 if (beta2<EPS3.0e-8) beta2=EPS3.0e-8;
2578
2579 double two_Me_betagamma_sq=2.*ELECTRON_MASS0.000511*betagamma2;
2580 double Tmax
2581 =two_Me_betagamma_sq/(1.+2.*sqrt(gamma2)*m_ratio+m_ratio_sq);
2582
2583 // density effect
2584 double delta=CalcDensityEffect(betagamma,rho_Z_over_A,LnI);
2585
2586 return K_rho_Z_over_A/beta2*(-log(two_Me_betagamma_sq*Tmax)
2587 +2.*(LnI + beta2)+delta);
2588}
2589
2590// Calculate the variance in the energy loss in a Gaussian approximation.
2591// The standard deviation of the energy loss distribution is
2592// approximated by sigma=(scale factor) x Xi, where
2593// Xi=0.1535*density*(Z/A)*x/beta^2 [MeV]
2594inline double DTrackFitterKalmanSIMD::GetEnergyVariance(double ds,
2595 double one_over_beta2,
2596 double K_rho_Z_over_A){
2597 if (K_rho_Z_over_A<=0.) return 0.;
2598 //return 0;
2599
2600 double sigma=10.0*K_rho_Z_over_A*one_over_beta2*ds;
2601
2602 return sigma*sigma;
2603}
2604
2605// Smoothing algorithm for the forward trajectory. Updates the state vector
2606// at each step (going in the reverse direction to the filter) based on the
2607// information from all the steps and outputs the state vector at the
2608// outermost step.
2609jerror_t DTrackFitterKalmanSIMD::SmoothForward(DMatrix5x1 &Ss,DMatrix5x5 &Cs){
2610 if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE;
2611
2612 DMatrix5x1 S;
2613 DMatrix5x5 C;
2614 DMatrix5x5 JT,A;
2615
2616 unsigned int max=forward_traj.size()-1;
2617 S=(forward_traj[max].Skk);
2618 C=(forward_traj[max].Ckk);
2619 JT=(forward_traj[max].JT);
2620 Ss=S;
2621 Cs=C;
2622 for (unsigned int m=max-1;m>0;m--){
2623 if (forward_traj[m].h_id>0){
2624 if (forward_traj[m].h_id<1000){
2625 unsigned int id=forward_traj[m].h_id-1;
2626 A=fdc_updates[id].C*JT*C.InvertSym();
2627 Ss=fdc_updates[id].S+A*(Ss-S);
2628 Cs=fdc_updates[id].C+A*(Cs-C)*A.Transpose();
2629 }
2630 else{
2631 unsigned int id=forward_traj[m].h_id-1000;
2632 A=cdc_updates[id].C*JT*C.InvertSym();
2633 Ss=cdc_updates[id].S+A*(Ss-S);
2634 Cs=cdc_updates[id].C+A*(Cs-C)*A.Transpose();
2635 }
2636 }
2637 else{
2638 A=forward_traj[m].Ckk*JT*C.InvertSym();
2639 Ss=forward_traj[m].Skk+A*(Ss-S);
2640 Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose();
2641 }
2642
2643 S=forward_traj[m].Skk;
2644 C=forward_traj[m].Ckk;
2645 JT=forward_traj[m].JT;
2646 }
2647 A=forward_traj[0].Ckk*JT*C.InvertSym();
2648 Ss=forward_traj[0].Skk+A*(Ss-S);
2649 Cs=forward_traj[0].Ckk+A*(Cs-C)*A.Transpose();
2650
2651 return NOERROR;
2652}
2653
2654// Compute estimate for t0 using central parametrization.
2655jerror_t
2656DTrackFitterKalmanSIMD::EstimateT0Central(const DCDCTrackHit *hit,
2657 const DKalmanUpdate_t &cdc_update){
2658 // Wire position and direction variables
2659 DVector3 origin=hit->wire->origin;
2660 DVector3 dir=hit->wire->udir;
2661 double uz=dir.z();
2662 double ux=dir.x();
2663 double uy=dir.y();
2664 double z0=origin.z();
2665 double cosstereo=cos(hit->wire->stereo);
2666
2667 // Wire position at doca
2668 DVector3 wirepos=origin+(cdc_update.pos.z()-z0)/uz*dir;
2669 // Difference between it and track position
2670 DVector3 diff=cdc_update.pos-wirepos;
2671 double dx=diff.x();
2672 double dy=diff.y();
2673 double d=diff.Perp();
2674 double doca=d*cosstereo;
2675
2676 // Use the track information to estimate t0
2677 // Use approximate functional form for the distance to time relationship:
2678 // t(d)=c1 d^2 +c2 d^4
2679 double c1=1131,c2=140.7;
2680 double d_sq=doca*doca;
2681 double bfrac=(CDC_DRIFT_B_SCALE_PAR1602.0+CDC_DRIFT_B_SCALE_PAR258.22*cdc_update.B)
2682 /CDC_DRIFT_B_SCALE;
2683 double t0=hit->tdrift-cdc_update.tflight-bfrac*(c1*d_sq+c2*d_sq*d_sq);
2684
2685 // Calculate the variance in t0
2686 double dt_dd=bfrac*(2.*c1*doca+4*c2*doca*d_sq);
2687 double cosstereo_over_d=cosstereo/d;
2688 double dd_dz=-cosstereo_over_d*(dx*ux+dy*uy)/uz;
2689 double cosphi=cos(cdc_update.S(state_phi));
2690 double sinphi=sin(cdc_update.S(state_phi));
2691 double dd_dD=cosstereo_over_d*(dy*cosphi-dx*sinphi);
2692 double dd_dphi=-cdc_update.S(state_D)*cosstereo_over_d*(dx*cosphi+dy*sinphi);
2693 double sigma_t=2.948+35.7*doca;
2694
2695 double one_over_var
2696 =1./(sigma_t*sigma_t
2697 + dt_dd*dt_dd*(dd_dz*dd_dz*cdc_update.C(state_z,state_z)
2698 +dd_dD*dd_dD*cdc_update.C(state_D,state_D)
2699 +dd_dphi*dd_dphi*cdc_update.C(state_phi,state_phi)
2700 +2.*dd_dz*dd_dphi*cdc_update.C(state_z,state_phi)
2701 +2.*dd_dz*dd_dD*cdc_update.C(state_z,state_D)
2702 +2.*dd_dphi*dd_dD*cdc_update.C(state_phi,state_D)));
2703
2704 // weighted average
2705 mT0Average+=t0*one_over_var;
2706 mInvVarT0+=one_over_var;
2707
2708 return NOERROR;
2709}
2710
2711
2712// Smoothing algorithm for the central trajectory. Updates the state vector
2713// at each step (going in the reverse direction to the filter) based on the
2714// information from all the steps.
2715jerror_t DTrackFitterKalmanSIMD::SmoothCentral(DMatrix5x1 &Ss,DMatrix5x5 &Cs){
2716 if (central_traj.size()<2) return RESOURCE_UNAVAILABLE;
2717
2718 DMatrix5x1 S;
2719 DMatrix5x5 C;
2720 DMatrix5x5 JT,A;
2721
2722 // Variables for estimating t0 from tracking
2723 mInvVarT0=mT0Average=0.;
2724
2725 // path length
2726 double s=0,ds=0;
2727 // flight time
2728 ftime=0;
2729
2730 unsigned int max=central_traj.size()-1;
2731 S=(central_traj[max].Skk);
2732 C=(central_traj[max].Ckk);
2733 JT=(central_traj[max].JT);
2734 Ss=S;
2735 Cs=C;
2736
2737
2738
2739 printf("-----------\n");
2740 for (unsigned int m=max-1;m>0;m--){
2741 // path length increment
2742 ds=central_traj[m].s-s;
2743 s=central_traj[m].s;
2744 double q_over_p=Ss(state_q_over_pt)*cos(atan(Ss(state_tanl)));
2745 ftime+=ds*sqrt(1.+mass2*q_over_p*q_over_p); // in units where c=1
2746 central_traj[m].t=ftime;
2747
2748 // central_traj[m].Skk.Print();
2749 // central_traj[m].Ckk.Print();
2750
2751 if (central_traj[m].h_id>0){
2752 unsigned int id=central_traj[m].h_id-1;
2753 A=cdc_updates[id].C*JT*C.InvertSym();
2754 Ss=cdc_updates[id].S+A*(Ss-S);
2755 Cs=cdc_updates[id].C+A*(Cs-C)*A.Transpose();
2756
2757 printf("======================\n");
2758 C.Print();
2759 cdc_updates[id].C.Print();
2760 JT.Print();
2761
2762
2763 // We will swim in both the +z and the -z direction to see if we have
2764 // bracketed the minimum doca to the wire
2765 DMatrix5x1 Splus(Ss);
2766 DMatrix5x5 Cplus(Cs);
2767 DMatrix5x1 Sminus(Ss);
2768 DMatrix5x5 Cminus(Cs);
2769 DMatrix5x5 J,C;
2770 DMatrix5x1 S;
2771
2772 DVector3 pos(central_traj[m].pos.x()-Ss(state_D)*sin(Ss(state_phi)),
2773 central_traj[m].pos.y()+Ss(state_D)*cos(Ss(state_phi)),
2774 Ss(state_z));
2775 DVector3 pos1(pos);
2776
2777 double dedx=0.;
2778 if (CORRECT_FOR_ELOSS){
2779 double q_over_p=Ss(state_q_over_pt)*cos(atan(Ss(state_tanl)));
2780 dedx=GetdEdx(q_over_p,central_traj[m].K_rho_Z_over_A,
2781 central_traj[m].rho_Z_over_A,central_traj[m].LnI);
2782 }
2783
2784 // Wire position and direction variables
2785 DVector3 origin=my_cdchits[id]->hit->wire->origin;
2786 DVector3 dir=my_cdchits[id]->hit->wire->udir;
2787 double uz=dir.z();
2788 double ux=dir.x();
2789 double uy=dir.y();
2790 double z0=origin.z();
2791 double cosstereo=cos(my_cdchits[id]->hit->wire->stereo);
2792 DVector3 wirepos=origin+(pos.z()-z0)/uz*dir;
2793
2794 double doca=(pos-wirepos).Perp();
2795
2796 StepJacobian(pos,1.0,Splus,dedx,J);
2797 // Update covariance matrix
2798 Cplus=Cplus.SandwichMultiply(J);
2799
2800 FixedStep(pos,1.0,Splus,dedx);
2801
2802
2803 wirepos=origin+(pos.z()-z0)/uz*dir;
2804 double docaplus=(pos-wirepos).Perp();
2805
2806
2807 StepJacobian(pos1,-1.0,Sminus,dedx,J);
2808 // Update covariance matrix
2809 Cminus=Cminus.SandwichMultiply(J);
2810
2811 FixedStep(pos1,-1.0,Sminus,dedx);
2812
2813 wirepos=origin+(pos1.z()-z0)/uz*dir;
2814 double docaminus=(pos1-wirepos).Perp();
2815
2816 if (docaminus>doca && docaplus>doca){
2817 S=Splus;
2818 double ds=BrentsAlgorithm(1.0,1.0,dedx,pos,origin,dir,Splus);
2819 StepJacobian(pos,ds,S,dedx,J);
2820 // Update covariance matrix
2821 C=Cplus.SandwichMultiply(J);
2822
2823 }
2824 else if (docaplus>doca && doca>docaminus){
2825 pos=pos1;
2826 while (doca>docaminus
2827 && pos.Perp()<R_MAX65.0
2828 && pos.z()>cdc_origin[2]&&pos.z()<endplate_z){
2829 docaminus=doca;
2830
2831 StepJacobian(pos,-1.0,Sminus,dedx,J);
2832 // Update covariance matrix
2833 Cminus=Cminus.SandwichMultiply(J);
2834
2835 FixedStep(pos,-1.0,Sminus,dedx);
2836
2837 wirepos=origin+(pos.z()-z0)/uz*dir;
2838 doca=(pos-wirepos).Perp();
2839
2840 break;
2841 }
2842
2843 S=Sminus;
2844 double ds=BrentsAlgorithm(-1.0,-1.0,dedx,pos,origin,dir,Sminus);
2845 StepJacobian(pos,ds,S,dedx,J);
2846 // Update covariance matrix
2847 C=Cminus.SandwichMultiply(J);
2848 }
2849 else{
2850 while (doca<docaplus
2851 && pos.Perp()<R_MAX65.0
2852 && pos.z()>cdc_origin[2]&&pos.z()<endplate_z){
2853 docaplus=doca;
2854
2855 StepJacobian(pos,1.0,Splus,dedx,J);
2856 // Update covariance matrix
2857 Cplus=Cplus.SandwichMultiply(J);
2858
2859 FixedStep(pos,1.0,Splus,dedx);
2860
2861 wirepos=origin+(pos.z()-z0)/uz*dir;
2862 doca=(pos-wirepos).Perp();
2863
2864 break;
2865 }
2866
2867 S=Splus;
2868 double ds=BrentsAlgorithm(1.0,1.0,dedx,pos1,origin,dir,Splus);
2869 StepJacobian(pos,ds,S,dedx,J);
2870 // Update covariance matrix
2871 C=Cplus.SandwichMultiply(J);
2872 }
2873
2874
2875 // Wire position at doca
2876 wirepos=origin+(pos.z()-z0)/uz*dir;
2877 // Difference between it and track position
2878 DVector3 diff=pos-wirepos;
2879 double dx=diff.x();
2880 double dy=diff.y();
2881 double d=diff.Perp();
2882 doca=d*cosstereo;
2883
2884 // Projection matrix
2885 DMatrix5x1 H_T;
2886 DMatrix1x5 H;
2887
2888 double sinphi=sin(S(state_phi));
2889 double cosphi=cos(S(state_phi));
2890 H(state_D)=H_T(state_D)=(dy*cosphi-dx*sinphi)*cosstereo/d;
2891 H(state_phi)=H_T(state_phi)
2892 =-S(state_D)*cosstereo*(dx*cosphi+dy*sinphi)/d;
2893 H(state_z)=H_T(state_z)=-cosstereo*(dx*ux+dy*uy)/(uz*d);
2894
2895 double V=0.2133;
2896 double res=-d*cosstereo;
2897 if (fit_type==kTimeBased){
2898 double dt=my_cdchits[id]->hit->tdrift-mT0-central_traj[m].t;
2899 double B=central_traj[m].B;
2900 res+=cdc_drift_distance(dt,B);
2901
2902 // Measurement error
2903 V=cdc_variance(B,S(state_tanl),dt);
2904 }
2905 //double var=V-H*C*H_T;
2906 double var=V-C.SandwichMultiply(H_T);
2907 my_cdchits[id]->residual=res;
2908 my_cdchits[id]->sigma=sqrt(var);
2909
2910 pulls.push_back(pull_t(my_cdchits[id]->residual,
2911 my_cdchits[id]->sigma,s));
2912 if (id==0) break;
2913 }
2914 else{
2915 A=central_traj[m].Ckk*JT*C.InvertSym();
2916 Ss=central_traj[m].Skk+A*(Ss-S);
2917 Cs=central_traj[m].Ckk+A*(Cs-C)*A.Transpose();
2918 }
2919 S=central_traj[m].Skk;
2920 C=central_traj[m].Ckk;
2921 JT=(central_traj[m].JT);
2922 }
2923
2924 // t0 estimate
2925 if (mInvVarT0>0) mT0Average/=mInvVarT0;
2926
2927 return NOERROR;
2928
2929}
2930
2931// Smoothing algorithm for the forward_traj_cdc trajectory.
2932// Updates the state vector
2933// at each step (going in the reverse direction to the filter) based on the
2934// information from all the steps and outputs the state vector at the
2935// outermost step.
2936jerror_t DTrackFitterKalmanSIMD::SmoothForwardCDC(DMatrix5x1 &Ss,
2937 DMatrix5x5 &Cs){
2938 if (forward_traj.size()<2) return RESOURCE_UNAVAILABLE;
2939
2940 DMatrix5x1 S;
2941 DMatrix5x5 C;
2942 DMatrix5x5 JT,A;
2943
2944 unsigned int max=forward_traj.size()-1;
2945 S=(forward_traj[max].Skk);
2946 C=(forward_traj[max].Ckk);
2947 JT=(forward_traj[max].JT);
2948 Ss=S;
2949 Cs=C;
2950
2951 for (unsigned int m=max-1;m>0;m--){
2952 if (forward_traj[m].h_id>0){
2953 unsigned int cdc_index=forward_traj[m].h_id-1;
2954
2955 A=cdc_updates[cdc_index].C*JT*C.InvertSym();
2956 Ss=cdc_updates[cdc_index].S+A*(Ss-S);
2957 Cs=cdc_updates[cdc_index].C+A*(Cs-C)*A.Transpose();
2958 }
2959 else{
2960 A=forward_traj[m].Ckk*JT*C.InvertSym();
2961 Ss=forward_traj[m].Skk+A*(Ss-S);
2962 Cs=forward_traj[m].Ckk+A*(Cs-C)*A.Transpose();
2963 }
2964
2965 S=forward_traj[m].Skk;
2966 C=forward_traj[m].Ckk;
2967 JT=forward_traj[m].JT;
2968 }
2969 A=forward_traj[0].Ckk*JT*C.InvertSym();
2970 Ss=forward_traj[0].Skk+A*(Ss-S);
2971 Cs=forward_traj[0].Ckk+A*(Cs-C)*A.Transpose();
2972
2973 return NOERROR;
2974}
2975
2976
2977// Interface routine for Kalman filter
2978jerror_t DTrackFitterKalmanSIMD::KalmanLoop(void){
2979 if (z_<Z_MIN0.) return VALUE_OUT_OF_RANGE;
2980
2981 // Vector to store the list of hits used in the fit for the forward parametrization
2982 vector<const DCDCTrackHit*>forward_cdc_used_in_fit;
2983
2984 // State vector and initial guess for covariance matrix
2985 DMatrix5x1 S0;
2986 DMatrix5x5 C0;
2987
2988 chisq_=MAX_CHI21e16;
2989
2990 // Angle with respect to beam line
2991 double theta_deg=(180/M_PI3.14159265358979323846)*input_params.momentum().Theta();
2992 double theta_deg_sq=theta_deg*theta_deg;
2993
2994 // Guess for momentum error
2995 double dp_over_p=0.;
2996 if (theta_deg<15){
2997 //dp_over_p=0.0833-0.016*theta_deg+0.00938*theta_deg*theta_deg;
2998 dp_over_p=1.773-0.3566*theta_deg+0.01869*theta_deg_sq;
2999 //dp_over_p=0.05;
3000 }
3001 else {
3002 dp_over_p=0.079+0.00186*theta_deg-9.14e-6*theta_deg_sq;
3003 if (fit_type==kWireBased && theta_deg<25.) dp_over_p=0.18;
3004 }
3005
3006 // Input charge
3007 double q=input_params.charge();
3008
3009 // Input momentum
3010 DVector3 pvec=input_params.momentum();
3011 double p_mag=pvec.Mag();
3012 if (MASS>0.9 && p_mag<MIN_PROTON_P0.3){
3013 pvec.SetMag(MIN_PROTON_P0.3);
3014 p_mag=MIN_PROTON_P0.3;
3015 }
3016 else if (MASS<0.9 && p_mag<MIN_PION_P0.08){
3017 pvec.SetMag(MIN_PION_P0.08);
3018 p_mag=MIN_PION_P0.08;
3019 }
3020 if (p_mag>MAX_P12.0){
3021 pvec.SetMag(MAX_P12.0);
3022 p_mag=MAX_P12.0;
3023 }
3024 double pz=pvec.z();
3025
3026 double momentum_factor=1.;
3027 if (MASS>0.9){
3028 double a=5.64e-3;
3029 double b=8.98e-2;
3030 momentum_factor=(a/(p_mag*p_mag)+b*p_mag)/(a/(0.5*0.5)+b*0.5);
3031 }
3032 else{
3033 double a=2.21e-3;
3034 double b=7.8e-2;
3035 momentum_factor=(a/(p_mag*p_mag)+b*p_mag)/(a/(0.4*0.4)+b*0.4);
3036 }
3037 dp_over_p*=momentum_factor;
3038
3039 // Initial position
3040 x_=input_params.position().x();
3041 y_=input_params.position().y();
3042 z_=input_params.position().z();
3043
3044 // Initial direction tangents
3045 tx_=pvec.x()/pz;
3046 ty_=pvec.y()/pz;
3047
3048 // deal with hits in FDC
3049 if (my_fdchits.size()>0){
3050 if (DEBUG_LEVEL>0){
3051 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3051<<" " << "Using forward parameterization." <<endl;
3052 }
3053
3054 // Initial guess for the state vector
3055 S0(state_x)=x_;
3056 S0(state_y)=y_;
3057 S0(state_tx)=tx_;
3058 S0(state_ty)=ty_;
3059 S0(state_q_over_p)=q_over_p_=q/p_mag;
3060
3061 // Initial guess for forward representation covariance matrix
3062 C0(state_x,state_x)=1;
3063 C0(state_y,state_y)=1;
3064 C0(state_tx,state_tx)=0.001;
3065 C0(state_ty,state_ty)=0.001;
3066 if (theta_deg<1.) C0(state_tx,state_tx)=C0(state_ty,state_ty)=0.1;
3067 C0(state_q_over_p,state_q_over_p)=dp_over_p*dp_over_p*q_over_p_*q_over_p_;
3068
3069 kalman_error_t error=ForwardFit(S0,C0);
3070 if (error==FIT_SUCCEEDED) return NOERROR;
3071 if (my_cdchits.size()<6) return UNRECOVERABLE_ERROR;
3072 }
3073
3074 // Deal with hits in the CDC
3075 if (my_cdchits.size()>5){
3076 kalman_error_t error=FIT_NOT_DONE;
3077 kalman_error_t cdc_error=FIT_NOT_DONE;
3078
3079 // Chi-squared, degrees of freedom, and probability
3080 double forward_prob=0.;
3081 double chisq_forward=MAX_CHI21e16;
3082 unsigned int ndof_forward=0;
3083
3084 // Parameters at "vertex"
3085 double D=0.,phi=0.,q_over_pt=0.,tanl=0.,x=0.,y=0.,z=0.;
3086
3087 // Use forward parameters for CDC-only tracks with theta<70 degrees
3088 if (theta_deg<70){
3089 if (DEBUG_LEVEL>0){
3090 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3090<<" " << "Using forward parameterization." <<endl;
3091 }
3092
3093 // Initialize the state vector
3094 S0(state_x)=x_;
3095 S0(state_y)=y_;
3096 S0(state_tx)=tx_;
3097 S0(state_ty)=ty_;
3098 S0(state_q_over_p)=q_over_p_=q/p_mag;
3099
3100 // Initial guess for forward representation covariance matrix
3101 C0(state_x,state_x)=2.0;
3102 C0(state_y,state_y)=2.0;
3103 double sig_lambda=0.017;
3104 if (theta_deg>28)
3105 sig_lambda=-0.0365+0.00222*theta_deg-1.23e-5*theta_deg_sq;
3106 double temp=sig_lambda*(1.+tx_*tx_+ty_*ty_);
3107 C0(state_tx,state_tx)=C0(state_ty,state_ty)=temp*temp;
3108 C0(state_q_over_p,state_q_over_p)=dp_over_p*dp_over_p*q_over_p_*q_over_p_;
3109
3110 error=ForwardCDCFit(S0,C0);
3111 if (error==FIT_SUCCEEDED){
3112 // Find the CL of the fit
3113 forward_prob=TMath::Prob(chisq_,ndf_);
3114 if (forward_prob<0.0001){
3115 // Save the best values for the parameters and chi2 for now
3116 chisq_forward=chisq_;
3117 ndof_forward=ndf_;
3118 D=D_;
3119 x=x_;
3120 y=y_;
3121 z=z_;
3122 phi=phi_;
3123 tanl=tanl_;
3124 q_over_pt=q_over_pt_;
3125
3126 // Save the list of hits used in the fit
3127 forward_cdc_used_in_fit.assign(cdchits_used_in_fit.begin(),cdchits_used_in_fit.end());
3128
3129 error=LOW_CL_FIT;
3130 }
3131 else return NOERROR;
3132 }
3133 }
3134
3135 // Attempt to fit the track using the central parametrization.
3136 if (DEBUG_LEVEL>0){
3137 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3137<<" " << "Using central parameterization." <<endl;
3138 }
3139
3140 // Initialize the state vector
3141 S0(state_q_over_pt)=q_over_pt_=q/pvec.Perp();
3142 S0(state_phi)=phi_=pvec.Phi();
3143 S0(state_tanl)=tanl_=tan(M_PI_21.57079632679489661923-pvec.Theta());
3144 S0(state_z)=z_=input_params.position().z();
3145 S0(state_D)=D_=0.;
3146
3147 // Initialize the covariance matrix
3148 double dz=2.5;
3149 // if (theta_deg<70) dz=3.0;
3150 //if (theta_deg<22.) dz=1.5;
3151
3152 C0(state_z,state_z)=dz*dz;
3153 if (theta_deg<28.){
3154 theta_deg=28.;
3155 theta_deg_sq=theta_deg*theta_deg;
3156 }
3157 else if (theta_deg>125.){
3158 theta_deg=125.;
3159 theta_deg_sq=theta_deg*theta_deg;
3160 }
3161 double sig_lambda=-0.0479+0.00253*theta_deg-1.423e-5*theta_deg_sq;
3162 double dpt_over_pt=0.00326+0.0029*theta_deg-2.715e-5*theta_deg_sq+9.242e-8*theta_deg*theta_deg_sq;
3163 if (fit_type==kWireBased && theta_deg<25.) dpt_over_pt=0.18;
3164 dpt_over_pt*=momentum_factor;
3165 C0(state_q_over_pt,state_q_over_pt)
3166 =dpt_over_pt*dpt_over_pt*q_over_pt_*q_over_pt_;
3167 double dphi=0.017;
3168 if (theta_deg<30) dphi=0.045-0.00091*theta_deg;
3169 C0(state_phi,state_phi)=dphi*dphi;
3170 C0(state_D,state_D)=1.0;
3171 double one_plus_tanl2=1.+tanl_*tanl_;
3172 C0(state_tanl,state_tanl)=(one_plus_tanl2)*(one_plus_tanl2)
3173 *sig_lambda*sig_lambda;
3174
3175 cdc_error=CentralFit(S0,C0);
3176 if (cdc_error==FIT_SUCCEEDED){
3177 // if the result of the fit using the forward parameterization succeeded
3178 // but the chi2 was too high, it still may provide the best estimate for
3179 // the track parameters...
3180 if (error==LOW_CL_FIT){
3181 double central_prob=TMath::Prob(chisq_,ndf_);
3182
3183 if (DEBUG_LEVEL>0){
3184 printf("chi2/nu f %f c %f\n",chisq_forward/ndof_forward,chisq_/ndf_);
3185 printf("Forward chi2 %f prob %g central chi2 %f prob %g\n",chisq_forward,forward_prob,chisq_,central_prob);
3186 }
3187
3188 if (central_prob<forward_prob || chisq_/ndf_>chisq_forward/ndof_forward){
3189 phi_=phi;
3190 q_over_pt_=q_over_pt;
3191 tanl_=tanl;
3192 D_=D;
3193 x_=x;
3194 y_=y;
3195 z_=z;
3196 chisq_=chisq_forward;
3197 ndf_= ndof_forward;
3198
3199 cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end());
3200 }
3201 return NOERROR;
3202 }
3203 }
3204 // otherwise if the fit using the forward parametrization worked, return that
3205 else if (error==LOW_CL_FIT){
3206 phi_=phi;
3207 q_over_pt_=q_over_pt;
3208 tanl_=tanl;
3209 D_=D;
3210 x_=x;
3211 y_=y;
3212 z_=z;
3213 chisq_=chisq_forward;
3214 ndf_= ndof_forward;
3215
3216 cdchits_used_in_fit.assign(forward_cdc_used_in_fit.begin(),forward_cdc_used_in_fit.end());
3217
3218 return NOERROR;
3219 }
3220 else return UNRECOVERABLE_ERROR;
3221 }
3222
3223 return NOERROR;
3224}
3225
3226#define ITMAX100 100
3227#define CGOLD0.3819660 0.3819660
3228#define ZEPS1.0e-10 1.0e-10
3229#define SHFT(a,b,c,d)(a)=(b);(b)=(c);(c)=(d); (a)=(b);(b)=(c);(c)=(d);
3230#define SIGN(a,b)((b)>=0.0?fabs(a):-fabs(a)) ((b)>=0.0?fabs(a):-fabs(a))
3231// Routine for finding the minimum of a function bracketed between two values
3232// (see Numerical Recipes in C, pp. 404-405).
3233double DTrackFitterKalmanSIMD::BrentsAlgorithm(double ds1,double ds2,
3234 double dedx,DVector3 &pos,
3235 const DVector3 &origin,
3236 const DVector3 &dir,
3237 DMatrix5x1 &Sc){
3238 double d=0.;
3239 double e=0.0; // will be distance moved on step before last
3240 double ax=0.;
3241 double bx=-ds1;
3242 double cx=-ds1-ds2;
3243
3244 double a=(ax<cx?ax:cx);
3245 double b=(ax>cx?ax:cx);
3246 double x=bx,w=bx,v=bx;
3247
3248 // printf("ds1 %f ds2 %f\n",ds1,ds2);
3249
3250 // Save the starting position
3251 // DVector3 pos0=pos;
3252 // DMatrix S0(Sc);
3253
3254 // Step to intermediate point
3255 FixedStep(pos,x,Sc,dedx);
3256 DVector3 wirepos=origin+((pos.z()-origin.z())/dir.z())*dir;
3257 double u_old=x;
3258 double u=0.;
3259
3260 // initialization
3261 double fw=(pos-wirepos).Perp();
3262 double fv=fw,fx=fw;
3263
3264 // main loop
3265 for (unsigned int iter=1;iter<=ITMAX100;iter++){
3266 double xm=0.5*(a+b);
3267 double tol1=EPS21.e-4*fabs(x)+ZEPS1.0e-10;
3268 double tol2=2.0*tol1;
3269
3270 //printf("z %f r %f\n",pos.Z(),pos.Perp());
3271 if (fabs(x-xm)<=(tol2-0.5*(b-a))){
3272 if (pos.z()<=cdc_origin[2]){
3273 unsigned int iter2=0;
3274 double ds_temp=0.;
3275 while (fabs(pos.z()-cdc_origin[2])>EPS21.e-4 && iter2<20){
3276 u=x-(cdc_origin[2]-pos.z())*sin(atan(Sc(state_tanl)));
3277 x=u;
3278 ds_temp+=u_old-u;
3279 // Function evaluation
3280 FixedStep(pos,u_old-u,Sc,dedx);
3281 u_old=u;
3282 iter2++;
3283 }
3284 //printf("new z %f ds %f \n",pos.z(),x);
3285 return ds_temp;
3286 }
3287 else if (pos.z()>=endplate_z){
3288 unsigned int iter2=0;
3289 double ds_temp=0.;
3290 while (fabs(pos.z()-endplate_z)>EPS21.e-4 && iter2<20){
3291 u=x-(endplate_z-pos.z())*sin(atan(Sc(state_tanl)));
3292 x=u;
3293 ds_temp+=u_old-u;
3294 // Function evaluation
3295 FixedStep(pos,u_old-u,Sc,dedx);
3296 u_old=u;
3297 iter2++;
3298 }
3299 //printf("new z %f ds %f \n",pos.z(),x);
3300 return ds_temp;
3301 }
3302
3303 return cx-x;
3304 }
3305 // trial parabolic fit
3306 if (fabs(e)>tol1){
3307 double x_minus_w=x-w;
3308 double x_minus_v=x-v;
3309 double r=x_minus_w*(fx-fv);
3310 double q=x_minus_v*(fx-fw);
3311 double p=x_minus_v*q-x_minus_w*r;
3312 q=2.0*(q-r);
3313 if (q>0.0) p=-p;
3314 q=fabs(q);
3315 double etemp=e;
3316 e=d;
3317 if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x))
3318 // fall back on the Golden Section technique
3319 d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x));
3320 else{
3321 // parabolic step
3322 d=p/q;
3323 u=x+d;
3324 if (u-a<tol2 || b-u <tol2)
3325 d=SIGN(tol1,xm-x)((xm-x)>=0.0?fabs(tol1):-fabs(tol1));
3326 }
3327 } else{
3328 d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x));
3329 }
3330 u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d)((d)>=0.0?fabs(tol1):-fabs(tol1)));
3331
3332 // Function evaluation
3333 FixedStep(pos,u_old-u,Sc,dedx);
3334 u_old=u;
3335
3336 wirepos=origin+((pos.z()-origin.z())/dir.z())*dir;
3337 double fu=(pos-wirepos).Perp();
3338
3339 //printf("Brent: z %f d %f\n",pos.z(),fu);
3340
3341 if (fu<=fx){
3342 if (u>=x) a=x; else b=x;
3343 SHFT(v,w,x,u)(v)=(w);(w)=(x);(x)=(u);;
3344 SHFT(fv,fw,fx,fu)(fv)=(fw);(fw)=(fx);(fx)=(fu);;
3345 }
3346 else {
3347 if (u<x) a=u; else b=u;
3348 if (fu<=fw || w==x){
3349 v=w;
3350 w=u;
3351 fv=fw;
3352 fw=fu;
3353 }
3354 else if (fu<=fv || v==x || v==w){
3355 v=u;
3356 fv=fu;
3357 }
3358 }
3359 }
3360
3361 return cx-x;
3362}
3363
3364// Routine for finding the minimum of a function bracketed between two values
3365// (see Numerical Recipes in C, pp. 404-405).
3366double DTrackFitterKalmanSIMD::BrentsAlgorithm(double z,double dz,
3367 double dedx,const DVector3 &origin,
3368 const DVector3 &dir,
3369 const DMatrix5x1 &S){
3370 double d=0.,u=0.;
3371 double e=0.0; // will be distance moved on step before last
3372 double ax=0.;
3373 double bx=-dz;
3374 double cx=-2.*dz;
3375
3376 double a=(ax<cx?ax:cx);
3377 double b=(ax>cx?ax:cx);
3378 double x=bx,w=bx,v=bx;
3379
3380 // Save the state vector after the last step
3381 DMatrix5x1 S0;
3382 S0=S;
3383
3384 // Step to intermediate point
3385 Step(z,z+x,dedx,S0);
3386
3387 DVector3 wirepos=origin+((z+x-origin.z())/dir.z())*dir;
3388 DVector3 pos(S0(state_x),S0(state_y),z+x);
3389
3390 // initialization
3391 double fw=(pos-wirepos).Perp();
3392 double fv=fw;
3393 double fx=fw;
3394
3395 // main loop
3396 for (unsigned int iter=1;iter<=ITMAX100;iter++){
3397 double xm=0.5*(a+b);
3398 double tol1=EPS21.e-4*fabs(x)+ZEPS1.0e-10;
3399 double tol2=2.0*tol1;
3400 if (fabs(x-xm)<=(tol2-0.5*(b-a))){
3401 if (pos.z()>=endplate_z) return (endplate_z-z);
3402 return x;
3403 }
3404 // trial parabolic fit
3405 if (fabs(e)>tol1){
3406 double x_minus_w=x-w;
3407 double x_minus_v=x-v;
3408 double r=x_minus_w*(fx-fv);
3409 double q=x_minus_v*(fx-fw);
3410 double p=x_minus_v*q-x_minus_w*r;
3411 q=2.0*(q-r);
3412 if (q>0.0) p=-p;
3413 q=fabs(q);
3414 double etemp=e;
3415 e=d;
3416 if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x))
3417 // fall back on the Golden Section technique
3418 d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x));
3419 else{
3420 // parabolic step
3421 d=p/q;
3422 u=x+d;
3423 if (u-a<tol2 || b-u <tol2)
3424 d=SIGN(tol1,xm-x)((xm-x)>=0.0?fabs(tol1):-fabs(tol1));
3425 }
3426 } else{
3427 d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x));
3428 }
3429 u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d)((d)>=0.0?fabs(tol1):-fabs(tol1)));
3430
3431 // Function evaluation
3432 S0=S;
3433 Step(z,z+u,dedx,S0);
3434
3435 wirepos=origin+((z+u-origin.z())/dir.z())*dir;
3436 pos.SetXYZ(S0(state_x),S0(state_y),z+u);
3437 double fu=(pos-wirepos).Perp();
3438
3439 if (fu<=fx){
3440 if (u>=x) a=x; else b=x;
3441 SHFT(v,w,x,u)(v)=(w);(w)=(x);(x)=(u);;
3442 SHFT(fv,fw,fx,fu)(fv)=(fw);(fw)=(fx);(fx)=(fu);;
3443 }
3444 else {
3445 if (u<x) a=u; else b=u;
3446 if (fu<=fw || w==x){
3447 v=w;
3448 w=u;
3449 fv=fw;
3450 fw=fu;
3451 }
3452 else if (fu<=fv || v==x || v==w){
3453 v=u;
3454 fv=fu;
3455 }
3456 }
3457 }
3458 return x;
3459}
3460
3461// Kalman engine for Central tracks; updates the position on the trajectory
3462// after the last hit (closest to the target) is added
3463kalman_error_t DTrackFitterKalmanSIMD::KalmanCentral(double anneal_factor,
3464 DMatrix5x1 &Sc,DMatrix5x5 &Cc,
3465 DVector3 &pos,double &chisq,
3466 unsigned int &my_ndf){
3467 DMatrix1x5 H; // Track projection matrix
3468 DMatrix5x1 H_T; // Transpose of track projection matrix
3469 DMatrix5x5 J; // State vector Jacobian matrix
3470 //DMatrix5x5 JT; // transpose of this matrix
3471 DMatrix5x5 Q; // Process noise covariance matrix
3472 DMatrix5x1 K; // KalmanSIMD gain matrix
3473 DMatrix5x5 Ctest; // covariance matrix
3474 double V=0.2028; //1.56*1.56/12.; // Measurement variance
3475 // double V=0.05332; // 0.8*0.8/12
3476 double InvV; // inverse of variance
3477 //DMatrix5x1 dS; // perturbation in state vector
3478 DMatrix5x1 S0,S0_; // state vector
3479
3480 // set the used_in_fit flags to false for cdc hits
3481 for (unsigned int i=0;i<cdc_updates.size();i++) cdc_updates[i].used_in_fit=false;
3482
3483 // Initialize the chi2 for this part of the track
3484 chisq=0.;
3485 my_ndf=0;
3486 double chi2cut=anneal_factor*anneal_factor*NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT;
3487
3488 // path length increment
3489 double ds2=0.;
3490
3491 //printf(">>>>>>>>>>>>>>>>\n");
3492
3493 // beginning position
3494 pos.SetXYZ(central_traj[0].pos.x()-Sc(state_D)*sin(Sc(state_phi)),
3495 central_traj[0].pos.y()+Sc(state_D)*cos(Sc(state_phi)),
3496 Sc(state_z));
3497
3498 // Wire origin and direction
3499 // unsigned int cdc_index=my_cdchits.size()-1;
3500 unsigned int cdc_index=break_point_cdc_index;
3501 DVector3 origin=my_cdchits[cdc_index]->hit->wire->origin;
3502 double z0w=origin.z();
3503 DVector3 dir=my_cdchits[cdc_index]->hit->wire->udir;
3504 double uz=dir.z();
3505 DVector3 wirepos=origin+((pos.z()-z0w)/uz)*dir;
3506
3507 // Save the starting values for C and S in the deque
3508 central_traj[0].Skk=Sc;
3509 central_traj[0].Ckk=Cc;
3510
3511 // doca variables
3512 double doca,old_doca=(pos-wirepos).Perp();
3513
3514 // energy loss
3515 double dedx=0.;
3516
3517 // Boolean for flagging when we are done with measurements
3518 bool more_measurements=true;
3519
3520 // Initialize S0_ and perform the loop over the trajectory
3521 S0_=central_traj[0].S;
3522
3523 for (unsigned int k=1;k<central_traj.size();k++){
3524 unsigned int k_minus_1=k-1;
3525
3526 // Check that C matrix is positive definite
3527 if (Cc(0,0)<0 || Cc(1,1)<0 || Cc(2,2)<0 || Cc(3,3)<0 || Cc(4,4)<0){
3528 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3528<<" " << "Broken covariance matrix!" <<endl;
3529 return BROKEN_COVARIANCE_MATRIX;
3530 }
3531
3532 // Get the state vector, jacobian matrix, and multiple scattering matrix
3533 // from reference trajectory
3534 S0=central_traj[k].S;
3535 J=central_traj[k].J;
3536 // JT=central_traj[k].JT;
3537 Q=central_traj[k].Q;
3538
3539 //Q.Print();
3540 //J.Print();
3541
3542 // State S is perturbation about a seed S0
3543 //dS=Sc-S0_;
3544 //dS.Zero();
3545
3546 // Update the actual state vector and covariance matrix
3547 Sc=S0+J*(Sc-S0_);
3548 // Cc=J*(Cc*JT)+Q;
3549 //Cc=Q.AddSym(J*Cc*JT);
3550 Cc=Q.AddSym(Cc.SandwichMultiply(J));
3551
3552 //Sc=central_traj[k].S+central_traj[k].J*(Sc-S0_);
3553 //Cc=central_traj[k].Q.AddSym(central_traj[k].J*Cc*central_traj[k].JT);
3554
3555 // update position based on new doca to reference trajectory
3556 pos.SetXYZ(central_traj[k].pos.x()-Sc(state_D)*sin(Sc(state_phi)),
3557 central_traj[k].pos.y()+Sc(state_D)*cos(Sc(state_phi)),
3558 Sc(state_z));
3559 // Bail if the position is grossly outside of the tracking volume
3560 if (pos.Perp()>R_MAX65.0 || Sc(state_z)<Z_MIN0. || Sc(state_z)>Z_MAX175.0){
3561 if (DEBUG_LEVEL>2)
3562 {
3563 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3563<<" "<< "Went outside of tracking volume at z="<<Sc(state_z)
3564 << " r="<<pos.Perp()<<endl;
3565 }
3566 //break;
3567 return POSITION_OUT_OF_RANGE;
3568 }
3569 // Bail if the transverse momentum has dropped below some minimum
3570 if (1./fabs(Sc(state_q_over_pt))<=PT_MIN0.01){
3571 if (DEBUG_LEVEL>2)
3572 {
3573 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3573<<" " << "Bailing: PT = " << 1./fabs(Sc(state_q_over_pt))
3574 << " at step " << k
3575 << endl;
3576 }
3577 return MOMENTUM_OUT_OF_RANGE;
3578 }
3579
3580
3581 // Save the current state of the reference trajectory
3582 S0_=S0;
3583
3584 // Save the current state and covariance matrix in the deque
3585 central_traj[k].Skk=Sc;
3586 central_traj[k].Ckk=Cc;
3587
3588 // new wire position
3589 wirepos=origin+((pos.z()-z0w)/uz)*dir;
3590
3591 // new doca
3592 doca=(pos-wirepos).Perp();
3593
3594 // Check if the doca is no longer decreasing
3595 if ((doca>old_doca+EPS31.e-2/* && pos.z()>cdc_origin[2]*/)
3596 && more_measurements){
3597 if (my_cdchits[cdc_index]->status==good_hit){
3598 // Save values at end of current step
3599 DVector3 pos0=central_traj[k].pos;
3600
3601 // dEdx for current position along trajectory
3602 double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
3603 if (CORRECT_FOR_ELOSS){
3604 dedx=GetdEdx(q_over_p, central_traj[k].K_rho_Z_over_A,
3605 central_traj[k].rho_Z_over_A,central_traj[k].LnI);
3606 }
3607 // Variables for the computation of D at the doca to the wire
3608 double D=Sc(state_D);
3609 double q=(Sc(state_q_over_pt)>0)?1.:-1.;
3610 double qpt=1./Sc(state_q_over_pt);
3611 double sinphi=sin(Sc(state_phi));
3612 double cosphi=cos(Sc(state_phi));
3613 //double qrc_old=qpt/fabs(qBr2p*bfield->GetBz(pos.x(),pos.y(),pos.z()));
3614 double qrc_old=qpt/fabs(qBr2p0.003*central_traj[k].B);
3615 double qrc_plus_D=D+qrc_old;
3616 double lambda=atan(Sc(state_tanl));
3617 double cosl=cos(lambda);
3618 double sinl=sin(lambda);
3619
3620 // wire direction variables
3621 double ux=dir.x();
3622 double uy=dir.y();
3623 // Variables relating wire direction and track direction
3624 double sinl_over_uz=sinl/uz;
3625 double my_ux=ux*sinl_over_uz-cosl*cosphi;
3626 double my_uy=uy*sinl_over_uz-cosl*sinphi;
3627 double denom=my_ux*my_ux+my_uy*my_uy;
3628
3629 // if the step size is small relative to the radius of curvature,
3630 // use a linear approximation to find ds2
3631 bool do_brent=false;
3632 double step1=mStepSizeS;
3633 double step2=mStepSizeS;
3634 if (k>=2){
3635 step1=-central_traj[k].s+central_traj[k_minus_1].s;
3636 step2=-central_traj[k_minus_1].s+central_traj[k-2].s;
3637 }
3638 //printf("step1 %f step 2 %f \n",step1,step2);
3639 double two_step=step1+step2;
3640 if (two_step*cosl/fabs(qrc_old)<0.01 && denom>EPS3.0e-8){
3641 double dzw=(pos.z()-z0w)/uz;
3642 ds2=((pos.x()-origin.x()-ux*dzw)*my_ux
3643 +(pos.y()-origin.y()-uy*dzw)*my_uy)/denom;
3644
3645 if (ds2<0){
3646 do_brent=true;
3647 }
3648 else{
3649 //if (fabs(ds2)<2.*mStepSizeS){
3650
3651 if (fabs(ds2)<two_step){
3652 double my_z=pos.z()+ds2*sinl;
3653 if(my_z<cdc_origin[2]){
3654 ds2=(cdc_origin[2]-pos.z())/sinl;
3655 }
3656 else if (my_z>endplate_z){
3657 ds2=(endplate_z-pos.z())/sinl;
3658 }
3659 FixedStep(pos,ds2,Sc,dedx);
3660 }
3661 else do_brent=true;
3662 }
3663 }
3664 else do_brent=true;
3665 if (do_brent){
3666 // ... otherwise, use Brent's algorithm.
3667 // See Numerical Recipes in C, pp 404-405
3668 //ds2=BrentsAlgorithm(-step1,-step2,dedx,pos,origin,dir,Sc);
3669 ds2=BrentsAlgorithm(-mStepSizeS,-mStepSizeS,dedx,pos,origin,dir,Sc);
3670 // printf("ds2 %f\n",ds2);
3671
3672 }
3673 //Step along the reference trajectory and compute the new covariance matrix
3674 StepStateAndCovariance(pos0,ds2,dedx,S0,J,Cc);
3675
3676 // Compute the value of D (signed distance to the reference trajectory)
3677 // at the doca to the wire
3678 DVector3 dpos1=pos0-central_traj[k].pos;
3679 double rc=sqrt(dpos1.Perp2()
3680 +2.*qrc_plus_D*(dpos1.x()*sinphi-dpos1.y()*cosphi)
3681 +qrc_plus_D*qrc_plus_D);
3682 Sc(state_D)=q*rc-qrc_old;
3683
3684 // wire position
3685 wirepos=origin+((pos.z()-z0w)/uz)*dir;
3686
3687 // prediction for measurement
3688 DVector3 diff=pos-wirepos;
3689 doca=diff.Perp();
3690 double cosstereo=cos(my_cdchits[cdc_index]->hit->wire->stereo);
3691 double prediction=doca*cosstereo;
3692
3693 // Measurement
3694 double measurement=0.,tdrift=0.;
3695 if (fit_type==kTimeBased){
3696 tdrift=my_cdchits[cdc_index]->hit->tdrift-mT0
3697 -central_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
3698 double B=central_traj[k].B;
3699 measurement=cdc_drift_distance(tdrift,B);
3700 // _DBG_ << "CDChit: t2d >> " << measurement << " tdrift="<<tdrift<<" with B="<<B<<endl;
3701
3702 // Measurement error
3703 V=cdc_variance(B,Sc(state_tanl),tdrift);
3704 double temp=1./(1131.+2.*140.7*measurement);
3705 V+=mVarT0*temp*temp;
3706
3707 }
3708 //compute t0 estimate
3709 else if (cdc_index==mMinDriftID-1000){
3710 mT0MinimumDriftTime=mMinDriftTime
3711 -central_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
3712 }
3713
3714 // Projection matrix
3715 sinphi=sin(Sc(state_phi));
3716 cosphi=cos(Sc(state_phi));
3717 double dx=diff.x();
3718 double dy=diff.y();
3719 double cosstereo_over_doca=cosstereo/doca;
3720 H(state_D)=H_T(state_D)=(dy*cosphi-dx*sinphi)*cosstereo_over_doca;
3721 H(state_phi)=H_T(state_phi)
3722 =-Sc(state_D)*cosstereo_over_doca*(dx*cosphi+dy*sinphi);
3723 H(state_z)=H_T(state_z)=-cosstereo_over_doca*(dx*ux+dy*uy)/uz;
3724
3725 // Difference and inverse of variance
3726 //InvV=1./(V+H*(Cc*H_T));
3727 InvV=1./(V+Cc.SandwichMultiply(H_T));
3728 double dm=measurement-prediction;
3729
3730 if (InvV<0.){
3731 if (DEBUG_LEVEL>1)
3732 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3732<<" " << k <<" "<< central_traj.size()-1<<" Negative variance??? " << V << " " << H*(Cc*H_T) <<endl;
3733
3734 return NEGATIVE_VARIANCE;
3735 }
3736 /*
3737 if (fabs(cosstereo)<1.){
3738 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);
3739 }
3740 */
3741
3742 // Check how far this hit is from the expected position
3743 double chi2check=dm*dm*InvV;
3744 if (chi2check<chi2cut){
3745 // Compute Kalman gain matrix
3746 K=InvV*(Cc*H_T);
3747
3748 // Update state vector covariance matrix
3749 //Cc=Cc-(K*(H*Cc));
3750 Ctest=Cc.SubSym(K*(H*Cc));
3751 // Joseph form
3752 // C = (I-KH)C(I-KH)^T + KVK^T
3753 //Ctest=Cc.SandwichMultiply(I5x5-K*H)+V*MultiplyTranspose(K);
3754 // Check that Ctest is positive definite
3755 if (Ctest(0,0)>0 && Ctest(1,1)>0 && Ctest(2,2)>0 && Ctest(3,3)>0
3756 && Ctest(4,4)>0){
3757 Cc=Ctest;
3758
3759 // Mark point on ref trajectory with a hit id for the straw
3760 central_traj[k_minus_1].h_id=cdc_index+1;
3761
3762 // Update the state vector
3763 //dS=dm*K;
3764 //dS.Zero();
3765 //Sc=Sc+dm*K;
3766 Sc+=dm*K;
3767
3768 // Store the "improved" values for the state vector and covariance
3769 double scale=1.-H*K;
3770 cdc_updates[cdc_index].S=Sc;
3771 cdc_updates[cdc_index].C=Cc;
3772 cdc_updates[cdc_index].tflight
3773 =central_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
3774 cdc_updates[cdc_index].tdrift=tdrift;
3775 cdc_updates[cdc_index].pos.SetXYZ(pos0.X()-Sc(state_D)*sin(Sc(state_phi)),
3776 pos0.Y()+Sc(state_D)*cos(Sc(state_phi)),
3777 Sc(state_z));
3778 cdc_updates[cdc_index].B=central_traj[k_minus_1].B;
3779 cdc_updates[cdc_index].s=central_traj[k_minus_1].s;
3780 cdc_updates[cdc_index].residual=dm*scale;
3781 cdc_updates[cdc_index].variance=V*scale;
3782 cdc_updates[cdc_index].used_in_fit=true;
3783
3784 // Update chi2 for this hit
3785 chisq+=scale*dm*dm/V;
3786 my_ndf++;
3787
3788 if (DEBUG_LEVEL>2)
3789 cout
3790 << "ring " << my_cdchits[cdc_index]->hit->wire->ring
3791 << " t " << my_cdchits[cdc_index]->hit->tdrift
3792 << " Dm-Dpred " << dm
3793 << " chi2 " << (1.-H*K)*dm*dm/V
3794 << endl;
3795
3796 break_point_cdc_index=cdc_index;
3797 break_point_step_index=k_minus_1;
3798 }
3799 //else printf("Negative variance!!!\n");
3800
3801
3802 }
3803
3804 // propagate the covariance matrix to the next point on the trajectory
3805 // Compute the Jacobian matrix
3806 StepJacobian(pos0,-ds2,S0,dedx,J);
3807
3808 // Update covariance matrix
3809 //Cc=J*Cc*J.Transpose();
3810 Cc=Cc.SandwichMultiply(J);
3811
3812 // Step to the next point on the trajectory
3813 Sc=S0_+J*(Sc-S0);
3814
3815 // update position on current trajectory based on corrected doca to
3816 // reference trajectory
3817 pos.SetXYZ(central_traj[k].pos.x()-Sc(state_D)*sin(Sc(state_phi)),
3818 central_traj[k].pos.y()+Sc(state_D)*cos(Sc(state_phi)),
3819 Sc(state_z));
3820
3821 }
3822 else {
3823 if (cdc_index>0) cdc_index--;
3824 else cdc_index=0;
3825 }
3826
3827
3828
3829
3830 // new wire origin and direction
3831 if (cdc_index>0){
3832 cdc_index--;
3833 origin=my_cdchits[cdc_index]->hit->wire->origin;
3834 dir=my_cdchits[cdc_index]->hit->wire->udir;
3835 }
3836 else{
3837 origin.SetXYZ(0.,0.,65.);
3838 dir.SetXYZ(0,0,1.);
3839 more_measurements=false;
3840 }
3841
3842 // Update the wire position
3843 z0w=origin.z();
3844 uz=dir.z();
3845 wirepos=origin+((pos.z()-z0w)/uz)*dir;
3846
3847 //s+=ds2;
3848 // new doca
3849 doca=(pos-wirepos).Perp();
3850 }
3851
3852 old_doca=doca;
3853 }
3854
3855 // If there are not enough degrees of freedom, something went wrong...
3856 if (my_ndf<6){
3857 chisq=MAX_CHI21e16;
3858 my_ndf=0;
3859
3860 return INVALID_FIT;
3861 }
3862 else my_ndf-=5;
3863
3864 // Check if we have a kink in the track or threw away too many cdc hits
3865 if (cdc_updates.size()>=MIN_HITS_FOR_REFIT8){
3866 unsigned int num_good=0;
3867 for (unsigned int j=0;j<cdc_updates.size();j++){
3868 if (cdc_updates[j].used_in_fit) num_good++;
3869 }
3870 if (break_point_cdc_index>0) return BREAK_POINT_FOUND;
3871 if (double(num_good)/double(my_cdchits.size())<MINIMUM_HIT_FRACTION0.25) return PRUNED_TOO_MANY_HITS;
3872 }
3873
3874 return FIT_SUCCEEDED;
3875}
3876
3877// Kalman engine for forward tracks
3878kalman_error_t DTrackFitterKalmanSIMD::KalmanForward(double anneal_factor,
3879 DMatrix5x1 &S,
3880 DMatrix5x5 &C,
3881 double &chisq,
3882 unsigned int &numdof){
3883 DMatrix2x1 Mdiff; // difference between measurement and prediction
3884 DMatrix2x5 H; // Track projection matrix
3885 DMatrix5x2 H_T; // Transpose of track projection matrix
3886 DMatrix1x5 Hc; // Track projection matrix for cdc hits
3887 DMatrix5x1 Hc_T; // Transpose of track projection matrix for cdc hits
3888 DMatrix5x5 J; // State vector Jacobian matrix
3889 //DMatrix5x5 J_T; // transpose of this matrix
3890 DMatrix5x5 Q; // Process noise covariance matrix
3891 DMatrix5x2 K; // Kalman gain matrix
3892 DMatrix5x1 Kc; // Kalman gain matrix for cdc hits
3893 DMatrix2x2 V(0.0833,0.,0.,FDC_CATHODE_VARIANCE0.000225); // Measurement covariance matrix
3894 DMatrix2x1 R; // Filtered residual
3895 DMatrix2x2 RC; // Covariance of filtered residual
3896 DMatrix5x1 S0,S0_; //State vector
3897 //DMatrix5x1 dS; // perturbation in state vector
3898 DMatrix2x2 InvV; // Inverse of error matrix
3899
3900 // Save the starting values for C and S in the deque
3901 forward_traj[0].Skk=S;
3902 forward_traj[0].Ckk=C;
3903
3904 // Initialize chi squared
3905 chisq=0;
3906
3907 // Initialize number of degrees of freedom
3908 numdof=0;
3909 // Variables for estimating t0 from tracking
3910 //mInvVarT0=mT0MinimumDriftTime=0.;
3911
3912 unsigned int num_fdc_hits=my_fdchits.size();
3913 unsigned int num_cdc_hits=my_cdchits.size();
3914 unsigned int cdc_index=0;
3915 if (num_cdc_hits>0) cdc_index=num_cdc_hits-1;
3916 double old_doca=1000.;
3917
3918 S0_=(forward_traj[0].S);
3919 for (unsigned int k=1;k<forward_traj.size();k++){
3920 unsigned int k_minus_1=k-1;
3921
3922 // Check that C matrix is positive definite
3923 if (C(0,0)<0 || C(1,1)<0 || C(2,2)<0 || C(3,3)<0 || C(4,4)<0){
3924 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3924<<" " << "Broken covariance matrix!" <<endl;
3925 return BROKEN_COVARIANCE_MATRIX;
3926 }
3927
3928 // Get the state vector, jacobian matrix, and multiple scattering matrix
3929 // from reference trajectory
3930 S0=(forward_traj[k].S);
3931 J=(forward_traj[k].J);
3932 //J_T=(forward_traj[k].JT);
3933 Q=(forward_traj[k].Q);
3934
3935 // State S is perturbation about a seed S0
3936 //dS=S-S0_;
3937
3938 // Update the actual state vector and covariance matrix
3939 S=S0+J*(S-S0_);
3940
3941 // Bail if the position is grossly outside of the tracking volume
3942 /*
3943 if (sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y))>R_MAX_FORWARD){
3944 if (DEBUG_LEVEL>2)
3945 {
3946 _DBG_<< "Went outside of tracking volume at z="<<forward_traj[k].pos.z()<<endl;
3947 }
3948 return POSITION_OUT_OF_RANGE;
3949 }
3950 */
3951 // Bail if the momentum has dropped below some minimum
3952 if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX100.){
3953 if (DEBUG_LEVEL>2)
3954 {
3955 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
3955<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl;
3956 }
3957 return MOMENTUM_OUT_OF_RANGE;
3958 }
3959
3960
3961
3962 //C=J*(C*J_T)+Q;
3963 //C=Q.AddSym(J*C*J_T);
3964 C=Q.AddSym(C.SandwichMultiply(J));
3965
3966 // Save the current state and covariance matrix in the deque
3967 forward_traj[k].Skk=S;
3968 forward_traj[k].Ckk=C;
3969
3970 // Save the current state of the reference trajectory
3971 S0_=S0;
3972
3973 // Add the hit
3974 if (num_fdc_hits>0){
3975 if (forward_traj[k].h_id>0 && forward_traj[k].h_id<1000){
3976 unsigned int id=forward_traj[k].h_id-1;
3977
3978 double cosa=my_fdchits[id]->cosa;
3979 double sina=my_fdchits[id]->sina;
3980 double u=my_fdchits[id]->uwire;
3981 double v=my_fdchits[id]->vstrip;
3982 double x=S(state_x);
3983 double y=S(state_y);
3984 double tx=S(state_tx);
3985 double ty=S(state_ty);
3986 double du=x*cosa-y*sina-u;
3987 double tu=tx*cosa-ty*sina;
3988 double one_plus_tu2=1.+tu*tu;
3989 double alpha=atan(tu);
3990 double cosalpha=cos(alpha);
3991 double sinalpha=sin(alpha);
3992 // (signed) distance of closest approach to wire
3993 double doca=du*cosalpha;
3994 // Correction for lorentz effect
3995 double nz=my_fdchits[id]->nz;
3996 double nr=my_fdchits[id]->nr;
3997 double nz_sinalpha_plus_nr_cosalpha=nz*sinalpha+nr*cosalpha;
3998
3999 // Variance in coordinate along wire
4000 double tanl=1./sqrt(tx*tx+ty*ty);
4001 V(1,1)=anneal_factor*fdc_y_variance(tanl,doca,my_fdchits[id]->dE);
4002
4003 // Difference between measurement and projection
4004 Mdiff(1)=v-(y*cosa+x*sina+doca*nz_sinalpha_plus_nr_cosalpha);
4005 if (fit_type==kWireBased){
4006 Mdiff(0)=-doca;
4007 }
4008 else{
4009 // Compute drift distance
4010 double drift_time=my_fdchits[id]->t-mT0
4011 -forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4012 double drift=(du>0?1.:-1.)*fdc_drift_distance(drift_time,forward_traj[k].B);
4013
4014 Mdiff(0)=drift-doca;
4015
4016 // Variance in drift distance
4017 V(0,0)=anneal_factor*fdc_drift_variance(drift_time);
4018 }
4019
4020 //compute t0 estimate
4021 if (fit_type==kWireBased && id==mMinDriftID-1){
4022 mT0MinimumDriftTime=mMinDriftTime
4023 -forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4024 }
4025
4026 // To transform from (x,y) to (u,v), need to do a rotation:
4027 // u = x*cosa-y*sina
4028 // v = y*cosa+x*sina
4029 H(0,state_x)=cosa*cosalpha;
4030 H_T(state_x,0)=H(0,state_x);
4031 H(1,state_x)=sina;
4032 H_T(state_x,1)=H(1,state_x);
4033 H(0,state_y)=-sina*cosalpha;
4034 H_T(state_y,0)=H(0,state_y);
4035 H(1,state_y)=cosa;
4036 H_T(state_y,1)=H(1,state_y);
4037 double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2;
4038 H(0,state_ty)=sina*factor;
4039 H_T(state_ty,0)=H(0,state_y);
4040 H(0,state_tx)=-cosa*factor;
4041 H_T(state_tx,0)=H(0,state_tx);
4042
4043 // Terms that depend on the correction for the Lorentz effect
4044 H(1,state_x)=sina+cosa*cosalpha*nz_sinalpha_plus_nr_cosalpha;
4045 H_T(state_x,1)=H(1,state_x);
4046 H(1,state_y)=cosa-sina*cosalpha*nz_sinalpha_plus_nr_cosalpha;
4047 H_T(state_y,1)=H(1,state_y);
4048 double temp=(du/one_plus_tu2)*(nz*(cosalpha*cosalpha-sinalpha*sinalpha)
4049 -2.*nr*cosalpha*sinalpha);
4050 H(1,state_tx)=cosa*temp;
4051 H_T(state_tx,1)=H(1,state_tx);
4052 H(1,state_ty)=-sina*temp;
4053 H_T(state_y,1)=H(1,state_ty);
4054
4055 // Check to see if we have multiple hits in the same plane
4056 if (forward_traj[k].num_hits>1){
4057 // If we do have multiple hits, then all of the hits within some
4058 // validation region are included with weights determined by how
4059 // close the hits are to the track projection of the state to the
4060 // "hit space".
4061 vector<DMatrix5x2> Klist;
4062 vector<DMatrix2x1> Mlist;
4063 vector<DMatrix2x5> Hlist;
4064 vector<DMatrix2x2> Vlist;
4065 vector<double>probs;
4066 DMatrix2x2 Vtemp;
4067
4068 // Deal with the first hit:
4069 Vtemp=V+H*C*H_T;
4070 InvV=Vtemp.Invert();
4071
4072 //probability
4073 double chi2_hit=Vtemp.Chi2(Mdiff);
4074 double prob_hit=exp(-0.5*chi2_hit)
4075 /(M_TWO_PI6.28318530717958647692*sqrt(Vtemp.Determinant()));
4076
4077 // Cut out outliers
4078 if (sqrt(chi2_hit)<NUM_FDC_SIGMA_CUT){
4079 probs.push_back(prob_hit);
4080 Vlist.push_back(V);
4081 Hlist.push_back(H);
4082 Mlist.push_back(Mdiff);
4083 Klist.push_back(C*H_T*InvV); // Kalman gain
4084 }
4085
4086 // loop over the remaining hits
4087 for (unsigned int m=1;m<forward_traj[k].num_hits;m++){
4088 unsigned int my_id=id-m;
4089 u=my_fdchits[my_id]->uwire;
4090 v=my_fdchits[my_id]->vstrip;
4091 double du=x*cosa-y*sina-u;
4092 doca=du*cosalpha;
4093
4094 // variance for coordinate along the wire
4095 V(1,1)=anneal_factor*fdc_y_variance(tanl,doca,my_fdchits[my_id]->dE);
4096
4097 // Difference between measurement and projection
4098 Mdiff(1)=v-(y*cosa+x*sina+doca*nz_sinalpha_plus_nr_cosalpha);
4099 if (fit_type==kWireBased){
4100 Mdiff(0)=-doca;
4101 }
4102 else{
4103 // Compute drift distance
4104 double drift_time=my_fdchits[id]->t-mT0
4105 -forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4106 //double drift=DRIFT_SPEED*drift_time*(du>0?1.:-1.);
4107 double drift=(du>0?1.:-1.)*fdc_drift_distance(drift_time,forward_traj[k].B);
4108
4109 Mdiff(0)=drift-doca;
4110
4111 // Variance in drift distance
4112 V(0,0)=anneal_factor*fdc_drift_variance(drift_time);
4113 }
4114
4115 //compute t0 estimate
4116 if (fit_type==kWireBased && my_id==mMinDriftID-1){
4117 mT0MinimumDriftTime=mMinDriftTime
4118 -forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4119 }
4120
4121 // Update the terms in H/H_T that depend on the particular hit
4122 factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2;
4123 H_T(state_ty,0)=sina*factor;
4124 H(0,state_ty)=H_T(state_ty,0);
4125 H_T(state_tx,0)=-cosa*factor;
4126 H(0,state_tx)=H_T(state_tx,0);
4127 temp=(du/one_plus_tu2)*(nz*(cosalpha*cosalpha-sinalpha*sinalpha)
4128 -2.*nr*cosalpha*sinalpha);
4129 H_T(state_tx,1)=cosa*temp;
4130 H(1,state_tx)=H_T(state_tx,1);
4131 H_T(state_ty,1)=-sina*temp;
4132 H(1,state_ty)=H_T(state_ty,1);
4133
4134 // Calculate the kalman gain for this hit
4135 Vtemp=V+H*C*H_T;
4136 InvV=Vtemp.Invert();
4137
4138 // probability
4139 chi2_hit=Vtemp.Chi2(Mdiff);
4140 prob_hit=exp(-0.5*chi2_hit)/(M_TWO_PI6.28318530717958647692*sqrt(Vtemp.Determinant()));
4141
4142 // Cut out outliers
4143 if(sqrt(chi2_hit)<NUM_FDC_SIGMA_CUT){
4144 probs.push_back(prob_hit);
4145 Mlist.push_back(Mdiff);
4146 Vlist.push_back(V);
4147 Hlist.push_back(H);
4148 Klist.push_back(C*H_T*InvV);
4149 }
4150 }
4151 double prob_tot=1e-100;
4152 for (unsigned int m=0;m<probs.size();m++){
4153 prob_tot+=probs[m];
4154 }
4155
4156 // Adjust the state vector and the covariance using the hit
4157 //information
4158 DMatrix5x5 sum=I5x5;
4159 DMatrix5x5 sum2;
4160 for (unsigned int m=0;m<Klist.size();m++){
4161 double my_prob=probs[m]/prob_tot;
4162 S+=my_prob*(Klist[m]*Mlist[m]);
4163 sum+=my_prob*(Klist[m]*Hlist[m]);
4164 sum2+=(my_prob*my_prob)*(Klist[m]*Vlist[m]*Transpose(Klist[m]));
4165 }
4166 C=C.SandwichMultiply(sum)+sum2;
4167
4168 if (fit_type==kTimeBased){
4169 for (unsigned int m=0;m<forward_traj[k].num_hits;m++){
4170 unsigned int my_id=id-m;
4171 if (fdc_updates[my_id].used_in_fit){
4172 fdc_updates[my_id].S=S;
4173 fdc_updates[my_id].C=C;
4174 fdc_updates[my_id].tflight
4175 =forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4176 fdc_updates[my_id].pos=forward_traj[k].pos;
4177 fdc_updates[my_id].B=forward_traj[k].B;
4178 fdc_updates[my_id].s=forward_traj[k].s;
4179 }
4180 }
4181 }
4182
4183 // update number of degrees of freedom
4184 numdof+=2;
4185
4186 }
4187 else{
4188 // Variance for this hit
4189 DMatrix2x2 Vtemp=V+H*C*H_T;
4190 InvV=Vtemp.Invert();
4191
4192 // Check if this hit is an outlier
4193 double chi2_hit=Vtemp.Chi2(Mdiff);
4194 /*
4195 if(fit_type==kTimeBased && sqrt(chi2_hit)>NUM_FDC_SIGMA_CUT){
4196 printf("outlier %d du %f dv %f sigu %f sigv %f sqrt(chi2) %f z %f \n",
4197 id, Mdiff(0),Mdiff(1),sqrt(Vtemp(0,0)),sqrt(V(1,1)),
4198 sqrt(chi2_hit),forward_traj[k].pos.z());
4199 }
4200 */
4201 if (sqrt(chi2_hit)<NUM_FDC_SIGMA_CUT)
4202 {
4203 // Compute Kalman gain matrix
4204 K=C*H_T*InvV;
4205
4206 // Update the state vector
4207 S+=K*Mdiff;
4208
4209 // Update state vector covariance matrix
4210 C=C.SubSym(K*(H*C));
4211 // Joseph form
4212 // C = (I-KH)C(I-KH)^T + KVK^T
4213 //DMatrix2x5 KT=Transpose(K);
4214 //C=C.SandwichMultiply(I5x5-K*H)+K*V*KT;
4215
4216 //C=C.SubSym(K*(H*C));
4217
4218 //C.Print();
4219
4220 if (fit_type==kTimeBased){
4221 fdc_updates[id].S=S;
4222 fdc_updates[id].C=C;
4223 fdc_updates[id].tflight
4224 =forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4225 fdc_updates[id].pos=forward_traj[k].pos;
4226 fdc_updates[id].B=forward_traj[k].B;
4227 fdc_updates[id].s=forward_traj[k].s;
4228 }
4229 fdc_updates[id].used_in_fit=true;
4230
4231 // Filtered residual and covariance of filtered residual
4232 R=Mdiff-H*K*Mdiff;
4233 RC=V-H*(C*H_T);
4234
4235 // Update chi2 for this segment
4236 chisq+=RC.Chi2(R);
4237
4238 // update number of degrees of freedom
4239 numdof+=2;
4240 }
4241 }
4242 if (num_fdc_hits>=forward_traj[k].num_hits)
4243 num_fdc_hits-=forward_traj[k].num_hits;
4244 }
4245 }
4246 else if (num_cdc_hits>0){
4247 DVector3 origin=my_cdchits[cdc_index]->hit->wire->origin;
4248 double z0w=origin.z();
4249 DVector3 dir=my_cdchits[cdc_index]->hit->wire->udir;
4250 double uz=dir.z();
4251 double z=forward_traj[k].pos.z();
4252 DVector3 wirepos=origin+((z-z0w)/uz)*dir;
4253
4254 // doca variables
4255 double dx=S(state_x)-wirepos.x();
4256 double dy=S(state_y)-wirepos.y();
4257 double doca=sqrt(dx*dx+dy*dy);
4258
4259 // Check if the doca is no longer decreasing
4260 if (doca>old_doca){
4261 if(true /*my_cdchits[cdc_index]->status==0*/){
4262 // Get energy loss
4263 double dedx=0.;
4264 if (CORRECT_FOR_ELOSS){
4265 dedx=GetdEdx(S(state_q_over_p),
4266 forward_traj[k].K_rho_Z_over_A,
4267 forward_traj[k].rho_Z_over_A,
4268 forward_traj[k].LnI);
4269 }
4270 double tx=S(state_tx);
4271 double ty=S(state_ty);
4272 double tanl=1./sqrt(tx*tx+ty*ty);
4273 double sinl=sin(atan(tanl));
4274
4275 // Wire direction variables
4276 double ux=dir.x();
4277 double uy=dir.y();
4278 // Variables relating wire direction and track direction
4279 double my_ux=tx-ux/uz;
4280 double my_uy=ty-uy/uz;
4281 double denom=my_ux*my_ux+my_uy*my_uy;
4282 double dz=0.;
4283
4284 // if the path length increment is small relative to the radius
4285 // of curvature, use a linear approximation to find dz
4286 bool do_brent=false;
4287 double step1=mStepSizeZ;
4288 double step2=mStepSizeZ;
4289 if (k>=2){
4290 step1=-forward_traj[k].pos.z()+forward_traj[k_minus_1].pos.z();
4291 step2=-forward_traj[k_minus_1].pos.z()+forward_traj[k-2].pos.z();
4292 }
4293 //printf("step1 %f step 2 %f \n",step1,step2);
4294 double two_step=step1+step2;
4295 if (fabs(qBr2p0.003*S(state_q_over_p)
4296 //*bfield->GetBz(S(state_x),S(state_y),z)
4297 *forward_traj[k].B
4298 *two_step/sinl)<0.01
4299 && denom>EPS3.0e-8){
4300 double dzw=(z-z0w)/uz;
4301 dz=-((S(state_x)-origin.x()-ux*dzw)*my_ux
4302 +(S(state_y)-origin.y()-uy*dzw)*my_uy)
4303 /(my_ux*my_ux+my_uy*my_uy);
4304
4305 if (fabs(dz)>two_step) do_brent=true;
4306 }
4307 else do_brent=true;
4308 if (do_brent){
4309 // We have bracketed the minimum doca: use Brent's agorithm
4310 /*
4311 double step_size
4312 =forward_traj[k].pos.z()-forward_traj[k_minus_1].pos.z();
4313 dz=BrentsAlgorithm(z,step_size,dedx,origin,dir,S);
4314 */
4315 dz=BrentsAlgorithm(z,-0.5*two_step,dedx,origin,dir,S);
4316 }
4317 double newz=z+dz;
4318 // Check for exiting the straw
4319 if (newz>endplate_z){
4320 newz=endplate_z;
4321 dz=endplate_z-z;
4322 }
4323
4324 // Step the state and covariance through the field
4325 int num_steps=0;
4326 double dz3=0.;
4327 double my_dz=0.;
4328 double t=forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4329 if (fabs(dz)>mStepSizeZ){
4330 my_dz=(dz>0?1.0:-1.)*mStepSizeZ;
4331 num_steps=int(fabs(dz/my_dz));
4332 dz3=dz-num_steps*my_dz;
4333
4334 double my_z=z;
4335 for (int m=0;m<num_steps;m++){
4336 newz=my_z+my_dz;
4337
4338 // Step current state by my_dz
4339 double ds=Step(z,newz,dedx,S);
4340
4341 //Adjust time-of-flight
4342 double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
4343 double one_over_beta2=1.+mass2*q_over_p_sq;
4344 if (one_over_beta2>BIG1.0e8) one_over_beta2=BIG1.0e8;
4345 t+=ds*sqrt(one_over_beta2); // in units where c=1
4346
4347 // propagate error matrix to z-position of hit
4348 StepJacobian(z,newz,S0,dedx,J);
4349 //C=J*C*J.Transpose();
4350 C=C.SandwichMultiply(J);
4351
4352 // Step reference trajectory by my_dz
4353 Step(z,newz,dedx,S0);
4354
4355 my_z=newz;
4356 }
4357
4358 newz=my_z+dz3;
4359
4360 // Step current state by dz3
4361 Step(my_z,newz,dedx,S);
4362
4363 // propagate error matrix to z-position of hit
4364 StepJacobian(my_z,newz,S0,dedx,J);
4365 //C=J*C*J.Transpose();
4366 C=C.SandwichMultiply(J);
4367
4368 // Step reference trajectory by dz3
4369 Step(my_z,newz,dedx,S0);
4370 }
4371 else{
4372 // Step current state by dz
4373 Step(z,newz,dedx,S);
4374
4375 // propagate error matrix to z-position of hit
4376 StepJacobian(z,newz,S0,dedx,J);
4377 //C=J*C*J.Transpose();
4378 C=C.SandwichMultiply(J);
4379
4380 // Step reference trajectory by dz
4381 Step(z,newz,dedx,S0);
4382 }
4383
4384 // Wire position at current z
4385 wirepos=origin+((newz-z0w)/uz)*dir;
4386 double xw=wirepos.x();
4387 double yw=wirepos.y();
4388
4389 // predicted doca taking into account the orientation of the wire
4390 dy=S(state_y)-yw;
4391 dx=S(state_x)-xw;
4392 double cosstereo=cos(my_cdchits[cdc_index]->hit->wire->stereo);
4393 double d=sqrt(dx*dx+dy*dy)*cosstereo;
4394
4395 // Track projection
4396 double cosstereo2_over_d=cosstereo*cosstereo/d;
4397 Hc_T(state_x)=dx*cosstereo2_over_d;
4398 Hc(state_x)=Hc_T(state_x);
4399 Hc_T(state_y)=dy*cosstereo2_over_d;
4400 Hc(state_y)=Hc_T(state_y);
4401
4402 //H.Print();
4403
4404 // The next measurement
4405 double dm=0.;
4406 double Vc=0.2133; //1.6*1.6/12.;
4407 //double V=0.05332; // 0.8*0.8/12.;
4408
4409 //V=4.*0.8*0.8; // Testing ideas...
4410
4411 if (fit_type==kTimeBased){
4412 double tdrift=my_cdchits[cdc_index]->hit->tdrift-mT0-t;
4413 double B=forward_traj[k].B;
4414 dm=cdc_drift_distance(tdrift,B);
4415
4416 // variance
4417 double tx=S(state_tx),ty=S(state_ty);
4418 double tanl=1./sqrt(tx*tx+ty*ty);
4419 Vc=cdc_variance(B,tanl,tdrift);
4420
4421 }
4422 //compute t0 estimate
4423 else if (cdc_index==mMinDriftID-1000){
4424 mT0MinimumDriftTime=mMinDriftTime-t;
4425 }
4426
4427 // Residual
4428 double res=dm-d;
4429
4430 // inverse variance including prediction
4431 double InvV1=1./(Vc+Hc*(C*Hc_T));
4432 if (InvV1<0.){
4433 if (DEBUG_LEVEL>0)
4434 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
4434<<" " << "Negative variance???" << endl;
4435 return NEGATIVE_VARIANCE;
4436 }
4437
4438 if (DEBUG_LEVEL>2)
4439 printf("Ring %d straw %d pred %f meas %f V %f %f sig %f t %f %f t0 %f\n",
4440 my_cdchits[cdc_index]->hit->wire->ring,
4441 my_cdchits[cdc_index]->hit->wire->straw,
4442 d,dm,Vc,1./InvV1,1./sqrt(InvV1),
4443 my_cdchits[cdc_index]->hit->tdrift,
4444 forward_traj[k_minus_1].t,
4445 mT0
4446 );
4447 // Check if this hit is an outlier
4448 double chi2_hit=res*res*InvV1;
4449 if (sqrt(chi2_hit)<NUM_CDC_SIGMA_CUT){
4450 // Flag place along the reference trajectory with hit id
4451 forward_traj[k_minus_1].h_id=1000+cdc_index;
4452
4453 // Compute KalmanSIMD gain matrix
4454 Kc=InvV1*(C*Hc_T);
4455
4456 // Update the state vector
4457 S+=res*Kc;
4458
4459 // Update state vector covariance matrix
4460 C=C.SubSym(K*(H*C));
4461 // Joseph form
4462 // C = (I-KH)C(I-KH)^T + KVK^T
4463 //C=C.SandwichMultiply(I5x5-Kc*Hc)+Vc*MultiplyTranspose(Kc);
4464
4465 // Store the "improved" values of the state and covariance matrix
4466 if (fit_type==kTimeBased){
4467 cdc_updates[cdc_index].S=S;
4468 cdc_updates[cdc_index].C=C;
4469 cdc_updates[cdc_index].tflight
4470 =forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4471 cdc_updates[cdc_index].pos=forward_traj[k_minus_1].pos;
4472 cdc_updates[cdc_index].B=forward_traj[k_minus_1].B;
4473 cdc_updates[cdc_index].s=forward_traj[k_minus_1].s;
4474 }
4475 cdc_updates[cdc_index].used_in_fit=true;
4476
4477 // Residual
4478 res*=1.-Hc*Kc;
4479
4480 // Update chi2 for this segment
4481 double err2 = Vc-Hc*(C*Hc_T)+1e-100;
4482 chisq+=anneal_factor*res*res/err2;
4483
4484 // update number of degrees of freedom
4485 numdof++;
4486
4487 }
4488
4489 if (num_steps==0){
4490 // Step C back to the z-position on the reference trajectory
4491 StepJacobian(newz,z,S0,dedx,J);
4492 //C=J*C*J.Transpose();
4493 C=C.SandwichMultiply(J);
4494
4495 // Step S to current position on the reference trajectory
4496 Step(newz,z,dedx,S);
4497 }
4498 else{
4499 double my_z=newz;
4500 for (int m=0;m<num_steps;m++){
4501 newz=my_z-my_dz;
4502
4503 // Step C along z
4504 StepJacobian(my_z,newz,S0,dedx,J);
4505 //C=J*C*J.Transpose();
4506 C=C.SandwichMultiply(J);
4507
4508 // Step S along z
4509 Step(my_z,newz,dedx,S);
4510
4511 // Step S0 along z
4512 Step(my_z,newz,dedx,S0);
4513
4514 my_z=newz;
4515 }
4516
4517 // Step C back to the z-position on the reference trajectory
4518 StepJacobian(my_z,z,S0,dedx,J);
4519 //C=J*C*J.Transpose();
4520 C=C.SandwichMultiply(J);
4521
4522 // Step S to current position on the reference trajectory
4523 Step(my_z,z,dedx,S);
4524 }
4525 }
4526
4527 // new wire origin and direction
4528 if (cdc_index>0){
4529 cdc_index--;
4530 origin=my_cdchits[cdc_index]->hit->wire->origin;
4531 dir=my_cdchits[cdc_index]->hit->wire->udir;
4532 }
4533
4534 // Update the wire position
4535 uz=dir.z();
4536 z0w=origin.z();
4537 wirepos=origin+((z-z0w)/uz)*dir;
4538
4539 // new doca
4540 dx=S(state_x)-wirepos.x();
4541 dy=S(state_y)-wirepos.y();
4542 doca=sqrt(dx*dx+dy*dy);
4543 if (num_cdc_hits>0) num_cdc_hits--;
4544 if (cdc_index==0 && num_cdc_hits>1) num_cdc_hits=0;
4545 }
4546 old_doca=doca;
4547 }
4548
4549 }
4550
4551 // If chisq is still zero after the fit, something went wrong...
4552 if (numdof<6){
4553 numdof=0;
4554 return INVALID_FIT;
4555 }
4556
4557 chisq*=anneal_factor;
4558 numdof-=5;
4559
4560 // Final position for this leg
4561 x_=S(state_x);
4562 y_=S(state_y);
4563 z_=forward_traj[forward_traj.size()-1].pos.Z();
4564
4565 return FIT_SUCCEEDED;
4566}
4567
4568
4569
4570// Kalman engine for forward tracks -- this routine adds CDC hits
4571kalman_error_t DTrackFitterKalmanSIMD::KalmanForwardCDC(double anneal,DMatrix5x1 &S,
4572 DMatrix5x5 &C,double &chisq,
4573 unsigned int &numdof){
4574 DMatrix1x5 H; // Track projection matrix
4575 DMatrix5x1 H_T; // Transpose of track projection matrix
4576 DMatrix5x5 J; // State vector Jacobian matrix
4577 //DMatrix5x5 J_T; // transpose of this matrix
4578 DMatrix5x5 Q; // Process noise covariance matrix
4579 DMatrix5x1 K; // KalmanSIMD gain matrix
4580 DMatrix5x1 S0,S0_,Stest; //State vector
4581 DMatrix5x5 Ctest; // covariance matrix
4582 //DMatrix5x1 dS; // perturbation in state vector
4583 double V=0.2028; // 1.56*1.56/12.;
4584 double InvV; // inverse of variance
4585 double rmax=R_MAX65.0;
4586
4587 // set used_in_fit flags to false for cdc hits
4588 for (unsigned int i=0;i<cdc_updates.size();i++) cdc_updates[i].used_in_fit=false;
4589
4590 // initialize chi2 info
4591 chisq=0.;
4592 numdof=0;
4593 double chi2cut=anneal*anneal*NUM_CDC_SIGMA_CUT*NUM_CDC_SIGMA_CUT;
4594
4595 // Save the starting values for C and S in the deque
4596 forward_traj[0].Skk=S;
4597 forward_traj[0].Ckk=C;
4598
4599 // z-position
4600 double z=forward_traj[0].pos.z();
4601
4602 // wire information
4603 unsigned int cdc_index=my_cdchits.size()-1;
4604 DVector3 origin=my_cdchits[cdc_index]->hit->wire->origin;
4605 double z0w=origin.z();
4606 DVector3 dir=my_cdchits[cdc_index]->hit->wire->udir;
4607 double uz=dir.z();
4608 DVector3 wirepos=origin+((z-z0w)/uz)*dir;
4609 bool more_measurements=true;
4610
4611 // doca variables
4612 double dx=S(state_x)-wirepos.x();
4613 double dy=S(state_y)-wirepos.y();
4614 double doca=0.,old_doca=sqrt(dx*dx+dy*dy);
4615
4616 // loop over entries in the trajectory
4617 S0_=(forward_traj[0].S);
4618 for (unsigned int k=1;k<forward_traj.size()/*-1*/;k++){
4619 unsigned int k_minus_1=k-1;
4620
4621 // Check that C matrix is positive definite
4622 if (C(0,0)<0 || C(1,1)<0 || C(2,2)<0 || C(3,3)<0 || C(4,4)<0){
4623 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
4623<<" " << "Broken covariance matrix!" <<endl;
4624 return BROKEN_COVARIANCE_MATRIX;
4625 }
4626
4627 z=forward_traj[k].pos.z();
4628
4629 // Get the state vector, jacobian matrix, and multiple scattering matrix
4630 // from reference trajectory
4631 S0=(forward_traj[k].S);
4632 J=(forward_traj[k].J);
4633 //J_T=(forward_traj[k].JT);
4634 Q=(forward_traj[k].Q);
4635
4636 // State S is perturbation about a seed S0
4637 //dS=S-S0_;
4638 /*
4639 dS.Print();
4640 J.Print();
4641 */
4642
4643 // Update the actual state vector and covariance matrix
4644 S=S0+J*(S-S0_);
4645
4646 // Bail if the position is grossly outside of the tracking volume
4647 if (sqrt(S(state_x)*S(state_x)+S(state_y)*S(state_y))>rmax){
4648 if (DEBUG_LEVEL>2)
4649 {
4650 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
4650<<" "<< "Went outside of tracking volume at z="<<z<<endl;
4651 }
4652 return POSITION_OUT_OF_RANGE;
4653 }
4654 // Bail if the momentum has dropped below some minimum
4655 if (fabs(S(state_q_over_p))>=Q_OVER_P_MAX100.){
4656 if (DEBUG_LEVEL>2)
4657 {
4658 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
4658<<" " << "Bailing: P = " << 1./fabs(S(state_q_over_p)) << endl;
4659 }
4660 return MOMENTUM_OUT_OF_RANGE;
4661 }
4662
4663
4664
4665 //C=J*(C*J_T)+Q;
4666 //C=Q.AddSym(J*C*J_T);
4667 C=Q.AddSym(C.SandwichMultiply(J));
4668
4669 // Save the current state of the reference trajectory
4670 S0_=S0;
4671
4672 // new wire position
4673 wirepos=origin+((z-z0w)/uz)*dir;
4674
4675 // new doca
4676 dx=S(state_x)-wirepos.x();
4677 dy=S(state_y)-wirepos.y();
4678 doca=sqrt(dx*dx+dy*dy);
4679
4680 // Check if the doca is no longer decreasing
4681 if ((doca>old_doca+EPS31.e-2 /*&& z<endplate_z*/)&& more_measurements){
4682 if (true /*my_cdchits[cdc_index]->status==0*/){
4683 // Get energy loss
4684 double dedx=0.;
4685 if (CORRECT_FOR_ELOSS){
4686 dedx=GetdEdx(S(state_q_over_p),
4687 forward_traj[k].K_rho_Z_over_A,
4688 forward_traj[k].rho_Z_over_A,
4689 forward_traj[k].LnI);
4690 }
4691 double tx=S(state_tx);
4692 double ty=S(state_ty);
4693 double tanl=1./sqrt(tx*tx+ty*ty);
4694 double sinl=sin(atan(tanl));
4695
4696 // Wire direction variables
4697 double ux=dir.x();
4698 double uy=dir.y();
4699 // Variables relating wire direction and track direction
4700 double my_ux=tx-ux/uz;
4701 double my_uy=ty-uy/uz;
4702 double denom=my_ux*my_ux+my_uy*my_uy;
4703 double dz=0.;
4704
4705 // if the path length increment is small relative to the radius
4706 // of curvature, use a linear approximation to find dz
4707 bool do_brent=false;
4708 double step1=mStepSizeZ;
4709 double step2=mStepSizeZ;
4710 if (k>=2){
4711 step1=-forward_traj[k].pos.z()+forward_traj[k_minus_1].pos.z();
4712 step2=-forward_traj[k_minus_1].pos.z()+forward_traj[k-2].pos.z();
4713 }
4714 //printf("step1 %f step 2 %f \n",step1,step2);
4715 double two_step=step1+step2;
4716 if (fabs(qBr2p0.003*S(state_q_over_p)
4717 //*bfield->GetBz(S(state_x),S(state_y),z)
4718 *forward_traj[k].B
4719 *two_step/sinl)<0.01
4720 && denom>EPS3.0e-8){
4721 double dzw=(z-z0w)/uz;
4722 dz=-((S(state_x)-origin.x()-ux*dzw)*my_ux
4723 +(S(state_y)-origin.y()-uy*dzw)*my_uy)
4724 /(my_ux*my_ux+my_uy*my_uy);
4725
4726 if (fabs(dz)>two_step || dz<0) do_brent=true;
4727 }
4728 else do_brent=true;
4729 if (do_brent){
4730 // We have bracketed the minimum doca: use Brent's agorithm
4731 /*
4732 double step_size
4733 =forward_traj[k].pos.z()-forward_traj[k_minus_1].pos.z();
4734 dz=BrentsAlgorithm(z,step_size,dedx,origin,dir,S);
4735 */
4736 //dz=BrentsAlgorithm(z,-0.5*two_step,dedx,origin,dir,S);
4737 dz=BrentsAlgorithm(z,-mStepSizeZ,dedx,origin,dir,S);
4738 }
4739 double newz=z+dz;
4740 // Check for exiting the straw
4741 if (newz>endplate_z){
4742 newz=endplate_z;
4743 dz=endplate_z-z;
4744 }
4745
4746 // Step the state and covariance through the field
4747 int num_steps=0;
4748 double dz3=0.;
4749 double my_dz=0.;
4750 if (fabs(dz)>mStepSizeZ){
4751 my_dz=(dz>0?1.0:-1.)*mStepSizeZ;
4752 num_steps=int(fabs(dz/my_dz));
4753 dz3=dz-num_steps*my_dz;
4754
4755 double my_z=z;
4756 for (int m=0;m<num_steps;m++){
4757 newz=my_z+my_dz;
4758
4759 // Step current state by my_dz
4760 Step(z,newz,dedx,S);
4761
4762 // propagate error matrix to z-position of hit
4763 StepJacobian(z,newz,S0,dedx,J);
4764 //C=J*C*J.Transpose();
4765 C=C.SandwichMultiply(J);
4766
4767 // Step reference trajectory by my_dz
4768 Step(z,newz,dedx,S0);
4769
4770 my_z=newz;
4771 }
4772
4773 newz=my_z+dz3;
4774
4775 // Step current state by dz3
4776 Step(my_z,newz,dedx,S);
4777
4778 // propagate error matrix to z-position of hit
4779 StepJacobian(my_z,newz,S0,dedx,J);
4780 //C=J*C*J.Transpose();
4781 C=C.SandwichMultiply(J);
4782
4783 // Step reference trajectory by dz3
4784 Step(my_z,newz,dedx,S0);
4785 }
4786 else{
4787 // Step current state by dz
4788 Step(z,newz,dedx,S);
4789
4790 // propagate error matrix to z-position of hit
4791 StepJacobian(z,newz,S0,dedx,J);
4792 //C=J*C*J.Transpose();
4793 C=C.SandwichMultiply(J);
4794
4795 // Step reference trajectory by dz
4796 Step(z,newz,dedx,S0);
4797 }
4798
4799 // Wire position at current z
4800 wirepos=origin+((newz-z0w)/uz)*dir;
4801 double xw=wirepos.x();
4802 double yw=wirepos.y();
4803
4804 // predicted doca taking into account the orientation of the wire
4805 dy=S(state_y)-yw;
4806 dx=S(state_x)-xw;
4807 double cosstereo=cos(my_cdchits[cdc_index]->hit->wire->stereo);
4808 double d=sqrt(dx*dx+dy*dy)*cosstereo;
4809
4810 //printf("z %f d %f z-1 %f\n",newz,d,forward_traj[k_minus_1].pos.z());
4811
4812 // Track projection
4813 double cosstereo2_over_d=cosstereo*cosstereo/d;
4814 H(state_x)=H_T(state_x)=dx*cosstereo2_over_d;
4815 H(state_y)=H_T(state_y)=dy*cosstereo2_over_d;
4816
4817 //H.Print();
4818
4819 // The next measurement
4820 double dm=0.,tdrift=0.;
4821 if (fit_type==kTimeBased){
4822 tdrift=my_cdchits[cdc_index]->hit->tdrift-mT0
4823 -forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4824 double B=forward_traj[k].B;
4825 dm=cdc_drift_distance(tdrift,B);
4826
4827 double tx=S(state_tx),ty=S(state_ty);
4828 double tanl=1./sqrt(tx*tx+ty*ty);
4829 V=cdc_forward_variance(B,tanl,tdrift);
4830 double temp=1./(1131.+2.*140.7*dm);
4831 V+=mVarT0*temp*temp;
4832
4833 if (DEBUG_HISTS){
4834 cdc_drift_forward->Fill(tdrift,d);
4835 //cdc_res_forward->Fill(tdrift,dm-d);
4836 }
4837
4838 //printf("V %f vart0 %f\n",V,0.0073*0.0073*0.09);
4839 }
4840 //compute t0 estimate
4841 else if (cdc_index==mMinDriftID-1000){
4842 mT0MinimumDriftTime=mMinDriftTime
4843 -forward_traj[k].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4844 }
4845
4846 // residual
4847 double res=dm-d;
4848
4849 // inverse of variance including prediction
4850 //InvV=1./(V+H*(C*H_T));
4851 InvV=1./(V+C.SandwichMultiply(H_T));
4852 if (InvV<0.){
4853 if (DEBUG_LEVEL>0)
4854 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
4854<<" " << "Negative variance???" << endl;
4855 return NEGATIVE_VARIANCE;
4856 }
4857
4858 // Check how far this hit is from the expected position
4859 double chi2check=res*res*InvV;
4860 //if (sqrt(chi2check)>NUM_CDC_SIGMA_CUT) InvV*=0.8;
4861 if (chi2check<chi2cut)
4862 {
4863 // Compute KalmanSIMD gain matrix
4864 K=InvV*(C*H_T);
4865
4866 // Update state vector covariance matrix
4867 Ctest=C.SubSym(K*(H*C));
4868 // Joseph form
4869 // C = (I-KH)C(I-KH)^T + KVK^T
4870 //Ctest=C.SandwichMultiply(I5x5-K*H)+V*MultiplyTranspose(K);
4871 // Check that Ctest is positive definite
4872 if (Ctest(0,0)>0 && Ctest(1,1)>0 && Ctest(2,2)>0 && Ctest(3,3)>0
4873 && Ctest(4,4)>0){
4874 C=Ctest;
4875
4876 // Mark point on ref trajectory with a hit id for the straw
4877 forward_traj[k_minus_1].h_id=cdc_index+1;
4878
4879 // Update the state vector
4880 //S=S+res*K;
4881 S+=res*K;
4882
4883 // Store the "improved" values of the state and covariance matrix
4884 double scale=1.-H*K;
4885 cdc_updates[cdc_index].S=S;
4886 cdc_updates[cdc_index].C=C;
4887 cdc_updates[cdc_index].tflight
4888 =forward_traj[k_minus_1].t*TIME_UNIT_CONVERSION3.33564095198152014e-02;
4889 cdc_updates[cdc_index].pos.SetXYZ(S(state_x),S(state_y),newz);
4890 cdc_updates[cdc_index].tdrift=tdrift;
4891 cdc_updates[cdc_index].B=forward_traj[k_minus_1].B;
4892 cdc_updates[cdc_index].s=forward_traj[k_minus_1].s;
4893 cdc_updates[cdc_index].residual=res*scale;
4894 cdc_updates[cdc_index].variance=V*scale;
4895 cdc_updates[cdc_index].used_in_fit=true;
4896
4897 // Update chi2 for this segment
4898 chisq+=scale*res*res/V;
4899 numdof++;
4900
4901 break_point_cdc_index=cdc_index;
4902 break_point_step_index=k_minus_1;
4903
4904
4905 if (DEBUG_LEVEL>2)
4906 printf("Ring %d straw %d pred %f meas %f chi2 %f\n",
4907 my_cdchits[cdc_index]->hit->wire->ring,
4908 my_cdchits[cdc_index]->hit->wire->straw,
4909 d,dm,(1.-H*K)*res*res/V);
4910
4911 }
4912 }
4913
4914 if (num_steps==0){
4915 // Step C back to the z-position on the reference trajectory
4916 StepJacobian(newz,z,S0,dedx,J);
4917 //C=J*C*J.Transpose();
4918 C=C.SandwichMultiply(J);
4919
4920 // Step S to current position on the reference trajectory
4921 Step(newz,z,dedx,S);
4922 }
4923 else{
4924 double my_z=newz;
4925 for (int m=0;m<num_steps;m++){
4926 z=my_z-my_dz;
4927
4928 // Step C along z
4929 StepJacobian(my_z,z,S0,dedx,J);
4930 //C=J*C*J.Transpose();
4931 C=C.SandwichMultiply(J);
4932
4933 // Step S along z
4934 Step(my_z,z,dedx,S);
4935
4936 // Step S0 along z
4937 Step(my_z,z,dedx,S0);
4938
4939 my_z=z;
4940 }
4941 z=my_z-dz3;
4942
4943 // Step C back to the z-position on the reference trajectory
4944 StepJacobian(my_z,z,S0,dedx,J);
4945 //C=J*C*J.Transpose();
4946 C=C.SandwichMultiply(J);
4947
4948 // Step S to current position on the reference trajectory
4949 Step(my_z,z,dedx,S);
4950 }
4951
4952 }
4953 else {
4954 if (cdc_index>0) cdc_index--;
4955 else cdc_index=0;
4956
4957 }
4958
4959 // new wire origin and direction
4960 if (cdc_index>0){
4961 cdc_index--;
4962 origin=my_cdchits[cdc_index]->hit->wire->origin;
4963 dir=my_cdchits[cdc_index]->hit->wire->udir;
4964 }
4965 else{
4966 origin.SetXYZ(0.,0.,65.);
4967 dir.SetXYZ(0,0,1.);
4968 more_measurements=false;
4969 }
4970
4971 // Update the wire position
4972 uz=dir.z();
4973 z0w=origin.z();
4974 wirepos=origin+((z-z0w)/uz)*dir;
4975
4976 // new doca
4977 dx=S(state_x)-wirepos.x();
4978 dy=S(state_y)-wirepos.y();
4979 doca=sqrt(dx*dx+dy*dy);
4980 }
4981 old_doca=doca;
4982
4983 // Save the current state and covariance matrix in the deque
4984 forward_traj[k].Skk=S;
4985 forward_traj[k].Ckk=C;
4986
4987 }
4988
4989 // Check that there were enough hits to make this a valid fit
4990 if (numdof<6){
4991 chisq=MAX_CHI21e16;
4992 numdof=0;
4993
4994 return INVALID_FIT;
4995 }
4996 numdof-=5;
4997
4998 // Final position for this leg
4999 x_=S(state_x);
5000 y_=S(state_y);
5001 z_=forward_traj[forward_traj.size()-1].pos.Z();
5002
5003 // Check if we have a kink in the track or threw away too many cdc hits
5004 if (cdc_updates.size()>=MIN_HITS_FOR_REFIT8){
5005 unsigned int num_good=0;
5006 for (unsigned int j=0;j<cdc_updates.size();j++){
5007 if (cdc_updates[j].used_in_fit) num_good++;
5008 }
5009 if (break_point_cdc_index>0) return BREAK_POINT_FOUND;
5010 if (double(num_good)/double(my_cdchits.size())<MINIMUM_HIT_FRACTION0.25) return PRUNED_TOO_MANY_HITS;
5011 }
5012
5013 return FIT_SUCCEEDED;
5014}
5015
5016// Extrapolate to the point along z of closest approach to the beam line using
5017// the forward track state vector parameterization. Converts to the central
5018// track representation at the end.
5019jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DMatrix5x1 &S,
5020 DMatrix5x5 &C){
5021 DMatrix5x5 J; // Jacobian matrix
5022 DMatrix5x5 Q; // multiple scattering matrix
5023 DMatrix5x1 S1(S); // copy of S
5024
5025 // position variables
5026 double z=z_,newz=z_;
5027 double dz=-mStepSizeZ;
5028 double r2_old=S(state_x)*S(state_x)+S(state_y)*S(state_y);
5029 double dz_old=0.;
5030 double dEdx=0.;
5031 double sign=1.;
5032
5033 // material properties
5034 double Z=0.,rho_Z_over_A=0.,LnI=0.,K_rho_Z_over_A=0.;
5035 double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.;
5036 DVector3 pos; // current position along trajectory
5037 double xstart=S(state_x),ystart=S(state_y);
5038
5039 // if (fit_type==kTimeBased)printf("------Extrapolating\n");
5040
5041 // printf("-----------\n");
5042 if (fit_type==kTimeBased){
5043 // get material properties from the Root Geometry
5044 pos.SetXYZ(S(state_x),S(state_y),z);
5045 if (geom->FindMatKalman(pos,Z,K_rho_Z_over_A,rho_Z_over_A,LnI,
5046 chi2c_factor,chi2a_factor,chi2a_corr,
5047 last_material_map)
5048 !=NOERROR){
5049 if (DEBUG_LEVEL>1)
5050 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5050<<" " << "Material error in ExtrapolateToVertex at (x,y,z)=("
5051 << pos.X() <<"," << pos.y()<<","<< pos.z()<<")"<< endl;
5052 return UNRECOVERABLE_ERROR;
5053 }
5054
5055 // Adjust the step size
5056 double ds_dz=sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
5057 dz=-mStepSizeS/ds_dz;
5058 if (fabs(dEdx)>EPS3.0e-8){
5059 dz=(-1.)
5060 *(fit_type==kWireBased?DE_PER_STEP_WIRE_BASED0.0005:DE_PER_STEP_TIME_BASED0.0005)
5061 /fabs(dEdx)/ds_dz;
5062 }
5063 if(fabs(dz)>mStepSizeZ) dz=-mStepSizeZ;
5064 if(fabs(dz)<MIN_STEP_SIZE0.1)dz=-MIN_STEP_SIZE0.1;
5065
5066 // Get dEdx for the upcoming step
5067 if (CORRECT_FOR_ELOSS){
5068 dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI);
5069 }
5070
5071 // Check direction of propagation
5072 DMatrix5x1 S2(S); // second copy of S
5073
5074 // Step test states through the field and compute squared radii
5075 Step(z,z-dz,dEdx,S1);
5076 double r2minus=S1(state_x)*S1(state_x)+S1(state_y)*S1(state_y);
5077 Step(z,z+dz,dEdx,S2);
5078 double r2plus=S2(state_x)*S2(state_x)+S2(state_y)*S2(state_y);
5079 // Check to see if we have already bracketed the minimum
5080 if (r2plus>r2_old && r2minus>r2_old){
5081 newz=z+dz;
5082 DVector3 dir(0,0,1);
5083 DVector3 origin(0,0,65);
5084 double dz2=BrentsAlgorithm(newz,dz,dEdx,origin,dir,S2);
5085 z_=newz+dz2;
5086
5087 // Compute the Jacobian matrix
5088 StepJacobian(z,z_,S,dEdx,J);
5089
5090 // Propagate the covariance matrix
5091 //C=Q.AddSym(J*C*J.Transpose());
5092 C=C.SandwichMultiply(J);
5093
5094 // Step to the position of the doca
5095 Step(z,z_,dEdx,S);
5096
5097 // update internal variables
5098 x_=S(state_x);
5099 y_=S(state_y);
5100
5101 return NOERROR;
5102 }
5103
5104 // Find direction to propagate toward minimum doca
5105 if (r2minus<r2_old && r2_old<r2plus){
5106 newz=z-dz;
5107
5108 // Compute the Jacobian matrix
5109 StepJacobian(z,newz,S,dEdx,J);
5110
5111 // Propagate the covariance matrix
5112 //C=Q.AddSym(J*C*J.Transpose());
5113 C=C.SandwichMultiply(J);
5114
5115 S2=S;
5116 S=S1;
5117 S1=S2;
5118 dz*=-1.;
5119 sign=-1.;
5120 dz_old=dz;
5121 r2_old=r2minus;
5122 z=z+dz;
5123 }
5124 if (r2minus>r2_old && r2_old>r2plus){
5125 newz=z+dz;
5126
5127 // Compute the Jacobian matrix
5128 StepJacobian(z,newz,S,dEdx,J);
5129
5130 // Propagate the covariance matrix
5131 //C=Q.AddSym(J*C*J.Transpose());
5132 C=C.SandwichMultiply(J);
5133
5134 S1=S;
5135 S=S2;
5136 dz_old=dz;
5137 r2_old=r2plus;
5138 z=z+dz;
5139 }
5140 }
5141
5142 double r=sqrt(r2_old);
5143 while (z>Z_MIN0. && r<R_MAX_FORWARD65.0 && z<Z_MAX175.0 && r>EPS21.e-4){
5144 // Relationship between arc length and z
5145 double dz_ds=1./sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
5146
5147 // get material properties from the Root Geometry
5148 pos.SetXYZ(S(state_x),S(state_y),z);
5149 double s_to_boundary=1.e6;
5150 if (ENABLE_BOUNDARY_CHECK && fit_type==kTimeBased){
5151 DVector3 mom(S(state_tx),S(state_ty),1.);
5152 if (geom->FindMatKalman(pos,mom,Z,K_rho_Z_over_A,rho_Z_over_A,LnI,
5153 chi2c_factor,chi2a_factor,chi2a_corr,
5154 last_material_map,&s_to_boundary)
5155 !=NOERROR){
5156 return UNRECOVERABLE_ERROR;
5157 }
5158 }
5159 else{
5160 if (geom->FindMatKalman(pos,Z,K_rho_Z_over_A,rho_Z_over_A,LnI,
5161 chi2c_factor,chi2a_factor,chi2a_corr,
5162 last_material_map)
5163 !=NOERROR){
5164 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5164<<" " << "Material error in ExtrapolateToVertex! " << endl;
5165 break;
5166 }
5167 }
5168
5169 // Get dEdx for the upcoming step
5170 if (CORRECT_FOR_ELOSS){
5171 dEdx=GetdEdx(S(state_q_over_p),K_rho_Z_over_A,rho_Z_over_A,LnI);
5172 }
5173
5174 // Adjust the step size
5175 //dz=-sign*mStepSizeS*dz_ds;
5176 double ds=mStepSizeS;
5177 if (fabs(dEdx)>EPS3.0e-8){
5178 /*
5179 dz=(-1.)*sign
5180 *(fit_type==kWireBased?DE_PER_STEP_WIRE_BASED:DE_PER_STEP_TIME_BASED)
5181 /fabs(dEdx)*dz_ds;
5182 */
5183 ds=(fit_type==kWireBased?DE_PER_STEP_WIRE_BASED0.0005:DE_PER_STEP_TIME_BASED0.0005)
5184 /fabs(dEdx);
5185 }
5186 /*
5187 if(fabs(dz)>mStepSizeZ) dz=-sign*mStepSizeZ;
5188 if (fabs(dz)<z_to_boundary) dz=-sign*z_to_boundary;
5189 if(fabs(dz)<MIN_STEP_SIZE)dz=-sign*MIN_STEP_SIZE;
5190 */
5191 if (ds>mStepSizeS) ds=mStepSizeS;
5192 if (ds>s_to_boundary) ds=s_to_boundary;
5193 if (ds<MIN_STEP_SIZE0.1) ds=MIN_STEP_SIZE0.1;
5194 dz=-sign*ds*dz_ds;
5195
5196 // Get the contribution to the covariance matrix due to multiple
5197 // scattering
5198 GetProcessNoise(ds,chi2c_factor,chi2a_factor,chi2a_corr,S,Q);
5199
5200 if (CORRECT_FOR_ELOSS){
5201 double q_over_p_sq=S(state_q_over_p)*S(state_q_over_p);
5202 double one_over_beta2=1.+mass2*q_over_p_sq;
5203 double varE=GetEnergyVariance(ds,one_over_beta2,K_rho_Z_over_A);
5204 Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2;
5205 }
5206
5207
5208 newz=z+dz;
5209 // Compute the Jacobian matrix
5210 StepJacobian(z,newz,S,dEdx,J);
5211
5212 // Propagate the covariance matrix
5213 //C=Q.AddSym(J*C*J.Transpose());
5214 C=Q.AddSym(C.SandwichMultiply(J));
5215
5216 // Step through field
5217 ds=Step(z,newz,dEdx,S);
Value stored to 'ds' is never read
5218
5219 // Check if we passed the minimum doca to the beam line
5220 double r2=S(state_x)*S(state_x)+S(state_y)*S(state_y);
5221 if (r2>r2_old && z<endplate_z){
5222 double two_step=dz+dz_old;
5223
5224 // Find the increment/decrement in z to get to the minimum doca to the
5225 // beam line
5226 DVector3 dir(0,0,1);
5227 DVector3 origin(0,0,65);
5228 dz=BrentsAlgorithm(newz,0.5*two_step,dEdx,origin,dir,S);
5229
5230 // Compute the Jacobian matrix
5231 z_=newz+dz;
5232 StepJacobian(newz,z_,S,dEdx,J);
5233
5234 // Propagate the covariance matrix
5235 //C=J*C*J.Transpose()+(dz/(newz-z))*Q;
5236 //C=((dz/newz-z)*Q).AddSym(C.SandwichMultiply(J));
5237 C=C.SandwichMultiply(J);
5238
5239 // Step to the "z vertex"
5240 ds=Step(newz,z_,dEdx,S);
5241
5242 // update internal variables
5243 x_=S(state_x);
5244 y_=S(state_y);
5245
5246
5247 if (fit_type==kWireBased){
5248 // Make small correction to estimate for start time using helical model
5249 double dx=xstart-x_;
5250 double dy=ystart-y_;
5251 double tanl=1./sqrt(S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
5252 double cosl=cos(atan(tanl));
5253 double one_over_beta=sqrt(1.+mass2*S(state_q_over_p)*S(state_q_over_p));
5254 double tworc=2.*cosl/fabs(S(state_q_over_p)*qBr2p0.003*Bz);
5255 double ratio=sqrt(dx*dx+dy*dy)/tworc;
5256 double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_21.57079632679489661923;
5257 mT0MinimumDriftTime-=sperp*one_over_beta*ONE_OVER_C3.33564095198152014e-02/cosl;
5258 }
5259
5260
5261 return NOERROR;
5262 }
5263 r2_old=r2;
5264 r=sqrt(r2_old);
5265 dz_old=dz;
5266 S1=S;
5267 z=newz;
5268 }
5269 // update internal variables
5270 x_=S(state_x);
5271 y_=S(state_y);
5272 z_=newz;
5273
5274 return NOERROR;
5275}
5276
5277
5278// Propagate track to point of distance of closest approach to origin
5279jerror_t DTrackFitterKalmanSIMD::ExtrapolateToVertex(DVector3 &pos,
5280 DMatrix5x1 &Sc,DMatrix5x5 &Cc){
5281 DMatrix5x5 Jc=I5x5; //.Jacobian matrix
5282 DMatrix5x5 Q; // multiple scattering matrix
5283
5284 // Initialize the beam position = center of target, and the direction
5285 DVector3 origin(0,0,65.);
5286 DVector3 dir(0,0,1.);
5287
5288 // Position and step variables
5289 double r=pos.Perp();
5290 double ds=-mStepSizeS; // step along path in cm
5291 double r_old=r;
5292
5293 // Energy loss
5294 double dedx=0.;
5295
5296 // Check direction of propagation
5297 DMatrix5x1 S0;
5298 S0=Sc;
5299 DVector3 pos0=pos;
5300 DVector3 pos1=pos;
5301 FixedStep(pos0,ds,S0,dedx);
5302 r=pos0.Perp();
5303 if (r>r_old) ds*=-1.;
5304 double ds_old=ds;
5305
5306 // if (fit_type==kTimeBased)printf("------Extrapolating\n");
5307
5308 if (central_traj.size()==0) return UNRECOVERABLE_ERROR;
5309
5310 // Rotate covariance matrix from coordinate system on reference trajectory to coordinate system on track
5311 double B=sqrt(Bx*Bx+By*By+Bz*Bz);
5312 double qrc_old=1./(qBr2p0.003*B*Sc(state_q_over_pt));
5313 double qrc_plus_D=Sc(state_D)+qrc_old;
5314 double q=(qrc_old>0)?1.:-1.;
5315 DVector3 dpos=pos-central_traj[central_traj.size()-1].pos;
5316 double dx=dpos.x();
5317 double dy=dpos.y();
5318 double sinphi=sin(Sc(state_phi));
5319 double cosphi=cos(Sc(state_phi));
5320 double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi;
5321 double rc=sqrt(dpos.Perp2()
5322 +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi)
5323 +qrc_plus_D*qrc_plus_D);
5324
5325 Jc(state_D,state_D)=q*(dx_sinphi_minus_dy_cosphi+qrc_plus_D)/rc;
5326 Jc(state_D,state_q_over_pt)=qrc_old*(Jc(state_D,state_D)-1.)/Sc(state_q_over_pt);
5327 Jc(state_D,state_phi)=q*qrc_plus_D*(dx*cosphi+dy*sinphi)/rc;
5328
5329 Cc=Cc.SandwichMultiply(Jc);
5330
5331 // D is now on the actual trajectory itself
5332 Sc(state_D)=0.;
5333
5334 // Track propagation loop
5335 while (Sc(state_z)>Z_MIN0. && Sc(state_z)<Z_MAX175.0
5336 && r<R_MAX65.0){
5337
5338 // get material properties from the Root Geometry
5339 double rho_Z_over_A=0.,Z=0,LnI=0.,K_rho_Z_over_A=0.;
5340 double chi2c_factor=0.,chi2a_factor=0.,chi2a_corr=0.;
5341 if (geom->FindMatKalman(pos,Z,K_rho_Z_over_A,rho_Z_over_A,LnI,
5342 chi2c_factor,chi2a_factor,chi2a_corr,
5343 last_material_map)
5344 !=NOERROR){
5345 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5345<<" " << "Material error in ExtrapolateToVertex! " << endl;
5346 break;
5347 }
5348
5349 // Get dEdx for the upcoming step
5350 double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
5351 if (CORRECT_FOR_ELOSS){
5352 dedx=GetdEdx(q_over_p,K_rho_Z_over_A,rho_Z_over_A,LnI);
5353 }
5354 // Adjust the step size
5355 double sign=(ds>0)?1.:-1.;
5356 if (fabs(dedx)>EPS3.0e-8){
5357 ds=sign
5358 *(fit_type==kWireBased?DE_PER_STEP_WIRE_BASED0.0005:DE_PER_STEP_TIME_BASED0.0005)
5359 /fabs(dedx);
5360 }
5361 if(fabs(ds)>mStepSizeS) ds=sign*mStepSizeS;
5362 if(fabs(ds)<MIN_STEP_SIZE0.1)ds=sign*MIN_STEP_SIZE0.1;
5363
5364 // Multiple scattering
5365 GetProcessNoiseCentral(ds,chi2c_factor,chi2a_factor,chi2a_corr,Sc,Q);
5366
5367 if (CORRECT_FOR_ELOSS){
5368 double q_over_p_sq=q_over_p*q_over_p;
5369 double one_over_beta2=1.+mass2*q_over_p*q_over_p;
5370 double varE=GetEnergyVariance(ds,one_over_beta2,K_rho_Z_over_A);
5371 Q(state_q_over_p,state_q_over_p)=varE*q_over_p_sq*q_over_p_sq*one_over_beta2;
5372 }
5373
5374 // Propagate the state and covariance through the field
5375 S0=Sc;
5376 DVector3 old_pos=pos;
5377 StepStateAndCovariance(pos,ds,dedx,Sc,Jc,Cc);
5378
5379 // Add contribution due to multiple scattering
5380 Cc=Q.AddSym(Cc);
5381
5382 r=pos.Perp();
5383 //printf("r %f r_old %f \n",r,r_old);
5384 if (r>r_old) {
5385 // We've passed the true minimum; backtrack to find the "vertex"
5386 // position
5387 double cosl=cos(atan(Sc(state_tanl)));
5388 if (fabs((ds+ds_old)*cosl*Sc(state_q_over_pt)*Bz*qBr2p0.003)<0.01){
5389 ds=-(pos.X()*cos(Sc(state_phi))+pos.Y()*sin(Sc(state_phi)))
5390 /cosl;
5391 FixedStep(pos,ds,Sc,dedx);
5392 //printf ("min r %f\n",pos.Perp());
5393 }
5394 else{
5395 ds=BrentsAlgorithm(ds,ds_old,dedx,pos,origin,dir,Sc);
5396 //printf ("Brent min r %f\n",pos.Perp());
5397 }
5398 // Compute the Jacobian matrix
5399 double my_ds=ds-ds_old;
5400 StepJacobian(old_pos,my_ds,S0,dedx,Jc);
5401
5402 // Propagate the covariance matrix
5403 //Cc=Jc*Cc*Jc.Transpose()+(my_ds/ds_old)*Q;
5404 Cc=((my_ds/ds_old)*Q).AddSym(Cc.SandwichMultiply(Jc));
5405
5406 if (fit_type==kWireBased){
5407 // Make small correction to estimate for start time using helical model
5408 double cosl=cos(atan(Sc(state_tanl)));
5409 double one_over_beta=sqrt(1.+mass2*Sc(state_q_over_pt)*Sc(state_q_over_pt)
5410 *cosl*cosl);
5411 double tworc=2./fabs(Sc(state_q_over_pt)*qBr2p0.003*Bz);
5412 double ratio=(pos1-pos).Perp()/tworc;
5413 double sperp=(ratio<1.)?tworc*asin(ratio):tworc*M_PI_21.57079632679489661923;
5414 mT0MinimumDriftTime-=sperp*one_over_beta*ONE_OVER_C3.33564095198152014e-02/cosl;
5415 }
5416
5417
5418 break;
5419 }
5420 r_old=r;
5421 ds_old=ds;
5422 }
5423
5424 return NOERROR;
5425}
5426
5427// Transform the 5x5 tracking error matrix into a 7x7 error matrix in cartesian
5428// coordinates
5429DMatrixDSym DTrackFitterKalmanSIMD::Get7x7ErrorMatrixForward(DMatrixDSym C){
5430 DMatrixDSym C7x7(7);
5431 DMatrix J(7,5);
5432
5433 double p=1./fabs(q_over_p_);
5434 double tanl=1./sqrt(tx_*tx_+ty_*ty_);
5435 double tanl2=tanl*tanl;
5436 double lambda=atan(tanl);
5437 double sinl=sin(lambda);
5438 double sinl3=sinl*sinl*sinl;
5439
5440 J(state_X,state_x)=J(state_Y,state_y)=1.;
5441 J(state_Pz,state_ty)=-p*ty_*sinl3;
5442 J(state_Pz,state_tx)=-p*tx_*sinl3;
5443 J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3;
5444 J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3;
5445 J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3;
5446 J(state_Pz,state_q_over_p)=-p*sinl/q_over_p_;
5447 J(state_Px,state_q_over_p)=tx_*J(state_Pz,state_q_over_p);
5448 J(state_Py,state_q_over_p)=ty_*J(state_Pz,state_q_over_p);
5449 J(state_Z,state_x)=-tx_*tanl2;
5450 J(state_Z,state_y)=-ty_*tanl2;
5451 double diff=tx_*tx_-ty_*ty_;
5452 double frac=tanl2*tanl2;
5453 J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac;
5454 J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac;
5455
5456 // C'= JCJ^T
5457 C7x7=C.Similarity(J);
5458
5459 return C7x7;
5460
5461}
5462
5463
5464
5465// Transform the 5x5 tracking error matrix into a 7x7 error matrix in cartesian
5466// coordinates
5467DMatrixDSym DTrackFitterKalmanSIMD::Get7x7ErrorMatrix(DMatrixDSym C){
5468 DMatrixDSym C7x7(7);
5469 DMatrix J(7,5);
5470 //double cosl=cos(atan(tanl_));
5471 double pt=1./fabs(q_over_pt_);
5472 //double p=pt/cosl;
5473 // double p_sq=p*p;
5474 // double E=sqrt(mass2+p_sq);
5475 double pt_sq=1./(q_over_pt_*q_over_pt_);
5476 double cosphi=cos(phi_);
5477 double sinphi=sin(phi_);
5478 double q=(q_over_pt_>0)?1.:-1.;
5479
5480 J(state_Px,state_q_over_pt)=-q*pt_sq*cosphi;
5481 J(state_Px,state_phi)=-pt*sinphi;
5482
5483 J(state_Py,state_q_over_pt)=-q*pt_sq*sinphi;
5484 J(state_Py,state_phi)=pt*cosphi;
5485
5486 J(state_Pz,state_q_over_pt)=-q*pt_sq*tanl_;
5487 J(state_Pz,state_tanl)=pt;
5488
5489 J(state_X,state_phi)=-D_*cosphi;
5490 J(state_X,state_D)=-sinphi;
5491
5492 J(state_Y,state_phi)=-D_*sinphi;
5493 J(state_Y,state_D)=cosphi;
5494
5495 J(state_Z,state_z)=1.;
5496
5497 // C'= JCJ^T
5498 C7x7=C.Similarity(J);
5499
5500 return C7x7;
5501}
5502
5503// estimate t0 using distance away from wire for CDC hits using forward parms
5504jerror_t DTrackFitterKalmanSIMD::EstimateT0Forward(const DCDCTrackHit *hit,
5505 const DKalmanUpdate_t &cdc_update){
5506 // Wire position and direction variables
5507 DVector3 origin=hit->wire->origin;
5508 DVector3 dir=hit->wire->udir;
5509 double uz=dir.z();
5510 double z0=origin.z();
5511 double cosstereo=cos(hit->wire->stereo);
5512
5513 // Wire position at doca
5514 DVector3 wirepos=origin+(cdc_update.pos.z()-z0)/uz*dir;
5515 // Difference between it and track position
5516 DVector3 diff=cdc_update.pos-wirepos;
5517 double dx=diff.x();
5518 double dy=diff.y();
5519 double d=diff.Perp();
5520 double doca=d*cosstereo;
5521
5522 // Use the track information to estimate t0.
5523 // Use approximate functional form for the distance to time relationship:
5524 // t(d)=c1 d^2 +c2 d^4
5525 double c1=1131,c2=140.7;
5526 double d_sq=doca*doca;
5527 double bfrac=(CDC_DRIFT_B_SCALE_PAR1602.0+CDC_DRIFT_B_SCALE_PAR258.22*cdc_update.B)
5528 /CDC_DRIFT_B_SCALE;
5529 double t0=hit->tdrift-cdc_update.tflight-bfrac*(c1*d_sq+c2*d_sq*d_sq);
5530
5531 // Compute variance in t0
5532 double dt_dd=bfrac*(2.*c1*doca+4*c2*doca*d_sq);
5533 double cos2=cosstereo*cosstereo;
5534 double dd_dx=dx*cos2/d;
5535 double dd_dy=dy*cos2/d;
5536 double var_t0=(dt_dd*dt_dd)*(dd_dx*dd_dx*cdc_update.C(state_x,state_x)
5537 +dd_dy*dd_dy*cdc_update.C(state_y,state_y)
5538 +2.*dd_dx*dd_dy*cdc_update.C(state_x,state_y));
5539
5540 double sigma_t=2.948+35.7*doca;
5541 var_t0+=sigma_t*sigma_t;
5542
5543 // weighted average
5544 mT0Average+=t0/var_t0;
5545 mInvVarT0+=1./var_t0;
5546
5547 return NOERROR;
5548}
5549
5550// estimate t0 using distance away from wire for FDC hits
5551jerror_t DTrackFitterKalmanSIMD::EstimateT0(const DKalmanUpdate_t &fdc_update,
5552 const DKalmanSIMDFDCHit_t *hit){
5553 // Wire coordinate variables
5554 double cosa=hit->cosa;
5555 double sina=hit->sina;
5556
5557 // Tangent variables
5558 double tu=fdc_update.S(state_tx)*cosa-fdc_update.S(state_ty)*sina;
5559 double alpha=atan(tu);
5560 double cosalpha=cos(alpha);
5561 double sinalpha=sin(alpha);
5562
5563 // Compute doca to wire
5564 double doca=fabs(fdc_update.S(state_x)*cosa-fdc_update.S(state_y)*sina
5565 -hit->uwire)*cosalpha;
5566
5567 // Correction factor to account for dependence of drift time on B
5568 double bfrac=(FDC_DRIFT_B_SCALE_PAR156.03+FDC_DRIFT_B_SCALE_PAR29.122*fdc_update.B)
5569 /FDC_DRIFT_B_SCALE;
5570
5571 // Estimate for time at "vertex" using approximate form for t(doca)
5572 // t(d)=c1 d^2 + c2 d^4
5573 double c1=1279,c2=-1158;
5574 double d_sq=doca*doca;
5575 double t0=hit->t-fdc_update.tflight-bfrac*(c1*d_sq+c2*d_sq*d_sq);
5576
5577 // Compute the variance in t0 using an approximate functional form
5578 // for t: t(d)=c1 d^2 + c2 d^4;
5579 double dt_dd=bfrac*(2*c1*doca+4*c2*doca*d_sq);
5580 double dd_dx=cosa*cosalpha;
5581 double dd_dy=-sina*cosalpha;
5582 double temp=sinalpha/(1.+tu*tu);
5583 double dd_dtx=-cosa*temp;
5584 double dd_dty=sina*temp;
5585
5586 double var_t0=(dt_dd*dt_dd)
5587 *(dd_dx*dd_dx*fdc_update.C(state_x,state_x)
5588 +dd_dy*dd_dy*fdc_update.C(state_y,state_y)
5589 +dd_dtx*dd_dtx*fdc_update.C(state_tx,state_tx)
5590 +dd_dty*dd_dty*fdc_update.C(state_ty,state_ty)
5591 +2.*dd_dtx*dd_dty*fdc_update.C(state_tx,state_ty)
5592 +2.*dd_dtx*dd_dx*fdc_update.C(state_tx,state_x)
5593 +2.*dd_dtx*dd_dy*fdc_update.C(state_tx,state_y)
5594 +2.*dd_dty*dd_dy*fdc_update.C(state_ty,state_y)
5595 +2.*dd_dty*dd_dx*fdc_update.C(state_ty,state_x)
5596 +2.*dd_dx*dd_dy*fdc_update.C(state_x,state_y));
5597 double sigma_t=1.567+44.3*doca-1.979*d_sq; // crude approximation
5598 var_t0+=sigma_t*sigma_t;
5599
5600 // Weighted average
5601 mT0Average+=t0/var_t0;
5602 mInvVarT0+=1./var_t0;
5603
5604 return NOERROR;
5605}
5606
5607
5608// Track recovery for Central tracks
5609//-----------------------------------
5610// This code attempts to recover tracks that are "broken". Sometimes the fit fails because too many hits were pruned
5611// such that the number of surviving hits is less than the minimum number of degrees of freedom for a five-parameter fit.
5612// This condition is flagged as an INVALID_FIT. It may also be the case that even if we used enough hits for the fit to
5613// be valid (i.e., make sense in terms of the number of degrees of freedom for the number of parameters (5) we are trying
5614// to determine), we throw away a number of hits near the target because the projected trajectory deviates too far away from
5615// the actual hits. This may be an indication of a kink in the trajectory. This condition is flagged as BREAK_POINT_FOUND.
5616// Sometimes the fit may succeed and no break point is found, but the pruning threw out a large number of hits. This
5617// condition is flagged as PRUNED_TOO_MANY_HITS. The recovery code proceeds by truncating the reference trajectory to
5618// to a point just after the hit where a break point was found and all hits are ignored beyond this point subject to a
5619// minimum number of hits set by MIN_HITS_FOR_REFIT. The recovery code always attempts to use the hits closest to the
5620// target. The code is allowed to iterate; with each iteration the trajectory and list of useable hits is further truncated.
5621kalman_error_t DTrackFitterKalmanSIMD::RecoverBrokenTracks(double anneal_factor,
5622 DMatrix5x1 &S,
5623 DMatrix5x5 &C,
5624 const DMatrix5x5 &C0,
5625 DVector3 &pos,
5626 double &chisq,
5627 unsigned int &numdof){
5628 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5628<<" " << "Attempting to recover broken track ... " <<endl;
5629
5630 // Initialize degrees of freedom and chi^2
5631 double refit_chisq=MAX_CHI21e16;
5632 unsigned int refit_ndf=0;
5633 // State vector and covariance matrix
5634 DMatrix5x5 C1;
5635 DMatrix5x1 S1;
5636 // Position vector
5637 DVector3 refit_pos;
5638
5639 // save the status of the hits used in the fit
5640 vector<int>old_cdc_used_status(cdc_updates.size());
5641 for (unsigned int j=0;j<cdc_updates.size();j++){
5642 old_cdc_used_status[j]=cdc_updates[j].used_in_fit;
5643 }
5644
5645 // Truncate the reference trajectory to just beyond the break point (or the minimum number of hits)
5646 unsigned int min_cdc_index_for_refit=MIN_HITS_FOR_REFIT8-1;
5647 if (break_point_cdc_index<min_cdc_index_for_refit){
5648 break_point_cdc_index=min_cdc_index_for_refit;
5649
5650 // Next determine where we need to truncate the trajectory
5651 DVector3 origin=my_cdchits[break_point_cdc_index]->hit->wire->origin;
5652 DVector3 dir=my_cdchits[break_point_cdc_index]->hit->wire->udir;
5653 double uz=dir.z();
5654 double z0=origin.z();
5655 unsigned int k=0;
5656 for (k=central_traj.size()-1;k>1;k--){
5657 double r=central_traj[k].pos.Perp();
5658 double rnext=central_traj[k-1].pos.Perp();
5659 double r_cdc=(origin+((central_traj[k].pos.z()-z0)/uz)*dir).Perp();
5660 if (rnext>r && r>r_cdc) break;
5661 }
5662 break_point_step_index=k;
5663 }
5664 if (break_point_step_index==central_traj.size()-1){
5665 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5665<<" " << "Invalid reference trajectory in track recovery..." << endl;
5666 return FIT_FAILED;
5667 }
5668
5669 // Here's were the truncation occurs
5670 for (unsigned int j=0;j<break_point_step_index;j++){
5671 central_traj.pop_front();
5672 }
5673
5674 kalman_error_t refit_error=FIT_NOT_DONE;
5675 unsigned old_cdc_index=break_point_cdc_index;
5676 unsigned int refit_iter=0;
5677 unsigned int num_cdchits=my_cdchits.size();
5678 while (break_point_cdc_index>4 && break_point_step_index>0 && central_traj.size()>0){
5679 refit_iter++;
5680
5681 // Flag the cdc hits within the radius of the break point cdc index
5682 // as good, the rest as bad.
5683 for (unsigned int j=0;j<=break_point_cdc_index;j++){
5684 my_cdchits[j]->status=good_hit;
5685 }
5686 for (unsigned int j=break_point_cdc_index+1;j<num_cdchits;j++){
5687 my_cdchits[j]->status=bad_hit;
5688 }
5689
5690 // Now refit with the truncated trajectory and list of hits
5691 C1=4.0*C0;
5692 S1=central_traj[0].S;
5693 refit_chisq=MAX_CHI21e16;
5694 refit_ndf=0;
5695 refit_error=KalmanCentral(anneal_factor,S1,C1,refit_pos,
5696 refit_chisq,refit_ndf);
5697
5698 if (refit_error==FIT_SUCCEEDED){
5699 C=C1;
5700 S=S1;
5701 pos=refit_pos;
5702 chisq=refit_chisq;
5703 numdof=refit_ndf;
5704
5705 return FIT_SUCCEEDED;
5706 }
5707
5708 break_point_cdc_index=old_cdc_index-refit_iter;
5709 central_traj.pop_front();
5710 }
5711
5712 // If the refit did not work, restore the old list hits used in the fit
5713 // before the track recovery was attempted.
5714 for (unsigned int k=0;k<cdc_updates.size();k++){
5715 cdc_updates[k].used_in_fit=old_cdc_used_status[k];
5716 }
5717
5718 return FIT_FAILED;
5719}
5720
5721// Track recovery for forward tracks
5722//-----------------------------------
5723// This code attempts to recover tracks that are "broken". Sometimes the fit fails because too many hits were pruned
5724// such that the number of surviving hits is less than the minimum number of degrees of freedom for a five-parameter fit.
5725// This condition is flagged as an INVALID_FIT. It may also be the case that even if we used enough hits for the fit to
5726// be valid (i.e., make sense in terms of the number of degrees of freedom for the number of parameters (5) we are trying
5727// to determine), we throw away a number of hits near the target because the projected trajectory deviates too far away from
5728// the actual hits. This may be an indication of a kink in the trajectory. This condition is flagged as BREAK_POINT_FOUND.
5729// Sometimes the fit may succeed and no break point is found, but the pruning threw out a large number of hits. This
5730// condition is flagged as PRUNED_TOO_MANY_HITS. The recovery code proceeds by truncating the reference trajectory to
5731// to a point just after the hit where a break point was found and all hits are ignored beyond this point subject to a
5732// minimum number of hits. The recovery code always attempts to use the hits closest to the target. The code is allowed to
5733// iterate; with each iteration the trajectory and list of useable hits is further truncated.
5734kalman_error_t DTrackFitterKalmanSIMD::RecoverBrokenTracks(double anneal_factor,
5735 DMatrix5x1 &S,
5736 DMatrix5x5 &C,
5737 const DMatrix5x5 &C0,
5738 double &chisq,
5739 unsigned int &numdof){
5740 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5740<<" " << "Attempting to recover broken track ... " <<endl;
5741
5742 unsigned int num_cdchits=my_cdchits.size();
5743
5744 // Initialize degrees of freedom and chi^2
5745 double refit_chisq=MAX_CHI21e16;
5746 unsigned int refit_ndf=0;
5747 // State vector and covariance matrix
5748 DMatrix5x5 C1;
5749 DMatrix5x1 S1;
5750
5751 // save the status of the hits used in the fit
5752 vector<int>old_cdc_used_status(cdc_updates.size());
5753 vector<int>old_fdc_used_status(fdc_updates.size());
5754 for (unsigned int j=0;j<fdc_updates.size();j++){
5755 old_fdc_used_status[j]=fdc_updates[j].used_in_fit;
5756 }
5757 for (unsigned int j=0;j<cdc_updates.size();j++){
5758 old_cdc_used_status[j]=cdc_updates[j].used_in_fit;
5759 }
5760
5761 unsigned int min_cdc_index=MIN_HITS_FOR_REFIT8-1;
5762 if (my_fdchits.size()>0){
5763 if (break_point_cdc_index<5){
5764 break_point_cdc_index=0;
5765 min_cdc_index=5;
5766 }
5767 }
5768 // Truncate the trajectory
5769 if (break_point_cdc_index<min_cdc_index){
5770 break_point_cdc_index=min_cdc_index;
5771
5772 // Truncate the reference trajectory
5773 DVector3 origin=my_cdchits[break_point_cdc_index]->hit->wire->origin;
5774 DVector3 dir=my_cdchits[break_point_cdc_index]->hit->wire->udir;
5775 double uz=dir.z();
5776 double z0=origin.z();
5777 unsigned int k=forward_traj.size()-1;
5778 for (;k>1;k--){
5779 double r=forward_traj[k].pos.Perp();
5780 double rnext=forward_traj[k-1].pos.Perp();
5781 double r_cdc=(origin+((forward_traj[k].pos.z()-z0)/uz)*dir).Perp();
5782
5783 if (rnext>r && r>r_cdc) break;
5784 }
5785 break_point_step_index=k;
5786 }
5787 if (break_point_step_index==forward_traj.size()-1){
5788 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5788<<" " << "Invalid reference trajectory in track recovery..." << endl;
5789 return FIT_FAILED;
5790 }
5791 for (unsigned int j=0;j<break_point_step_index;j++){
5792 forward_traj.pop_front();
5793 }
5794 // printf("z %f %f\n",forward_traj[0].pos.z(),forward_traj[forward_traj.size()-1].pos.z());
5795
5796
5797 // Attemp to refit the track using the abreviated list of hits and the truncated
5798 // reference trajectory. Iterates if the fit fails.
5799 kalman_error_t refit_error=FIT_NOT_DONE;
5800 unsigned old_cdc_index=break_point_cdc_index;
5801 unsigned int refit_iter=0;
5802 while (break_point_cdc_index>4 && break_point_step_index>0 && forward_traj.size()>0){
5803 refit_iter++;
5804
5805 // Flag the cdc hits within the radius of the break point cdc index
5806 // as good, the rest as bad.
5807 for (unsigned int j=0;j<=break_point_cdc_index;j++){
5808 my_cdchits[j]->status=good_hit;
5809 }
5810 for (unsigned int j=break_point_cdc_index+1;j<num_cdchits;j++){
5811 my_cdchits[j]->status=bad_hit;
5812 }
5813
5814 // Re-initialize the state vector, covariance, chisq and number of degrees of freedom
5815 C1=4.0*C0;
5816 S1=forward_traj[0].S;
5817 refit_chisq=MAX_CHI21e16;
5818 refit_ndf=0;
5819 // Now refit with the truncated trajectory and list of hits
5820 refit_error=KalmanForwardCDC(anneal_factor,S1,C1,
5821 refit_chisq,refit_ndf);
5822 if (refit_error==FIT_SUCCEEDED){
5823 C=C1;
5824 S=S1;
5825 chisq=refit_chisq;
5826 numdof=refit_ndf;
5827 return FIT_SUCCEEDED;
5828 }
5829 break_point_cdc_index=old_cdc_index-refit_iter;
5830 forward_traj.pop_front();
5831 }
5832 // If the refit did not work, restore the old list hits used in the fit
5833 // before the track recovery was attempted.
5834 for (unsigned int k=0;k<cdc_updates.size();k++){
5835 cdc_updates[k].used_in_fit=old_cdc_used_status[k];
5836 }
5837 for (unsigned int k=0;k<fdc_updates.size();k++){
5838 fdc_updates[k].used_in_fit=old_fdc_used_status[k];
5839 }
5840
5841 return FIT_FAILED;
5842}
5843
5844
5845// Routine to fit hits in the FDC and the CDC using the forward parametrization
5846kalman_error_t DTrackFitterKalmanSIMD::ForwardFit(const DMatrix5x1 &S0,const DMatrix5x5 &C0){
5847 unsigned int num_cdchits=my_cdchits.size();
5848
5849 // Vectors to keep track of updated state vectors and covariance matrices (after
5850 // adding the hit information)
5851 vector<DKalmanUpdate_t>last_cdc_updates;
5852 vector<DKalmanUpdate_t>last_fdc_updates;
5853
5854 // Charge
5855 // double q=input_params.charge();
5856
5857 // Covariance matrix and state vector
5858 DMatrix5x5 C;
5859 DMatrix5x1 S=S0;
5860
5861 // Create matrices to store results from previous iteration
5862 DMatrix5x1 Slast(S);
5863 DMatrix5x5 Clast(C0);
5864
5865 double anneal_factor=ANNEAL_SCALE6.0+1.; // variable for scaling cut for hit pruning
5866 // Chi-squared and degrees of freedom
5867 double chisq=MAX_CHI21e16,chisq_forward=MAX_CHI21e16;
5868 unsigned int my_ndf=0;
5869 unsigned int last_ndf=1;
5870
5871 // Iterate over reference trajectories
5872 for (int iter=0;
5873 iter<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20);
5874 iter++) {
5875 // These variables provide the approximate location along the trajectory
5876 // where there is an indication of a kink in the track
5877 break_point_fdc_index=0;
5878 break_point_cdc_index=0;
5879 break_point_step_index=0;
5880
5881 // Reset material map index
5882 last_material_map=0;
5883
5884 // Abort if momentum is too low
5885 if (fabs(S(state_q_over_p))>Q_OVER_P_MAX100.) break;
5886
5887 // Initialize path length variable and flight time
5888 len=0;
5889 ftime=0.;
5890
5891 // Scale cut for pruning hits according to the iteration number
5892 if (fit_type==kTimeBased){
5893 anneal_factor=ANNEAL_SCALE6.0/pow(ANNEAL_POW_CONST10.0,iter)+1.;
5894 }
5895
5896 // Swim once through the field out to the most upstream FDC hit
5897 jerror_t ref_track_error=SetReferenceTrajectory(S);
5898 if (ref_track_error==NOERROR && forward_traj.size()> 1){
5899 // Reset the status of the cdc hits
5900 for (unsigned int j=0;j<num_cdchits;j++){
5901 my_cdchits[j]->status=good_hit;
5902 }
5903
5904 // perform the kalman filter
5905 C=C0;
5906 kalman_error_t error=KalmanForward(anneal_factor,S,C,chisq,my_ndf);
5907 // Try to recover tracks that failed the first attempt at fitting
5908 if (error!=FIT_SUCCEEDED){
5909 DMatrix5x5 Ctemp=C;
5910 DMatrix5x1 Stemp=S;
5911 unsigned int temp_ndf=my_ndf;
5912 double temp_chi2=chisq;
5913 double x=x_,y=y_,z=z_;
5914
5915
5916 kalman_error_t refit_error=FIT_NOT_DONE;
5917 if (num_cdchits>=MIN_HITS_FOR_REFIT8) refit_error=RecoverBrokenTracks(anneal_factor,S,C,C0,chisq,my_ndf);
5918 if (refit_error!=FIT_SUCCEEDED){
5919 if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){
5920 C=Ctemp;
5921 S=Stemp;
5922 my_ndf=temp_ndf;
5923 chisq=temp_chi2;
5924 x_=x,y_=y,z_=z;
5925
5926 error=FIT_SUCCEEDED;
5927 }
5928 else error=FIT_FAILED;
5929 }
5930 else error=FIT_SUCCEEDED;
5931 }
5932 if (error!=FIT_SUCCEEDED){
5933 if (iter==0) return FIT_FAILED; // first iteration failed
5934 break;
5935 }
5936
5937 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
5937<<" " << "Iter: " << iter+1 << " Chi2=" << chisq << " Ndf=" << my_ndf << endl;
5938 // Check the charge relative to the hypothesis for protons
5939 /*
5940 if (MASS>0.9){
5941 double my_q=S(state_q_over_p)>0?1.:-1.;
5942 if (q!=my_q){
5943 if (DEBUG_LEVEL>0)
5944 _DBG_ << "Sign change in fit for protons" <<endl;
5945 S(state_q_over_p)=fabs(S(state_q_over_p));
5946 }
5947 }
5948 */
5949 // Break out of loop if the chisq is increasing or not changing much
5950 if (chisq/my_ndf>chisq_forward/last_ndf || fabs(chisq-chisq_forward)<0.1) break;
5951
5952 chisq_forward=chisq;
5953 last_ndf=my_ndf;
5954 Slast=S;
5955 Clast=C;
5956
5957 if (fdc_updates.size()>0){
5958 last_fdc_updates.assign(fdc_updates.begin(),fdc_updates.end());
5959 }
5960 if (cdc_updates.size()>0){
5961 last_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end());
5962 }
5963 } //iteration
5964 else{
5965 if (iter==0) return FIT_FAILED;
5966 }
5967 }
5968
5969 // total chisq and ndf
5970 chisq_=chisq_forward;
5971 ndf_=last_ndf;
5972
5973 // Initialize the time variables
5974 mT0Average=mInvVarT0=0.;
5975
5976 // output lists of hits used in the fit and fill pull vector
5977 cdchits_used_in_fit.clear();
5978 pulls.clear();
5979 for (unsigned int m=0;m<last_cdc_updates.size();m++){
5980 if (last_cdc_updates[m].used_in_fit){
5981 cdchits_used_in_fit.push_back(my_cdchits[m]->hit);
5982 pulls.push_back(pull_t(last_cdc_updates[m].residual,
5983 sqrt(last_cdc_updates[m].variance),
5984 last_cdc_updates[m].s));
5985
5986 if (fit_type==kTimeBased){
5987 if (ESTIMATE_T0_TB){
5988 EstimateT0Forward(my_cdchits[m]->hit,last_cdc_updates[m]);
5989 }
5990
5991 if (fit_type==kTimeBased && DEBUG_HISTS){
5992 double tdrift=last_cdc_updates[m].tdrift;
5993 double res=last_cdc_updates[m].residual;
5994 //double B=last_cdc_updates[m].B;
5995 cdc_res_forward->Fill(tdrift,res);
5996 }
5997 }
5998 }
5999 }
6000 fdchits_used_in_fit.clear();
6001 for (unsigned int m=0;m<last_fdc_updates.size();m++){
6002 if (last_fdc_updates[m].used_in_fit){
6003 fdchits_used_in_fit.push_back(my_fdchits[m]->hit);
6004 pulls.push_back(pull_t(last_fdc_updates[m].residual,
6005 sqrt(last_fdc_updates[m].variance),
6006 last_fdc_updates[m].s));
6007
6008 if (fit_type==kTimeBased && ESTIMATE_T0_TB){
6009 EstimateT0(last_fdc_updates[m],my_fdchits[m]);
6010 }
6011 }
6012 }
6013 if (mInvVarT0>0)mT0Average/=mInvVarT0;
6014
6015 // Extrapolate to the point of closest approach to the beam line
6016 z_=forward_traj[forward_traj.size()-1].pos.z();
6017 if (sqrt(Slast(state_x)*Slast(state_x)+Slast(state_y)*Slast(state_y))
6018 >EPS21.e-4)
6019 if (ExtrapolateToVertex(Slast,Clast)!=NOERROR) return POSITION_OUT_OF_RANGE;
6020
6021 // Convert from forward rep. to central rep.
6022 DMatrix5x1 Sc;
6023 DMatrix5x5 Cc;
6024 ConvertStateVectorAndCovariance(z_,Slast,Sc,Clast,Cc);
6025
6026 // Track Parameters at "vertex"
6027 phi_=Sc(state_phi);
6028 q_over_pt_=Sc(state_q_over_pt);
6029 tanl_=Sc(state_tanl);
6030 D_=Sc(state_D);
6031
6032 // Covariance matrix
6033 vector<double>dummy;
6034 if (FORWARD_PARMS_COV==true){
6035 for (unsigned int i=0;i<5;i++){
6036 dummy.clear();
6037 for(unsigned int j=0;j<5;j++){
6038 dummy.push_back(Clast(i,j));
6039 }
6040 fcov.push_back(dummy);
6041 }
6042 }
6043 // Central parametrization
6044 for (unsigned int i=0;i<5;i++){
6045 dummy.clear();
6046 for(unsigned int j=0;j<5;j++){
6047 dummy.push_back(Cc(i,j));
6048 }
6049 cov.push_back(dummy);
6050 }
6051
6052
6053 return FIT_SUCCEEDED;
6054}
6055
6056// Routine to fit hits in the CDC using the forward parametrization
6057kalman_error_t DTrackFitterKalmanSIMD::ForwardCDCFit(const DMatrix5x1 &S0,const DMatrix5x5 &C0){
6058 unsigned int num_cdchits=my_cdchits.size();
6059
6060 // Charge
6061 // double q=input_params.charge();
6062
6063 // Covariance matrix and state vector
6064 DMatrix5x5 C;
6065 DMatrix5x1 S=S0;
6066
6067 // Create matrices to store results from previous iteration
6068 DMatrix5x1 Slast(S);
6069 DMatrix5x5 Clast(C0);
6070
6071 // Vectors to keep track of updated state vectors and covariance matrices (after
6072 // adding the hit information)
6073 vector<DKalmanUpdate_t>last_cdc_updates;
6074 vector<DKalmanUpdate_t>last_fdc_updates;
6075
6076 double anneal_factor=1.; // variable for scaling cut for hit pruning
6077 // Chi-squared and degrees of freedom
6078 double chisq=MAX_CHI21e16,chisq_forward=MAX_CHI21e16;
6079 unsigned int my_ndf=0;
6080 unsigned int last_ndf=1;
6081 // last z position
6082 double zlast=0.;
6083
6084 // Iterate over reference trajectories
6085 for (int iter2=0;
6086 iter2<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20);
6087 iter2++){
6088 if (DEBUG_LEVEL>0){
6089 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
6089<<" " <<"-------- iteration " << iter2+1 << " -----------" <<endl;
6090 }
6091 // These variables provide the approximate location along the trajectory
6092 // where there is an indication of a kink in the track
6093 break_point_cdc_index=num_cdchits-1;
6094 break_point_step_index=0;
6095
6096 // Reset material map index
6097 last_material_map=0;
6098
6099 // Abort if momentum is too low
6100 if (fabs(S(state_q_over_p))>Q_OVER_P_MAX100.){
6101 //printf("Too low momentum? %f\n",1/S(state_q_over_p));
6102 break;
6103 }
6104
6105 // Scale cut for pruning hits according to the iteration number
6106 if (fit_type==kTimeBased){
6107 anneal_factor=ANNEAL_SCALE6.0/pow(ANNEAL_POW_CONST10.0,iter2)+1.;
6108 }
6109
6110 // Initialize path length variable and flight time
6111 len=0;
6112 ftime=0.;
6113
6114 // Swim to create the reference trajectory
6115 jerror_t ref_track_error=SetCDCForwardReferenceTrajectory(S);
6116 if (ref_track_error==NOERROR && forward_traj.size()> 1){
6117 // Reset the status of the cdc hits
6118 for (unsigned int j=0;j<num_cdchits;j++){
6119 my_cdchits[j]->status=good_hit;
6120 }
6121
6122 // perform the filter
6123 C=C0;
6124 kalman_error_t error=KalmanForwardCDC(anneal_factor,S,C,chisq,my_ndf);
6125
6126 // Try to recover tracks that failed the first attempt at fitting
6127 if (error!=FIT_SUCCEEDED && num_cdchits>=MIN_HITS_FOR_REFIT8){
6128 DMatrix5x5 Ctemp=C;
6129 DMatrix5x1 Stemp=S;
6130 unsigned int temp_ndf=my_ndf;
6131 double temp_chi2=chisq;
6132 double x=x_,y=y_,z=z_;
6133
6134 kalman_error_t refit_error=RecoverBrokenTracks(anneal_factor,S,C,C0,chisq,my_ndf);
6135
6136 if (refit_error!=FIT_SUCCEEDED){
6137 if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){
6138 C=Ctemp;
6139 S=Stemp;
6140 my_ndf=temp_ndf;
6141 chisq=temp_chi2;
6142 x_=x,y_=y,z_=z;
6143
6144 error=FIT_SUCCEEDED;
6145 }
6146 else error=FIT_FAILED;
6147 }
6148 else error=FIT_SUCCEEDED;
6149 }
6150 if (error!=FIT_SUCCEEDED){
6151 if (iter2==0) return error;
6152 break;
6153 }
6154
6155 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
6155<<" " << "--> Chisq " << chisq << " NDF "
6156 << my_ndf << endl;
6157 // Check the charge relative to the hypothesis for protons
6158 /*
6159 if (MASS>0.9){
6160 double my_q=S(state_q_over_p)>0?1.:-1.;
6161 if (q!=my_q){
6162 if (DEBUG_LEVEL>0)
6163 _DBG_ << "Sign change in fit for protons" <<endl;
6164 S(state_q_over_p)=fabs(S(state_q_over_p));
6165 }
6166 }
6167 */
6168 if (chisq/my_ndf>chisq_forward/last_ndf || fabs(chisq-chisq_forward)<0.1) break;
6169 chisq_forward=chisq;
6170 Slast=S;
6171 Clast=C;
6172 last_ndf=my_ndf;
6173 zlast=z_;
6174
6175 last_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end());
6176
6177 } //iteration
6178 else{
6179 if (iter2==0) return FIT_FAILED;
6180 break;
6181 }
6182 }
6183
6184 // total chisq and ndf
6185 chisq_=chisq_forward;
6186 ndf_=last_ndf;
6187
6188 // Initialize the time variables
6189 mT0Average=mInvVarT0=0.;
6190
6191 // output lists of hits used in the fit and fill the pull vector
6192 cdchits_used_in_fit.clear();
6193 pulls.clear();
6194 for (unsigned int m=0;m<last_cdc_updates.size();m++){
6195 if (last_cdc_updates[m].used_in_fit){
6196 cdchits_used_in_fit.push_back(my_cdchits[m]->hit);
6197 pulls.push_back(pull_t(last_cdc_updates[m].residual,
6198 sqrt(last_cdc_updates[m].variance),
6199 last_cdc_updates[m].s));
6200
6201 if (fit_type==kTimeBased){
6202 if (ESTIMATE_T0_TB){
6203 EstimateT0Forward(my_cdchits[m]->hit,last_cdc_updates[m]);
6204 }
6205
6206 if (fit_type==kTimeBased && DEBUG_HISTS){
6207 double tdrift=last_cdc_updates[m].tdrift;
6208 double res=last_cdc_updates[m].residual;
6209 //double B=last_cdc_updates[m].B;
6210 cdc_res_forward->Fill(tdrift,res);
6211 }
6212 }
6213 }
6214 }
6215 if (mInvVarT0>0)mT0Average/=mInvVarT0;
6216
6217 // Extrapolate to the point of closest approach to the beam line
6218 z_=zlast;
6219 if (sqrt(Slast(state_x)*Slast(state_x)+Slast(state_y)*Slast(state_y))
6220 >EPS21.e-4)
6221 if (ExtrapolateToVertex(Slast,Clast)!=NOERROR) return EXTRAPOLATION_FAILED;
6222
6223 // Convert from forward rep. to central rep.
6224 DMatrix5x1 Sc;
6225 DMatrix5x5 Cc;
6226 ConvertStateVectorAndCovariance(z_,Slast,Sc,Clast,Cc);
6227
6228 // Track Parameters at "vertex"
6229 phi_=Sc(state_phi);
6230 q_over_pt_=Sc(state_q_over_pt);
6231 tanl_=Sc(state_tanl);
6232 D_=Sc(state_D);
6233
6234 // Covariance matrix
6235 vector<double>dummy;
6236 // ... forward parameterization
6237 if (FORWARD_PARMS_COV==true){
6238 for (unsigned int i=0;i<5;i++){
6239 dummy.clear();
6240 for(unsigned int j=0;j<5;j++){
6241 dummy.push_back(Clast(i,j));
6242 }
6243 fcov.push_back(dummy);
6244 }
6245 }
6246 // Central parametrization
6247 for (unsigned int i=0;i<5;i++){
6248 dummy.clear();
6249 for(unsigned int j=0;j<5;j++){
6250 dummy.push_back(Cc(i,j));
6251 }
6252 cov.push_back(dummy);
6253 }
6254
6255
6256 return FIT_SUCCEEDED;
6257}
6258
6259// Routine to fit hits in the CDC using the central parametrization
6260kalman_error_t DTrackFitterKalmanSIMD::CentralFit(const DMatrix5x1 &S0,const DMatrix5x5 &C0){
6261 // Initial position
6262 DVector3 pos=input_params.position();
6263
6264 // Charge
6265 // double q=input_params.charge();
6266
6267 // Covariance matrix and state vector
6268 DMatrix5x5 Cc;
6269 DMatrix5x1 Sc=S0;
6270
6271 // Variables to store values from previous iterations
6272 DMatrix5x1 Sclast(Sc);
6273 DMatrix5x5 Cclast(C0);
6274 DVector3 last_pos=pos;
6275
6276 unsigned int num_cdchits=my_cdchits.size();
6277
6278 // Vectors to keep track of updated state vectors and covariance matrices (after
6279 // adding the hit information)
6280 vector<DKalmanUpdate_t>last_cdc_updates;
6281
6282 double anneal_factor=1.; // Factor for scaling cut for pruning hits
6283 //Initialization of chisq, ndf, and error status
6284 double chisq_iter=MAX_CHI21e16,chisq=MAX_CHI21e16;
6285 unsigned int my_ndf=0;
6286 unsigned int last_ndf=1;
6287 kalman_error_t error=FIT_NOT_DONE;
6288
6289 // Iterate over reference trajectories
6290 int iter2=0;
6291 for (;iter2<(fit_type==kTimeBased?MAX_TB_PASSES20:MAX_WB_PASSES20);
6292 iter2++){
6293 if (DEBUG_LEVEL>0){
6294 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
6294<<" " <<"-------- iteration " << iter2+1 << " -----------" <<endl;
6295 }
6296
6297 // These variables provide the approximate location along the trajectory
6298 // where there is an indication of a kink in the track
6299 break_point_cdc_index=num_cdchits-1;
6300 break_point_step_index=0;
6301
6302 // Reset material map index
6303 last_material_map=0;
6304
6305 // Break out of loop if p is too small
6306 double q_over_p=Sc(state_q_over_pt)*cos(atan(Sc(state_tanl)));
6307 if (fabs(q_over_p)>Q_OVER_P_MAX100.) break;
6308
6309 // Initialize path length variable and flight time
6310 len=0.;
6311 ftime=0.;
6312
6313 // Scale cut for pruning hits according to the iteration number
6314 if (fit_type==kTimeBased){
6315 anneal_factor=ANNEAL_SCALE6.0/pow(ANNEAL_POW_CONST10.0,iter2)+1.;
6316 }
6317
6318 // Initialize trajectory deque
6319 jerror_t ref_track_error=SetCDCReferenceTrajectory(last_pos,Sc);
6320 if (ref_track_error==NOERROR && central_traj.size()>1){
6321 // Reset the status of the cdc hits
6322 for (unsigned int j=0;j<num_cdchits;j++){
6323 my_cdchits[j]->status=good_hit;
6324 }
6325
6326 // perform the fit
6327 Cc=C0;
6328 error=KalmanCentral(anneal_factor,Sc,Cc,pos,chisq,my_ndf);
6329 // Try to recover tracks that failed the first attempt at fitting
6330 if (error!=FIT_SUCCEEDED && num_cdchits>=MIN_HITS_FOR_REFIT8){
6331 DVector3 temp_pos=pos;
6332 DMatrix5x1 Stemp=Sc;
6333 DMatrix5x5 Ctemp=Cc;
6334 unsigned int temp_ndf=my_ndf;
6335 double temp_chi2=chisq;
6336
6337 kalman_error_t refit_error=RecoverBrokenTracks(anneal_factor,Sc,Cc,C0,pos,chisq,my_ndf);
6338 if (refit_error!=FIT_SUCCEEDED){
6339 if (error==PRUNED_TOO_MANY_HITS || error==BREAK_POINT_FOUND){
6340 Cc=Ctemp;
6341 Sc=Stemp;
6342 my_ndf=temp_ndf;
6343 chisq=temp_chi2;
6344 pos=temp_pos;
6345
6346 error=FIT_SUCCEEDED;
6347 }
6348 else error=FIT_FAILED;
6349 }
6350 else error=FIT_SUCCEEDED;
6351 }
6352 if (error!=FIT_SUCCEEDED){
6353 if (iter2==0) return error;
6354 break;
6355 }
6356
6357 if (DEBUG_LEVEL>0) _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
6357<<" " << "--> Chisq " << chisq << " Ndof " << my_ndf
6358 << endl;
6359 // Check the charge relative to the hypothesis for protons
6360 /*
6361 if (MASS>0.9){
6362 double my_q=Sc(state_q_over_pt)>0?1.:-1.;
6363 if (q!=my_q){
6364 if (DEBUG_LEVEL>0)
6365 _DBG_ << "Sign change in fit for protons" <<endl;
6366 Sc(state_q_over_pt)=fabs(Sc(state_q_over_pt));
6367 }
6368 }
6369 */
6370
6371 if (chisq/my_ndf>chisq_iter/last_ndf || fabs(chisq_iter-chisq)<0.1) break;
6372
6373 // Save the current state vector and covariance matrix
6374 Cclast=Cc;
6375 Sclast=Sc;
6376 last_pos=pos;
6377 chisq_iter=chisq;
6378 last_ndf=my_ndf;
6379
6380 last_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end());
6381 }
6382 else{
6383 if (iter2==0) return FIT_FAILED;
6384 break;
6385 }
6386 }
6387
6388 if (last_pos.Perp()>EPS21.e-4){
6389 if (ExtrapolateToVertex(last_pos,Sclast,Cclast)!=NOERROR) return EXTRAPOLATION_FAILED;
6390 }
6391
6392 // Initialize the time variables
6393 mT0Average=mInvVarT0=0.;
6394
6395 // output lists of hits used in the fit and fill pull vector
6396 cdchits_used_in_fit.clear();
6397 pulls.clear();
6398 for (unsigned int m=0;m<last_cdc_updates.size();m++){
6399 if (last_cdc_updates[m].used_in_fit){
6400 cdchits_used_in_fit.push_back(my_cdchits[m]->hit);
6401 pulls.push_back(pull_t(last_cdc_updates[m].residual,
6402 sqrt(last_cdc_updates[m].variance),
6403 last_cdc_updates[m].s));
6404
6405 if (fit_type==kTimeBased){
6406 if (ESTIMATE_T0_TB){
6407 EstimateT0Central(my_cdchits[m]->hit,last_cdc_updates[m]);
6408 }
6409 if (DEBUG_HISTS){
6410 double tdrift=last_cdc_updates[m].tdrift;
6411 double res=last_cdc_updates[m].residual;
6412 double sigma=sqrt(last_cdc_updates[m].variance);
6413 double B=last_cdc_updates[m].B;
6414
6415 // Wire position and direction variables
6416 DVector3 origin=my_cdchits[m]->hit->wire->origin;
6417 DVector3 dir=my_cdchits[m]->hit->wire->udir;
6418 double uz=dir.z();
6419 double z0=origin.z();
6420 double cosstereo=cos(my_cdchits[m]->hit->wire->stereo);
6421
6422 // Wire position at doca
6423 DVector3 wirepos=origin+(last_cdc_updates[m].pos.z()-z0)/uz*dir;
6424 // Difference between it and track position
6425 DVector3 diff=last_cdc_updates[m].pos-wirepos;
6426 double d=diff.Perp();
6427 double doca=d*cosstereo;
6428
6429 cdc_drift->Fill(tdrift,doca);
6430 cdc_time_vs_d->Fill(doca,tdrift);
6431 if (my_cdchits[m]->hit->wire->ring==1)
6432 cdc_res->Fill(tdrift,res);
6433 cdc_res_vs_tanl->Fill(last_cdc_updates[m].S(state_tanl),res);
6434 cdc_res_vs_dE->Fill(my_cdchits[m]->hit->dE,res);
6435 cdc_res_vs_B->Fill(B,res);
6436 if (doca>0.75) cdc_drift_vs_B->Fill(B,tdrift);
6437
6438 cdc_residuals->Fill(my_cdchits[m]->hit->wire->ring,res/sigma);
6439 }
6440 }
6441 }
6442 }
6443 if (mInvVarT0>0)mT0Average/=mInvVarT0;
6444
6445 // Rotate covariance matrix from a coordinate system whose origin is on the track to the global coordinate system
6446 double B=sqrt(Bx*Bx+By*By+Bz*Bz);
6447 double qrc_old=1./(qBr2p0.003*B*Sclast(state_q_over_pt));
6448 double qrc_plus_D=Sclast(state_D)+qrc_old;
6449 double q=(qrc_old>0)?1.:-1.;
6450 double dx=-last_pos.x();
6451 double dy=-last_pos.y();
6452 double d2=dx*dx+dy*dy;
6453 double sinphi=sin(Sclast(state_phi));
6454 double cosphi=cos(Sclast(state_phi));
6455 double dx_sinphi_minus_dy_cosphi=dx*sinphi-dy*cosphi;
6456 double rc=sqrt(d2
6457 +2.*qrc_plus_D*(dx_sinphi_minus_dy_cosphi)
6458 +qrc_plus_D*qrc_plus_D);
6459
6460 DMatrix5x5 Jc=I5x5;
6461 Jc(state_D,state_D)=q*(dx_sinphi_minus_dy_cosphi+qrc_plus_D)/rc;
6462 Jc(state_D,state_q_over_pt)=qrc_old*(Jc(state_D,state_D)-1.)/Sclast(state_q_over_pt);
6463 Jc(state_D,state_phi)=q*qrc_plus_D*(dx*cosphi+dy*sinphi)/rc;
6464
6465 Cclast=Cclast.SandwichMultiply(Jc);
6466
6467 // Track Parameters at "vertex"
6468 phi_=Sclast(state_phi);
6469 q_over_pt_=Sclast(state_q_over_pt);
6470 tanl_=Sclast(state_tanl);
6471 x_=last_pos.x();
6472 y_=last_pos.y();
6473 z_=last_pos.z();
6474 D_=sqrt(d2);
6475 if ((x_>0 && sinphi>0) || (y_ <0 && cosphi>0) || (y_>0 && cosphi<0)
6476 || (x_<0 && sinphi<0)) D_*=-1.;
6477
6478 if (!isfinite(x_) || !isfinite(y_) || !isfinite(z_) || !isfinite(phi_)
6479 || !isfinite(q_over_pt_) || !isfinite(tanl_)){
6480 if (DEBUG_LEVEL>0){
6481 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
6481<<" " << "At least one parameter is NaN or +-inf!!" <<endl;
6482 _DBG_std::cerr<<"DTrackFitterKalmanSIMD.cc"<<":"<<
6482<<" " << "x " << x_ << " y " << y_ << " z " << z_ << " phi " << phi_
6483 << " q/pt " << q_over_pt_ << " tanl " << tanl_ << endl;
6484 }
6485 return INVALID_FIT;
6486 }
6487
6488 // Covariance matrix at vertex
6489 fcov.clear();
6490 vector<double>dummy;
6491 for (unsigned int i=0;i<5;i++){
6492 dummy.clear();
6493 for(unsigned int j=0;j<5;j++){
6494 dummy.push_back(Cclast(i,j));
6495 }
6496 cov.push_back(dummy);
6497 }
6498
6499 // total chisq and ndf
6500 chisq_=chisq_iter;
6501 ndf_=last_ndf;
6502 //printf("NDof %d\n",ndf);
6503
6504 return FIT_SUCCEEDED;
6505}