| f8832015 |
1 | #include <TString.h> |
| 32d38de2 |
2 | #include <TMath.h> |
| 3 | #include "AliITSUSeed.h" |
| 716ccba7 |
4 | #include "AliLog.h" |
| 3e4e3c23 |
5 | #include "AliESDtrack.h" |
| 32d38de2 |
6 | using namespace TMath; |
| 7 | |
| 8 | ClassImp(AliITSUSeed) |
| 9 | |
| 10 | //_________________________________________________________________________ |
| 11 | AliITSUSeed::AliITSUSeed() |
| f8832015 |
12 | : fHitsPattern(0) |
| b8b59e05 |
13 | ,fNChildren(0) |
| f8832015 |
14 | ,fClID(0) |
| 15 | ,fChi2Glo(0) |
| 16 | ,fChi2Cl(0) |
| 716ccba7 |
17 | ,fChi2Penalty(0) |
| c61e50c3 |
18 | ,fParent(0) |
| 32d38de2 |
19 | { |
| 20 | // def c-tor |
| 44785f3e |
21 | ResetFMatrix(); |
| 32d38de2 |
22 | } |
| 23 | |
| 24 | //_________________________________________________________________________ |
| 25 | AliITSUSeed::~AliITSUSeed() |
| 26 | { |
| 716ccba7 |
27 | // d-tor |
| 32d38de2 |
28 | } |
| 29 | |
| 30 | //_________________________________________________________________________ |
| 31 | AliITSUSeed::AliITSUSeed(const AliITSUSeed& src) |
| c61e50c3 |
32 | :AliExternalTrackParam(src) |
| f8832015 |
33 | ,fHitsPattern(src.fHitsPattern) |
| b8b59e05 |
34 | ,fNChildren(src.fNChildren) |
| c61e50c3 |
35 | ,fClID(src.fClID) |
| f8832015 |
36 | ,fChi2Glo(src.fChi2Glo) |
| 37 | ,fChi2Cl(src.fChi2Cl) |
| 716ccba7 |
38 | ,fChi2Penalty(src.fChi2Penalty) |
| c61e50c3 |
39 | ,fParent(src.fParent) |
| 32d38de2 |
40 | { |
| 41 | // def c-tor |
| 44785f3e |
42 | for (int i=kNFElem;i--;) fFMatrix[i] = src.fFMatrix[i]; |
| 716ccba7 |
43 | for (int i=kNKElem;i--;) fKMatrix[i] = src.fKMatrix[i]; |
| 44 | for (int i=kNRElem;i--;) fRMatrix[i] = src.fRMatrix[i]; |
| 943e1898 |
45 | fResid[0]=src.fResid[0]; |
| 46 | fResid[1]=src.fResid[1]; |
| 47 | fCovIYZ[0]=src.fCovIYZ[0]; |
| 48 | fCovIYZ[1]=src.fCovIYZ[1]; |
| 49 | fCovIYZ[2]=src.fCovIYZ[2]; |
| 50 | // |
| 32d38de2 |
51 | } |
| 52 | |
| 53 | //_________________________________________________________________________ |
| 54 | AliITSUSeed &AliITSUSeed::operator=(const AliITSUSeed& src) |
| 55 | { |
| 56 | // def c-tor |
| 57 | if (this == &src) return *this; |
| 943e1898 |
58 | this->~AliITSUSeed(); |
| 59 | new(this) AliITSUSeed(src); |
| 32d38de2 |
60 | return *this; |
| 61 | } |
| f8832015 |
62 | |
| 63 | //_________________________________________________________________________ |
| 64 | void AliITSUSeed::Print(Option_t* opt) const |
| 65 | { |
| 66 | // print seed info |
| 67 | int lr,cl = GetLrCluster(lr); |
| b8b59e05 |
68 | printf("%cLr%d Nchild: %3d Cl:%4d Chi2Glo:%7.2f(%7.2f) Chi2Cl:%7.2f Penalty: %7.2f",IsKilled() ? '-':' ', |
| 69 | lr,GetNChildren(),cl,GetChi2Glo(),GetChi2GloNrm(),GetChi2Cl(), GetChi2Penalty()); |
| f8832015 |
70 | printf(" |"); |
| 716ccba7 |
71 | int lrc=0; |
| 72 | const AliITSUSeed *sdc = this; |
| 73 | while(1) { |
| 74 | if (lrc<lr) printf("."); |
| 75 | else { |
| 76 | sdc = sdc->GetParent(lrc); |
| 77 | if (!sdc) break; |
| 78 | printf("%c",sdc->GetClusterID()<0 ? '.': (sdc->IsFake() ? '-':'+')); |
| 79 | } |
| 80 | lrc++; |
| 81 | } |
| 82 | printf("|\n"); |
| f8832015 |
83 | TString opts = opt; opts.ToLower(); |
| 84 | if (opts.Contains("etp")) AliExternalTrackParam::Print(); |
| 85 | if (opts.Contains("parent") && GetParent()) GetParent()->Print(opt); |
| 86 | } |
| 3dd9c283 |
87 | |
| 3e4e3c23 |
88 | //______________________________________________________________________________ |
| 89 | void AliITSUSeed::InitFromESDTrack(const AliESDtrack* esdTr) |
| 90 | { |
| 91 | // init seed from ESD track |
| 92 | TObject::Clear(); |
| 93 | AliExternalTrackParam::operator=(*esdTr); |
| 94 | ResetFMatrix(); |
| 95 | fHitsPattern = 0; |
| 96 | fClID = 0; |
| b8b59e05 |
97 | fNChildren = 0; |
| 3e4e3c23 |
98 | fChi2Glo = fChi2Cl = fChi2Penalty = 0; |
| 99 | fParent = 0; //!!! |
| 100 | } |
| 101 | |
| 102 | |
| 3dd9c283 |
103 | //______________________________________________________________________________ |
| 104 | Float_t AliITSUSeed::GetChi2GloNrm() const |
| 105 | { |
| 106 | int ndf = 2*GetNLayersHit() - 5; |
| 716ccba7 |
107 | return (ndf>0 ? fChi2Glo/ndf : fChi2Glo) + fChi2Penalty; |
| 3dd9c283 |
108 | } |
| 109 | |
| 110 | |
| 111 | //______________________________________________________________________________ |
| 112 | Int_t AliITSUSeed::Compare(const TObject* obj) const |
| 113 | { |
| 114 | // compare clusters accodring to specific mode |
| f9c7eb32 |
115 | const AliITSUSeed* sd = (const AliITSUSeed*)obj; |
| 3dd9c283 |
116 | const Float_t kTol = 1e-5; |
| 117 | if (!IsKilled() && sd->IsKilled()) return -1; |
| 118 | if ( IsKilled() &&!sd->IsKilled()) return 1; |
| 119 | // |
| 716ccba7 |
120 | float chi2This = GetChi2GloNrm(); |
| 121 | float chi2Other = sd->GetChi2GloNrm(); |
| 122 | |
| 123 | if (chi2This+kTol<chi2Other) return -1; |
| 124 | else if (chi2This-kTol>chi2Other) return 1; |
| 3dd9c283 |
125 | return 0; |
| 126 | } |
| 127 | |
| 128 | //______________________________________________________________________________ |
| 129 | Bool_t AliITSUSeed::IsEqual(const TObject* obj) const |
| 130 | { |
| 131 | // compare clusters accodring to specific mode |
| f9c7eb32 |
132 | const AliITSUSeed* sd = (const AliITSUSeed*)obj; |
| 3dd9c283 |
133 | const Float_t kTol = 1e-5; |
| 134 | if (IsKilled() != sd->IsKilled()) return kFALSE; |
| 716ccba7 |
135 | return Abs(GetChi2GloNrm() - sd->GetChi2GloNrm())<kTol; |
| 3dd9c283 |
136 | } |
| 44785f3e |
137 | |
| 138 | //______________________________________________________________________________ |
| 139 | Bool_t AliITSUSeed::PropagateToX(Double_t xk, Double_t b) |
| 140 | { |
| 141 | // Propagate this track to the plane X=xk (cm) in the field "b" (kG) |
| 142 | Double_t dx=xk-fX; |
| 143 | if (TMath::Abs(dx)<=kAlmost0) return kTRUE; |
| 144 | |
| 145 | Double_t crv=GetC(b); |
| 146 | if (TMath::Abs(b) < kAlmost0Field) crv=0.; |
| 147 | Double_t x2r = crv*dx; |
| 148 | Double_t f1=fP[2], f2=f1 + x2r; |
| 149 | if (TMath::Abs(f1) >= kAlmost1) return kFALSE; |
| 150 | if (TMath::Abs(f2) >= kAlmost1) return kFALSE; |
| 151 | if (TMath::Abs(fP[4])< kAlmost0) return kFALSE; |
| 152 | |
| 153 | Double_t &fP0=fP[0], &fP1=fP[1], &fP2=fP[2], &fP3=fP[3], &fP4=fP[4]; |
| 154 | Double_t |
| 155 | &fC00=fC[0], |
| 156 | &fC10=fC[1], &fC11=fC[2], |
| 157 | &fC20=fC[3], &fC21=fC[4], &fC22=fC[5], |
| 158 | &fC30=fC[6], &fC31=fC[7], &fC32=fC[8], &fC33=fC[9], |
| 159 | &fC40=fC[10], &fC41=fC[11], &fC42=fC[12], &fC43=fC[13], &fC44=fC[14]; |
| 160 | |
| 161 | Double_t r1=TMath::Sqrt((1.-f1)*(1.+f1)), r2=TMath::Sqrt((1.-f2)*(1.+f2)); |
| 162 | if (TMath::Abs(r1)<kAlmost0) return kFALSE; |
| 163 | if (TMath::Abs(r2)<kAlmost0) return kFALSE; |
| 164 | |
| 165 | fX=xk; |
| 166 | double dy2dx = (f1+f2)/(r1+r2); |
| 167 | fP0 += dx*dy2dx; |
| 168 | if (TMath::Abs(x2r)<0.05) { |
| 169 | fP1 += dx*(r2 + f2*dy2dx)*fP3; // Many thanks to P.Hristov ! |
| 170 | fP2 += x2r; |
| 171 | } |
| 172 | else { |
| 173 | // for small dx/R the linear apporximation of the arc by the segment is OK, |
| 174 | // but at large dx/R the error is very large and leads to incorrect Z propagation |
| 175 | // angle traversed delta = 2*asin(dist_start_end / R / 2), hence the arc is: R*deltaPhi |
| 176 | // The dist_start_end is obtained from sqrt(dx^2+dy^2) = x/(r1+r2)*sqrt(2+f1*f2+r1*r2) |
| 177 | // Similarly, the rotation angle in linear in dx only for dx<<R |
| 178 | double chord = dx*TMath::Sqrt(1+dy2dx*dy2dx); // distance from old position to new one |
| 179 | double rot = 2*TMath::ASin(0.5*chord*crv); // angular difference seen from the circle center |
| 180 | fP1 += rot/crv*fP3; |
| 181 | fP2 = TMath::Sin(rot + TMath::ASin(fP2)); |
| 182 | } |
| 183 | |
| 184 | //f = F - 1 |
| 185 | double r1i = 1./r1; |
| 186 | double r2i = 1./r2; |
| 187 | double tg1 = f1*r1i; |
| 188 | double tg2 = f2*r2i; |
| 189 | double v0 = 1. + dy2dx*tg2; |
| 190 | double v1 = (r1i+r2i)*(dy2dx*(tg1+tg2)+2); |
| 191 | double v2 = (r1i+r2i)*v0; |
| 192 | // |
| 193 | double f24 = dx*crv/fP4; |
| 194 | double f02 = dx*v1; |
| 195 | double f04 = dx*v2*f24; |
| 196 | double f12 = dx*fP3* (f2*v1+dy2dx-tg2); |
| 197 | double f13 = dx*r2*v0; |
| 198 | double f14 = dx*f24*fP3*(f2*v2+dy2dx-tg2); |
| 199 | // |
| 200 | //b = C*ft |
| 201 | Double_t b00=f02*fC20 + f04*fC40, b01=f12*fC20 + f14*fC40 + f13*fC30; |
| 202 | Double_t b02=f24*fC40; |
| 203 | Double_t b10=f02*fC21 + f04*fC41, b11=f12*fC21 + f14*fC41 + f13*fC31; |
| 204 | Double_t b12=f24*fC41; |
| 205 | Double_t b20=f02*fC22 + f04*fC42, b21=f12*fC22 + f14*fC42 + f13*fC32; |
| 206 | Double_t b22=f24*fC42; |
| 207 | Double_t b40=f02*fC42 + f04*fC44, b41=f12*fC42 + f14*fC44 + f13*fC43; |
| 208 | Double_t b42=f24*fC44; |
| 209 | Double_t b30=f02*fC32 + f04*fC43, b31=f12*fC32 + f14*fC43 + f13*fC33; |
| 210 | Double_t b32=f24*fC43; |
| 211 | |
| 212 | //a = f*b = f*C*ft |
| 213 | Double_t a00=f02*b20+f04*b40,a01=f02*b21+f04*b41,a02=f02*b22+f04*b42; |
| 214 | Double_t a11=f12*b21+f14*b41+f13*b31,a12=f12*b22+f14*b42+f13*b32; |
| 215 | Double_t a22=f24*b42; |
| 216 | |
| 217 | //F*C*Ft = C + (b + bt + a) |
| 218 | fC00 += b00 + b00 + a00; |
| 219 | fC10 += b10 + b01 + a01; |
| 220 | fC20 += b20 + b02 + a02; |
| 221 | fC30 += b30; |
| 222 | fC40 += b40; |
| 223 | fC11 += b11 + b11 + a11; |
| 224 | fC21 += b21 + b12 + a12; |
| 225 | fC31 += b31; |
| 226 | fC41 += b41; |
| 227 | fC22 += b22 + b22 + a22; |
| 228 | fC32 += b32; |
| 229 | fC42 += b42; |
| 230 | // |
| 231 | // update stored transformation matrix F = Fnew*Fold |
| 232 | fFMatrix[kF04] += f04 + f24*fFMatrix[kF02]; |
| 233 | fFMatrix[kF14] += f14 + f24*fFMatrix[kF12]; |
| 234 | fFMatrix[kF02] += f02; |
| 235 | fFMatrix[kF12] += f12; |
| 236 | fFMatrix[kF13] += f13; |
| 237 | fFMatrix[kF24] += f24; |
| 238 | // |
| 239 | CheckCovariance(); |
| 240 | |
| 241 | return kTRUE; |
| 242 | } |
| 943e1898 |
243 | |
| e7d83d38 |
244 | //__________________________________________________________________ |
| 245 | Int_t AliITSUSeed::GetClusterIndex(Int_t ind) const |
| 246 | { |
| 247 | // get ind-th cluster index |
| 248 | int ncl = 0; |
| 249 | const AliITSUSeed* seed = this; |
| 250 | while(seed) { |
| 0bc46b99 |
251 | if ( HasCluster() && (ncl++==ind) ) return seed->GetLrClusterID();//GetClusterID(); |
| e7d83d38 |
252 | seed = (AliITSUSeed*)seed->GetParent(); |
| 253 | } |
| 254 | return -1; |
| 255 | // |
| 256 | } |
| 257 | |
| 716ccba7 |
258 | //______________________________________________________________________________ |
| 259 | Bool_t AliITSUSeed::RotateToAlpha(Double_t alpha) |
| 260 | { |
| 261 | // Transform this track to the local coord. system rotated |
| 262 | // by angle "alpha" (rad) with respect to the global coord. system. |
| 263 | // |
| 264 | if (TMath::Abs(fP[2]) >= kAlmost1) { |
| 265 | AliError(Form("Precondition is not satisfied: |sin(phi)|>1 ! %f",fP[2])); |
| 266 | return kFALSE; |
| 267 | } |
| 268 | // |
| 269 | if (alpha < -TMath::Pi()) alpha += 2*TMath::Pi(); |
| 270 | else if (alpha >= TMath::Pi()) alpha -= 2*TMath::Pi(); |
| 271 | // |
| 272 | Double_t &fP0=fP[0]; |
| 273 | Double_t &fP2=fP[2]; |
| 274 | Double_t &fC00=fC[0]; |
| 275 | Double_t &fC10=fC[1]; |
| 276 | Double_t &fC20=fC[3]; |
| 277 | Double_t &fC21=fC[4]; |
| 278 | Double_t &fC22=fC[5]; |
| 279 | Double_t &fC30=fC[6]; |
| 280 | Double_t &fC32=fC[8]; |
| 281 | Double_t &fC40=fC[10]; |
| 282 | Double_t &fC42=fC[12]; |
| 283 | // |
| 284 | Double_t x=fX; |
| 285 | Double_t ca=TMath::Cos(alpha-fAlpha), sa=TMath::Sin(alpha-fAlpha); |
| 286 | Double_t sf=fP2, cf=TMath::Sqrt((1.- fP2)*(1.+fP2)); // Improve precision |
| 287 | // RS: check if rotation does no invalidate track model (cos(local_phi)>=0, i.e. particle |
| 288 | // direction in local frame is along the X axis |
| 289 | if ((cf*ca+sf*sa)<0) { |
| 290 | AliDebug(1,Form("Rotation failed: local cos(phi) would become %.2f",cf*ca+sf*sa)); |
| 291 | return kFALSE; |
| 292 | } |
| 293 | // |
| 294 | Double_t tmp=sf*ca - cf*sa; |
| 295 | |
| 296 | if (TMath::Abs(tmp) >= kAlmost1) { |
| 297 | if (TMath::Abs(tmp) > 1.+ Double_t(FLT_EPSILON)) |
| 298 | AliWarning(Form("Rotation failed ! %.10e",tmp)); |
| 299 | return kFALSE; |
| 300 | } |
| 301 | fAlpha = alpha; |
| 302 | fX = x*ca + fP0*sa; |
| 303 | fP0= -x*sa + fP0*ca; |
| 304 | fP2= tmp; |
| 305 | |
| 306 | if (TMath::Abs(cf)<kAlmost0) { |
| 307 | AliError(Form("Too small cosine value %f",cf)); |
| 308 | cf = kAlmost0; |
| 309 | } |
| 310 | |
| 311 | Double_t rr=(ca+sf/cf*sa); |
| 312 | |
| 313 | fC00 *= (ca*ca); |
| 314 | fC10 *= ca; |
| 315 | fC20 *= ca*rr; |
| 316 | fC21 *= rr; |
| 317 | fC22 *= rr*rr; |
| 318 | fC30 *= ca; |
| 319 | fC32 *= rr; |
| 320 | fC40 *= ca; |
| 321 | fC42 *= rr; |
| 322 | // |
| 323 | fRMatrix[kR00] = ca; |
| 324 | fRMatrix[kR22] = rr; |
| 325 | // |
| 326 | CheckCovariance(); |
| 327 | |
| 328 | return kTRUE; |
| 329 | } |
| 330 | |
| 943e1898 |
331 | //______________________________________________________________________________ |
| 332 | Bool_t AliITSUSeed::GetTrackingXAtXAlpha(double xOther, double alpOther, double bz, double &xdst) |
| 333 | { |
| 334 | // calculate X and Y in the tracking frame of the track, corresponding to other X,Alpha tracking |
| 335 | double ca=TMath::Cos(alpOther-fAlpha), sa=TMath::Sin(alpOther-fAlpha); |
| 336 | double &y=fP[0], &sf=fP[2], cf=Sqrt((1.-sf)*(1.+sf)); |
| 337 | double eta = xOther - fX*ca - y*sa; |
| 338 | double xi = sf*ca - cf*sa; |
| 716ccba7 |
339 | if (Abs(xi)>= kAlmost1) return kFALSE; |
| 943e1898 |
340 | double nu = xi + GetC(bz)*eta; |
| 716ccba7 |
341 | if (Abs(nu)>= kAlmost1) return kFALSE; |
| 943e1898 |
342 | xdst = xOther*ca - sa*( y*ca-fX*sa + eta*(xi+nu)/(Sqrt((1.-xi)*(1.+xi)) + Sqrt((1.-nu)*(1.+nu))) ); |
| 343 | return kTRUE; |
| 344 | } |
| 345 | |
| 346 | //____________________________________________________________________ |
| 347 | Double_t AliITSUSeed::GetPredictedChi2(Double_t p[2],Double_t cov[3]) |
| 348 | { |
| 349 | // Estimate the chi2 of the space point "p" with the cov. matrix "cov" |
| 350 | // Store info needed for update and smoothing |
| 351 | Double_t sdd = fC[0] + cov[0]; |
| 352 | Double_t sdz = fC[1] + cov[1]; |
| 353 | Double_t szz = fC[2] + cov[2]; |
| 354 | Double_t det = sdd*szz - sdz*sdz; |
| 355 | if (TMath::Abs(det) < kAlmost0) return kVeryBig; |
| 356 | det = 1./det; |
| 357 | fCovIYZ[0] = szz*det; |
| 358 | fCovIYZ[1] = -sdz*det; |
| 359 | fCovIYZ[2] = sdd*det; |
| 360 | double &dy = fResid[0] = p[0] - fP[0]; |
| 361 | double &dz = fResid[1] = p[1] - fP[1]; |
| 362 | // |
| 363 | return dy*(dy*fCovIYZ[0]+dz*fCovIYZ[1]) + dz*(dy*fCovIYZ[1]+dz*(fCovIYZ[2])); |
| 364 | // |
| 365 | } |
| 366 | |
| 367 | //____________________________________________________________________ |
| 368 | Bool_t AliITSUSeed::Update() |
| 369 | { |
| 370 | // Update the track parameters with the measurement stored during GetPredictedChi2 |
| 371 | // |
| 716ccba7 |
372 | Double_t &fP0=fP[0], &fP1=fP[1], &fP2=fP[2], &fP3=fP[3], &fP4=fP[4], |
| 373 | &fC00=fC[kS00], |
| 374 | &fC10=fC[kS10], &fC11=fC[kS11], |
| 375 | &fC20=fC[kS20], &fC21=fC[kS21], &fC22=fC[kS22], |
| 376 | &fC30=fC[kS30], &fC31=fC[kS31], &fC32=fC[kS32], &fC33=fC[kS33], |
| 377 | &fC40=fC[kS40], &fC41=fC[kS41], &fC42=fC[kS42], &fC43=fC[kS43], &fC44=fC[kS44]; |
| 943e1898 |
378 | // |
| 379 | double &r00=fCovIYZ[0],&r01=fCovIYZ[1],&r11=fCovIYZ[2]; |
| 380 | double &dy=fResid[0], &dz=fResid[1]; |
| 381 | // |
| 716ccba7 |
382 | // store info needed for smoothing in the fKMatrix |
| 383 | double &k00 = fKMatrix[kK00] = fC00*r00+fC10*r01; |
| 384 | double &k01 = fKMatrix[kK01] = fC00*r01+fC10*r11; |
| 385 | double &k10 = fKMatrix[kK10] = fC10*r00+fC11*r01; |
| 386 | double &k11 = fKMatrix[kK11] = fC10*r01+fC11*r11; |
| 387 | double &k20 = fKMatrix[kK20] = fC20*r00+fC21*r01; |
| 388 | double &k21 = fKMatrix[kK21] = fC20*r01+fC21*r11; |
| 389 | double &k30 = fKMatrix[kK30] = fC30*r00+fC31*r01; |
| 390 | double &k31 = fKMatrix[kK31] = fC30*r01+fC31*r11; |
| 391 | double &k40 = fKMatrix[kK40] = fC40*r00+fC41*r01; |
| 392 | double &k41 = fKMatrix[kK41] = fC40*r01+fC41*r11; |
| 943e1898 |
393 | // |
| 394 | Double_t sf=fP2 + k20*dy + k21*dz; |
| 395 | if (TMath::Abs(sf) > kAlmost1) return kFALSE; |
| 396 | |
| 397 | fP0 += k00*dy + k01*dz; |
| 398 | fP1 += k10*dy + k11*dz; |
| 399 | fP2 = sf; |
| 400 | fP3 += k30*dy + k31*dz; |
| 401 | fP4 += k40*dy + k41*dz; |
| 402 | // |
| 403 | Double_t c01=fC10, c02=fC20, c03=fC30, c04=fC40; |
| 404 | Double_t c12=fC21, c13=fC31, c14=fC41; |
| 405 | |
| 406 | fC00-=k00*fC00+k01*fC10; fC10-=k00*c01+k01*fC11; |
| 407 | fC20-=k00*c02+k01*c12; fC30-=k00*c03+k01*c13; |
| 408 | fC40-=k00*c04+k01*c14; |
| 409 | |
| 410 | fC11-=k10*c01+k11*fC11; |
| 411 | fC21-=k10*c02+k11*c12; fC31-=k10*c03+k11*c13; |
| 412 | fC41-=k10*c04+k11*c14; |
| 413 | |
| 414 | fC22-=k20*c02+k21*c12; fC32-=k20*c03+k21*c13; |
| 415 | fC42-=k20*c04+k21*c14; |
| 416 | |
| 417 | fC33-=k30*c03+k31*c13; |
| 418 | fC43-=k30*c04+k31*c14; |
| 419 | |
| 420 | fC44-=k40*c04+k41*c14; |
| 421 | // |
| 943e1898 |
422 | CheckCovariance(); |
| 716ccba7 |
423 | // |
| 424 | return kTRUE; |
| 425 | } |
| 426 | |
| 427 | |
| 428 | //____________________________________________________________________ |
| 429 | Bool_t AliITSUSeed::Smooth(Double_t vecL[5],Double_t matL[15]) |
| 430 | { |
| 431 | // Prepare MBF smoothing auxiliary params for smoothing at prev. point: |
| 432 | // \hat{l_N} = 0 |
| 433 | // \hat{L_N} = 0 |
| 434 | // \tilde{l_j} = -H_j^T N_{j}^{-1} z_j + B_{j}^T \hat{l_j} |
| 435 | // \tilde{L_j} = H_j^T N_{j}^{-1} H_j + B_j^T \hat{L_j} B_j |
| 436 | // \hat{l_j} = F_j^T \tilde{l_{j+1}} |
| 437 | // \hat{L_j} = F_j^T \tilde{L_{j+1}} F_j |
| 438 | // |
| 439 | // P_{j/N} = P_{j/j} - P_{j/j} \hat{L_j} P_{j/j} |
| 440 | // \hat{x_{j/N}} = \hat{x_{j/j}} - P_{j/j} \hat{l_j} |
| 441 | // |
| 442 | // N^-1 = fCovIYZ |
| 443 | // z = fResid |
| 444 | // B = I - K H |
| 445 | // H = {{1,0,0,0,0},{0,1,0,0,0}} |
| 446 | // |
| 447 | // calc. \tilde{l_j} |
| 448 | // |
| 449 | if (GetClusterID()<0) return kTRUE; |
| 450 | // |
| 451 | |
| 452 | double |
| 453 | &k00=fKMatrix[kK00],&k01=fKMatrix[kK01], |
| 454 | &k10=fKMatrix[kK10],&k11=fKMatrix[kK11], |
| 455 | &k20=fKMatrix[kK20],&k21=fKMatrix[kK21], |
| 456 | &k30=fKMatrix[kK30],&k31=fKMatrix[kK31], |
| 457 | &k40=fKMatrix[kK40],&k41=fKMatrix[kK41]; |
| 458 | double |
| 459 | &l00=matL[kS00], |
| 460 | &l10=matL[kS10], &l11=matL[kS11], |
| 461 | &l20=matL[kS20], &l21=matL[kS21], &l22=matL[kS22], |
| 462 | &l30=matL[kS30], &l31=matL[kS31], &l32=matL[kS32], &l33=matL[kS33], |
| 463 | &l40=matL[kS40], &l41=matL[kS41], &l42=matL[kS42], &l43=matL[kS43], &l44=matL[kS44]; |
| 464 | // |
| 465 | // calculate correction |
| 466 | double corrVec[5]={0},corrMat[15]={0}; |
| 467 | corrVec[0] = fC[kS00]*vecL[0] + fC[kS10]*vecL[1] + fC[kS20]*vecL[2] + fC[kS30]*vecL[3] + fC[kS40]*vecL[4]; |
| 468 | corrVec[1] = fC[kS10]*vecL[0] + fC[kS11]*vecL[1] + fC[kS21]*vecL[2] + fC[kS31]*vecL[3] + fC[kS41]*vecL[4]; |
| 469 | corrVec[2] = fC[kS20]*vecL[0] + fC[kS21]*vecL[1] + fC[kS22]*vecL[2] + fC[kS32]*vecL[3] + fC[kS42]*vecL[4]; |
| 470 | corrVec[3] = fC[kS30]*vecL[0] + fC[kS31]*vecL[1] + fC[kS32]*vecL[2] + fC[kS33]*vecL[3] + fC[kS43]*vecL[4]; |
| 471 | corrVec[4] = fC[kS40]*vecL[0] + fC[kS41]*vecL[1] + fC[kS42]*vecL[2] + fC[kS43]*vecL[3] + fC[kS44]*vecL[4]; |
| 472 | // |
| 473 | double *crm = ProdABA(fC,matL); |
| 474 | for (int i=0;i<15;i++) corrMat[i] = crm[i]; |
| 475 | |
| 476 | double vcL0 = vecL[0], vcL1 = vecL[1]; |
| 477 | vecL[0] -= k00*vcL0+k10*vcL1+k20*vecL[2]+k30*vecL[3]+k40*vecL[4] + fCovIYZ[0]*fResid[0] + fCovIYZ[1]*fResid[1]; |
| 478 | vecL[1] -= k01*vcL0+k11*vcL1+k21*vecL[2]+k31*vecL[3]+k41*vecL[4] + fCovIYZ[1]*fResid[0] + fCovIYZ[2]*fResid[1]; |
| 479 | |
| 480 | /* |
| 481 | double vcL0 = vecL[0], vcL1 = vecL[1]; |
| 482 | vecL[0] -= k00*vcL0+k10*vcL1+fKMatrix[kK20]*vecL[2]+k30*vecL[3]+k40*vecL[4] + fCovIYZ[0]*fResid[0] + fCovIYZ[1]*fResid[1]; |
| 483 | vecL[1] -= k01*vcL0+fKMatrix[kK11]*vcL1+k21*vecL[2]+k31*vecL[3]+k41*vecL[4] + fCovIYZ[1]*fResid[0] + fCovIYZ[2]*fResid[1]; |
| 484 | vecL[3] += fFMatrix[kF13]*vecL[1]; |
| 485 | vecL[4] = fFMatrix[kF04]*vecL[0] + fFMatrix[kF14]*vecL[1] + fFMatrix[kF24]*vecL[2] + fFMatrix[kF44]*vecL[4]; |
| 486 | vecL[2] += fFMatrix[kF02]*vecL[0] + fFMatrix[kF12]*vecL[1]; |
| 487 | // |
| 488 | */ |
| 489 | // and \hat{l_j} in one go |
| 490 | |
| 491 | // L = H^T * sg * H + (I-KH)^T * L * (I - KH) |
| 492 | double v00 = k00*l00+k10*l10+k20*l20+k30*l30+k40*l40; |
| 493 | double v10 = k00*l10+k10*l11+k20*l21+k30*l31+k40*l41; |
| 494 | double v20 = k00*l20+k10*l21+k20*l22+k30*l32+k40*l42; |
| 495 | double v30 = k00*l30+k10*l31+k20*l32+k30*l33+k40*l43; |
| 496 | double v40 = k00*l40+k10*l41+k20*l42+k30*l43+k40*l44; |
| 497 | // |
| 498 | double v01 = k01*l00+k11*l10+k21*l20+k31*l30+k41*l40; |
| 499 | double v11 = k01*l10+k11*l11+k21*l21+k31*l31+k41*l41; |
| 500 | double v21 = k01*l20+k11*l21+k21*l22+k31*l32+k41*l42; |
| 501 | double v31 = k01*l30+k11*l31+k21*l32+k31*l33+k41*l43; |
| 502 | double v41 = k01*l40+k11*l41+k21*l42+k31*l43+k41*l44; |
| 503 | // |
| 504 | // (H^T * K^T * L * K * H) - (L * K * H) - (H^T * K^T * L) + (H^T*N^-1*H) |
| 505 | l00 += k00*v00 + k10*v10 + k20*v20 + k30*v30 + k40*v40 - v00 - v00 + fCovIYZ[0]; |
| 506 | l10 += k01*v00 + k11*v10 + k21*v20 + k31*v30 + k41*v40 - v01 - v10 + fCovIYZ[1]; |
| 507 | l11 += k01*v01 + k11*v11 + k21*v21 + k31*v31 + k41*v41 - v11 - v11 + fCovIYZ[2]; |
| 508 | // |
| 509 | l20 -= v20; |
| 510 | l21 -= v21; |
| 511 | l30 -= v30; |
| 512 | l31 -= v31; |
| 513 | l40 -= v40; |
| 514 | l41 -= v41; |
| 515 | // |
| 516 | printf("CorrMt:\n"); |
| 517 | printf("%+e\n%+e %+e\n%+e %+e %+e\n%+e %+e %+e %+e\n%+e %+e %+e %+e %+e\n", |
| 518 | corrMat[kS00],corrMat[kS10],corrMat[kS11],corrMat[kS20],corrMat[kS21],corrMat[kS22], |
| 519 | corrMat[kS30],corrMat[kS31],corrMat[kS32],corrMat[kS33], |
| 520 | corrMat[kS40],corrMat[kS41],corrMat[kS42],corrMat[kS43],corrMat[kS44]); |
| 521 | |
| 522 | printf("SMcorr: %+e %+e %+e %+e %+e\n",corrVec[0],corrVec[1],corrVec[2],corrVec[3],corrVec[4]); |
| 523 | |
| 524 | printf("State : "); this->AliExternalTrackParam::Print(""); |
| 525 | // |
| 526 | printf("\nBefore transport back (RotElems: %+e %+e)\n",fRMatrix[kR00],fRMatrix[kR22]); |
| 527 | printf("Res: %+e %+e | Err: %+e %+e %+e\n",fResid[0],fResid[1],fCovIYZ[0],fCovIYZ[1],fCovIYZ[2]); |
| 528 | printf("Lr%d VecL: ",GetLayerID()); for (int i=0;i<5;i++) printf("%+e ",vecL[i]); printf("\n"); |
| 529 | // |
| 530 | printf("%+e\n%+e %+e\n%+e %+e %+e\n%+e %+e %+e %+e\n%+e %+e %+e %+e %+e\n", |
| 531 | matL[kS00],matL[kS10],matL[kS11],matL[kS20],matL[kS21],matL[kS22], |
| 532 | matL[kS30],matL[kS31],matL[kS32],matL[kS33],matL[kS40],matL[kS41],matL[kS42],matL[kS43],matL[kS44]); |
| 533 | // |
| 534 | printf("F: "); for (int i=0;i<kNFElem;i++) printf("%+e ",fFMatrix[i]); printf("\n"); |
| 535 | printf("K: "); for (int i=0;i<kNKElem;i++) printf("%+e ",fKMatrix[i]); printf("\n"); |
| 536 | // |
| 537 | // apply rotation matrix (diagonal) |
| 538 | vecL[0] *= fRMatrix[kR00]; |
| 539 | vecL[2] *= fRMatrix[kR22]; |
| 540 | // |
| 541 | l00 *= fRMatrix[kR00]*fRMatrix[kR00]; |
| 542 | l10 *= fRMatrix[kR00]; |
| 543 | l20 *= fRMatrix[kR22]*fRMatrix[kR00]; |
| 544 | l21 *= fRMatrix[kR22]; |
| 545 | l22 *= fRMatrix[kR22]*fRMatrix[kR22]; |
| 546 | l30 *= fRMatrix[kR00]; |
| 547 | l32 *= fRMatrix[kR22]; |
| 548 | l40 *= fRMatrix[kR00]; |
| 549 | l42 *= fRMatrix[kR22]; |
| 550 | // |
| 551 | // Apply translation matrix F^T. Note, that fFMatrix keeps non-trivial elems of F-1 = f, except the e-loss coeff f44 |
| 552 | // We need F^T*L* F = L + (L*f) + (L*f)^T + f^T * (L*f) |
| 553 | // |
| 554 | double |
| 555 | &f02=fFMatrix[kF02],&f04=fFMatrix[kF04], |
| 556 | &f12=fFMatrix[kF12],&f13=fFMatrix[kF13],&f14=fFMatrix[kF14], |
| 557 | &f24=fFMatrix[kF24], |
| 558 | f44 =fFMatrix[kF44]; |
| 559 | // |
| 560 | vecL[4] = f04*vecL[0]+f14*vecL[1]+f24*vecL[2]+f44*vecL[4]; |
| 561 | vecL[3] += f13*vecL[1]; |
| 562 | vecL[2] += f02*vecL[0]+f12*vecL[1]; |
| 563 | // |
| 564 | f44 -= 1.0; // !!!!! |
| 565 | // |
| 566 | //b = L*f |
| 567 | Double_t b02=l00*f02+l10*f12, b03=l10*f13, b04=l00*f04+l10*f14+l20*f24+l40*f44; |
| 568 | Double_t b12=l10*f02+l11*f12, b13=l11*f13, b14=l10*f04+l11*f14+l21*f24+l41*f44; |
| 569 | Double_t b22=l20*f02+l21*f12, b23=l21*f13, b24=l20*f04+l21*f14+l22*f24+l42*f44; |
| 570 | Double_t b32=l30*f02+l31*f12, b33=l31*f13, b34=l30*f04+l31*f14+l32*f24+l43*f44; |
| 571 | Double_t b42=l40*f02+l41*f12, b43=l41*f13, b44=l40*f04+l41*f14+l42*f24+l44*f44; |
| 572 | // |
| 573 | //a = f^T * b = f^T * L * f, profit from symmetry |
| 574 | Double_t a22=f02*b02+f12*b12, a33=f13*b13, a44=f04*b04+f14*b14+f24*b24+f44*b44, |
| 575 | a32=f13*b12, //= a23=f02*b03+f12*b13, |
| 576 | a42=f02*b04+f12*b14, //f04*b02+f14*b12+f24*b22+f44*b42 = a24 |
| 577 | a43=f13*b14; //f04*b03+f14*b13+f24*b23+f44*b43 = a34 |
| 578 | // |
| 579 | // F^T*L* F = L + (b + b^T + a) |
| 580 | l44 += b44 + b44 + a44; |
| 581 | l43 += b43 + b34 + a43; |
| 582 | l42 += b42 + b24 + a42; |
| 583 | l41 += b14; |
| 584 | l40 += b04; |
| 585 | l33 += b33 + b33 + a33; |
| 586 | l32 += b32 + b23 + a32; |
| 587 | l31 += b13; |
| 588 | l30 += b03; |
| 589 | l22 += b22 + b23 + a22; |
| 590 | l21 += b12; |
| 591 | l20 += b02; |
| 592 | // |
| 593 | printf("After transport back\n"); |
| 594 | printf("Lr%d VecL: ",GetLayerID()); for (int i=0;i<5;i++) printf("%+e ",vecL[i]); printf("\n"); |
| 595 | // |
| 596 | printf("%+e\n%+e %+e\n%+e %+e %+e\n%+e %+e %+e %+e\n%+e %+e %+e %+e %+e\n", |
| 597 | matL[kS00],matL[kS10],matL[kS11],matL[kS20],matL[kS21],matL[kS22], |
| 598 | matL[kS30],matL[kS31],matL[kS32],matL[kS33],matL[kS40],matL[kS41],matL[kS42],matL[kS43],matL[kS44]); |
| 599 | |
| 600 | return kTRUE; |
| 601 | } |
| 602 | |
| 603 | //____________________________________________________________________ |
| 604 | Double_t* AliITSUSeed::ProdABA(const double a[15],const double b[15]) const |
| 605 | { |
| 606 | // product of symmetric matrices A*B*A |
| 607 | // |
| 608 | const Short_t knd[5][5] = { |
| 609 | {kS00,kS10,kS20,kS30,kS40}, |
| 610 | {kS10,kS11,kS21,kS31,kS41}, |
| 611 | {kS20,kS21,kS22,kS32,kS42}, |
| 612 | {kS30,kS31,kS32,kS33,kS43}, |
| 613 | {kS40,kS41,kS42,kS43,kS44} |
| 614 | }; |
| 615 | // |
| 616 | static double aba[15]; |
| 617 | // 1) ba = B*A |
| 618 | double ba[5][5]; |
| 619 | for (int i=5;i--;) for (int j=5;j--;) { |
| 620 | ba[i][j] = 0; |
| 621 | for (int k=5;k--;) ba[i][j] += b[knd[i][k]]*a[knd[k][j]]; |
| 622 | } |
| 623 | // |
| 624 | // 2) A * ba, lower triangle only |
| 625 | for (int i=5;i--;) for (int j=i+1;j--;) { |
| 626 | aba[knd[i][j]] = 0; |
| 627 | for (int k=5;k--;) aba[knd[i][j]] += a[knd[i][k]]*ba[k][j]; |
| 628 | } |
| 629 | // |
| 630 | return &aba[0]; |
| 631 | } |
| 943e1898 |
632 | |
| 716ccba7 |
633 | /* |
| 634 | //____________________________________________________________________ |
| 635 | Bool_t AliITSUSeed::Smooth(Double_t vecL[5],Double_t matL[15]) |
| 636 | { |
| 637 | // Prepare MBF smoothing auxiliary params for smoothing at prev. point: |
| 638 | // \hat{l_N} = 0 |
| 639 | // \hat{L_N} = 0 |
| 640 | // \tilde{l_j} = -H_j^T N_{j}^{-1} z_j + B_{j}^T \hat{l_j} |
| 641 | // \tilde{L_j} = H_j^T N_{j}^{-1} H_j + B_j^T \hat{L_j} B_j |
| 642 | // \hat{l_j} = F_j^T \tilde{l_{j+1}} |
| 643 | // \hat{L_j} = F_j^T \tilde{L_{j+1}} F_j |
| 644 | // |
| 645 | // P_{j/N} = P_{j/j} - P_{j/j} \hat{L_j} P_{j/j} |
| 646 | // \hat{x_{j/N}} = \hat{x_{j/j}} - P_{j/j} \hat{l_j} |
| 647 | // |
| 648 | // N^-1 = fCovIYZ |
| 649 | // z = fResid |
| 650 | // B = I - K H |
| 651 | // H = {{1,0,0,0,0},{0,1,0,0,0}} |
| 652 | // |
| 653 | // calc. \tilde{l_j} and \hat{l_j} in one go |
| 654 | // |
| 655 | if (GetClusterID()<0) return kTRUE; |
| 656 | // |
| 657 | double |
| 658 | &k00=fKMatrix[kK00],&k01=fKMatrix[kK01], |
| 659 | &k10=fKMatrix[kK10],&k11=fKMatrix[kK11], |
| 660 | &k20=fKMatrix[kK20],&k21=fKMatrix[kK21], |
| 661 | &k30=fKMatrix[kK30],&k31=fKMatrix[kK31], |
| 662 | &k40=fKMatrix[kK40],&k41=fKMatrix[kK41]; |
| 663 | double |
| 664 | &matL00=matL[kS00], |
| 665 | &matL10=matL[kS01], &matL11=matL[kS11], |
| 666 | &matL20=matL[kS20], &matL21=matL[kS21], &matL22=matL[kS22], |
| 667 | &matL30=matL[kS30], &matL31=matL[kS31], &matL32=matL[kS32], &matL33=matL[kS33], |
| 668 | &matL40=matL[kS40], &matL41=matL[kS41], &matL42=matL[kS42], &matL43=matL[kS43], &matL44=matL[kS44]; |
| 669 | // |
| 670 | double vcL0 = vecL[0], vcL1 = vecL[1]; |
| 671 | vecL[0] -= k00*vcL0+k10*vcL1+fKMatrix[kK20]*vecL[2]+k30*vecL[3]+k40*vecL[4] + fCovIYZ[0]*fResid[0] + fCovIYZ[1]*fResid[1]; |
| 672 | vecL[1] -= k01*vcL0+fKMatrix[kK11]*vcL1+k21*vecL[2]+k31*vecL[3]+k41*vecL[4] + fCovIYZ[1]*fResid[0] + fCovIYZ[2]*fResid[1]; |
| 673 | vecL[3] += fFMatrix[kF13]*vecL[1]; |
| 674 | vecL[4] = fFMatrix[kF04]*vecL[0] + fFMatrix[kF14]*vecL[1] + fFMatrix[kF24]*vecL[2] + fFMatrix[kF44]*vecL[4]; |
| 675 | vecL[2] += fFMatrix[kF02]*vecL[0] + fFMatrix[kF12]*vecL[1]; |
| 676 | // |
| 677 | |
| 678 | // L = H^T * sg * H + (I-KH)^T * L * (I - KH) |
| 679 | double v00 = k00*matL00+k10*matL10+k20*matL20+k30*matL30+k40*matL40; |
| 680 | double v10 = k00*matL10+k10*matL11+k20*matL21+k30*matL31+k40*matL41; |
| 681 | double v20 = k00*matL20+k10*matL12+k20*matL22+k30*matL32+k40*matL42; |
| 682 | double v30 = k00*matL30+k10*matL13+k20*matL23+k30*matL33+k40*matL43; |
| 683 | double v40 = k00*matL40+k10*matL14+k20*matL24+k30*matL34+k40*matL44; |
| 684 | // |
| 685 | double v01 = k01*matL00+k11*matL10+k21*matL20+k31*matL30+k41*matL40; |
| 686 | double v11 = k01*matL01+k11*matL11+k21*matL21+k31*matL31+k41*matL41; |
| 687 | double v21 = k01*matL02+k11*matL12+k21*matL22+k31*matL32+k41*matL42; |
| 688 | double v31 = k01*matL03+k11*matL13+k21*matL23+k31*matL33+k41*matL43; |
| 689 | double v41 = k01*matL04+k11*matL14+k21*matL24+k31*matL34+k41*matL44; |
| 690 | // |
| 691 | double t00 = k00*matL00+k10*matL01+k20*matL02+k30*matL03+k40*matL04; |
| 692 | double t10 = k00*matL10+k10*matL11+k20*matL12+k30*matL13+k40*matL14; |
| 693 | double t20 = k00*matL20+k10*matL21+k20*matL22+k30*matL23+k40*matL24; |
| 694 | double t30 = k00*matL30+k10*matL31+k20*matL32+k30*matL33+k40*matL34; |
| 695 | double t40 = k00*matL40+k10*matL41+k20*matL42+k30*matL43+k40*matL44; |
| 696 | // |
| 697 | double t01 = k01*matL00+k11*matL01+k21*matL02+k31*matL03+k41*matL04; |
| 698 | double t11 = k01*matL10+k11*matL11+k21*matL12+k31*matL13+k41*matL14; |
| 699 | double t21 = k01*matL20+k11*matL21+k21*matL22+k31*matL23+k41*matL24; |
| 700 | double t31 = k01*matL30+k11*matL31+k21*matL32+k31*matL33+k41*matL34; |
| 701 | double t41 = k01*matL40+k11*matL41+k21*matL42+k31*matL43+k41*matL44; |
| 702 | // |
| 703 | // (H^T * K^T * L * K * H) - (L * K * H) - (H^T * K^T * L) + (H^T*N^-1*H) |
| 704 | matL00 += k00*v00+k10*v10+k20*v20*k30*v30+k40*v40 - t00 - v00 + fCovIYZ[0]; |
| 705 | matL01 += k01*v00+k11*v10+k21*v20*k31*v30+k41*v40 - t01 - v10 + fCovIYZ[1]; |
| 706 | matL10 += k00*v01+k10*v11+k20*v21*k30*v31+k40*v41 - t10 - v01 + fCovIYZ[1]; |
| 707 | matL11 += k01*v01+k11*v11+k21*v21*k31*v31+k41*v41 - t11 - v11 + fCovIYZ[2]; |
| 708 | // |
| 709 | matL20 -= t20; |
| 710 | matL21 -= t21; |
| 711 | matL30 -= t30; |
| 712 | matL31 -= t31; |
| 713 | matL40 -= t40; |
| 714 | matL41 -= t41; |
| 715 | // |
| 716 | matL02 -= v20; |
| 717 | matL03 -= v30; |
| 718 | matL04 -= v40; |
| 719 | matL12 -= v21; |
| 720 | matL13 -= v31; |
| 721 | matL14 -= v41; |
| 722 | // |
| 723 | printf("Lr%d VecL: ",GetLayerID()); for (int i=0;i<5;i++) printf("%+e ",vecL[i]); printf("\n"); |
| 724 | printf("F: "); for (int i=0;i<kNFElem;i++) printf("%+e ",fFMatrix[i]); printf("\n"); |
| 725 | printf("K: "); for (int i=0;i<kNKElem;i++) printf("%+e ",fKMatrix[i]); printf("\n"); |
| 726 | // |
| 727 | for (int j=0;j<5;j++) { |
| 728 | for (int i=0;i<5;i++) printf("%+e ",matL[j][i]); printf("\n"); |
| 729 | } |
| 730 | // |
| 943e1898 |
731 | return kTRUE; |
| 732 | } |
| 716ccba7 |
733 | |
| 734 | */ |
| e7d83d38 |
735 | |
git://git.uio.no / u / mrichter / AliRoot.git / blame