STEER/AliTrackFitterKalman.cxx

   1 /**************************************************************************
   2  * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
   3  *                                                                        *
   4  * Author: The ALICE Off-line Project.                                    *
   5  * Contributors are mentioned in the code where appropriate.              *
   6  *                                                                        *
   7  * Permission to use, copy, modify and distribute this software and its   *
   8  * documentation strictly for non-commercial purposes is hereby granted   *
   9  * without fee, provided that the above copyright notice appears in all   *
  10  * copies and that both the copyright notice and this permission notice   *
  11  * appear in the supporting documentation. The authors make no claims     *
  12  * about the suitability of this software for any purpose. It is          *
  13  * provided "as is" without express or implied warranty.                  *
  14  **************************************************************************/
  15
  16 ///////////////////////////////////////////////////////////////////////////////
  17 //
  18 //                      Kalman-Filter-like fit
  19 //   to a straight-line crossing a set of arbitrarily oriented planes.
  20 //   The resulting line is given by the equation:
  21 //                  (x-x0)/vx = (y-y0)/1 = (z-z0)/vz
  22 //   Parameters of the fit are:
  23 //        x0,y0,z0 (a point on the line) and
  24 //        vx,1,vz  (a vector collinear with the line)
  25 //
  26 //   LIMITATION:  The line must not be perpendicular to the Y axis.
  27 //
  28 //          Origin: Iouri Belikov, CERN, Jouri.Belikov@cern.ch
  29 //
  30 //////////////////////////////////////////////////////////////////////////////
  31
  32 #include <TMatrixD.h>
  33 #include <TMatrixDSym.h>
  34
  35 #include "AliLog.h"
  36 #include "AliTrackFitterKalman.h"
  37
  38 ClassImp(AliTrackFitterKalman)
  39
  40 const Double_t AliTrackFitterKalman::fgkMaxChi2=33.;
  41
  42 AliTrackFitterKalman::
  43 AliTrackFitterKalman(AliTrackPointArray *array, Bool_t owner):
  44   AliTrackFitter(array,owner),
  45   fMaxChi2(fgkMaxChi2),
  46   fSeed(kFALSE)
  47 {
  48   //
  49   // Constructor
  50   //
  51 }
  52
  53 Bool_t AliTrackFitterKalman::Begin(Int_t first, Int_t last) {
  54   //
  55   // Make a seed out of the track points with the indices "first" and "last".
  56   // This is the "default" seed.
  57   //
  58   if (fSeed) return kTRUE;
  59   fPoints->Sort();
  60   AliTrackPoint fp,lp;
  61   fPoints->GetPoint(fp,first); fPoints->GetPoint(lp,last);
  62   return MakeSeed(&fp,&lp);
  63 }
  64
  65
  66 Bool_t AliTrackFitterKalman::
  67 MakeSeed(const AliTrackPoint *p0, const AliTrackPoint *p1) {
  68   //
  69   // Make a seed out of the two track points
  70   //
  71   Double_t x0=p0->GetX(), y0=p0->GetY(), z0=p0->GetZ();
  72   Double_t dx=p1->GetX()-x0, dy=p1->GetY()-y0, dz=p1->GetZ()-z0;
  73   if (dy==0.) {
  74      AliError("Seeds perpendicular to Y axis are not allowed !");
  75      return kFALSE;
  76   }
  77   Double_t vx=dx/dy;
  78   Double_t vz=dz/dy;
  79   Double_t par[5]={x0,y0,z0,vx,vz};
  80
  81   const Float_t *cv0=p0->GetCov();
  82   const Float_t *cv1=p1->GetCov();
  83   Double_t rdy2=(cv0[3]+cv1[3])/dy/dy;
  84   Double_t svx2=(cv0[0]+cv1[0])/dy/dy + vx*vx*rdy2;
  85   Double_t svz2=(cv0[5]+cv1[5])/dy/dy + vz*vz*rdy2;
  86   Double_t cov[15]={
  87      cv0[0],
  88      cv0[1],cv0[3],
  89      cv0[2],cv0[4],cv0[5],
  90      0.,0.,0.,svx2,
  91      0.,0.,0.,0.,svz2
  92   };
  93
  94   SetSeed(par,cov);
  95
  96   return kTRUE;
  97 }
  98
  99 Bool_t AliTrackFitterKalman::AddPoint(const AliTrackPoint *p)
 100 {
 101   //
 102   // Add a point to the fit
 103   //
 104
 105   if (!Propagate(p))   return kFALSE;
 106   Double_t chi2=GetPredictedChi2(p);
 107   if (chi2>fMaxChi2)   return kFALSE;
 108   if (!Update(p,chi2)) return kFALSE;
 109
 110   return kTRUE;
 111 }
 112
 113
 114 Double_t AliTrackFitterKalman::GetPredictedChi2(const AliTrackPoint *p) const {
 115   //
 116   // Calculate the predicted chi2 increment.
 117   //
 118
 119   Float_t txyz[3]={GetParam()[0],GetParam()[1],GetParam()[2]};
 120   TMatrixDSym &cv=*fCov;
 121   Float_t tcov[6]={
 122     cv(0,0),cv(1,0),cv(2,0),
 123             cv(1,1),cv(2,1),
 124                     cv(2,2)
 125   };
 126   AliTrackPoint tp(txyz,tcov,p->GetVolumeID());
 127
 128   Double_t chi2=p->GetResidual(tp,kTRUE);
 129
 130   return chi2;
 131 }
 132
 133
 134 Bool_t AliTrackFitterKalman::Propagate(const AliTrackPoint *p) {
 135   //
 136   // Propagate the track towards the measured point "p"
 137   //
 138
 139   TMatrixDSym &cv=*fCov;
 140   Double_t s=p->GetY() - fParams[1];
 141   Double_t sig2=s*s/12.;
 142
 143   Double_t vx=fParams[3], vz=fParams[4];
 144   fParams[0] += s*vx;
 145   fParams[1] += s;
 146   fParams[2] += s*vz;
 147
 148   Double_t
 149   c00 = cv(0,0) + 2*s*cv(3,0) + s*s*cv(3,3) + vx*vx*sig2,
 150
 151   c10 = cv(1,0) + s*cv(1,3) + vx*sig2, c11=cv(1,1) + sig2,
 152
 153   c20 = cv(2,0) + s*(cv(4,0) + cv(2,3)) + s*s*cv(4,3) + vx*vz*sig2,
 154   c21 = cv(2,1) + s*cv(4,1) + vz*sig2,
 155   c22 = cv(2,2) + 2*s*cv(4,2) + s*s*cv(4,4) + vz*vz*sig2,
 156
 157   c30 = cv(3,0) + s*cv(3,3), c31 = cv(3,1),
 158   c32 = cv(3,2) + s*cv(3,4), c33 = cv(3,3),
 159
 160   c40 = cv(4,0) + s*cv(4,3), c41 = cv(4,1),
 161   c42 = cv(4,2) + s*cv(4,4), c43 = cv(4,3), c44 = cv(4,4);
 162
 163
 164   cv(0,0)=c00; cv(0,1)=c10; cv(0,2)=c20; cv(0,3)=c30; cv(0,4)=c40;
 165   cv(1,0)=c10; cv(1,1)=c11; cv(1,2)=c21; cv(1,3)=c31; cv(1,4)=c41;
 166   cv(2,0)=c20; cv(2,1)=c21; cv(2,2)=c22; cv(2,3)=c32; cv(2,4)=c42;
 167   cv(3,0)=c30; cv(3,1)=c31; cv(3,2)=c32; cv(3,3)=c33; cv(3,4)=c43;
 168   cv(4,0)=c40; cv(4,1)=c41; cv(4,2)=c42; cv(4,3)=c43; cv(4,4)=c44;
 169
 170   return kTRUE;
 171 }
 172
 173 Bool_t AliTrackFitterKalman::Update(const AliTrackPoint *p, Double_t chi2) {
 174   //
 175   // Update the track params using the measured point "p"
 176   //
 177
 178   TMatrixDSym &c=*fCov;
 179   const Float_t *cov=p->GetCov();
 180
 181   TMatrixDSym v(3);
 182   v(0,0)=cov[0]+c(0,0); v(0,1)=cov[1]+c(0,1); v(0,2)=cov[2]+c(0,2);
 183   v(1,0)=cov[1]+c(1,0); v(1,1)=cov[3]+c(1,1); v(1,2)=cov[4]+c(1,2);
 184   v(2,0)=cov[2]+c(2,0); v(2,1)=cov[4]+c(2,1); v(2,2)=cov[5]+c(2,2);
 185   if (TMath::Abs(v.Determinant()) < 1.e-33) return kFALSE;
 186   v.Invert();
 187
 188   TMatrixD ch(5,3);
 189   ch(0,0)=c(0,0); ch(0,1)=c(0,1); ch(0,2)=c(0,2);
 190   ch(1,0)=c(1,0); ch(1,1)=c(1,1); ch(1,2)=c(1,2);
 191   ch(2,0)=c(2,0); ch(2,1)=c(2,1); ch(2,2)=c(2,2);
 192   ch(3,0)=c(3,0); ch(3,1)=c(3,1); ch(3,2)=c(3,2);
 193   ch(4,0)=c(4,0); ch(4,1)=c(4,1); ch(4,2)=c(4,2);
 194
 195   TMatrixD k(ch,TMatrixD::kMult,v);
 196
 197   TMatrixD d(3,1);
 198   d(0,0) = p->GetX() - fParams[0];
 199   d(1,0) = p->GetY() - fParams[1];
 200   d(2,0) = p->GetZ() - fParams[2];
 201
 202   TMatrixD x(k,TMatrixD::kMult,d);
 203
 204   fParams[0]+=x(0,0);
 205   fParams[1]+=x(1,0);
 206   fParams[2]+=x(2,0);
 207   fParams[3]+=x(3,0);
 208   fParams[4]+=x(4,0);
 209
 210   TMatrixD hc(3,5);
 211   hc(0,0)=c(0,0);hc(0,1)=c(0,1);hc(0,2)=c(0,2);hc(0,3)=c(0,3);hc(0,4)=c(0,4);
 212   hc(1,0)=c(1,0);hc(1,1)=c(1,1);hc(1,2)=c(1,2);hc(1,3)=c(1,3);hc(1,4)=c(1,4);
 213   hc(2,0)=c(2,0);hc(2,1)=c(2,1);hc(2,2)=c(2,2);hc(2,3)=c(2,3);hc(2,4)=c(2,4);
 214
 215   TMatrixD s(k,TMatrixD::kMult,hc);
 216
 217   c(0,0)-=s(0,0);c(0,1)-=s(0,1);c(0,2)-=s(0,2);c(0,3)-=s(0,3);c(0,4)-=s(0,4);
 218   c(1,0)-=s(1,0);c(1,1)-=s(1,1);c(1,2)-=s(1,2);c(1,3)-=s(1,3);c(1,4)-=s(1,4);
 219   c(2,0)-=s(2,0);c(2,1)-=s(2,1);c(2,2)-=s(2,2);c(2,3)-=s(2,3);c(2,4)-=s(2,4);
 220   c(3,0)-=s(3,0);c(3,1)-=s(3,1);c(3,2)-=s(3,2);c(3,3)-=s(3,3);c(3,4)-=s(3,4);
 221   c(4,0)-=s(4,0);c(4,1)-=s(4,1);c(4,2)-=s(4,2);c(4,3)-=s(4,3);c(4,4)-=s(4,4);
 222
 223   fChi2 += chi2;
 224   fNdf  += 2;
 225
 226   return kTRUE;
 227 }
 228
 229 //_____________________________________________________________________________
 230 Bool_t AliTrackFitterKalman::GetPCA(const AliTrackPoint &p, AliTrackPoint &i) const
 231 {
 232   //
 233   // Get the intersection point "i" between the track and the plane
 234   // the point "p" belongs to.
 235   //
 236
 237   TMatrixD t(3,1);
 238   Double_t s=p.GetY() - fParams[1];
 239   Double_t vx=fParams[3], vz=fParams[4];
 240   t(0,0) = fParams[0] + s*vx;
 241   t(1,0) = fParams[1] + s;
 242   t(2,0) = fParams[2] + s*vz;
 243
 244   TMatrixDSym tC(3);
 245   {
 246   Double_t sig2=s*s/12.;
 247
 248   tC(0,0) = vx*vx*sig2;
 249   tC(1,0) = vx*sig2;
 250   tC(1,1) = sig2;
 251   tC(2,0) = vx*vz*sig2;
 252   tC(2,1) = vz*sig2;
 253   tC(2,2) = vz*vz*sig2;
 254
 255   tC(0,1) = tC(1,0); tC(0,2) = tC(2,0);
 256                      tC(1,2) = tC(2,1);
 257   }
 258
 259   TMatrixD m(3,1);
 260   m(0,0)=p.GetX();
 261   m(1,0)=p.GetY();
 262   m(2,0)=p.GetZ();
 263
 264   TMatrixDSym mC(3);
 265   {
 266   const Float_t *cv=p.GetCov();
 267   mC(0,0)=cv[0]; mC(0,1)=cv[1]; mC(0,2)=cv[2];
 268   mC(1,0)=cv[1]; mC(1,1)=cv[3]; mC(1,2)=cv[4];
 269   mC(2,0)=cv[2]; mC(2,1)=cv[4]; mC(2,2)=cv[5];
 270   }
 271
 272   TMatrixDSym tmW(tC);
 273   tmW+=mC;
 274   if (TMath::Abs(tmW.Determinant()) < 1.e-33) return kFALSE;
 275   tmW.Invert();
 276
 277   TMatrixD mW(tC,TMatrixD::kMult,tmW);
 278   TMatrixD tW(mC,TMatrixD::kMult,tmW);
 279
 280   TMatrixD mi(mW,TMatrixD::kMult,m);
 281   TMatrixD ti(tW,TMatrixD::kMult,t);
 282   ti+=mi;
 283
 284   TMatrixD iC(tC,TMatrixD::kMult,tmW);
 285   iC*=mC;
 286   Double_t sig2=1.;  // Releasing the matrix by 1 cm along the track direction
 287   iC(0,0) += vx*vx*sig2;
 288   iC(0,1) += vx*sig2;
 289   iC(1,1) += sig2;
 290   iC(0,2) += vx*vz*sig2;
 291   iC(1,2) += vz*sig2;
 292   iC(2,2) += vz*vz*sig2;
 293
 294
 295   Float_t cov[6]={
 296     iC(0,0), iC(0,1), iC(0,2),
 297              iC(1,1), iC(1,2),
 298                       iC(2,2)
 299   };
 300   i.SetXYZ(ti(0,0),ti(1,0),ti(2,0),cov);
 301   UShort_t id=p.GetVolumeID();
 302   i.SetVolumeID(id);
 303
 304   return kTRUE;
 305 }
 306
 307
 308 //_____________________________________________________________________________
 309 void
 310 AliTrackFitterKalman::SetSeed(const Double_t par[5], const Double_t cov[15]) {
 311   //
 312   // Set the initial approximation for the track parameters
 313   //
 314   for (Int_t i=0; i<5; i++) fParams[i]=par[i];
 315   fParams[5]=0.;
 316
 317   delete fCov;
 318   fCov=new TMatrixDSym(5);
 319   TMatrixDSym &cv=*fCov;
 320
 321   cv(0,0)=cov[0 ];
 322   cv(1,0)=cov[1 ]; cv(1,1)=cov[2 ];
 323   cv(2,0)=cov[3 ]; cv(2,1)=cov[4 ]; cv(2,2)=cov[5 ];
 324   cv(3,0)=cov[6 ]; cv(3,1)=cov[7 ]; cv(3,2)=cov[8 ]; cv(3,3)=cov[9 ];
 325   cv(4,0)=cov[10]; cv(4,1)=cov[11]; cv(4,2)=cov[12]; cv(4,3)=cov[13]; cv(4,4)=cov[14];
 326
 327   cv(0,1)=cv(1,0);
 328   cv(0,2)=cv(2,0); cv(1,2)=cv(2,1);
 329   cv(0,3)=cv(3,0); cv(1,3)=cv(3,1); cv(2,3)=cv(3,2);
 330   cv(0,4)=cv(4,0); cv(1,4)=cv(4,1); cv(2,4)=cv(4,2); cv(3,4)=cv(4,3);
 331
 332   fSeed=kTRUE;
 333 }