| File: | /home/sdobbs/work/clang/halld_recon/src/libraries/TRACKING/DTrackFitterStraightTrack.cc |
| Warning: | line 2156, column 7 Value stored to 'z' is never read |
Press '?' to see keyboard shortcuts
Keyboard shortcuts:
| 1 | // $Id$ |
| 2 | // |
| 3 | // File: DTrackFitterStraightTrack.cc |
| 4 | // Created: Tue Mar 12 10:22:32 EDT 2019 |
| 5 | // Creator: staylor (on Linux ifarm1402.jlab.org 3.10.0-327.el7.x86_64 x86_64) |
| 6 | // |
| 7 | |
| 8 | #include "DTrackFitterStraightTrack.h" |
| 9 | |
| 10 | //--------------------------------- |
| 11 | // DTrackFitterStraightTrack (Constructor) |
| 12 | //--------------------------------- |
| 13 | DTrackFitterStraightTrack::DTrackFitterStraightTrack(JEventLoop *loop):DTrackFitter(loop){ |
| 14 | int runnumber=(loop->GetJEvent()).GetRunNumber(); |
| 15 | DApplication* dapp = dynamic_cast<DApplication*>(loop->GetJApplication()); |
| 16 | JCalibration *jcalib = dapp->GetJCalibration(runnumber); |
| 17 | const DGeometry *geom = dapp->GetDGeometry(runnumber); |
| 18 | |
| 19 | // Get pointer to TrackFinder object |
| 20 | vector<const DTrackFinder *> finders; |
| 21 | loop->Get(finders); |
| 22 | |
| 23 | // Drop the const qualifier from the DTrackFinder pointer |
| 24 | finder = const_cast<DTrackFinder*>(finders[0]); |
| 25 | |
| 26 | // Resource pool for matrices |
| 27 | dResourcePool_TMatrixFSym = std::make_shared<DResourcePool<TMatrixFSym>> |
| 28 | (); |
| 29 | dResourcePool_TMatrixFSym->Set_ControlParams(20, 20, 50); |
| 30 | |
| 31 | //**************** |
| 32 | // FDC parameters |
| 33 | //**************** |
| 34 | |
| 35 | map<string,double>drift_res_parms; |
| 36 | jcalib->Get("FDC/drift_resolution_parms",drift_res_parms); |
| 37 | DRIFT_RES_PARMS[0]=drift_res_parms["p0"]; |
| 38 | DRIFT_RES_PARMS[1]=drift_res_parms["p1"]; |
| 39 | DRIFT_RES_PARMS[2]=drift_res_parms["p2"]; |
| 40 | |
| 41 | // Time-to-distance function parameters for FDC |
| 42 | map<string,double>drift_func_parms; |
| 43 | jcalib->Get("FDC/drift_function_parms",drift_func_parms); |
| 44 | DRIFT_FUNC_PARMS[0]=drift_func_parms["p0"]; |
| 45 | DRIFT_FUNC_PARMS[1]=drift_func_parms["p1"]; |
| 46 | DRIFT_FUNC_PARMS[2]=drift_func_parms["p2"]; |
| 47 | DRIFT_FUNC_PARMS[3]=drift_func_parms["p3"]; |
| 48 | DRIFT_FUNC_PARMS[4]=1000.; |
| 49 | DRIFT_FUNC_PARMS[5]=0.; |
| 50 | map<string,double>drift_func_ext; |
| 51 | if (jcalib->Get("FDC/drift_function_ext",drift_func_ext)==false){ |
| 52 | DRIFT_FUNC_PARMS[4]=drift_func_ext["p4"]; |
| 53 | DRIFT_FUNC_PARMS[5]=drift_func_ext["p5"]; |
| 54 | } |
| 55 | |
| 56 | PLANE_TO_SKIP=0; |
| 57 | gPARMS->SetDefaultParameter("TRKFIT:PLANE_TO_SKIP",PLANE_TO_SKIP); |
| 58 | |
| 59 | //**************** |
| 60 | // CDC parameters |
| 61 | //**************** |
| 62 | RING_TO_SKIP=0; |
| 63 | gPARMS->SetDefaultParameter("TRKFIT:RING_TO_SKIP",RING_TO_SKIP); |
| 64 | |
| 65 | map<string, double> cdc_res_parms; |
| 66 | jcalib->Get("CDC/cdc_resolution_parms_v2", cdc_res_parms); |
| 67 | CDC_RES_PAR1 = cdc_res_parms["res_par1"]; |
| 68 | CDC_RES_PAR2 = cdc_res_parms["res_par2"]; |
| 69 | CDC_RES_PAR3 = cdc_res_parms["res_par3"]; |
| 70 | |
| 71 | vector< map<string, double> > tvals; |
| 72 | cdc_drift_table.clear(); |
| 73 | if (jcalib->Get("CDC/cdc_drift_table", tvals)==false){ |
| 74 | for(unsigned int i=0; i<tvals.size(); i++){ |
| 75 | map<string, double> &row = tvals[i]; |
| 76 | cdc_drift_table.push_back(1000.*row["t"]); |
| 77 | } |
| 78 | } |
| 79 | else{ |
| 80 | jerr << " CDC time-to-distance table not available... bailing..." << endl; |
| 81 | exit(0); |
| 82 | } |
| 83 | |
| 84 | if (jcalib->Get("CDC/drift_parameters", tvals)==false){ |
| 85 | map<string, double> &row = tvals[0]; //long drift side |
| 86 | long_drift_func[0][0]=row["a1"]; |
| 87 | long_drift_func[0][1]=row["a2"]; |
| 88 | long_drift_func[0][2]=row["a3"]; |
| 89 | long_drift_func[1][0]=row["b1"]; |
| 90 | long_drift_func[1][1]=row["b2"]; |
| 91 | long_drift_func[1][2]=row["b3"]; |
| 92 | long_drift_func[2][0]=row["c1"]; |
| 93 | long_drift_func[2][1]=row["c2"]; |
| 94 | long_drift_func[2][2]=row["c3"]; |
| 95 | |
| 96 | row = tvals[1]; // short drift side |
| 97 | short_drift_func[0][0]=row["a1"]; |
| 98 | short_drift_func[0][1]=row["a2"]; |
| 99 | short_drift_func[0][2]=row["a3"]; |
| 100 | short_drift_func[1][0]=row["b1"]; |
| 101 | short_drift_func[1][1]=row["b2"]; |
| 102 | short_drift_func[1][2]=row["b3"]; |
| 103 | short_drift_func[2][0]=row["c1"]; |
| 104 | short_drift_func[2][1]=row["c2"]; |
| 105 | short_drift_func[2][2]=row["c3"]; |
| 106 | } |
| 107 | |
| 108 | // Get the straw sag parameters from the database |
| 109 | max_sag.clear(); |
| 110 | sag_phi_offset.clear(); |
| 111 | unsigned int numstraws[28]={42,42,54,54,66,66,80,80,93,93,106,106,123,123, |
| 112 | 135,135,146,146,158,158,170,170,182,182,197,197,209,209}; |
| 113 | unsigned int straw_count=0,ring_count=0; |
| 114 | if (jcalib->Get("CDC/sag_parameters", tvals)==false){ |
| 115 | vector<double>temp,temp2; |
| 116 | for(unsigned int i=0; i<tvals.size(); i++){ |
| 117 | map<string, double> &row = tvals[i]; |
| 118 | |
| 119 | temp.push_back(row["offset"]); |
| 120 | temp2.push_back(row["phi"]); |
| 121 | |
| 122 | straw_count++; |
| 123 | if (straw_count==numstraws[ring_count]){ |
| 124 | max_sag.push_back(temp); |
| 125 | sag_phi_offset.push_back(temp2); |
| 126 | temp.clear(); |
| 127 | temp2.clear(); |
| 128 | straw_count=0; |
| 129 | ring_count++; |
| 130 | } |
| 131 | } |
| 132 | } |
| 133 | |
| 134 | // CDC Geometry |
| 135 | vector<double>cdc_origin; |
| 136 | vector<double>cdc_center; |
| 137 | vector<double>cdc_upstream_endplate_pos; |
| 138 | vector<double>cdc_endplate_dim; |
| 139 | geom->Get("//posXYZ[@volume='CentralDC'/@X_Y_Z",cdc_origin); |
| 140 | geom->Get("//posXYZ[@volume='centralDC']/@X_Y_Z",cdc_center); |
| 141 | geom->Get("//posXYZ[@volume='CDPU']/@X_Y_Z",cdc_upstream_endplate_pos); |
| 142 | geom->Get("//tubs[@name='CDPU']/@Rio_Z",cdc_endplate_dim); |
| 143 | cdc_origin[2]+=cdc_center[2]+cdc_upstream_endplate_pos[2] |
| 144 | +0.5*cdc_endplate_dim[2]; |
| 145 | upstreamEndplate=cdc_origin[2]; |
| 146 | |
| 147 | double endplate_dz=0.; |
| 148 | geom->GetCDCEndplate(downstreamEndplate,endplate_dz,cdc_endplate_rmin, |
| 149 | cdc_endplate_rmax); |
| 150 | downstreamEndplate-=0.5*endplate_dz; |
| 151 | |
| 152 | // Outer detector geometry parameters |
| 153 | if (geom->GetDIRCZ(dDIRCz)==false) dDIRCz=1000.; |
| 154 | geom->GetFCALZ(dFCALz); |
| 155 | vector<double>tof_face; |
| 156 | geom->Get("//section/composition/posXYZ[@volume='ForwardTOF']/@X_Y_Z", |
| 157 | tof_face); |
| 158 | vector<double>tof_plane; |
| 159 | geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='0']", tof_plane); |
| 160 | dTOFz=tof_face[2]+tof_plane[2]; |
| 161 | geom->Get("//composition[@name='ForwardTOF']/posXYZ[@volume='forwardTOF']/@X_Y_Z/plane[@value='1']", tof_plane); |
| 162 | dTOFz+=tof_face[2]+tof_plane[2]; |
| 163 | dTOFz*=0.5; // mid plane between tof planes |
| 164 | geom->GetTRDZ(dTRDz_vec); // TRD planes |
| 165 | |
| 166 | // Get start counter geometry; |
| 167 | if (geom->GetStartCounterGeom(sc_pos,sc_norm)){ |
| 168 | // Create vector of direction vectors in scintillator planes |
| 169 | for (int i=0;i<30;i++){ |
| 170 | vector<DVector3>temp; |
| 171 | for (unsigned int j=0;j<sc_pos[i].size()-1;j++){ |
| 172 | double dx=sc_pos[i][j+1].x()-sc_pos[i][j].x(); |
| 173 | double dy=sc_pos[i][j+1].y()-sc_pos[i][j].y(); |
| 174 | double dz=sc_pos[i][j+1].z()-sc_pos[i][j].z(); |
| 175 | temp.push_back(DVector3(dx/dz,dy/dz,1.)); |
| 176 | } |
| 177 | sc_dir.push_back(temp); |
| 178 | } |
| 179 | SC_END_NOSE_Z=sc_pos[0][12].z(); |
| 180 | SC_BARREL_R=sc_pos[0][0].Perp(); |
| 181 | SC_PHI_SECTOR1=sc_pos[0][0].Phi(); |
| 182 | } |
| 183 | |
| 184 | |
| 185 | //************************* |
| 186 | // Command-line parameters |
| 187 | //************************* |
| 188 | VERBOSE=0; |
| 189 | gPARMS->SetDefaultParameter("TRKFIT:VERBOSE",VERBOSE); |
| 190 | COSMICS=false; |
| 191 | gPARMS->SetDefaultParameter("TRKFIND:COSMICS",COSMICS); |
| 192 | CHI2CUT = 15.0; |
| 193 | gPARMS->SetDefaultParameter("TRKFIT:CHI2CUT",CHI2CUT); |
| 194 | DO_PRUNING = true; |
| 195 | gPARMS->SetDefaultParameter("TRKFIT:DO_PRUNING",DO_PRUNING); |
| 196 | } |
| 197 | |
| 198 | //--------------------------------- |
| 199 | // ~DTrackFitterStraightTrack (Destructor) |
| 200 | //--------------------------------- |
| 201 | DTrackFitterStraightTrack::~DTrackFitterStraightTrack() |
| 202 | { |
| 203 | |
| 204 | } |
| 205 | |
| 206 | //---------------------------------------------------- |
| 207 | // Comparison routines for sorting |
| 208 | //---------------------------------------------------- |
| 209 | bool DTrackFitterStraightTrack_cdc_hit_cmp(const DCDCTrackHit *a, |
| 210 | const DCDCTrackHit *b){ |
| 211 | |
| 212 | return(a->wire->origin.Y()>b->wire->origin.Y()); |
| 213 | } |
| 214 | |
| 215 | bool DTrackFitterStraightTrack_cdc_hit_reverse_cmp(const DCDCTrackHit *a, |
| 216 | const DCDCTrackHit *b){ |
| 217 | |
| 218 | return(a->wire->origin.Y()<b->wire->origin.Y()); |
| 219 | } |
| 220 | |
| 221 | bool DTrackFitterStraightTrack_cdc_hit_radius_cmp(const DCDCTrackHit *a, |
| 222 | const DCDCTrackHit *b){ |
| 223 | if (a==NULL__null || b==NULL__null){ |
| 224 | cout << "Null pointer in CDC hit list??" << endl; |
| 225 | return false; |
| 226 | } |
| 227 | const DCDCWire *wire_a= a->wire; |
| 228 | const DCDCWire *wire_b= b->wire; |
| 229 | if(wire_a->ring == wire_b->ring){ |
| 230 | return wire_a->straw < wire_b->straw; |
| 231 | } |
| 232 | |
| 233 | return (wire_a->ring<wire_b->ring); |
| 234 | } |
| 235 | |
| 236 | bool DTrackFitterStraightTrack_fdc_hit_cmp(const DFDCPseudo *a, |
| 237 | const DFDCPseudo *b){ |
| 238 | |
| 239 | return(a->wire->origin.z()<b->wire->origin.z()); |
| 240 | } |
| 241 | |
| 242 | //---------------------------------------------------- |
| 243 | // CDC fitting routines |
| 244 | //---------------------------------------------------- |
| 245 | |
| 246 | // Smearing function derived from fitting residuals |
| 247 | inline double DTrackFitterStraightTrack::CDCDriftVariance(double t) const { |
| 248 | if (t<0.) t=0.; |
| 249 | double sigma=CDC_RES_PAR1/(t+1.)+CDC_RES_PAR2 + CDC_RES_PAR3*t; |
| 250 | return sigma*sigma; |
| 251 | } |
| 252 | |
| 253 | // Convert time to distance for the cdc. Also returns the variance in d. |
| 254 | void DTrackFitterStraightTrack::CDCDriftParameters(double dphi,double delta, |
| 255 | double t, double &d, |
| 256 | double &V) const{ |
| 257 | |
| 258 | // NSJ 26 May 2020 included long side a3, b3 and short side c1, c2, c3 |
| 259 | // Previously these parameters were not used (0 in ccdb) for production runs |
| 260 | // except intensity scan run 72312 by accident 5 May 2020, superseded 8 May. |
| 261 | // They were used in 2015 for runs 0-3050. |
| 262 | |
| 263 | d=0.; |
| 264 | double dd_dt=0; |
| 265 | if (t>0){ |
| 266 | double f_0=0.; |
| 267 | double f_delta=0.; |
| 268 | |
| 269 | if (delta>0){ |
| 270 | double a1=long_drift_func[0][0]; |
| 271 | double a2=long_drift_func[0][1]; |
| 272 | double a3=long_drift_func[0][2]; |
| 273 | double b1=long_drift_func[1][0]; |
| 274 | double b2=long_drift_func[1][1]; |
| 275 | double b3=long_drift_func[1][2]; |
| 276 | double c1=long_drift_func[2][0]; |
| 277 | double c2=long_drift_func[2][1]; |
| 278 | double c3=long_drift_func[2][2]; |
| 279 | |
| 280 | // use "long side" functional form |
| 281 | double my_t=0.001*t; |
| 282 | double sqrt_t=sqrt(my_t); |
| 283 | double t3=my_t*my_t*my_t; |
| 284 | double delta_mag=fabs(delta); |
| 285 | |
| 286 | double delta_sq=delta*delta; |
| 287 | double a=a1+a2*delta_mag+a3*delta_sq; |
| 288 | double b=b1+b2*delta_mag+b3*delta_sq; |
| 289 | double c=c1+c2*delta_mag+c3*delta_sq; |
| 290 | |
| 291 | f_delta=a*sqrt_t+b*my_t+c*t3; |
| 292 | f_0=a1*sqrt_t+b1*my_t+c1*t3; |
| 293 | |
| 294 | dd_dt=0.001*(0.5*a/sqrt_t+b+3.*c*my_t*my_t); |
| 295 | |
| 296 | } |
| 297 | else{ |
| 298 | double my_t=0.001*t; |
| 299 | double sqrt_t=sqrt(my_t); |
| 300 | double t3=my_t*my_t*my_t; |
| 301 | double delta_mag=fabs(delta); |
| 302 | |
| 303 | // use "short side" functional form |
| 304 | double a1=short_drift_func[0][0]; |
| 305 | double a2=short_drift_func[0][1]; |
| 306 | double a3=short_drift_func[0][2]; |
| 307 | double b1=short_drift_func[1][0]; |
| 308 | double b2=short_drift_func[1][1]; |
| 309 | double b3=short_drift_func[1][2]; |
| 310 | double c1=short_drift_func[2][0]; |
| 311 | double c2=short_drift_func[2][1]; |
| 312 | double c3=short_drift_func[2][2]; |
| 313 | |
| 314 | double delta_sq=delta*delta; |
| 315 | double a=a1+a2*delta_mag+a3*delta_sq; |
| 316 | double b=b1+b2*delta_mag+b3*delta_sq; |
| 317 | double c=c1+c2*delta_mag+c3*delta_sq; |
| 318 | |
| 319 | f_delta=a*sqrt_t+b*my_t+c*t3; |
| 320 | f_0=a1*sqrt_t+b1*my_t+c1*t3; |
| 321 | |
| 322 | dd_dt=0.001*(0.5*a/sqrt_t+b+3.*c*my_t*my_t); |
| 323 | |
| 324 | } |
| 325 | |
| 326 | unsigned int max_index=cdc_drift_table.size()-1; |
| 327 | if (t>cdc_drift_table[max_index]){ |
| 328 | //_DBG_ << "t: " << t <<" d " << f_delta <<endl; |
| 329 | d=f_delta; |
| 330 | } |
| 331 | |
| 332 | // Drift time is within range of table -- interpolate... |
| 333 | unsigned int index=0; |
| 334 | index=Locate(cdc_drift_table,t); |
| 335 | double dt=cdc_drift_table[index+1]-cdc_drift_table[index]; |
| 336 | double frac=(t-cdc_drift_table[index])/dt; |
| 337 | double d_0=0.01*(double(index)+frac); |
| 338 | |
| 339 | double P=0.; |
| 340 | double tcut=250.0; // ns |
| 341 | if (t<tcut) { |
| 342 | P=(tcut-t)/tcut; |
| 343 | } |
| 344 | d=f_delta*(d_0/f_0*P+1.-P); |
| 345 | } |
| 346 | double VarMs=0.001; // kludge for material effects |
| 347 | V=CDCDriftVariance(t)+mVarT0*dd_dt*dd_dt+VarMs; |
| 348 | } |
| 349 | |
| 350 | // Perform the Kalman Filter for the current set of cdc hits |
| 351 | jerror_t DTrackFitterStraightTrack::KalmanFilter(DMatrix4x1 &S,DMatrix4x4 &C, |
| 352 | vector<int>&used_hits, |
| 353 | vector<cdc_update_t>&updates, |
| 354 | double &chi2, |
| 355 | int &ndof, |
| 356 | unsigned int iter){ |
| 357 | DMatrix1x4 H; // Track projection matrix |
| 358 | DMatrix4x1 H_T; // Transpose of track projection matrix |
| 359 | DMatrix4x1 K; // Kalman gain matrix |
| 360 | DMatrix4x4 J; // Jacobian matrix |
| 361 | DMatrix4x1 S0; // State vector from reference trajectory |
| 362 | double V=1.15*(0.78*0.78/12.); // sigma=cell_size/sqrt(12.)*scale_factor |
| 363 | |
| 364 | const double d_EPS=1e-8; |
| 365 | |
| 366 | // Zero out the vector of used hit flags |
| 367 | for (unsigned int i=0;i<used_hits.size();i++) used_hits[i]=0; |
| 368 | |
| 369 | //Initialize chi2 and ndof |
| 370 | chi2=0.; |
| 371 | ndof=0; |
| 372 | |
| 373 | //Save the starting values for C and S |
| 374 | trajectory[0].Skk=S; |
| 375 | trajectory[0].Ckk=C; |
| 376 | |
| 377 | double doca2=0.; |
| 378 | |
| 379 | // CDC index and wire position variables |
| 380 | unsigned int cdc_index=cdchits.size()-1; |
| 381 | bool more_hits=true; |
| 382 | const DCDCWire *wire=cdchits[cdc_index]->wire; |
| 383 | DVector3 origin=wire->origin; |
| 384 | double z0=origin.z(); |
| 385 | double vz=wire->udir.z(); |
| 386 | if (VERBOSE) jout << " Starting in Ring " << wire->ring << endl; |
| 387 | DVector3 wdir=(1./vz)*wire->udir; |
| 388 | DVector3 wirepos=origin+(trajectory[0].z-z0)*wdir; |
| 389 | |
| 390 | /// compute initial doca^2 to first wire |
| 391 | double dx=S(state_x)-wirepos.X(); |
| 392 | double dy=S(state_y)-wirepos.Y(); |
| 393 | double old_doca2=dx*dx+dy*dy; |
| 394 | |
| 395 | // Loop over all steps in the trajectory |
| 396 | S0=trajectory[0].S; |
| 397 | J=trajectory[0].J; |
| 398 | for (unsigned int k=1;k<trajectory.size();k++){ |
| 399 | if (!C.IsPosDef()) return UNRECOVERABLE_ERROR; |
| 400 | |
| 401 | // Propagate the state and covariance matrix forward in z |
| 402 | S=trajectory[k].S+J*(S-S0); |
| 403 | C=J*C*J.Transpose(); |
| 404 | |
| 405 | // Save the current state and covariance matrix in the deque |
| 406 | trajectory[k].Skk=S; |
| 407 | trajectory[k].Ckk=C; |
| 408 | |
| 409 | // Save S and J for the next step |
| 410 | S0=trajectory[k].S; |
| 411 | J=trajectory[k].J; |
| 412 | |
| 413 | // Position along wire |
| 414 | wirepos=origin+(trajectory[k].z-z0)*wdir; |
| 415 | |
| 416 | // New doca^2 |
| 417 | dx=S(state_x)-wirepos.X(); |
| 418 | dy=S(state_y)-wirepos.Y(); |
| 419 | doca2=dx*dx+dy*dy; |
| 420 | if (VERBOSE > 10) jout<< "At Position " << S(state_x) << " " << S(state_y) << " " << trajectory[k].z << " doca2 " << doca2 << endl; |
| 421 | |
| 422 | if (doca2>old_doca2 && more_hits){ |
| 423 | |
| 424 | // zero-position and direction of line describing particle trajectory |
| 425 | double tx=S(state_tx),ty=S(state_ty); |
| 426 | DVector3 pos0(S(state_x),S(state_y),trajectory[k].z); |
| 427 | DVector3 tdir(tx,ty,1.); |
| 428 | |
| 429 | // Find the true doca to the wire |
| 430 | DVector3 diff=pos0-origin; |
| 431 | double dx0=diff.x(),dy0=diff.y(); |
| 432 | double wdir_dot_diff=diff.Dot(wdir); |
| 433 | double tdir_dot_diff=diff.Dot(tdir); |
| 434 | double tdir_dot_wdir=tdir.Dot(wdir); |
| 435 | double tdir2=tdir.Mag2(); |
| 436 | double wdir2=wdir.Mag2(); |
| 437 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
| 438 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
| 439 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
| 440 | double scale=1./D; |
| 441 | double s=scale*N; |
| 442 | double t=scale*N1; |
| 443 | diff+=s*tdir-t*wdir; |
| 444 | double d=diff.Mag()+d_EPS; // prevent division by zero |
| 445 | |
| 446 | // The next measurement |
| 447 | double dmeas=0.39,delta=0.; |
| 448 | double tdrift=cdchits[cdc_index]->tdrift-trajectory[k].t; |
| 449 | if (fit_type==kTimeBased){ |
| 450 | double phi_d=diff.Phi(); |
| 451 | double dphi=phi_d-origin.Phi(); |
| 452 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
| 453 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
| 454 | |
| 455 | int ring_index=cdchits[cdc_index]->wire->ring-1; |
| 456 | int straw_index=cdchits[cdc_index]->wire->straw-1; |
| 457 | double dz=t*wdir.z(); |
| 458 | double delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.) |
| 459 | *cos(phi_d+sag_phi_offset[ring_index][straw_index]); |
| 460 | CDCDriftParameters(dphi,delta,tdrift,dmeas,V); |
| 461 | } |
| 462 | |
| 463 | // residual |
| 464 | double res=dmeas-d; |
| 465 | if (VERBOSE>5) jout << " Residual " << res << endl; |
| 466 | |
| 467 | // Track projection |
| 468 | |
| 469 | double one_over_d=1./d; |
| 470 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
| 471 | double wx=wdir.x(),wy=wdir.y(); |
| 472 | |
| 473 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
| 474 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
| 475 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
| 476 | |
| 477 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
| 478 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
| 479 | double dtdty=scale*(dN1dty-t*dDdty); |
| 480 | |
| 481 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
| 482 | double dsdtx=scale*(dNdtx-s*dDdtx); |
| 483 | |
| 484 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
| 485 | double dsdty=scale*(dNdty-s*dDdty); |
| 486 | |
| 487 | H(state_tx)=H_T(state_tx) |
| 488 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
| 489 | +diffz*(dsdtx-dtdtx)); |
| 490 | H(state_ty)=H_T(state_ty) |
| 491 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
| 492 | +diffz*(dsdty-dtdty)); |
| 493 | |
| 494 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
| 495 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
| 496 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
| 497 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
| 498 | |
| 499 | H(state_x)=H_T(state_x) |
| 500 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
| 501 | +diffz*(dsdx-dtdx)); |
| 502 | H(state_y)=H_T(state_y) |
| 503 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
| 504 | +diffz*(dsdy-dtdy)); |
| 505 | |
| 506 | double InvV=1./(V+H*C*H_T); |
| 507 | |
| 508 | // Check how far this hit is from the projection |
| 509 | double chi2check=res*res*InvV; |
| 510 | if (chi2check < CHI2CUT || DO_PRUNING == 0 || (COSMICS && iter == 0)){ |
| 511 | if (VERBOSE>5) jout << "Hit Added to track " << endl; |
| 512 | if (cdchits[cdc_index]->wire->ring!=RING_TO_SKIP){ |
| 513 | |
| 514 | // Compute Kalman gain matrix |
| 515 | K=InvV*(C*H_T); |
| 516 | |
| 517 | // Update state vector covariance matrix |
| 518 | DMatrix4x4 Ctest=C-K*(H*C); |
| 519 | |
| 520 | //C.Print(); |
| 521 | //K.Print(); |
| 522 | //Ctest.Print(); |
| 523 | |
| 524 | // Check that Ctest is positive definite |
| 525 | if (!Ctest.IsPosDef()) return VALUE_OUT_OF_RANGE; |
| 526 | C=Ctest; |
| 527 | if(VERBOSE>10) C.Print(); |
| 528 | // Update the state vector |
| 529 | //S=S+res*K; |
| 530 | S+=res*K; |
| 531 | if(VERBOSE>10) S.Print(); |
| 532 | |
| 533 | // Compute new residual |
| 534 | //d=finder->FindDoca(trajectory[k].z,S,wdir,origin); |
| 535 | res=res-H*K*res; |
| 536 | |
| 537 | // Update chi2 |
| 538 | double fit_V=V-H*C*H_T; |
| 539 | chi2+=res*res/fit_V; |
| 540 | ndof++; |
| 541 | |
| 542 | // fill pull vector entry |
| 543 | updates[cdc_index].V=fit_V; |
| 544 | } |
| 545 | else { |
| 546 | updates[cdc_index].V=V; |
| 547 | } |
| 548 | |
| 549 | // Flag that we used this hit |
| 550 | used_hits[cdc_index]=1; |
| 551 | |
| 552 | // fill updates |
| 553 | updates[cdc_index].resi=res; |
| 554 | updates[cdc_index].d=d; |
| 555 | updates[cdc_index].delta=delta; |
| 556 | updates[cdc_index].S=S; |
| 557 | updates[cdc_index].C=C; |
| 558 | updates[cdc_index].tdrift=tdrift; |
| 559 | updates[cdc_index].ddrift=dmeas; |
| 560 | updates[cdc_index].s=29.98*trajectory[k].t; // assume beta=1 |
| 561 | trajectory[k].id=cdc_index+1; |
| 562 | } |
| 563 | // move to next cdc hit |
| 564 | if (cdc_index>0){ |
| 565 | cdc_index--; |
| 566 | |
| 567 | //New wire position |
| 568 | wire=cdchits[cdc_index]->wire; |
| 569 | if (VERBOSE>5) { |
| 570 | jout << " Next Wire ring " << wire->ring << " straw " << wire->straw << " origin udir" << endl; |
| 571 | wire->origin.Print(); wire->udir.Print(); |
| 572 | } |
| 573 | origin=wire->origin; |
| 574 | z0=origin.z(); |
| 575 | vz=wire->udir.z(); |
| 576 | wdir=(1./vz)*wire->udir; |
| 577 | wirepos=origin+((trajectory[k].z-z0))*wdir; |
| 578 | |
| 579 | // New doca^2 |
| 580 | dx=S(state_x)-wirepos.x(); |
| 581 | dy=S(state_y)-wirepos.y(); |
| 582 | doca2=dx*dx+dy*dy; |
| 583 | |
| 584 | } |
| 585 | else more_hits=false; |
| 586 | } |
| 587 | |
| 588 | old_doca2=doca2; |
| 589 | } |
| 590 | if (ndof<=4) return VALUE_OUT_OF_RANGE; |
| 591 | |
| 592 | ndof-=4; |
| 593 | |
| 594 | return NOERROR; |
| 595 | } |
| 596 | |
| 597 | // Smooth the CDC only tracks |
| 598 | jerror_t DTrackFitterStraightTrack::Smooth(vector<cdc_update_t>&cdc_updates){ |
| 599 | unsigned int max=best_trajectory.size()-1; |
| 600 | DMatrix4x1 S=(best_trajectory[max].Skk); |
| 601 | DMatrix4x4 C=(best_trajectory[max].Ckk); |
| 602 | DMatrix4x4 JT=best_trajectory[max].J.Transpose(); |
| 603 | DMatrix4x1 Ss=S; |
| 604 | DMatrix4x4 Cs=C; |
| 605 | DMatrix4x4 A,dC; |
| 606 | DMatrix1x4 H; // Track projection matrix |
| 607 | DMatrix4x1 H_T; // Transpose of track projection matrix |
| 608 | |
| 609 | const double d_EPS=1e-8; |
| 610 | |
| 611 | for (unsigned int m=max-1;m>0;m--){ |
| 612 | if (best_trajectory[m].id>0){ |
| 613 | unsigned int id=best_trajectory[m].id-1; |
| 614 | A=cdc_updates[id].C*JT*C.Invert(); |
| 615 | Ss=cdc_updates[id].S+A*(Ss-S); |
| 616 | |
| 617 | dC=A*(Cs-C)*A.Transpose(); |
| 618 | Cs=cdc_updates[id].C+dC; |
| 619 | if (VERBOSE > 10) { |
| 620 | jout << " In Smoothing Step Using ID " << id << "/" << cdc_updates.size() << " for ring " << cdchits[id]->wire->ring << endl; |
| 621 | jout << " A cdc_updates[id].C Ss Cs " << endl; |
| 622 | A.Print(); cdc_updates[id].C.Print(); Ss.Print(); Cs.Print(); |
| 623 | } |
| 624 | if(!Cs.IsPosDef()) { |
| 625 | if (VERBOSE) jout << "Cs is not PosDef!" << endl; |
| 626 | return VALUE_OUT_OF_RANGE; |
| 627 | } |
| 628 | |
| 629 | const DCDCWire *wire=cdchits[id]->wire; |
| 630 | DVector3 origin=wire->origin; |
| 631 | double z0=origin.z(); |
| 632 | double vz=wire->udir.z(); |
| 633 | DVector3 wdir=(1./vz)*wire->udir; |
| 634 | DVector3 wirepos=origin+(best_trajectory[m].z-z0)*wdir; |
| 635 | // Position and direction from state vector |
| 636 | double x=Ss(state_x); |
| 637 | double y=Ss(state_y); |
| 638 | double tx=Ss(state_tx); |
| 639 | double ty=Ss(state_ty); |
| 640 | |
| 641 | DVector3 pos0(x,y,best_trajectory[m].z); |
| 642 | DVector3 tdir(tx,ty,1.); |
| 643 | |
| 644 | // Find the true doca to the wire |
| 645 | DVector3 diff=pos0-origin; |
| 646 | double dx0=diff.x(),dy0=diff.y(); |
| 647 | double wdir_dot_diff=diff.Dot(wdir); |
| 648 | double tdir_dot_diff=diff.Dot(tdir); |
| 649 | double tdir_dot_wdir=tdir.Dot(wdir); |
| 650 | double tdir2=tdir.Mag2(); |
| 651 | double wdir2=wdir.Mag2(); |
| 652 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
| 653 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
| 654 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
| 655 | double scale=1./D; |
| 656 | double s=scale*N; |
| 657 | double t=scale*N1; |
| 658 | diff+=s*tdir-t*wdir; |
| 659 | double d=diff.Mag()+d_EPS; // prevent division by zero |
| 660 | double ddrift = cdc_updates[id].ddrift; |
| 661 | |
| 662 | double resi = ddrift - d; |
| 663 | // Track projection |
| 664 | |
| 665 | { |
| 666 | double one_over_d=1./d; |
| 667 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
| 668 | double wx=wdir.x(),wy=wdir.y(); |
| 669 | |
| 670 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
| 671 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
| 672 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
| 673 | |
| 674 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
| 675 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
| 676 | double dtdty=scale*(dN1dty-t*dDdty); |
| 677 | |
| 678 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
| 679 | double dsdtx=scale*(dNdtx-s*dDdtx); |
| 680 | |
| 681 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
| 682 | double dsdty=scale*(dNdty-s*dDdty); |
| 683 | |
| 684 | H(state_tx)=H_T(state_tx) |
| 685 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
| 686 | +diffz*(dsdtx-dtdtx)); |
| 687 | H(state_ty)=H_T(state_ty) |
| 688 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
| 689 | +diffz*(dsdty-dtdty)); |
| 690 | |
| 691 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
| 692 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
| 693 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
| 694 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
| 695 | |
| 696 | H(state_x)=H_T(state_x) |
| 697 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
| 698 | +diffz*(dsdx-dtdx)); |
| 699 | H(state_y)=H_T(state_y) |
| 700 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
| 701 | +diffz*(dsdy-dtdy)); |
| 702 | } |
| 703 | double V=cdc_updates[id].V; |
| 704 | |
| 705 | if (VERBOSE > 10) jout << " d " << d << " H*S " << H*S << endl; |
| 706 | if (cdchits[id]->wire->ring==RING_TO_SKIP){ |
| 707 | V=V+H*Cs*H_T; |
| 708 | } |
| 709 | else{ |
| 710 | V=V-H*Cs*H_T; |
| 711 | } |
| 712 | if (V<0) return VALUE_OUT_OF_RANGE; |
| 713 | |
| 714 | // Add the pull |
| 715 | double myscale=1./sqrt(1.+tx*tx+ty*ty); |
| 716 | double cosThetaRel=wire->udir.Dot(DVector3(myscale*tx,myscale*ty,myscale)); |
| 717 | pull_t thisPull(resi,sqrt(V), |
| 718 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
| 719 | cdc_updates[id].tdrift, |
| 720 | d,cdchits[id], NULL__null, |
| 721 | diff.Phi(), //docaphi |
| 722 | best_trajectory[m].z,cosThetaRel, |
| 723 | cdc_updates[id].tdrift); |
| 724 | |
| 725 | // Derivatives for alignment |
| 726 | double wtx=wire->udir.X(), wty=wire->udir.Y(), wtz=wire->udir.Z(); |
| 727 | double wx=wire->origin.X(), wy=wire->origin.Y(), wz=wire->origin.Z(); |
| 728 | |
| 729 | double z=best_trajectory[m].z; |
| 730 | double tx2=tx*tx, ty2=ty*ty; |
| 731 | double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz; |
| 732 | double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz) +d_EPS; |
| 733 | double denom2=denom*denom; |
| 734 | double c1=-(wtx - tx*wtz)*(wy - y); |
| 735 | double c2=wty*(wx - tx*wz - x + tx*z); |
| 736 | double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z); |
| 737 | double dscale=0.5*(1./d); |
| 738 | |
| 739 | vector<double> derivatives(11); |
| 740 | |
| 741 | derivatives[CDCTrackD::dDOCAdOriginX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3))/denom; |
| 742 | |
| 743 | derivatives[CDCTrackD::dDOCAdOriginY]=dscale*(2*(-wtx + tx*wtz)*(c1 + c2 + c3))/denom; |
| 744 | |
| 745 | derivatives[CDCTrackD::dDOCAdOriginZ]=dscale*(2*(ty*wtx - tx*wty)*(c1 + c2 + c3))/denom; |
| 746 | |
| 747 | derivatives[CDCTrackD::dDOCAdDirX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3)* |
| 748 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
| 749 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
| 750 | |
| 751 | derivatives[CDCTrackD::dDOCAdDirY]=dscale*(-2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
| 752 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
| 753 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
| 754 | |
| 755 | derivatives[CDCTrackD::dDOCAdDirZ]=dscale*(-2*(ty*wtx - tx*wty)*(c1 + c2 + c3)* |
| 756 | (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) + |
| 757 | wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2; |
| 758 | |
| 759 | derivatives[CDCTrackD::dDOCAdS0]=-derivatives[CDCTrackD::dDOCAdOriginX]; |
| 760 | |
| 761 | derivatives[CDCTrackD::dDOCAdS1]=-derivatives[CDCTrackD::dDOCAdOriginY]; |
| 762 | |
| 763 | derivatives[CDCTrackD::dDOCAdS2]=dscale*(2*(wty - ty*wtz)*(-c1 - c2 - c3)* |
| 764 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
| 765 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
| 766 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
| 767 | |
| 768 | derivatives[CDCTrackD::dDOCAdS3]=dscale*(2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
| 769 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
| 770 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
| 771 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
| 772 | |
| 773 | thisPull.AddTrackDerivatives(derivatives); |
| 774 | pulls.push_back(thisPull); |
| 775 | } |
| 776 | else{ |
| 777 | A=best_trajectory[m].Ckk*JT*C.Invert(); |
| 778 | Ss=best_trajectory[m].Skk+A*(Ss-S); |
| 779 | Cs=best_trajectory[m].Ckk+A*(Cs-C)*A.Transpose(); |
| 780 | } |
| 781 | |
| 782 | S=best_trajectory[m].Skk; |
| 783 | C=best_trajectory[m].Ckk; |
| 784 | JT=best_trajectory[m].J.Transpose(); |
| 785 | } |
| 786 | |
| 787 | return NOERROR; |
| 788 | } |
| 789 | |
| 790 | //Reference trajectory for the track for cdc tracks |
| 791 | jerror_t DTrackFitterStraightTrack::SetReferenceTrajectory(double t0,double z, |
| 792 | DMatrix4x1 &S, |
| 793 | const DCDCTrackHit *last_cdc, |
| 794 | double &dzsign){ |
| 795 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
| 796 | |
| 797 | double ds=0.5; |
| 798 | double t=t0; |
| 799 | |
| 800 | // last y position of hit (approximate, using center of wire) |
| 801 | double last_y=last_cdc->wire->origin.y(); |
| 802 | double last_r2=last_cdc->wire->origin.Perp2(); |
| 803 | // Check that track points towards last wire, otherwise swap deltaz |
| 804 | DVector3 dir(S(state_tx),S(state_ty),dzsign); |
| 805 | if (fabs(dir.Theta() - M_PI_21.57079632679489661923) < 0.2) ds = 0.1; |
| 806 | |
| 807 | //jout << "dPhi " << dphi << " theta " << dir.Theta() << endl; |
| 808 | double dz=dzsign*ds/sqrt(1.+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty)); |
| 809 | |
| 810 | if (VERBOSE) { |
| 811 | if (COSMICS) jout << " Swimming Reference Trajectory last CDC y " << last_y << " dz "<< dz << endl; |
| 812 | else jout << " Swimming Reference Trajectory last CDC r2 " << last_r2 << " dz "<< dz << endl; |
| 813 | jout << " S" << endl; S.Print(); jout << "z= "<< z << endl; |
| 814 | jout << " Last CDC ring " << last_cdc->wire->ring << " straw " << last_cdc->wire->straw << endl; |
| 815 | } |
| 816 | unsigned int numsteps=0; |
| 817 | const unsigned int MAX_STEPS=5000; |
| 818 | bool done=false; |
| 819 | do{ |
| 820 | numsteps++; |
| 821 | z+=dz; |
| 822 | J(state_x,state_tx)=-dz; |
| 823 | J(state_y,state_ty)=-dz; |
| 824 | // Flight time: assume particle is moving at the speed of light |
| 825 | t+=ds/29.98; |
| 826 | //propagate the state to the next z position |
| 827 | S(state_x)+=S(state_tx)*dz; |
| 828 | S(state_y)+=S(state_ty)*dz; |
| 829 | |
| 830 | if (z > downstreamEndplate && dz < 0) continue; |
| 831 | if (z < upstreamEndplate && dz > 0) continue; |
| 832 | trajectory.push_front(trajectory_t(z,t,S,J,DMatrix4x1(),DMatrix4x4())); |
| 833 | |
| 834 | done=((z>downstreamEndplate) || (z<upstreamEndplate)); |
| 835 | if (COSMICS==false){ |
| 836 | double r2=S(state_x)*S(state_x)+S(state_y)*S(state_y); |
| 837 | done |= (r2>last_r2); |
| 838 | } |
| 839 | }while (!done && numsteps<MAX_STEPS); |
| 840 | |
| 841 | if (VERBOSE) |
| 842 | { |
| 843 | if (VERBOSE > 10){ |
| 844 | printf("Trajectory:\n"); |
| 845 | for (unsigned int i=0;i<trajectory.size();i++){ |
| 846 | printf(" x %f y %f z %f\n",trajectory[i].S(state_x), |
| 847 | trajectory[i].S(state_y),trajectory[i].z); |
| 848 | } |
| 849 | } |
| 850 | else{ |
| 851 | printf("%i step trajectory Begin/End:\n", numsteps); |
| 852 | printf(" x %f y %f z %f\n",trajectory[0].S(state_x), trajectory[0].S(state_y), trajectory[0].z); |
| 853 | if (trajectory.size() > 1) printf(" x %f y %f z %f\n",trajectory[trajectory.size()-1].S(state_x), |
| 854 | trajectory[trajectory.size()-1].S(state_y),trajectory[trajectory.size()-1].z); |
| 855 | } |
| 856 | } |
| 857 | if (trajectory.size()<2) return UNRECOVERABLE_ERROR; |
| 858 | return NOERROR; |
| 859 | } |
| 860 | |
| 861 | // Routine for steering the fit of the track |
| 862 | DTrackFitter::fit_status_t DTrackFitterStraightTrack::FitTrack(void){ |
| 863 | DTrackFitter::fit_status_t status=DTrackFitter::kFitNotDone; |
| 864 | if (cdchits.size()+fdchits.size() < 4) return status; |
| 865 | |
| 866 | // Initial guess for state vector |
| 867 | DVector3 input_pos=input_params.position(); |
| 868 | DVector3 input_mom=input_params.momentum(); |
| 869 | double pz=input_mom.z(); |
| 870 | DMatrix4x1 S(input_pos.x(),input_pos.y(),input_mom.x()/pz,input_mom.y()/pz); |
| 871 | |
| 872 | // Initial guess for covariance matrix |
| 873 | DMatrix4x4 C; |
| 874 | if (fit_type==kWireBased){ |
| 875 | C(state_x,state_x)=C(state_y,state_y)=4.0; |
| 876 | C(state_tx,state_tx)=C(state_ty,state_ty)=1.0; |
| 877 | } |
| 878 | else{ |
| 879 | C(state_x,state_x)=C(state_y,state_y)=1.; |
| 880 | C(state_tx,state_tx)=C(state_ty,state_ty)=0.01; |
| 881 | } |
| 882 | |
| 883 | // Starting z-position and time |
| 884 | double z=input_pos.z(); |
| 885 | double t0=input_params.t0(); |
| 886 | // Variance in start time |
| 887 | switch(input_params.t0_detector()){ |
| 888 | case SYS_TOF: |
| 889 | mVarT0=0.01; |
| 890 | break; |
| 891 | case SYS_CDC: |
| 892 | mVarT0=25.; |
| 893 | break; |
| 894 | case SYS_FDC: |
| 895 | mVarT0=25.; |
| 896 | break; |
| 897 | case SYS_BCAL: |
| 898 | mVarT0=0.25; |
| 899 | break; |
| 900 | case SYS_START: |
| 901 | mVarT0=0.09; |
| 902 | break; |
| 903 | default: |
| 904 | mVarT0=0.; |
| 905 | break; |
| 906 | } |
| 907 | |
| 908 | // Chisq and ndof |
| 909 | chisq=1e16; |
| 910 | Ndof=0; |
| 911 | |
| 912 | // Sort the CDC hits by radius or y (for cosmics) |
| 913 | if (cdchits.size()>0){ |
| 914 | if (COSMICS){ |
| 915 | stable_sort(cdchits.begin(),cdchits.end(),DTrackFitterStraightTrack_cdc_hit_cmp); |
| 916 | } |
| 917 | else{ |
| 918 | stable_sort(cdchits.begin(),cdchits.end(),DTrackFitterStraightTrack_cdc_hit_radius_cmp); |
| 919 | } |
| 920 | } |
| 921 | |
| 922 | if (fdchits.size()>0){ |
| 923 | // Sort FDC hits by z |
| 924 | stable_sort(fdchits.begin(),fdchits.end(),DTrackFitterStraightTrack_fdc_hit_cmp); |
| 925 | status=FitForwardTrack(t0,z,S,C,chisq,Ndof); |
| 926 | } |
| 927 | else if (cdchits.size()>0){ |
| 928 | double dzsign=(pz>0)?1.:-1.; |
| 929 | status=FitCentralTrack(z,t0,dzsign,S,C,chisq,Ndof); |
| 930 | } |
| 931 | |
| 932 | if (status==DTrackFitter::kFitSuccess){ |
| 933 | // Output fit results |
| 934 | double tx=S(state_tx),ty=S(state_ty); |
| 935 | double phi=atan2(ty,tx); |
| 936 | double tanl=1./sqrt(tx*tx+ty*ty); |
| 937 | // Check for tracks heading upstream |
| 938 | if (cdchits_used_in_fit.size()>0){ |
| 939 | double phi_diff=phi-cdchits_used_in_fit[0]->wire->origin.Phi()-M_PI3.14159265358979323846; |
| 940 | if (phi_diff<-M_PI3.14159265358979323846) phi_diff+=2.*M_PI3.14159265358979323846; |
| 941 | if (phi_diff> M_PI3.14159265358979323846) phi_diff-=2.*M_PI3.14159265358979323846; |
| 942 | if (fabs(phi_diff)<M_PI_21.57079632679489661923){ |
| 943 | tanl*=-1; |
| 944 | phi+=M_PI3.14159265358979323846; |
| 945 | } |
| 946 | } |
| 947 | double pt=5.0*cos(atan(tanl)); // arbitrary magnitude... |
| 948 | DVector3 mom(pt*cos(phi),pt*sin(phi),pt*tanl); |
| 949 | |
| 950 | // Convert 4x4 covariance matrix to a TMatrixFSym for output |
| 951 | TMatrixFSym errMatrix(5); |
| 952 | for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){ |
| 953 | for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){ |
| 954 | errMatrix(loc_i, loc_j) = C(loc_i, loc_j); |
| 955 | } |
| 956 | } |
| 957 | errMatrix(4,4)=1.; |
| 958 | |
| 959 | // Add 7x7 covariance matrix to output |
| 960 | double sign=(mom.Theta()>M_PI_21.57079632679489661923)?-1.:1.; |
| 961 | fit_params.setErrorMatrix(Get7x7ErrorMatrix(errMatrix,S,sign)); |
| 962 | |
| 963 | DVector3 pos; |
| 964 | if (COSMICS==false){ |
| 965 | DVector3 beamdir(0.,0.,1.); |
| 966 | DVector3 beampos(0.,0.,65.); |
| 967 | finder->FindDoca(z,S,beamdir,beampos,&pos); |
| 968 | |
| 969 | // Jacobian matrix |
| 970 | double dz=pos.z()-z; |
| 971 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
| 972 | J(state_x,state_tx)=dz; |
| 973 | J(state_y,state_ty)=dz; |
| 974 | // Transform the matrix to the position of the doca |
| 975 | C=J*C*J.Transpose(); |
| 976 | } |
| 977 | else{ |
| 978 | pos.SetXYZ(S(state_x),S(state_y),z); |
| 979 | } |
| 980 | |
| 981 | fit_params.setForwardParmFlag(true); |
| 982 | |
| 983 | fit_params.setPosition(pos); |
| 984 | fit_params.setMomentum(mom); |
| 985 | |
| 986 | // Add tracking covariance matrix to output |
| 987 | auto locTrackingCovarianceMatrix = dResourcePool_TMatrixFSym->Get_SharedResource(); |
| 988 | locTrackingCovarianceMatrix->ResizeTo(5, 5); |
| 989 | locTrackingCovarianceMatrix->Zero(); |
| 990 | for(unsigned int loc_i = 0; loc_i < 4; ++loc_i){ |
| 991 | for(unsigned int loc_j = 0; loc_j < 4; ++loc_j){ |
| 992 | (*locTrackingCovarianceMatrix)(loc_i, loc_j) = C(loc_i, loc_j); |
| 993 | } |
| 994 | } |
| 995 | (*locTrackingCovarianceMatrix)(4,4)=1.; |
| 996 | fit_params.setTrackingErrorMatrix(locTrackingCovarianceMatrix); |
| 997 | |
| 998 | // Get extrapolations to other detectors |
| 999 | GetExtrapolations(pos,mom); |
| 1000 | |
| 1001 | } |
| 1002 | |
| 1003 | return status; |
| 1004 | } |
| 1005 | |
| 1006 | // Locate a position in vector xx given x |
| 1007 | unsigned int DTrackFitterStraightTrack::Locate(const vector<double>&xx,double x) const{ |
| 1008 | int n=xx.size(); |
| 1009 | if (x==xx[0]) return 0; |
| 1010 | else if (x==xx[n-1]) return n-2; |
| 1011 | |
| 1012 | int jl=-1; |
| 1013 | int ju=n; |
| 1014 | int ascnd=(xx[n-1]>=xx[0]); |
| 1015 | while(ju-jl>1){ |
| 1016 | int jm=(ju+jl)>>1; |
| 1017 | if ( (x>=xx[jm])==ascnd) |
| 1018 | jl=jm; |
| 1019 | else |
| 1020 | ju=jm; |
| 1021 | } |
| 1022 | return jl; |
| 1023 | } |
| 1024 | |
| 1025 | // Steering routine for fitting CDC-only tracks |
| 1026 | DTrackFitter::fit_status_t DTrackFitterStraightTrack::FitCentralTrack(double &z0,double t0, |
| 1027 | double dzsign, |
| 1028 | DMatrix4x1 &Sbest, |
| 1029 | DMatrix4x4 &Cbest, |
| 1030 | double &chi2_best, |
| 1031 | int &ndof_best){ |
| 1032 | // State vector and covariance matrix |
| 1033 | DMatrix4x1 S(Sbest); |
| 1034 | DMatrix4x4 C(Cbest),C0(Cbest); |
| 1035 | |
| 1036 | double chi2=chi2_best; |
| 1037 | int ndof=ndof_best; |
| 1038 | |
| 1039 | unsigned int numhits=cdchits.size(); |
| 1040 | unsigned int maxindex=numhits-1; |
| 1041 | |
| 1042 | // vectors of indexes to cdc hits used in the fit |
| 1043 | vector<int> used_cdc_hits(numhits); |
| 1044 | vector<int> used_cdc_hits_best_fit(numhits); |
| 1045 | |
| 1046 | // vectors of residual information |
| 1047 | vector<cdc_update_t>updates(numhits); |
| 1048 | vector<cdc_update_t>best_updates(numhits); |
| 1049 | |
| 1050 | // Rest deque for "best" trajectory |
| 1051 | best_trajectory.clear(); |
| 1052 | |
| 1053 | // Perform the fit |
| 1054 | int iter=0; |
| 1055 | for(iter=0;iter<20;iter++){ |
| 1056 | if (VERBOSE) jout << " Performing Pass iter " << iter << endl; |
| 1057 | |
| 1058 | trajectory.clear(); |
| 1059 | if (SetReferenceTrajectory(t0,z0,S,cdchits[maxindex],dzsign) |
| 1060 | !=NOERROR) break; |
| 1061 | |
| 1062 | if (VERBOSE) jout << " Reference Trajectory Set " << endl; |
| 1063 | C=C0; |
| 1064 | if (KalmanFilter(S,C,used_cdc_hits,updates,chi2,ndof,iter)!=NOERROR) break; |
| 1065 | if (VERBOSE) jout << " Filter returns NOERROR" << endl; |
| 1066 | if (iter>0 && (fabs(chi2_best-chi2)<0.1 || chi2>chi2_best)) break; |
| 1067 | |
| 1068 | // Save the current state and covariance matrixes |
| 1069 | Cbest=C; |
| 1070 | Sbest=S; |
| 1071 | |
| 1072 | // Save the current used hit and trajectory information |
| 1073 | best_trajectory.assign(trajectory.begin(),trajectory.end()); |
| 1074 | used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end()); |
| 1075 | best_updates.assign(updates.begin(),updates.end()); |
| 1076 | |
| 1077 | chi2_best=chi2; |
| 1078 | ndof_best=ndof; |
| 1079 | } |
| 1080 | if (iter==0) return DTrackFitter::kFitFailed; |
| 1081 | |
| 1082 | //Final z position (closest to beam line) |
| 1083 | z0=best_trajectory[best_trajectory.size()-1].z; |
| 1084 | |
| 1085 | //Run the smoother |
| 1086 | if (Smooth(best_updates) == NOERROR) IsSmoothed=true; |
| 1087 | |
| 1088 | // output list of cdc hits used in the fit |
| 1089 | cdchits_used_in_fit.clear(); |
| 1090 | for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){ |
| 1091 | if (used_cdc_hits_best_fit[m]){ |
| 1092 | cdchits_used_in_fit.push_back(cdchits[m]); |
| 1093 | } |
| 1094 | } |
| 1095 | |
| 1096 | return DTrackFitter::kFitSuccess; |
| 1097 | } |
| 1098 | |
| 1099 | |
| 1100 | //---------------------------------------------------- |
| 1101 | // FDC fitting routines |
| 1102 | //---------------------------------------------------- |
| 1103 | |
| 1104 | // parametrization of time-to-distance for FDC |
| 1105 | double DTrackFitterStraightTrack::fdc_drift_distance(double time) const { |
| 1106 | if (time<0.) return 0.; |
| 1107 | double d=0.; |
| 1108 | double tsq=time*time; |
| 1109 | double t_high=DRIFT_FUNC_PARMS[4]; |
| 1110 | |
| 1111 | if (time<t_high){ |
| 1112 | d=DRIFT_FUNC_PARMS[0]*sqrt(time)+DRIFT_FUNC_PARMS[1]*time |
| 1113 | +DRIFT_FUNC_PARMS[2]*tsq+DRIFT_FUNC_PARMS[3]*tsq*time; |
| 1114 | } |
| 1115 | else{ |
| 1116 | double t_high_sq=t_high*t_high; |
| 1117 | d=DRIFT_FUNC_PARMS[0]*sqrt(t_high)+DRIFT_FUNC_PARMS[1]*t_high |
| 1118 | +DRIFT_FUNC_PARMS[2]*t_high_sq+DRIFT_FUNC_PARMS[3]*t_high_sq*t_high; |
| 1119 | d+=DRIFT_FUNC_PARMS[5]*(time-t_high); |
| 1120 | } |
| 1121 | |
| 1122 | return d; |
| 1123 | |
| 1124 | } |
| 1125 | |
| 1126 | // Crude approximation for the variance in drift distance due to smearing |
| 1127 | double DTrackFitterStraightTrack::fdc_drift_variance(double t) const{ |
| 1128 | //return FDC_ANODE_VARIANCE; |
| 1129 | if (t<5.) t=5.; |
| 1130 | double sigma=DRIFT_RES_PARMS[0]/(t+1.)+DRIFT_RES_PARMS[1]+DRIFT_RES_PARMS[2]*t*t; |
| 1131 | |
| 1132 | return sigma*sigma; |
| 1133 | } |
| 1134 | |
| 1135 | // Steering routine for the kalman filter |
| 1136 | DTrackFitter::fit_status_t |
| 1137 | DTrackFitterStraightTrack::FitForwardTrack(double t0,double &start_z, |
| 1138 | DMatrix4x1 &Sbest,DMatrix4x4 &Cbest,double &chi2_best,int &ndof_best){ |
| 1139 | // State vector and covariance matrix |
| 1140 | DMatrix4x1 S(Sbest); |
| 1141 | DMatrix4x4 C(Cbest),C0(Cbest); |
| 1142 | |
| 1143 | // vectors of indexes to fdc hits used in the fit |
| 1144 | unsigned int numfdchits=fdchits.size(); |
| 1145 | vector<int> used_fdc_hits(numfdchits); |
| 1146 | vector<int> used_fdc_hits_best_fit(numfdchits); |
| 1147 | // vectors of indexes to cdc hits used in the fit |
| 1148 | unsigned int numcdchits=cdchits.size(); |
| 1149 | vector<int> used_cdc_hits(numcdchits); |
| 1150 | vector<int> used_cdc_hits_best_fit(numcdchits); |
| 1151 | |
| 1152 | // vectors of residual information |
| 1153 | vector<fdc_update_t>updates(numfdchits); |
| 1154 | vector<fdc_update_t>best_updates(numfdchits); |
| 1155 | vector<cdc_update_t>cdc_updates(numcdchits); |
| 1156 | vector<cdc_update_t>best_cdc_updates(numcdchits); |
| 1157 | |
| 1158 | vector<const DCDCTrackHit *> matchedCDCHits; |
| 1159 | |
| 1160 | // Chi-squared and degrees of freedom |
| 1161 | double chi2=chi2_best; |
| 1162 | int ndof=ndof_best; |
| 1163 | |
| 1164 | // Rest deque for "best" trajectory |
| 1165 | best_trajectory.clear(); |
| 1166 | |
| 1167 | unsigned iter=0; |
| 1168 | // First pass |
| 1169 | for(iter=0;iter<20;iter++){ |
| 1170 | trajectory.clear(); |
| 1171 | if (SetReferenceTrajectory(t0,start_z,S)!=NOERROR) break; |
| 1172 | |
| 1173 | C=C0; |
| 1174 | if (KalmanFilter(S,C,used_fdc_hits,used_cdc_hits,updates,cdc_updates, |
| 1175 | chi2,ndof)!=NOERROR) break; |
| 1176 | |
| 1177 | // printf(" == iter %d =====chi2 %f ndof %d \n",iter,chi2,ndof); |
| 1178 | if (iter>0 && (chi2>chi2_best || fabs(chi2_best-chi2)<0.1)) break; |
| 1179 | |
| 1180 | // Save the current state and covariance matrixes |
| 1181 | Cbest=C; |
| 1182 | Sbest=S; |
| 1183 | |
| 1184 | // Save the current used hit and trajectory information |
| 1185 | best_trajectory.assign(trajectory.begin(),trajectory.end()); |
| 1186 | used_cdc_hits_best_fit.assign(used_cdc_hits.begin(),used_cdc_hits.end()); |
| 1187 | used_fdc_hits_best_fit.assign(used_fdc_hits.begin(),used_fdc_hits.end()); |
| 1188 | best_updates.assign(updates.begin(),updates.end()); |
| 1189 | best_cdc_updates.assign(cdc_updates.begin(),cdc_updates.end()); |
| 1190 | |
| 1191 | chi2_best=chi2; |
| 1192 | ndof_best=ndof; |
| 1193 | } |
| 1194 | if (iter==0) return DTrackFitter::kFitFailed; |
| 1195 | |
| 1196 | //Final z position (closest to beam line) |
| 1197 | start_z=best_trajectory[best_trajectory.size()-1].z; |
| 1198 | |
| 1199 | //Run the smoother |
| 1200 | if (Smooth(best_updates,best_cdc_updates) == NOERROR) IsSmoothed=true; |
| 1201 | |
| 1202 | // output list of hits used in the fit |
| 1203 | cdchits_used_in_fit.clear(); |
| 1204 | for (unsigned int m=0;m<used_cdc_hits_best_fit.size();m++){ |
| 1205 | if (used_cdc_hits_best_fit[m]){ |
| 1206 | cdchits_used_in_fit.push_back(cdchits[m]); |
| 1207 | } |
| 1208 | } |
| 1209 | fdchits_used_in_fit.clear(); |
| 1210 | for (unsigned int m=0;m<used_fdc_hits_best_fit.size();m++){ |
| 1211 | if (used_fdc_hits_best_fit[m]){ |
| 1212 | fdchits_used_in_fit.push_back(fdchits[m]); |
| 1213 | } |
| 1214 | } |
| 1215 | |
| 1216 | |
| 1217 | return DTrackFitter::kFitSuccess; |
| 1218 | } |
| 1219 | |
| 1220 | |
| 1221 | // Reference trajectory for the track |
| 1222 | jerror_t |
| 1223 | DTrackFitterStraightTrack::SetReferenceTrajectory(double t0,double z, |
| 1224 | DMatrix4x1 &S){ |
| 1225 | const double EPS=1e-3; |
| 1226 | |
| 1227 | // Jacobian matrix |
| 1228 | DMatrix4x4 J(1.,0.,1.,0., 0.,1.,0.,1., 0.,0.,1.,0., 0.,0.,0.,1.); |
| 1229 | |
| 1230 | double dz=1.1; |
| 1231 | double t=t0; |
| 1232 | trajectory.push_front(trajectory_t(z,t,S,J,DMatrix4x1(),DMatrix4x4())); |
| 1233 | |
| 1234 | double zhit=z; |
| 1235 | double old_zhit=z; |
| 1236 | for (unsigned int i=0;i<fdchits.size();i++){ |
| 1237 | zhit=fdchits[i]->wire->origin.z(); |
| 1238 | dz=1.1; |
| 1239 | |
| 1240 | if (fabs(zhit-old_zhit)<EPS && i>0){ |
| 1241 | trajectory[0].numhits++; |
| 1242 | continue; |
| 1243 | } |
| 1244 | // propagate until we would step beyond the FDC hit plane |
| 1245 | bool done=false; |
| 1246 | while (!done){ |
| 1247 | double new_z=z+dz; |
| 1248 | |
| 1249 | if (new_z>zhit){ |
| 1250 | dz=zhit-z; |
| 1251 | new_z=zhit; |
| 1252 | done=true; |
| 1253 | } |
| 1254 | J(state_x,state_tx)=-dz; |
| 1255 | J(state_y,state_ty)=-dz; |
| 1256 | // Flight time: assume particle is moving at the speed of light |
| 1257 | t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98; |
| 1258 | //propagate the state to the next z position |
| 1259 | S(state_x)+=S(state_tx)*dz; |
| 1260 | S(state_y)+=S(state_ty)*dz; |
| 1261 | |
| 1262 | trajectory.push_front(trajectory_t(new_z,t,S,J,DMatrix4x1(), |
| 1263 | DMatrix4x4())); |
| 1264 | if (done){ |
| 1265 | trajectory[0].id=i+1; |
| 1266 | trajectory[0].numhits=1; |
| 1267 | } |
| 1268 | |
| 1269 | z=new_z; |
| 1270 | } |
| 1271 | old_zhit=zhit; |
| 1272 | } |
| 1273 | // One last step |
| 1274 | dz=1.1; |
| 1275 | J(state_x,state_tx)=-dz; |
| 1276 | J(state_y,state_ty)=-dz; |
| 1277 | |
| 1278 | // Flight time: assume particle is moving at the speed of light |
| 1279 | t+=dz*sqrt(1+S(state_tx)*S(state_tx)+S(state_ty)*S(state_ty))/29.98; |
| 1280 | |
| 1281 | //propagate the state to the next z position |
| 1282 | S(state_x)+=S(state_tx)*dz; |
| 1283 | S(state_y)+=S(state_ty)*dz; |
| 1284 | trajectory.push_front(trajectory_t(z+dz,t,S,J,DMatrix4x1(),DMatrix4x4())); |
| 1285 | |
| 1286 | if (false) |
| 1287 | { |
| 1288 | printf("Trajectory:\n"); |
| 1289 | for (unsigned int i=0;i<trajectory.size();i++){ |
| 1290 | printf(" x %f y %f z %f first hit %d num in layer %d\n",trajectory[i].S(state_x), |
| 1291 | trajectory[i].S(state_y),trajectory[i].z,trajectory[i].id, |
| 1292 | trajectory[i].numhits); |
| 1293 | } |
| 1294 | } |
| 1295 | |
| 1296 | return NOERROR; |
| 1297 | } |
| 1298 | |
| 1299 | // Perform Kalman Filter for the current trajectory |
| 1300 | jerror_t DTrackFitterStraightTrack::KalmanFilter(DMatrix4x1 &S,DMatrix4x4 &C, |
| 1301 | vector<int>&used_fdc_hits, |
| 1302 | vector<int>&used_cdc_hits, |
| 1303 | vector<fdc_update_t>&updates, |
| 1304 | vector<cdc_update_t>&cdc_updates, |
| 1305 | double &chi2,int &ndof){ |
| 1306 | DMatrix2x4 H; // Track projection matrix |
| 1307 | DMatrix4x2 H_T; // Transpose of track projection matrix |
| 1308 | DMatrix4x2 K; // Kalman gain matrix |
| 1309 | double VarMs=0.001; // kludge for material |
| 1310 | DMatrix2x2 V(0.0833,0.,0.,0.000256+VarMs); // Measurement variance |
| 1311 | DMatrix2x2 Vtemp,InvV; |
| 1312 | DMatrix2x1 Mdiff; |
| 1313 | DMatrix4x4 I; // identity matrix |
| 1314 | DMatrix4x4 J; // Jacobian matrix |
| 1315 | DMatrix4x1 S0; // State vector from reference trajectory |
| 1316 | |
| 1317 | DMatrix1x4 H_CDC; // Track projection matrix |
| 1318 | DMatrix4x1 H_T_CDC; // Transpose of track projection matrix |
| 1319 | DMatrix4x1 K_CDC; // Kalman gain matrix |
| 1320 | double V_CDC; |
| 1321 | |
| 1322 | const double d_EPS=1e-8; |
| 1323 | |
| 1324 | // Zero out the vectors of used hit flags |
| 1325 | for (unsigned int i=0;i<used_fdc_hits.size();i++) used_fdc_hits[i]=0; |
| 1326 | for (unsigned int i=0;i<used_cdc_hits.size();i++) used_cdc_hits[i]=0; |
| 1327 | |
| 1328 | //Initialize chi2 and ndof |
| 1329 | chi2=0.; |
| 1330 | ndof=0; |
| 1331 | |
| 1332 | // Save the starting values for C and S in the deque |
| 1333 | trajectory[0].Skk=S; |
| 1334 | trajectory[0].Ckk=C; |
| 1335 | |
| 1336 | // Loop over all steps in the trajectory |
| 1337 | S0=trajectory[0].S; |
| 1338 | J=trajectory[0].J; |
| 1339 | |
| 1340 | // CDC index and wire position variables |
| 1341 | bool more_hits = cdchits.size() == 0 ? false: true; |
| 1342 | bool firstCDCStep=true; |
| 1343 | unsigned int cdc_index=0; |
| 1344 | const DCDCWire *wire; |
| 1345 | DVector3 origin,wdir,wirepos; |
| 1346 | double doca2=0.0, old_doca2=0.0; |
| 1347 | if(more_hits){ |
| 1348 | cdc_index=cdchits.size()-1; |
| 1349 | wire=cdchits[cdc_index]->wire; |
| 1350 | origin=wire->origin; |
| 1351 | double vz=wire->udir.z(); |
| 1352 | if (VERBOSE) jout << " Additional CDC Hits in FDC track Starting in Ring " << wire->ring << endl; |
| 1353 | wdir=(1./vz)*wire->udir; |
| 1354 | } |
| 1355 | |
| 1356 | for (unsigned int k=1;k<trajectory.size();k++){ |
| 1357 | if (C(0,0)<=0. || C(1,1)<=0. || C(2,2)<=0. || C(3,3)<=0.) |
| 1358 | return UNRECOVERABLE_ERROR; |
| 1359 | |
| 1360 | // Propagate the state and covariance matrix forward in z |
| 1361 | S=trajectory[k].S+J*(S-S0); |
| 1362 | C=J*C*J.Transpose(); |
| 1363 | |
| 1364 | // Save the current state and covariance matrix in the deque |
| 1365 | trajectory[k].Skk=S; |
| 1366 | trajectory[k].Ckk=C; |
| 1367 | |
| 1368 | // Save S and J for the next step |
| 1369 | S0=trajectory[k].S; |
| 1370 | J=trajectory[k].J; |
| 1371 | |
| 1372 | // Correct S and C for the hit |
| 1373 | if (trajectory[k].id>0){ |
| 1374 | unsigned int id=trajectory[k].id-1; |
| 1375 | |
| 1376 | double cospsi=cos(fdchits[id]->wire->angle); |
| 1377 | double sinpsi=sin(fdchits[id]->wire->angle); |
| 1378 | |
| 1379 | // Small angle alignment correction |
| 1380 | double x = S(state_x) + fdchits[id]->wire->angles.Z()*S(state_y); |
| 1381 | double y = S(state_y) - fdchits[id]->wire->angles.Z()*S(state_x); |
| 1382 | //tz = 1. + my_fdchits[id]->phiY*tx - my_fdchits[id]->phiX*ty; |
| 1383 | double tx = (S(state_tx) + fdchits[id]->wire->angles.Z()*S(state_ty) - fdchits[id]->wire->angles.Y()); |
| 1384 | double ty = (S(state_ty) - fdchits[id]->wire->angles.Z()*S(state_tx) + fdchits[id]->wire->angles.X()); |
| 1385 | |
| 1386 | if (std::isnan(x) || std::isnan(y)) return UNRECOVERABLE_ERROR; |
| 1387 | |
| 1388 | // x,y and tx,ty in local coordinate system |
| 1389 | // To transform from (x,y) to (u,v), need to do a rotation: |
| 1390 | // u = x*cos(psi)-y*sin(psi) |
| 1391 | // v = y*cos(psi)+x*sin(psi) |
| 1392 | // (without alignment offsets) |
| 1393 | double vpred_wire_plane=y*cospsi+x*sinpsi; |
| 1394 | double upred_wire_plane=x*cospsi-y*sinpsi; |
| 1395 | double tu=tx*cospsi-ty*sinpsi; |
| 1396 | double tv=tx*sinpsi+ty*cospsi; |
| 1397 | |
| 1398 | // Variables for angle of incidence with respect to the z-direction in |
| 1399 | // the u-z plane |
| 1400 | double alpha=atan(tu); |
| 1401 | double cosalpha=cos(alpha); |
| 1402 | double cos2_alpha=cosalpha*cosalpha; |
| 1403 | double sinalpha=sin(alpha); |
| 1404 | double sin2_alpha=sinalpha*sinalpha; |
| 1405 | double cos2_alpha_minus_sin2_alpha=cos2_alpha-sin2_alpha; |
| 1406 | |
| 1407 | // Difference between measurement and projection |
| 1408 | for (int m=trajectory[k].numhits-1;m>=0;m--){ |
| 1409 | unsigned int my_id=id+m; |
| 1410 | double uwire=fdchits[my_id]->w; |
| 1411 | // (signed) distance of closest approach to wire |
| 1412 | double du=upred_wire_plane-uwire; |
| 1413 | double doca=du*cosalpha; |
| 1414 | |
| 1415 | // Predicted avalanche position along the wire |
| 1416 | double vpred=vpred_wire_plane; |
| 1417 | |
| 1418 | // Measured position of hit along wire |
| 1419 | double v=fdchits[my_id]->s; |
| 1420 | |
| 1421 | // Difference between measurements and predictions |
| 1422 | double drift=0.; // assume hit at wire position |
| 1423 | if (fit_type==kTimeBased){ |
| 1424 | double drift_time=fdchits[my_id]->time-trajectory[k].t; |
| 1425 | drift=(du>0.0?1.:-1.)*fdc_drift_distance(drift_time); |
| 1426 | V(0,0)=fdc_drift_variance(drift_time)+VarMs; |
| 1427 | } |
| 1428 | Mdiff(0)=drift-doca; |
| 1429 | Mdiff(1)=v-vpred; |
| 1430 | |
| 1431 | // Matrix for transforming from state-vector space to measurement space |
| 1432 | H_T(state_x,0)=cospsi*cosalpha; |
| 1433 | H_T(state_y,0)=-sinpsi*cosalpha; |
| 1434 | double temp=-du*sinalpha*cos2_alpha; |
| 1435 | H_T(state_tx,0)=cospsi*temp; |
| 1436 | H_T(state_ty,0)=-sinpsi*temp; |
| 1437 | double temp2=cosalpha*sinalpha*tv; |
| 1438 | H_T(state_x,1)=sinpsi-temp2*cospsi; |
| 1439 | H_T(state_y,1)=cospsi+temp2*sinpsi; |
| 1440 | double temp4=sinalpha*doca; |
| 1441 | double temp5=tv*cos2_alpha*du*cos2_alpha_minus_sin2_alpha; |
| 1442 | H_T(state_tx,1)=-sinpsi*temp4-cospsi*temp5; |
| 1443 | H_T(state_ty,1)=-cospsi*temp4+sinpsi*temp5; |
| 1444 | |
| 1445 | // Matrix transpose H_T -> H |
| 1446 | H(0,state_x)=H_T(state_x,0); |
| 1447 | H(0,state_y)=H_T(state_y,0); |
| 1448 | H(0,state_tx)=H_T(state_tx,0); |
| 1449 | H(0,state_ty)=H_T(state_ty,0); |
| 1450 | H(1,state_x)=H_T(state_x,1); |
| 1451 | H(1,state_y)=H_T(state_y,1); |
| 1452 | H(1,state_tx)=H_T(state_tx,1); |
| 1453 | H(1,state_ty)=H_T(state_ty,1); |
| 1454 | |
| 1455 | // Variance for this hit |
| 1456 | InvV=(V+H*C*H_T).Invert(); |
| 1457 | |
| 1458 | // Compute Kalman gain matrix |
| 1459 | K=(C*H_T)*InvV; |
| 1460 | |
| 1461 | if (fdchits[my_id]->wire->layer!=PLANE_TO_SKIP){ |
| 1462 | /* |
| 1463 | if(DEBUG_HISTS){ |
| 1464 | hFDCOccTrkFit->Fill(fdchits[my_id]->wire->layer); |
| 1465 | } |
| 1466 | */ |
| 1467 | // Update the state vector |
| 1468 | S+=K*Mdiff; |
| 1469 | if(VERBOSE) S.Print(); |
| 1470 | // Update state vector covariance matrix |
| 1471 | C=C-K*(H*C); |
| 1472 | |
| 1473 | // Update the filtered measurement covariane matrix and put results in |
| 1474 | // update vector |
| 1475 | DMatrix2x2 RC=V-H*C*H_T; |
| 1476 | DMatrix2x1 res=Mdiff-H*K*Mdiff; |
| 1477 | |
| 1478 | chi2+=RC.Chi2(res); |
| 1479 | ndof+=2; |
| 1480 | |
| 1481 | // fill pull vector entries |
| 1482 | updates[my_id].V=RC; |
| 1483 | } |
| 1484 | else{ |
| 1485 | updates[my_id].V=V; |
| 1486 | } |
| 1487 | |
| 1488 | used_fdc_hits[my_id]=1; |
| 1489 | |
| 1490 | // fill pull vector |
| 1491 | updates[my_id].d=doca; |
| 1492 | updates[my_id].S=S; |
| 1493 | updates[my_id].C=C; |
| 1494 | updates[my_id].tdrift=fdchits[my_id]->time-trajectory[k].t; |
| 1495 | updates[my_id].s=29.98*trajectory[k].t; // assume beta=1 |
| 1496 | } |
| 1497 | } |
| 1498 | |
| 1499 | if (more_hits && trajectory[k].z < downstreamEndplate){ |
| 1500 | // Position along wire |
| 1501 | double z0=origin.Z(); |
| 1502 | wirepos=origin+(trajectory[k].z-z0)*wdir; |
| 1503 | |
| 1504 | // New doca^2 |
| 1505 | double dx=S(state_x)-wirepos.X(); |
| 1506 | double dy=S(state_y)-wirepos.Y(); |
| 1507 | doca2=dx*dx+dy*dy; |
| 1508 | if (VERBOSE > 10) jout<< "At Position " << S(state_x) << " " << S(state_y) << " " << trajectory[k].z << " doca2 " << doca2 << endl; |
| 1509 | |
| 1510 | if (doca2>old_doca2 && more_hits && !firstCDCStep){ |
| 1511 | |
| 1512 | // zero-position and direction of line describing particle trajectory |
| 1513 | double tx=S(state_tx),ty=S(state_ty); |
| 1514 | DVector3 pos0(S(state_x),S(state_y),trajectory[k].z); |
| 1515 | DVector3 tdir(tx,ty,1.); |
| 1516 | |
| 1517 | // Find the true doca to the wire |
| 1518 | DVector3 diff=pos0-origin; |
| 1519 | double dx0=diff.x(),dy0=diff.y(); |
| 1520 | double wdir_dot_diff=diff.Dot(wdir); |
| 1521 | double tdir_dot_diff=diff.Dot(tdir); |
| 1522 | double tdir_dot_wdir=tdir.Dot(wdir); |
| 1523 | double tdir2=tdir.Mag2(); |
| 1524 | double wdir2=wdir.Mag2(); |
| 1525 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
| 1526 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
| 1527 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
| 1528 | double scale=1./D; |
| 1529 | double s=scale*N; |
| 1530 | double t=scale*N1; |
| 1531 | diff+=s*tdir-t*wdir; |
| 1532 | double d=diff.Mag()+d_EPS; // prevent division by zero |
| 1533 | |
| 1534 | // The next measurement |
| 1535 | double dmeas=0.39,delta=0.; |
| 1536 | double tdrift=cdchits[cdc_index]->tdrift-trajectory[k].t; |
| 1537 | if (fit_type==kTimeBased){ |
| 1538 | double phi_d=diff.Phi(); |
| 1539 | double dphi=phi_d-origin.Phi(); |
| 1540 | while (dphi>M_PI3.14159265358979323846) dphi-=2*M_PI3.14159265358979323846; |
| 1541 | while (dphi<-M_PI3.14159265358979323846) dphi+=2*M_PI3.14159265358979323846; |
| 1542 | |
| 1543 | int ring_index=cdchits[cdc_index]->wire->ring-1; |
| 1544 | int straw_index=cdchits[cdc_index]->wire->straw-1; |
| 1545 | double dz=t*wdir.z(); |
| 1546 | double delta=max_sag[ring_index][straw_index]*(1.-dz*dz/5625.) |
| 1547 | *cos(phi_d+sag_phi_offset[ring_index][straw_index]); |
| 1548 | CDCDriftParameters(dphi,delta,tdrift,dmeas,V_CDC); |
| 1549 | } |
| 1550 | |
| 1551 | // residual |
| 1552 | double res=dmeas-d; |
| 1553 | if (VERBOSE>5) jout << " Residual " << res << endl; |
| 1554 | // Track projection |
| 1555 | double one_over_d=1./d; |
| 1556 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
| 1557 | double wx=wdir.x(),wy=wdir.y(); |
| 1558 | |
| 1559 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
| 1560 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
| 1561 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
| 1562 | |
| 1563 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
| 1564 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
| 1565 | double dtdty=scale*(dN1dty-t*dDdty); |
| 1566 | |
| 1567 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
| 1568 | double dsdtx=scale*(dNdtx-s*dDdtx); |
| 1569 | |
| 1570 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
| 1571 | double dsdty=scale*(dNdty-s*dDdty); |
| 1572 | |
| 1573 | H_CDC(state_tx)=H_T_CDC(state_tx) |
| 1574 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
| 1575 | +diffz*(dsdtx-dtdtx)); |
| 1576 | H_CDC(state_ty)=H_T_CDC(state_ty) |
| 1577 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
| 1578 | +diffz*(dsdty-dtdty)); |
| 1579 | |
| 1580 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
| 1581 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
| 1582 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
| 1583 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
| 1584 | |
| 1585 | H_CDC(state_x)=H_T_CDC(state_x) |
| 1586 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
| 1587 | +diffz*(dsdx-dtdx)); |
| 1588 | H_CDC(state_y)=H_T_CDC(state_y) |
| 1589 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
| 1590 | +diffz*(dsdy-dtdy)); |
| 1591 | |
| 1592 | double InvV=1./(V_CDC+H_CDC*C*H_T_CDC); |
| 1593 | |
| 1594 | // Check how far this hit is from the projection |
| 1595 | double chi2check=res*res*InvV; |
| 1596 | if (chi2check < CHI2CUT || DO_PRUNING == 0){ |
| 1597 | if (VERBOSE) jout << "CDC Hit Added to FDC track " << endl; |
| 1598 | if (cdchits[cdc_index]->wire->ring!=RING_TO_SKIP){ |
| 1599 | |
| 1600 | // Compute Kalman gain matrix |
| 1601 | K_CDC=InvV*(C*H_T_CDC); |
| 1602 | // Update state vector covariance matrix |
| 1603 | DMatrix4x4 Ctest=C-K_CDC*(H_CDC*C); |
| 1604 | |
| 1605 | //C.Print(); |
| 1606 | //K.Print(); |
| 1607 | //Ctest.Print(); |
| 1608 | |
| 1609 | // Check that Ctest is positive definite |
| 1610 | if (!Ctest.IsPosDef()) return VALUE_OUT_OF_RANGE; |
| 1611 | C=Ctest; |
| 1612 | if(VERBOSE>10) C.Print(); |
| 1613 | // Update the state vector |
| 1614 | //S=S+res*K; |
| 1615 | S+=res*K_CDC; |
| 1616 | if(VERBOSE) {jout << "traj[z]=" << trajectory[k].z<< endl; S.Print();} |
| 1617 | |
| 1618 | // Compute new residual |
| 1619 | //d=finder->FindDoca(trajectory[k].z,S,wdir,origin); |
| 1620 | res=res-H_CDC*K_CDC*res; |
| 1621 | |
| 1622 | // Update chi2 |
| 1623 | double fit_V=V_CDC-H_CDC*C*H_T_CDC; |
| 1624 | chi2+=res*res/fit_V; |
| 1625 | ndof++; |
| 1626 | |
| 1627 | // fill pull vector entry |
| 1628 | cdc_updates[cdc_index].V=fit_V; |
| 1629 | } |
| 1630 | else { |
| 1631 | cdc_updates[cdc_index].V=V_CDC; |
| 1632 | } |
| 1633 | |
| 1634 | // fill updates |
| 1635 | cdc_updates[cdc_index].resi=res; |
| 1636 | cdc_updates[cdc_index].d=d; |
| 1637 | cdc_updates[cdc_index].delta=delta; |
| 1638 | cdc_updates[cdc_index].S=S; |
| 1639 | cdc_updates[cdc_index].C=C; |
| 1640 | cdc_updates[cdc_index].tdrift=tdrift; |
| 1641 | cdc_updates[cdc_index].ddrift=dmeas; |
| 1642 | cdc_updates[cdc_index].s=29.98*trajectory[k].t; // assume beta=1 |
| 1643 | trajectory[k].id=cdc_index+1000; |
| 1644 | |
| 1645 | used_cdc_hits[cdc_index]=1; |
| 1646 | } |
| 1647 | // move to next cdc hit |
| 1648 | if (cdc_index>0){ |
| 1649 | cdc_index--; |
| 1650 | |
| 1651 | //New wire position |
| 1652 | wire=cdchits[cdc_index]->wire; |
| 1653 | if (VERBOSE>5) jout << " Next Wire ring " << wire->ring << endl; |
| 1654 | origin=wire->origin; |
| 1655 | double vz=wire->udir.z(); |
| 1656 | wdir=(1./vz)*wire->udir; |
| 1657 | wirepos=origin+((trajectory[k].z-z0))*wdir; |
| 1658 | |
| 1659 | // New doca^2 |
| 1660 | dx=S(state_x)-wirepos.x(); |
| 1661 | dy=S(state_y)-wirepos.y(); |
| 1662 | doca2=dx*dx+dy*dy; |
| 1663 | |
| 1664 | } |
| 1665 | else more_hits=false; |
| 1666 | } |
| 1667 | firstCDCStep=false; |
| 1668 | old_doca2=doca2; |
| 1669 | } |
| 1670 | } |
| 1671 | |
| 1672 | ndof-=4; |
| 1673 | |
| 1674 | return NOERROR; |
| 1675 | } |
| 1676 | |
| 1677 | |
| 1678 | // Smoothing algorithm for the forward trajectory. Updates the state vector |
| 1679 | // at each step (going in the reverse direction to the filter) based on the |
| 1680 | // information from all the steps and outputs the state vector at the |
| 1681 | // outermost step. |
| 1682 | |
| 1683 | jerror_t |
| 1684 | DTrackFitterStraightTrack::Smooth(vector<fdc_update_t>&fdc_updates, |
| 1685 | vector<cdc_update_t>&cdc_updates){ |
| 1686 | unsigned int max=best_trajectory.size()-1; |
| 1687 | DMatrix4x1 S=(best_trajectory[max].Skk); |
| 1688 | DMatrix4x4 C=(best_trajectory[max].Ckk); |
| 1689 | DMatrix4x4 JT=best_trajectory[max].J.Transpose(); |
| 1690 | DMatrix4x1 Ss=S; |
| 1691 | DMatrix4x4 Cs=C; |
| 1692 | DMatrix4x4 A,dC; |
| 1693 | |
| 1694 | const double d_EPS=1e-8; |
| 1695 | |
| 1696 | for (unsigned int m=max-1;m>0;m--){ |
| 1697 | if (best_trajectory[m].id>0 && best_trajectory[m].id<1000){ // FDC Hit |
| 1698 | unsigned int id=best_trajectory[m].id-1; |
| 1699 | A=fdc_updates[id].C*JT*C.Invert(); |
| 1700 | Ss=fdc_updates[id].S+A*(Ss-S); |
| 1701 | |
| 1702 | dC=A*(Cs-C)*A.Transpose(); |
| 1703 | Cs=fdc_updates[id].C+dC; |
| 1704 | |
| 1705 | double cosa=cos(fdchits[id]->wire->angle); |
| 1706 | double cos2a=cos(2*fdchits[id]->wire->angle); |
| 1707 | double sina=sin(fdchits[id]->wire->angle); |
| 1708 | double u=fdchits[id]->w; |
| 1709 | double v=fdchits[id]->s; |
| 1710 | |
| 1711 | // Small angle alignment correction |
| 1712 | double x = Ss(state_x) + fdchits[id]->wire->angles.Z() * Ss(state_y); |
| 1713 | double y = Ss(state_y) - fdchits[id]->wire->angles.Z() * Ss(state_x); |
| 1714 | // tz = 1.0 + my_fdchits[id]->phiY * tx - my_fdchits[id]->phiX * ty; |
| 1715 | double tx = Ss(state_tx) + fdchits[id]->wire->angles.Z() * Ss(state_ty) - fdchits[id]->wire->angles.Y(); |
| 1716 | double ty = Ss(state_ty) - fdchits[id]->wire->angles.Z() * Ss(state_tx) + fdchits[id]->wire->angles.X(); |
| 1717 | |
| 1718 | // Projected position along the wire |
| 1719 | double vpred=x*sina+y*cosa; |
| 1720 | |
| 1721 | // Projected position in the plane of the wires transverse to the wires |
| 1722 | double upred=x*cosa-y*sina; |
| 1723 | |
| 1724 | // Direction tangent in the u-z plane |
| 1725 | double tu=tx*cosa-ty*sina; |
| 1726 | double alpha=atan(tu); |
| 1727 | double cosalpha=cos(alpha); |
| 1728 | //double cosalpha2=cosalpha*cosalpha; |
| 1729 | double sinalpha=sin(alpha); |
| 1730 | |
| 1731 | // (signed) distance of closest approach to wire |
| 1732 | double du=upred-u; |
| 1733 | double doca=du*cosalpha; |
| 1734 | // Difference between measurement and projection for the cathodes |
| 1735 | double tv=tx*sina+ty*cosa; |
| 1736 | double resi_c=v-vpred; |
| 1737 | |
| 1738 | // Difference between measurement and projection perpendicular to the wire |
| 1739 | double drift = 0.0; // assume hit at wire position |
| 1740 | int left_right = -999; |
| 1741 | double drift_time = fdc_updates[id].tdrift; |
| 1742 | if (fit_type == kTimeBased) { |
| 1743 | drift = (du > 0.0 ? 1.0 : -1.0) * fdc_drift_distance(drift_time); |
| 1744 | left_right = (du > 0.0 ? +1 : -1); |
| 1745 | } |
| 1746 | double resi_a = drift - doca; |
| 1747 | |
| 1748 | // Variance from filter step |
| 1749 | DMatrix2x2 V=fdc_updates[id].V; |
| 1750 | // Compute projection matrix and find the variance for the residual |
| 1751 | DMatrix4x2 H_T; |
| 1752 | double temp2=-tv*sinalpha; |
| 1753 | H_T(state_x,1)=sina+cosa*cosalpha*temp2; |
| 1754 | H_T(state_y,1)=cosa-sina*cosalpha*temp2; |
| 1755 | |
| 1756 | double cos2_minus_sin2=cosalpha*cosalpha-sinalpha*sinalpha; |
| 1757 | double doca_cosalpha=doca*cosalpha; |
| 1758 | H_T(state_tx,1)=-doca_cosalpha*(tu*sina+tv*cosa*cos2_minus_sin2); |
| 1759 | H_T(state_ty,1)=-doca_cosalpha*(tu*cosa-tv*sina*cos2_minus_sin2); |
| 1760 | |
| 1761 | H_T(state_x,0)=cosa*cosalpha; |
| 1762 | H_T(state_y,0)=-sina*cosalpha; |
| 1763 | double one_plus_tu2=1.+tu*tu; |
| 1764 | double factor=du*tu/sqrt(one_plus_tu2)/one_plus_tu2; |
| 1765 | H_T(state_ty,0)=sina*factor; |
| 1766 | H_T(state_tx,0)=-cosa*factor; |
| 1767 | |
| 1768 | // Matrix transpose H_T -> H |
| 1769 | DMatrix2x4 H; |
| 1770 | H(0,state_x)=H_T(state_x,0); |
| 1771 | H(0,state_y)=H_T(state_y,0); |
| 1772 | H(0,state_tx)=H_T(state_tx,0); |
| 1773 | H(0,state_ty)=H_T(state_ty,0); |
| 1774 | H(1,state_x)=H_T(state_x,1); |
| 1775 | H(1,state_y)=H_T(state_y,1); |
| 1776 | H(1,state_tx)=H_T(state_tx,1); |
| 1777 | H(1,state_ty)=H_T(state_ty,1); |
| 1778 | |
| 1779 | if (fdchits[id]->wire->layer == PLANE_TO_SKIP) { |
| 1780 | // V += Cs.SandwichMultiply(H_T); |
| 1781 | V = V + H * Cs * H_T; |
| 1782 | } else { |
| 1783 | // V -= dC.SandwichMultiply(H_T); |
| 1784 | |
| 1785 | // R. Fruehwirth, Nucl. Instrum. Methods Phys. Res. A 262, 444 (1987). |
| 1786 | // Eq. (9) |
| 1787 | // The following V (lhs) corresponds to R^n_k in the paper. |
| 1788 | // dC corresponds to 'A_k * (C^n_{k+1} - C^k_{k+1}) * A_k^T' in the paper. |
| 1789 | V = V - H * dC * H_T; |
| 1790 | } |
| 1791 | |
| 1792 | /* |
| 1793 | if(DEBUG_HISTS){ |
| 1794 | hFDCOccTrkSmooth->Fill(fdchits[id]->wire->layer); |
| 1795 | } |
| 1796 | */ |
| 1797 | // Implement derivatives wrt track parameters needed for millepede alignment |
| 1798 | // Add the pull |
| 1799 | double scale=1./sqrt(1.+tx*tx+ty*ty); |
| 1800 | double cosThetaRel=fdchits[id]->wire->udir.Dot(DVector3(scale*tx,scale*ty,scale)); |
| 1801 | DTrackFitter::pull_t thisPull(resi_a,sqrt(V(0,0)), |
| 1802 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
| 1803 | fdc_updates[id].tdrift, |
| 1804 | fdc_updates[id].d, |
| 1805 | NULL__null,fdchits[id], |
| 1806 | 0.0, //docaphi |
| 1807 | best_trajectory[m].z,cosThetaRel, |
| 1808 | 0.0, //tcorr |
| 1809 | resi_c, sqrt(V(1,1)) |
| 1810 | ); |
| 1811 | thisPull.left_right = left_right; |
| 1812 | |
| 1813 | if (fdchits[id]->wire->layer!=PLANE_TO_SKIP){ |
| 1814 | vector<double> derivatives; |
| 1815 | derivatives.resize(FDCTrackD::size); |
| 1816 | |
| 1817 | //dDOCAW/dDeltaX |
| 1818 | derivatives[FDCTrackD::dDOCAW_dDeltaX] = -(1/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
| 1819 | |
| 1820 | //dDOCAW/dDeltaZ |
| 1821 | derivatives[FDCTrackD::dDOCAW_dDeltaZ] = (tx*cosa - ty*sina)/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
| 1822 | |
| 1823 | //dDOCAW/ddeltaPhiX |
| 1824 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiX] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
| 1825 | |
| 1826 | //dDOCAW/ddeltaphiY |
| 1827 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiY] = (cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
| 1828 | |
| 1829 | //dDOCAW/ddeltaphiZ |
| 1830 | derivatives[FDCTrackD::dDOCAW_dDeltaPhiZ] = (tx*ty*u*cos2a + (x + pow(ty,2)*x - tx*ty*y)*sina + |
| 1831 | cosa*(-(tx*ty*x) + y + pow(tx,2)*y + (pow(tx,2) - pow(ty,2))*u*sina))/ |
| 1832 | pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
| 1833 | |
| 1834 | // dDOCAW/dx |
| 1835 | derivatives[FDCTrackD::dDOCAW_dx] = cosa/sqrt(1 + pow(tx*cosa - ty*sina,2)); |
| 1836 | |
| 1837 | // dDOCAW/dy |
| 1838 | derivatives[FDCTrackD::dDOCAW_dy] = -(sina/sqrt(1 + pow(tx*cosa - ty*sina,2))); |
| 1839 | |
| 1840 | // dDOCAW/dtx |
| 1841 | derivatives[FDCTrackD::dDOCAW_dtx] = -((cosa*(tx*cosa - ty*sina)*(-u + x*cosa - y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5)); |
| 1842 | |
| 1843 | // dDOCAW/dty |
| 1844 | derivatives[FDCTrackD::dDOCAW_dty] = (sina*(-(tx*cosa) + ty*sina)*(u - x*cosa + y*sina))/pow(1 + pow(tx*cosa - ty*sina,2),1.5); |
| 1845 | |
| 1846 | // dDOCAW/dt0 |
| 1847 | double t0shift = 4.0; // ns |
| 1848 | double drift_shift = 0.0; |
| 1849 | if (drift_time < 0.0) { |
| 1850 | drift_shift = drift; |
| 1851 | } else { |
| 1852 | drift_shift = |
| 1853 | (du > 0.0 ? 1.0 : -1.0) * |
| 1854 | fdc_drift_distance(drift_time + t0shift); |
| 1855 | } |
| 1856 | derivatives[FDCTrackD::dW_dt0] = (drift_shift - drift) / t0shift; |
| 1857 | |
| 1858 | // And the cathodes |
| 1859 | //dDOCAW/ddeltax |
| 1860 | derivatives[FDCTrackD::dDOCAC_dDeltaX] = 0.; |
| 1861 | |
| 1862 | //dDOCAW/ddeltax |
| 1863 | derivatives[FDCTrackD::dDOCAC_dDeltaZ] = ty*cosa + tx*sina; |
| 1864 | |
| 1865 | //dDOCAW/ddeltaPhiX |
| 1866 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiX] = 0.; |
| 1867 | |
| 1868 | //dDOCAW/ddeltaPhiX |
| 1869 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiY] = 0.; |
| 1870 | |
| 1871 | //dDOCAW/ddeltaPhiX |
| 1872 | derivatives[FDCTrackD::dDOCAC_dDeltaPhiZ] = -(x*cosa) + y*sina; |
| 1873 | |
| 1874 | // dDOCAW/dx |
| 1875 | derivatives[FDCTrackD::dDOCAC_dx] = sina; |
| 1876 | |
| 1877 | // dDOCAW/dy |
| 1878 | derivatives[FDCTrackD::dDOCAW_dy] = cosa; |
| 1879 | |
| 1880 | // dDOCAW/dtx |
| 1881 | derivatives[FDCTrackD::dDOCAW_dtx] = 0.; |
| 1882 | |
| 1883 | // dDOCAW/dty |
| 1884 | derivatives[FDCTrackD::dDOCAW_dty] = 0.; |
| 1885 | |
| 1886 | thisPull.AddTrackDerivatives(derivatives); |
| 1887 | } |
| 1888 | pulls.push_back(thisPull); |
| 1889 | } |
| 1890 | else if (best_trajectory[m].id>=1000){ // CDC Hit |
| 1891 | unsigned int id=best_trajectory[m].id-1000; |
| 1892 | A=cdc_updates[id].C*JT*C.Invert(); |
| 1893 | Ss=cdc_updates[id].S+A*(Ss-S); |
| 1894 | |
| 1895 | dC=A*(Cs-C)*A.Transpose(); |
| 1896 | Cs=cdc_updates[id].C+dC; |
| 1897 | if (VERBOSE > 10) { |
| 1898 | jout << " In Smoothing Step Using ID " << id << "/" << cdc_updates.size() << " for ring " << cdchits[id]->wire->ring << endl; |
| 1899 | jout << " A cdc_updates[id].C Ss Cs " << endl; |
| 1900 | A.Print(); cdc_updates[id].C.Print(); Ss.Print(); Cs.Print(); |
| 1901 | } |
| 1902 | if(!Cs.IsPosDef()) { |
| 1903 | if (VERBOSE) jout << "Cs is not PosDef!" << endl; |
| 1904 | return VALUE_OUT_OF_RANGE; |
| 1905 | } |
| 1906 | |
| 1907 | const DCDCWire *wire=cdchits[id]->wire; |
| 1908 | DVector3 origin=wire->origin; |
| 1909 | double z0=origin.z(); |
| 1910 | double vz=wire->udir.z(); |
| 1911 | DVector3 wdir=(1./vz)*wire->udir; |
| 1912 | DVector3 wirepos=origin+(best_trajectory[m].z-z0)*wdir; |
| 1913 | // Position and direction from state vector |
| 1914 | double x=Ss(state_x); |
| 1915 | double y=Ss(state_y); |
| 1916 | double tx=Ss(state_tx); |
| 1917 | double ty=Ss(state_ty); |
| 1918 | |
| 1919 | DVector3 pos0(x,y,best_trajectory[m].z); |
| 1920 | DVector3 tdir(tx,ty,1.); |
| 1921 | |
| 1922 | // Find the true doca to the wire |
| 1923 | DVector3 diff=pos0-origin; |
| 1924 | double dx0=diff.x(),dy0=diff.y(); |
| 1925 | double wdir_dot_diff=diff.Dot(wdir); |
| 1926 | double tdir_dot_diff=diff.Dot(tdir); |
| 1927 | double tdir_dot_wdir=tdir.Dot(wdir); |
| 1928 | double tdir2=tdir.Mag2(); |
| 1929 | double wdir2=wdir.Mag2(); |
| 1930 | double D=tdir2*wdir2-tdir_dot_wdir*tdir_dot_wdir; |
| 1931 | double N=tdir_dot_wdir*wdir_dot_diff-wdir2*tdir_dot_diff; |
| 1932 | double N1=tdir2*wdir_dot_diff-tdir_dot_wdir*tdir_dot_diff; |
| 1933 | double scale=1./D; |
| 1934 | double s=scale*N; |
| 1935 | double t=scale*N1; |
| 1936 | diff+=s*tdir-t*wdir; |
| 1937 | double d=diff.Mag()+d_EPS; // prevent division by zero |
| 1938 | double ddrift = cdc_updates[id].ddrift; |
| 1939 | |
| 1940 | double resi = ddrift - d; |
| 1941 | |
| 1942 | |
| 1943 | // Track projection |
| 1944 | DMatrix1x4 H; DMatrix4x1 H_T; |
| 1945 | { |
| 1946 | double one_over_d=1./d; |
| 1947 | double diffx=diff.x(),diffy=diff.y(),diffz=diff.z(); |
| 1948 | double wx=wdir.x(),wy=wdir.y(); |
| 1949 | |
| 1950 | double dN1dtx=2.*tx*wdir_dot_diff-wx*tdir_dot_diff-tdir_dot_wdir*dx0; |
| 1951 | double dDdtx=2.*tx*wdir2-2.*tdir_dot_wdir*wx; |
| 1952 | double dtdtx=scale*(dN1dtx-t*dDdtx); |
| 1953 | |
| 1954 | double dN1dty=2.*ty*wdir_dot_diff-wy*tdir_dot_diff-tdir_dot_wdir*dy0; |
| 1955 | double dDdty=2.*ty*wdir2-2.*tdir_dot_wdir*wy; |
| 1956 | double dtdty=scale*(dN1dty-t*dDdty); |
| 1957 | |
| 1958 | double dNdtx=wx*wdir_dot_diff-wdir2*dx0; |
| 1959 | double dsdtx=scale*(dNdtx-s*dDdtx); |
| 1960 | |
| 1961 | double dNdty=wy*wdir_dot_diff-wdir2*dy0; |
| 1962 | double dsdty=scale*(dNdty-s*dDdty); |
| 1963 | |
| 1964 | H(state_tx)=H_T(state_tx) |
| 1965 | =one_over_d*(diffx*(s+tx*dsdtx-wx*dtdtx)+diffy*(ty*dsdtx-wy*dtdtx) |
| 1966 | +diffz*(dsdtx-dtdtx)); |
| 1967 | H(state_ty)=H_T(state_ty) |
| 1968 | =one_over_d*(diffx*(tx*dsdty-wx*dtdty)+diffy*(s+ty*dsdty-wy*dtdty) |
| 1969 | +diffz*(dsdty-dtdty)); |
| 1970 | |
| 1971 | double dsdx=scale*(tdir_dot_wdir*wx-wdir2*tx); |
| 1972 | double dtdx=scale*(tdir2*wx-tdir_dot_wdir*tx); |
| 1973 | double dsdy=scale*(tdir_dot_wdir*wy-wdir2*ty); |
| 1974 | double dtdy=scale*(tdir2*wy-tdir_dot_wdir*ty); |
| 1975 | |
| 1976 | H(state_x)=H_T(state_x) |
| 1977 | =one_over_d*(diffx*(1.+dsdx*tx-dtdx*wx)+diffy*(dsdx*ty-dtdx*wy) |
| 1978 | +diffz*(dsdx-dtdx)); |
| 1979 | H(state_y)=H_T(state_y) |
| 1980 | =one_over_d*(diffx*(dsdy*tx-dtdy*wx)+diffy*(1.+dsdy*ty-dtdy*wy) |
| 1981 | +diffz*(dsdy-dtdy)); |
| 1982 | } |
| 1983 | double V=cdc_updates[id].V; |
| 1984 | |
| 1985 | if (VERBOSE > 10) jout << " d " << d << " H*S " << H*S << endl; |
| 1986 | if (cdchits[id]->wire->ring==RING_TO_SKIP){ |
| 1987 | V=V+H*Cs*H_T; |
| 1988 | } |
| 1989 | else{ |
| 1990 | V=V-H*Cs*H_T; |
| 1991 | } |
| 1992 | if (V<0) return VALUE_OUT_OF_RANGE; |
| 1993 | |
| 1994 | // Add the pull |
| 1995 | double myscale=1./sqrt(1.+tx*tx+ty*ty); |
| 1996 | double cosThetaRel=wire->udir.Dot(DVector3(myscale*tx,myscale*ty,myscale)); |
| 1997 | DTrackFitter::pull_t thisPull(resi,sqrt(V), |
| 1998 | best_trajectory[m].t*SPEED_OF_LIGHT29.9792458, |
| 1999 | cdc_updates[id].tdrift, |
| 2000 | d,cdchits[id], NULL__null, |
| 2001 | diff.Phi(), //docaphi |
| 2002 | best_trajectory[m].z,cosThetaRel, |
| 2003 | cdc_updates[id].tdrift); |
| 2004 | |
| 2005 | // Derivatives for alignment |
| 2006 | double wtx=wire->udir.X(), wty=wire->udir.Y(), wtz=wire->udir.Z(); |
| 2007 | double wx=wire->origin.X(), wy=wire->origin.Y(), wz=wire->origin.Z(); |
| 2008 | |
| 2009 | double z=best_trajectory[m].z; |
| 2010 | double tx2=tx*tx, ty2=ty*ty; |
| 2011 | double wtx2=wtx*wtx, wty2=wty*wty, wtz2=wtz*wtz; |
| 2012 | double denom=(1 + ty2)*wtx2 + (1 + tx2)*wty2 - 2*ty*wty*wtz + (tx2 + ty2)*wtz2 - 2*tx*wtx*(ty*wty + wtz)+d_EPS; |
| 2013 | double denom2=denom*denom; |
| 2014 | double c1=-(wtx - tx*wtz)*(wy - y); |
| 2015 | double c2=wty*(wx - tx*wz - x + tx*z); |
| 2016 | double c3=ty*(-(wtz*wx) + wtx*wz + wtz*x - wtx*z); |
| 2017 | double dscale=0.5*(1/d); |
| 2018 | |
| 2019 | vector<double> derivatives(11); |
| 2020 | |
| 2021 | derivatives[CDCTrackD::dDOCAdOriginX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3))/denom; |
| 2022 | |
| 2023 | derivatives[CDCTrackD::dDOCAdOriginY]=dscale*(2*(-wtx + tx*wtz)*(c1 + c2 + c3))/denom; |
| 2024 | |
| 2025 | derivatives[CDCTrackD::dDOCAdOriginZ]=dscale*(2*(ty*wtx - tx*wty)*(c1 + c2 + c3))/denom; |
| 2026 | |
| 2027 | derivatives[CDCTrackD::dDOCAdDirX]=dscale*(2*(wty - ty*wtz)*(c1 + c2 + c3)* |
| 2028 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
| 2029 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
| 2030 | |
| 2031 | derivatives[CDCTrackD::dDOCAdDirY]=dscale*(-2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
| 2032 | (tx*(ty*wty + wtz)*(wx - x) + (wty - ty*wtz)*(-wy + y + ty*(wz - z)) + |
| 2033 | wtx*(-((1 + ty2)*wx) + (1 + ty2)*x + tx*(ty*wy + wz - ty*y - z)) + tx2*(wty*(-wy + y) + wtz*(-wz + z))))/denom2; |
| 2034 | |
| 2035 | derivatives[CDCTrackD::dDOCAdDirZ]=dscale*(-2*(ty*wtx - tx*wty)*(c1 + c2 + c3)* |
| 2036 | (-(tx*(ty*wty + wtz)*(wx - x)) + tx2*(wty*(wy - y) + wtz*(wz - z)) + (wty - ty*wtz)*(wy - y + ty*(-wz + z)) + |
| 2037 | wtx*((1 + ty2)*wx - (1 + ty2)*x + tx*(-(ty*wy) - wz + ty*y + z))))/denom2; |
| 2038 | |
| 2039 | derivatives[CDCTrackD::dDOCAdS0]=-derivatives[CDCTrackD::dDOCAdOriginX]; |
| 2040 | |
| 2041 | derivatives[CDCTrackD::dDOCAdS1]=-derivatives[CDCTrackD::dDOCAdOriginY]; |
| 2042 | |
| 2043 | derivatives[CDCTrackD::dDOCAdS2]=dscale*(2*(wty - ty*wtz)*(-c1 - c2 - c3)* |
| 2044 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
| 2045 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
| 2046 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
| 2047 | |
| 2048 | derivatives[CDCTrackD::dDOCAdS3]=dscale*(2*(wtx - tx*wtz)*(c1 + c2 + c3)* |
| 2049 | (-(wtx*wtz*wx) - wty*wtz*wy + wtx2*wz + wty2*wz + wtx*wtz*x + wty*wtz*y - wtx2*z - wty2*z + |
| 2050 | tx*(wty2*(wx - x) + wtx*wty*(-wy + y) + wtz*(wtz*wx - wtx*wz - wtz*x + wtx*z)) + |
| 2051 | ty*(wtx*wty*(-wx + x) + wtx2*(wy - y) + wtz*(wtz*wy - wty*wz - wtz*y + wty*z))))/denom2; |
| 2052 | |
| 2053 | thisPull.AddTrackDerivatives(derivatives); |
| 2054 | |
| 2055 | pulls.push_back(thisPull); |
| 2056 | } |
| 2057 | else{ |
| 2058 | A=best_trajectory[m].Ckk*JT*C.Invert(); |
| 2059 | Ss=best_trajectory[m].Skk+A*(Ss-S); |
| 2060 | Cs=best_trajectory[m].Ckk+A*(Cs-C)*A.Transpose(); |
| 2061 | } |
| 2062 | |
| 2063 | S=best_trajectory[m].Skk; |
| 2064 | C=best_trajectory[m].Ckk; |
| 2065 | JT=best_trajectory[m].J.Transpose(); |
| 2066 | } |
| 2067 | |
| 2068 | return NOERROR; |
| 2069 | } |
| 2070 | |
| 2071 | shared_ptr<TMatrixFSym> |
| 2072 | DTrackFitterStraightTrack::Get7x7ErrorMatrix(TMatrixFSym C,DMatrix4x1 &S,double sign){ |
| 2073 | auto C7x7 = dResourcePool_TMatrixFSym->Get_SharedResource(); |
| 2074 | C7x7->ResizeTo(7, 7); |
| 2075 | DMatrix J(7,5); |
| 2076 | |
| 2077 | double p=5.; // fixed: cannot measure |
| 2078 | double tx_=S(state_tx); |
| 2079 | double ty_=S(state_ty); |
| 2080 | double x_=S(state_x); |
| 2081 | double y_=S(state_y); |
| 2082 | double tanl=sign/sqrt(tx_*tx_+ty_*ty_); |
| 2083 | double tanl2=tanl*tanl; |
| 2084 | double lambda=atan(tanl); |
| 2085 | double sinl=sin(lambda); |
| 2086 | double sinl3=sinl*sinl*sinl; |
| 2087 | |
| 2088 | J(state_X,state_x)=J(state_Y,state_y)=1.; |
| 2089 | J(state_Pz,state_ty)=-p*ty_*sinl3; |
| 2090 | J(state_Pz,state_tx)=-p*tx_*sinl3; |
| 2091 | J(state_Px,state_ty)=J(state_Py,state_tx)=-p*tx_*ty_*sinl3; |
| 2092 | J(state_Px,state_tx)=p*(1.+ty_*ty_)*sinl3; |
| 2093 | J(state_Py,state_ty)=p*(1.+tx_*tx_)*sinl3; |
| 2094 | J(state_Pz,4)=-p*p*sinl; |
| 2095 | J(state_Px,4)=tx_*J(state_Pz,4); |
| 2096 | J(state_Py,4)=ty_*J(state_Pz,4); |
| 2097 | J(state_Z,state_x)=-tx_*tanl2; |
| 2098 | J(state_Z,state_y)=-ty_*tanl2; |
| 2099 | double diff=tx_*tx_-ty_*ty_; |
| 2100 | double frac=tanl2*tanl2; |
| 2101 | J(state_Z,state_tx)=(x_*diff+2.*tx_*ty_*y_)*frac; |
| 2102 | J(state_Z,state_ty)=(2.*tx_*ty_*x_-y_*diff)*frac; |
| 2103 | |
| 2104 | // C'= JCJ^T |
| 2105 | *C7x7=C.Similarity(J); |
| 2106 | |
| 2107 | return C7x7; |
| 2108 | } |
| 2109 | |
| 2110 | // Routine to get extrapolations to other detectors |
| 2111 | void DTrackFitterStraightTrack::GetExtrapolations(const DVector3 &pos0, |
| 2112 | const DVector3 &dir){ |
| 2113 | double x0=pos0.x(),y0=pos0.y(),z0=pos0.z(); |
| 2114 | double s=0.,t=0.; |
| 2115 | DVector3 pos(0,0,0); |
| 2116 | DVector3 diff(0,0,0); |
| 2117 | ClearExtrapolations(); |
| 2118 | |
| 2119 | double z=z0; |
| 2120 | double dz=0.1; |
| 2121 | double uz=dir.z(); |
| 2122 | double ux=dir.x()/uz,ux2=ux*ux; |
| 2123 | double uy=dir.y()/uz,uy2=uy*uy; |
| 2124 | |
| 2125 | // Extrapolate to start counter |
| 2126 | double Rd=SC_BARREL_R; |
| 2127 | double A=ux*x0 + uy*y0; |
| 2128 | double B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
| 2129 | double C=ux2 + uy2; |
| 2130 | if (sc_pos.empty()==false && z<SC_END_NOSE_Z && B>0){ |
| 2131 | dz=-(A-sqrt(B))/C; |
| 2132 | pos=pos0+(dz/uz)*dir; |
| 2133 | z=pos.z(); |
| 2134 | bool done=(z<sc_pos[0][0].z()||z>SC_END_NOSE_Z); |
| 2135 | while(!done){ |
| 2136 | double d_old=1000.,d=1000.; |
| 2137 | unsigned int index=0; |
| 2138 | |
| 2139 | for (unsigned int m=0;m<12;m++){ |
| 2140 | double dphi=pos.Phi()-SC_PHI_SECTOR1; |
| 2141 | if (dphi<0) dphi+=2.*M_PI3.14159265358979323846; |
| 2142 | index=int(floor(dphi/(2.*M_PI3.14159265358979323846/30.))); |
| 2143 | if (index>29) index=0; |
| 2144 | d=sc_norm[index][m].Dot(pos-sc_pos[index][m]); |
| 2145 | if (d*d_old<0){ // break if we cross the current plane |
| 2146 | pos+=d*dir; |
| 2147 | s=(pos-pos0).Mag(); |
| 2148 | t=s/29.98; |
| 2149 | extrapolations[SYS_START].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2150 | done=true; |
| 2151 | break; |
| 2152 | } |
| 2153 | d_old=d; |
| 2154 | } |
| 2155 | pos-=(0.1/uz)*dir; |
| 2156 | z=pos.z(); |
Value stored to 'z' is never read | |
| 2157 | } |
| 2158 | } |
| 2159 | // Extrapolate to BCAL |
| 2160 | Rd=65.; // approximate BCAL inner radius |
| 2161 | B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
| 2162 | if (B>0){ |
| 2163 | diff=-(A-sqrt(B))/(C*uz)*dir; |
| 2164 | s=diff.Mag(); |
| 2165 | t=s/29.98; |
| 2166 | pos=pos0+diff; |
| 2167 | extrapolations[SYS_BCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2168 | Rd=89.; // approximate BCAL outer radius |
| 2169 | B=uy2*(Rd - x0)*(Rd + x0) + 2*ux*uy*x0*y0 + ux2*(Rd - y0)*(Rd + y0); |
| 2170 | if (B>0){ |
| 2171 | diff=-(A-sqrt(B))/(C*uz)*dir; |
| 2172 | s=diff.Mag(); |
| 2173 | t=s/29.98; |
| 2174 | pos=pos0+diff; |
| 2175 | extrapolations[SYS_BCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2176 | } |
| 2177 | } |
| 2178 | |
| 2179 | // Extrapolate to TRD |
| 2180 | for (unsigned int i=0;i<dTRDz_vec.size();i++){ |
| 2181 | diff=((dTRDz_vec[i]-z0)/uz)*dir; |
| 2182 | pos=pos0+diff; |
| 2183 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
| 2184 | s=diff.Mag(); |
| 2185 | t=s/29.98; |
| 2186 | extrapolations[SYS_TRD].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2187 | } |
| 2188 | } |
| 2189 | |
| 2190 | // Extrapolate to DIRC |
| 2191 | diff=((dDIRCz-z0)/uz)*dir; |
| 2192 | pos=pos0+diff; |
| 2193 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
| 2194 | s=diff.Mag(); |
| 2195 | t=s/29.98; |
| 2196 | extrapolations[SYS_DIRC].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2197 | } |
| 2198 | |
| 2199 | // Extrapolate to TOF |
| 2200 | diff=((dTOFz-z0)/uz)*dir; |
| 2201 | pos=pos0+diff; |
| 2202 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
| 2203 | double s=diff.Mag(); |
| 2204 | double t=s/29.98; |
| 2205 | s=diff.Mag(); |
| 2206 | t=s/29.98; |
| 2207 | extrapolations[SYS_TOF].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2208 | } |
| 2209 | |
| 2210 | // Extrapolate to FCAL |
| 2211 | diff=((dFCALz-z0)/uz)*dir; |
| 2212 | pos=pos0+diff; |
| 2213 | if (fabs(pos.x())<130. && fabs(pos.y())<130.){ |
| 2214 | s=diff.Mag(); |
| 2215 | t=s/29.98; |
| 2216 | extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2217 | |
| 2218 | // extrapolate to exit of FCAL |
| 2219 | diff=((dFCALz+45.-z0)/uz)*dir; |
| 2220 | pos=pos0+diff; |
| 2221 | s=diff.Mag(); |
| 2222 | t=s/29.98; |
| 2223 | extrapolations[SYS_FCAL].push_back(DTrackFitter::Extrapolation_t(pos,dir,t,s)); |
| 2224 | } |
| 2225 | } |