Fork me on GitHub

source: git/external/TrackCovariance/SolTrack.cc@ 1ad8eca

Last change on this file since 1ad8eca was 170a11d, checked in by michele <michele.selvaggi@…>, 4 years ago

included acceptance and hits in TrackCovariance
  • Property mode set to 100644
File size: 18.5 KB
Line 
1#include <iostream>
2
3#include <TString.h>
4#include <TMath.h>
5#include <TMatrixD.h>
6#include <TMatrixDSym.h>
7#include <TDecompChol.h>
8#include <TMatrixDSymEigen.h>
9
10#include "SolGeom.h"
11#include "SolTrack.h"
12
13using namespace std;
14
15SolTrack::SolTrack(Double_t *x, Double_t *p, SolGeom *G)
16{
17 fG = G;
18 // Store momentum
19 fp[0] = p[0]; fp[1] = p[1]; fp[2] = p[2];
20 Double_t px = p[0]; Double_t py = p[1]; Double_t pz = p[2];
21 fx[0] = x[0]; fx[1] = x[1]; fx[2] = x[2];
22 Double_t xx = x[0]; Double_t yy = x[1]; Double_t zz = x[2];
23 // Store parameters
24 Double_t pt = TMath::Sqrt(px*px + py*py);
25 Double_t Charge = 1.0; // Don't worry about charge for now
26 Double_t a = -Charge*G->B()*0.2998; // Normalized speed of light
27 Double_t C = a / (2 * pt); // pt in GeV, B in Tesla, C in meters
28 Double_t r2 = xx*xx + yy*yy;
29 Double_t cross = xx*py - yy*px;
30 Double_t T = TMath::Sqrt(pt*pt - 2 * a*cross + a*a*r2);
31 Double_t phi0 = TMath::ATan2((py - a*xx) / T, (px + a*yy) / T);
32 Double_t D;
33 if (pt < 10.0) D = (T - pt) / a;
34 else D = (-2 * cross + a*r2) / (T + pt);
35 Double_t B = C*TMath::Sqrt((r2 - D*D) / (1 + 2 * C*D));
36 Double_t st = TMath::ASin(B) / C;
37 Double_t ct = pz / pt;
38 Double_t z0 = zz - ct*st;
39 fpar[0] = D;
40 fpar[1] = phi0;
41 fpar[2] = C;
42 fpar[3] = z0;
43 fpar[4] = ct;
44 // Init covariances
45 fCov.ResizeTo(5, 5);
46}
47SolTrack::SolTrack(TVector3 x, TVector3 p, SolGeom* G)
48{
49 fG = G;
50 // Store momentum
51 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 parameters
56 Double_t pt = TMath::Sqrt(px * px + py * py);
57 Double_t Charge = 1.0; // Don't worry about charge for now
58 Double_t a = -Charge * G->B() * 0.2998; // Normalized speed of light
59 Double_t C = a / (2 * pt); // pt in GeV, B in Tesla, C in meters
60 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 covariances
79 //
80 fCov.ResizeTo(5, 5);
81}
82
83SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G)
84{
85 fG = G;
86 // Store parameters
87 fpar[0] = D;
88 fpar[1] = phi0;
89 fpar[2] = C;
90 fpar[3] = z0;
91 fpar[4] = ct;
92 // Store momentum
93 Double_t pt = G->B()*0.2998 / TMath::Abs(2 * C);
94 Double_t px = pt*TMath::Cos(phi0);
95 Double_t py = pt*TMath::Sin(phi0);
96 Double_t pz = pt*ct;
97
98 fp[0] = px; fp[1] = py; fp[2] = pz;
99 fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0); fx[2] = z0;
100 // Init covariances
101 fCov.ResizeTo(5, 5);
102}
103// Destructor
104SolTrack::~SolTrack()
105{
106 delete[] & fp;
107 delete[] & fpar;
108 fCov.Clear();
109}
110// Calculate intersection with given layer
111Bool_t SolTrack::HitLayer(Int_t il, Double_t &R, Double_t &phi, Double_t &zz)
112{
113 Double_t Di = D();
114 Double_t phi0i = phi0();
115 Double_t Ci = C();
116 Double_t z0i = z0();
117 Double_t cti = ct();
118
119 R = 0; phi = 0; zz = 0;
120
121 Bool_t val = kFALSE;
122 if (fG->lTyp(il) == 1) // Cylinder: layer at constant R
123 {
124 R = fG->lPos(il);
125 Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
126 if (TMath::Abs(argph) < 1.0)
127 {
128 Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di));
129 if (TMath::Abs(argz) < 1.0)
130 {
131 zz = z0i + cti*TMath::ASin(argz) / Ci;
132 if (zz > fG->lxMin(il) && zz < fG->lxMax(il))
133 {
134 phi = phi0i + TMath::ASin(argph);
135 val = kTRUE;
136 }
137 }
138 }
139 }
140 else if (fG->lTyp(il) == 2) // disk: layer at constant z
141 {
142 zz = fG->lPos(il);
143 Double_t arg = Ci*(zz - z0i) / cti;
144 if (TMath::Abs(arg) < 1.0 && (zz - z0i) / cti > 0)
145 {
146 R = TMath::Sqrt(Di*Di + (1. + 2.*Ci*Di)*pow(TMath::Sin(arg), 2) / (Ci*Ci));
147 if (R > fG->lxMin(il) && R < fG->lxMax(il))
148 {
149 Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
150 if (TMath::Abs(arg1) < 1.0)
151 {
152 phi = phi0i + TMath::ASin(arg1);
153 val = kTRUE;
154 }
155 }
156 }
157 }
158 //
159 return val;
160}
161// # of layers hit
162Int_t SolTrack::nHit()
163{
164 Int_t kh = 0;
165 Double_t R; Double_t phi; Double_t zz;
166 for (Int_t i = 0; i < fG->Nl(); i++)
167 if (HitLayer(i, R, phi, zz))kh++;
168
169 return kh;
170}
171
172// # of measurement layers hit
173Int_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// List of layers hit with intersections
186Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh)
187{
188 // Return lists of hits associated to a track including all scattering layers.
189 // Return value is the total number of measurement hits
190 // kmh = total number of measurement layers hit for given type
191 // ihh = pointer to layer number
192 // rhh = radius of hit
193 // zhh = z of hit
194
195 // ***** NB: double layers with stereo on lower layer not included
196
197 Int_t kh = 0; // Number of layers hit
198 Int_t kmh = 0; // Number of measurement layers of given type
199 for (Int_t i = 0; i < fG->Nl(); i++)
200 {
201 Double_t R; Double_t phi; Double_t zz;
202 if (HitLayer(i, R, phi, zz)) // Only barrel type layers
203 {
204 zhh[kh] = zz;
205 rhh[kh] = R;
206 ihh[kh] = i;
207 if (fG->isMeasure(i))kmh++;
208 kh++;
209 }
210 }
211
212 return kmh;
213}
214
215// List of XYZ measurements without any error
216Int_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 hits
221 // kmh = total number of measurement layers hit for given type
222 // ihh = pointer to layer number
223 // Xh, Yh, Zh = X, Y, Z of hit - No measurement error - No multiple scattering
224 //
225 //
226 Int_t kmh = 0; // Number of measurement layers hit
227 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 layers
231 {
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// Covariance matrix estimation
247//
248void SolTrack::CovCalc(Bool_t Res, Bool_t MS)
249{
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);
543}
544
545// Force positive definitness in normalized matrix
546TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat)
547{
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}
Note: See TracBrowser for help on using the repository browser.