libraries/TRACKING/DTrackFitterKalmanSIMD.cc

Bug Summary

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