Fork me on GitHub

source: git/external/TrackCovariance/SolTrack.cc@ 4e8e72b

Last change on this file since 4e8e72b was 3a105e5, checked in by michele <michele.selvaggi@…>, 3 years ago

added first hit to track
  • Property mode set to 100644
File size: 20.7 KB
Line 
1
2#include "SolGeom.h"
3#include "SolTrack.h"
4#include <TString.h>
5#include <TMath.h>
6#include <TMatrixD.h>
7#include <TMatrixDSym.h>
8#include <TDecompChol.h>
9#include <TMatrixDSymEigen.h>
10#include <TGraph.h>
11#include <iostream>
12//
13// Constructors
14SolTrack::SolTrack(Double_t *x, Double_t *p, SolGeom *G)
15{
16 // Set B field
17 fG = G; // Store geometry
18 Double_t B = G->B();
19 SetB(B);
20 // Store momentum and position
21 fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2];
22 fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2];
23 // Get generated parameters
24 TVector3 xv(fx);
25 TVector3 pv(fp);
26 Double_t Charge = 1.0; // Don't worry about charge for now
27 TVectorD gPar = XPtoPar(xv, pv, Charge);
28 // Store parameters
29 fpar[0] = gPar(0);
30 fpar[1] = gPar(1);
31 fpar[2] = gPar(2);
32 fpar[3] = gPar(3);
33 fpar[4] = gPar(4);
34 //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
35 //
36 // Init covariances
37 //
38 fCov.ResizeTo(5, 5);
39}
40SolTrack::SolTrack(TVector3 x, TVector3 p, SolGeom* G)
41{
42 // set B field
43 fG = G; // Store geometry
44 Double_t B = G->B();
45 SetB(B);
46 // Store momentum
47 fp[0] = p(0); fp[1] = p(1); fp[2] = p(2);
48 fx[0] = x(0); fx[1] = x(1); fx[2] = x(2);
49 // Get generated parameters
50 Double_t Charge = 1.0; // Don't worry about charge for now
51 TVectorD gPar = XPtoPar(x, p, Charge);
52 // Store parameters
53 fpar[0] = gPar(0);
54 fpar[1] = gPar(1);
55 fpar[2] = gPar(2);
56 fpar[3] = gPar(3);
57 fpar[4] = gPar(4);
58 //cout << "SolTrack:: C = " << C << ", fpar[2] = " << fpar[2] << endl;
59 //
60 // Init covariances
61 //
62 fCov.ResizeTo(5, 5);
63}
64//
65SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G)
66{
67 fG = G;
68 Double_t B = G->B();
69 SetB(B);
70 // Store parameters
71 fpar[0] = D;
72 fpar[1] = phi0;
73 fpar[2] = C;
74 fpar[3] = z0;
75 fpar[4] = ct;
76 // Store momentum
77 Double_t pt = B * TrkUtil::cSpeed() / TMath::Abs(2 * C);
78 Double_t px = pt*TMath::Cos(phi0);
79 Double_t py = pt*TMath::Sin(phi0);
80 Double_t pz = pt*ct;
81 //
82 fp[0] = px; fp[1] = py; fp[2] = pz;
83 fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0); fx[2] = z0;
84 //
85 // Init covariances
86 //
87 fCov.ResizeTo(5, 5);
88}
89// Destructor
90SolTrack::~SolTrack()
91{
92 fCov.Clear();
93}
94//
95// Calculate intersection with given layer
96Bool_t SolTrack::HitLayer(Int_t il, Double_t &R, Double_t &phi, Double_t &zz)
97{
98 Double_t Di = D();
99 Double_t phi0i = phi0();
100 Double_t Ci = C();
101 Double_t z0i = z0();
102 Double_t cti = ct();
103 //
104 R = 0; phi = 0; zz = 0;
105 Bool_t val = kFALSE;
106 Double_t Rmin = TMath::Sqrt(fx[0] * fx[0] + fx[1] * fx[1]); // Smallest track radius
107 if (Rmin < TMath::Abs(Di)) return val;
108 //
109 Double_t ArgzMin = Ci * TMath::Sqrt((Rmin * Rmin - Di * Di) / (1 + 2 * Ci * Di));
110 Double_t stMin = TMath::ASin(ArgzMin) / Ci; // Arc length at track origin
111 //
112 if (fG->lTyp(il) == 1) // Cylinder: layer at constant R
113 {
114 R = fG->lPos(il);
115 Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
116 if (TMath::Abs(argph) < 1.0 && R > Rmin)
117 {
118 Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di));
119 if (TMath::Abs(argz) < 1.0)
120 {
121 zz = z0i + cti*TMath::ASin(argz) / Ci;
122 if (zz > fG->lxMin(il) && zz < fG->lxMax(il))
123 {
124 phi = phi0i + TMath::ASin(argph);
125 val = kTRUE;
126 }
127 }
128 }
129 }
130 else if (fG->lTyp(il) == 2) // disk: layer at constant z
131 {
132 zz = fG->lPos(il);
133 Double_t st = (zz - z0i) / cti;
134 if (TMath::Abs(Ci * st) < 1.0 && st > stMin)
135 {
136 R = TMath::Sqrt(Di * Di + (1. + 2. * Ci * Di) * pow(TMath::Sin(Ci * st), 2) / (Ci * Ci));
137 if (R > fG->lxMin(il) && R < fG->lxMax(il))
138 {
139 Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
140 if (TMath::Abs(arg1) < 1.0)
141 {
142 phi = phi0i + TMath::ASin(arg1);
143 val = kTRUE;
144 }
145 }
146 }
147 }
148 //
149 return val;
150}
151//
152// # of layers hit
153Int_t SolTrack::nHit()
154{
155 Int_t kh = 0;
156 Double_t R; Double_t phi; Double_t zz;
157 for (Int_t i = 0; i < fG->Nl(); i++)
158 if (HitLayer(i, R, phi, zz))kh++;
159 //
160 return kh;
161}
162//
163// # of measurement layers hit
164Int_t SolTrack::nmHit()
165{
166 Int_t kmh = 0;
167 Double_t R; Double_t phi; Double_t zz;
168 for (Int_t i = 0; i < fG->Nl(); i++)
169 if (HitLayer(i, R, phi, zz))if (fG->isMeasure(i))kmh++;
170 //
171 return kmh;
172}
173//
174// List of layers hit with intersections
175Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh)
176{
177 //
178 // Return lists of hits associated to a track including all scattering layers.
179 // Return value is the total number of measurement hits
180 // kmh = total number of measurement layers hit for given type
181 // ihh = pointer to layer number
182 // rhh = radius of hit
183 // zhh = z of hit
184 //
185 // ***** NB: double layers with stereo on lower layer not included
186 //
187 Int_t kh = 0; // Number of layers hit
188 Int_t kmh = 0; // Number of measurement layers of given type
189 for (Int_t i = 0; i < fG->Nl(); i++)
190 {
191 Double_t R; Double_t phi; Double_t zz;
192 if (HitLayer(i, R, phi, zz))
193 {
194 zhh[kh] = zz;
195 rhh[kh] = R;
196 ihh[kh] = i;
197 if (fG->isMeasure(i))kmh++;
198 kh++;
199 }
200 }
201 //
202 return kmh;
203}
204//
205// List of XYZ measurements without any error
206Int_t SolTrack::HitListXYZ(Int_t *&ihh, Double_t *&Xh, Double_t *&Yh, Double_t *&Zh)
207{
208 //
209 // Return lists of hits associated to a track for all measurement layers.
210 // Return value is the total number of measurement hits
211 // kmh = total number of measurement layers hit for given type
212 // ihh = pointer to layer number
213 // Xh, Yh, Zh = X, Y, Z of hit - No measurement error - No multiple scattering
214 //
215 //
216 Int_t kmh = 0; // Number of measurement layers hit
217 for (Int_t i = 0; i < fG->Nl(); i++)
218 {
219 Double_t R; Double_t phi; Double_t zz;
220 if (HitLayer(i, R, phi, zz)) // Only barrel type layers
221 {
222 if (fG->isMeasure(i))
223 {
224 ihh[kmh] = i;
225 Xh[kmh] = R*cos(phi);
226 Yh[kmh] = R*sin(phi);
227 Zh[kmh] = zz;
228 kmh++;
229 }
230 }
231 }
232 //
233 return kmh;
234}
235//
236Int_t SolTrack::FirstHit(Double_t &Xfirst, Double_t &Yfirst, Double_t &Zfirst)
237{
238 Int_t iFirst = -1;
239 Int_t iFirstLay = -1; // Default return with no hits
240 Xfirst = 0.;
241 Yfirst = 0.;
242 Zfirst = 0.;
243 Int_t Nmh = nmHit(); // # measurement hits
244 if(Nmh > 0){
245 Int_t *ih = new Int_t [Nmh];
246 Double_t *Xh = new Double_t[Nmh];
247 Double_t *Yh = new Double_t[Nmh];
248 Double_t *Zh = new Double_t[Nmh];
249 Double_t *dh = new Double_t[Nmh];
250 //
251 Int_t n = HitListXYZ(ih, Xh, Yh, Zh);
252 //
253 for(Int_t i=0; i<Nmh; i++){
254 Double_t rr = TMath::Sqrt(Xh[i]*Xh[i]+Yh[i]*Yh[i]); // Hit radius
255 dh[i] = TMath::ASin(C() * TMath::Sqrt((rr * rr - D() * D()) / (1. + 2 * C() * D()))) / C(); // Arc length traveled
256 }
257 //
258 Int_t *hord = new Int_t[Nmh]; // hit order by increasing arc length
259 TMath::Sort(Nmh, dh, hord, kFALSE); // Order by increasing arc length
260 iFirst = hord[0]; // First hit pointer
261 Xfirst = Xh[iFirst];
262 Yfirst = Yh[iFirst];
263 Zfirst = Zh[iFirst];
264 iFirstLay = ih[iFirst];
265 //
266 // Clean
267 delete [] ih;
268 delete [] Xh;
269 delete [] Yh;
270 delete [] Zh;
271 delete [] dh;
272 delete [] hord;
273 }
274//
275 return iFirstLay; // Return first hit layer number
276}
277//
278// Track plot
279//
280TGraph *SolTrack::TrkPlot()
281{
282 //
283 // Fill list of layers hit
284 //
285 Int_t Nhit = nHit(); // Total number of layers hit
286 //cout << "Nhit = " << Nhit << endl;
287 Double_t *zh = new Double_t[Nhit]; // z of hit
288 Double_t *rh = new Double_t[Nhit]; // r of hit
289 Int_t *ih = new Int_t [Nhit]; // true index of layer
290 Int_t kmh; // Number of measurement layers hit
291 //
292 kmh = HitList(ih, rh, zh); // hit layer list
293 //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
294 Double_t *dh = new Double_t[Nhit]; // Hit distance from origin
295 for(Int_t i=0; i<Nhit; i++)dh[i] = TMath::ASin(C() * TMath::Sqrt((rh[i] * rh[i] - D() * D()) / (1. + 2 * C() * D()))) / C(); // Arc length traveled;
296 //
297 Int_t *hord = new Int_t[Nhit];
298 TMath::Sort(Nhit, dh, hord, kFALSE); // Order by increasing phase
299 Double_t *z = new Double_t[Nhit]; // z of ordered hit
300 Double_t *r = new Double_t[Nhit]; // r of ordered hit
301 for (Int_t i = 0; i < Nhit; i++)
302 {
303 z[i] = zh[hord[i]];
304 r[i] = rh[hord[i]];
305 }
306 //cout << "After ordering" << endl;
307 //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
308 TGraph *gr = new TGraph(Nhit, z, r); // graph intersection with layers
309 gr->SetMarkerStyle(kCircle);
310 gr->SetMarkerColor(kMagenta);
311 gr->SetMarkerSize(1);
312 gr->SetLineColor(kMagenta);
313 //
314 // clean up
315 //
316 delete[] zh;
317 delete[] rh;
318 delete[] ih;
319 delete[] hord;
320 return gr;
321}
322//
323// Covariance matrix estimation
324//
325void SolTrack::CovCalc(Bool_t Res, Bool_t MS)
326{
327 //
328 //
329 // Input flags:
330 // Res = .TRUE. turn on resolution effects/Use standard resolutions
331 // .FALSE. set all resolutions to 0
332 // MS = .TRUE. include Multiple Scattering
333 //
334 // Assumptions:
335 // 1. Measurement layers can do one or two measurements
336 // 2. On disks at constant z:
337 // - Upper side measurement is phi
338 // - Lower side measurement is R
339 //
340 // Fill list of layers hit
341 //
342 Int_t ntry = 0;
343 Int_t ntrymax = 0;
344 Int_t Nhit = nHit(); // Total number of layers hit
345 Double_t *zhh = new Double_t[Nhit]; // z of hit
346 Double_t *rhh = new Double_t[Nhit]; // r of hit
347 Double_t *dhh = new Double_t[Nhit]; // distance of hit from origin
348 Int_t *ihh = new Int_t[Nhit]; // true index of layer
349 Int_t kmh; // Number of measurement layers hit
350 //
351 kmh = HitList(ihh, rhh, zhh); // hit layer list
352 Int_t mTot = 0; // Total number of measurements
353 for (Int_t i = 0; i < Nhit; i++)
354 {
355 Double_t rr = rhh[i];
356 dhh[i] = TMath::ASin(C() * TMath::Sqrt((rr * rr - D() * D()) / (1. + 2 * C() * D()))) / C(); // Arc length traveled
357 if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]); // Count number of measurements
358 }
359 //
360 // Order hit list by increasing arc length
361 //
362 Int_t *hord = new Int_t[Nhit]; // hit order by increasing distance from origin
363 TMath::Sort(Nhit, dhh, hord, kFALSE); // Order by increasing distance from origin
364 Double_t *zh = new Double_t[Nhit]; // d-ordered z of hit
365 Double_t *rh = new Double_t[Nhit]; // d-ordered r of hit
366 Int_t *ih = new Int_t[Nhit]; // d-ordered true index of layer
367 for (Int_t i = 0; i < Nhit; i++)
368 {
369 Int_t il = hord[i]; // Hit layer numbering
370 zh[i] = zhh[il];
371 rh[i] = rhh[il];
372 ih[i] = ihh[il];
373 }
374 //
375 // Store interdistances and multiple scattering angles
376 //
377 Double_t sn2t = 1.0 / (1.0 + ct()*ct()); //sin^2 theta of track
378 Double_t cs2t = 1.0 - sn2t; //cos^2 theta
379 Double_t snt = TMath::Sqrt(sn2t); // sin theta
380 Double_t cst = TMath::Sqrt(cs2t); // cos theta
381 Double_t px0 = pt() * TMath::Cos(phi0()); // Momentum at minimum approach
382 Double_t py0 = pt() * TMath::Sin(phi0());
383 Double_t pz0 = pt() * ct();
384 //
385 TMatrixDSym dik(Nhit); dik.Zero(); // Distances between layers
386 Double_t *thms = new Double_t[Nhit]; // Scattering angles/plane
387 Double_t* cs = new Double_t[Nhit]; // Cosine of angle with normal in transverse plane
388 //
389 for (Int_t ii = 0; ii < Nhit; ii++) // Hit layer loop
390 {
391 Int_t i = ih[ii]; // Get true layer number
392 Int_t il = hord[ii]; // Unordered layer
393 Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D()));
394 //
395 Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B); // Momentum at scattering layer
396 Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B);
397 Double_t pzi = pz0;
398 Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D());
399 //
400 Double_t phi = phi0() + TMath::ASin(ArgRp);
401 Double_t nx = TMath::Cos(phi); // Barrel layer normal
402 Double_t ny = TMath::Sin(phi);
403 Double_t nz = 0.0;
404 cs[ii] = TMath::Abs((pxi * nx + pyi * ny) / pt());
405 //
406 if (fG->lTyp(i) == 2) // this is Z layer
407 {
408 nx = 0.0;
409 ny = 0.0;
410 nz = 1.0;
411 }
412 Double_t corr = TMath::Abs(pxi*nx + pyi * ny + pzi * nz) / p();
413 Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i)); // Rad. length fraction
414 thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p(); // MS angle
415 if (!MS)thms[ii] = 0;
416 //
417 for (Int_t kk = 0; kk < ii; kk++) // Fill distances between layers
418 {
419 Int_t kl = hord[kk]; // Unordered layer
420 dik(ii, kk) = TMath::Abs(dhh[il] - dhh[kl])/snt;
421 dik(kk, ii) = dik(ii, kk);
422 }
423 }
424 //
425 // Fill measurement covariance
426 //
427 TVectorD tPar(5,fpar);
428 //
429 TMatrixDSym Sm(mTot); Sm.Zero(); // Measurement covariance
430 TMatrixD Rm(mTot, 5); // Derivative matrix
431 Int_t im = 0; // Initialize number of measurement counter
432 //
433 // Fill derivatives and error matrix with MS
434 //
435 for (Int_t ii = 0; ii < Nhit; ii++)
436 {
437 Int_t i = ih[ii]; // True layer number
438 Int_t ityp = fG->lTyp(i); // Layer type Barrel or Z
439 Int_t nmeai = fG->lND(i); // # measurements in layer
440
441 if (fG->isMeasure(i))
442 {
443 Double_t Ri = rh[ii];
444 Double_t zi = zh[ii];
445 //
446 for (Int_t nmi = 0; nmi < nmeai; nmi++)
447 {
448 Double_t stri = 0; // Stereo angle
449 Double_t sig = 0; // Layer resolution
450 // Constant R derivatives
451 TVectorD dRphi(5); dRphi.Zero(); // R-phi derivatives @ const. R
452 TVectorD dRz(5); dRz.Zero(); // z derivatives @ const. R
453 //
454 if (nmi + 1 == 1) // Upper layer measurements
455 {
456 stri = fG->lStU(i); // Stereo angle
457 Double_t csa = TMath::Cos(stri);
458 Double_t ssa = TMath::Sin(stri);
459 //
460 sig = fG->lSgU(i); // Resolution
461 if (ityp == 1) // Barrel type layer (Measure R-phi, stereo or z at const. R)
462 {
463 //
464 // Exact solution
465 dRphi = derRphi_R(tPar, Ri);
466 dRz = derZ_R (tPar, Ri);
467 //
468 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative
469 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative
470 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative
471 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative
472 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative
473 }
474 if (ityp == 2) // Z type layer (Measure R-phi at const. Z)
475 {
476 TVectorD dRphz(5); dRphz.Zero(); // R-phi derivatives @ const. z
477 dRphz = derRphi_Z(tPar, zi);
478 //
479 Rm(im, 0) = dRphz(0); // D derivative
480 Rm(im, 1) = dRphz(1); // phi0 derivative
481 Rm(im, 2) = dRphz(2); // C derivative
482 Rm(im, 3) = dRphz(3); // z0 derivative
483 Rm(im, 4) = dRphz(4); // cot(theta) derivative
484 }
485 }
486 if (nmi + 1 == 2) // Lower layer measurements
487 {
488 stri = fG->lStL(i); // Stereo angle
489 Double_t csa = TMath::Cos(stri);
490 Double_t ssa = TMath::Sin(stri);
491 sig = fG->lSgL(i); // Resolution
492 if (ityp == 1) // Barrel type layer (measure R-phi, stereo or z at const. R)
493 {
494 //
495 // Exact solution
496 dRphi = derRphi_R(tPar, Ri);
497 dRz = derZ_R (tPar, Ri);
498 //
499 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative
500 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative
501 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative
502 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative
503 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative
504 }
505 if (ityp == 2) // Z type layer (Measure R at const. z)
506 {
507 TVectorD dRRz(5); dRRz.Zero(); // R derivatives @ const. z
508 dRRz = derR_Z(tPar, zi);
509 //
510 Rm(im, 0) = dRRz(0); // D derivative
511 Rm(im, 1) = dRRz(1); // phi0 derivative
512 Rm(im, 2) = dRRz(2); // C derivative
513 Rm(im, 3) = dRRz(3); // z0 derivative
514 Rm(im, 4) = dRRz(4); // cot(theta) derivative
515 }
516 }
517 // Derivative calculation completed
518 //
519 // Now calculate measurement error matrix
520 //
521 Int_t km = 0;
522 Double_t CosMin = TMath::Sin(TMath::Pi() / 9.); // Protect for derivative explosion
523 for (Int_t kk = 0; kk <= ii; kk++)
524 {
525 Int_t k = ih[kk]; // True layer number
526 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or disk
527 Int_t nmeak = fG->lND(k); // # measurements in layer
528 if (fG->isMeasure(k))
529 {
530 for (Int_t nmk = 0; nmk < nmeak; nmk++)
531 {
532 Double_t strk = 0;
533 if (nmk + 1 == 1) strk = fG->lStU(k); // Stereo angle upper
534 if (nmk + 1 == 2) strk = fG->lStL(k); // Stereo angle lower
535 //if (im == km && Res) Sm(im, km) += sig*sig; // Detector resolution on diagonal
536 if (im == km && Res) {
537 Double_t sg = sig;
538 if(TMath::Abs(strk) < TMath::Pi()/6. && cs[kk] < CosMin)
539 TMath::Min(1000.*sig,sg = sig/pow(cs[kk],4));
540 Sm(im, km) += sg * sg; // Detector resolution on diagonal
541 }
542 //
543 // Loop on all layers below for MS contributions
544 for (Int_t jj = 0; jj < kk; jj++)
545 {
546 Double_t di = dik(ii, jj);
547 Double_t dk = dik(kk, jj);
548 Double_t ms = thms[jj];
549 Double_t msk = ms; Double_t msi = ms;
550 if (ityp == 1) msi = ms / snt; // Barrel
551 else if (ityp == 2) msi = ms / cst; // Disk
552 if (ktyp == 1) msk = ms / snt; // Barrel
553 else if (ktyp == 2) msk = ms / cst; // Disk
554 Double_t ci = TMath::Abs(TMath::Cos(stri)); Double_t si = TMath::Abs(TMath::Sin(stri));
555 Double_t ck = TMath::Abs(TMath::Cos(strk)); Double_t sk = TMath::Abs(TMath::Sin(strk));
556 Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk); // Ms contribution
557 }
558 //
559 Sm(km, im) = Sm(im, km);
560 km++;
561 }
562 }
563 }
564 im++; mTot = im;
565 }
566 }
567 }
568 Sm.ResizeTo(mTot, mTot);
569 TMatrixDSym SmTemp = Sm;
570 Rm.ResizeTo(mTot, 5);
571 //
572 //**********************************************************************
573 // Calculate covariance from derivatives and measurement error matrix *
574 //**********************************************************************
575 //
576 TMatrixDSym DSmInv(mTot); DSmInv.Zero();
577 for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id));
578 TMatrixDSym SmN = Sm.Similarity(DSmInv); // Normalize diagonal to 1
579 //
580 // Protected matrix inversions
581 //
582 TDecompChol Chl(SmN,1.e-12);
583 TMatrixDSym SmNinv = SmN;
584 if (Chl.Decompose())
585 {
586 Bool_t OK;
587 SmNinv = Chl.Invert(OK);
588 }
589 else
590 {
591 std::cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << std::endl;
592 //cout << "pt = " << pt() << endl;
593 if (ntry < ntrymax)
594 {
595 SmNinv.Print();
596 ntry++;
597 }
598 //
599 TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN;
600 TDecompChol rChl(SmN);
601 SmNinv = SmN;
602 Bool_t OK = rChl.Decompose();
603 SmNinv = rChl.Invert(OK);
604 }
605 Sm = SmNinv.Similarity(DSmInv); // Error matrix inverted
606 TMatrixDSym H = Sm.SimilarityT(Rm); // Calculate half Hessian
607 const Int_t Npar = 5;
608 TMatrixDSym DHinv(Npar); DHinv.Zero();
609 for (Int_t i = 0; i < Npar; i++)DHinv(i, i) = 1.0 / TMath::Sqrt(H(i, i));
610 TMatrixDSym Hnrm = H.Similarity(DHinv);
611 // Invert and restore
612 Hnrm.Invert();
613 fCov = Hnrm.Similarity(DHinv);
614 //
615 // debug
616 //
617 if(TMath::IsNaN(fCov(0,0)))
618 {
619 std::cout<<"SolTrack::CovCalc: NaN found in covariance matrix"<<std::endl;
620 }
621 //
622 // Lots of cleanup to do
623 delete[] zhh;
624 delete[] rhh;
625 delete[] dhh;
626 delete[] ihh;
627 delete[] hord;
628 delete[] zh;
629 delete[] rh;
630 delete[] ih;
631 delete[] cs;
632 delete[] thms;
633}
634//
635// Force positive definitness in normalized matrix
636TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat)
637{
638 //
639 // Input: symmetric matrix with 1's on diagonal
640 // Output: positive definite matrix with 1's on diagonal
641 //
642 // Default return value
643 TMatrixDSym rMatN = NormMat;
644 // Check the diagonal
645 Bool_t Check = kFALSE;
646 Int_t Size = NormMat.GetNcols();
647 for (Int_t i = 0; i < Size; i++)if (TMath::Abs(NormMat(i, i) - 1.0)>1.0E-15)Check = kTRUE;
648 if (Check)
649 {
650 std::cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << std::endl;
651 return rMatN;
652 }
653 //
654 // Diagonalize matrix
655 TMatrixDSymEigen Eign(NormMat);
656 TMatrixD U = Eign.GetEigenVectors();
657 TVectorD lambda = Eign.GetEigenValues();
658 //cout << "Eigenvalues:"; lambda.Print();
659 //cout << "Input matrix: "; NormMat.Print();
660 // Reset negative eigenvalues to small positive value
661 TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13;
662 for (Int_t i = 0; i < Size; i++)
663 {
664 D(i, i) = lambda(i);
665 if (lambda(i) <= 0) D(i, i) = eps;
666 }
667 //Rebuild matrix
668 TMatrixD Ut(TMatrixD::kTransposed, U);
669 TMatrixD rMat = (U*D)*Ut; // Now it is positive defite
670 // Restore all ones on diagonal
671 for (Int_t i1 = 0; i1 < Size; i1++)
672 {
673 Double_t rn1 = TMath::Sqrt(rMat(i1, i1));
674 for (Int_t i2 = 0; i2 <= i1; i2++)
675 {
676 Double_t rn2 = TMath::Sqrt(rMat(i2, i2));
677 rMatN(i1, i2) = 0.5*(rMat(i1, i2) + rMat(i2, i1)) / (rn1*rn2);
678 rMatN(i2, i1) = rMatN(i1, i2);
679 }
680 }
681 //cout << "Rebuilt matrix: "; rMatN.Print();
682 return rMatN;
683}
Note: See TracBrowser for help on using the repository browser.