| 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 |
|
|---|
| 13 | using namespace std;
|
|---|
| 14 |
|
|---|
| 15 | SolTrack::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 | }
|
|---|
| 47 |
|
|---|
| 48 | SolTrack::SolTrack(Double_t D, Double_t phi0, Double_t C, Double_t z0, Double_t ct, SolGeom *G)
|
|---|
| 49 | {
|
|---|
| 50 | fG = G;
|
|---|
| 51 | // Store parameters
|
|---|
| 52 | fpar[0] = D;
|
|---|
| 53 | fpar[1] = phi0;
|
|---|
| 54 | fpar[2] = C;
|
|---|
| 55 | fpar[3] = z0;
|
|---|
| 56 | fpar[4] = ct;
|
|---|
| 57 | // Store momentum
|
|---|
| 58 | Double_t pt = G->B()*0.2998 / TMath::Abs(2 * C);
|
|---|
| 59 | Double_t px = pt*TMath::Cos(phi0);
|
|---|
| 60 | Double_t py = pt*TMath::Sin(phi0);
|
|---|
| 61 | Double_t pz = pt*ct;
|
|---|
| 62 |
|
|---|
| 63 | fp[0] = px; fp[1] = py; fp[2] = pz;
|
|---|
| 64 | fx[0] = -D*TMath::Sin(phi0); fx[1] = D*TMath::Cos(phi0); fx[2] = z0;
|
|---|
| 65 | // Init covariances
|
|---|
| 66 | fCov.ResizeTo(5, 5);
|
|---|
| 67 | }
|
|---|
| 68 | // Destructor
|
|---|
| 69 | SolTrack::~SolTrack()
|
|---|
| 70 | {
|
|---|
| 71 | fCov.Clear();
|
|---|
| 72 | }
|
|---|
| 73 | // Calculate intersection with given layer
|
|---|
| 74 | Bool_t SolTrack::HitLayer(Int_t il, Double_t &R, Double_t &phi, Double_t &zz)
|
|---|
| 75 | {
|
|---|
| 76 | Double_t Di = D();
|
|---|
| 77 | Double_t phi0i = phi0();
|
|---|
| 78 | Double_t Ci = C();
|
|---|
| 79 | Double_t z0i = z0();
|
|---|
| 80 | Double_t cti = ct();
|
|---|
| 81 |
|
|---|
| 82 | R = 0; phi = 0; zz = 0;
|
|---|
| 83 |
|
|---|
| 84 | Bool_t val = kFALSE;
|
|---|
| 85 | if (fG->lTyp(il) == 1) // Cylinder: layer at constant R
|
|---|
| 86 | {
|
|---|
| 87 | R = fG->lPos(il);
|
|---|
| 88 | Double_t argph = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
|
|---|
| 89 | if (TMath::Abs(argph) < 1.0)
|
|---|
| 90 | {
|
|---|
| 91 | Double_t argz = Ci*TMath::Sqrt((R*R - Di*Di) / (1 + 2 * Ci*Di));
|
|---|
| 92 | if (TMath::Abs(argz) < 1.0)
|
|---|
| 93 | {
|
|---|
| 94 | zz = z0i + cti*TMath::ASin(argz) / Ci;
|
|---|
| 95 | if (zz > fG->lxMin(il) && zz < fG->lxMax(il))
|
|---|
| 96 | {
|
|---|
| 97 | phi = phi0i + TMath::ASin(argph);
|
|---|
| 98 | val = kTRUE;
|
|---|
| 99 | }
|
|---|
| 100 | }
|
|---|
| 101 | }
|
|---|
| 102 | }
|
|---|
| 103 | else if (fG->lTyp(il) == 2) // disk: layer at constant z
|
|---|
| 104 | {
|
|---|
| 105 | zz = fG->lPos(il);
|
|---|
| 106 | Double_t arg = Ci*(zz - z0i) / cti;
|
|---|
| 107 | if (TMath::Abs(arg) < 1.0 && (zz - z0i) / cti > 0)
|
|---|
| 108 | {
|
|---|
| 109 | R = TMath::Sqrt(Di*Di + (1. + 2.*Ci*Di)*pow(TMath::Sin(arg), 2) / (Ci*Ci));
|
|---|
| 110 | if (R > fG->lxMin(il) && R < fG->lxMax(il))
|
|---|
| 111 | {
|
|---|
| 112 | Double_t arg1 = (Ci*R + (1 + Ci*Di)*Di / R) / (1. + 2.*Ci*Di);
|
|---|
| 113 | if (TMath::Abs(arg1) < 1.0)
|
|---|
| 114 | {
|
|---|
| 115 | phi = phi0i + TMath::ASin(arg1);
|
|---|
| 116 | val = kTRUE;
|
|---|
| 117 | }
|
|---|
| 118 | }
|
|---|
| 119 | }
|
|---|
| 120 | }
|
|---|
| 121 | //
|
|---|
| 122 | return val;
|
|---|
| 123 | }
|
|---|
| 124 | // # of layers hit
|
|---|
| 125 | Int_t SolTrack::nHit()
|
|---|
| 126 | {
|
|---|
| 127 | Int_t kh = 0;
|
|---|
| 128 | Double_t R; Double_t phi; Double_t zz;
|
|---|
| 129 | for (Int_t i = 0; i < fG->Nl(); i++)
|
|---|
| 130 | if (HitLayer(i, R, phi, zz))kh++;
|
|---|
| 131 |
|
|---|
| 132 | return kh;
|
|---|
| 133 | }
|
|---|
| 134 | // List of layers hit with intersections
|
|---|
| 135 | Int_t SolTrack::HitList(Int_t *&ihh, Double_t *&rhh, Double_t *&zhh)
|
|---|
| 136 | {
|
|---|
| 137 | // Return lists of hits associated to a track including all scattering layers.
|
|---|
| 138 | // Return value is the total number of measurement hits
|
|---|
| 139 | // kmh = total number of measurement layers hit for given type
|
|---|
| 140 | // ihh = pointer to layer number
|
|---|
| 141 | // rhh = radius of hit
|
|---|
| 142 | // zhh = z of hit
|
|---|
| 143 |
|
|---|
| 144 | // ***** NB: double layers with stereo on lower layer not included
|
|---|
| 145 |
|
|---|
| 146 | Int_t kh = 0; // Number of layers hit
|
|---|
| 147 | Int_t kmh = 0; // Number of measurement layers of given type
|
|---|
| 148 | for (Int_t i = 0; i < fG->Nl(); i++)
|
|---|
| 149 | {
|
|---|
| 150 | Double_t R; Double_t phi; Double_t zz;
|
|---|
| 151 | if (HitLayer(i, R, phi, zz)) // Only barrel type layers
|
|---|
| 152 | {
|
|---|
| 153 | zhh[kh] = zz;
|
|---|
| 154 | rhh[kh] = R;
|
|---|
| 155 | ihh[kh] = i;
|
|---|
| 156 | if (fG->isMeasure(i))kmh++;
|
|---|
| 157 | kh++;
|
|---|
| 158 | }
|
|---|
| 159 | }
|
|---|
| 160 |
|
|---|
| 161 | return kmh;
|
|---|
| 162 | }
|
|---|
| 163 | // Covariance matrix estimation
|
|---|
| 164 | void SolTrack::CovCalc(Bool_t Res, Bool_t MS)
|
|---|
| 165 | {
|
|---|
| 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);
|
|---|
| 431 | }
|
|---|
| 432 |
|
|---|
| 433 | // Force positive definitness in normalized matrix
|
|---|
| 434 | TMatrixDSym SolTrack::MakePosDef(TMatrixDSym NormMat)
|
|---|
| 435 | {
|
|---|
| 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 | }
|
|---|
