Bug Summary

File:plugins/Analysis/dc_alignment/DEventProcessor_dc_alignment.cc
Location:line 835, column 10
Description:Value stored to 'anneal_factor' during its initialization is never read

Annotated Source Code

1// $Id$
2//
3// File: DEventProcessor_dc_alignment.cc
4// Created: Thu Oct 18 17:15:41 EDT 2012
5// Creator: staylor (on Linux ifarm1102 2.6.18-274.3.1.el5 x86_64)
6//
7
8#include "DEventProcessor_dc_alignment.h"
9using namespace jana;
10
11#include <TROOT.h>
12#include <TCanvas.h>
13#include <TPolyLine.h>
14
15#define MAX_STEPS1000 1000
16
17// Routine used to create our DEventProcessor
18#include <JANA/JApplication.h>
19extern "C"{
20void InitPlugin(JApplication *app){
21 InitJANAPlugin(app);
22 app->AddProcessor(new DEventProcessor_dc_alignment());
23}
24} // "C"
25
26
27bool cdc_hit_cmp(const DCDCTrackHit *a,const DCDCTrackHit *b){
28
29 return(a->wire->origin.Y()>b->wire->origin.Y());
30}
31
32bool fdc_pseudo_cmp(const DFDCPseudo *a,const DFDCPseudo *b){
33 return (a->wire->origin.z()<b->wire->origin.z());
34}
35
36
37bool bcal_cmp(const bcal_match_t &a,const bcal_match_t &b){
38 return (a.match->y>b.match->y);
39}
40
41// Locate a position in vector xx given x
42unsigned int DEventProcessor_dc_alignment::locate(vector<double>&xx,double x){
43 int n=xx.size();
44 if (x==xx[0]) return 0;
45 else if (x==xx[n-1]) return n-2;
46
47 int jl=-1;
48 int ju=n;
49 int ascnd=(xx[n-1]>=xx[0]);
50 while(ju-jl>1){
51 int jm=(ju+jl)>>1;
52 if ( (x>=xx[jm])==ascnd)
53 jl=jm;
54 else
55 ju=jm;
56 }
57 return jl;
58}
59
60
61// Convert time to distance for the cdc
62double DEventProcessor_dc_alignment::cdc_drift_distance(double dphi,
63 double delta,double t){
64 double d=0.;
65 if (t>0){
66 double f_0=0.;
67 double f_delta=0.;
68
69 if (dphi*delta>0){
70 double a1=long_drift_func[0][0];
71 double a2=long_drift_func[0][1];
72 double b1=long_drift_func[1][0];
73 double b2=long_drift_func[1][1];
74 double c1=long_drift_func[2][0];
75 double c2=long_drift_func[2][1];
76 double c3=long_drift_func[2][2];
77
78 // use "long side" functional form
79 double my_t=0.001*t;
80 double sqrt_t=sqrt(my_t);
81 double t3=my_t*my_t*my_t;
82 double delta_mag=fabs(delta);
83 f_delta=(a1+a2*delta_mag)*sqrt_t+(b1+b2*delta_mag)*my_t
84 +(c1+c2*delta_mag+c3*delta*delta)*t3;
85 f_0=a1*sqrt_t+b1*my_t+c1*t3;
86 }
87 else{
88 double my_t=0.001*t;
89 double sqrt_t=sqrt(my_t);
90 double delta_mag=fabs(delta);
91
92 // use "short side" functional form
93 double a1=short_drift_func[0][0];
94 double a2=short_drift_func[0][1];
95 double a3=short_drift_func[0][2];
96 double b1=short_drift_func[1][0];
97 double b2=short_drift_func[1][1];
98 double b3=short_drift_func[1][2];
99
100 double delta_sq=delta*delta;
101 f_delta= (a1+a2*delta_mag+a3*delta_sq)*sqrt_t
102 +(b1+b2*delta_mag+b3*delta_sq)*my_t;
103 f_0=a1*sqrt_t+b1*my_t;
104 }
105
106 unsigned int max_index=cdc_drift_table.size()-1;
107 if (t>cdc_drift_table[max_index]){
108 //_DBG_ << "t: " << t <<" d " << f_delta <<endl;
109 d=f_delta;
110
111 return d;
112 }
113
114 // Drift time is within range of table -- interpolate...
115 unsigned int index=0;
116 index=locate(cdc_drift_table,t);
117 double dt=cdc_drift_table[index+1]-cdc_drift_table[index];
118 double frac=(t-cdc_drift_table[index])/dt;
119 double d_0=0.01*(double(index)+frac);
120
121 double P=0.;
122 double tcut=250.0; // ns
123 if (t<tcut) {
124 P=(tcut-t)/tcut;
125 }
126 d=f_delta*(d_0/f_0*P+1.-P);
127 }
128 return d;
129}
130
131
132// Interpolate on a table to convert time to distance for the fdc
133double DEventProcessor_dc_alignment::fdc_drift_distance(double t){
134 double d=0.;
135 if (t>fdc_drift_table[fdc_drift_table.size()-1]) return 0.5;
136 if (t>0){
137 unsigned int index=0;
138 index=locate(fdc_drift_table,t);
139 double dt=fdc_drift_table[index+1]-fdc_drift_table[index];
140 double frac=(t-fdc_drift_table[index])/dt;
141 d=0.01*(double(index)+frac);
142 }
143 return d;
144}
145
146
147
148//------------------
149// DEventProcessor_dc_alignment (Constructor)
150//------------------
151DEventProcessor_dc_alignment::DEventProcessor_dc_alignment()
152{
153 fdc_ptr = &fdc;
154 fdc_c_ptr= &fdc_c;
155 cdc_ptr = &cdc;
156
157 pthread_mutex_init(&mutex, NULL__null);
158
159}
160
161//------------------
162// ~DEventProcessor_dc_alignment (Destructor)
163//------------------
164DEventProcessor_dc_alignment::~DEventProcessor_dc_alignment()
165{
166
167}
168
169//------------------
170// init
171//------------------
172jerror_t DEventProcessor_dc_alignment::init(void)
173{
174 myevt=0;
175 one_over_zrange=1./150.;
176
177 printf("Initializing..........\n");
178
179 RUN_BENCHMARK=false;
180 gPARMS->SetDefaultParameter("DCALIGN:RUN_BENCHMARK",RUN_BENCHMARK);
181 USE_BCAL=false;
182 gPARMS->SetDefaultParameter("DCALIGN:USE_BCAL", USE_BCAL);
183 USE_FCAL=false;
184 gPARMS->SetDefaultParameter("DCALIGN:USE_FCAL", USE_FCAL);
185 COSMICS=false;
186 gPARMS->SetDefaultParameter("DCALIGN:COSMICS", COSMICS);
187 USE_DRIFT_TIMES=false;
188 gPARMS->SetDefaultParameter("DCALIGN:USE_DRIFT_TIMES",USE_DRIFT_TIMES);
189 READ_CDC_FILE=false;
190 gPARMS->SetDefaultParameter("DCALIGN:READ_CDC_FILE",READ_CDC_FILE);
191 READ_ANODE_FILE=false;
192 gPARMS->SetDefaultParameter("DCALIGN:READ_ANODE_FILE",READ_ANODE_FILE);
193 READ_CATHODE_FILE=false;
194 gPARMS->SetDefaultParameter("DCALIGN:READ_CATHODE_FILE",READ_CATHODE_FILE);
195 ALIGN_WIRE_PLANES=true;
196 gPARMS->SetDefaultParameter("DCALIGN:ALIGN_WIRE_PLANES",ALIGN_WIRE_PLANES);
197 FILL_TREE=false;
198 gPARMS->SetDefaultParameter("DCALIGN:FILL_TREE",FILL_TREE);
199 MIN_PSEUDOS=12;
200 gPARMS->SetDefaultParameter("DCALIGN:MIN_PSEUDOS",MIN_PSEUDOS);
201 MIN_INTERSECTIONS=10;
202 gPARMS->SetDefaultParameter("DCALIGN:MIN_INTERSECTIONS",MIN_INTERSECTIONS);
203
204 fdc_alignments.resize(24);
205 for (unsigned int i=0;i<24;i++){
206 fdc_alignments[i].A=DMatrix2x1();
207 if (RUN_BENCHMARK==false){
208 fdc_alignments[i].E=DMatrix2x2(0.000001,0.,0.,0.0001);
209 }
210 else{
211 fdc_alignments[i].E=DMatrix2x2();
212 }
213 }
214 fdc_cathode_alignments.resize(24);
215 for (unsigned int i=0;i<24;i++){
216 double var=0.0001;
217 double var_phi=0.000001;
218 if (RUN_BENCHMARK==false){
219 fdc_cathode_alignments[i].E=DMatrix4x4(var_phi,0.,0.,0., 0.,var,0.,0.,
220 0.,0.,var_phi,0., 0.,0.,0.,var);
221 }
222 else{
223 fdc_cathode_alignments[i].E=DMatrix4x4();
224 }
225 fdc_cathode_alignments[i].A=DMatrix4x1();
226 }
227
228 if (READ_ANODE_FILE){
229 ifstream fdcfile("fdc_alignment.dat");
230 // Skip first line, used to identify columns in file
231 //char sdummy[40];
232 // fdcfile.getline(sdummy,40);
233 // loop over remaining entries
234 for (unsigned int i=0;i<24;i++){
235 double du,dphi,dz;
236
237 fdcfile >> dphi;
238 fdcfile >> du;
239 fdcfile >> dz;
240
241 fdc_alignments[i].A(kU)=du;
242 fdc_alignments[i].A(kPhiU)=dphi;
243 }
244 fdcfile.close();
245 }
246
247 if (READ_CATHODE_FILE){
248 ifstream fdcfile("fdc_cathode_alignment.dat");
249 // Skip first line, used to identify columns in file
250 //char sdummy[40];
251 // fdcfile.getline(sdummy,40);
252 // loop over remaining entries
253 for (unsigned int i=0;i<24;i++){
254 double du,dphiu,dv,dphiv;
255
256 fdcfile >> dphiu;
257 fdcfile >> du;
258 fdcfile >> dphiv;
259 fdcfile >> dv;
260
261 fdc_cathode_alignments[i].A(kU)=du;
262 fdc_cathode_alignments[i].A(kPhiU)=dphiu;
263 fdc_cathode_alignments[i].A(kV)=dv;
264 fdc_cathode_alignments[i].A(kPhiV)=dphiv;
265
266 }
267 fdcfile.close();
268 }
269
270 fdc_drift_parms(0)=0.;
271 fdc_drift_parms(1)=0.;
272 fdc_drift_parms(2)=0.03;
273
274 unsigned int numstraws[28]={42,42,54,54,66,66,80,80,93,93,106,106,123,123,
275 135,135,146,146,158,158,170,170,182,182,197,197,
276 209,209};
277 for (unsigned int i=0;i<28;i++){
278 vector<cdc_align_t>tempvec;
279 for (unsigned int j=0;j<numstraws[i];j++){
280 cdc_align_t temp;
281 temp.A=DMatrix4x1(0.,0.,0.,0.);
282 double var=0.01;
283 if (RUN_BENCHMARK==false){
284 temp.E=DMatrix4x4(var,0.,0.,0., 0.,var,0.,0., 0.,0.,var,0., 0.,0.,0.,var);
285 }
286 else {
287 temp.E=DMatrix4x4();
288 }
289 tempvec.push_back(temp);
290 }
291 cdc_alignments.push_back(tempvec);
292 }
293
294 if (READ_CDC_FILE){
295 ifstream cdcfile("cdc_alignment.dat");
296 for (unsigned int ring=0;ring<cdc_alignments.size();ring++){
297 for (unsigned int straw=0;straw<cdc_alignments[ring].size();
298 straw++){
299 double dxu,dyu,dxd,dyd;
300
301 cdcfile >> dxu;
302 cdcfile >> dyu;
303 cdcfile >> dxd;
304 cdcfile >> dyd;
305
306 cdc_alignments[ring][straw].A(k_dXu)=dxu;
307 cdc_alignments[ring][straw].A(k_dYu)=dyu;
308 cdc_alignments[ring][straw].A(k_dXd)=dxd;
309 cdc_alignments[ring][straw].A(k_dYd)=dyd;
310
311 double var=0.0001;
312 cdc_alignments[ring][straw].E=DMatrix4x4(var,0.,0.,0., 0.,var,0.,0., 0.,0.,var,0., 0.,0.,0.,var);
313 }
314 }
315 cdcfile.close();
316 }
317
318
319
320 if (FILL_TREE){
321 // Create Tree
322 fdctree = new TTree("fdc","FDC alignments");
323 fdcbranch = fdctree->Branch("T","FDC_branch",&fdc_ptr);
324
325 // Create Tree
326 fdcCtree = new TTree("fdc_c","FDC alignments");
327 fdcCbranch = fdcCtree->Branch("T","FDC_c_branch",&fdc_c_ptr);
328
329 // Create Tree
330 cdctree = new TTree("cdc","CDC alignments");
331 cdcbranch = cdctree->Branch("T","CDC_branch",&cdc_ptr);
332 }
333
334 return NOERROR;
335}
336
337//------------------
338// brun
339//------------------
340jerror_t DEventProcessor_dc_alignment::brun(JEventLoop *loop, int runnumber)
341{
342 DApplication* dapp=dynamic_cast<DApplication*>(loop->GetJApplication());
343 dgeom = dapp->GetDGeometry(runnumber);
344 //dgeom->GetFDCWires(fdcwires);
345
346 // Get the position of the CDC downstream endplate from DGeometry
347 double endplate_dz,endplate_rmin,endplate_rmax;
348 dgeom->GetCDCEndplate(endplate_z,endplate_dz,endplate_rmin,endplate_rmax);
349 endplate_z+=0.5*endplate_dz;
350
351
352 JCalibration *jcalib = dapp->GetJCalibration((loop->GetJEvent()).GetRunNumber());
353 vector< map<string, double> > tvals;
354 cdc_drift_table.clear();
355 if (jcalib->Get("CDC/cdc_drift_table::NoBField", tvals)==false){
356 for(unsigned int i=0; i<tvals.size(); i++){
357 map<string, double> &row = tvals[i];
358 cdc_drift_table.push_back(1000.*row["t"]);
359 }
360 }
361 else{
362 jerr << " CDC time-to-distance table not available... bailing..." << endl;
363 exit(0);
364 }
365
366 unsigned int numstraws[28]={42,42,54,54,66,66,80,80,93,93,106,106,123,123,
367 135,135,146,146,158,158,170,170,182,182,197,197,
368 209,209};
369
370 map<string, double> cdc_res_parms;
371 jcalib->Get("CDC/cdc_resolution_parms", cdc_res_parms);
372 CDC_RES_PAR1 = cdc_res_parms["res_par1"];
373 CDC_RES_PAR2 = cdc_res_parms["res_par2"];
374
375 fdc_drift_table.clear();
376 if (jcalib->Get("FDC/fdc_drift_table", tvals)==false){
377 for(unsigned int i=0; i<tvals.size(); i++){
378 map<string, double> &row = tvals[i];
379 fdc_drift_table.push_back(1000.*row["t"]);
380 }
381 }
382 else{
383 jerr << " FDC time-to-distance table not available... bailing..." << endl;
384 exit(0);
385 }
386
387 // Get offsets tweaking nominal geometry from calibration database
388 vector<map<string,double> >vals;
389 vector<cdc_offset_t>tempvec;
390
391 if (jcalib->Get("CDC/wire_alignment",vals)==false){
392 unsigned int straw_count=0,ring_count=0;
393 for(unsigned int i=0; i<vals.size(); i++){
394 map<string,double> &row = vals[i];
395
396 // put the vector of offsets for the current ring into the offsets vector
397 if (straw_count==numstraws[ring_count]){
398 straw_count=0;
399 ring_count++;
400
401 cdc_offsets.push_back(tempvec);
402
403 tempvec.clear();
404 }
405
406 // Get the offsets from the calibration database
407 cdc_offset_t temp;
408 temp.dx_u=row["dxu"];
409 //temp.dx_u=0.;
410
411 temp.dy_u=row["dyu"];
412 //temp.dy_u=0.;
413
414 temp.dx_d=row["dxd"];
415 //temp.dx_d=0.;
416
417 temp.dy_d=row["dyd"];
418 //temp.dy_d=0.;
419
420 tempvec.push_back(temp);
421
422 straw_count++;
423 }
424 cdc_offsets.push_back(tempvec);
425 }
426 else{
427 jerr<< "CDC wire alignment table not available... bailing... " <<endl;
428 exit(0);
429 }
430
431 // Get the straw sag parameters from the database
432 max_sag.clear();
433 sag_phi_offset.clear();
434 unsigned int straw_count=0,ring_count=0;
435 if (jcalib->Get("CDC/sag_parameters", tvals)==false){
436 vector<double>temp,temp2;
437 for(unsigned int i=0; i<tvals.size(); i++){
438 map<string, double> &row = tvals[i];
439
440 temp.push_back(row["offset"]);
441 temp2.push_back(row["phi"]);
442
443 straw_count++;
444 if (straw_count==numstraws[ring_count]){
445 max_sag.push_back(temp);
446 sag_phi_offset.push_back(temp2);
447 temp.clear();
448 temp2.clear();
449 straw_count=0;
450 ring_count++;
451 }
452 }
453 }
454
455 if (jcalib->Get("CDC/drift_parameters::NoBField", tvals)==false){
456 map<string, double> &row = tvals[0]; //long drift side
457 long_drift_func[0][0]=row["a1"];
458 long_drift_func[0][1]=row["a2"];
459 long_drift_func[0][2]=row["a3"];
460 long_drift_func[1][0]=row["b1"];
461 long_drift_func[1][1]=row["b2"];
462 long_drift_func[1][2]=row["b3"];
463 long_drift_func[2][0]=row["c1"];
464 long_drift_func[2][1]=row["c2"];
465 long_drift_func[2][2]=row["c3"];
466
467 row = tvals[1]; // short drift side
468 short_drift_func[0][0]=row["a1"];
469 short_drift_func[0][1]=row["a2"];
470 short_drift_func[0][2]=row["a3"];
471 short_drift_func[1][0]=row["b1"];
472 short_drift_func[1][1]=row["b2"];
473 short_drift_func[1][2]=row["b3"];
474 short_drift_func[2][0]=row["c1"];
475 short_drift_func[2][1]=row["c2"];
476 short_drift_func[2][2]=row["c3"];
477 }
478
479 dapp->Lock();
480
481 for (int i=0;i<28;i++){
482 char title[40];
483 sprintf(title,"cdc_residual_ring%d",i+1);
484 Hcdc_ring_res[i]=(TH2F*)gROOT->FindObject(title);
485 if (!Hcdc_ring_res[i]){
486 Hcdc_ring_res[i]=new TH2F(title,title,numstraws[i],0.5,numstraws[i]+0.5,
487 100,-1,1);
488 }
489 }
490 for (int i=0;i<28;i++){
491 char title[40];
492 sprintf(title,"cdc_drift_time_ring%d",i+1);
493 Hcdc_ring_time[i]=(TH2F*)gROOT->FindObject(title);
494 if (!Hcdc_ring_time[i]){
495 Hcdc_ring_time[i]=new TH2F(title,title,numstraws[i],0.5,numstraws[i]+0.5,
496 900,-100,800);
497 }
498 }
499
500 Hprob = (TH1F*)gROOT->FindObject("Hprob");
501 if (!Hprob){
502 Hprob=new TH1F("Hprob","Confidence level for final fit",100,0.0,1.);
503 }
504 Hpseudo_prob = (TH1F*)gROOT->FindObject("Hpseudo_prob");
505 if (!Hpseudo_prob){
506 Hpseudo_prob=new TH1F("Hpseudo_prob","Confidence level for final fit",100,0.0,1.);
507 }
508
509 Hcdc_prob = (TH1F*)gROOT->FindObject("Hcdc_prob");
510 if (!Hcdc_prob){
511 Hcdc_prob=new TH1F("Hcdc_prob","Confidence level for time-based fit",100,0.0,1.);
512 }
513 Hcdc_prelimprob = (TH1F*)gROOT->FindObject("Hcdc_prelimprob");
514 if (!Hcdc_prelimprob){
515 Hcdc_prelimprob=new TH1F("Hcdc_prelimprob","Confidence level for prelimary fit",100,0.0,1.);
516 }
517 Hintersection_match = (TH1F*)gROOT->FindObject("Hintersection_match");
518 if (!Hintersection_match){
519 Hintersection_match=new TH1F("Hintersection_match","Intersection matching distance",100,0.0,25.);
520 }
521 Hintersection_link_match = (TH1F*)gROOT->FindObject("Hintersection_link_match");
522 if (!Hintersection_link_match){
523 Hintersection_link_match=new TH1F("Hintersection_link_match","Segment matching distance",100,0.0,25.);
524 }
525
526 Hcdcmatch = (TH1F*)gROOT->FindObject("Hcdcmatch");
527 if (!Hcdcmatch){
528 Hcdcmatch=new TH1F("Hcdcmatch","CDC hit matching distance",1000,0.0,50.);
529 }
530 Hcdcmatch_stereo = (TH1F*)gROOT->FindObject("Hcdcmatch_stereo");
531 if (!Hcdcmatch_stereo){
532 Hcdcmatch_stereo=new TH1F("Hcdcmatch_stereo","CDC stereo hit matching distance",1000,0.0,50.);
533 }
534
535 Hmatch = (TH1F*)gROOT->FindObject("Hmatch");
536 if (!Hmatch){
537 Hmatch=new TH1F("Hmatch","Segment matching distance",100,0.0,25.);
538 }
539 Hlink_match = (TH1F*)gROOT->FindObject("Hlink_match");
540 if (!Hlink_match){
541 Hlink_match=new TH1F("link_match","Segment matching distance",100,0.0,25.);
542 }
543
544
545 Hbeta = (TH1F*)gROOT->FindObject("Hbeta");
546 if (!Hbeta){
547 Hbeta=new TH1F("Hbeta","Estimate for #beta",100,0.0,1.5);
548 Hbeta->SetXTitle("#beta");
549 }
550
551 Hztarg = (TH1F*)gROOT->FindObject("Hztarg");
552 if (!Hztarg){
553 Hztarg=new TH1F("Hztarg","Estimate for target z",1200,-300.0,300.0);
554 }
555 Hures_vs_layer=(TH2F*)gROOT->FindObject("Hures_vs_layer");
556 if (!Hures_vs_layer){
557 Hures_vs_layer=new TH2F("Hures_vs_layer","Cathode u-view residuals",
558 24,0.5,24.5,200,-0.5,0.5);
559 }
560 Hres_vs_layer=(TH2F*)gROOT->FindObject("Hres_vs_layer");
561 if (!Hres_vs_layer){
562 Hres_vs_layer=new TH2F("Hres_vs_layer","wire-based residuals",
563 24,0.5,24.5,200,-0.5,0.5);
564 }
565 Hvres_vs_layer=(TH2F*)gROOT->FindObject("Hvres_vs_layer");
566 if (!Hvres_vs_layer){
567 Hvres_vs_layer=new TH2F("Hvres_vs_layer","Cathode v-view residuals",
568 24,0.5,24.5,200,-0.5,0.5);
569 }
570 Hcdc_time_vs_d=(TH2F*)gROOT->FindObject("Hcdc_time_vs_d");
571 if (!Hcdc_time_vs_d){
572 Hcdc_time_vs_d=new TH2F("Hcdc_time_vs_d",
573 "cdc drift time vs doca",120,0,1.2,600,-20,1180);
574 }
575 Hcdcdrift_time=(TH2F*)gROOT->FindObject("Hcdcdrift_time");
576 if (!Hcdcdrift_time){
577 Hcdcdrift_time=new TH2F("Hcdcdrift_time",
578 "cdc doca vs drift time",1201,-21,1181,100,0,1);
579 }
580 Hcdcres_vs_drift_time=(TH2F*)gROOT->FindObject("Hcdcres_vs_drift_time");
581 if (!Hcdcres_vs_drift_time){
582 Hcdcres_vs_drift_time=new TH2F("Hcdcres_vs_drift_time","cdc Residual vs drift time",600,-20,1180,500,-1.,1.);
583 }
584 Hcdcres_vs_d=(TH2F*)gROOT->FindObject("Hcdcres_vs_d");
585 if (!Hcdcres_vs_d){
586 Hcdcres_vs_d=new TH2F("Hcdcres_vs_d","cdc Residual vs distance to wire",600,0,1.2,500,-1.,1.);
587 }
588
589 Hdrift_time=(TH2F*)gROOT->FindObject("Hdrift_time");
590 if (!Hdrift_time){
591 Hdrift_time=new TH2F("Hdrift_time",
592 "doca vs drift time",201,-21,381,100,0,1);
593 }
594 Hres_vs_drift_time=(TH2F*)gROOT->FindObject("Hres_vs_drift_time");
595 if (!Hres_vs_drift_time){
596 Hres_vs_drift_time=new TH2F("Hres_vs_drift_time","Residual vs drift time",320,-20,300,1000,-1,1);
597 }
598 Hdv_vs_dE=(TH2F*)gROOT->FindObject("Hdv_vs_dE");
599 if (!Hdv_vs_dE){
600 Hdv_vs_dE=new TH2F("Hdv_vs_dE","dv vs energy dep",100,0,20e-6,200,-1,1);
601 }
602
603 Hbcalmatch=(TH2F*)gROOT->FindObject("Hbcalmatch");
604 if (!Hbcalmatch){
605 Hbcalmatch=new TH2F("Hbcalmatch","BCAL #deltar vs #deltaz",100,-50.,50.,
606 100,0.,10.);
607 }
608 Hbcalmatchxy=(TH2F*)gROOT->FindObject("Hbcalmatchxy");
609 if (!Hbcalmatchxy){
610 Hbcalmatchxy=new TH2F("Hbcalmatchxy","BCAL #deltay vs #deltax",400,-50.,50.,
611 400,-50.,50.);
612 }
613 Hfcalmatch=(TH1F*)gROOT->FindObject("Hfcalmatch");
614 if (!Hfcalmatch){
615 Hfcalmatch=new TH1F("Hfcalmatch","FCAL #deltar",400,0.,50.);
616 }
617
618
619
620 dapp->Unlock();
621
622 // Get pointer to TrackFinder object
623 vector<const DTrackFinder *> finders;
624 loop->Get(finders);
625
626 if(finders.size()<1){
627 _DBG_std::cerr<<"plugins/Analysis/dc_alignment/DEventProcessor_dc_alignment.cc"
<<":"<<627<<" "<<"Unable to get a DTrackFinder object!"<<endl;
628 return RESOURCE_UNAVAILABLE;
629 }
630
631 // Drop the const qualifier from the DTrackFinder pointer
632 finder = const_cast<DTrackFinder*>(finders[0]);
633
634 return NOERROR;
635}
636
637//------------------
638// erun
639//------------------
640jerror_t DEventProcessor_dc_alignment::erun(void)
641{
642
643
644 return NOERROR;
645}
646
647//------------------
648// fini
649//------------------
650jerror_t DEventProcessor_dc_alignment::fini(void)
651{
652
653 printf("Events processed = %d\n",myevt);
654
655 if (RUN_BENCHMARK==false){
656
657 for (unsigned int ring=0;ring<cdc_alignments.size();ring++){
658 for (unsigned int straw=0;straw<cdc_alignments[ring].size();
659 straw++){
660 if (fabs(cdc_alignments[ring][straw].A(k_dXu))>0.19)
661 cout << cdc_alignments[ring][straw].A(k_dXu) << " "
662 << sqrt(cdc_alignments[ring][straw].E(k_dXu,k_dXu)) << endl;
663 }
664 }
665
666 ofstream cdcfile("cdc_alignment.dat");
667 //cdcfile << "Ring straw dXu dYu dXd dYd" << endl;
668 for (unsigned int ring=0;ring<cdc_alignments.size();ring++){
669 for (unsigned int straw=0;straw<cdc_alignments[ring].size();
670 straw++){
671 // cdcfile << ring+1 << " " << straw+1 << " "
672 cdcfile << cdc_alignments[ring][straw].A(k_dXu) << " "
673 << cdc_alignments[ring][straw].A(k_dYu) << " "
674 << cdc_alignments[ring][straw].A(k_dXd) << " "
675 << cdc_alignments[ring][straw].A(k_dYd) << endl;
676 }
677 }
678 cdcfile.close();
679
680 ofstream cdcfile2("cdc_alignment_update.dat");
681 //cdcfile << "Ring straw dXu dYu dXd dYd" << endl;
682 for (unsigned int ring=0;ring<cdc_alignments.size();ring++){
683 for (unsigned int straw=0;straw<cdc_alignments[ring].size();
684 straw++){
685 // cdcfile << ring+1 << " " << straw+1 << " "
686 cdcfile2 << (cdc_alignments[ring][straw].A(k_dXu)+cdc_offsets[ring][straw].dx_u) << " "
687 << (cdc_alignments[ring][straw].A(k_dYu)+cdc_offsets[ring][straw].dy_u) << " "
688 << (cdc_alignments[ring][straw].A(k_dXd)+cdc_offsets[ring][straw].dx_d) << " "
689 << (cdc_alignments[ring][straw].A(k_dYd)+cdc_offsets[ring][straw].dy_d) << endl;
690 }
691 }
692 cdcfile2.close();
693
694
695
696
697 if (ALIGN_WIRE_PLANES){
698 ofstream fdcfile("fdc_alignment.dat");
699 //fdcfile << "dPhi dU sig(dU)" <<endl;
700 for (unsigned int layer=0;layer<24;layer++){
701 double du=fdc_alignments[layer].A(kU);
702 double dphi=fdc_alignments[layer].A(kPhiU);
703
704 fdcfile << dphi <<" " <<" " << du << " " << "0." << endl;
705 }
706 fdcfile.close();
707 }
708 else{
709 ofstream fdcfile("fdc_cathode_alignment.dat");
710 for (unsigned int layer=0;layer<24;layer++){
711 fdcfile << fdc_cathode_alignments[layer].A(kPhiU)
712 << " " << fdc_cathode_alignments[layer].A(kU)
713 << " " << fdc_cathode_alignments[layer].A(kPhiV)
714 << " " << fdc_cathode_alignments[layer].A(kV) << endl;
715 }
716 fdcfile.close();
717 }
718 }
719
720 return NOERROR;
721}
722
723//------------------
724// evnt
725//------------------
726jerror_t DEventProcessor_dc_alignment::evnt(JEventLoop *loop, int eventnumber){
727 myevt++;
728
729 // Reset the track finder
730 finder->Reset();
731
732 // Get BCAL showers, FCAL showers and FDC space points
733 vector<const DFCALShower*>fcalshowers;
734 if (USE_FCAL) loop->Get(fcalshowers);
735 vector<const DBCALShower*>bcalshowers;
736 if (USE_BCAL)loop->Get(bcalshowers);
737
738 vector<const DFDCPseudo*>pseudos;
739 loop->Get(pseudos);
740 vector<const DCDCTrackHit*>cdcs;
741 //if (COSMICS)
742 loop->Get(cdcs);
743
744 if (cdcs.size()>20 /* && cdcs.size()<60*/){
745 // Add the hits to the finder helper class, link axial hits into segments
746 // then link axial hits and stereo hits together to form track candidates
747 for (size_t i=0;i<cdcs.size();i++) finder->AddHit(cdcs[i]);
748 finder->FindAxialSegments();
749 finder->LinkCDCSegments();
750
751 // Get the list of linked segments and fit the hits to lines
752 const vector<DTrackFinder::cdc_track_t>tracks=finder->GetCDCTracks();
753 for (unsigned int i=0;i<tracks.size();i++){
754 // Add lists of stereo and axial hits associated with this track
755 // and sort
756 vector<const DCDCTrackHit *>hits=tracks[i].axial_hits;
757 hits.insert(hits.end(),tracks[i].stereo_hits.begin(),tracks[i].stereo_hits.end());
758 sort(hits.begin(),hits.end(),cdc_hit_cmp);
759
760 // Use earliest cdc time to estimate t0
761 double t0=1e6;
762 for (unsigned int j=0;j<hits.size();j++){
763 double L=(hits[0]->wire->origin-hits[j]->wire->origin).Perp();
764 double t_test=hits[j]->tdrift-L/29.98;
765 if (t_test<t0) t0=t_test;
766 }
767
768 // Initial guess for state vector
769 DMatrix4x1 S(tracks[i].S);
770
771 // Run the Kalman Filter algorithm
772 DoFilter(t0,tracks[i].z,S,hits);
773 }
774 }
775
776 //-------------------------------------------------------------------------
777 // FDC alignment
778 //-------------------------------------------------------------------------
779 if (pseudos.size()>MIN_PSEUDOS
780 //&&((fcalshowers.size()>0&&fcalshowers.size()<3)
781 // || (bcalshowers.size()>0&&bcalshowers.size()<3))
782 ){
783 // Add hits to the track finder helper class, link hits into segments
784 // then link segments together to form track candidates
785 for (size_t i=0;i<pseudos.size();i++) finder->AddHit(pseudos[i]);
786 finder->FindFDCSegments();
787 finder->LinkFDCSegments();
788
789 // Get the list of linked segments
790 const vector<DTrackFinder::fdc_segment_t>tracks=finder->GetFDCTracks();
791
792 // Loop over linked segments
793 for (unsigned int k=0;k<tracks.size();k++){
794 vector<const DFDCPseudo *>hits=tracks[k].hits;
795
796 if (hits.size()>MIN_PSEUDOS){
797 sort(hits.begin(),hits.end(),fdc_pseudo_cmp);
798
799 // Initial guess for state vector
800 DMatrix4x1 S(tracks[k].S);
801
802 // Move x and y to just before the first hit
803 double my_z=hits[0]->wire->origin.z()-1.;
804 S(state_x)+=my_z*S(state_tx);
805 S(state_y)+=my_z*S(state_ty);
806
807 // Use earliest fdc time to estimate t0
808 double t0=1e6;
809 double dsdz=sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
810 for (unsigned int m=0;m<hits.size();m++){
811 if (hits[m]->time<t0){
812 double L=(hits[m]->wire->origin.z()-my_z)*dsdz;
813 t0=hits[m]->time-L/29.98; // assume moving at speed of light
814 }
815 }
816
817 // Run the Kalman Filter algorithm
818 if (ALIGN_WIRE_PLANES) DoFilterAnodePlanes(t0,my_z,S,hits);
819 else DoFilterCathodePlanes(t0,my_z,S,hits);
820 }
821 } //loop over tracks
822 } // minimimum number of pseudopoints?
823
824 return NOERROR;
825}
826
827// Steering routine for the kalman filter
828jerror_t
829DEventProcessor_dc_alignment::DoFilter(double t0,double OuterZ,DMatrix4x1 &S,
830 vector<const DCDCTrackHit *>&hits){
831 unsigned int numhits=hits.size();
832 unsigned int maxindex=numhits-1;
833
834 int NEVENTS=100000;
835 double anneal_factor=pow(1e4,(double(NEVENTS-myevt))/(NEVENTS-1.));
Value stored to 'anneal_factor' during its initialization is never read
836 if (myevt>NEVENTS) anneal_factor=1.;
837 anneal_factor=1.;
838 if (RUN_BENCHMARK) anneal_factor=1.;
839
840 // deques to store reference trajectories
841 deque<trajectory_t>trajectory;
842 deque<trajectory_t>best_traj;
843
844 // State vector to store "best" values
845 DMatrix4x1 Sbest;
846
847 // Covariance matrix
848 DMatrix4x4 C0,C,Cbest;
849 C0(state_x,state_x)=C0(state_y,state_y)=1.0;
850 C0(state_tx,state_tx)=C0(state_ty,state_ty)=0.01;
851
852 vector<cdc_update_t>updates(hits.size());
853 vector<cdc_update_t>best_updates;
854 double chi2=1e16,chi2_old=1e16;
855 unsigned int ndof=0,ndof_old=0;
856 unsigned int iter=0;
857
858 //printf("wirebased-----------\n");
859
860 // Perform a wire-based pass
861 for(iter=0;iter<20;iter++){
862 chi2_old=chi2;
863 ndof_old=ndof;
864
865 trajectory.clear();
866 if (SetReferenceTrajectory(t0,OuterZ,S,trajectory,
867 hits[maxindex])!=NOERROR) break;
868
869 C=C0;
870 if (KalmanFilter(anneal_factor,S,C,hits,trajectory,updates,chi2,ndof)!=NOERROR)
871 break;
872
873 //printf(">>>>>>chi2 %f ndof %d\n",chi2,ndof);
874
875 if (fabs(chi2_old-chi2)<0.1 || chi2>chi2_old) break;
876
877 // Save the current state and covariance matrixes
878 Cbest=C;
879 Sbest=S;
880 best_updates.assign(updates.begin(),updates.end());
881 best_traj.assign(trajectory.begin(),trajectory.end());
882
883 // run the smoother (opposite direction to filter)
884 //Smooth(S,C,trajectory,updates);
885 }
886 if (iter>0){
887 double prelimprob=TMath::Prob(chi2_old,ndof_old);
888 Hcdc_prelimprob->Fill(prelimprob);
889
890 if (prelimprob>0.0001){
891
892 // Perform a time-based pass
893 S=Sbest;
894 chi2=1e16;
895
896 //printf("Timebased-----------\n");
897 //if (false)
898 for (iter=0;iter<20;iter++){
899 chi2_old=chi2;
900 ndof_old=ndof;
901
902 trajectory.clear();
903 if (SetReferenceTrajectory(t0,OuterZ,S,trajectory,hits[maxindex])
904 ==NOERROR){
905 C=C0;
906 KalmanFilter(anneal_factor,S,C,hits,trajectory,updates,chi2,ndof,true);
907
908 //printf(">>>>>>chi2 %f ndof %d\n",chi2,ndof);
909 if (fabs(chi2-chi2_old)<0.1
910 || TMath::Prob(chi2,ndof)<TMath::Prob(chi2_old,ndof_old)) break;
911
912 Sbest=S;
913 Cbest=C;
914 best_updates.assign(updates.begin(),updates.end());
915 best_traj.assign(trajectory.begin(),trajectory.end());
916 }
917 else break;
918 }
919 if (iter>0){
920 double prob=TMath::Prob(chi2_old,ndof_old);
921 Hcdc_prob->Fill(prob);
922
923 PlotLines(trajectory);
924
925 if (prob>1e-3)
926 {
927 // run the smoother (opposite direction to filter)
928 vector<cdc_update_t>smoothed_updates(updates.size());
929 for (unsigned int k=0;k<smoothed_updates.size();k++){
930 smoothed_updates[k].used_in_fit=false;
931 }
932 Smooth(Sbest,Cbest,best_traj,hits,best_updates,smoothed_updates);
933
934 for (unsigned int k=0;k<smoothed_updates.size();k++){
935 if (smoothed_updates[k].used_in_fit==true){
936 double tdrift=smoothed_updates[k].drift_time;
937 double d=smoothed_updates[k].doca;
938 double res=smoothed_updates[k].res;
939 int ring_id=smoothed_updates[k].ring_id;
940 int straw_id=smoothed_updates[k].straw_id;
941 Hcdcres_vs_drift_time->Fill(tdrift,res);
942 Hcdcres_vs_d->Fill(d,res);
943 Hcdcdrift_time->Fill(tdrift,d);
944 Hcdc_time_vs_d->Fill(d,tdrift);
945 Hcdc_ring_res[ring_id]->Fill(straw_id+1,res);
946 Hcdc_ring_time[ring_id]->Fill(straw_id+1,tdrift);
947 }
948 }
949
950 if (prob>0.001 && RUN_BENCHMARK==false){
951 FindOffsets(hits,smoothed_updates);
952
953 if (FILL_TREE){
954 for (unsigned int ring=0;ring<cdc_alignments.size();ring++){
955 for (unsigned int straw=0;straw<cdc_alignments[ring].size();
956 straw++){
957 // Set up to fill tree
958 cdc.dXu=cdc_alignments[ring][straw].A(k_dXu);
959 cdc.dYu=cdc_alignments[ring][straw].A(k_dYu);
960 cdc.dXd=cdc_alignments[ring][straw].A(k_dXd);
961 cdc.dYd=cdc_alignments[ring][straw].A(k_dYd);
962 cdc.straw=straw+1;
963 cdc.ring=ring+1;
964 cdc.N=myevt;
965
966
967 // Lock mutex
968 pthread_mutex_lock(&mutex);
969
970 cdctree->Fill();
971
972 // Unlock mutex
973 pthread_mutex_unlock(&mutex);
974
975 }
976 }
977 }
978 }
979
980 } // check on final fit CL
981 } // at least one time-based fit worked?
982 } // check on preliminary fit CL
983 } // at least one iteration worked?
984
985 return NOERROR;
986}
987
988
989// Steering routine for the kalman filter
990jerror_t
991DEventProcessor_dc_alignment::DoFilterCathodePlanes(double t0,double start_z,
992 DMatrix4x1 &S,
993 vector<const DFDCPseudo *>&hits){
994 unsigned int num_hits=hits.size();
995 vector<update_t>updates(num_hits);
996 vector<update_t>best_updates;
997 vector<update_t>smoothed_updates(num_hits);
998
999 int NEVENTS=100000;
1000 double anneal_factor=pow(1e3,(double(NEVENTS-myevt))/(NEVENTS-1.));
1001 if (myevt>NEVENTS) anneal_factor=1.;
1002 //anneal_factor=10.;
1003 if (RUN_BENCHMARK) anneal_factor=1.;
1004 //anneal_factor=1e3;
1005
1006 // Best guess for state vector at the beginning of the trajectory
1007 DMatrix4x1 Sbest;
1008
1009 // Use the result from the initial line fit to form a reference trajectory
1010 // for the track.
1011 deque<trajectory_t>trajectory;
1012 deque<trajectory_t>best_traj;
1013
1014 // Intial guess for covariance matrix
1015 DMatrix4x4 C,C0,Cbest;
1016 C0(state_x,state_x)=C0(state_y,state_y)=1.;
1017 C0(state_tx,state_tx)=C0(state_ty,state_ty)=0.01;
1018
1019 // Chi-squared and degrees of freedom
1020 double chi2=1e16,chi2_old=1e16;
1021 unsigned int ndof=0,ndof_old=0;
1022 unsigned iter=0;
1023 for(;;){
1024 iter++;
1025 chi2_old=chi2;
1026 ndof_old=ndof;
1027
1028 trajectory.clear();
1029 if (SetReferenceTrajectory(t0,start_z,S,trajectory,hits)!=NOERROR) break;
1030 C=C0;
1031 if (KalmanFilter(anneal_factor,S,C,hits,trajectory,updates,chi2,ndof)
1032 !=NOERROR) break;
1033
1034 //printf("== event %d == iter %d =====chi2 %f ndof %d \n",myevt,iter,chi2,ndof);
1035 if (chi2>chi2_old || fabs(chi2_old-chi2)<0.1 || iter==ITER_MAX20) break;
1036
1037 // Save the current state and covariance matrixes
1038 Cbest=C;
1039 Sbest=S;
1040 best_updates.assign(updates.begin(),updates.end());
1041 best_traj.assign(trajectory.begin(),trajectory.end());
1042 // run the smoother (opposite direction to filter)
1043 //Smooth(S,C,trajectory,hits,updates,smoothed_updates);
1044 }
1045
1046 if (iter>1){
1047 double prob=TMath::Prob(chi2_old,ndof_old);
1048 Hpseudo_prob->Fill(prob);
1049
1050 // printf("prob %f\n",prob);
1051
1052 PlotLines(trajectory);
1053
1054 if (prob>0.00001)
1055 {
1056 // run the smoother (opposite direction to filter)
1057 Smooth(Sbest,Cbest,best_traj,hits,best_updates,smoothed_updates);
1058
1059 //Hbeta->Fill(mBeta);
1060 for (unsigned int i=0;i<smoothed_updates.size();i++){
1061 unsigned int layer=hits[i]->wire->layer;
1062
1063 Hures_vs_layer->Fill(layer,smoothed_updates[i].res(0));
1064 Hvres_vs_layer->Fill(layer,smoothed_updates[i].res(1));
1065 Hdv_vs_dE->Fill(hits[i]->dE,smoothed_updates[i].res(1));
1066
1067 Hdrift_time->Fill(smoothed_updates[i].drift_time,
1068 smoothed_updates[i].doca);
1069 }
1070
1071 if (prob>0.001 && RUN_BENCHMARK==false){
1072 FindOffsets(hits,smoothed_updates);
1073
1074 if (FILL_TREE){
1075 for (unsigned int layer=0;layer<24;layer++){
1076 fdc_c.dPhiU=fdc_cathode_alignments[layer].A(kPhiU);
1077 fdc_c.dU=fdc_cathode_alignments[layer].A(kU);
1078 fdc_c.dPhiV=fdc_cathode_alignments[layer].A(kPhiV);
1079 fdc_c.dV=fdc_cathode_alignments[layer].A(kV);
1080
1081 fdc_c.layer=layer+1;
1082 fdc_c.N=myevt;
1083
1084 // Lock mutex
1085 pthread_mutex_lock(&mutex);
1086
1087 fdcCtree->Fill();
1088
1089 // Unlock mutex
1090 pthread_mutex_unlock(&mutex);
1091 }
1092 }
1093 }
1094 return NOERROR;
1095 }
1096 }
1097
1098
1099 return VALUE_OUT_OF_RANGE;
1100}
1101
1102// Steering routine for the kalman filter
1103jerror_t
1104DEventProcessor_dc_alignment::DoFilterAnodePlanes(double t0,double start_z,
1105 DMatrix4x1 &S,
1106 vector<const DFDCPseudo *>&hits){
1107 unsigned int num_hits=hits.size();
1108 vector<wire_update_t>updates(num_hits);
1109 vector<wire_update_t>best_updates;
1110 vector<wire_update_t>smoothed_updates(num_hits);
1111
1112 int NEVENTS=75000;
1113 double anneal_factor=1.;
1114 if (USE_DRIFT_TIMES){
1115 anneal_factor=pow(1000.,(double(NEVENTS-myevt))/(NEVENTS-1.));
1116 if (myevt>NEVENTS) anneal_factor=1.;
1117 }
1118 if (RUN_BENCHMARK) anneal_factor=1.;
1119 //anneal_factor=1e3;
1120
1121 // Best guess for state vector at "vertex"
1122 DMatrix4x1 Sbest;
1123
1124 // Use the result from the initial line fit to form a reference trajectory
1125 // for the track.
1126 deque<trajectory_t>trajectory;
1127 deque<trajectory_t>best_traj;
1128
1129 // Intial guess for covariance matrix
1130 DMatrix4x4 C,C0,Cbest;
1131 C0(state_x,state_x)=C0(state_y,state_y)=1.;
1132 C0(state_tx,state_tx)=C0(state_ty,state_ty)=0.001;
1133
1134 // Chi-squared and degrees of freedom
1135 double chi2=1e16,chi2_old=1e16;
1136 unsigned int ndof=0,ndof_old=0;
1137 unsigned iter=0;
1138 for(;;){
1139 iter++;
1140 chi2_old=chi2;
1141 ndof_old=ndof;
1142
1143 trajectory.clear();
1144 if (SetReferenceTrajectory(t0,start_z,S,trajectory,hits)!=NOERROR) break;
1145 C=C0;
1146 if (KalmanFilter(anneal_factor,S,C,hits,trajectory,updates,chi2,ndof)
1147 !=NOERROR) break;
1148
1149 //printf("== event %d == iter %d =====chi2 %f ndof %d \n",myevt,iter,chi2,ndof);
1150 if (chi2>chi2_old || iter==ITER_MAX20) break;
1151
1152 // Save the current state and covariance matrixes
1153 Cbest=C;
1154 Sbest=S;
1155 best_updates.assign(updates.begin(),updates.end());
1156 best_traj.assign(trajectory.begin(),trajectory.end());
1157 // run the smoother (opposite direction to filter)
1158 //Smooth(S,C,trajectory,hits,updates,smoothed_updates);
1159 }
1160
1161 if (iter>1){
1162 double prob=TMath::Prob(chi2_old,ndof_old);
1163 Hprob->Fill(prob);
1164
1165 PlotLines(trajectory);
1166
1167 if (prob>0.001)
1168 {
1169 // run the smoother (opposite direction to filter)
1170 Smooth(Sbest,Cbest,best_traj,hits,best_updates,smoothed_updates);
1171
1172 //Hbeta->Fill(mBeta);
1173 for (unsigned int i=0;i<smoothed_updates.size();i++){
1174 unsigned int layer=hits[i]->wire->layer;
1175
1176 Hres_vs_layer->Fill(layer,smoothed_updates[i].ures);
1177 if (prob>0.1/*&&layer==smoothed_updates.size()/2*/){
1178 Hdrift_time->Fill(smoothed_updates[i].drift_time,
1179 smoothed_updates[i].doca);
1180 Hres_vs_drift_time->Fill(smoothed_updates[i].drift_time,
1181 smoothed_updates[i].ures);
1182
1183 }
1184
1185 }
1186
1187 if (RUN_BENCHMARK==false){
1188 FindOffsets(hits,smoothed_updates);
1189
1190 if (FILL_TREE){
1191 for (unsigned int layer=0;layer<24;layer++){
1192 fdc.dPhi=fdc_alignments[layer].A(kPhiU);
1193 fdc.dX=fdc_alignments[layer].A(kU);
1194
1195 fdc.layer=layer+1;
1196 fdc.N=myevt;
1197
1198 // Lock mutex
1199 pthread_mutex_lock(&mutex);
1200
1201 fdctree->Fill();
1202
1203 // Unlock mutex
1204 pthread_mutex_unlock(&mutex);
1205 }
1206 }
1207 }
1208 return NOERROR;
1209
1210 }
1211 }
1212
1213
1214 return VALUE_OUT_OF_RANGE;
1215}
1216
1217
1218// Kalman smoother
1219jerror_t DEventProcessor_dc_alignment::Smooth(DMatrix4x1 &Ss,DMatrix4x4 &Cs,
1220 deque<trajectory_t>&trajectory,
1221 vector<const DFDCPseudo *>&hits,
1222 vector<update_t>updates,
1223 vector<update_t>&smoothed_updates
1224 ){
1225 DMatrix4x1 S;
1226 DMatrix4x4 C,dC;
1227 DMatrix4x4 JT,A;
1228 DMatrix2x1 Mdiff;
1229
1230 unsigned int max=trajectory.size()-1;
1231 S=(trajectory[max].Skk);
1232 C=(trajectory[max].Ckk);
1233 JT=(trajectory[max].J.Transpose());
1234 //Ss=S;
1235 //Cs=C;
1236 for (unsigned int m=max-1;m>0;m--){
1237 if (trajectory[m].h_id==0){
1238 A=trajectory[m].Ckk*JT*C.Invert();
1239 Ss=trajectory[m].Skk+A*(Ss-S);
1240 Cs=trajectory[m].Ckk+A*(Cs-C)*A.Transpose();
1241 }
1242 else if (trajectory[m].h_id>0){
1243 unsigned int first_id=trajectory[m].h_id-1;
1244 for (int k=trajectory[m].num_hits-1;k>=0;k--){
1245 unsigned int id=first_id+k;
1246 A=updates[id].C*JT*C.Invert();
1247 dC=A*(Cs-C)*A.Transpose();
1248 Ss=updates[id].S+A*(Ss-S);
1249 Cs=updates[id].C+dC;
1250
1251 // Nominal rotation of wire planes
1252 double cosa=hits[id]->wire->udir.y();
1253 double sina=hits[id]->wire->udir.x();
1254
1255 // State vector
1256 double x=Ss(state_x);
1257 double y=Ss(state_y);
1258 double tx=Ss(state_tx);
1259 double ty=Ss(state_ty);
1260
1261 // Get the aligment parameters for this layer
1262 unsigned int layer=hits[id]->wire->layer-1;
1263 DMatrix4x1 A=fdc_cathode_alignments[layer].A;
1264 DMatrix2x1 Aw=fdc_alignments[layer].A;
1265 double delta_u=Aw(kU);
1266 double sindphi=sin(Aw(kPhiU));
1267 double cosdphi=cos(Aw(kPhiU));
1268
1269 // Components of rotation matrix for converting global to local coords.
1270 double cospsi=cosa*cosdphi+sina*sindphi;
1271 double sinpsi=sina*cosdphi-cosa*sindphi;
1272
1273 // x,y and tx,ty in local coordinate system
1274 // To transform from (x,y) to (u,v), need to do a rotation:
1275 // u = x*cosa-y*sina
1276 // v = y*cosa+x*sina
1277 double upred_wire_plane=x*cospsi-y*sinpsi;
1278 double vpred_wire_plane=x*sinpsi+y*cospsi;
1279 double tu=tx*cospsi-ty*sinpsi;
1280 double tv=tx*sinpsi+ty*cospsi;
1281
1282 // Variables for angle of incidence with respect to the z-direction in
1283 // the u-z plane
1284 double alpha=atan(tu);
1285 double cosalpha=cos(alpha);
1286 double sinalpha=sin(alpha);
1287
1288 // Doca from wire
1289 double uwire=hits[id]->wire->u+delta_u;
1290 double d=(upred_wire_plane-uwire)*cosalpha;
1291
1292 // Predicted avalanche position along the wire
1293 double vpred=vpred_wire_plane-tv*sinalpha*d;
1294
1295 // predicted positions in two cathode planes' coordinate systems
1296 double phi_u=hits[id]->phi_u+A(kPhiU);
1297 double phi_v=hits[id]->phi_v+A(kPhiV);
1298 double cosphi_u=cos(phi_u);
1299 double sinphi_u=sin(phi_u);
1300 double cosphi_v=cos(phi_v);
1301 double sinphi_v=sin(phi_v);
1302 double vv=-vpred*sinphi_v+uwire*cosphi_v+A(kV);
1303 double vu=-vpred*sinphi_u+uwire*cosphi_u+A(kU);
1304
1305 // Difference between measurements and predictions
1306 Mdiff(0)=hits[id]->u-vu;
1307 Mdiff(1)=hits[id]->v-vv;
1308
1309 smoothed_updates[id].res=Mdiff;
1310 smoothed_updates[id].doca=fabs(d);
1311
1312 smoothed_updates[id].drift=updates[id].drift;
1313 smoothed_updates[id].drift_time=updates[id].drift_time;
1314 smoothed_updates[id].S=Ss;
1315 smoothed_updates[id].C=Cs;
1316 smoothed_updates[id].V=updates[id].V-updates[id].H*dC*updates[id].H_T;
1317 }
1318 }
1319 S=trajectory[m].Skk;
1320 C=trajectory[m].Ckk;
1321 JT=trajectory[m].J.Transpose();
1322 }
1323
1324 A=trajectory[0].Ckk*JT*C.Invert();
1325 Ss=trajectory[0].Skk+A*(Ss-S);
1326 Cs=trajectory[0].Ckk+A*(Cs-C)*A.Transpose();
1327
1328 return NOERROR;
1329}
1330
1331
1332// Kalman smoother
1333jerror_t DEventProcessor_dc_alignment::Smooth(DMatrix4x1 &Ss,DMatrix4x4 &Cs,
1334 deque<trajectory_t>&trajectory,
1335 vector<const DFDCPseudo *>&hits,
1336 vector<wire_update_t>updates,
1337 vector<wire_update_t>&smoothed_updates
1338 ){
1339 DMatrix4x1 S;
1340 DMatrix4x4 C,dC;
1341 DMatrix4x4 JT,A;
1342
1343 unsigned int max=trajectory.size()-1;
1344 S=(trajectory[max].Skk);
1345 C=(trajectory[max].Ckk);
1346 JT=(trajectory[max].J.Transpose());
1347 //Ss=S;
1348 //Cs=C;
1349 for (unsigned int m=max-1;m>0;m--){
1350 if (trajectory[m].h_id==0){
1351 A=trajectory[m].Ckk*JT*C.Invert();
1352 Ss=trajectory[m].Skk+A*(Ss-S);
1353 Cs=trajectory[m].Ckk+A*(Cs-C)*A.Transpose();
1354 }
1355 else if (trajectory[m].h_id>0){
1356 unsigned int first_id=trajectory[m].h_id-1;
1357 for (int k=trajectory[m].num_hits-1;k>=0;k--){
1358 unsigned int id=first_id+k;
1359 A=updates[id].C*JT*C.Invert();
1360 dC=A*(Cs-C)*A.Transpose();
1361 Ss=updates[id].S+A*(Ss-S);
1362 Cs=updates[id].C+dC;
1363
1364 // Nominal rotation of wire planes
1365 double cosa=hits[id]->wire->udir.y();
1366 double sina=hits[id]->wire->udir.x();
1367
1368 // State vector
1369 double x=Ss(state_x);
1370 double y=Ss(state_y);
1371 double tx=Ss(state_tx);
1372 double ty=Ss(state_ty);
1373
1374 // Get the aligment vector and error matrix for this layer
1375 unsigned int layer=hits[id]->wire->layer-1;
1376 DMatrix2x2 E=fdc_alignments[layer].E;
1377 DMatrix2x1 A=fdc_alignments[layer].A;
1378 double delta_u=A(kU);
1379 double sindphi=sin(A(kPhiU));
1380 double cosdphi=cos(A(kPhiU));
1381
1382 // Components of rotation matrix for converting global to local coords.
1383 double cospsi=cosa*cosdphi+sina*sindphi;
1384 double sinpsi=sina*cosdphi-cosa*sindphi;
1385
1386 // x,y and tx,ty in local coordinate system
1387 // To transform from (x,y) to (u,v), need to do a rotation:
1388 // u = x*cosa-y*sina
1389 // v = y*cosa+x*sina
1390 // (without alignment offsets)
1391 double upred=x*cospsi-y*sinpsi;
1392 double tu=tx*cospsi-ty*sinpsi;
1393
1394 // Variables for angle of incidence with respect to the z-direction in
1395 // the u-z plane
1396 double alpha=atan(tu);
1397 double cosalpha=cos(alpha);
1398
1399 // Smoothed residuals
1400 double uwire=hits[id]->wire->u+delta_u;
1401 double d=(upred-uwire)*cosalpha;
1402 smoothed_updates[id].ures=(d>0?1.:-1.)*updates[id].drift-d;
1403 smoothed_updates[id].doca=fabs(d);
1404
1405 smoothed_updates[id].drift=updates[id].drift;
1406 smoothed_updates[id].drift_time=updates[id].drift_time;
1407 smoothed_updates[id].S=Ss;
1408 smoothed_updates[id].C=Cs;
1409 smoothed_updates[id].R=updates[id].R-updates[id].H*dC*updates[id].H_T;
1410 }
1411 }
1412 S=trajectory[m].Skk;
1413 C=trajectory[m].Ckk;
1414 JT=trajectory[m].J.Transpose();
1415 }
1416
1417 A=trajectory[0].Ckk*JT*C.Invert();
1418 Ss=trajectory[0].Skk+A*(Ss-S);
1419 Cs=trajectory[0].Ckk+A*(Cs-C)*A.Transpose();
1420
1421 return NOERROR;
1422}
1423
1424// Kalman smoother
1425jerror_t
1426DEventProcessor_dc_alignment::Smooth(DMatrix4x1 &Ss,DMatrix4x4 &Cs,
1427 deque<trajectory_t>&trajectory,
1428 vector<const DCDCTrackHit *>&hits,
1429 vector<cdc_update_t>&updates,
1430 vector<cdc_update_t>&smoothed_updates
1431 ){
1432 DMatrix4x1 S;
1433 DMatrix4x4 C,dC;
1434 DMatrix4x4 JT,A;
1435
1436 unsigned int max=trajectory.size()-1;
1437 S=(trajectory[max].Skk);
1438 C=(trajectory[max].Ckk);
1439 JT=(trajectory[max].J.Transpose());
1440 //Ss=S;
1441 //Cs=C;
1442 //printf("--------\n");
1443 for (unsigned int m=max-1;m>0;m--){
1444 if (trajectory[m].h_id==0){
1445 A=trajectory[m].Ckk*JT*C.Invert();
1446 Ss=trajectory[m].Skk+A*(Ss-S);
1447 Cs=trajectory[m].Ckk+A*(Cs-C)*A.Transpose();
1448 }
1449 else{
1450 unsigned int id=trajectory[m].h_id-1;
1451 smoothed_updates[id].used_in_fit=false;
1452 //printf("%d:%d used ? %d\n",m,id,updates[id].used_in_fit);
1453 if (updates[id].used_in_fit){
1454 smoothed_updates[id].used_in_fit=true;
1455
1456 A=updates[id].C*JT*C.Invert();
1457 dC=A*(Cs-C)*A.Transpose();
1458 Ss=updates[id].S+A*(Ss-S);
1459 Cs=updates[id].C+dC;
1460
1461 // CDC index and wire position variables
1462 const DCDCWire *wire=hits[id]->wire;
1463 DVector3 origin=wire->origin;
1464 DVector3 wdir=wire->udir;
1465
1466 unsigned int ring=hits[id]->wire->ring-1;
1467 unsigned int straw=hits[id]->wire->straw-1;
1468 UpdateWireOriginAndDir(ring,straw,origin,wdir);
1469
1470 // doca using smoothed state vector
1471 double d=finder->FindDoca(trajectory[m].z,Ss,wdir,origin);
1472 smoothed_updates[id].ring_id=ring;
1473 smoothed_updates[id].straw_id=straw;
1474 smoothed_updates[id].doca=d;
1475 smoothed_updates[id].res=updates[id].drift-d;
1476 smoothed_updates[id].drift=updates[id].drift;
1477 smoothed_updates[id].drift_time=updates[id].drift_time;
1478 smoothed_updates[id].S=Ss;
1479 smoothed_updates[id].C=Cs;
1480 smoothed_updates[id].V=updates[id].V-updates[id].H*dC*updates[id].H_T;
1481 smoothed_updates[id].z=updates[id].z;
1482
1483 // Reset h_id for this position along the reference trajectory
1484 trajectory[m].h_id=0;
1485 }
1486 else{
1487 A=trajectory[m].Ckk*JT*C.Invert();
1488 Ss=trajectory[m].Skk+A*(Ss-S);
1489 Cs=trajectory[m].Ckk+A*(Cs-C)*A.Transpose();
1490 }
1491 }
1492
1493 S=trajectory[m].Skk;
1494 C=trajectory[m].Ckk;
1495 JT=trajectory[m].J.Transpose();
1496 }
1497
1498 A=trajectory[0].Ckk*JT*C.Invert();
1499 Ss=trajectory[0].Skk+A*(Ss-S);
1500 Cs=trajectory[0].Ckk+A*(Cs-C)*A.Transpose();
1501
1502 return NOERROR;
1503}
1504
1505// Perform the Kalman Filter for the current set of cdc hits
1506jerror_t
1507DEventProcessor_dc_alignment::KalmanFilter(double anneal_factor,
1508 DMatrix4x1 &S,DMatrix4x4 &C,
1509 vector<const DCDCTrackHit *>&hits,
1510 deque<trajectory_t>&trajectory,
1511 vector<cdc_update_t>&updates,
1512 double &chi2,unsigned int &ndof,
1513 bool timebased){
1514 DMatrix1x4 H; // Track projection matrix
1515 DMatrix4x1 H_T; // Transpose of track projection matrix
1516 DMatrix4x1 K; // Kalman gain matrix
1517 DMatrix4x4 I; // identity matrix
1518 DMatrix4x4 J; // Jacobian matrix
1519 DMatrix4x1 S0; // State vector from reference trajectory
1520 double V=1.15*(0.78*0.78/12.); // sigma=cell_size/sqrt(12.)*scale_factor
1521
1522 for (unsigned int i=0;i<updates.size();i++){
1523 updates[i].used_in_fit=false;
1524 }
1525
1526 //Initialize chi2 and ndof
1527 chi2=0.;
1528 ndof=0;
1529
1530 double doca2=0.;
1531 const double d_EPS=1e-8;
1532
1533 // CDC index and wire position variables
1534 unsigned int cdc_index=hits.size()-1;
1535 bool more_hits=true;
1536 const DCDCWire *wire=hits[cdc_index]->wire;
1537 DVector3 origin=wire->origin;
1538 double z0=origin.z();
1539 double vz=wire->udir.z();
1540 DVector3 wdir=(1./vz)*wire->udir;
1541
1542 // Wire offsets
1543 unsigned int ring=wire->ring-1;
1544 unsigned int straw=wire->straw-1;
1545 UpdateWireOriginAndDir(ring,straw,origin,wdir);
1546
1547 DVector3 wirepos=origin+(trajectory[0].z-z0)*wdir;
1548
1549 /// compute initial doca^2 to first wire
1550 double dx=S(state_x)-wirepos.X();
1551 double dy=S(state_y)-wirepos.Y();
1552 double old_doca2=dx*dx+dy*dy;
1553
1554 // Loop over all steps in the trajectory
1555 S0=trajectory[0].S;
1556 J=trajectory[0].J;
1557 trajectory[0].Skk=S;
1558 trajectory[0].Ckk=C;
1559 for (unsigned int k=1;k<trajectory.size();k++){
1560 if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.)
1561 return UNRECOVERABLE_ERROR;
1562
1563 // Propagate the state and covariance matrix forward in z
1564 S=trajectory[k].S+J*(S-S0);
1565 C=J*C*J.Transpose();
1566
1567 // Save the current state and covariance matrix
1568 trajectory[k].Skk=S;
1569 trajectory[k].Ckk=C;
1570
1571 // Save S and J for the next step
1572 S0=trajectory[k].S;
1573 J=trajectory[k].J;
1574
1575 // Position along wire
1576 wirepos=origin+(trajectory[k].z-z0)*wdir;
1577
1578 // New doca^2
1579 dx=S(state_x)-wirepos.X();
1580 dy=S(state_y)-wirepos.Y();
1581 doca2=dx*dx+dy*dy;
1582
1583 if (doca2>old_doca2 && more_hits){
1584
1585 // zero-position and direction of line describing particle trajectory
1586 double tx=S(state_tx),ty=S(state_ty);
1587 DVector3 pos0(S(state_x),S(state_y),trajectory[k].z);
1588 DVector3 tdir(tx,ty,1.);
1589
1590 // Find the true doca to the wire
1591 DVector3 diff=pos0-origin;
1592 double dx0=diff.x(),dy0=diff.y();
1593 double wdir_dot_diff=diff.Dot(wdir);
1594 double tdir_dot_diff=diff.Dot(tdir);
1595 double tdir_dot_wdir=tdir.Dot(wdir);
1596 double tdir2=tdir.Mag2();
1597 double wdir2=wdir.Mag2();
1598 double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
1599 double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
1600 double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
1601 double scale=1./D;
1602 double s=scale*N;
1603 double t=scale*N1;
1604 diff+=s*tdir-t*wdir;
1605 double d=diff.Mag()+d_EPS; // prevent division by zero
1606
1607 // The next measurement and its variance
1608 double tdrift=hits[cdc_index]->tdrift-trajectory[k].t;
1609 double dmeas=0.39;
1610 if (timebased){
1611 double drift_var=cdc_variance(tdrift);
1612 V=anneal_factor*drift_var;
1613
1614 double phi_d=diff.Phi();
1615 double dphi=phi_d-origin.Phi();
1616 while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846;
1617 while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846;
1618
1619 int ring_index=hits[cdc_index]->wire->ring-1;
1620 int straw_index=hits[cdc_index]->wire->straw-1;
1621 double dz=t*wdir.z();
1622 double delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.)
1623 *sin(phi_d+sag_phi_offset[ring_index][straw_index]);
1624 dmeas=cdc_drift_distance(dphi,delta,tdrift);
1625
1626 //printf("t %f d %f %f V %f\n",hits[cdc_index]->tdrift,dmeas,d,V);
1627 }
1628
1629 // residual
1630 double res=dmeas-d;
1631
1632 // Track projection
1633 double one_over_d=1./d;
1634 double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
1635 double wx=wdir.x(),wy=wdir.y();
1636
1637 double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0;
1638 double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx;
1639 double dtdtx=scale*(dN1dtx-t*dDdtx);
1640
1641 double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0;
1642 double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy;
1643 double dtdty=scale*(dN1dty-t*dDdty);
1644
1645 double dNdtx=wx*wdir_dot_diff-wdir2*dx0;
1646 double dsdtx=scale*(dNdtx-s*dDdtx);
1647
1648 double dNdty=wy*wdir_dot_diff-wdir2*dy0;
1649 double dsdty=scale*(dNdty-s*dDdty);
1650
1651 H(state_tx)=H_T(state_tx)
1652 =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx)
1653 +diffz*(dsdtx-dtdtx));
1654 H(state_ty)=H_T(state_ty)
1655 =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty)
1656 +diffz*(dsdty-dtdty));
1657
1658 double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx);
1659 double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx);
1660 double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty);
1661 double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty);
1662
1663 H(state_x)=H_T(state_x)
1664 =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy)
1665 +diffz*(dsdx-dtdx));
1666 H(state_y)=H_T(state_y)
1667 =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy)
1668 +diffz*(dsdy-dtdy));
1669
1670 // Matrices to rotate alignment error matrix into measurement space
1671 DMatrix1x4 G;
1672 DMatrix4x1 G_T;
1673 ComputeGMatrices(s,t,scale,tx,ty,tdir2,one_over_d,wx,wy,wdir2,tdir_dot_wdir,
1674 tdir_dot_diff,wdir_dot_diff,dx0,dy0,diffx,diffy,diffz,
1675 G,G_T);
1676
1677 // inverse of variance including prediction
1678 DMatrix4x4 E=cdc_alignments[ring][straw].E;
1679 double Vtemp=V+G*E*G_T;
1680 double InvV=1./(Vtemp+H*C*H_T);
1681
1682 // Compute Kalman gain matrix
1683 K=InvV*(C*H_T);
1684
1685 // Update state vector covariance matrix
1686 DMatrix4x4 Ctest=C-K*(H*C);
1687
1688 //C.Print();
1689 //K.Print();
1690 //Ctest.Print();
1691
1692 // Check that Ctest is positive definite
1693 if (Ctest(0,0)>0.0 && Ctest(1,1)>0.0 && Ctest(2,2)>0.0 && Ctest(3,3)>0.0)
1694 {
1695 C=Ctest;
1696
1697 // Update the state vector
1698 //S=S+res*K;
1699 S+=res*K;
1700
1701 // Compute new residual
1702 d=finder->FindDoca(trajectory[k].z,S,wdir,origin);
1703 res=dmeas-d;
1704
1705 // Update chi2 for this segment
1706 Vtemp-=H*C*H_T;
1707 chi2+=res*res/Vtemp;
1708 ndof++;
1709 }
1710 else{
1711 // _DBG_ << "Bad C!" << endl;
1712 return VALUE_OUT_OF_RANGE;
1713 }
1714
1715 updates[cdc_index].S=S;
1716 updates[cdc_index].C=C;
1717 updates[cdc_index].drift=dmeas;
1718 updates[cdc_index].drift_time=tdrift;
1719 updates[cdc_index].doca=d;
1720 updates[cdc_index].res=res;
1721 updates[cdc_index].V=Vtemp;
1722 updates[cdc_index].H_T=H_T;
1723 updates[cdc_index].H=H;
1724 updates[cdc_index].z=trajectory[k].z;
1725 updates[cdc_index].used_in_fit=true;
1726
1727 trajectory[k].h_id=cdc_index+1;
1728
1729 // move to next cdc hit
1730 if (cdc_index>0){
1731 cdc_index--;
1732
1733 //New wire position
1734 wire=hits[cdc_index]->wire;
1735 origin=wire->origin;
1736 vz=wire->udir.z();
1737 wdir=(1./vz)*wire->udir;
1738
1739 ring=hits[cdc_index]->wire->ring-1;
1740 straw=hits[cdc_index]->wire->straw-1;
1741 UpdateWireOriginAndDir(ring,straw,origin,wdir);
1742
1743 wirepos=origin+((trajectory[k].z-z0))*wdir;
1744
1745 // New doca^2
1746 dx=S(state_x)-wirepos.x();
1747 dy=S(state_y)-wirepos.y();
1748 doca2=dx*dx+dy*dy;
1749
1750 }
1751 else more_hits=false;
1752 }
1753
1754 old_doca2=doca2;
1755 }
1756
1757 ndof-=4;
1758
1759 return NOERROR;
1760}
1761
1762// Perform Kalman Filter for the current trajectory
1763jerror_t
1764DEventProcessor_dc_alignment::KalmanFilter(double anneal_factor,
1765 DMatrix4x1 &S,DMatrix4x4 &C,
1766 vector<const DFDCPseudo *>&hits,
1767 deque<trajectory_t>&trajectory,
1768 vector<update_t>&updates,
1769 double &chi2,unsigned int &ndof){
1770 DMatrix2x4 H; // Track projection matrix
1771 DMatrix4x2 H_T; // Transpose of track projection matrix
1772 DMatrix4x2 K; // Kalman gain matrix
1773 DMatrix2x2 V(0.0008*anneal_factor,0.,0.,0.0008*anneal_factor); // Measurement variance
1774 DMatrix2x2 Vtemp,InvV;
1775 DMatrix2x1 Mdiff;
1776 DMatrix4x4 I; // identity matrix
1777 DMatrix4x4 J; // Jacobian matrix
1778 DMatrix4x1 S0; // State vector from reference trajectory
1779
1780 //Initialize chi2 and ndof
1781 chi2=0.;
1782 ndof=0;
1783
1784 // Loop over all steps in the trajectory
1785 S0=trajectory[0].S;
1786 J=trajectory[0].J;
1787 trajectory[0].Skk=S;
1788 trajectory[0].Ckk=C;
1789 for (unsigned int k=1;k<trajectory.size();k++){
1790 if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.)
1791 return UNRECOVERABLE_ERROR;
1792
1793 // Propagate the state and covariance matrix forward in z
1794 S=trajectory[k].S+J*(S-S0);
1795 C=J*C*J.Transpose();
1796
1797 // Save the current state and covariance matrix
1798 trajectory[k].Skk=S;
1799 trajectory[k].Ckk=C;
1800
1801 // Save S and J for the next step
1802 S0=trajectory[k].S;
1803 J=trajectory[k].J;
1804
1805 // Correct S and C for the hit
1806 if (trajectory[k].h_id>0){
1807 unsigned int id=trajectory[k].h_id-1;
1808
1809 double cosa=hits[id]->wire->udir.y();
1810 double sina=hits[id]->wire->udir.x();
1811
1812 // State vector
1813 double x=S(state_x);
1814 double y=S(state_y);
1815 double tx=S(state_tx);
1816 double ty=S(state_ty);
1817 if (std::isnan(x) || std::isnan(y)) return UNRECOVERABLE_ERROR;
1818
1819 // Get the alignment vector and error matrix for this layer
1820 unsigned int layer=hits[id]->wire->layer-1;
1821 DMatrix2x1 Aw=fdc_alignments[layer].A;
1822 double delta_u=Aw(kU);
1823 double sindphi=sin(Aw(kPhiU));
1824 double cosdphi=cos(Aw(kPhiU));
1825
1826 // Components of rotation matrix for converting global to local coords.
1827 double cospsi=cosa*cosdphi+sina*sindphi;
1828 double sinpsi=sina*cosdphi-cosa*sindphi;
1829
1830 // x,y and tx,ty in local coordinate system
1831 // To transform from (x,y) to (u,v), need to do a rotation:
1832 // u = x*cosa-y*sina
1833 // v = y*cosa+x*sina
1834 // (without alignment offsets)
1835 double vpred_wire_plane=y*cospsi+x*sinpsi;
1836 double upred_wire_plane=x*cospsi-y*sinpsi;
1837 double tu=tx*cospsi-ty*sinpsi;
1838 double tv=tx*sinpsi+ty*cospsi;
1839
1840 // Variables for angle of incidence with respect to the z-direction in
1841 // the u-z plane
1842 double alpha=atan(tu);
1843 double cosalpha=cos(alpha);
1844 double cos2_alpha=cosalpha*cosalpha;
1845 double sinalpha=sin(alpha);
1846 double sin2_alpha=sinalpha*sinalpha;
1847
1848 // Alignment parameters for cathode planes
1849 DMatrix4x4 E=fdc_cathode_alignments[layer].E;
1850 DMatrix4x1 A=fdc_cathode_alignments[layer].A;
1851
1852 // Difference between measurement and projection
1853 for (int m=trajectory[k].num_hits-1;m>=0;m--){
1854 unsigned int my_id=id+m;
1855 double uwire=hits[my_id]->wire->u+delta_u;
1856 // (signed) distance of closest approach to wire
1857 double doca=(upred_wire_plane-uwire)*cosalpha;
1858
1859 // Predicted avalanche position along the wire
1860 double vpred=vpred_wire_plane-tv*sinalpha*doca;
1861
1862 // predicted positions in two cathode planes' coordinate systems
1863 double phi_u=hits[my_id]->phi_u+A(kPhiU);
1864 double phi_v=hits[my_id]->phi_v+A(kPhiV);
1865 double cosphi_u=cos(phi_u);
1866 double sinphi_u=sin(phi_u);
1867 double cosphi_v=cos(phi_v);
1868 double sinphi_v=sin(phi_v);
1869 double vv=-vpred*sinphi_v-uwire*cosphi_v+A(kV);
1870 double vu=-vpred*sinphi_u-uwire*cosphi_u+A(kU);
1871
1872 // Difference between measurements and predictions
1873 Mdiff(0)=hits[my_id]->u-vu;
1874 Mdiff(1)=hits[my_id]->v-vv;
1875
1876 // Start filling the update vector
1877 updates[my_id].drift_time=hits[my_id]->time-trajectory[k].t;
1878
1879 // Matrix for transforming from state-vector space to measurement space
1880 double temp2=tv*sinalpha*cosalpha;
1881 double dvdy=cospsi+sinpsi*temp2;
1882 double dvdx=sinpsi-cospsi*temp2;
1883
1884 H_T(state_x,0)=-dvdx*sinphi_u;
1885 H_T(state_y,0)=-dvdy*sinphi_u;
1886 H_T(state_x,1)=-dvdx*sinphi_v;
1887 H_T(state_y,1)=-dvdy*sinphi_v;
1888
1889 double cos2_minus_sin2=cos2_alpha-sin2_alpha;
1890 double doca_cosalpha=doca*cosalpha;
1891 double dvdtx=-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2);
1892 double dvdty=-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2);
1893
1894 H_T(state_tx,0)=-dvdtx*sinphi_u;
1895 H_T(state_ty,0)=-dvdty*sinphi_u;
1896 H_T(state_tx,1)=-dvdtx*sinphi_v;
1897 H_T(state_ty,1)=-dvdty*sinphi_v;
1898
1899 // Matrix transpose H_T -> H
1900 H(0,state_x)=H_T(state_x,0);
1901 H(0,state_y)=H_T(state_y,0);
1902 H(0,state_tx)=H_T(state_tx,0);
1903 H(0,state_ty)=H_T(state_ty,0);
1904 H(1,state_x)=H_T(state_x,1);
1905 H(1,state_y)=H_T(state_y,1);
1906 H(1,state_tx)=H_T(state_tx,1);
1907 H(1,state_ty)=H_T(state_ty,1);
1908
1909 updates[my_id].H=H;
1910 updates[my_id].H_T=H_T;
1911
1912 // Matrices to rotate alignment error matrix into measurement space
1913 DMatrix2x4 G;
1914 DMatrix4x2 G_T;
1915
1916 G_T(kU,0)=1.;
1917 G_T(kPhiU,0)=-vpred*cosphi_u-uwire*sinphi_u;
1918 G_T(kV,1)=1.;
1919 G_T(kPhiV,1)=-vpred*cosphi_v-uwire*sinphi_v;
1920
1921 // G-matrix transpose
1922 G(0,kU)=G_T(kU,0);
1923 G(0,kPhiU)=G_T(kPhiU,0);
1924 G(1,kV)=G_T(kV,1);
1925 G(1,kPhiV)=G_T(kPhiV,1);
1926
1927 Vtemp=V+G*E*G_T;
1928
1929 // Variance for this hit
1930 InvV=(Vtemp+H*C*H_T).Invert();
1931
1932 // Compute Kalman gain matrix
1933 K=(C*H_T)*InvV;
1934
1935 // Update the state vector
1936 S+=K*Mdiff;
1937
1938 // Update state vector covariance matrix
1939 C=C-K*(H*C);
1940
1941 // Update the filtered measurement covariane matrix and put results in
1942 // update vector
1943 DMatrix2x2 RC=Vtemp-H*C*H_T;
1944 updates[my_id].res=Mdiff-H*K*Mdiff;
1945 updates[my_id].V=RC;
1946 updates[my_id].S=S;
1947 updates[my_id].C=C;
1948
1949 chi2+=RC.Chi2(updates[my_id].res);
1950 ndof+=2;
1951 }
1952
1953 }
1954
1955 }
1956 // chi2*=anneal_factor;
1957 ndof-=4;
1958
1959 return NOERROR;
1960}
1961
1962
1963
1964// Perform Kalman Filter for the current trajectory
1965jerror_t
1966DEventProcessor_dc_alignment::KalmanFilter(double anneal_factor,
1967 DMatrix4x1 &S,DMatrix4x4 &C,
1968 vector<const DFDCPseudo *>&hits,
1969 deque<trajectory_t>&trajectory,
1970 vector<wire_update_t>&updates,
1971 double &chi2,unsigned int &ndof){
1972 DMatrix1x4 H; // Track projection matrix
1973 DMatrix4x1 H_T; // Transpose of track projection matrix
1974 DMatrix4x1 K; // Kalman gain matrix
1975 double V=0.020833; // Measurement variance
1976 double Vtemp,Mdiff,InvV;
1977 DMatrix4x4 I; // identity matrix
1978 DMatrix4x4 J; // Jacobian matrix
1979 DMatrix4x1 S0; // State vector from reference trajectory
1980
1981 //Initialize chi2 and ndof
1982 chi2=0.;
1983 ndof=0;
1984
1985 // Loop over all steps in the trajectory
1986 S0=trajectory[0].S;
1987 J=trajectory[0].J;
1988 trajectory[0].Skk=S;
1989 trajectory[0].Ckk=C;
1990 for (unsigned int k=1;k<trajectory.size();k++){
1991 if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.)
1992 return UNRECOVERABLE_ERROR;
1993
1994 // Propagate the state and covariance matrix forward in z
1995 S=trajectory[k].S+J*(S-S0);
1996 C=J*C*J.Transpose();
1997
1998 // Save the current state and covariance matrix
1999 trajectory[k].Skk=S;
2000 trajectory[k].Ckk=C;
2001
2002 // Save S and J for the next step
2003 S0=trajectory[k].S;
2004 J=trajectory[k].J;
2005
2006 // Correct S and C for the hit
2007 if (trajectory[k].h_id>0){
2008 unsigned int id=trajectory[k].h_id-1;
2009
2010 double cosa=hits[id]->wire->udir.y();
2011 double sina=hits[id]->wire->udir.x();
2012
2013 // State vector
2014 double x=S(state_x);
2015 double y=S(state_y);
2016 double tx=S(state_tx);
2017 double ty=S(state_ty);
2018 if (std::isnan(x) || std::isnan(y)) return UNRECOVERABLE_ERROR;
2019
2020 // Get the alignment vector and error matrix for this layer
2021 unsigned int layer=hits[id]->wire->layer-1;
2022 DMatrix2x2 E=fdc_alignments[layer].E;
2023 DMatrix2x1 A=fdc_alignments[layer].A;
2024 double delta_u=A(kU);
2025 double sindphi=sin(A(kPhiU));
2026 double cosdphi=cos(A(kPhiU));
2027
2028 // Components of rotation matrix for converting global to local coords.
2029 double cospsi=cosa*cosdphi+sina*sindphi;
2030 double sinpsi=sina*cosdphi-cosa*sindphi;
2031
2032 // x,y and tx,ty in local coordinate system
2033 // To transform from (x,y) to (u,v), need to do a rotation:
2034 // u = x*cosa-y*sina
2035 // v = y*cosa+x*sina
2036 // (without alignment offsets)
2037 double upred=x*cospsi-y*sinpsi;
2038 double tu=tx*cospsi-ty*sinpsi;
2039 double tv=tx*sinpsi+ty*cospsi;
2040
2041 // Variables for angle of incidence with respect to the z-direction in
2042 // the u-z plane
2043 double alpha=atan(tu);
2044 double cosalpha=cos(alpha);
2045 double sinalpha=sin(alpha);
2046
2047 // Difference between measurement and projection
2048 for (int m=trajectory[k].num_hits-1;m>=0;m--){
2049 unsigned int my_id=id+m;
2050 double uwire=hits[my_id]->wire->u+delta_u;
2051
2052 // Find drift distance
2053 double drift_time=hits[my_id]->time-trajectory[k].t;
2054 updates[my_id].drift_time=drift_time;
2055 updates[my_id].t=trajectory[k].t;
2056
2057 double du=upred-uwire;
2058 double d=du*cosalpha;
2059 double sign=(du>0)?1.:-1.;
2060
2061 // Difference between measured and predicted vectors
2062 // assume the track passes through the center of the cell
2063 double drift=0.25;
2064 if (USE_DRIFT_TIMES){
2065 drift=0.;
2066 if (drift_time>0){
2067 drift=fdc_drift_distance(drift_time);
2068
2069 //V=0.0004+0.020433*(anneal_factor/1000.);
2070 double sigma=0.0135-3.98e-4*drift_time+5.62e-6*drift_time*drift_time;
2071 V=anneal_factor*sigma*sigma;
2072 }
2073 }
2074 Mdiff=sign*drift-d;
2075 updates[my_id].drift=drift;
2076
2077 // Matrix for transforming from state-vector space to measurement space
2078 double sinalpha_cosalpha=sinalpha*cosalpha;
2079 H_T(state_x)=cospsi*cosalpha;
2080 H_T(state_y)=-sinpsi*cosalpha;
2081
2082 double temp=d*sinalpha_cosalpha;
2083 H_T(state_tx)=-temp*cospsi;
2084 H_T(state_ty)=+temp*sinpsi;
2085
2086 // H-matrix transpose
2087 H(state_x)=H_T(state_x);
2088 H(state_y)=H_T(state_y);
2089
2090 H(state_tx)=H_T(state_tx);
2091 H(state_ty)=H_T(state_ty);
2092
2093 updates[my_id].H=H;
2094 updates[my_id].H_T=H_T;
2095
2096 // Matrices to rotate alignment error matrix into measurement space
2097 DMatrix1x2 G;
2098 DMatrix2x1 G_T;
2099
2100 G_T(kU)=-cosalpha;
2101 G_T(kPhiU)=cosalpha*(x*sinpsi+y*cospsi-tv*d);
2102
2103 // G-matrix transpose
2104 G(kU)=G_T(kU);
2105 G(kPhiU)=G_T(kPhiU);
2106
2107 Vtemp=V+G*E*G_T;
2108
2109 // Variance for this hit
2110 InvV=1./(Vtemp+H*C*H_T);
2111
2112 // Compute Kalman gain matrix
2113 K=InvV*(C*H_T);
2114
2115 // Update the state vector
2116 S+=Mdiff*K;
2117 updates[my_id].S=S;
2118
2119 // Update state vector covariance matrix
2120 C=C-K*(H*C);
2121 updates[my_id].C=C;
2122
2123 // Update chi2 for this trajectory
2124 x=S(state_x);
2125 y=S(state_y);
2126 tx=S(state_tx);
2127 ty=S(state_ty);
2128 upred=x*cospsi-y*sinpsi;
2129 tu=tx*cospsi-ty*sinpsi;
2130
2131 // Variables for angle of incidence with respect to the z-direction in
2132 // the u-z plane
2133 alpha=atan(tu);
2134 cosalpha=cos(alpha);
2135 du=upred-uwire;
2136 d=du*cosalpha;
2137 sinalpha=sin(alpha);
2138
2139 sign=(du>0)?1.:-1.;
2140 Mdiff=sign*drift-d;
2141
2142 double RC=Vtemp-H*C*H_T;
2143 updates[my_id].ures=Mdiff;
2144 updates[my_id].R=RC;
2145
2146 chi2+=Mdiff*Mdiff/RC;
2147 ndof++;
2148 }
2149
2150 }
2151
2152 }
2153 // chi2*=anneal_factor;
2154 ndof-=4;
2155
2156 return NOERROR;
2157}
2158
2159//Reference trajectory for the track for cdc tracks
2160jerror_t DEventProcessor_dc_alignment
2161::SetReferenceTrajectory(double t0,double z,DMatrix4x1 &S,
2162 deque<trajectory_t>&trajectory,
2163 const DCDCTrackHit *last_cdc){
2164 DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
2165
2166 double ds=1.0;
2167 double dz=(S(state_ty)>0.?-1.:1.)*ds/sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty));
2168 double t=t0;
2169
2170 //y-position after which we cut off the loop
2171 double min_y=last_cdc->wire->origin.y()-5.;
2172 unsigned int numsteps=0;
2173 do{
2174 z+=dz;
2175 J(state_x,state_tx)=-dz;
2176 J(state_y,state_ty)=-dz;
2177 // Flight time: assume particle is moving at the speed of light
2178 t+=ds/29.98;
2179 //propagate the state to the next z position
2180 S(state_x)+=S(state_tx)*dz;
2181 S(state_y)+=S(state_ty)*dz;
2182 trajectory.push_front(trajectory_t(z,t0,S,J,Zero4x1,Zero4x4));
2183
2184 numsteps++;
2185 }while (S(state_y)>min_y && numsteps<MAX_STEPS1000);
2186
2187 if (trajectory.size()<2) return UNRECOVERABLE_ERROR;
2188 if (false)
2189 {
2190 printf("Trajectory:\n");
2191 for (unsigned int i=0;i<trajectory.size();i++){
2192 printf(" x %f y %f z %f\n",trajectory[i].S(state_x),
2193 trajectory[i].S(state_y),trajectory[i].z);
2194 }
2195 }
2196
2197
2198
2199
2200 return NOERROR;
2201}
2202
2203
2204// Reference trajectory for the track
2205jerror_t DEventProcessor_dc_alignment
2206::SetReferenceTrajectory(double t0,double z,DMatrix4x1 &S,
2207 deque<trajectory_t>&trajectory,
2208 vector<const DFDCPseudo *>&pseudos){
2209 // Jacobian matrix
2210 DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.);
2211
2212 double dz=1.1;
2213 double t=t0;
2214 trajectory.push_front(trajectory_t(z,t0,S,J,Zero4x1,Zero4x4));
2215
2216 double zhit=z;
2217 double old_zhit=z;
2218 for (unsigned int i=0;i<pseudos.size();i++){
2219 zhit=pseudos[i]->wire->origin.z();
2220 dz=1.1;
2221
2222 if (fabs(zhit-old_zhit)<EPS1e-3){
2223 trajectory[0].num_hits++;
2224 continue;
2225 }
2226 bool done=false;
2227 while (!done){
2228 double new_z=z+dz;
2229
2230 if (new_z>zhit){
2231 dz=zhit-z;
2232 new_z=zhit;
2233 done=true;
2234 }
2235 J(state_x,state_tx)=-dz;
2236 J(state_y,state_ty)=-dz;
2237 // Flight time: assume particle is moving at the speed of light
2238 t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98;
2239 //propagate the state to the next z position
2240 S(state_x)+=S(state_tx)*dz;
2241 S(state_y)+=S(state_ty)*dz;
2242
2243 trajectory.push_front(trajectory_t(new_z,t,S,J,Zero4x1,Zero4x4));
2244 if (done){
2245 trajectory[0].h_id=i+1;
2246 trajectory[0].num_hits=1;
2247 }
2248
2249 z=new_z;
2250 }
2251 old_zhit=zhit;
2252 }
2253 // One last step
2254 dz=1.1;
2255 J(state_x,state_tx)=-dz;
2256 J(state_y,state_ty)=-dz;
2257
2258 // Flight time: assume particle is moving at the speed of light
2259 t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98;
2260
2261 //propagate the state to the next z position
2262 S(state_x)+=S(state_tx)*dz;
2263 S(state_y)+=S(state_ty)*dz;
2264 trajectory.push_front(trajectory_t(z+dz,t,S,J,Zero4x1,Zero4x4));
2265
2266 if (false)
2267 {
2268 printf("Trajectory:\n");
2269 for (unsigned int i=0;i<trajectory.size();i++){
2270 printf(" x %f y %f z %f first hit %d num in layer %d\n",trajectory[i].S(state_x),
2271 trajectory[i].S(state_y),trajectory[i].z,trajectory[i].h_id,
2272 trajectory[i].num_hits);
2273 }
2274 }
2275
2276 return NOERROR;
2277}
2278
2279// Crude approximation for the variance in drift distance due to smearing
2280double DEventProcessor_dc_alignment::GetDriftVariance(double t){
2281 if (t<0) t=0;
2282 else if (t>110.) t=110.;
2283 double sigma=0.01639/sqrt(t+1.)+5.405e-3+4.936e-4*exp(0.09654*(t-66.86));
2284 return sigma*sigma;
2285}
2286
2287// convert time to distance for the fdc
2288double DEventProcessor_dc_alignment::GetDriftDistance(double t){
2289 if (t<0.) return 0.;
2290 double d=0.0268*sqrt(t)/*-3.051e-4*/+7.438e-4*t;
2291 if (d>0.5) d=0.5;
2292 return d;
2293}
2294
2295void DEventProcessor_dc_alignment::UpdateWireOriginAndDir(unsigned int ring,
2296 unsigned int straw,
2297 DVector3 &origin,
2298 DVector3 &wdir){
2299 double zscale=75.0/wdir.z();
2300 DVector3 upstream=origin-zscale*wdir;
2301 DVector3 downstream=origin+zscale*wdir;
2302 DVector3 du(cdc_alignments[ring][straw].A(k_dXu),
2303 cdc_alignments[ring][straw].A(k_dYu),0.);
2304 DVector3 dd(cdc_alignments[ring][straw].A(k_dXd),
2305 cdc_alignments[ring][straw].A(k_dYd),0.);
2306 upstream+=du;
2307 downstream+=dd;
2308
2309 origin=0.5*(upstream+downstream);
2310 wdir=downstream-upstream;
2311 wdir.SetMag(1.);
2312}
2313
2314
2315
2316jerror_t
2317DEventProcessor_dc_alignment::FindOffsets(vector<const DCDCTrackHit*>&hits,
2318 vector<cdc_update_t>&updates){
2319 for (unsigned int i=0;i<updates.size();i++){
2320 if (updates[i].used_in_fit==true){
2321 // wire data
2322 const DCDCWire *wire=hits[i]->wire;
2323 DVector3 origin=wire->origin;
2324 DVector3 wdir=wire->udir;
2325
2326 unsigned int ring=wire->ring-1;
2327 unsigned int straw=wire->straw-1;
2328 UpdateWireOriginAndDir(ring,straw,origin,wdir);
2329
2330 // zero-position and direction of line describing particle trajectory
2331 double tx=updates[i].S(state_tx),ty=updates[i].S(state_ty);
2332 DVector3 pos0(updates[i].S(state_x),updates[i].S(state_y),updates[i].z);
2333 DVector3 diff=pos0-origin;
2334 double dx0=diff.x(),dy0=diff.y();
2335 DVector3 tdir(tx,ty,1.);
2336 double wdir_dot_diff=diff.Dot(wdir);
2337 double tdir_dot_diff=diff.Dot(tdir);
2338 double tdir_dot_wdir=tdir.Dot(wdir);
2339 double tdir2=tdir.Mag2();
2340 double wdir2=wdir.Mag2();
2341 double wx=wdir.x(),wy=wdir.y();
2342 double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir;
2343 double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff;
2344 double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff;
2345 double scale=1./D;
2346 double s=scale*N;
2347 double t=scale*N1;
2348 diff+=s*tdir-t*wdir;
2349 double diffx=diff.x(),diffy=diff.y(),diffz=diff.z();
2350 double one_over_d=1./diff.Mag();
2351
2352 // Matrices to rotate alignment error matrix into measurement space
2353 DMatrix1x4 G;
2354 DMatrix4x1 G_T;
2355 ComputeGMatrices(s,t,scale,tx,ty,tdir2,one_over_d,wx,wy,wdir2,tdir_dot_wdir,
2356 tdir_dot_diff,wdir_dot_diff,dx0,dy0,diffx,diffy,diffz,
2357 G,G_T);
2358
2359 // Offset error matrix
2360 DMatrix4x4 E=cdc_alignments[ring][straw].E;
2361
2362 // Inverse error
2363 double InvV=1./updates[i].V;
2364
2365 // update the alignment vector and covariance
2366 DMatrix4x1 Ka=InvV*(E*G_T);
2367 DMatrix4x1 dA=updates[i].res*Ka;
2368 DMatrix4x4 Etemp=E-Ka*G*E;
2369 //dA.Print();
2370 //Etemp.Print();
2371 if (Etemp(0,0)>0 && Etemp(1,1)>0 && Etemp(2,2)>0&&Etemp(3,3)>0.){
2372 //cdc_alignments[ring][straw].A.Print();
2373 //dA.Print();
2374 //Etemp.Print();
2375 DMatrix4x1 A=cdc_alignments[ring][straw].A+dA;
2376 // Restrict offsets to less than 2 mm
2377 if (fabs(A(k_dXu))<0.2 && fabs(A(k_dXd))<0.2 && fabs(A(k_dYu))<0.2
2378 && fabs(A(k_dYd))<0.2){
2379 cdc_alignments[ring][straw].E=Etemp;
2380 cdc_alignments[ring][straw].A=A;
2381 }
2382 }
2383 }
2384 }
2385
2386 return NOERROR;
2387}
2388
2389
2390jerror_t
2391DEventProcessor_dc_alignment::FindOffsets(vector<const DFDCPseudo *>&hits,
2392 vector<update_t>&smoothed_updates){
2393 DMatrix2x4 G;//matrix relating alignment vector to measurement coords
2394 DMatrix4x2 G_T; // .. and its transpose
2395
2396 unsigned int num_hits=hits.size();
2397
2398 for (unsigned int i=0;i<num_hits;i++){
2399 // Get the cathode planes aligment vector and error matrix for this layer
2400 unsigned int layer=hits[i]->wire->layer-1;
2401 DMatrix4x1 A=fdc_cathode_alignments[layer].A;
2402 DMatrix4x4 E=fdc_cathode_alignments[layer].E;
2403
2404 // Rotation of wire planes
2405 double cosa=hits[i]->wire->udir.y();
2406 double sina=hits[i]->wire->udir.x();
2407
2408 // State vector
2409 DMatrix4x1 S=smoothed_updates[i].S;
2410 double x=S(state_x);
2411 double y=S(state_y);
2412 double tx=S(state_tx);
2413 double ty=S(state_ty);
2414 if (std::isnan(x) || std::isnan(y)) return UNRECOVERABLE_ERROR;
2415
2416 // Get the wire plane alignment vector and error matrix for this layer
2417 DMatrix2x1 Aw=fdc_alignments[layer].A;
2418 double delta_u=Aw(kU);
2419 double sindphi=sin(Aw(kPhiU));
2420 double cosdphi=cos(Aw(kPhiU));
2421
2422 // Components of rotation matrix for converting global to local coords.
2423 double cospsi=cosa*cosdphi+sina*sindphi;
2424 double sinpsi=sina*cosdphi-cosa*sindphi;
2425
2426 // x,y and tx,ty in local coordinate system
2427 // To transform from (x,y) to (u,v), need to do a rotation:
2428 // u = x*cosa-y*sina
2429 // v = y*cosa+x*sina
2430 // (without alignment offsets)
2431 double vpred_wire_plane=y*cospsi+x*sinpsi;
2432 double upred_wire_plane=x*cospsi-y*sinpsi;
2433 double tu=tx*cospsi-ty*sinpsi;
2434 double tv=tx*sinpsi+ty*cospsi;
2435 double alpha=atan(tu);
2436 double cosalpha=cos(alpha);
2437 double sinalpha=sin(alpha);
2438
2439 // Wire position in wire-plane local coordinate system
2440 double uwire=hits[i]->wire->u+delta_u;
2441 // (signed) distance of closest approach to wire
2442 double doca=(upred_wire_plane-uwire)*cosalpha;
2443
2444 // Predicted avalanche position along the wire
2445 double vpred=vpred_wire_plane-tv*sinalpha*doca;
2446
2447 // Matrices to rotate alignment error matrix into measurement space
2448 DMatrix2x4 G;
2449 DMatrix4x2 G_T;
2450
2451 double phi_u=hits[i]->phi_u+A(kPhiU);
2452 double phi_v=hits[i]->phi_v+A(kPhiV);
2453
2454 G_T(kU,0)=1.;
2455 G_T(kPhiU,0)=-vpred*cos(phi_u)-uwire*sin(phi_u);
2456 G_T(kV,1)=1.;
2457 G_T(kPhiV,1)=-vpred*cos(phi_v)-uwire*sin(phi_v);
2458
2459 // update the alignment vector and covariance
2460 DMatrix4x2 Ka=(E*G_T)*smoothed_updates[i].V.Invert();
2461 DMatrix4x1 dA=Ka*smoothed_updates[i].res;
2462 DMatrix4x4 Etemp=E-Ka*G*E;
2463 if (Etemp(0,0)>0 && Etemp(1,1)>0 && Etemp(2,2)>0 && Etemp(3,3)>0){
2464 fdc_cathode_alignments[layer].E=Etemp;
2465 fdc_cathode_alignments[layer].A=A+dA;
2466 }
2467 else {
2468 /*
2469 printf("-------t= %f\n",smoothed_updates[i].drift_time);
2470 E.Print();
2471 Etemp.Print();
2472 */
2473 }
2474 }
2475
2476 return NOERROR;
2477}
2478
2479jerror_t
2480DEventProcessor_dc_alignment::FindOffsets(vector<const DFDCPseudo *>&hits,
2481 vector<wire_update_t>&smoothed_updates){
2482 DMatrix1x2 G;//matrix relating alignment vector to measurement coords
2483 DMatrix2x1 G_T; // .. and its transpose
2484
2485 unsigned int num_hits=hits.size();
2486
2487
2488 for (unsigned int i=0;i<num_hits;i++){
2489 double x=smoothed_updates[i].S(state_x);
2490 double y=smoothed_updates[i].S(state_y);
2491 double tx=smoothed_updates[i].S(state_tx);
2492 double ty=smoothed_updates[i].S(state_ty);
2493
2494 double cosa=hits[i]->wire->udir.y();
2495 double sina=hits[i]->wire->udir.x();
2496
2497 // Get the aligment vector and error matrix for this layer
2498 unsigned int layer=hits[i]->wire->layer-1;
2499 DMatrix2x1 A=fdc_alignments[layer].A;
2500 DMatrix2x2 E=fdc_alignments[layer].E;
2501 double delta_u=A(kU);
2502 double sindphi=sin(A(kPhiU));
2503 double cosdphi=cos(A(kPhiU));
2504
2505 // Components of rotation matrix for converting global to local coords.
2506 double cospsi=cosa*cosdphi+sina*sindphi;
2507 double sinpsi=sina*cosdphi-cosa*sindphi;
2508
2509 // x,y and tx,ty in local coordinate system
2510 // To transform from (x,y) to (u,v), need to do a rotation:
2511 // u = x*cosa-y*sina
2512 // v = y*cosa+x*sina
2513 // (without alignment offsets)
2514 double uwire=hits[i]->wire->u+delta_u;
2515 double upred=x*cospsi-y*sinpsi;
2516 double tu=tx*cospsi-ty*sinpsi;
2517 double tv=tx*sinpsi+ty*cospsi;
2518 double du=upred-uwire;
2519
2520 // Variables for angle of incidence with respect to the z-direction in
2521 // the u-z plane
2522 double alpha=atan(tu);
2523 double cosalpha=cos(alpha);
2524
2525 // Transform from alignment vector coords to measurement coords
2526 G_T(kU)=-cosalpha;
2527
2528 double d=du*cosalpha;
2529 G_T(kPhiU)=cosalpha*(x*sinpsi+y*cospsi-tv*d);
2530
2531 // G-matrix transpose
2532 G(kU)=G_T(kU);
2533 G(kPhiU)=G_T(kPhiU);
2534
2535 // Inverse of error "matrix"
2536 double InvV=1./smoothed_updates[i].R;
2537
2538 // update the alignment vector and covariance
2539 DMatrix2x1 Ka=InvV*(E*G_T);
2540 DMatrix2x1 dA=smoothed_updates[i].ures*Ka;
2541 DMatrix2x2 Etemp=E-Ka*G*E;
2542 if (Etemp(0,0)>0 && Etemp(1,1)>0){
2543 fdc_alignments[layer].E=Etemp;
2544 fdc_alignments[layer].A=A+dA;
2545 }
2546 else {
2547 /*
2548 printf("-------t= %f\n",smoothed_updates[i].drift_time);
2549 E.Print();
2550 Etemp.Print();
2551 */
2552 }
2553 }
2554
2555 return NOERROR;
2556}
2557
2558// Compute matrices for rotating the aligment error matrix into the measurement
2559// space
2560void
2561DEventProcessor_dc_alignment::ComputeGMatrices(double s,double t,double scale,
2562 double tx,double ty,double tdir2,
2563 double one_over_d,
2564 double wx,double wy,double wdir2,
2565 double tdir_dot_wdir,
2566 double tdir_dot_diff,
2567 double wdir_dot_diff,
2568 double dx0,double dy0,
2569 double diffx,double diffy,
2570 double diffz,
2571 DMatrix1x4 &G,DMatrix4x1 &G_T){
2572 double dsdDx=scale*(tdir_dot_wdir*wx-wdir2*tx);
2573 double dsdDy=scale*(tdir_dot_wdir*wy-wdir2*ty);
2574
2575 double dNdvx=tx*wdir_dot_diff+tdir_dot_wdir*dx0-2.*wx*tdir_dot_diff;
2576 double dDdvx=2.*wx*tdir2-2.*tdir_dot_wdir*tx;
2577 double dsdvx=scale*(dNdvx-s*dDdvx);
2578
2579 double dNdvy=ty*wdir_dot_diff+tdir_dot_wdir*dy0-2.*wy*tdir_dot_diff;
2580 double dDdvy=2.*wy*tdir2-2.*tdir_dot_wdir*ty;;
2581 double dsdvy=scale*(dNdvy-s*dDdvy);
2582
2583 double dsddxu=-0.5*dsdDx-one_over_zrange*dsdvx;
2584 double dsddxd=-0.5*dsdDx+one_over_zrange*dsdvx;
2585 double dsddyu=-0.5*dsdDy-one_over_zrange*dsdvy;
2586 double dsddyd=-0.5*dsdDy+one_over_zrange*dsdvy;
2587
2588 double dtdDx=scale*(tdir2*wx-tdir_dot_wdir*tx);
2589 double dtdDy=scale*(tdir2*wy-tdir_dot_wdir*ty);
2590
2591 double dN1dvx=tdir2*dx0-tdir_dot_diff*tx;
2592 double dtdvx=scale*(dN1dvx-t*dDdvx);
2593
2594 double dN1dvy=tdir2*dy0-tdir_dot_diff*ty;
2595 double dtdvy=scale*(dN1dvy-t*dDdvy);
2596
2597 double dtddxu=-0.5*dtdDx-one_over_zrange*dtdvx;
2598 double dtddxd=-0.5*dtdDx+one_over_zrange*dtdvx;
2599 double dtddyu=-0.5*dtdDy-one_over_zrange*dtdvy;
2600 double dtddyd=-0.5*dtdDy+one_over_zrange*dtdvy;
2601
2602 double t_over_zrange=one_over_zrange*t;
2603 G(k_dXu)=one_over_d*(diffx*(-0.5+tx*dsddxu+t_over_zrange-wx*dtddxu)
2604 +diffy*(ty*dsddxu-wy*dtddxu)+diffz*(dsddxu-dtddxu));
2605 G(k_dXd)=one_over_d*(diffx*(-0.5+tx*dsddxd-t_over_zrange-wx*dtddxd)
2606 +diffy*(ty*dsddxd-wy*dtddxd)+diffz*(dsddxd-dtddxd));
2607 G(k_dYu)=one_over_d*(diffx*(tx*dsddyu-wx*dtddyu)+diffz*(dsddyu-dtddyu)
2608 +diffy*(-0.5+ty*dsddyu+t_over_zrange-wy*dtddyu));
2609 G(k_dYd)=one_over_d*(diffx*(tx*dsddyd-wx*dtddyd)+diffz*(dsddyd-dtddyd)
2610 +diffy*(-0.5+ty*dsddyd-t_over_zrange-wy*dtddyd));
2611 G_T(k_dXu)=G(k_dXu);
2612 G_T(k_dXd)=G(k_dXd);
2613 G_T(k_dYu)=G(k_dYu);
2614 G_T(k_dYd)=G(k_dYd);
2615}
2616
2617// If the event viewer is available, grab parts of the hdview2 display and
2618// overlay the results of the line fit on the tracking views.
2619void DEventProcessor_dc_alignment::PlotLines(deque<trajectory_t>&traj){
2620 unsigned int last_index=traj.size()-1;
2621
2622 TCanvas *c1=dynamic_cast<TCanvas *>(gROOT->FindObject("endviewA Canvas"));
2623 if (c1!=NULL__null){
2624 c1->cd();
2625 TPolyLine *line=new TPolyLine();
2626
2627 line->SetLineColor(1);
2628 line->SetLineWidth(1);
2629
2630 line->SetNextPoint(traj[last_index].S(state_x),traj[last_index].S(state_y));
2631 line->SetNextPoint(traj[0].S(state_x),traj[0].S(state_y));
2632 line->Draw();
2633
2634 c1->Update();
2635
2636 delete line;
2637 }
2638
2639 c1=dynamic_cast<TCanvas *>(gROOT->FindObject("endviewA Large Canvas"));
2640 if (c1!=NULL__null){
2641 c1->cd();
2642 TPolyLine *line=new TPolyLine();
2643
2644 line->SetLineColor(1);
2645 line->SetLineWidth(1);
2646
2647 line->SetNextPoint(traj[last_index].S(state_x),traj[last_index].S(state_y));
2648 line->SetNextPoint(traj[0].S(state_x),traj[0].S(state_y));
2649 line->Draw();
2650
2651 c1->Update();
2652
2653 delete line;
2654 }
2655
2656 c1=dynamic_cast<TCanvas *>(gROOT->FindObject("sideviewA Canvas"));
2657 if (c1!=NULL__null){
2658 c1->cd();
2659 TPolyLine *line=new TPolyLine();
2660
2661 line->SetLineColor(1);
2662 line->SetLineWidth(1);
2663
2664 line->SetNextPoint(traj[last_index].z,traj[last_index].S(state_x));
2665 line->SetNextPoint(traj[0].z,traj[0].S(state_x));
2666 line->Draw();
2667
2668 c1->Update();
2669
2670 delete line;
2671 }
2672
2673 c1=dynamic_cast<TCanvas *>(gROOT->FindObject("sideviewB Canvas"));
2674 if (c1!=NULL__null){
2675 c1->cd();
2676 TPolyLine *line=new TPolyLine();
2677
2678 line->SetLineColor(1);
2679 line->SetLineWidth(1);
2680
2681 line->SetNextPoint(traj[last_index].z,traj[last_index].S(state_y));
2682 line->SetNextPoint(traj[0].z,traj[0].S(state_y));
2683 line->Draw();
2684
2685 c1->Update();
2686 delete line;
2687 }
2688 // end of drawing code
2689
2690}