Fork me on GitHub

source: git/external/TrackCovariance/SolTrack.cc@ a09b75f

Last change on this file since a09b75f was ebf40fd, checked in by Franco BEDESCHI <bed@…>, 3 years ago

Major update to handle highly displaced tracks
  • Property mode set to 100644
File size: 19.6 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//
236// Track plot
237//
238TGraph *SolTrack::TrkPlot()
239{
240 //
241 // Fill list of layers hit
242 //
243 Int_t Nhit = nHit(); // Total number of layers hit
244 //cout << "Nhit = " << Nhit << endl;
245 Double_t *zh = new Double_t[Nhit]; // z of hit
246 Double_t *rh = new Double_t[Nhit]; // r of hit
247 Int_t *ih = new Int_t [Nhit]; // true index of layer
248 Int_t kmh; // Number of measurement layers hit
249 //
250 kmh = HitList(ih, rh, zh); // hit layer list
251 //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
252 Double_t *dh = new Double_t[Nhit]; // Hit distance from origin
253 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;
254 //
255 Int_t *hord = new Int_t[Nhit];
256 TMath::Sort(Nhit, dh, hord, kFALSE); // Order by increasing phase
257 Double_t *z = new Double_t[Nhit]; // z of ordered hit
258 Double_t *r = new Double_t[Nhit]; // r of ordered hit
259 for (Int_t i = 0; i < Nhit; i++)
260 {
261 z[i] = zh[hord[i]];
262 r[i] = rh[hord[i]];
263 }
264 //cout << "After ordering" << endl;
265 //for (Int_t j = 0; j < Nhit; j++) cout << "r = " << rh[j] << ", z = " << zh[j] << endl;
266 TGraph *gr = new TGraph(Nhit, z, r); // graph intersection with layers
267 gr->SetMarkerStyle(kCircle);
268 gr->SetMarkerColor(kMagenta);
269 gr->SetMarkerSize(1);
270 gr->SetLineColor(kMagenta);
271 //
272 // clean up
273 //
274 delete[] zh;
275 delete[] rh;
276 delete[] ih;
277 delete[] hord;
278 return gr;
279}
280//
281// Covariance matrix estimation
282//
283void SolTrack::CovCalc(Bool_t Res, Bool_t MS)
284{
285 //
286 //
287 // Input flags:
288 // Res = .TRUE. turn on resolution effects/Use standard resolutions
289 // .FALSE. set all resolutions to 0
290 // MS = .TRUE. include Multiple Scattering
291 //
292 // Assumptions:
293 // 1. Measurement layers can do one or two measurements
294 // 2. On disks at constant z:
295 // - Upper side measurement is phi
296 // - Lower side measurement is R
297 //
298 // Fill list of layers hit
299 //
300 Int_t ntry = 0;
301 Int_t ntrymax = 0;
302 Int_t Nhit = nHit(); // Total number of layers hit
303 Double_t *zhh = new Double_t[Nhit]; // z of hit
304 Double_t *rhh = new Double_t[Nhit]; // r of hit
305 Double_t *dhh = new Double_t[Nhit]; // distance of hit from origin
306 Int_t *ihh = new Int_t[Nhit]; // true index of layer
307 Int_t kmh; // Number of measurement layers hit
308 //
309 kmh = HitList(ihh, rhh, zhh); // hit layer list
310 Int_t mTot = 0; // Total number of measurements
311 for (Int_t i = 0; i < Nhit; i++)
312 {
313 Double_t rr = rhh[i];
314 dhh[i] = TMath::ASin(C() * TMath::Sqrt((rr * rr - D() * D()) / (1. + 2 * C() * D()))) / C(); // Arc length traveled
315 if (fG->isMeasure(ihh[i])) mTot += fG->lND(ihh[i]); // Count number of measurements
316 }
317 //
318 // Order hit list by increasing arc length
319 //
320 Int_t *hord = new Int_t[Nhit]; // hit order by increasing distance from origin
321 TMath::Sort(Nhit, dhh, hord, kFALSE); // Order by increasing distance from origin
322 Double_t *zh = new Double_t[Nhit]; // d-ordered z of hit
323 Double_t *rh = new Double_t[Nhit]; // d-ordered r of hit
324 Int_t *ih = new Int_t[Nhit]; // d-ordered true index of layer
325 for (Int_t i = 0; i < Nhit; i++)
326 {
327 Int_t il = hord[i]; // Hit layer numbering
328 zh[i] = zhh[il];
329 rh[i] = rhh[il];
330 ih[i] = ihh[il];
331 }
332 //
333 // Store interdistances and multiple scattering angles
334 //
335 Double_t sn2t = 1.0 / (1.0 + ct()*ct()); //sin^2 theta of track
336 Double_t cs2t = 1.0 - sn2t; //cos^2 theta
337 Double_t snt = TMath::Sqrt(sn2t); // sin theta
338 Double_t cst = TMath::Sqrt(cs2t); // cos theta
339 Double_t px0 = pt() * TMath::Cos(phi0()); // Momentum at minimum approach
340 Double_t py0 = pt() * TMath::Sin(phi0());
341 Double_t pz0 = pt() * ct();
342 //
343 TMatrixDSym dik(Nhit); dik.Zero(); // Distances between layers
344 Double_t *thms = new Double_t[Nhit]; // Scattering angles/plane
345 Double_t* cs = new Double_t[Nhit]; // Cosine of angle with normal in transverse plane
346 //
347 for (Int_t ii = 0; ii < Nhit; ii++) // Hit layer loop
348 {
349 Int_t i = ih[ii]; // Get true layer number
350 Int_t il = hord[ii]; // Unordered layer
351 Double_t B = C()*TMath::Sqrt((rh[ii] * rh[ii] - D()*D()) / (1 + 2 * C()*D()));
352 //
353 Double_t pxi = px0*(1-2*B*B)-2*py0*B*TMath::Sqrt(1-B*B); // Momentum at scattering layer
354 Double_t pyi = py0*(1-2*B*B)+2*px0*B*TMath::Sqrt(1-B*B);
355 Double_t pzi = pz0;
356 Double_t ArgRp = (rh[ii]*C() + (1 + C() * D())*D() / rh[ii]) / (1 + 2 * C()*D());
357 //
358 Double_t phi = phi0() + TMath::ASin(ArgRp);
359 Double_t nx = TMath::Cos(phi); // Barrel layer normal
360 Double_t ny = TMath::Sin(phi);
361 Double_t nz = 0.0;
362 cs[ii] = TMath::Abs((pxi * nx + pyi * ny) / pt());
363 //
364 if (fG->lTyp(i) == 2) // this is Z layer
365 {
366 nx = 0.0;
367 ny = 0.0;
368 nz = 1.0;
369 }
370 Double_t corr = TMath::Abs(pxi*nx + pyi * ny + pzi * nz) / p();
371 Double_t Rlf = fG->lTh(i) / (corr*fG->lX0(i)); // Rad. length fraction
372 thms[ii] = 0.0136*TMath::Sqrt(Rlf)*(1.0 + 0.038*TMath::Log(Rlf)) / p(); // MS angle
373 if (!MS)thms[ii] = 0;
374 //
375 for (Int_t kk = 0; kk < ii; kk++) // Fill distances between layers
376 {
377 Int_t kl = hord[kk]; // Unordered layer
378 dik(ii, kk) = TMath::Abs(dhh[il] - dhh[kl])/snt;
379 dik(kk, ii) = dik(ii, kk);
380 }
381 }
382 //
383 // Fill measurement covariance
384 //
385 TVectorD tPar(5,fpar);
386 //
387 TMatrixDSym Sm(mTot); Sm.Zero(); // Measurement covariance
388 TMatrixD Rm(mTot, 5); // Derivative matrix
389 Int_t im = 0; // Initialize number of measurement counter
390 //
391 // Fill derivatives and error matrix with MS
392 //
393 for (Int_t ii = 0; ii < Nhit; ii++)
394 {
395 Int_t i = ih[ii]; // True layer number
396 Int_t ityp = fG->lTyp(i); // Layer type Barrel or Z
397 Int_t nmeai = fG->lND(i); // # measurements in layer
398
399 if (fG->isMeasure(i))
400 {
401 Double_t Ri = rh[ii];
402 Double_t zi = zh[ii];
403 //
404 for (Int_t nmi = 0; nmi < nmeai; nmi++)
405 {
406 Double_t stri = 0; // Stereo angle
407 Double_t sig = 0; // Layer resolution
408 // Constant R derivatives
409 TVectorD dRphi(5); dRphi.Zero(); // R-phi derivatives @ const. R
410 TVectorD dRz(5); dRz.Zero(); // z derivatives @ const. R
411 //
412 if (nmi + 1 == 1) // Upper layer measurements
413 {
414 stri = fG->lStU(i); // Stereo angle
415 Double_t csa = TMath::Cos(stri);
416 Double_t ssa = TMath::Sin(stri);
417 //
418 sig = fG->lSgU(i); // Resolution
419 if (ityp == 1) // Barrel type layer (Measure R-phi, stereo or z at const. R)
420 {
421 //
422 // Exact solution
423 dRphi = derRphi_R(tPar, Ri);
424 dRz = derZ_R (tPar, Ri);
425 //
426 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative
427 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative
428 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative
429 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative
430 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative
431 }
432 if (ityp == 2) // Z type layer (Measure R-phi at const. Z)
433 {
434 TVectorD dRphz(5); dRphz.Zero(); // R-phi derivatives @ const. z
435 dRphz = derRphi_Z(tPar, zi);
436 //
437 Rm(im, 0) = dRphz(0); // D derivative
438 Rm(im, 1) = dRphz(1); // phi0 derivative
439 Rm(im, 2) = dRphz(2); // C derivative
440 Rm(im, 3) = dRphz(3); // z0 derivative
441 Rm(im, 4) = dRphz(4); // cot(theta) derivative
442 }
443 }
444 if (nmi + 1 == 2) // Lower layer measurements
445 {
446 stri = fG->lStL(i); // Stereo angle
447 Double_t csa = TMath::Cos(stri);
448 Double_t ssa = TMath::Sin(stri);
449 sig = fG->lSgL(i); // Resolution
450 if (ityp == 1) // Barrel type layer (measure R-phi, stereo or z at const. R)
451 {
452 //
453 // Exact solution
454 dRphi = derRphi_R(tPar, Ri);
455 dRz = derZ_R (tPar, Ri);
456 //
457 Rm(im, 0) = csa * dRphi(0) - ssa * dRz(0); // D derivative
458 Rm(im, 1) = csa * dRphi(1) - ssa * dRz(1); // phi0 derivative
459 Rm(im, 2) = csa * dRphi(2) - ssa * dRz(2); // C derivative
460 Rm(im, 3) = csa * dRphi(3) - ssa * dRz(3); // z0 derivative
461 Rm(im, 4) = csa * dRphi(4) - ssa * dRz(4); // cot(theta) derivative
462 }
463 if (ityp == 2) // Z type layer (Measure R at const. z)
464 {
465 TVectorD dRRz(5); dRRz.Zero(); // R derivatives @ const. z
466 dRRz = derR_Z(tPar, zi);
467 //
468 Rm(im, 0) = dRRz(0); // D derivative
469 Rm(im, 1) = dRRz(1); // phi0 derivative
470 Rm(im, 2) = dRRz(2); // C derivative
471 Rm(im, 3) = dRRz(3); // z0 derivative
472 Rm(im, 4) = dRRz(4); // cot(theta) derivative
473 }
474 }
475 // Derivative calculation completed
476 //
477 // Now calculate measurement error matrix
478 //
479 Int_t km = 0;
480 Double_t CosMin = TMath::Sin(TMath::Pi() / 9.); // Protect for derivative explosion
481 for (Int_t kk = 0; kk <= ii; kk++)
482 {
483 Int_t k = ih[kk]; // True layer number
484 Int_t ktyp = fG->lTyp(k); // Layer type Barrel or disk
485 Int_t nmeak = fG->lND(k); // # measurements in layer
486 if (fG->isMeasure(k))
487 {
488 for (Int_t nmk = 0; nmk < nmeak; nmk++)
489 {
490 Double_t strk = 0;
491 if (nmk + 1 == 1) strk = fG->lStU(k); // Stereo angle upper
492 if (nmk + 1 == 2) strk = fG->lStL(k); // Stereo angle lower
493 //if (im == km && Res) Sm(im, km) += sig*sig; // Detector resolution on diagonal
494 if (im == km && Res) {
495 Double_t sg = sig;
496 if(TMath::Abs(strk) < TMath::Pi()/6. && cs[kk] < CosMin)
497 TMath::Min(1000.*sig,sg = sig/pow(cs[kk],4));
498 Sm(im, km) += sg * sg; // Detector resolution on diagonal
499 }
500 //
501 // Loop on all layers below for MS contributions
502 for (Int_t jj = 0; jj < kk; jj++)
503 {
504 Double_t di = dik(ii, jj);
505 Double_t dk = dik(kk, jj);
506 Double_t ms = thms[jj];
507 Double_t msk = ms; Double_t msi = ms;
508 if (ityp == 1) msi = ms / snt; // Barrel
509 else if (ityp == 2) msi = ms / cst; // Disk
510 if (ktyp == 1) msk = ms / snt; // Barrel
511 else if (ktyp == 2) msk = ms / cst; // Disk
512 Double_t ci = TMath::Abs(TMath::Cos(stri)); Double_t si = TMath::Abs(TMath::Sin(stri));
513 Double_t ck = TMath::Abs(TMath::Cos(strk)); Double_t sk = TMath::Abs(TMath::Sin(strk));
514 Sm(im, km) += di*dk*(ci*ck*ms*ms + si*sk*msi*msk); // Ms contribution
515 }
516 //
517 Sm(km, im) = Sm(im, km);
518 km++;
519 }
520 }
521 }
522 im++; mTot = im;
523 }
524 }
525 }
526 Sm.ResizeTo(mTot, mTot);
527 TMatrixDSym SmTemp = Sm;
528 Rm.ResizeTo(mTot, 5);
529 //
530 //**********************************************************************
531 // Calculate covariance from derivatives and measurement error matrix *
532 //**********************************************************************
533 //
534 TMatrixDSym DSmInv(mTot); DSmInv.Zero();
535 for (Int_t id = 0; id < mTot; id++) DSmInv(id, id) = 1.0 / TMath::Sqrt(Sm(id, id));
536 TMatrixDSym SmN = Sm.Similarity(DSmInv); // Normalize diagonal to 1
537 //
538 // Protected matrix inversions
539 //
540 TDecompChol Chl(SmN,1.e-12);
541 TMatrixDSym SmNinv = SmN;
542 if (Chl.Decompose())
543 {
544 Bool_t OK;
545 SmNinv = Chl.Invert(OK);
546 }
547 else
548 {
549 std::cout << "SolTrack::CovCalc: Error matrix not positive definite. Recovering ...." << std::endl;
550 //cout << "pt = " << pt() << endl;
551 if (ntry < ntrymax)
552 {
553 SmNinv.Print();
554 ntry++;
555 }
556 //
557 TMatrixDSym rSmN = MakePosDef(SmN); SmN = rSmN;
558 TDecompChol rChl(SmN);
559 SmNinv = SmN;
560 Bool_t OK = rChl.Decompose();
561 SmNinv = rChl.Invert(OK);
562 }
563 Sm = SmNinv.Similarity(DSmInv); // Error matrix inverted
564 TMatrixDSym H = Sm.SimilarityT(Rm); // Calculate half Hessian
565 const Int_t Npar = 5;
566 TMatrixDSym DHinv(Npar); DHinv.Zero();
567 for (Int_t i = 0; i < Npar; i++)DHinv(i, i) = 1.0 / TMath::Sqrt(H(i, i));
568 TMatrixDSym Hnrm = H.Similarity(DHinv);
569 // Invert and restore
570 Hnrm.Invert();
571 fCov = Hnrm.Similarity(DHinv);
572 //
573 // debug
574 //
575 if(TMath::IsNaN(fCov(0,0)))
576 {
577 std::cout<<"SolTrack::CovCalc: NaN found in covariance matrix"<<std::endl;
578 }
579 //
580 // Lots of cleanup to do
581 delete[] zhh;
582 delete[] rhh;
583 delete[] dhh;
584 delete[] ihh;
585 delete[] hord;
586 delete[] zh;
587 delete[] rh;
588 delete[] ih;
589 delete[] cs;
590 delete[] thms;
591}
592//
593// Force positive definitness in normalized matrix
594TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat)
595{
596 //
597 // Input: symmetric matrix with 1's on diagonal
598 // Output: positive definite matrix with 1's on diagonal
599 //
600 // Default return value
601 TMatrixDSym rMatN = NormMat;
602 // Check the diagonal
603 Bool_t Check = kFALSE;
604 Int_t Size = NormMat.GetNcols();
605 for (Int_t i = 0; i < Size; i++)if (TMath::Abs(NormMat(i, i) - 1.0)>1.0E-15)Check = kTRUE;
606 if (Check)
607 {
608 std::cout << "SolTrack::MakePosDef: input matrix doesn ot have 1 on diagonal. Abort." << std::endl;
609 return rMatN;
610 }
611 //
612 // Diagonalize matrix
613 TMatrixDSymEigen Eign(NormMat);
614 TMatrixD U = Eign.GetEigenVectors();
615 TVectorD lambda = Eign.GetEigenValues();
616 //cout << "Eigenvalues:"; lambda.Print();
617 //cout << "Input matrix: "; NormMat.Print();
618 // Reset negative eigenvalues to small positive value
619 TMatrixDSym D(Size); D.Zero(); Double_t eps = 1.0e-13;
620 for (Int_t i = 0; i < Size; i++)
621 {
622 D(i, i) = lambda(i);
623 if (lambda(i) <= 0) D(i, i) = eps;
624 }
625 //Rebuild matrix
626 TMatrixD Ut(TMatrixD::kTransposed, U);
627 TMatrixD rMat = (U*D)*Ut; // Now it is positive defite
628 // Restore all ones on diagonal
629 for (Int_t i1 = 0; i1 < Size; i1++)
630 {
631 Double_t rn1 = TMath::Sqrt(rMat(i1, i1));
632 for (Int_t i2 = 0; i2 <= i1; i2++)
633 {
634 Double_t rn2 = TMath::Sqrt(rMat(i2, i2));
635 rMatN(i1, i2) = 0.5*(rMat(i1, i2) + rMat(i2, i1)) / (rn1*rn2);
636 rMatN(i2, i1) = rMatN(i1, i2);
637 }
638 }
639 //cout << "Rebuilt matrix: "; rMatN.Print();
640 return rMatN;
641}
Note: See TracBrowser for help on using the repository browser.