HLT/kalman/AliL3KalmanTrack.cxx

   1 #include "TMath.h"
   2 #include "AliL3KalmanTrack.h"
   3 #include "AliL3SpacePointData.h"
   4 #include "AliL3Logging.h"
   5 #include "AliL3Transform.h"
   6 #include "AliL3StandardIncludes.h"
   7
   8 // includes for offline comparison, will be removed
   9 #include "AliTPCtrack.h"
  10 // includes for offline comparison, will be removed
  11
  12 #include "Riostream.h"
  13
  14
  15 ClassImp(AliL3KalmanTrack)
  16
  17 // Class for kalman tracks
  18
  19 AliL3KalmanTrack::AliL3KalmanTrack()
  20 {
  21   fX = 0;
  22
  23   fMaxChi2 = 1000;
  24   // Constructor
  25 }
  26
  27 AliL3KalmanTrack::~AliL3KalmanTrack()
  28 {
  29   // Destructor
  30 }
  31
  32 Int_t AliL3KalmanTrack::MakeSeed(AliL3Track *track, AliL3SpacePointData *points0, UInt_t pos0, Int_t slice0, AliL3SpacePointData *points1, UInt_t pos1, Int_t slice1, AliL3SpacePointData *points2, UInt_t pos2, Int_t slice2)
  33 {
  34   Float_t xyz[3];
  35   xyz[0] = points0[pos0].fX;
  36   xyz[1] = points0[pos0].fY;
  37   AliL3Transform::Global2LocHLT(xyz,slice0);
  38   fX = xyz[0];
  39   fP0 = xyz[1];
  40   fP1 = points0[pos0].fZ;
  41
  42   xyz[0] = points1[pos1].fX;
  43   xyz[1] = points1[pos1].fY;
  44   AliL3Transform::Global2LocHLT(xyz,slice1);
  45   Float_t x2 = xyz[0];
  46   Float_t y2 = xyz[1];
  47   Float_t z2 = points1[pos1].fZ;
  48
  49   xyz[0] = points2[pos2].fX;
  50   xyz[1] = points2[pos2].fY;
  51   AliL3Transform::Global2LocHLT(xyz,slice2);
  52   Float_t x3 = 0;//xyz[0];
  53   Float_t y3 = 0;//xyz[1];
  54   Float_t z3 = 0;//points2[pos2].fZ;
  55
  56
  57   fP2 = f2(fX,fP0,x2,y2,x3,y3);
  58   fP3 = f3(fX,fP0,x2,y2,fP1,z2);
  59   if (TMath::Abs(fP3) > 1.2) return 0;
  60   fP4 = f4(fX,fP0,x2,y2,x3,y3);
  61   if (TMath::Abs(fP4) >= 0.0066) return 0;
  62
  63   Float_t sy1=points0[pos0].fSigmaY2, sz1=points0[pos0].fSigmaZ2;
  64   Float_t sy2=points2[pos2].fSigmaY2, sz2=points2[pos2].fSigmaZ2;
  65   //Double_t sy3=400*3./12., sy=0.1, sz=0.1;
  66   Float_t sy3=25000*fP4*fP4+0.1, sy=0.1, sz=0.1;
  67
  68   Float_t f40=(f4(fX,fP0+sy,x2,y2,x3,y3)-fP4)/sy;
  69   Float_t f42=(f4(fX,fP0,x2,y2+sy,x3,y3)-fP4)/sy;
  70   Float_t f43=(f4(fX,fP0,x2,y2,x3,y3+sy)-fP4)/sy;
  71   Float_t f20=(f2(fX,fP0+sy,x2,y2,x3,y3)-fP2)/sy;
  72   Float_t f22=(f2(fX,fP0,x2,y2+sy,x3,y3)-fP2)/sy;
  73   Float_t f23=(f2(fX,fP0,x2,y2,x3,y3+sy)-fP2)/sy;
  74   Float_t f30=(f3(fX,fP0+sy,x2,y2,fP1,z2)-fP3)/sy;
  75   Float_t f31=(f3(fX,fP0,x2,y2,fP1+sz,z2)-fP3)/sz;
  76   Float_t f32=(f3(fX,fP0,x2,y2+sy,fP1,z2)-fP3)/sy;
  77   Float_t f34=(f3(fX,fP0,x2,y2,fP1,z2+sz)-fP3)/sz;
  78
  79   fC00=sy1;
  80   fC10=0.;       fC11=sz1;
  81   fC20=f20*sy1;  fC21=0.;       fC22=f20*sy1*f20+f22*sy2*f22+f23*sy3*f23;
  82   fC30=f30*sy1;  fC31=f31*sz1;  fC32=f30*sy1*f20+f32*sy2*f22;
  83   fC33=f30*sy1*f30+f31*sz1*f31+f32*sy2*f32+f34*sz2*f34;
  84   fC40=f40*sy1; fC41=0.; fC42=f40*sy1*f20+f42*sy2*f22+f43*sy3*f23;
  85   fC43=f30*sy1*f40+f32*sy2*f42;
  86   fC44=f40*sy1*f40+f42*sy2*f42+f43*sy3*f43;
  87
  88   return 1;
  89 }
  90
  91 Int_t AliL3KalmanTrack::Init(AliL3Track *track, AliL3SpacePointData *points, UInt_t pos, Int_t slice)
  92 {
  93
  94   // Can also find seed for x4 by track->CalculateHelix() and fP4 = track->GetKappa()
  95   // See if it's any difference in the time it takes. ??
  96   //track->CalculateHelix();
  97
  98   Float_t xyz[3];
  99   xyz[0] = points[pos].fX;
 100   xyz[1] = points[pos].fY;
 101   AliL3Transform::Global2LocHLT(xyz,slice);
 102
 103   fX = xyz[0];
 104
 105   fP0 = xyz[1];
 106   fP1 = points[pos].fZ;
 107   fP3 = track->GetTgl(); //cout << fP3;
 108   if (TMath::Abs(fP3) > 1.2) return 0; //From AliTPCtrackerMI
 109
 110   // The expression for fP4 is from ALICE internal note: ALICE/97-24, June 27, 1997
 111   // Is this consistent with what's used in AliTPCtrack and AliTPCtracker ??
 112   // Except for the factor 0.5, it is somewhat consistent with the calculation of Pt in AliTPCtrack.
 113   // When I plot fP4, it is more consistent with offline if the factor 1/2 is included.
 114   //fP4 = (-track->GetCharge()*1./(track->GetPt()/(0.0029980*AliL3Transform::GetBField())));
 115   fP4 = 0.5*(-track->GetCharge()*1./(track->GetPt()/(0.0029980*AliL3Transform::GetBField())));
 116   //cout << fP4 << endl;
 117   //fP4 = 0.5*track->GetKappa();
 118   if (TMath::Abs(fP4) >= 0.0066) return 0; // From AliTPCtrackerMI
 119
 120   /*Float_t firstXY[2];
 121   firstXY[0] = track->GetFirstPointX();
 122   firstXY[1] = track->GetFirstPointY();
 123   AliL3Transform::Global2LocHLT(firstXY,slice);
 124   */
 125   //Float_t centerX = track->GetFirstPointX() - ((track->GetPt()/(0.0029980*AliL3Transform::GetBField())) * TMath::Cos(track->GetPsi() + track->GetCharge() * 0.5 * 3.14159265358979323846));
 126   //Float_t centerX = firstXY[0] - ((track->GetPt()/(0.0029980*AliL3Transform::GetBField())) * TMath::Cos(track->GetPsi() + track->GetCharge() * 0.5 * 3.14159265358979323846));
 127
 128   //fP2 = fP4*centerX; // Curvature times center of curvature
 129   // track->GetPointPsi() is almost always zero, why??. Still it's good for seed.
 130   fP2 = TMath::Sin(track->GetPointPsi());
 131
 132   //cout << track->GetPt() << endl;
 133   if (TMath::Abs(fP4*fX - fP2) >= 0.9) // What's this??
 134     {
 135       return 0;
 136     }
 137
 138   //cout << "AliL3KalmanTrack::Init, " << fP0 << " " << fP1 << " " << fP2 << " " << fP3 << " " << fP4 << endl;
 139   //Float_t num = 12;
 140
 141   fC00 = points[pos].fSigmaY2;
 142   fC10 = 0; fC11 = points[pos].fSigmaZ2;
 143   fC20 = 0; fC21 = 0; fC22 = 5e-05;
 144   fC30 = 0; fC31 = 0; fC32 = 0; fC33 = 5e-05;
 145   fC40 = 0; fC41 = 0; fC42 = 0; fC43 = 0; fC44 = 5e-09;
 146   /*Float_t num = 12;
 147   fC00 = points[pos].fSigmaY2;
 148   fC10 = 0; fC11 = points[pos].fSigmaZ2;
 149   fC20 = 0.0005; fC21 = 0; fC22 = 5.5e-05;
 150   fC30 = 0.0001; fC31 = 0.001;
 151   fC32 = 5.5e-06; fC33 = 6.5e-05;
 152   fC40 = 5e-06;
 153   fC41 = 0; fC42 = 4e-07; fC43 = 4e-08; fC44 = 3e-09;*/
 154   //cout << "Init: errors " << fC00 << " " << fC11 << " " << fC22 << " " << fC33 << " " << fC44 << endl;
 155
 156   fChisq = 0;
 157
 158   return 1;
 159 }
 160
 161 Int_t AliL3KalmanTrack::Propagate(AliL3SpacePointData *points, UInt_t pos, Int_t slice)
 162 {
 163   // Propagates track to the plane of the next found cluster
 164   Float_t Xold = fX; // X position for previous space point
 165   //Float_t Xnew = points[pos].fX; // X position of current space point
 166   //Float_t dx = Xnew - Xold; cout << Xnew << endl;
 167   Float_t Yold = fP0; // Y position of old point
 168   Float_t Zold = fP1; // Z position of old point
 169   Float_t par2old = fP2;
 170   Float_t par3old = fP3;
 171   Float_t par4old = fP4;
 172   Float_t oldfC00 = fC00;
 173   Float_t oldfC10 = fC10;
 174   Float_t oldfC11 = fC11;
 175   Float_t oldfC20 = fC20;
 176   Float_t oldfC21 = fC21;
 177   Float_t oldfC22 = fC22;
 178   Float_t oldfC30 = fC30;
 179   Float_t oldfC31 = fC31;
 180   Float_t oldfC32 = fC32;
 181   Float_t oldfC33 = fC33;
 182   Float_t oldfC40 = fC40;
 183   Float_t oldfC41 = fC41;
 184   Float_t oldfC42 = fC42;
 185   Float_t oldfC43 = fC43;
 186   Float_t oldfC44 = fC44;
 187
 188   Float_t xyz[3];
 189
 190   xyz[0] = points[pos].fX;
 191   xyz[1] = points[pos].fY;
 192
 193   //AliL3Transform::Global2Local(xyz,slice);
 194   AliL3Transform::Global2LocHLT(xyz,slice);
 195
 196   Float_t Xnew = xyz[0];
 197   Float_t dx = Xnew - Xold; //cout << Xnew << endl;
 198
 199   if (TMath::Abs(fP4*Xnew - fP2) >= 0.9) // What's this??
 200     {
 201       //cout << "Propagation failed! Stiff track. " << pos << endl;
 202       return 0;
 203     }
 204
 205   // R must be approx. of the radius of the circle based on
 206   // the old and new spacepoint. What then is Cold and Cnew??
 207   // C has something to do with curvature at least (curvature times x).
 208
 209   Float_t Cold = fP4*Xold - fP2;
 210   //if (TMath::Abs(Cold) >= 1.0) Cold = 0.9;
 211     /*{
 212     cout << "Cold " << endl << fP4*Xnew - fP2 << endl;
 213     return 0;
 214     }*/
 215   Float_t Rold = TMath::Sqrt(1 - Cold*Cold);
 216   Float_t Cnew = fP4*Xnew - fP2;
 217   //if (TMath::Abs(Cnew) >= 1.0) Cnew = 0.9;
 218     /*{
 219     cout << "Cnew " << endl;
 220     return 0;
 221     }*/
 222   Float_t Rnew = TMath::Sqrt(1 - Cnew*Cnew);
 223   //if (Rold < 0.9) cout << "Cold = " << Cold << " , Rold = " << Rold << " , Cnew = " << Cnew << " , Rnew = " << Rnew << endl;
 224
 225   // Prediction of the y- and z- coordinate in the next plane
 226   fP0 += dx*(Cold+Cnew)/(Rold+Rnew);
 227   fP1 += dx*(Cold+Cnew)/(Cold*Rnew + Cnew*Rold)*fP3;
 228   //cout << "Old point " << Yold << " " << Zold << endl;
 229   //cout << "Propagate " << fP0 << " " << fP1 << endl;
 230   //cout << "Measured  " << points[pos].fY << " " << points[pos].fZ << endl;
 231
 232   // f = F - 1 //What is this??
 233   // Must be the f-matrix for the prediction, as in eq 1 in ALICE Kalman paper
 234   Float_t RR = Rold + Rnew;
 235   Float_t CC = Cold + Cnew;
 236   Float_t XX = Xold + Xnew;
 237
 238   Float_t f02 = -dx*(2*RR + CC*(Cold/Rold + Cnew/Rnew))/(RR*RR);
 239   Float_t f04 = dx*(RR*XX + CC*(Cold*Xold/Rold + Cnew*Xnew/Rnew))/(RR*RR);
 240   Float_t CR = Cold*Rnew + Cnew*Rold;
 241
 242   Float_t f12 = -dx*fP3*(2*CR + CC*(Cnew*(Cold/Rold)-Rold + Cold*(Cnew/Rnew)-Rnew))/(CR*CR);
 243   Float_t f13 = dx*CC/CR;
 244   Float_t f14 = dx*fP3*(CR*XX-CC*(Rold*Xnew-Cnew*Cold*Xold/Rold + Rnew*Xold-Cold*Cnew*Xnew/Rnew))/(CR*CR);
 245
 246   // b = C*ft // This? MUST BE (f*C)t = Ct*ft??, that gives the expressions under at least.
 247   Float_t b00=f02*fC20 + f04*fC40;
 248   Float_t b01=f12*fC20 + f14*fC40 + f13*fC30;
 249   Float_t b10=f02*fC21 + f04*fC41;
 250   Float_t b11=f12*fC21 + f14*fC41 + f13*fC31;
 251   Float_t b20=f02*fC22 + f04*fC42;
 252   Float_t b21=f12*fC22 + f14*fC42 + f13*fC32;
 253   Float_t b30=f02*fC32 + f04*fC43;
 254   Float_t b31=f12*fC32 + f14*fC43 + f13*fC33;
 255   Float_t b40=f02*fC42 + f04*fC44;
 256   Float_t b41=f12*fC42 + f14*fC44 + f13*fC43;
 257
 258   //a = f*b = f*C*ft
 259   Float_t a00 = f02*b20 + f04*b40;
 260   Float_t a01 = f02*b21 + f04*b41;
 261   Float_t a11 = f12*b21 + f14*b41+f13*b31;
 262
 263   //F*C*Ft = C + (a + b + bt) , This is the covariance matrix, the samll t
 264   // means transform. Then F must be df/dx
 265   fC00 += a00 + 2*b00;
 266   fC10 += a01 + b01 + b10;
 267   fC20 += b20;
 268   fC30 += b30;
 269   fC40 += b40;
 270   fC11 += a11 + 2*b11;
 271   fC21 += b21;
 272   fC31 += b31;
 273   fC41 += b41;
 274
 275   // Multiple scattering (from AliTPCtrack::PropagateTo)
 276   // Should this be included??
 277   /*
 278   Float_t d = TMath::Sqrt((Xold-Xnew)*(Xold-Xnew)+(Yold-fP0)*(Yold-fP0)+(Zold-fP1)*(Zold-fP1));
 279   Float_t Pt = (1e-9*TMath::Abs(fP4)/fP4 + fP4 * (1000/0.299792458/4));
 280   if (TMath::Abs(Pt) > 100) {
 281     return 0;
 282   }
 283   if (TMath::Abs(Pt) < 0.01) return 0;
 284
 285   Float_t p2 = (1+fP3*fP3)/(Pt*Pt);
 286   Float_t beta2 = p2/(p2 +0.14*0.14);
 287   Float_t theta2 = 1.0259e-6*10*10/20/(beta2*p2)*d*0.9e-3;
 288
 289   Float_t ey=fP4*Xnew - fP2;
 290   Float_t ez=fP3;
 291   Float_t xz=fP4*ez;
 292   Float_t zz1=ez*ez+1;
 293   Float_t xy=fP2+ey;
 294
 295   fC22 += (2*ey*ez*ez*fP2+1-ey*ey+ez*ez+fP2*fP2*ez*ez)*theta2;
 296   fC32 += ez*zz1*xy*theta2;
 297   fC33 += zz1*zz1*theta2;
 298   fC42 += xz*ez*xy*theta2;
 299   fC43 += xz*zz1*theta2;
 300   fC44 += xz*xz*theta2;
 301   if (TMath::Abs(beta2) >= 1) cout << "here? " << beta2 << ", " << Pt << endl;
 302
 303   // Energy loss assuming track is pion
 304   Float_t dE = 0.153e-3/beta2*(log(5940*beta2/(1-beta2))-beta2)*d*0.9e-3;
 305   if (Xold < Xnew) dE = -dE;
 306   CC = fP4;
 307   fP4 *= (1 - TMath::Sqrt(p2+0.14*0.14)/p2*dE);
 308   fP2 += Xnew*(fP4-CC);
 309   */
 310   // Update the track parameters with the measured values of the new point
 311
 312   Int_t value = UpdateTrack(points, pos, slice);
 313
 314   if (value == 0)
 315     {
 316       fP0 = Yold;
 317       fP1 = Zold;
 318       fP2 = par2old;
 319       fP3 = par3old;
 320       fP4 = par4old;
 321       fC00 = oldfC00;
 322       fC10 = oldfC10;
 323       fC11 = oldfC11;
 324       fC20 = oldfC20;
 325       fC21 = oldfC21;
 326       fC22 = oldfC22;
 327       fC30 = oldfC30;
 328       fC31 = oldfC31;
 329       fC32 = oldfC32;
 330       fC33 = oldfC33;
 331       fC40 = oldfC40;
 332       fC41 = oldfC41;
 333       fC42 = oldfC42;
 334       fC43 = oldfC43;
 335       fC44 = oldfC44;
 336
 337       return value;
 338     }
 339
 340   else
 341     return value;
 342   //return UpdateTrack(points, pos, slice);
 343 }
 344
 345 Int_t AliL3KalmanTrack::UpdateTrack(AliL3SpacePointData *points, UInt_t pos, Int_t slice)
 346 {
 347   // Update the track parameters with the measured values
 348
 349   Float_t xyz[3];
 350
 351   xyz[0] = points[pos].fX;
 352   xyz[1] = points[pos].fY;
 353
 354   //AliL3Transform::Global2Local(xyz,slice);
 355   AliL3Transform::Global2LocHLT(xyz,slice);
 356
 357   //fX = points[pos].fX;
 358   fX = xyz[0];
 359
 360   // The errors from the measurement of the spacepoint
 361   Float_t sigmaY2 = points[pos].fSigmaY2;
 362   Float_t sigmaZ2 = points[pos].fSigmaZ2;
 363   Float_t sigmaYZ = 0;
 364
 365   // points[pos].fSigmaY2 and Z2 is needed in calculation of chisq.
 366   // There use realSigmaZ2(z2), because sigmaY2(Z2) are changed underneath
 367   // ANother possibility is to give realSigmaY2(Z2)(YZ) after += fC00(11)10 calculations.
 368   // After all it is these that are used in chisq calculation.
 369   // NB! In that case GetChisqIncrement must be changed so fC00(11)(10) are not added twice??.
 370   // Check if it's any change in timing.
 371   Float_t realSigmaY2 = sigmaY2;
 372   Float_t realSigmaZ2 = sigmaZ2;
 373
 374   sigmaY2 += fC00;
 375   sigmaZ2 += fC11;
 376   sigmaYZ += fC10;
 377
 378   Float_t det = sigmaY2*sigmaZ2 - sigmaYZ*sigmaYZ;
 379   Float_t tmp = sigmaY2;
 380   sigmaY2 = sigmaZ2/det;
 381   sigmaZ2 = tmp/det;
 382   sigmaYZ = -sigmaYZ/det;
 383
 384   // Kalman gain matrix
 385   Float_t k00 = fC00*sigmaY2 + fC10*sigmaYZ;
 386   Float_t k01 = fC00*sigmaYZ + fC10*sigmaZ2;
 387   Float_t k10 = fC10*sigmaY2 + fC11*sigmaYZ;
 388   Float_t k11 = fC10*sigmaYZ + fC11*sigmaZ2;
 389   Float_t k20 = fC20*sigmaY2 + fC21*sigmaYZ;
 390   Float_t k21 = fC20*sigmaYZ + fC21*sigmaZ2;
 391   Float_t k30 = fC30*sigmaY2 + fC31*sigmaYZ;
 392   Float_t k31 = fC30*sigmaYZ + fC31*sigmaZ2;
 393   Float_t k40 = fC40*sigmaY2 + fC41*sigmaYZ;
 394   Float_t k41 = fC40*sigmaYZ + fC41*sigmaZ2;
 395
 396   // Deviation between the predicted and measured values of y and z
 397   //Float_t dy = points[pos].fY-fP0; //cout << "dy = " << dy;
 398   Float_t dy = xyz[1] - fP0; //cout << dy << endl;;
 399   Float_t dz = points[pos].fZ-fP1; //cout << dz << endl;
 400   //cout << "Measured " << xyz[2] << " " << points[pos].fZ << endl;
 401
 402   // Update of fP2 and fP4
 403   Float_t cur = fP4 + k40*dy + k41*dz;
 404   Float_t eta = fP2 + k20*dy + k21*dz;
 405
 406   if (TMath::Abs(cur*fX - eta) >= 0.9)
 407     {
 408       //cout << "Update failed! Stiff track. " << pos << endl;
 409       return 0;
 410     }
 411
 412   // Filtered state vector
 413   fP0 += k00*dy + k01*dz;
 414   fP1 += k10*dy + k11*dz;
 415   fP2 = eta;
 416   fP3 += k30*dy + k31*dz; //cout << "update " << fP3 << endl;
 417   fP4 = cur;
 418   //cout << "AliL3KalmanTrack::Update, " << fP0 << " " << fP1 << " " << fP2 << " " << fP3 << " " << fP4 << endl;
 419   //cout << "Measured, " << points[pos].fY << " " << points[pos].fZ << endl;
 420
 421   Float_t c10 = fC10;
 422   Float_t c20 = fC20;
 423   Float_t c30 = fC30;
 424   Float_t c40 = fC40;
 425   Float_t c21 = fC21;
 426   Float_t c31 = fC31;
 427   Float_t c41 = fC41;
 428
 429   // Filtered covariance matrix
 430   // This is how it is in AliTPCtrack::Update
 431   fC00 -= k00*fC00 + k01*fC10;
 432   fC10 -= k00*c10 + k01*fC11;
 433   fC11 -= k10*c10 + k11*fC11;
 434   fC20 -= k00*c20 + k01*c21;
 435   fC21 -= k10*c20 + k11*c21;
 436   fC22 -= k20*c20 + k21*c21;
 437   fC30 -= k00*c30 + k01*c31;
 438   fC31 -= k10*c30 + k11*c31;
 439   fC32 -= k20*c30 + k21*c31;
 440   fC33 -= k30*c30 + k31*c31;
 441   fC40 -= k00*c40 + k01*c41;
 442   fC41 -= k10*c40 + k11*c41;
 443   fC42 -= k20*c40 + k21*c41;
 444   fC43 -= k40*c30 + k41*c31;
 445   fC44 -= k40*c40 + k41*c41;
 446
 447   // Alternative filtering
 448   // This is how (I think) it should be from the Kalman filter equations, and the C-matrix in Propagate ??
 449   /*Float_t c00 = fC00;
 450   Float_t c11 = fC11;
 451   fC00 -= k00*fC00 + k01*fC10;
 452   fC10 -= k10*c00 + k11*fC10;
 453   fC11 -= k10*c10 + k11*fC11;
 454   fC20 -= k20*c00 + k21*c10;
 455   fC21 -= k20*c10 + k21*c11;
 456   fC22 -= k20*c20 + k21*c21;
 457   fC30 -= k30*c00 + k31*c10;
 458   fC31 -= k30*c10 + k31*c11;
 459   fC32 -= k30*c20 + k31*c21;
 460   fC33 -= k30*c30 + k31*c31;
 461   fC40 -= k40*c00 + k41*c10;
 462   fC41 -= k40*c10 + k41*c11;
 463   fC42 -= k40*c20 + k41*c21;
 464   fC43 -= k40*c30 + k41*c31;
 465   fC44 -= k40*c40 + k41*c41;*/
 466
 467   /*  sigmaY2 = sigmaY2*det;
 468   sigmaZ2 = sigmaZ2*det;
 469   sigmaYZ = sigmaYZ*det;*/
 470   //cout << "AliL3KalmanTrack::Update, Chi2 = " << GetChisq() << endl;
 471   //cout << "AliKalmanTrack::Update, sigmaY2 = " << sigmaY2 << " sigmaZ2 = " << sigmaZ2 << " sigmaYZ = " << sigmaYZ << " dy = " << dy << " dz = " << dz << endl;
 472
 473   // Calculate increase of chisquare
 474   //fChisq = GetChisq() + GetChisqIncrement(xyz[1],sigmaY2,points[pos].fZ,sigmaZ2);
 475   fChisq = GetChisq() + GetChisqIncrement(xyz[1],realSigmaY2,points[pos].fZ,realSigmaZ2);
 476   //fChisq = GetChisq() + GetChisqIncrement(points[pos].fY,realSigmaY2,points[pos].fZ,realSigmaZ2);
 477  //cout << "fChisq = " << fChisq << endl;
 478 //(dy*sigmaY2*dy + 2*sigmaYZ*dy*dz + dz*sigmaZ2*dz) / (sigmaY2*sigmaZ2 - sigmaYZ*sigmaYZ);
 479   // Must at some point make an cut on chisq. Here?
 480   //  if (fChisq > fMaxChi2) return 0;
 481
 482   return 1;
 483 }
 484
 485 //Float_t AliL3KalmanTrack::GetChisqIncrement(AliL3SpacePointData *points, UInt_t pos)
 486 Float_t AliL3KalmanTrack::GetChisqIncrement(Float_t y, Float_t error_y, Float_t z, Float_t error_z)
 487 {
 488   // This function calculates a predicted chi2 increment.
 489   //-----------------------------------------------------------------
 490   //Float_t r00=points[pos].fSigmaY2, r01=0., r11=points[pos].fSigmaZ2;
 491   Float_t r00=error_y, r01=0., r11=error_z;
 492   r00+=fC00; r01+=fC10; r11+=fC11;
 493
 494   Double_t det=r00*r11 - r01*r01;
 495   /*if (TMath::Abs(det) < 1.e-10) {
 496     Int_t n=GetNumberOfClusters();
 497     if (n>4) cerr<<n<<" AliKalmanTrack warning: Singular matrix !\n";
 498     return 1e10;
 499     }*/
 500   Double_t tmp=r00; r00=r11; r11=tmp; r01=-r01;
 501
 502   Double_t dy=y - fP0, dz=z - fP1;
 503   //cout << det << endl;
 504   //cout << "dy = " << dy << " , dz = " << dz << " , r00 = " << r00 << " , r01 = " << r01 << " r11 = " << r11 << endl;
 505   return (dy*r00*dy + 2*r01*dy*dz + dz*r11*dz)/det;
 506 }
 507
 508 Float_t AliL3KalmanTrack::f2(Float_t x1,Float_t y1,
 509                              Float_t x2,Float_t y2,
 510                              Float_t x3,Float_t y3)
 511 {
 512   //-----------------------------------------------------------------
 513   // Initial approximation of the track curvature times center of curvature
 514   // Will be removed ??
 515   //-----------------------------------------------------------------
 516   Double_t d=(x2-x1)*(y3-y2)-(x3-x2)*(y2-y1);
 517   Double_t a=0.5*((y3-y2)*(y2*y2-y1*y1+x2*x2-x1*x1)-
 518                   (y2-y1)*(y3*y3-y2*y2+x3*x3-x2*x2));
 519   Double_t b=0.5*((x2-x1)*(y3*y3-y2*y2+x3*x3-x2*x2)-
 520                   (x3-x2)*(y2*y2-y1*y1+x2*x2-x1*x1));
 521
 522   Double_t xr=TMath::Abs(d/(d*x1-a)), yr=d/(d*y1-b);
 523
 524   return -a/(d*y1-b)*xr/sqrt(xr*xr+yr*yr);
 525 }
 526
 527 Float_t AliL3KalmanTrack::f3(Float_t x1,Float_t y1,
 528                              Float_t x2,Float_t y2,
 529                              Float_t z1,Float_t z2)
 530 {
 531   //-----------------------------------------------------------------
 532   // Initial approximation of the tangent of the track dip angle
 533   // Will be removed ??
 534   //-----------------------------------------------------------------
 535   return (z1 - z2)/sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
 536 }
 537
 538 Float_t AliL3KalmanTrack::f4(Float_t x1,Float_t y1,
 539                              Float_t x2,Float_t y2,
 540                              Float_t x3,Float_t y3)
 541 {
 542   //-----------------------------------------------------------------
 543   // Initial approximation of the track curvature
 544   // Will be removed??
 545   //-----------------------------------------------------------------
 546   Double_t d=(x2-x1)*(y3-y2)-(x3-x2)*(y2-y1);
 547   Double_t a=0.5*((y3-y2)*(y2*y2-y1*y1+x2*x2-x1*x1)-
 548                   (y2-y1)*(y3*y3-y2*y2+x3*x3-x2*x2));
 549   Double_t b=0.5*((x2-x1)*(y3*y3-y2*y2+x3*x3-x2*x2)-
 550                   (x3-x2)*(y2*y2-y1*y1+x2*x2-x1*x1));
 551
 552   Double_t xr=TMath::Abs(d/(d*x1-a)), yr=d/(d*y1-b);
 553
 554   return -xr*yr/sqrt(xr*xr+yr*yr);
 555 }
 556
 557 void AliL3KalmanTrack::Set(AliL3KalmanTrack *track)
 558 {
 559
 560   AliL3KalmanTrack *tpt = (AliL3KalmanTrack*)track;
 561   SetX0(tpt->GetX0());
 562   SetX1(tpt->GetX1());
 563   SetX2(tpt->GetX2());
 564   SetX3(tpt->GetX3());
 565   SetX4(tpt->GetX4());
 566
 567   SetC0(tpt->GetC0());
 568   SetC1(tpt->GetC1());
 569   SetC2(tpt->GetC2());
 570   SetC3(tpt->GetC3());
 571   SetC4(tpt->GetC4());
 572   SetC5(tpt->GetC5());
 573   SetC6(tpt->GetC6());
 574   SetC7(tpt->GetC7());
 575   SetC8(tpt->GetC8());
 576   SetC9(tpt->GetC9());
 577   SetC10(tpt->GetC10());
 578   SetC11(tpt->GetC11());
 579   SetC12(tpt->GetC12());
 580   SetC13(tpt->GetC13());
 581   SetC14(tpt->GetC14());
 582
 583   SetNHits(tpt->GetNHits());
 584 }
 585
 586 Int_t AliL3KalmanTrack::PropagateOfflineTrack(Double_t x, Double_t y, Double_t z, Double_t ey, Double_t ez)
 587 {
 588   // Propagates track to the plane of the next found cluster
 589   Float_t Xold = fX; // X position for previous space point
 590   Float_t Xnew = x; // X position of current space point
 591   Float_t dx = Xnew - Xold; //cout << dx << endl;
 592   Float_t Yold = fP0; // Y position of old point
 593   Float_t Zold = fP1; // Z position of old point
 594   Float_t par2old = fP2;
 595   Float_t par3old = fP3;
 596   Float_t par4old = fP4;
 597   Float_t oldfC00 = fC00;
 598   Float_t oldfC10 = fC10;
 599   Float_t oldfC11 = fC11;
 600   Float_t oldfC20 = fC20;
 601   Float_t oldfC21 = fC21;
 602   Float_t oldfC22 = fC22;
 603   Float_t oldfC30 = fC30;
 604   Float_t oldfC31 = fC31;
 605   Float_t oldfC32 = fC32;
 606   Float_t oldfC33 = fC33;
 607   Float_t oldfC40 = fC40;
 608   Float_t oldfC41 = fC41;
 609   Float_t oldfC42 = fC42;
 610   Float_t oldfC43 = fC43;
 611   Float_t oldfC44 = fC44;
 612
 613   if (TMath::Abs(fP4*Xnew - fP2) >= 0.9) // What's this??
 614     {
 615       //cout << "Propagation failed! Stiff track. " << pos << endl;
 616       return 0;
 617     }
 618
 619   if (TMath::Abs(dx) > 5) return 0;
 620
 621   /*if (TMath::Abs(fP4*Xold - fP2) >= 0.9) // What's this??
 622     {
 623       return 0;
 624       }*/
 625
 626   // R must be approx. of the radius of the circle based on
 627   // the old and new spacepoint. What then is Cod and Cnew??
 628   // C has something to do with curvature at least.
 629
 630   Float_t Cold = fP4*Xold - fP2;
 631
 632   //if (TMath::Abs(Cold) >= 1.0) return 0;
 633   /*{
 634     cout << "Cold " << endl << fP4*Xnew - fP2 << endl;
 635     return 0;
 636     }*/
 637   Float_t Rold = TMath::Sqrt(1 - Cold*Cold);
 638   Float_t Cnew = fP4*Xnew - fP2;
 639
 640   //if (TMath::Abs(Cnew) >= 1.0) Cnew = 0.9;
 641     /*{
 642     cout << "Cnew " << endl;
 643     return 0;
 644     }*/
 645   Float_t Rnew = TMath::Sqrt(1 - Cnew*Cnew);
 646   //if (Rold < 0.9)
 647   //cout << "Cold = " << Cold << " , Rold = " << Rold << " , Cnew = " << Cnew << " , Rnew = " << Rnew << endl;
 648   // Prediction of the y- and z- coordinate in the next plane
 649   fP0 += dx*(Cold+Cnew)/(Rold+Rnew);
 650   fP1 += dx*(Cold+Cnew)/(Cold*Rnew + Cnew*Rold)*fP3;
 651   //cout << "Old point " << Yold << " " << Zold << endl;
 652   //cout << "Propagate " << fP0 << " " << fP1 << endl;
 653   //cout << "Measured  " << points[pos].fY << " " << points[pos].fZ << endl;
 654
 655   fX = Xnew;
 656
 657   // f = F - 1 //What is this??
 658   // Must be the f-matrix for the prediction, as in eq 1 in ALICE Kalman paper
 659   Float_t RR = Rold + Rnew;
 660   Float_t CC = Cold + Cnew;
 661   Float_t XX = Xold + Xnew;
 662
 663   Float_t f02 = -dx*(2*RR + CC*(Cold/Rold + Cnew/Rnew))/(RR*RR);
 664   Float_t f04 = dx*(RR*XX + CC*(Cold*Xold/Rold + Cnew*Xnew/Rnew))/(RR*RR);
 665   Float_t CR = Cold*Rnew + Cnew*Rold;
 666   Float_t f12 = -dx*fP3*(2*CR + CC*(Cnew*(Cold/Rold)-Rold + Cold*(Cnew/Rnew)-Rnew))/(CR*CR);
 667   Float_t f13 = dx*CC/CR;
 668   Float_t f14 = dx*fP3*(CR*XX-CC*(Rold*Xnew-Cnew*Cold*Xold/Rold + Rnew*Xold-Cold*Cnew*Xnew/Rnew))/(CR*CR);
 669
 670   // b = C*ft // This?
 671   Float_t b00=f02*fC20 + f04*fC40;
 672   Float_t b01=f12*fC20 + f14*fC40 + f13*fC30;
 673   Float_t b10=f02*fC21 + f04*fC41;
 674   Float_t b11=f12*fC21 + f14*fC41 + f13*fC31;
 675   Float_t b20=f02*fC22 + f04*fC42;
 676   Float_t b21=f12*fC22 + f14*fC42 + f13*fC32;
 677   Float_t b30=f02*fC32 + f04*fC43;
 678   Float_t b31=f12*fC32 + f14*fC43 + f13*fC33;
 679   Float_t b40=f02*fC42 + f04*fC44;
 680   Float_t b41=f12*fC42 + f14*fC44 + f13*fC43;
 681
 682   //a = f*b = f*C*ft
 683   Float_t a00 = f02*b20 + f04*b40;
 684   Float_t a01 = f02*b21 + f04*b41;
 685   Float_t a11 = f12*b21 + f14*b41+f13*b31;
 686
 687   //F*C*Ft = C + (a + b + bt) /This is the covariance matrix, the samll t
 688   // means transform. Then F must be df/dx
 689   fC00 += a00 + 2*b00;
 690   fC10 += a01 + b01 + b10;
 691   fC20 += b20;
 692   fC30 += b30;
 693   fC40 += b40;
 694   fC11 += a11 + 2*b11;
 695   fC21 += b21;
 696   fC31 += b31;
 697   fC41 += b41;
 698
 699   // Update the track parameters with the measured values of the new point
 700   Int_t value = UpdateOfflineTrack(x,y,z,ey,ez);
 701
 702   if (value == 0)
 703     {
 704       fP0 = Yold;
 705       fP1 = Zold;
 706       fP2 = par2old;
 707       fP3 = par3old;
 708       fP4 = par4old;
 709       fC00 = oldfC00;
 710       fC10 = oldfC10;
 711       fC11 = oldfC11;
 712       fC20 = oldfC20;
 713       fC21 = oldfC21;
 714       fC22 = oldfC22;
 715       fC30 = oldfC30;
 716       fC31 = oldfC31;
 717       fC32 = oldfC32;
 718       fC33 = oldfC33;
 719       fC40 = oldfC40;
 720       fC41 = oldfC41;
 721       fC42 = oldfC42;
 722       fC43 = oldfC43;
 723       fC44 = oldfC44;
 724
 725       return value;
 726       }
 727
 728   else
 729     return value;
 730   //return 1;
 731   //return UpdateTrack(points, pos);
 732
 733 }
 734
 735 Int_t AliL3KalmanTrack::UpdateOfflineTrack(Double_t x, Double_t y, Double_t z, Double_t ey, Double_t ez)
 736 {
 737   // Update the track parameters with the measured values
 738   //fX = x;
 739
 740   // The errors from the measurement of the spacepoint
 741   Float_t sigmaY2 = ey;
 742   Float_t sigmaZ2 = ez;
 743   Float_t sigmaYZ = 0;
 744
 745   sigmaY2 += fC00;
 746   sigmaZ2 += fC11;
 747   sigmaYZ += fC10;
 748
 749   Float_t det = sigmaY2*sigmaZ2 - sigmaYZ*sigmaYZ;
 750   Float_t tmp = sigmaY2;
 751   sigmaY2 = sigmaZ2/det;
 752   sigmaZ2 = tmp/det;
 753   sigmaYZ = -sigmaYZ/det;
 754
 755   // Kalman gain matrix
 756   Float_t k00 = fC00*sigmaY2 + fC10*sigmaYZ, k01 = fC00*sigmaYZ + fC10*sigmaZ2;
 757   Float_t k10 = fC10*sigmaY2 + fC11*sigmaYZ, k11 = fC10*sigmaYZ + fC11*sigmaZ2;
 758   Float_t k20 = fC20*sigmaY2 + fC21*sigmaYZ, k21 = fC20*sigmaYZ + fC21*sigmaZ2;
 759   Float_t k30 = fC30*sigmaY2 + fC31*sigmaYZ, k31 = fC30*sigmaYZ + fC31*sigmaZ2;
 760   Float_t k40 = fC40*sigmaY2 + fC41*sigmaYZ, k41 = fC40*sigmaYZ + fC41*sigmaZ2;
 761   //cout << "x = " << fX << endl;
 762   // Deviation between the predicted and measured values of y and z
 763   Float_t dy = y-fP0; //cout << dy << endl;
 764   Float_t dz = z-fP1; //cout << ", dz = " << dz << endl;
 765   //cout << "Measured " << points[pos].fY << " " << points[pos].fZ << endl;
 766
 767   // Prediction of fP2 and fP4
 768   Float_t cur = fP4 + k40*dy + k41*dz;
 769   Float_t eta = fP2 + k20*dy + k21*dz;
 770
 771   if (TMath::Abs(cur*fX - eta) >= 0.9)
 772     {
 773       //cout << "Update failed! Stiff track. " << pos << endl;
 774       return 0;
 775     }
 776
 777   // Filtered state vector
 778   fP0 += k00*dy + k01*dz;
 779   fP1 += k10*dy + k11*dz;
 780   fP2 = eta;
 781   fP3 += k30*dy + k31*dz; //cout << "update " << fP3 << endl;
 782   fP4 = cur;
 783   //cout << "AliL3KalmanTrack::Update, " << fP0 << " " << fP1 << " " << fP2 << " " << fP3 << " " << fP4 << endl;
 784   //cout << "Measured, " << points[pos].fY << " " << points[pos].fZ << endl;
 785
 786   Float_t c01 = fC10;
 787   Float_t c02 = fC20;
 788   Float_t c03 = fC30;
 789   Float_t c04 = fC40;
 790   Float_t c12 = fC21;
 791   Float_t c13 = fC31;
 792   Float_t c14 = fC41;
 793
 794   // Filtered covariance matrix
 795   fC00 -= k00*fC00 + k01*fC10; fC10 -= k00*c01 + k01*fC11;
 796   fC20 -= k00*c02 + k01*c12;   fC30 -= k00*c03 + k01*c13;
 797   fC40 -= k00*c04 + k01*c14;
 798
 799   fC11 -= k10*c01 + k11*fC11;
 800   fC21 -= k10*c02 + k11*c12;   fC31 -= k10*c03 + k11*c13;
 801   fC41 -= k10*c04 + k11*c14;
 802
 803   fC22 -= k20*c02 + k21*c12;   fC32 -= k20*c03 + k21*c13;
 804   fC42 -= k20*c04 + k21*c14;
 805
 806   fC33 -= k30*c03 + k31*c13;
 807   fC43 -= k40*c03 + k41*c13;
 808
 809   fC44 -= k40*c04 + k41*c14;
 810
 811   //cout << "AliL3KalmanTrack::Update, error " << fC00 << " " << fC11 << " " << fC22 << " " << fC33 << " " << fC44 << endl;
 812
 813   sigmaY2 = sigmaY2*det;
 814   sigmaZ2 = sigmaZ2*det;
 815   sigmaYZ = sigmaYZ*det;
 816   //cout << "AliL3KalmanTrack::Update, Chi2 = " << GetChisq() << endl;
 817
 818   fChisq = GetChisq() + GetChisqIncrementOfflineTrack(y,z,ey,ez);
 819   //cout << "fChisq = " << fChisq << endl;
 820
 821   // Must at some point make an cut on chisq. Here?
 822   //if (fChisq > fMaxChi2) return 0;
 823
 824   return 1;
 825
 826 }
 827
 828 Float_t AliL3KalmanTrack::GetChisqIncrementOfflineTrack(Double_t y, Double_t z, Double_t ey, Double_t ez)
 829 {
 830   // This function calculates a predicted chi2 increment.
 831   //-----------------------------------------------------------------
 832   Float_t r00=ey, r01=0., r11=ez;
 833   r00+=fC00; r01+=fC10; r11+=fC11;
 834
 835   Double_t det=r00*r11 - r01*r01;
 836   /*if (TMath::Abs(det) < 1.e-10) {
 837     Int_t n=GetNumberOfClusters();
 838     if (n>4) cerr<<n<<" AliKalmanTrack warning: Singular matrix !\n";
 839     return 1e10;
 840     }*/
 841   Double_t tmp=r00; r00=r11; r11=tmp; r01=-r01;
 842
 843   Double_t dy=y - fP0, dz=z - fP1;
 844   //cout << "AliTPCtrack::GetPredictedChi2, r00 = " << r00 << " r11 = " << r11 << " ro1 = " << r01 << " dy = " << dy << " dz = " << dz << endl;
 845   return (dy*r00*dy + 2*r01*dy*dz + dz*r11*dz)/det;
 846 }