Changes in external/TrackCovariance/SolTrack.cc [170a11d:ff9fb2d9] in git
- File:
-
- 1 edited
-
external/TrackCovariance/SolTrack.cc (modified) (5 diffs)
Legend:
- Unmodified
- Added
- Removed
-
external/TrackCovariance/SolTrack.cc
r170a11d rff9fb2d9 45 45 fCov.ResizeTo(5, 5); 46 46 } 47 SolTrack::SolTrack(TVector3 x, TVector3 p, SolGeom* G)48 {49 fG = G;50 // Store momentum51 fp[0] = p(0); fp[1] = p(1); fp[2] = p(2);52 Double_t px = p(0); Double_t py = p(1); Double_t pz = p(2);53 fx[0] = x(0); fx[1] = x(1); fx[2] = x(2);54 Double_t xx = x(0); Double_t yy = x(1); Double_t zz = x(2);55 // Store parameters56 Double_t pt = TMath::Sqrt(px * px + py * py);57 Double_t Charge = 1.0; // Don't worry about charge for now58 Double_t a = -Charge * G->B() * 0.2998; // Normalized speed of light59 Double_t C = a / (2 * pt); // pt in GeV, B in Tesla, C in meters60 Double_t r2 = xx * xx + yy * yy;61 Double_t cross = xx * py - yy * px;62 Double_t T = TMath::Sqrt(pt * pt - 2 * a * cross + a * a * r2);63 Double_t phi0 = TMath::ATan2((py - a * xx) / T, (px + a * yy) / T);64 Double_t D;65 if (pt < 10.0) D = (T - pt) / a;66 else D = (-2 * cross + a * r2) / (T + pt);67 Double_t B = C * TMath::Sqrt((r2 - D * D) / (1 + 2 * C * D));68 Double_t st = TMath::ASin(B) / C;69 Double_t ct = pz / pt;70 Double_t z0 = zz - ct * st;71 fpar[0] = D;72 fpar[1] = phi0;73 fpar[2] = C;74 fpar[3] = z0;75 fpar[4] = ct;76 //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;77 //78 // Init covariances79 //80 fCov.ResizeTo(5, 5);81 }82 47 83 48 SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G) … … 104 69 SolTrack::~SolTrack() 105 70 { 106 delete[] & fp;107 delete[] & fpar;108 71 fCov.Clear(); 109 72 } … … 169 132 return kh; 170 133 } 171 172 // # of measurement layers hit173 Int_t SolTrack::nmHit()174 {175 Int_t kmh = 0;176 Double_t R; Double_t phi; Double_t zz;177 for (Int_t i = 0; i < fG->Nl(); i++)178 if (HitLayer(i, R, phi, zz))if (fG->isMeasure(i))kmh++;179 //180 return kmh;181 }182 //183 184 185 134 // List of layers hit with intersections 186 135 Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh) … … 212 161 return kmh; 213 162 } 214 215 // List of XYZ measurements without any error216 Int_t SolTrack::HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh)217 {218 //219 // Return lists of hits associated to a track for all measurement layers.220 // Return value is the total number of measurement hits221 // kmh = total number of measurement layers hit for given type222 // ihh = pointer to layer number223 // Xh, Yh, Zh = X, Y, Z of hit - No measurement error - No multiple scattering224 //225 //226 Int_t kmh = 0; // Number of measurement layers hit227 for (Int_t i = 0; i < fG->Nl(); i++)228 {229 Double_t R; Double_t phi; Double_t zz;230 if (HitLayer(i, R, phi, zz)) // Only barrel type layers231 {232 if (fG->isMeasure(i))233 {234 ihh[kmh] = i;235 Xh[kmh] = R*cos(phi);236 Yh[kmh] = R*sin(phi);237 Zh[kmh] = zz;238 kmh++;239 }240 }241 }242 //243 return kmh;244 }245 //246 163 // Covariance matrix estimation 247 //248 164 void SolTrack::CovCalc(Bool_t Res, Bool_t MS) 249 165 { 250 // 251 // 252 // Input flags: 253 // Res = .TRUE. turn on resolution effects/Use standard resolutions 254 // .FALSE. set all resolutions to 0 255 // MS = .TRUE. include Multiple Scattering 256 // 257 // Assumptions: 258 // 1. Measurement layers can do one or two measurements 259 // 2. On disks at constant z: 260 // - Upper side measurement is phi 261 // - Lower side measurement is R 262 // 263 // Fill list of layers hit 264 // 265 Int_t ntry = 0; 266 Int_t ntrymax = 0; 267 Int_t Nhit = nHit(); // Total number of layers hit 268 Double_t *zhh = new Double_t[Nhit]; // z of hit 269 Double_t *rhh = new Double_t[Nhit]; // r of hit 270 Double_t *dhh = new Double_t[Nhit]; // distance of hit from origin 271 Int_t *ihh = new Int_t[Nhit]; // true index of layer 272 Int_t kmh; // Number of measurement layers hit 273 // 274 kmh = HitList(ihh, rhh, zhh); // hit layer list 275 Int_t mTot = 0; // Total number of measurements 276 for (Int_t i = 0; i < Nhit; i++) 277 { 278 dhh[i] = TMath::Sqrt(rhh[i] * rhh[i] + zhh[i] * zhh[i]); 279 if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]); // Count number of measurements 280 } 281 // 282 // Order hit list by increasing distance from origin 283 // 284 Int_t *hord = new Int_t[Nhit]; // hit order by increasing distance from origin 285 TMath::Sort(Nhit, dhh, hord, kFALSE); // Order by increasing distance from origin 286 Double_t *zh = new Double_t[Nhit]; // d-ordered z of hit 287 Double_t *rh = new Double_t[Nhit]; // d-ordered r of hit 288 Int_t *ih = new Int_t[Nhit]; // d-ordered true index of layer 289 for (Int_t i = 0; i < Nhit; i++) 290 { 291 Int_t il = hord[i]; // Hit layer numbering 292 zh[i] = zhh[il]; 293 rh[i] = rhh[il]; 294 ih[i] = ihh[il]; 295 } 296 // 297 // Store interdistances and multiple scattering angles 298 // 299 Double_t sn2t = 1.0 / (1 + ct()*ct()); //sin^2 theta of track 300 Double_t cs2t = 1.0 - sn2t; //cos^2 theta 301 Double_t snt = TMath::Sqrt(sn2t); // sin theta 302 Double_t cst = TMath::Sqrt(cs2t); // cos theta 303 Double_t px0 = pt() * TMath::Cos(phi0()); // Momentum at minimum approach 304 Double_t py0 = pt() * TMath::Sin(phi0()); 305 Double_t pz0 = pt() * ct(); 306 // 307 TMatrixDSym dik(Nhit); dik.Zero(); // Distances between layers 308 Double_t *thms = new Double_t[Nhit]; // Scattering angles/plane 309 Double_t *cs = new Double_t[Nhit]; // Cosine of angle with layer normal 310 for (Int_t ii = 0; ii < Nhit; ii++) // Hit layer loop 311 { 312 Int_t i = ih[ii]; // Get true layer number 313 Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D())); 314 Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B); // Momentum at scattering layer 315 Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B); 316 Double_t pzi = pz0; 317 Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D()); 318 Double_t phi = phi0() + TMath::ASin(ArgRp); 319 Double_t nx = TMath::Cos(phi); // Barrel layer normal 320 Double_t ny = TMath::Sin(phi); 321 Double_t nz = 0.0; 322 if (fG->lTyp(i) == 2) // this is Z layer 323 { 324 nx = 0.0; 325 ny = 0.0; 326 nz = 1.0; 327 } 328 Double_t corr = (pxi*nx + pyi * ny + pzi * nz) / p(); 329 cs[ii] = corr; 330 Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i)); // Rad. length fraction 331 thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p(); // MS angle 332 if (!MS)thms[ii] = 0; 333 // 334 for (Int_t kk = 0; kk < ii; kk++) // Fill distances between layers 335 { 336 //dik(ii, kk) = TMath::Sqrt(pow(rh[ii] - rh[kk], 2) + pow(zh[ii] - zh[kk], 2)); 337 Double_t Ci = C(); 338 dik(ii, kk) = (TMath::ASin(Ci*rh[ii])-TMath::ASin(Ci*rh[kk]))/(Ci*snt); 339 dik(kk, ii) = dik(ii, kk); 340 } 341 // 342 // Store momentum components for resolution correction cosines 343 // 344 Double_t *pRi = new Double_t[Nhit]; 345 pRi[ii] = TMath::Abs(pxi * TMath::Cos(phi) + pyi * TMath::Sin(phi)); // Radial component 346 Double_t *pPhi = new Double_t[Nhit]; 347 pPhi[ii] = TMath::Abs(pxi * TMath::Sin(phi) - pyi * TMath::Cos(phi)); // Phi component 348 } 349 // 350 // Fill measurement covariance 351 // 352 Int_t *mTl = new Int_t[mTot]; // Pointer from measurement number to true layer number 353 TMatrixDSym Sm(mTot); Sm.Zero(); // Measurement covariance 354 TMatrixD Rm(mTot, 5); // Derivative matrix 355 Int_t im = 0; 356 // 357 // Fill derivatives and error matrix with MS 358 // 359 Double_t AngMax = 90.; Double_t AngMx = AngMax * TMath::Pi() / 180.; 360 Double_t csMin = TMath::Cos(AngMx); // Set maximum angle wrt normal 361 // 362 for (Int_t ii = 0; ii < Nhit; ii++) 363 { 364 Int_t i = ih[ii]; // True layer number 365 Int_t ityp = fG->lTyp(i); // Layer type Barrel or Z 366 Int_t nmeai = fG->lND(i); // # measurements in layer 367 if (fG->isMeasure(i) && nmeai >0 && cs[ii] > csMin) 368 { 369 Double_t Di = D(); // Get true track parameters 370 Double_t phi0i = phi0(); 371 Double_t Ci = C(); 372 Double_t z0i = z0(); 373 Double_t cti = ct(); 374 // 375 Double_t Ri = rh[ii]; 376 Double_t ArgRp = (Ri*Ci + (1 + Ci * Di)*Di / Ri) / (1 + 2 * Ci*Di); 377 Double_t ArgRz = Ci * TMath::Sqrt((Ri*Ri - Di * Di) / (1 + 2 * Ci*Di)); 378 TVectorD dRphi(5); dRphi.Zero(); // R-phi derivatives @ const. R 379 TVectorD dRz(5); dRz.Zero(); // z derivatives @ const. R 380 // 381 // Derivative overflow protection 382 Double_t dMin = 0.8; 383 dRphi(0) = (1 - 2 * Ci*Ci*Ri*Ri) / 384 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp)); // D derivative 385 dRphi(1) = Ri; // phi0 derivative 386 dRphi(2) = Ri * Ri / 387 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp)); // C derivative 388 dRphi(3) = 0.0; // z0 derivative 389 dRphi(4) = 0.0; // cot(theta) derivative 390 // 391 dRz(0) = -cti * Di / 392 (Ri*TMath::Max(dMin,TMath::Sqrt(1 - Ci * Ci*Ri*Ri))); // D 393 dRz(1) = 0.0; // Phi0 394 dRz(2) = cti * (Ri*Ci / TMath::Sqrt(1-Ri*Ri*Ci*Ci) - // C 395 TMath::ASin(Ri*Ci)) / (Ci*Ci); 396 dRz(3) = 1.0; // Z0 397 dRz(4) = TMath::ASin(ArgRz) / Ci; // Cot(theta) 398 // 399 for (Int_t nmi = 0; nmi < nmeai; nmi++) 400 { 401 mTl[im] = i; 402 Double_t stri = 0; 403 Double_t sig = 0; 404 if (nmi + 1 == 1) // Upper layer measurements 405 { 406 stri = fG->lStU(i); // Stereo angle 407 Double_t csa = TMath::Cos(stri); 408 Double_t ssa = TMath::Sin(stri); 409 sig = fG->lSgU(i); // Resolution 410 if (ityp == 1) // Barrel type layer (Measure R-phi, stereo or z at const. R) 411 { 412 // 413 // Almost exact solution (CD<<1, D<<R) 414 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 415 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative 416 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative 417 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative 418 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 419 } 420 if (ityp == 2) // Z type layer (Measure phi at const. Z) 421 { 422 Rm(im, 0) = 1.0; // D derivative 423 Rm(im, 1) = rh[ii]; // phi0 derivative 424 Rm(im, 2) = rh[ii] * rh[ii]; // C derivative 425 Rm(im, 3) = 0; // z0 derivative 426 Rm(im, 4) = 0; // cot(theta) derivative 427 } 428 } 429 if (nmi + 1 == 2) // Lower layer measurements 430 { 431 stri = fG->lStL(i); // Stereo angle 432 Double_t csa = TMath::Cos(stri); 433 Double_t ssa = TMath::Sin(stri); 434 sig = fG->lSgL(i); // Resolution 435 if (ityp == 1) // Barrel type layer (measure R-phi, stereo or z at const. R) 436 { 437 // 438 // Almost exact solution (CD<<1, D<<R) 439 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 440 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative 441 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative 442 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative 443 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 444 } 445 if (ityp == 2) // Z type layer (Measure R at const. z) 446 { 447 Rm(im, 0) = 0; // D derivative 448 Rm(im, 1) = 0; // phi0 derivative 449 Rm(im, 2) = 0; // C derivative 450 Rm(im, 3) = -1.0 / ct(); // z0 derivative 451 Rm(im, 4) = -rh[ii] / ct(); // cot(theta) derivative 452 } 453 } 454 // Derivative calculation completed 455 // 456 // Now calculate measurement error matrix 457 // 458 Int_t km = 0; 459 for (Int_t kk = 0; kk <= ii; kk++) 460 { 461 Int_t k = ih[kk]; // True layer number 462 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or 463 Int_t nmeak = fG->lND(k); // # measurements in layer 464 if (fG->isMeasure(k) && nmeak > 0 &&cs[kk] > csMin) 465 { 466 for (Int_t nmk = 0; nmk < nmeak; nmk++) 467 { 468 Double_t strk = 0; 469 if (nmk + 1 == 1) strk = fG->lStU(k); // Stereo angle 470 if (nmk + 1 == 2) strk = fG->lStL(k); // Stereo angle 471 if (im == km && Res) Sm(im, km) += sig*sig; // Detector resolution on diagonal 472 // 473 // Loop on all layers below for MS contributions 474 for (Int_t jj = 0; jj < kk; jj++) 475 { 476 Double_t di = dik(ii, jj); 477 Double_t dk = dik(kk, jj); 478 Double_t ms = thms[jj]; 479 Double_t msk = ms; Double_t msi = ms; 480 if (ityp == 1) msi = ms / snt; // Barrel 481 else if (ityp == 2) msi = ms / cst; // Disk 482 if (ktyp == 1) msk = ms / snt; // Barrel 483 else if (ktyp == 2) msk = ms / cst; // Disk 484 Double_t ci = TMath::Cos(stri); Double_t si = TMath::Sin(stri); 485 Double_t ck = TMath::Cos(strk); Double_t sk = TMath::Sin(strk); 486 Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk); // Ms contribution 487 } 488 // 489 Sm(km, im) = Sm(im, km); 490 km++; 491 } 492 } 493 } 494 im++; mTot = im; 495 } 496 } 497 } 498 Sm.ResizeTo(mTot, mTot); 499 Rm.ResizeTo(mTot, 5); 500 // 501 //********************************************************************** 502 // Calculate covariance from derivatives and measurement error matrix * 503 //********************************************************************** 504 // 505 TMatrixDSym DSmInv(mTot); DSmInv.Zero(); 506 for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id)); 507 TMatrixDSym SmN = Sm.Similarity(DSmInv); // Normalize diagonal to 1 508 //SmN.Invert(); 509 // 510 // Protected matrix inversions 511 // 512 TDecompChol Chl(SmN); 513 TMatrixDSym SmNinv = SmN; 514 if (Chl.Decompose()) 515 { 516 Bool_t OK; 517 SmNinv = Chl.Invert(OK); 518 } 519 else 520 { 521 cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << endl; 522 if (ntry < ntrymax) 523 { 524 SmNinv.Print(); 525 ntry++; 526 } 527 TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN; 528 TDecompChol rChl(SmN); 529 SmNinv = SmN; 530 Bool_t OK = rChl.Decompose(); 531 SmNinv = rChl.Invert(OK); 532 } 533 Sm = SmNinv.Similarity(DSmInv); // Error matrix inverted 534 TMatrixDSym H = Sm.SimilarityT(Rm); // Calculate half Hessian 535 // Normalize before inversion 536 const Int_t Npar = 5; 537 TMatrixDSym DHinv(Npar); DHinv.Zero(); 538 for (Int_t i = 0; i < Npar; i++)DHinv(i, i) = 1.0 / TMath::Sqrt(H(i, i)); 539 TMatrixDSym Hnrm = H.Similarity(DHinv); 540 // Invert and restore 541 Hnrm.Invert(); 542 fCov = Hnrm.Similarity(DHinv); 166 // Input flags: 167 // Res = .TRUE. turn on resolution effects/Use standard resolutions 168 // .FALSE. set all resolutions to 0 169 // MS = .TRUE. include Multiple Scattering 170 // Assumptions: 171 // 1. Measurement layers can do one or two measurements 172 // 2. On disks at constant z: 173 // - Upper side measurement is phi 174 // - Lower side measurement is R 175 176 // Fill list of layers hit 177 Int_t ntry = 0; 178 Int_t ntrymax = 0; 179 Int_t Nhit = nHit(); // Total number of layers hit 180 Double_t *zhh = new Double_t[Nhit]; // z of hit 181 Double_t *rhh = new Double_t[Nhit]; // r of hit 182 Double_t *dhh = new Double_t[Nhit]; // distance of hit from origin 183 Int_t *ihh = new Int_t[Nhit]; // true index of layer 184 Int_t kmh; // Number of measurement layers hit 185 186 kmh = HitList(ihh, rhh, zhh); // hit layer list 187 Int_t mTot = 0; // Total number of measurements 188 for (Int_t i = 0; i < Nhit; i++) 189 { 190 dhh[i] = TMath::Sqrt(rhh[i] * rhh[i] + zhh[i] * zhh[i]); 191 if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]); // Count number of measurements 192 } 193 // Order hit list by increasing distance from origin 194 Int_t *hord = new Int_t[Nhit]; // hit order by increasing distance from origin 195 TMath::Sort(Nhit, dhh, hord, kFALSE); // Order by increasing distance from origin 196 Double_t *zh = new Double_t[Nhit]; // d-ordered z of hit 197 Double_t *rh = new Double_t[Nhit]; // d-ordered r of hit 198 Int_t *ih = new Int_t[Nhit]; // d-ordered true index of layer 199 for (Int_t i = 0; i < Nhit; i++) 200 { 201 Int_t il = hord[i]; // Hit layer numbering 202 zh[i] = zhh[il]; 203 rh[i] = rhh[il]; 204 ih[i] = ihh[il]; 205 } 206 // Store interdistances and multiple scattering angles 207 Double_t sn2t = 1.0 / (1 + ct()*ct()); //sin^2 theta of track 208 Double_t cs2t = 1.0 - sn2t; //cos^2 theta 209 Double_t snt = TMath::Sqrt(sn2t); // sin theta 210 Double_t cst = TMath::Sqrt(cs2t); // cos theta 211 Double_t px0 = pt() * TMath::Cos(phi0()); // Momentum at minimum approach 212 Double_t py0 = pt() * TMath::Sin(phi0()); 213 Double_t pz0 = pt() * ct(); 214 TMatrixDSym dik(Nhit); dik.Zero(); // Distances between layers 215 Double_t *thms = new Double_t[Nhit]; // Scattering angles/plane 216 Double_t *cs = new Double_t[Nhit]; // Cosine of angle with layer normal 217 for (Int_t ii = 0; ii < Nhit; ii++) // Hit layer loop 218 { 219 Int_t i = ih[ii]; // Get true layer number 220 Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D())); 221 Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B); // Momentum at scattering layer 222 Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B); 223 Double_t pzi = pz0; 224 Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D()); 225 Double_t phi = phi0() + TMath::ASin(ArgRp); 226 Double_t nx = TMath::Cos(phi); // Barrel layer normal 227 Double_t ny = TMath::Sin(phi); 228 Double_t nz = 0.0; 229 if (fG->lTyp(i) == 2) // this is Z layer 230 { 231 nx = 0.0; 232 ny = 0.0; 233 nz = 1.0; 234 } 235 Double_t corr = (pxi*nx + pyi * ny + pzi * nz) / p(); 236 cs[ii] = corr; 237 Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i)); // Rad. length fraction 238 thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p(); // MS angle 239 if (!MS)thms[ii] = 0; 240 // 241 for (Int_t kk = 0; kk < ii; kk++) // Fill distances between layers 242 { 243 Double_t Ci = C(); 244 dik(ii, kk) = (TMath::ASin(Ci*rh[ii])-TMath::ASin(Ci*rh[kk]))/(Ci*snt); 245 dik(kk, ii) = dik(ii, kk); 246 } 247 // Store momentum components for resolution correction cosines 248 Double_t *pRi = new Double_t[Nhit]; 249 pRi[ii] = TMath::Abs(pxi * TMath::Cos(phi) + pyi * TMath::Sin(phi)); // Radial component 250 Double_t *pPhi = new Double_t[Nhit]; 251 pPhi[ii] = TMath::Abs(pxi * TMath::Sin(phi) - pyi * TMath::Cos(phi)); // Phi component 252 } 253 // Fill measurement covariance 254 Int_t *mTl = new Int_t[mTot]; // Pointer from measurement number to true layer number 255 TMatrixDSym Sm(mTot); Sm.Zero(); // Measurement covariance 256 TMatrixD Rm(mTot, 5); // Derivative matrix 257 Int_t im = 0; 258 // Fill derivatives and error matrix with MS 259 Double_t AngMax = 90.; Double_t AngMx = AngMax * TMath::Pi() / 180.; 260 Double_t csMin = TMath::Cos(AngMx); // Set maximum angle wrt normal 261 // 262 for (Int_t ii = 0; ii < Nhit; ii++) 263 { 264 Int_t i = ih[ii]; // True layer number 265 Int_t ityp = fG->lTyp(i); // Layer type Barrel or Z 266 Int_t nmeai = fG->lND(i); // # measurements in layer 267 if (fG->isMeasure(i) && nmeai >0 && cs[ii] > csMin) 268 { 269 Double_t Di = D(); // Get true track parameters 270 Double_t phi0i = phi0(); 271 Double_t Ci = C(); 272 Double_t z0i = z0(); 273 Double_t cti = ct(); 274 // 275 Double_t Ri = rh[ii]; 276 Double_t ArgRp = (Ri*Ci + (1 + Ci * Di)*Di / Ri) / (1 + 2 * Ci*Di); 277 Double_t ArgRz = Ci * TMath::Sqrt((Ri*Ri - Di * Di) / (1 + 2 * Ci*Di)); 278 TVectorD dRphi(5); dRphi.Zero(); // R-phi derivatives @ const. R 279 TVectorD dRz(5); dRz.Zero(); // z derivatives @ const. R 280 // Derivative overflow protection 281 Double_t dMin = 0.8; 282 dRphi(0) = (1 - 2 * Ci*Ci*Ri*Ri) / 283 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp)); // D derivative 284 dRphi(1) = Ri; // phi0 derivative 285 dRphi(2) = Ri * Ri / 286 TMath::Max(dMin,TMath::Sqrt(1 - ArgRp * ArgRp)); // C derivative 287 dRphi(3) = 0.0; // z0 derivative 288 dRphi(4) = 0.0; // cot(theta) derivative 289 290 dRz(0) = -cti * Di / 291 (Ri*TMath::Max(dMin,TMath::Sqrt(1 - Ci * Ci*Ri*Ri))); // D 292 dRz(1) = 0.0; // Phi0 293 dRz(2) = cti * (Ri*Ci / TMath::Sqrt(1-Ri*Ri*Ci*Ci) - 294 TMath::ASin(Ri*Ci)) / (Ci*Ci); // C 295 dRz(3) = 1.0; // Z0 296 dRz(4) = TMath::ASin(ArgRz) / Ci; // Cot(theta) 297 298 for (Int_t nmi = 0; nmi < nmeai; nmi++) 299 { 300 mTl[im] = i; 301 Double_t stri = 0; 302 Double_t sig = 0; 303 if (nmi + 1 == 1) // Upper layer measurements 304 { 305 stri = fG->lStU(i); // Stereo angle 306 Double_t csa = TMath::Cos(stri); 307 Double_t ssa = TMath::Sin(stri); 308 sig = fG->lSgU(i); // Resolution 309 if (ityp == 1) // Barrel type layer (Measure R-phi, stereo or z at const. R) 310 { 311 // Almost exact solution (CD<<1, D<<R) 312 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 313 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative 314 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative 315 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative 316 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 317 } 318 if (ityp == 2) // Z type layer (Measure phi at const. Z) 319 { 320 Rm(im, 0) = 1.0; // D derivative 321 Rm(im, 1) = rh[ii]; // phi0 derivative 322 Rm(im, 2) = rh[ii] * rh[ii]; // C derivative 323 Rm(im, 3) = 0; // z0 derivative 324 Rm(im, 4) = 0; // cot(theta) derivative 325 } 326 } 327 if (nmi + 1 == 2) // Lower layer measurements 328 { 329 stri = fG->lStL(i); // Stereo angle 330 Double_t csa = TMath::Cos(stri); 331 Double_t ssa = TMath::Sin(stri); 332 sig = fG->lSgL(i); // Resolution 333 if (ityp == 1) // Barrel type layer (measure R-phi, stereo or z at const. R) 334 { 335 // Almost exact solution (CD<<1, D<<R) 336 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative 337 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative 338 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative 339 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative 340 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative 341 } 342 if (ityp == 2) // Z type layer (Measure R at const. z) 343 { 344 Rm(im, 0) = 0; // D derivative 345 Rm(im, 1) = 0; // phi0 derivative 346 Rm(im, 2) = 0; // C derivative 347 Rm(im, 3) = -1.0 / ct(); // z0 derivative 348 Rm(im, 4) = -rh[ii] / ct(); // cot(theta) derivative 349 } 350 } 351 // Derivative calculation completed 352 // Now calculate measurement error matrix 353 Int_t km = 0; 354 for (Int_t kk = 0; kk <= ii; kk++) 355 { 356 Int_t k = ih[kk]; // True layer number 357 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or 358 Int_t nmeak = fG->lND(k); // # measurements in layer 359 if (fG->isMeasure(k) && nmeak > 0 &&cs[kk] > csMin) 360 { 361 for (Int_t nmk = 0; nmk < nmeak; nmk++) 362 { 363 Double_t strk = 0; 364 if (nmk + 1 == 1) strk = fG->lStU(k); // Stereo angle 365 if (nmk + 1 == 2) strk = fG->lStL(k); // Stereo angle 366 if (im == km && Res) Sm(im, km) += sig*sig; // Detector resolution on diagonal 367 // 368 // Loop on all layers below for MS contributions 369 for (Int_t jj = 0; jj < kk; jj++) 370 { 371 Double_t di = dik(ii, jj); 372 Double_t dk = dik(kk, jj); 373 Double_t ms = thms[jj]; 374 Double_t msk = ms; Double_t msi = ms; 375 if (ityp == 1) msi = ms / snt; // Barrel 376 else if (ityp == 2) msi = ms / cst; // Disk 377 if (ktyp == 1) msk = ms / snt; // Barrel 378 else if (ktyp == 2) msk = ms / cst; // Disk 379 Double_t ci = TMath::Cos(stri); Double_t si = TMath::Sin(stri); 380 Double_t ck = TMath::Cos(strk); Double_t sk = TMath::Sin(strk); 381 Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk); // Ms contribution 382 } 383 Sm(km, im) = Sm(im, km); 384 km++; 385 } 386 } 387 } 388 im++; mTot = im; 389 } 390 } 391 } 392 Sm.ResizeTo(mTot, mTot); 393 Rm.ResizeTo(mTot, 5); 394 395 // Calculate covariance from derivatives and measurement error matrix 396 TMatrixDSym DSmInv(mTot); DSmInv.Zero(); 397 for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id)); 398 TMatrixDSym SmN = Sm.Similarity(DSmInv); // Normalize diagonal to 1 399 // Protected matrix inversions 400 TDecompChol Chl(SmN); 401 TMatrixDSym SmNinv = SmN; 402 if (Chl.Decompose()) 403 { 404 Bool_t OK; 405 SmNinv = Chl.Invert(OK); 406 } 407 else 408 { 409 cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << endl; 410 if (ntry < ntrymax) 411 { 412 SmNinv.Print(); 413 ntry++; 414 } 415 TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN; 416 TDecompChol rChl(SmN); 417 SmNinv = SmN; 418 Bool_t OK = rChl.Decompose(); 419 SmNinv = rChl.Invert(OK); 420 } 421 Sm = SmNinv.Similarity(DSmInv); // Error matrix inverted 422 TMatrixDSym H = Sm.SimilarityT(Rm); // Calculate half Hessian 423 // Normalize before inversion 424 const Int_t Npar = 5; 425 TMatrixDSym DHinv(Npar); DHinv.Zero(); 426 for (Int_t i = 0; i < Npar; i++)DHinv(i, i) = 1.0 / TMath::Sqrt(H(i, i)); 427 TMatrixDSym Hnrm = H.Similarity(DHinv); 428 // Invert and restore 429 Hnrm.Invert(); 430 fCov = Hnrm.Similarity(DHinv); 543 431 } 544 432 … … 546 434 TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat) 547 435 { 548 // 549 // Input: symmetric matrix with 1's on diagonal 550 // Output: positive definite matrix with 1's on diagonal 551 // 552 // Default return value 553 TMatrixDSym rMatN = NormMat; 554 // Check the diagonal 555 Bool_t Check = kFALSE; 556 Int_t Size = NormMat.GetNcols(); 557 for (Int_t i = 0; i < Size; i++)if (TMath::Abs(NormMat(i, i) - 1.0)>1.0E-15)Check = kTRUE; 558 if (Check) 559 { 560 cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << endl; 561 return rMatN; 562 } 563 // 564 // Diagonalize matrix 565 TMatrixDSymEigen Eign(NormMat); 566 TMatrixD U = Eign.GetEigenVectors(); 567 TVectorD lambda = Eign.GetEigenValues(); 568 // Reset negative eigenvalues to small positive value 569 TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13; 570 for (Int_t i = 0; i < Size; i++) 571 { 572 D(i, i) = lambda(i); 573 if (lambda(i) <= 0) D(i, i) = eps; 574 } 575 //Rebuild matrix 576 TMatrixD Ut(TMatrixD::kTransposed, U); 577 TMatrixD rMat = (U*D)*Ut; // Now it is positive defite 578 // Restore all ones on diagonal 579 for (Int_t i1 = 0; i1 < Size; i1++) 580 { 581 Double_t rn1 = TMath::Sqrt(rMat(i1, i1)); 582 for (Int_t i2 = 0; i2 <= i1; i2++) 583 { 584 Double_t rn2 = TMath::Sqrt(rMat(i2, i2)); 585 rMatN(i1, i2) = 0.5*(rMat(i1, i2) + rMat(i2, i1)) / (rn1*rn2); 586 rMatN(i2, i1) = rMatN(i1, i2); 587 } 588 } 589 return rMatN; 590 } 436 // Input: symmetric matrix with 1's on diagonal 437 // Output: positive definite matrix with 1's on diagonal 438 439 // Default return value 440 TMatrixDSym rMatN = NormMat; 441 // Check the diagonal 442 Bool_t Check = kFALSE; 443 Int_t Size = NormMat.GetNcols(); 444 for (Int_t i = 0; i < Size; i++)if (TMath::Abs(NormMat(i, i) - 1.0)>1.0E-15)Check = kTRUE; 445 if (Check) 446 { 447 cout << "SolTrack::MakePosDef: input matrix doesn't have 1 on diagonal. Abort." << endl; 448 return rMatN; 449 } 450 // Diagonalize matrix 451 TMatrixDSymEigen Eign(NormMat); 452 TMatrixD U = Eign.GetEigenVectors(); 453 TVectorD lambda = Eign.GetEigenValues(); 454 // Reset negative eigenvalues to small positive value 455 TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13; 456 for (Int_t i = 0; i < Size; i++) 457 { 458 D(i, i) = lambda(i); 459 if (lambda(i) <= 0) D(i, i) = eps; 460 } 461 // Rebuild matrix 462 TMatrixD Ut(TMatrixD::kTransposed, U); 463 TMatrixD rMat = (U*D)*Ut; // Now it is positive defite 464 // Restore all ones on diagonal 465 for (Int_t i1 = 0; i1 < Size; i1++) 466 { 467 Double_t rn1 = TMath::Sqrt(rMat(i1, i1)); 468 for (Int_t i2 = 0; i2 <= i1; i2++) 469 { 470 Double_t rn2 = TMath::Sqrt(rMat(i2, i2)); 471 rMatN(i1, i2) = 0.5*(rMat(i1, i2) + rMat(i2, i1)) / (rn1*rn2); 472 rMatN(i2, i1) = rMatN(i1, i2); 473 } 474 } 475 return rMatN; 476 }
Note:
See TracChangeset
for help on using the changeset viewer.
