STEER/AliRelAlignerKalman.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 based aligner:
  19 //    Finds alignement constants for  two tracking volumes (by default ITS
  20 //    and TPC)
  21 //    Determines the inverse transformation of the second volume (TPC)
  22 //    with respect to the first (ITS) (how to realign TPC to ITS)
  23 //    by measuring the residual between the 2 tracks.
  24 //    Additionally calculates some callibration parameters for TPC
  25 //    Fit parameters are:
  26 //    - 3 shifts, x,y,z
  27 //    - 3 Cardan angles, psi, theta, phi (see definition in alignment docs),
  28 //    - TPC drift velocity correction,
  29 //    - TPC time offset correction.
  30 //
  31 //    Basic usage:
  32 //    When aligning two volumes, at any given time a single instance of
  33 //    the class should be active. The fit of the parameters is updated
  34 //    by adding new data using one of the Add.... methods:
  35 //
  36 //    In collision events add an ESD event to update the fit (adds all tracks):
  37 //
  38 //        Bool_t AddESDevent( AliESDevent* pTrack );
  39 //
  40 //    or add each individual track
  41 //
  42 //        AddESDtrack( AliESDtrack* pTrack );
  43 //
  44 //    For cosmic data, the assumption is that the tracking is done twice:
  45 //    once global and once only ITS and the tracklets are saved inside
  46 //    one AliESDEvent. The method
  47 //
  48 //        Bool_t AddCosmicEvent( AliESDEvent* pEvent );
  49 //
  50 //    then searches the event for matching tracklets and upon succes it updates.
  51 //    One cosmic ideally triggers two updates: for the upper and lower half of
  52 //    the cosmic (upper ITS tracklet+upper TPC tracklet, idem dito for lower)
  53 //
  54 //    by default give misalignment parameters for TPC as they appear to be.
  55 //    TPC calibration parameters are always given as correction to values used in reco.
  56 //
  57 //    _________________________________________________________________________
  58 //    Expert options:
  59 //    look at AddESDevent() and AddCosmicEvent() to get the idea of how the
  60 //    aligner works, it's safe to repeat the needed steps outside of the class,
  61 //    only public methods are used.
  62 //
  63 //    Origin: Mikolaj Krzewicki, Nikhef, Mikolaj.Krzewicki@cern.ch
  64 //
  65 //////////////////////////////////////////////////////////////////////////////
  66
  67 #include <iostream>
  68 #include <TObject.h>
  69 #include <TMath.h>
  70 #include <TMatrix.h>
  71 #include <TVector.h>
  72 #include <TVector3.h>
  73 #include <TDecompLU.h>
  74 #include <TArrayI.h>
  75 #include <TH1D.h>
  76 #include <TF1.h>
  77
  78 #include "AliESDtrack.h"
  79 #include "AliESDEvent.h"
  80 #include "AliExternalTrackParam.h"
  81
  82 #include "AliRelAlignerKalman.h"
  83
  84 ClassImp(AliRelAlignerKalman)
  85
  86 //______________________________________________________________________________
  87 AliRelAlignerKalman::AliRelAlignerKalman():
  88     TObject(),
  89     fPTrackParamArr1(new AliExternalTrackParam[fgkNTracksPerMeasurement]),
  90     fPTrackParamArr2(new AliExternalTrackParam[fgkNTracksPerMeasurement]),
  91     fMagField(0.),
  92     fNMeasurementParams(4),
  93     fPX(new TVectorD( fgkNSystemParams )),
  94     fPXcov(new TMatrixDSym( fgkNSystemParams )),
  95     fPH(new TMatrixD( fNMeasurementParams, fgkNSystemParams )),
  96     fQ(1.e-15),
  97     fPMeasurement(new TVectorD( fNMeasurementParams )),
  98     fPMeasurementCov(new TMatrixDSym( fNMeasurementParams )),
  99     fPMeasurementPrediction(new TVectorD( fNMeasurementParams )),
 100     fOutRejSigmas(1.),
 101     fYZOnly(kFALSE),
 102     fNumericalParanoia(kTRUE),
 103     fRejectOutliers(kTRUE),
 104     fRequireMatchInTPC(kFALSE),
 105     fCuts(kFALSE),
 106     fMinPointsVol1(3),
 107     fMinPointsVol2(50),
 108     fMinMom(0.),
 109     fMaxMom(1.e100),
 110     fMaxMatchingAngle(0.1),
 111     fMaxMatchingDistance(10.),  //in cm
 112     fCorrectionMode(kFALSE),
 113     fNTracks(0),
 114     fNUpdates(0),
 115     fNOutliers(0),
 116     fNMatchedCosmics(0),
 117     fNMatchedTPCtracklets(0),
 118     fNProcessedEvents(0),
 119     fTrackInBuffer(0),
 120     fTimeStamp(0),
 121     fRunNumber(0),
 122     fNMerges(0),
 123     fNMergesFailed(0),
 124     fTPCvd(2.64),
 125     fTPCZLengthA(2.4972500e02),
 126     fTPCZLengthC(2.4969799e02)
 127 {
 128   //Default constructor
 129
 130   //default seed: zero, reset errors to large default
 131   Reset();
 132 }
 133
 134 //______________________________________________________________________________
 135 AliRelAlignerKalman::AliRelAlignerKalman(const AliRelAlignerKalman& a):
 136     TObject(static_cast<TObject>(a)),
 137     fPTrackParamArr1(new AliExternalTrackParam[fgkNTracksPerMeasurement]),
 138     fPTrackParamArr2(new AliExternalTrackParam[fgkNTracksPerMeasurement]),
 139     fMagField(a.fMagField),
 140     fNMeasurementParams(a.fNMeasurementParams),
 141     fPX(new TVectorD( *a.fPX )),
 142     fPXcov(new TMatrixDSym( *a.fPXcov )),
 143     fPH(new TMatrixD( fNMeasurementParams, fgkNSystemParams )),
 144     fQ(a.fQ),
 145     fPMeasurement(new TVectorD( fNMeasurementParams )),
 146     fPMeasurementCov(new TMatrixDSym( fNMeasurementParams )),
 147     fPMeasurementPrediction(new TVectorD( fNMeasurementParams )),
 148     fOutRejSigmas(a.fOutRejSigmas),
 149     fYZOnly(a.fYZOnly),
 150     fNumericalParanoia(a.fNumericalParanoia),
 151     fRejectOutliers(a.fRejectOutliers),
 152     fRequireMatchInTPC(a.fRequireMatchInTPC),
 153     fCuts(a.fCuts),
 154     fMinPointsVol1(a.fMinPointsVol1),
 155     fMinPointsVol2(a.fMinPointsVol2),
 156     fMinMom(a.fMinMom),
 157     fMaxMom(a.fMaxMom),
 158     fMaxMatchingAngle(a.fMaxMatchingAngle),
 159     fMaxMatchingDistance(a.fMaxMatchingDistance),  //in cm
 160     fCorrectionMode(a.fCorrectionMode),
 161     fNTracks(a.fNTracks),
 162     fNUpdates(a.fNUpdates),
 163     fNOutliers(a.fNOutliers),
 164     fNMatchedCosmics(a.fNMatchedCosmics),
 165     fNMatchedTPCtracklets(a.fNMatchedTPCtracklets),
 166     fNProcessedEvents(a.fNProcessedEvents),
 167     fTrackInBuffer(0),
 168     fTimeStamp(a.fTimeStamp),
 169     fRunNumber(a.fRunNumber),
 170     fNMerges(a.fNMerges),
 171     fNMergesFailed(a.fNMergesFailed),
 172     fTPCvd(a.fTPCvd),
 173     fTPCZLengthA(a.fTPCZLengthA),
 174     fTPCZLengthC(a.fTPCZLengthC)
 175 {
 176   //copy constructor
 177   memcpy(fDelta,a.fDelta,fgkNSystemParams*sizeof(Double_t));
 178 }
 179
 180 //______________________________________________________________________________
 181 AliRelAlignerKalman& AliRelAlignerKalman::operator=(const AliRelAlignerKalman& a)
 182 {
 183   //assignment operator
 184   fMagField=a.fMagField,
 185   fNMeasurementParams=a.fNMeasurementParams;
 186   *fPX = *a.fPX;
 187   *fPXcov = *a.fPXcov;
 188   fQ=a.fQ;
 189   fOutRejSigmas=a.fOutRejSigmas;
 190   memcpy(fDelta,a.fDelta,fgkNSystemParams*sizeof(Double_t));
 191   fYZOnly=a.fYZOnly;
 192   fNumericalParanoia=a.fNumericalParanoia;
 193   fRejectOutliers=a.fRejectOutliers;
 194   fRequireMatchInTPC=a.fRequireMatchInTPC;
 195   fCuts=a.fCuts;
 196   fMinPointsVol1=a.fMinPointsVol1;
 197   fMinPointsVol2=a.fMinPointsVol2;
 198   fMinMom=a.fMinMom;
 199   fMaxMom=a.fMaxMom;
 200   fMaxMatchingAngle=a.fMaxMatchingAngle;
 201   fMaxMatchingDistance=a.fMaxMatchingDistance;  //in c;
 202   fCorrectionMode=a.fCorrectionMode;
 203   fNTracks=a.fNTracks;
 204   fNUpdates=a.fNUpdates;
 205   fNOutliers=a.fNOutliers;
 206   fNMatchedCosmics=a.fNMatchedCosmics;
 207   fNMatchedTPCtracklets=a.fNMatchedTPCtracklets;
 208   fNProcessedEvents=a.fNProcessedEvents;
 209   fTrackInBuffer=0; //because the array is reset, not copied
 210   fTimeStamp=a.fTimeStamp;
 211   fRunNumber=a.fRunNumber;
 212   fNMerges=a.fNMerges;
 213   fTPCvd=a.fTPCvd;
 214   fTPCZLengthA=a.fTPCZLengthA;
 215   fTPCZLengthC=a.fTPCZLengthC;
 216   return *this;
 217 }
 218
 219 //______________________________________________________________________________
 220 AliRelAlignerKalman::~AliRelAlignerKalman()
 221 {
 222   //destructor
 223   if (fPTrackParamArr1) delete [] fPTrackParamArr1;
 224   if (fPTrackParamArr2) delete [] fPTrackParamArr2;
 225   if (fPX) delete fPX;
 226   if (fPXcov) delete fPXcov;
 227   if (fPH) delete fPH;
 228   if (fPMeasurement) delete fPMeasurement;
 229   if (fPMeasurementCov) delete fPMeasurementCov;
 230 }
 231
 232 //______________________________________________________________________________
 233 Bool_t AliRelAlignerKalman::AddESDevent( const AliESDEvent* pEvent )
 234 {
 235   //Add all tracks in an ESD event
 236
 237   fNProcessedEvents++; //update the counter
 238
 239   Bool_t success=kFALSE;
 240   SetMagField( pEvent->GetMagneticField() );
 241   AliESDtrack* track;
 242
 243   for (Int_t i=0; i<pEvent->GetNumberOfTracks(); i++)
 244   {
 245     track = pEvent->GetTrack(i);
 246     if (!track) continue;
 247     if ( ((track->GetStatus()&AliESDtrack::kTPCin)>0)&&
 248          ((track->GetStatus()&AliESDtrack::kITSout)>0)&&
 249          (track->GetNcls(0)>=fMinPointsVol1)&&
 250          (track->GetNcls(1)>=fMinPointsVol2) )
 251     {
 252       success = ( AddESDtrack( track ) || success );
 253     }
 254   }
 255   if (success)
 256   {
 257     fTimeStamp = pEvent->GetTimeStamp();
 258     fRunNumber = pEvent->GetRunNumber();
 259   }
 260   return success;
 261 }
 262
 263 //______________________________________________________________________________
 264 Bool_t AliRelAlignerKalman::AddESDtrack( const AliESDtrack* pTrack )
 265 {
 266   //Adds a full track, returns true if results in a new estimate
 267   //  gets the inner TPC parameters from AliESDTrack::GetInnerParam()
 268   //  gets the outer ITS parameters from AliESDTrack::GetOuterParam()
 269
 270   const AliExternalTrackParam* pconstparamsITS = pTrack->GetOuterParam();
 271   if (!pconstparamsITS) return kFALSE;
 272   const AliExternalTrackParam* pconstparamsTPC = pTrack->GetInnerParam();
 273   if (!pconstparamsTPC) return kFALSE;
 274
 275   //TPC part
 276   AliExternalTrackParam paramsTPC = (*pconstparamsTPC);
 277   paramsTPC.Rotate(pconstparamsITS->GetAlpha());
 278   paramsTPC.PropagateTo(pconstparamsITS->GetX(), fMagField);
 279
 280   return (AddTrackParams(pconstparamsITS, &paramsTPC));
 281 }
 282
 283 //______________________________________________________________________________
 284 Bool_t AliRelAlignerKalman::AddTrackParams( const AliExternalTrackParam* p1, const AliExternalTrackParam* p2 )
 285 {
 286   //Update the estimate using new matching tracklets
 287
 288   if (!SetTrackParams(p1, p2)) return kFALSE;
 289   return Update();
 290 }
 291
 292 //______________________________________________________________________________
 293 Bool_t AliRelAlignerKalman::AddCosmicEvent( const AliESDEvent* pEvent )
 294 {
 295   //Add an cosmic with separately tracked ITS and TPC parts, do trackmatching
 296
 297   fNProcessedEvents++; //update the counter
 298
 299   Bool_t success=kFALSE;
 300   TArrayI trackTArrITS(1);
 301   TArrayI trackTArrTPC(1);
 302   if (!FindCosmicTrackletNumbersInEvent( trackTArrITS, trackTArrTPC, pEvent )) return kFALSE;
 303   SetMagField( pEvent->GetMagneticField() );
 304   AliESDtrack* ptrack;
 305   const AliExternalTrackParam* pconstparams1;
 306   const AliExternalTrackParam* pconstparams2;
 307   AliExternalTrackParam params1;
 308   AliExternalTrackParam params2;
 309
 310   ////////////////////////////////
 311   for (Int_t i=0;i<trackTArrITS.GetSize();i++)
 312   {
 313     //ITS track
 314     ptrack = pEvent->GetTrack(trackTArrITS[i]);
 315     pconstparams1 = ptrack->GetOuterParam();
 316     if (!pconstparams1) continue;
 317     params1 = *pconstparams1; //make copy to be safe
 318
 319     //TPC track
 320     ptrack = pEvent->GetTrack(trackTArrTPC[i]);
 321     pconstparams2 = ptrack->GetInnerParam();
 322     if (!pconstparams2) continue;
 323     params2 = *pconstparams2; //make copy
 324     params2.Rotate(params1.GetAlpha());
 325     params2.PropagateTo( params1.GetX(), fMagField );
 326
 327     if (!SetTrackParams( &params1, &params2 )) continue;
 328
 329     //do some accounting and update
 330     if (Update())
 331       success = kTRUE;
 332     else
 333       continue;
 334   }
 335   fTimeStamp=pEvent->GetTimeStamp(); //always update timestamp even when no update performed
 336   fRunNumber=pEvent->GetRunNumber();
 337   return success;
 338 }
 339
 340 //______________________________________________________________________________
 341 void AliRelAlignerKalman::SetPoint2Track( Bool_t set )
 342 {
 343   fNMeasurementParams = (set)?2:4;
 344   if (fPH) delete fPH;
 345   fPH = new TMatrixD( fNMeasurementParams, fgkNSystemParams );
 346   if (fPMeasurement) delete fPMeasurement;
 347   fPMeasurement = new TVectorD( fNMeasurementParams );
 348   if (fPMeasurementCov) delete fPMeasurementCov;
 349   fPMeasurementCov = new TMatrixDSym( fNMeasurementParams );
 350   if (fPMeasurementPrediction) delete fPMeasurementPrediction;
 351   fPMeasurementPrediction = new TVectorD( fNMeasurementParams );
 352   fYZOnly = set;
 353 }
 354
 355 //______________________________________________________________________________
 356 void AliRelAlignerKalman::Print(Option_t*) const
 357 {
 358   //Print some useful info
 359   Double_t rad2deg = 180./TMath::Pi();
 360   printf("\nAliRelAlignerKalman\n");
 361   if (fCorrectionMode) printf("(Correction mode)\n");
 362   printf("  %i inputs, %i updates, %i outliers, %i merges, %i failed merges\n", fNTracks, fNUpdates, fNOutliers, fNMerges, fNMergesFailed );
 363   printf("  psi(x):           % .3f ± (%.2f) mrad  |  % .3f ± (%.2f) deg\n",1e3*(*fPX)(0), 1e3*TMath::Sqrt((*fPXcov)(0,0)),(*fPX)(0)*rad2deg,TMath::Sqrt((*fPXcov)(0,0))*rad2deg);
 364   printf("  theta(y):         % .3f ± (%.2f) mrad  |  % .3f ± (%.2f) deg\n",1e3*(*fPX)(1), 1e3*TMath::Sqrt((*fPXcov)(1,1)),(*fPX)(1)*rad2deg,TMath::Sqrt((*fPXcov)(1,1))*rad2deg);
 365   printf("  phi(z):           % .3f ± (%.2f) mrad  |  % .3f ± (%.2f) deg\n",1e3*(*fPX)(2), 1e3*TMath::Sqrt((*fPXcov)(2,2)),(*fPX)(2)*rad2deg,TMath::Sqrt((*fPXcov)(2,2))*rad2deg);
 366   printf("  x:                % .3f ± (%.2f) micron\n", 1e4*(*fPX)(3), 1e4*TMath::Sqrt((*fPXcov)(3,3)));
 367   printf("  y:                % .3f ± (%.2f) micron\n", 1e4*(*fPX)(4), 1e4*TMath::Sqrt((*fPXcov)(4,4)));
 368   printf("  z:                % .3f ± (%.2f) micron\n", 1e4*(*fPX)(5), 1e4*TMath::Sqrt((*fPXcov)(5,5)));
 369   if (fgkNSystemParams>6) printf("  vd corr           % .5g ± (%.2g)    [ vd should be %.4g (was %.4g in reco) ]\n", (*fPX)(6), TMath::Sqrt((*fPXcov)(6,6)), (*fPX)(6)*fTPCvd, fTPCvd);
 370   if (fgkNSystemParams>7) printf("  t0                % .5g ± (%.2g) us  |  %.4g ± (%.2g) cm     [ t0_real = t0_rec+t0 ]\n",(*fPX)(7), TMath::Sqrt((*fPXcov)(7,7)), fTPCvd*(*fPX)(7), fTPCvd*TMath::Sqrt((*fPXcov)(7,7)));
 371   if (fgkNSystemParams>8) printf("  vd/dy             % .5f ± (%.2f) (cm/us)/m\n", (*fPX)(8), TMath::Sqrt((*fPXcov)(8,8)));
 372   printf("  run: %i, timestamp: %i, magfield: %.3f\n", fRunNumber, fTimeStamp, fMagField);
 373   printf("\n");
 374   return;
 375 }
 376
 377 //______________________________________________________________________________
 378 void AliRelAlignerKalman::PrintSystemMatrix()
 379 {
 380   //Print the system matrix for this measurement
 381   printf("Kalman system matrix:\n");
 382   for ( Int_t i=0; i<fNMeasurementParams; i++ )
 383   {
 384     for ( Int_t j=0; j<fgkNSystemParams; j++ )
 385     {
 386       printf("% -2.2f  ", (*fPH)(i,j) );
 387     }//for i
 388     printf("\n");
 389   }//for j
 390   printf("\n");
 391   return;
 392 }
 393
 394 //______________________________________________________________________________
 395 Bool_t AliRelAlignerKalman::SetTrackParams( const AliExternalTrackParam* exparam1, const AliExternalTrackParam* exparam2 )
 396 {
 397   //Set the parameters, exparam1 will normally be ITS and exparam 2 tht TPC
 398
 399   fPTrackParamArr1[fTrackInBuffer] = *exparam1;
 400   fPTrackParamArr2[fTrackInBuffer] = *exparam2;
 401
 402   fTrackInBuffer++;
 403
 404   if (fTrackInBuffer == fgkNTracksPerMeasurement)
 405   {
 406     fTrackInBuffer = 0;
 407     return kTRUE;
 408   }
 409   return kFALSE;
 410 }
 411
 412 //______________________________________________________________________________
 413 Bool_t AliRelAlignerKalman::Update()
 414 {
 415   //perform the update
 416
 417   //if (fCalibrationMode) return UpdateCalibration();
 418   //if (fFillHistograms)
 419   //{
 420   //  if (!UpdateEstimateKalman()) return kFALSE;
 421   //  return UpdateCalibration(); //Update histograms only when update ok.
 422   //}
 423   //else return UpdateEstimateKalman();
 424   if (!PrepareMeasurement()) return kFALSE;
 425   if (!PrepareSystemMatrix()) return kFALSE;
 426   if (!PreparePrediction()) return kFALSE;
 427   return UpdateEstimateKalman();
 428 }
 429
 430 //______________________________________________________________________________
 431 void AliRelAlignerKalman::RotMat( TMatrixD &R, const TVectorD& angles )
 432 {
 433   //Get Rotation matrix R given the Cardan angles psi, theta, phi (around x, y, z).
 434   Double_t sinpsi = TMath::Sin(angles(0));
 435   Double_t sintheta = TMath::Sin(angles(1));
 436   Double_t sinphi = TMath::Sin(angles(2));
 437   Double_t cospsi = TMath::Cos(angles(0));
 438   Double_t costheta = TMath::Cos(angles(1));
 439   Double_t cosphi = TMath::Cos(angles(2));
 440
 441   R(0,0) = costheta*cosphi;
 442   R(0,1) = -costheta*sinphi;
 443   R(0,2) = sintheta;
 444   R(1,0) = sinpsi*sintheta*cosphi + cospsi*sinphi;
 445   R(1,1) = -sinpsi*sintheta*sinphi + cospsi*cosphi;
 446   R(1,2) = -costheta*sinpsi;
 447   R(2,0) = -cospsi*sintheta*cosphi + sinpsi*sinphi;
 448   R(2,1) = cospsi*sintheta*sinphi + sinpsi*cosphi;
 449   R(2,2) = costheta*cospsi;
 450 }
 451
 452 //______________________________________________________________________________
 453 Bool_t AliRelAlignerKalman::PrepareMeasurement()
 454 {
 455   //Calculate the residuals and their covariance matrix
 456
 457   for (Int_t i=0;i<fgkNTracksPerMeasurement;i++)
 458   {
 459     const Double_t* pararr1 = fPTrackParamArr1[i].GetParameter();
 460     const Double_t* pararr2 = fPTrackParamArr2[i].GetParameter();
 461
 462     //Take the track parameters and calculate the input to the Kalman filter
 463     Int_t x = i*4;
 464     (*fPMeasurement)(x+0) = pararr2[0]-pararr1[0];
 465     (*fPMeasurement)(x+1) = pararr2[1]-pararr1[1];
 466     if (!fYZOnly)
 467     {
 468       (*fPMeasurement)(x+2) = pararr2[2]-pararr1[2];
 469       (*fPMeasurement)(x+3) = pararr2[3]-pararr1[3];
 470     }
 471
 472     //the covariance
 473     const Double_t* parcovarr1 = fPTrackParamArr1[i].GetCovariance();
 474     const Double_t* parcovarr2 = fPTrackParamArr2[i].GetCovariance();
 475     (*fPMeasurementCov)(x+0,x+0)=parcovarr1[0];
 476     (*fPMeasurementCov)(x+0,x+1)=parcovarr1[1];
 477     (*fPMeasurementCov)(x+1,x+0)=parcovarr1[1];
 478     (*fPMeasurementCov)(x+1,x+1)=parcovarr1[2];
 479     (*fPMeasurementCov)(x+0,x+0)+=parcovarr2[0];
 480     (*fPMeasurementCov)(x+0,x+1)+=parcovarr2[1];
 481     (*fPMeasurementCov)(x+1,x+0)+=parcovarr2[1];
 482     (*fPMeasurementCov)(x+1,x+1)+=parcovarr2[2];
 483     if (!fYZOnly)
 484     {
 485       (*fPMeasurementCov)(x+0,x+2)=parcovarr1[3];
 486       (*fPMeasurementCov)(x+0,x+3)=parcovarr1[6];
 487       (*fPMeasurementCov)(x+1,x+2)=parcovarr1[4];
 488       (*fPMeasurementCov)(x+1,x+3)=parcovarr1[7];
 489       (*fPMeasurementCov)(x+2,x+0)=parcovarr1[3];
 490       (*fPMeasurementCov)(x+2,x+1)=parcovarr1[4];
 491       (*fPMeasurementCov)(x+2,x+2)=parcovarr1[5];
 492       (*fPMeasurementCov)(x+2,x+3)=parcovarr1[8];
 493       (*fPMeasurementCov)(x+3,x+0)=parcovarr1[6];
 494       (*fPMeasurementCov)(x+3,x+1)=parcovarr1[7];
 495       (*fPMeasurementCov)(x+3,x+2)=parcovarr1[8];
 496       (*fPMeasurementCov)(x+3,x+3)=parcovarr1[9];
 497       (*fPMeasurementCov)(x+0,x+2)+=parcovarr2[3];
 498       (*fPMeasurementCov)(x+0,x+3)+=parcovarr2[6];
 499       (*fPMeasurementCov)(x+1,x+2)+=parcovarr2[4];
 500       (*fPMeasurementCov)(x+1,x+3)+=parcovarr2[7];
 501       (*fPMeasurementCov)(x+2,x+0)+=parcovarr2[3];
 502       (*fPMeasurementCov)(x+2,x+1)+=parcovarr2[4];
 503       (*fPMeasurementCov)(x+2,x+2)+=parcovarr2[5];
 504       (*fPMeasurementCov)(x+2,x+3)+=parcovarr2[8];
 505       (*fPMeasurementCov)(x+3,x+0)+=parcovarr2[6];
 506       (*fPMeasurementCov)(x+3,x+1)+=parcovarr2[7];
 507       (*fPMeasurementCov)(x+3,x+2)+=parcovarr2[8];
 508       (*fPMeasurementCov)(x+3,x+3)+=parcovarr2[9];
 509     }
 510
 511     fNTracks++; //count added track sets
 512   }
 513   //if (fApplyCovarianceCorrection)
 514   //  *fPMeasurementCov += *fPMeasurementCovCorr;
 515   return kTRUE;
 516 }
 517
 518 //______________________________________________________________________________
 519 Bool_t AliRelAlignerKalman::PrepareSystemMatrix()
 520 {
 521   //Calculate the system matrix for the Kalman filter
 522   //approximate the system using as reference the track in the first volume
 523
 524   TVectorD z1( fNMeasurementParams );
 525   TVectorD z2( fNMeasurementParams );
 526   TVectorD x1( fgkNSystemParams );
 527   TVectorD x2( fgkNSystemParams );
 528   //get the derivatives
 529   for ( Int_t i=0; i<fgkNSystemParams; i++ )
 530   {
 531     x1 = *fPX;
 532     x2 = *fPX;
 533     x1(i) = x1(i) - fDelta[i]/(2.0);
 534     x2(i) = x2(i) + fDelta[i]/(2.0);
 535     if (!PredictMeasurement( z1, x1 )) return kFALSE;
 536     if (!PredictMeasurement( z2, x2 )) return kFALSE;
 537     for (Int_t j=0; j<fNMeasurementParams; j++ )
 538     {
 539       (*fPH)(j,i) = ( z2(j)-z1(j) ) / fDelta[i];
 540     }
 541   }
 542   return kTRUE;
 543 }
 544
 545 //______________________________________________________________________________
 546 Bool_t AliRelAlignerKalman::PreparePrediction()
 547 {
 548   //Prepare the prediction of the measurement using state vector
 549   return PredictMeasurement( (*fPMeasurementPrediction), (*fPX) );
 550 }
 551
 552 //______________________________________________________________________________
 553 Bool_t AliRelAlignerKalman::PredictMeasurement( TVectorD& pred, const TVectorD& state )
 554 {
 555   // Implements a system model for the Kalman fit
 556   // pred is [dy,dz,dsinphi,dtgl]
 557   // state is [psi,theta,phi,x,y,z,driftTPC,offsetTPC]
 558   // note: the measurement is in a local frame, so the prediction also has to be
 559   // note: state is the misalignment in global reference system
 560
 561   if (fCorrectionMode)
 562   {
 563     for (Int_t i=0;i<fgkNTracksPerMeasurement;i++)
 564     {
 565       AliExternalTrackParam track(fPTrackParamArr2[i]); //make a copy track
 566       if (!CorrectTrack( &track, state )) return kFALSE; //predict what the ideal track would be by applying correction
 567
 568       const Double_t* oldparam = fPTrackParamArr2[i].GetParameter();
 569       const Double_t* newparam = track.GetParameter();
 570
 571       Int_t x = 4*i;
 572       //calculate the predicted residual
 573       pred(x+0) = oldparam[0] - newparam[0];
 574       pred(x+1) = oldparam[1] - newparam[1];
 575       if (!fYZOnly)
 576       {
 577         pred(x+2) = oldparam[2] - newparam[2];
 578         pred(x+3) = oldparam[3] - newparam[3];
 579       }
 580       return kTRUE;
 581     }
 582   }
 583   else
 584   {
 585     for (Int_t i=0;i<fgkNTracksPerMeasurement;i++)
 586     {
 587       AliExternalTrackParam track(fPTrackParamArr1[i]); //make a copy track
 588       if (!MisalignTrack( &track, state )) return kFALSE; //predict what the measured track would be by applying misalignment
 589
 590       const Double_t* oldparam = fPTrackParamArr1[i].GetParameter();
 591       const Double_t* newparam = track.GetParameter();
 592
 593       Int_t x = 4*i;
 594       //calculate the predicted residual
 595       pred(x+0) = newparam[0] - oldparam[0];
 596       pred(x+1) = newparam[1] - oldparam[1];
 597       if (!fYZOnly)
 598       {
 599         pred(x+2) = newparam[2] - oldparam[2];
 600         pred(x+3) = newparam[3] - oldparam[3];
 601       }
 602       return kTRUE;
 603     }
 604   }
 605   return kFALSE;
 606 }
 607
 608 //______________________________________________________________________________
 609 Bool_t AliRelAlignerKalman::UpdateEstimateKalman()
 610 {
 611   //Kalman estimation of noisy constants: in the model A=1
 612   //The arguments are (following the usual convention):
 613   //  fPX - the state vector (parameters)
 614   //  fPXcov - the state covariance matrix (parameter errors)
 615   //  fPMeasurement - measurement vector
 616   //  fPMeasurementCov - measurement covariance matrix
 617   //  fPH - measurement model matrix ( fPMeasurement = Hx + v ) v being measurement noise (error fR)
 618
 619   TMatrixDSym identity(TMatrixDSym::kUnit, (*fPXcov));            //unit matrix
 620
 621   //predict the state
 622   //(*fPXcov) = (*fPXcov) + fQ*identity;  //add some process noise (diagonal)
 623
 624   // update prediction with measurement
 625   // calculate Kalman gain
 626   // K = PH/(HPH+fPMeasurementCov)
 627   TMatrixD pht( (*fPXcov), TMatrixD::kMultTranspose, (*fPH) );  //common factor (used twice)
 628   TMatrixD hpht( (*fPH), TMatrixD::kMult, pht );
 629   hpht += (*fPMeasurementCov);
 630
 631   //shit happens so protect yourself!
 632 //  if (fNumericalParanoia)
 633 //  {
 634 //    TDecompLU lu(hpht);
 635 //    if (lu.Condition() > 1e12) return kFALSE;
 636 //    lu.Invert(hpht);
 637 //  }
 638 //  else
 639 //  {
 640     Double_t det;
 641     hpht.InvertFast(&det); //since the matrix is small...
 642     if (det < 2e-55) return kFALSE; //we need some sort of protection even in this case....
 643 //  }
 644   //printf("KalmanUpdate: det(hpht): %.4g\n",det);
 645
 646   TMatrixD k(pht, TMatrixD::kMult, hpht ); //compute K (hpht is already inverted)
 647
 648   // update the state and its covariance matrix
 649   TVectorD xupdate(fgkNSystemParams);
 650   xupdate = k*((*fPMeasurement)-(*fPMeasurementPrediction));
 651
 652   //SIMPLE OUTLIER REJECTION
 653   if ( IsOutlier( xupdate, (*fPXcov) ) && fRejectOutliers )
 654   {
 655     fNOutliers++;
 656     return kFALSE;
 657   }
 658
 659   TMatrixD kh( k, TMatrixD::kMult, (*fPH) );
 660   TMatrixD ikh(fgkNSystemParams,fgkNSystemParams); //this is because for some reason TMatrixD::kAdd didn't work
 661   ikh = identity - kh;
 662   TMatrixD ikhp( ikh, TMatrixD::kMult, (*fPXcov) ); // (identity-KH)fPXcov
 663   if (!IsPositiveDefinite(ikhp)) return kFALSE;
 664
 665   (*fPX) += xupdate;
 666   TMatrixDSymFromTMatrixD( (*fPXcov), ikhp ); //make the matrix completely symetrical
 667
 668   fNUpdates++;
 669
 670   return kTRUE;
 671 }
 672
 673 //______________________________________________________________________________
 674 Bool_t AliRelAlignerKalman::IsOutlier( const TVectorD& update, const TMatrixDSym& covmatrix )
 675 {
 676   //check whether an update is an outlier given the covariance matrix of the fit
 677
 678   Bool_t is=kFALSE;
 679   for (Int_t i=0;i<fgkNSystemParams;i++)
 680   {
 681     if (covmatrix(i,i)<0.) return kTRUE; //if cov matrix has neg diagonals something went wrong
 682     is = (is) || (TMath::Abs(update(i)) > fOutRejSigmas*TMath::Sqrt((covmatrix)(i,i)));
 683   }
 684   return is;
 685 }
 686
 687 //______________________________________________________________________________
 688 Bool_t AliRelAlignerKalman::IsPositiveDefinite( const TMatrixD& mat ) const
 689 {
 690   //check for positive definiteness
 691
 692   for (Int_t i=0; i<mat.GetNcols(); i++)
 693   {
 694     if (mat(i,i)<=0.) return kFALSE;
 695   }
 696
 697   if (!fNumericalParanoia) return kTRUE;
 698
 699   TDecompLU lu(mat);
 700   return (lu.Decompose());
 701 }
 702
 703 //______________________________________________________________________________
 704 void AliRelAlignerKalman::TMatrixDSymFromTMatrixD( TMatrixDSym& matsym, const TMatrixD& mat )
 705 {
 706   //Produce a valid symmetric matrix out of an almost symmetric TMatrixD
 707
 708   for (Int_t i=0; i<mat.GetNcols(); i++)
 709   {
 710     matsym(i,i) = mat(i,i); //copy diagonal
 711     for (Int_t j=i+1; j<mat.GetNcols(); j++)
 712     {
 713       //copy the rest
 714       Double_t average = (mat(i,j)+mat(j,i))/2.;
 715       matsym(i,j)=average;
 716       matsym(j,i)=average;
 717     }
 718   }
 719   matsym.MakeValid();
 720   return;
 721 }
 722
 723 //______________________________________________________________________________
 724 void AliRelAlignerKalman::Angles( TVectorD &angles, const TMatrixD &rotmat )
 725 {
 726   //Calculate the Cardan angles (psi,theta,phi) from rotation matrix
 727   //b = R*a
 728   angles(0) = TMath::ATan2( -rotmat(1,2), rotmat(2,2) );
 729   angles(1) = TMath::ASin( rotmat(0,2) );
 730   angles(2) = TMath::ATan2( -rotmat(0,1), rotmat(0,0) );
 731   return;
 732 }
 733
 734 //______________________________________________________________________________
 735 void AliRelAlignerKalman::PrintCorrelationMatrix()
 736 {
 737   //Print the correlation matrix for the fitted parameters
 738   printf("Correlation matrix for system parameters:\n");
 739   for ( Int_t i=0; i<fgkNSystemParams; i++ )
 740   {
 741     for ( Int_t j=0; j<i+1; j++ )
 742     {
 743       if ((*fPXcov)(i,i)==0. || (*fPXcov)(j,j)==0.) printf("   NaN  ");
 744       else
 745         printf("% -1.3f  ", (*fPXcov)(i,j)/TMath::Sqrt( (*fPXcov)(i,i) * (*fPXcov)(j,j) ) );
 746     }//for j
 747     printf("\n");
 748   }//for i
 749   printf("\n");
 750   return;
 751 }
 752
 753 //______________________________________________________________________________
 754 Bool_t AliRelAlignerKalman::FindCosmicTrackletNumbersInEvent( TArrayI& outITSindexTArr, TArrayI& outTPCindexTArr, const AliESDEvent* pEvent )
 755 {
 756   //Find matching track segments in an event with tracks in TPC and ITS(standalone)
 757
 758   //Sanity cuts on tracks + check which tracks are ITS which are TPC
 759   Int_t ntracks = pEvent->GetNumberOfTracks(); ////printf("number of tracks in event: %i\n", ntracks);
 760   fMagField = pEvent->GetMagneticField();
 761   if (ntracks<2)
 762   {
 763     //printf("TrackFinder: less than 2 tracks!\n");
 764     return kFALSE;
 765   }
 766   Float_t* phiArr = new Float_t[ntracks];
 767   Float_t* thetaArr = new Float_t[ntracks];
 768   Int_t* goodtracksArr = new Int_t[ntracks];
 769   Int_t* candidateTPCtracksArr = new Int_t[ntracks];
 770   Int_t* matchedITStracksArr = new Int_t[ntracks];
 771   Int_t* matchedTPCtracksArr = new Int_t[ntracks];
 772   Int_t* tracksArrITS = new Int_t[ntracks];
 773   Int_t* tracksArrTPC = new Int_t[ntracks];
 774   Int_t* nPointsArr = new Int_t[ntracks];
 775   Int_t nITStracks = 0;
 776   Int_t nTPCtracks = 0;
 777   Int_t nGoodTracks = 0;
 778   Int_t nCandidateTPCtracks = 0;
 779   Int_t nMatchedITStracks = 0;
 780   AliESDtrack* pTrack = NULL;
 781   Bool_t foundMatchTPC = kFALSE;
 782
 783   //select and clasify tracks
 784   for (Int_t itrack=0; itrack < ntracks; itrack++)
 785   {
 786     pTrack = pEvent->GetTrack(itrack);
 787     if (!pTrack)
 788     {
 789       //std::cout<<"no track!"<<std::endl;
 790       continue;
 791     }
 792     if (fCuts)
 793     {
 794       if (pTrack->GetP()<fMinMom || pTrack->GetP()>fMaxMom) continue;
 795     }
 796     goodtracksArr[nGoodTracks]=itrack;
 797     Float_t phi = pTrack->GetAlpha()+TMath::ASin(pTrack->GetSnp());
 798     Float_t theta = 0.5*TMath::Pi()-TMath::ATan(pTrack->GetTgl());
 799     phiArr[nGoodTracks]=phi;
 800     thetaArr[nGoodTracks]=theta;
 801
 802     //check if track is ITS
 803     Int_t nClsITS = pTrack->GetNcls(0);
 804     Int_t nClsTPC = pTrack->GetNcls(1);
 805     if ( ((pTrack->GetStatus()&AliESDtrack::kITSout)>0)&&
 806          !((pTrack->GetStatus()&AliESDtrack::kTPCin)>0)&&
 807          !(nClsITS<fMinPointsVol1) )  //enough points
 808     {
 809       tracksArrITS[nITStracks] = nGoodTracks;
 810       nITStracks++;
 811       nGoodTracks++;
 812       continue;
 813     }
 814
 815     //check if track is TPC
 816     if ( ((pTrack->GetStatus()&AliESDtrack::kTPCin)>0)&&
 817          !(nClsTPC<fMinPointsVol2) )  //enough points
 818     {
 819       tracksArrTPC[nTPCtracks] = nGoodTracks;
 820       nTPCtracks++;
 821       nGoodTracks++;
 822       //printf("tracksArrTPC[%d]=%d, goodtracksArr[%d]=%d\n",nTPCtracks-1,tracksArrTPC[nTPCtracks-1],nGoodTracks-1,goodtracksArr[nGoodTracks-1]);
 823       continue;
 824     }
 825   }//for itrack   -   selection fo tracks
 826
 827   //printf("TrackFinder: %d ITS | %d TPC out of %d tracks in event\n", nITStracks,nTPCtracks,ntracks);
 828
 829   if ( nITStracks < 1 || nTPCtracks < 1 )
 830   {
 831     delete [] phiArr;
 832     delete [] thetaArr;
 833     delete [] goodtracksArr;
 834     delete [] matchedITStracksArr;
 835     delete [] candidateTPCtracksArr;
 836     delete [] matchedTPCtracksArr;
 837     delete [] tracksArrITS;
 838     delete [] tracksArrTPC;
 839     delete [] nPointsArr;
 840     return kFALSE;
 841   }
 842
 843   //find matching in TPC
 844   if (nTPCtracks>1)  //if there is something to be matched, try and match it
 845   {
 846     Float_t min = 10000000.;
 847     for (Int_t itr1=0; itr1<nTPCtracks; itr1++)
 848     {
 849       for (Int_t itr2=itr1+1; itr2<nTPCtracks; itr2++)
 850       {
 851         Float_t deltatheta = TMath::Abs(TMath::Pi()-thetaArr[tracksArrTPC[itr1]]-thetaArr[tracksArrTPC[itr2]]);
 852         if (deltatheta > fMaxMatchingAngle) continue;
 853         Float_t deltaphi = TMath::Abs(TMath::Abs(phiArr[tracksArrTPC[itr1]]-phiArr[tracksArrTPC[itr2]])-TMath::Pi());
 854         if (deltaphi > fMaxMatchingAngle) continue;
 855         if (deltatheta+deltaphi<min) //only the best matching pair
 856         {
 857           min=deltatheta+deltaphi;
 858           candidateTPCtracksArr[0] = tracksArrTPC[itr1];  //store the index of track in goodtracksArr[]
 859           candidateTPCtracksArr[1] = tracksArrTPC[itr2];
 860           nCandidateTPCtracks = 2;
 861           foundMatchTPC = kTRUE;
 862           //printf("TrackFinder: Matching TPC tracks candidates:\n");
 863           //printf("TrackFinder: candidateTPCtracksArr[0]=%d\n",tracksArrTPC[itr1]);
 864           //printf("TrackFinder: candidateTPCtracksArr[1]=%d\n",tracksArrTPC[itr2]);
 865         }
 866       }
 867     }
 868   }//if nTPCtracks>1
 869   else //if nTPCtracks==1 - if nothing to match, take the only one we've got
 870   {
 871     candidateTPCtracksArr[0] = tracksArrTPC[0];
 872     nCandidateTPCtracks = 1;
 873     foundMatchTPC = kFALSE;
 874   }
 875   if (foundMatchTPC) fNMatchedTPCtracklets++;
 876   //if no match but the requirement is set return kFALSE
 877   if (fRequireMatchInTPC && !foundMatchTPC)
 878   {
 879     delete [] phiArr;
 880     delete [] thetaArr;
 881     delete [] goodtracksArr;
 882     delete [] candidateTPCtracksArr;
 883     delete [] matchedITStracksArr;
 884     delete [] matchedTPCtracksArr;
 885     delete [] tracksArrITS;
 886     delete [] tracksArrTPC;
 887     delete [] nPointsArr;
 888     //printf("TrackFinder: no match in TPC && required\n");
 889     return kFALSE;
 890   }
 891
 892   //if no match and more than one track take all TPC tracks
 893   if (!fRequireMatchInTPC && !foundMatchTPC)
 894   {
 895     for (Int_t i=0;i<nTPCtracks;i++)
 896     {
 897       candidateTPCtracksArr[i] = tracksArrTPC[i];
 898     }
 899     nCandidateTPCtracks = nTPCtracks;
 900   }
 901   //printf("TrackFinder: nCandidateTPCtracks: %i\n", nCandidateTPCtracks);
 902
 903   Double_t* minDifferenceArr = new Double_t[nCandidateTPCtracks];
 904
 905   //find ITS matches for good TPC tracks
 906   Bool_t matchedITStracks=kFALSE;
 907   for (Int_t itpc=0;itpc<nCandidateTPCtracks;itpc++)
 908   {
 909     minDifferenceArr[nMatchedITStracks] = 10000000.;
 910     matchedITStracks=kFALSE;
 911     for (Int_t iits=0; iits<nITStracks; iits++)
 912     {
 913       AliESDtrack* itstrack = pEvent->GetTrack(goodtracksArr[tracksArrITS[iits]]);
 914       const AliExternalTrackParam* parits = itstrack->GetOuterParam();
 915       AliESDtrack* tpctrack = pEvent->GetTrack(goodtracksArr[candidateTPCtracksArr[itpc]]);
 916       const AliExternalTrackParam* tmp = tpctrack->GetInnerParam();
 917       AliExternalTrackParam partpc(*tmp);  //make a copy to avoid tampering with original params
 918       partpc.Rotate(parits->GetAlpha());
 919       partpc.PropagateTo(parits->GetX(),fMagField);
 920       Float_t dtgl = TMath::Abs(partpc.GetTgl()-parits->GetTgl());
 921       if (dtgl > fMaxMatchingAngle) continue;
 922       Float_t dsnp = TMath::Abs(partpc.GetSnp()-parits->GetSnp());
 923       if (dsnp > fMaxMatchingAngle) continue;
 924       Float_t dy = TMath::Abs(partpc.GetY()-parits->GetY());
 925       Float_t dz = TMath::Abs(partpc.GetZ()-parits->GetZ());
 926       if (TMath::Sqrt(dy*dy+dz*dz) > fMaxMatchingDistance) continue;
 927       if (dtgl+dsnp<minDifferenceArr[nMatchedITStracks]) //only the best matching pair
 928       {
 929         minDifferenceArr[nMatchedITStracks]=dtgl+dsnp;
 930         matchedITStracksArr[nMatchedITStracks] = tracksArrITS[iits];
 931         matchedTPCtracksArr[nMatchedITStracks] = candidateTPCtracksArr[itpc]; //this tells us minDifferenceArrwhich TPC track this ITS track belongs to
 932         //printf("TrackFinder: Matching ITS to TPC:\n");
 933         //printf("TrackFinder: minDifferenceArr[%i]=%.2f\n",nMatchedITStracks,minDifferenceArr[nMatchedITStracks]);
 934         //printf("TrackFinder: matchedITStracksArr[%i]=%i\n",nMatchedITStracks,matchedITStracksArr[nMatchedITStracks]);
 935         //printf("TrackFinder: matchedTPCtracksArr[%i]=%i\n",nMatchedITStracks,matchedTPCtracksArr[nMatchedITStracks]);
 936         matchedITStracks=kTRUE;;
 937       }
 938     }
 939     if (matchedITStracks) nMatchedITStracks++;
 940   }
 941
 942   if (nMatchedITStracks==0) //no match in ITS
 943   {
 944     delete [] phiArr;
 945     delete [] thetaArr;
 946     delete [] minDifferenceArr;
 947     delete [] goodtracksArr;
 948     delete [] matchedITStracksArr;
 949     delete [] candidateTPCtracksArr;
 950     delete [] matchedTPCtracksArr;
 951     delete [] tracksArrITS;
 952     delete [] tracksArrTPC;
 953     delete [] nPointsArr;
 954     //printf("TrackFinder: No match in ITS\n");
 955     return kFALSE;
 956   }
 957
 958   //printf("TrackFinder: nMatchedITStracks: %i\n",nMatchedITStracks);
 959   //we found a cosmic
 960   fNMatchedCosmics++;
 961
 962   //Now we may have ended up with more matches than we want in the case there was
 963   //no TPC match and there were many TPC tracks
 964   //a cosmic in a scenario like this will only have produced 1 pair, the rest is garbage
 965   //so take only the best pair.
 966   //same applies when there are more matches than ITS tracks - means that one ITS track
 967   //matches more TPC tracks.
 968   if ((nMatchedITStracks>2 && !foundMatchTPC) || nMatchedITStracks>nITStracks)
 969   {
 970     Int_t imin = TMath::LocMin(nMatchedITStracks,minDifferenceArr);
 971     matchedITStracksArr[0] = matchedITStracksArr[imin];
 972     matchedTPCtracksArr[0] = matchedTPCtracksArr[imin];
 973     nMatchedITStracks = 1;
 974     //printf("TrackFinder: too many matches - take only the best one\n");
 975     //printf("TrackFinder: LocMin in matched its tracks: %d\n",imin);
 976     //printf("TrackFinder: matchedITStracksArr[0]=%d\n",matchedITStracksArr[0]);
 977     //printf("TrackFinder: matchedTPCtracksArr[0]=%d\n",matchedTPCtracksArr[0]);
 978   }
 979
 980   ///////////////////////////////////////////////////////////////////////////
 981   outITSindexTArr.Set(nMatchedITStracks);
 982   outTPCindexTArr.Set(nMatchedITStracks);
 983   for (Int_t i=0;i<nMatchedITStracks;i++)
 984   {
 985     outITSindexTArr.AddAt( goodtracksArr[matchedITStracksArr[i]], i );
 986     outTPCindexTArr.AddAt( goodtracksArr[matchedTPCtracksArr[i]], i );
 987     //printf("TrackFinder: Fill the output\n");
 988     //printf("TrackFinder: matchedITStracksArr[%d]=%d\n",i,matchedITStracksArr[i]);
 989     //printf("TrackFinder: matchedTPCtracksArr[%d]=%d\n",i,matchedTPCtracksArr[i]);
 990   }
 991   //printf("TrackFinder: Size of outputarrays: %d, %d\n", outITSindexTArr.GetSize(), outTPCindexTArr.GetSize());
 992   ///////////////////////////////////////////////////////////////////////////
 993
 994   delete [] phiArr;
 995   delete [] thetaArr;
 996   delete [] minDifferenceArr;
 997   delete [] goodtracksArr;
 998   delete [] candidateTPCtracksArr;
 999   delete [] matchedITStracksArr;
1000   delete [] matchedTPCtracksArr;
1001   delete [] tracksArrITS;
1002   delete [] tracksArrTPC;
1003   delete [] nPointsArr;
1004   return kTRUE;
1005 }
1006
1007 //______________________________________________________________________________
1008 Bool_t AliRelAlignerKalman::CorrectTrack( AliExternalTrackParam* tr, const TVectorD& misal ) const
1009 {
1010   //implements the system model -
1011   //applies correction for misalignment and calibration to track
1012   //track needs to be already propagated to the global reference plane
1013
1014   Double_t x = tr->GetX();
1015   Double_t alpha = tr->GetAlpha();
1016   Double_t point[3],dir[3];
1017   tr->GetXYZ(point);
1018   tr->GetDirection(dir);
1019   TVector3 Point(point);
1020   TVector3 Dir(dir);
1021
1022   //Apply corrections to track
1023
1024   //Shift
1025   Point(0) -= misal(3); //add shift in x
1026   Point(1) -= misal(4); //add shift in y
1027   Point(2) -= misal(5); //add shift in z
1028   //Rotation
1029   TMatrixD rotmat(3,3);
1030   RotMat( rotmat, misal );
1031   Point = rotmat.T() * Point;
1032   Dir = rotmat * Dir;
1033
1034   //TPC vdrift and T0 corrections
1035   TVector3 Point2; //second point of the track
1036   Point2 = Point + Dir;
1037   Double_t vdCorr = 1./misal(6);
1038   Double_t t0 = misal(7);
1039   Double_t vdY = 0.0;
1040   if (fgkNSystemParams>8) vdY = misal(8)/100.; //change over 100cm.
1041
1042   //my model
1043   if (Point(2)>0)
1044   {
1045     //A-Side
1046     Point(2) = Point(2)   - (fTPCZLengthA-Point(2))  * (vdCorr-1.+vdY*Point(1)/fTPCvd)  - (fTPCvd*vdCorr+vdY*Point(1))*t0;
1047     Point2(2) = Point2(2) - (fTPCZLengthA-Point2(2)) * (vdCorr-1.+vdY*Point2(1)/fTPCvd) - (fTPCvd*vdCorr+vdY*Point2(1))*t0;
1048   }
1049   else
1050   {
1051     //C-side
1052     Point(2) = Point(2)   - (fTPCZLengthC+Point(2))  * (1.-vdCorr-vdY*Point(1)/fTPCvd)  + (fTPCvd*vdCorr+vdY*Point(1))*t0;
1053     Point2(2) = Point2(2) - (fTPCZLengthC+Point2(2)) * (1.-vdCorr-vdY*Point2(1)/fTPCvd) + (fTPCvd*vdCorr+vdY*Point2(1))*t0;
1054   }
1055
1056   //Stefan's model
1057   //if (Point(2)>0)
1058   //{
1059   //  //A-Side
1060   //  Point(2) = Point(2)   - (fTPCZLengthA-Point(2))  * (1.-vdCorr+vdY*Point(1)/fTPCvd)  - (fTPCvd*vdCorr+vdY*Point(1))*t0;
1061   //  Point2(2) = Point2(2) - (fTPCZLengthA-Point2(2)) * (1.-vdCorr+vdY*Point2(1)/fTPCvd) - (fTPCvd*vdCorr+vdY*Point2(1))*t0;
1062   //}
1063   //else
1064   //{
1065   //  //C-side
1066   //  Point(2) = Point(2)   + (fTPCZLengthC+Point(2))  * (1.-vdCorr+vdY*Point(1)/fTPCvd)  + (fTPCvd*vdCorr+vdY*Point(1))*t0;
1067   //  Point2(2) = Point2(2) + (fTPCZLengthC+Point2(2)) * (1.-vdCorr+vdY*Point2(1)/fTPCvd) + (fTPCvd*vdCorr+vdY*Point2(1))*t0;
1068   //}
1069
1070   Dir = Point2-Point;
1071   Dir=Dir.Unit(); //keep unit length
1072
1073   //Turn back to local system
1074   Dir.GetXYZ(dir);
1075   Point.GetXYZ(point);
1076   tr->Global2LocalPosition(point,alpha);
1077   tr->Global2LocalPosition(dir,alpha);
1078
1079   //Calculate new intersection point with ref plane
1080   Double_t p[5],pcov[15];
1081   if (dir[0]==0.0) return kFALSE;
1082   Double_t s=(x-point[0])/dir[0];
1083   p[0] = point[1]+s*dir[1];
1084   p[1] = point[2]+s*dir[2];
1085   Double_t pt = TMath::Sqrt(dir[0]*dir[0]+dir[1]*dir[1]);
1086   if (pt==0.0) return kFALSE;
1087   p[2] = dir[1]/pt;
1088   p[3] = dir[2]/pt;
1089   //insert everything back into track
1090   const Double_t* pcovtmp = tr->GetCovariance();
1091   p[4] = tr->GetSigned1Pt(); //copy the momentum
1092   memcpy(pcov,pcovtmp,15*sizeof(Double_t));
1093   tr->Set(x,alpha,p,pcov);
1094   return kTRUE;
1095
1096   ////put params back into track and propagate to ref
1097   //Double_t p[5],pcov[15];
1098   //p[0] = point[1];
1099   //p[1] = point[2];
1100   //Double_t xnew = point[0];
1101   //Double_t pt = TMath::Sqrt(dir[0]*dir[0]+dir[1]*dir[1]);
1102   //if (pt==0.0) return kFALSE;
1103   //p[2] = dir[1]/pt;
1104   //p[3] = dir[2]/pt;
1105   //p[4] = tr->GetSigned1Pt(); //copy the momentum
1106   //const Double_t* pcovtmp = tr->GetCovariance();
1107   //memcpy(pcov,pcovtmp,15*sizeof(Double_t));
1108   //tr->Set(xnew,alpha,p,pcov);
1109   //return tr->PropagateTo(x,fMagField);
1110 }
1111
1112 //______________________________________________________________________________
1113 Bool_t AliRelAlignerKalman::MisalignTrack( AliExternalTrackParam* tr, const TVectorD& misal ) const
1114 {
1115   //implements the system model -
1116   //applies misalignment and miscalibration to reference track
1117   //trackparams have to be at the global reference plane
1118
1119   Double_t x = tr->GetX();
1120   Double_t alpha = tr->GetAlpha();
1121   Double_t point[3],dir[3];
1122   tr->GetXYZ(point);
1123   tr->GetDirection(dir);
1124   TVector3 Point(point);
1125   TVector3 Dir(dir);
1126
1127   //Apply misalignment to track
1128
1129   //TPC vdrift and T0 corrections
1130   TVector3 Point2; //second point of the track
1131   Point2 = Point + Dir;
1132   Double_t vdCorr = 1./misal(6);
1133   Double_t t0 = misal(7);
1134   Double_t vdY = 0.0;
1135   if (fgkNSystemParams>8) vdY = misal(8)/100.; //change over 100cm.
1136
1137   if (Point(2)>0)
1138   {
1139     //A-Side
1140     Point(2) = Point(2)   + ((fTPCZLengthA-Point(2))/(vdCorr*fTPCvd+vdY*Point(1)))
1141                           * (fTPCvd*(vdCorr-1.)+vdY*Point(1)) + fTPCvd*t0;
1142     Point2(2) = Point2(2) + ((fTPCZLengthA-Point2(2))/(vdCorr*fTPCvd+vdY*Point2(1)))
1143                           * (fTPCvd*(vdCorr-1.)+vdY*Point2(1)) + fTPCvd*t0;
1144   }
1145   else
1146   {
1147     //C-side
1148     Point(2) = Point(2)   + (fTPCZLengthC+Point(2))/(vdCorr*fTPCvd+vdY*Point(1))
1149                           * (fTPCvd*(1.-vdCorr)-vdY*Point(1)) - fTPCvd*t0;
1150     Point2(2) = Point2(2) + (fTPCZLengthC+Point2(2))/(vdCorr*fTPCvd+vdY*Point2(1))
1151                           * (fTPCvd*(1.-vdCorr)-vdY*Point2(1)) - fTPCvd*t0;
1152   }
1153   Dir = Point2-Point;
1154   Dir=Dir.Unit(); //keep unit length
1155
1156   //Rotation
1157   TMatrixD rotmat(3,3);
1158   RotMat( rotmat, misal );
1159   Point = rotmat * Point;
1160   Dir = rotmat * Dir;
1161   //Shift
1162   Point(0) += misal(3); //add shift in x
1163   Point(1) += misal(4); //add shift in y
1164   Point(2) += misal(5); //add shift in z
1165
1166   //Turn back to local system
1167   Dir.GetXYZ(dir);
1168   Point.GetXYZ(point);
1169   tr->Global2LocalPosition(point,alpha);
1170   tr->Global2LocalPosition(dir,alpha);
1171
1172   //Calculate new intersection point with ref plane
1173   Double_t p[5],pcov[15];
1174   if (dir[0]==0.0) return kFALSE;
1175   Double_t s=(x-point[0])/dir[0];
1176   p[0] = point[1]+s*dir[1];
1177   p[1] = point[2]+s*dir[2];
1178   Double_t pt = TMath::Sqrt(dir[0]*dir[0]+dir[1]*dir[1]);
1179   if (pt==0.0) return kFALSE;
1180   p[2] = dir[1]/pt;
1181   p[3] = dir[2]/pt;
1182   //insert everything back into track
1183   const Double_t* pcovtmp = tr->GetCovariance();
1184   p[4] = tr->GetSigned1Pt(); //copy the momentum
1185   memcpy(pcov,pcovtmp,15*sizeof(Double_t));
1186   tr->Set(x,alpha,p,pcov);
1187   return kTRUE;
1188
1189   ////put params back into track and propagate to ref
1190   //Double_t p[5];
1191   //Double_t pcov[15];
1192   //p[0] = point[1];
1193   //p[1] = point[2];
1194   //Double_t xnew = point[0];
1195   //Double_t pt = TMath::Sqrt(dir[0]*dir[0]+dir[1]*dir[1]);
1196   //if (pt==0.0) return kFALSE;
1197   //p[2] = dir[1]/pt;
1198   //p[3] = dir[2]/pt;
1199   //p[4] = tr->GetSigned1Pt(); //copy the momentum
1200   //const Double_t* pcovtmp = tr->GetCovariance();
1201   //memcpy(pcov,pcovtmp,15*sizeof(Double_t));
1202   //printf("x before: %.5f, after: %.5f\n",x, xnew);
1203   //printf("before: %.4f %.4f %.4f %.4f %.4f \n",tr->GetParameter()[0],tr->GetParameter()[1],tr->GetParameter()[2],tr->GetParameter()[3],tr->GetParameter()[4]);
1204   //printf("after:  %.4f %.4f %.4f %.4f %.4f \n",p[0],p[1],p[2],p[3],p[4]);
1205   //tr->Set(xnew,alpha,p,pcov);
1206   //return tr->PropagateTo(x,fMagField);
1207 }
1208
1209 //______________________________________________________________________________
1210 void AliRelAlignerKalman::Reset()
1211 {
1212   //full reset to defaults
1213   fPX->Zero();
1214   (*fPX)(6)=1.;
1215   ResetCovariance();
1216
1217   //initialize the differentials per parameter
1218   for (Int_t i=0;i<fgkNSystemParams;i++) fDelta[i] = 1.e-6;
1219
1220   fNMatchedCosmics=0;
1221   fNMatchedTPCtracklets=0;
1222   fNUpdates=0;
1223   fNOutliers=0;
1224   fNTracks=0;
1225   fNProcessedEvents=0;
1226   fRunNumber=0;
1227   fTimeStamp=0;
1228 }
1229
1230 //______________________________________________________________________________
1231 void AliRelAlignerKalman::ResetCovariance( const Double_t number )
1232 {
1233   //Resets the covariance to the default if arg=0 or resets the off diagonals
1234   //to zero and releases the diagonals by factor arg.
1235   if (number!=0.)
1236   {
1237     for (Int_t z=0;z<6;z++)
1238     {
1239       for (Int_t zz=0;zz<6;zz++)
1240       {
1241         if (zz==z) continue; //don't touch diagonals
1242         (*fPXcov)(zz,z) = 0.;
1243         (*fPXcov)(z,zz) = 0.;
1244       }
1245       (*fPXcov)(z,z) = (*fPXcov)(z,z) * number;
1246     }
1247   }
1248   else
1249   {
1250     //Resets the covariance of the fit to a default value
1251     fPXcov->Zero();
1252     (*fPXcov)(0,0) = .08*.08; //psi (rad)
1253     (*fPXcov)(1,1) = .08*.08; //theta (rad
1254     (*fPXcov)(2,2) = .08*.08; //phi (rad)
1255     (*fPXcov)(3,3) = .3*.3; //x (cm)
1256     (*fPXcov)(4,4) = .3*.3; //y (cm)
1257     (*fPXcov)(5,5) = .3*.3; //z (cm)
1258   }
1259   ResetTPCparamsCovariance(number);
1260 }
1261
1262 //______________________________________________________________________________
1263 void AliRelAlignerKalman::ResetTPCparamsCovariance( const Double_t number )
1264 {
1265   //Resets the covariance to the default if arg=0 or resets the off diagonals
1266   //to zero and releases the diagonals by factor arg.
1267
1268   //release diagonals
1269   if (number==0.)
1270   {
1271     if (fgkNSystemParams>6) (*fPXcov)(6,6) = .1*.1;
1272     if (fgkNSystemParams>7) (*fPXcov)(7,7) = 1.*1.;
1273     if (fgkNSystemParams>8) (*fPXcov)(8,8) = .1*.1;
1274   }
1275   else
1276   {
1277     if (fgkNSystemParams>6) (*fPXcov)(6,6) = number * (*fPXcov)(6,6);
1278     if (fgkNSystemParams>7) (*fPXcov)(7,7) = number * (*fPXcov)(7,7);
1279     if (fgkNSystemParams>8) (*fPXcov)(8,8) = number * (*fPXcov)(8,8);
1280   }
1281
1282   //set crossterms to zero
1283   for (Int_t i=0;i<fgkNSystemParams;i++)
1284   {
1285     for (Int_t j=6;j<fgkNSystemParams;j++) //TPC params
1286     {
1287       if (i==j) continue; //don't touch diagonals
1288       (*fPXcov)(i,j) = 0.;
1289       (*fPXcov)(j,i) = 0.;
1290     }
1291   }
1292 }
1293
1294 //______________________________________________________________________________
1295 Bool_t AliRelAlignerKalman::Merge( const AliRelAlignerKalman* al )
1296 {
1297   //Merge two aligners
1298
1299   if (!al) return kFALSE;
1300   if (al==this) return kTRUE;
1301   if (al->fNUpdates == 0) return kTRUE; //no point in merging with an empty one
1302
1303   //store the pointers to current stuff
1304   TVectorD* pmes = fPMeasurement;
1305   TMatrixDSym* pmescov = fPMeasurementCov;
1306   TVectorD* pmespred = fPMeasurementPrediction;
1307   TMatrixD* ph = fPH;
1308
1309   //make a unity system matrix
1310   TMatrixD tmp(fgkNSystemParams,fgkNSystemParams);
1311   fPH = new TMatrixD(TMatrixD::kUnit, tmp);
1312
1313   //mesurement is the state of the new aligner
1314   fPMeasurement = al->fPX;
1315   fPMeasurementCov = al->fPXcov;
1316
1317   //the mesurement prediction is the state
1318   fPMeasurementPrediction = fPX; //this is safe as fPX doesn't change until end
1319
1320   //do the merging
1321   Bool_t success = UpdateEstimateKalman();
1322
1323   //restore pointers to old stuff
1324   fPMeasurement = pmes;
1325   fPMeasurementCov = pmescov;
1326   fPMeasurementPrediction = pmespred;
1327   delete fPH;
1328   fPH = ph;
1329
1330   //merge stats
1331   if (!success)
1332   {
1333     fNMergesFailed++;
1334     return kFALSE; //no point in merging stats if merge not succesful
1335   }
1336   fNProcessedEvents += al->fNProcessedEvents;
1337   fNUpdates += al->fNUpdates;
1338   fNOutliers += al->fNOutliers;
1339   fNTracks += al->fNTracks;
1340   fNMatchedTPCtracklets += al->fNMatchedTPCtracklets;
1341   fNMatchedCosmics += al->fNMatchedCosmics;
1342   fNMerges += al->fNMerges;
1343   if (fTimeStamp < al->fTimeStamp) fTimeStamp = al->fTimeStamp; //take the newer one
1344
1345   return success;
1346 }
1347
1348 //______________________________________________________________________________
1349 Long64_t AliRelAlignerKalman::Merge( TCollection* list )
1350 {
1351   //merge all aligners in the collection
1352   Long64_t numberOfMerges=0;
1353   AliRelAlignerKalman* alignerFromList;
1354   if (!list) return 0;
1355   TIter next(list);
1356   while ( (alignerFromList = dynamic_cast<AliRelAlignerKalman*>(next())) )
1357   {
1358     if (alignerFromList == this) continue;
1359     if (Merge(alignerFromList)) numberOfMerges++;
1360   }
1361   return numberOfMerges;
1362 }
1363
1364 //______________________________________________________________________________
1365 Int_t AliRelAlignerKalman::Compare(const TObject *obj) const
1366 {
1367   if (this == obj) return 0;
1368   const AliRelAlignerKalman* aobj = dynamic_cast<const AliRelAlignerKalman*>(obj);
1369   if (!aobj) return 0;
1370   if (fTimeStamp < aobj->fTimeStamp) return -1;
1371   else if (fTimeStamp > aobj->fTimeStamp) return 1;
1372   else return 0;
1373 }
1374