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 //    Aligns two tracking volumes (by default ITS and TPC)
  20 //    Determines the transformation of the second volume (TPC) wrt the first (ITS)
  21 //    additionally calculates some callibration parameters for TPC
  22 //    Fit parameters are:
  23 //    - 3 shifts, x,y,z
  24 //    - 3 Cardan angles, psi, theta, phi (see definition in alignment docs),
  25 //    - TPC drift velocity correction (vel_true = vel*(1+correction)),
  26 //    - TPC offset correction.
  27 //
  28 //    Basic usage:
  29 //    When aligning two volumes at any given time a single instance of
  30 //    the class should be active. The fit of the parameters is updated
  31 //    by adding new data using one of the Add.... methods.
  32 //
  33 //    User methods:
  34 //    In collision events add an ESD track to update the fit,
  35 //    track will be automatically split in ITS and TPC tracklets
  36 //    and the fit of the alignment constants will be updated:
  37 //
  38 //        Bool_t AddESDTrack( AliESDtrack* pTrack );
  39 //
  40 //    You may also give it the AliTrackPointArray,
  41 //    the fit will be updated if there are enough ITS and TPC
  42 //    points in it. Again, all is done automatically:
  43 //
  44 //        Bool_t AddTrackPointArray( AliTrackPointArray* pTrackPointArr );
  45 //
  46 //    For cosmic data, the assumption is that the tracking is done separately
  47 //    for ITS and TPC and the tracklets are saved inside one AliESDEvent. The
  48 //    method
  49 //
  50 //        Bool_t AddCosmicEventSeparateTracking( AliESDEvent* pEvent );
  51 //
  52 //    then searches the event for matching tracklets and upon succes it updates
  53 //    the fit. For cosmics the fitter switches to a 3 tracks mode, the 3 tracks
  54 //    being: 1 ITS and 2 TPC tracklets (upper and lower).
  55 //
  56 //    Experts only:
  57 //
  58 //
  59 //    Origin: Mikolaj Krzewicki, Nikhef, Mikolaj.Krzewicki@cern.ch
  60 //
  61 //////////////////////////////////////////////////////////////////////////////
  62
  63 #include "AliRelAlignerKalman.h"
  64
  65 ClassImp(AliRelAlignerKalman)
  66
  67 //########################################################
  68 //
  69 //    Control section
  70 //
  71 //########################################################
  72
  73 AliRelAlignerKalman::AliRelAlignerKalman():
  74     fPTrackPointArray1(new AliTrackPointArray(1)),
  75     fPTrackPointArray2(new AliTrackPointArray(1)),
  76     fPTrackPointArray2b(new AliTrackPointArray(1)),
  77     fPVolIDArray1(new TArrayI(1)),
  78     fPVolIDArray2(new TArrayI(1)),
  79     fPVolIDArray2b(new TArrayI(1)),
  80     fPDir1(new TVector3), //Direction
  81     fPDir2(new TVector3),
  82     fPDir2b(new TVector3),
  83     fPDir1Cov(new TMatrixDSym(3)),
  84     fPDir2Cov(new TMatrixDSym(3)),
  85     fPDir2bCov(new TMatrixDSym(3)),
  86     fPDir1ThetaPhiCov(new TMatrixDSym(2)), //Direction vectors are unit vectors, therefore 2 independent components!
  87     fPDir2ThetaPhiCov(new TMatrixDSym(2)),
  88     fPDir2bThetaPhiCov(new TMatrixDSym(2)),
  89     fPPoint1(new TVector3),
  90     fPPoint2(new TVector3),
  91     fPPoint2b(new TVector3),
  92     fPPoint1Cov(new TMatrixDSym(3)),
  93     fPPoint2Cov(new TMatrixDSym(3)),
  94     fPPoint2bCov(new TMatrixDSym(3)),
  95     fPRefPoint(new TVector3(0.,0.,0.)),
  96     fPRefPointDir(new TVector3(0.,1.,0.)),
  97     fPRefAliTrackPoint(new AliTrackPoint()),
  98     fPRotMat(new TMatrixD(3,3)),
  99     fPX(new TVectorD( fgkNSystemParams )),
 100     fPXcov(new TMatrixDSym( fgkNSystemParams )),
 101     fPH(new TMatrixD( fgkNMeasurementParams2TrackMode, fgkNSystemParams )),
 102     fQ(1e-10),
 103     fPMeasurement(new TVectorD( fgkNMeasurementParams2TrackMode )),
 104     fPMeasurementCov(new TMatrixDSym( fgkNMeasurementParams2TrackMode )),
 105     fNMeasurementParams(fgkNMeasurementParams2TrackMode),
 106     fNSystemParams(fgkNSystemParams),
 107     fOutRejSigmas(2.),
 108     f3TracksMode(kFALSE),
 109     fSortTrackPointsWithY(kTRUE),
 110     fFixedMeasurementCovariance(kTRUE),
 111     fCalibrationPass(kFALSE),
 112     fCalibrationUpdated(kFALSE),
 113     fFitted(kFALSE),
 114     fCuts(kFALSE),
 115     fNTracks(0),
 116     fNUpdates(0),
 117     fNOutliers(0),
 118     fNUnfitted(0),
 119     fNMatchedCosmics(0),
 120     fMinPointsVol1(2),
 121     fMinPointsVol2(10),
 122     fMinMom(0.),
 123     fMaxMom(1e100),
 124     fMinAbsSinPhi(0.),
 125     fMaxAbsSinPhi(1.),
 126     fMinSinTheta(-1.),
 127     fMaxSinTheta(1.),
 128     fMaxMatchingAngle(0.1),
 129     fMaxMatchingDistance(1.),  //in cm
 130     fFirstLayerVol1(1),
 131     fLastLayerVol1(6),
 132     fFirstLayerVol2(7),
 133     fLastLayerVol2(8),
 134     fPVec010(new TVector3(0,1,0)),
 135     fPXMesHist(new TH1D("x","x", 50, 0, 0)),
 136     fPZMesHist(new TH1D("z","z", 50, 0, 0)),
 137     fPPhiMesHist(new TH1D("phi","phi", 50, 0, 0)),
 138     fPThetaMesHist(new TH1D("theta","theta", 50, 0, 0)),
 139     fPXMesHist2(new TH1D("x_","x_", 50, 0, 0)),
 140     fPZMesHist2(new TH1D("z_","z_", 50, 0, 0)),
 141     fPPhiMesHist2(new TH1D("phi_","phi_", 50, 0, 0)),
 142     fPThetaMesHist2(new TH1D("theta_","theta_", 50, 0, 0)),
 143     fPMesCov11Hist(new TH1D("mescov11","mescov11", 50, 0, 0)),
 144     fPMesCov22Hist(new TH1D("mescov22","mescov22", 50, 0, 0)),
 145     fPMesCov33Hist(new TH1D("mescov33","mescov33", 50, 0, 0)),
 146     fPMesCov44Hist(new TH1D("mescov44","mescov44", 50, 0, 0)),
 147     fPMesCov55Hist(new TH1D("mescov55","mescov55", 50, 0, 0)),
 148     fPMesCov66Hist(new TH1D("mescov66","mescov66", 50, 0, 0)),
 149     fPMesCov77Hist(new TH1D("mescov77","mescov77", 50, 0, 0)),
 150     fPMesCov88Hist(new TH1D("mescov88","mescov88", 50, 0, 0)),
 151     fPMeasurementCovCorr(new TMatrixDSym(fgkNMeasurementParams2TrackMode))
 152 {
 153     //Default constructor
 154
 155     Float_t refpointcov[6] = { 1., 0., 0.,
 156                                    0., 0.,
 157                                        1.  };
 158     fPRefAliTrackPoint->SetXYZ( 0., 0., 0., refpointcov );
 159
 160     //default seed: zero, unit(large) errors
 161     fPXcov->UnitMatrix();
 162     (*fPXcov) *= 1.;
 163
 164 }
 165
 166 Bool_t AliRelAlignerKalman::AddESDTrack( AliESDtrack* pTrack )
 167 {
 168     //Add an ESD track from a central event
 169     //from the ESDtrack extract the pointarray
 170
 171     SetCosmicEvent(kFALSE);
 172
 173     const AliTrackPointArray *pTrackPointArrayConst = pTrack->GetTrackPointArray();
 174     AliTrackPointArray* pTrackPointArray = const_cast < AliTrackPointArray* > (pTrackPointArrayConst);
 175     if (pTrackPointArray == 0) return kFALSE;
 176
 177     if (!AddTrackPointArray( pTrackPointArray )) return kFALSE;
 178     return kTRUE;
 179 }
 180
 181 Bool_t AliRelAlignerKalman::AddTrackPointArray( AliTrackPointArray* pTrackPointArray )
 182 {
 183     //Add a trackpointarray from a central event
 184
 185     SetCosmicEvent(kFALSE);
 186     fNTracks++;
 187
 188     if (!ExtractSubTrackPointArray( fPTrackPointArray1, fPVolIDArray1, pTrackPointArray, 1,6 ))
 189         return kFALSE;
 190     if (!ExtractSubTrackPointArray( fPTrackPointArray2, fPVolIDArray2, pTrackPointArray, 7,8 ))
 191         return kFALSE;
 192
 193     if (!ProcessTrackPointArrays()) return kFALSE;
 194     if (!PrepareUpdate()) return kFALSE;
 195     if (!Update()) return kFALSE;
 196
 197     return kTRUE;
 198 }
 199
 200 Bool_t AliRelAlignerKalman::AddCosmicEventSeparateTracking( AliESDEvent* pEvent )
 201 {
 202     //Add an cosmic with separately tracked ITS and TPC parts, do trackmatching
 203
 204     SetCosmicEvent(kTRUE);  //set the cosmic flag
 205     fNTracks++;
 206
 207     if (!FindCosmicInEvent( pEvent )) return kFALSE;
 208     if (!ProcessTrackPointArrays()) return kFALSE;
 209     //PrintDebugInfo();
 210     if (!PrepareUpdate()) return kFALSE;
 211     if (!Update()) return kFALSE;
 212     printf("fpMeasurementCov:\n");
 213     fPMeasurementCov->Print();
 214
 215     return kTRUE;
 216 }
 217
 218 void AliRelAlignerKalman::Print()
 219 {
 220     //Print some useful info
 221     printf("\nAliRelAlignerKalman:\n");
 222     printf("  %i tracks, %i updates, %i outliers, ", fNTracks, fNUpdates, fNOutliers );
 223     printf("%i not fitted, %i matched cosmic tracklets\n", fNUnfitted, fNMatchedCosmics );
 224     printf("  psi(x):         %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(0),1e3*TMath::Sqrt((*fPXcov)(0,0)));
 225     printf("  theta(y):       %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(1),1e3*TMath::Sqrt((*fPXcov)(1,1)));
 226     printf("  phi(z):         %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(2),1e3*TMath::Sqrt((*fPXcov)(2,2)));
 227     printf("  x:              %.1f ± (%.1f) micron\n", 1e4*(*fPX)(3),1e4*TMath::Sqrt((*fPXcov)(3,3)));
 228     printf("  y:              %.1f ± (%.1f) micron\n", 1e4*(*fPX)(4),1e4*TMath::Sqrt((*fPXcov)(4,4)));
 229     printf("  z:              %.1f ± (%.1f) micron\n", 1e4*(*fPX)(5),1e4*TMath::Sqrt((*fPXcov)(5,5)));
 230     printf("  TPC(drift corr) %.1f ± (%.1f) percent\n", 1e2*(*fPX)(6),1e2*TMath::Sqrt((*fPXcov)(6,6)));
 231     printf("  TPC(offset)     %.1f ± (%.1f) micron\n", 1e4*(*fPX)(7),1e4*TMath::Sqrt((*fPXcov)(7,7)));
 232     return;
 233 }
 234
 235 void AliRelAlignerKalman::SetTrackParams1( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov )
 236 {
 237     //Set the trackparameters for track in the first volume at reference
 238     *fPPoint1 = point;
 239     *fPPoint1Cov = pointcov;
 240     *fPDir1 = dir;
 241     *fPDir1Cov = dircov;
 242 }
 243
 244 void AliRelAlignerKalman::SetTrackParams2( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov )
 245 {
 246     //Set the trackparameters for track in the second volume at reference
 247     *fPPoint2 = point;
 248     *fPPoint2Cov = pointcov;
 249     *fPDir2 = dir;
 250     *fPDir2Cov = dircov;
 251 }
 252
 253 void AliRelAlignerKalman::SetTrackParams2b( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov )
 254 {
 255     //Set the trackparameters for second track in the second volume at reference
 256     *fPPoint2b = point;
 257     *fPPoint2bCov = pointcov;
 258     *fPDir2b = dir;
 259     *fPDir2bCov = dircov;
 260 }
 261
 262 void AliRelAlignerKalman::SetRefSurface( const Double_t yref )
 263 {
 264     //set the reference surface
 265     fPRefPoint->SetXYZ( 0., 1.*yref, 0. );
 266     fPRefPointDir->SetXYZ( 0., 1., 0. );
 267     Float_t refpointcov[6] = { 1., 0., 0.,
 268                                    0., 0.,
 269                                        1.  };
 270     fPRefAliTrackPoint->SetXYZ( 0., 1.*yref, 0., refpointcov );
 271 }
 272
 273 void AliRelAlignerKalman::SetRefSurface( const TVector3& point, const TVector3& dir )
 274 {
 275     //set the reference surface
 276     *fPRefPoint = point;
 277     *fPRefPointDir = dir;
 278 }
 279
 280 void AliRelAlignerKalman::SetRefSurface( const AliTrackPoint& point, const Bool_t horizontal )
 281 {
 282     //set the reference surface
 283     (*fPRefAliTrackPoint) = point;
 284
 285     (*fPRefPoint)(0) = point.GetX();
 286     (*fPRefPoint)(1) = point.GetY();
 287     (*fPRefPoint)(2) = point.GetZ();
 288
 289     if (horizontal)
 290     {
 291         (*fPRefPointDir)(0) = 0.;
 292         (*fPRefPointDir)(1) = 1.;
 293         (*fPRefPointDir)(2) = 0.;
 294
 295         Float_t refpointcov[6] = { 1., 0., 0.,
 296                                        0., 0.,
 297                                            1.  };
 298         fPRefAliTrackPoint->SetXYZ( point.GetX(), point.GetY() , point.GetZ(), refpointcov );
 299
 300     }
 301     else
 302     {
 303         Double_t angle = point.GetAngle();
 304         (*fPRefPointDir)(0) = TMath::Cos(angle);
 305         (*fPRefPointDir)(1) = TMath::Sin(angle);
 306         (*fPRefPointDir)(2) = 0.;
 307         *fPRefPointDir = fPRefPointDir->Unit();
 308     }
 309 }
 310
 311 void AliRelAlignerKalman::Set3TracksMode( Bool_t mode )
 312 {
 313     //set the mode where 2 tracklets are allowed in volume 2 (default:TPC)
 314     //used for alignment with cosmics
 315
 316     f3TracksMode = mode;
 317     if (mode)
 318     {
 319         if (fNMeasurementParams!=fgkNMeasurementParams3TrackMode) //do something only if necessary
 320         {
 321             fNMeasurementParams = fgkNMeasurementParams3TrackMode;
 322             delete fPMeasurement;
 323             delete fPMeasurementCov;
 324             delete fPH;
 325             fPMeasurement = new TVectorD( fNMeasurementParams );
 326             fPMeasurementCov = new TMatrixDSym( fNMeasurementParams );
 327             fPH = new TMatrixD( fNMeasurementParams, fNSystemParams );
 328         }
 329     }
 330     else
 331     {
 332         if (fNMeasurementParams!=fgkNMeasurementParams2TrackMode)
 333         {
 334             fNMeasurementParams = fgkNMeasurementParams2TrackMode;
 335             delete fPMeasurement;
 336             delete fPMeasurementCov;
 337             delete fPH;
 338             fPMeasurement = new TVectorD( fNMeasurementParams );
 339             fPMeasurementCov = new TMatrixDSym( fNMeasurementParams );
 340             fPH = new TMatrixD( fNMeasurementParams, fNSystemParams );
 341         }
 342     }
 343 }
 344
 345 Bool_t AliRelAlignerKalman::PrepareUpdate()
 346 {
 347     //Cast the extrapolated data (points and directions) into
 348     //the internal Kalman filter data representation.
 349     //takes the 3d coordinates of the points of intersection with
 350     //the reference surface and projects them onto a 2D plane.
 351     //does the same for angles, combines the result in one vector
 352
 353     if (!FillMeasurement()) return kFALSE;
 354     if (!FillMeasurementMatrix()) return kFALSE;
 355     return kTRUE;
 356 }
 357
 358 Bool_t AliRelAlignerKalman::Update()
 359 {
 360     //perform the update - either kalman or calibration
 361     if (fCalibrationPass) return UpdateCalibration();
 362     else return UpdateEstimateKalman();
 363 }
 364
 365 void AliRelAlignerKalman::RotMat( TMatrixD &R, const TVectorD& angles )
 366 {
 367 //Get Rotation matrix R given the Cardan angles psi, theta, phi (around x, y, z).
 368     Double_t sinpsi = TMath::Sin(angles(0));
 369     Double_t sintheta = TMath::Sin(angles(1));
 370     Double_t sinphi = TMath::Sin(angles(2));
 371     Double_t cospsi = TMath::Cos(angles(0));
 372     Double_t costheta = TMath::Cos(angles(1));
 373     Double_t cosphi = TMath::Cos(angles(2));
 374
 375     R(0,0) = costheta*cosphi;
 376     R(0,1) = -costheta*sinphi;
 377     R(0,2) = sintheta;
 378     R(1,0) = sinpsi*sintheta*cosphi + cospsi*sinphi;
 379     R(1,1) = -sinpsi*sintheta*sinphi + cospsi*cosphi;
 380     R(1,2) = -costheta*sinpsi;
 381     R(2,0) = -cospsi*sintheta*cosphi + sinpsi*sinphi;
 382     R(2,1) = cospsi*sintheta*sinphi + sinpsi*cosphi;
 383     R(2,2) = costheta*cospsi;
 384     return;
 385 }
 386
 387 Bool_t AliRelAlignerKalman::FillMeasurement()
 388 {
 389     //Take the track parameters and calculate the input to the Kalman filter
 390
 391     //Direction vectors
 392     //Measured is the difference between the polar angles - convert to that
 393     (*fPMeasurement)(0) = fPDir2->Theta() - fPDir1->Theta();
 394     (*fPMeasurement)(1) = fPDir2->Phi() - fPDir1->Phi();
 395
 396     //Points
 397     //Measured is the distance in XZ plane at Y of the reference point
 398     (*fPMeasurement)(2) = fPPoint2->X() - fPPoint1->X();
 399     (*fPMeasurement)(3) = fPPoint2->Z() - fPPoint1->Z();
 400
 401     //if 3 track mode set, set also the stuff for 2nd TPC track
 402     if (f3TracksMode)
 403     {
 404         (*fPMeasurement)(4) = fPDir2b->Theta() - fPDir1->Theta();
 405         (*fPMeasurement)(5) = fPDir2b->Phi() - fPDir1->Phi();
 406         (*fPMeasurement)(6) = fPPoint2b->X() - fPPoint1->X();
 407         (*fPMeasurement)(7) = fPPoint2b->Z() - fPPoint1->Z();
 408     }
 409
 410     //Fill the covariance
 411     if (fFixedMeasurementCovariance) return kTRUE;
 412     //the dirs
 413     GetThetaPhiCov( *fPDir1ThetaPhiCov, *fPDir1, *fPDir1Cov );
 414     GetThetaPhiCov( *fPDir2ThetaPhiCov, *fPDir2, *fPDir2Cov );
 415     fPMeasurementCov->SetSub( 0, (*fPDir1ThetaPhiCov) + (*fPDir2ThetaPhiCov) );
 416
 417     //the points
 418     (*fPMeasurementCov)(2,2) = (*fPPoint1Cov)(0,0)+(*fPPoint2Cov)(0,0);
 419     (*fPMeasurementCov)(2,3) = (*fPPoint1Cov)(0,2)+(*fPPoint2Cov)(0,2);
 420     (*fPMeasurementCov)(3,2) = (*fPPoint1Cov)(2,0)+(*fPPoint2Cov)(2,0);
 421     (*fPMeasurementCov)(3,3) = (*fPPoint1Cov)(2,2)+(*fPPoint2Cov)(2,2);
 422
 423     //if 3 track mode set, set also the stuff for 2nd TPC track
 424     if (f3TracksMode)
 425     {
 426         GetThetaPhiCov( *fPDir2bThetaPhiCov, *fPDir2b, *fPDir2bCov );
 427         fPMeasurementCov->SetSub( 4, (*fPDir1ThetaPhiCov) + (*fPDir2bThetaPhiCov) );
 428         (*fPMeasurementCov)(6,6) = (*fPPoint1Cov)(0,0)+(*fPPoint2bCov)(0,0);
 429         (*fPMeasurementCov)(6,7) = (*fPPoint1Cov)(0,2)+(*fPPoint2bCov)(0,2);
 430         (*fPMeasurementCov)(7,6) = (*fPPoint1Cov)(2,0)+(*fPPoint2bCov)(2,0);
 431         (*fPMeasurementCov)(7,7) = (*fPPoint1Cov)(2,2)+(*fPPoint2bCov)(2,2);
 432     }
 433     return kTRUE;
 434 }
 435
 436 Bool_t AliRelAlignerKalman::FillMeasurementMatrix()
 437 {
 438     //Calculate the system matrix for the Kalman filter
 439     //approximate the system using as reference the track in the first volume
 440
 441     Double_t delta = 1.e-8;
 442     TVectorD z1( fNMeasurementParams );
 443     TVectorD z2( fNMeasurementParams );
 444     TVectorD x1( *fPX );
 445     TVectorD x2( *fPX );
 446     TMatrixD D( fNMeasurementParams, 1 );
 447     for ( Int_t i=0; i<fNSystemParams; i++ )
 448     {
 449         x1 = *fPX;
 450         x2 = *fPX;
 451         x1(i) -= delta;
 452         x2(i) += delta;
 453         if (!PredictMeasurement( z1, x1 )) return kFALSE;
 454         if (!PredictMeasurement( z2, x2 )) return kFALSE;
 455         for (Int_t j=0; j<fNMeasurementParams; j++ )
 456             D.GetMatrixArray()[j] = (z2.GetMatrixArray()[j]-z1.GetMatrixArray()[j])/(2.*delta);
 457         fPH->SetSub( 0, i, D );
 458     }
 459     return kTRUE;
 460 }
 461
 462 Bool_t AliRelAlignerKalman::PredictMeasurement( TVectorD& pred, const TVectorD& state )
 463 {
 464     // Implements a system model for the Kalman fit
 465     // pred is [dtheta, dphi, dx, dz, [dtheta', dphi', dx', dz'] ] - second part is for 2nd TPC track in cosmics
 466     // state is [psi,theta,phi,x,y,z,driftTPC,offsetTPC]
 467
 468     TVector3 newPoint = *fPPoint1;
 469     TVector3 newDir = *fPDir1;
 470     TVector3 shift;
 471     //TPC drift correction
 472     newDir(2) *= (1.+state(6));
 473     newPoint(2) *= (1.+state(6));
 474     //TPC offset
 475     if (newPoint(2)>0) newPoint(2) += state(7);
 476     else newPoint(2) -= state(7);
 477     //rotate
 478     TMatrixD rotmat(3,3);
 479     RotMat( rotmat, state );
 480     newDir = rotmat * newDir;
 481     newPoint = rotmat * newPoint;
 482     //shift
 483     shift(0) = state(3);
 484     shift(1) = state(4);
 485     shift(2) = state(5);
 486     newPoint += shift;
 487     //Predicted measurement
 488     if (!IntersectionLinePlane( newPoint, newPoint, newDir, *fPRefPoint, *fPVec010 )) return kFALSE;
 489     pred(0) = newDir.Theta() - fPDir1->Theta();
 490     pred(1) = newDir.Phi() - fPDir1->Phi();
 491     pred(2) = newPoint(0) - fPPoint1->X();
 492     pred(3) = newPoint(2) - fPPoint1->Z();
 493
 494     if (f3TracksMode)
 495     {
 496         //Do the same for second TPC track
 497         //should be the same as the first TPC track
 498         pred(4) = pred(0);
 499         pred(5) = pred(1);
 500         pred(6) = pred(2);
 501         pred(7) = pred(3);
 502     }
 503     return kTRUE;
 504 }
 505
 506 Bool_t AliRelAlignerKalman::UpdateEstimateKalman()
 507 {
 508     //Kalman estimation of noisy constants: in the model A=1
 509     //The arguments are (following the usual convention):
 510     //  x - the state vector (parameters)
 511     //  P - the state covariance matrix (parameter errors)
 512     //  z - measurement vector
 513     //  R - measurement covariance matrix
 514     //  H - measurement model matrix ( z = Hx + v ) v being measurement noise (error fR)
 515     TVectorD *x = fPX;
 516     TMatrixDSym *P = fPXcov;
 517     TVectorD *z = fPMeasurement;
 518     TMatrixDSym *R = fPMeasurementCov;
 519     TMatrixD *H = fPH;
 520
 521     TMatrixDSym I(TMatrixDSym::kUnit, *P);            //unit matrix
 522
 523     //predict the state
 524     *P = *P + fQ*I;  //add some process noise (diagonal)
 525
 526     // update prediction with measurement
 527         // calculate Kalman gain
 528         // K = PH/(HPH+R)
 529     TMatrixD PHT( *P, TMatrixD::kMultTranspose, *H );  //common factor (used twice)
 530     TMatrixD HPHT( *H, TMatrixD::kMult, PHT );
 531     HPHT += *R;
 532     TMatrixD K(PHT, TMatrixD::kMult, HPHT.Invert());                 //compute K
 533
 534         // update the state and its covariance matrix
 535     TVectorD xupdate(*x);
 536     TVectorD Hx(*z);
 537     PredictMeasurement( Hx, *x );
 538     xupdate = K*((*z)-Hx);
 539
 540     //SIMPLE OUTLIER REJECTION
 541     if (
 542             (TMath::Abs(xupdate(0)) > fOutRejSigmas*TMath::Sqrt((*P)(0,0))) ||
 543             (TMath::Abs(xupdate(1)) > fOutRejSigmas*TMath::Sqrt((*P)(1,1))) ||
 544             (TMath::Abs(xupdate(2)) > fOutRejSigmas*TMath::Sqrt((*P)(2,2))) ||
 545             (TMath::Abs(xupdate(3)) > fOutRejSigmas*TMath::Sqrt((*P)(3,3))) ||
 546             (TMath::Abs(xupdate(4)) > fOutRejSigmas*TMath::Sqrt((*P)(4,4))) ||
 547             (TMath::Abs(xupdate(5)) > fOutRejSigmas*TMath::Sqrt((*P)(5,5))) ||
 548             (TMath::Abs(xupdate(6)) > fOutRejSigmas*TMath::Sqrt((*P)(6,6))) ||
 549             (TMath::Abs(xupdate(7)) > fOutRejSigmas*TMath::Sqrt((*P)(7,7)))
 550         )
 551     {
 552         fNOutliers++;
 553         return kFALSE;
 554     }
 555
 556     *x += xupdate;
 557     TMatrixD KH( K, TMatrixD::kMult, *H );
 558     TMatrixD IKH(I);
 559     IKH = I - KH;
 560     TMatrixD IKHP( IKH, TMatrixD::kMult, *P ); // (I-KH)P
 561     TMatrixDSym_from_TMatrixD( *P, IKHP );
 562     fNUpdates++;
 563     return kTRUE;
 564 }
 565
 566 void AliRelAlignerKalman::TMatrixDSym_from_TMatrixD( TMatrixDSym& matsym, const TMatrixD& mat )
 567 {
 568     //Produce a valid symmetric matrix out of a TMatrixD
 569
 570     //not very efficient, diagonals are computer twice, but who cares
 571     for (Int_t i=0; i<mat.GetNcols(); i++)
 572     {
 573         for (Int_t j=i; j<mat.GetNcols(); j++)
 574         {
 575             Double_t average = (mat(i,j)+mat(j,i))/2.;
 576             matsym(i,j)=average;
 577             matsym(j,i)=average;
 578         }
 579     }
 580     return;
 581 }
 582
 583 void AliRelAlignerKalman::GetThetaPhiCov( TMatrixDSym& cov, const TVector3& vec, const TMatrixDSym& vecCov )
 584 {
 585     //Given the covariance matrix of a vector in euclid.space
 586     //give cov matrix of the 2 spherical angles
 587     const Double_t delta = 1e-8;
 588     TVector3 vec1;
 589     TVector3 vec2;
 590     TMatrixD T(2,3);
 591     for (Int_t i=0; i<3; i++)
 592     {
 593         vec1 = vec;
 594         vec2 = vec;
 595         vec1(i) -= delta/2.;
 596         vec2(i) += delta/2.;
 597         T(0,i) = (vec2.Theta() - vec1.Theta())/delta;
 598         T(1,i) = (vec2.Phi() - vec1.Phi())/delta;
 599     }
 600     TMatrixD tmp( T, TMatrixD::kMult, vecCov );
 601     TMatrixD nscov( tmp, TMatrixD::kMultTranspose, T );
 602     cov(0,0) = nscov(0,0); cov(0,1) = nscov(0,1);
 603     cov(1,0) = nscov(0,1); cov(1,1) = nscov(1,1);
 604     return;
 605 }
 606
 607 Bool_t AliRelAlignerKalman::IntersectionLinePlane( TVector3& intersection, const TVector3& linebase, const TVector3& linedir, const TVector3& planepoint, const TVector3& planenormal )
 608 {
 609     //calculate the intersection point between a straight line and a plane
 610     //plane is defined with a point in it and a normal, line has a point and direction
 611     if (planenormal==TVector3(0,1,0))
 612     {
 613         if (linedir(1)==0) return kFALSE;
 614         Double_t t = ( planepoint(1) - linebase(1) ) / linedir(1);
 615         intersection = linebase + t*linedir;
 616         return kTRUE;
 617     }
 618     else
 619     {
 620         Double_t dirdotn = linedir.Dot(planenormal);
 621         if (dirdotn==0) return kFALSE;
 622         Double_t t = ( planepoint.Dot(planenormal) - linebase.Dot(planenormal) ) / dirdotn;
 623         intersection = linebase + t*linedir;
 624         return kTRUE;
 625     }
 626 }
 627
 628 void AliRelAlignerKalman::Angles( TVectorD &angles, const TMatrixD &rotmat )
 629 {
 630 //Calculate the Cardan angles (psi,theta,phi) from rotation matrix
 631 //b = R*a
 632 //
 633         angles(0) = TMath::ATan2( -rotmat(1,2), rotmat(2,2) );
 634         angles(1) = TMath::ASin( rotmat(0,2) );
 635         angles(2) = TMath::ATan2( -rotmat(0,1), rotmat(0,0) );
 636         return;
 637 }
 638
 639 void AliRelAlignerKalman::PrintCorrelationMatrix()
 640 {
 641     //Print the correlation matrix for the fitted parameters
 642
 643     for ( Int_t j=0; j<fNSystemParams; j++ )
 644     {
 645         for ( Int_t i=0; i<fNSystemParams; i++ )
 646         {
 647             printf("%1.2f  ", (*fPXcov)(i,j)/TMath::Sqrt( (*fPXcov)(i,i) * (*fPXcov)(j,j) ) );
 648         }//for i
 649         printf("\n");
 650     }//for j
 651     printf("\n");
 652     return;
 653 }
 654
 655 Bool_t AliRelAlignerKalman::FindCosmicInEvent( AliESDEvent* pEvent )
 656 {
 657     //Find track point arrays belonging to one cosmic in a separately tracked ESD
 658     //and put them in the apropriate data members
 659
 660     //Sanity cuts on tracks + check which tracks are ITS which are TPC
 661     Int_t ntracks = pEvent->GetNumberOfTracks(); //printf("number of tracks in event: %i\n", ntracks);
 662     if(ntracks<2) return kFALSE;
 663     Float_t* phiArr = new Float_t[ntracks];
 664     Float_t* thetaArr = new Float_t[ntracks];
 665     Double_t* distanceFromVertexArr = new Double_t[ntracks];
 666     Int_t* goodtracksArr = new Int_t[ntracks];
 667     Int_t* ITStracksArr = new Int_t[ntracks];
 668     Int_t* TPCtracksArr = new Int_t[ntracks];
 669     Int_t* nPointsArr = new Int_t[ntracks];
 670     Int_t nITStracks = 0;
 671     Int_t nTPCtracks = 0;
 672     Int_t nGoodTracks = 0;
 673     AliESDtrack* pTrack = NULL;
 674
 675     const AliESDVertex* pVertex = pEvent->GetVertex();
 676     Double_t vertexposition[3];
 677     pVertex->GetXYZ(vertexposition);
 678
 679     Double_t magfield = pEvent->GetMagneticField();
 680
 681     //select sane tracks
 682     for (Int_t itrack=0; itrack < ntracks; itrack++)
 683     {
 684         pTrack = pEvent->GetTrack(itrack);
 685         if (!pTrack) {std::cout<<"no track!"<<std::endl;continue;}
 686         if(pTrack->GetNcls(0)+pTrack->GetNcls(1) < fMinPointsVol1) continue;
 687         Float_t phi = pTrack->GetAlpha()+TMath::ASin(pTrack->GetSnp());
 688         Float_t theta = 0.5*TMath::Pi()-TMath::ATan(pTrack->GetTgl());
 689         //printf("phi: %4.2f theta: %4.2f\n", phi, theta);
 690         if(fCuts)
 691         {
 692             if(pTrack->GetP()<fMinMom || pTrack->GetP()>fMaxMom) continue;
 693             Float_t abssinphi = TMath::Abs(TMath::Sin(phi));
 694             if(abssinphi<fMinAbsSinPhi || abssinphi>fMaxAbsSinPhi) continue;
 695             Float_t sintheta = TMath::Sin(theta);
 696             if(sintheta<fMinSinTheta || sintheta>fMaxSinTheta) continue;
 697         }
 698         goodtracksArr[nGoodTracks]=itrack;
 699         phiArr[nGoodTracks]=phi;
 700         thetaArr[nGoodTracks]=theta;
 701
 702         pTrack->RelateToVertex( pVertex, magfield, 10000. );
 703         Double_t trackposition[3];
 704         pTrack->GetXYZ( trackposition );
 705         distanceFromVertexArr[nGoodTracks] =
 706               TMath::Sqrt((trackposition[0]-vertexposition[0])*(trackposition[0]-vertexposition[0])
 707                        + (trackposition[1]-vertexposition[1])*(trackposition[1]-vertexposition[1])
 708                        + (trackposition[2]-vertexposition[2])*(trackposition[2]-vertexposition[2]));
 709
 710         //check if track is ITS or TPC
 711         if ( ((pTrack->GetStatus()&AliESDtrack::kITSin)>0)&&
 712              !((pTrack->GetStatus()&AliESDtrack::kTPCin)>0) )
 713         {
 714             ITStracksArr[nITStracks] = nGoodTracks;
 715             nITStracks++;
 716         }
 717
 718         if ( ((pTrack->GetStatus()&AliESDtrack::kTPCin)>0)&&
 719              !((pTrack->GetStatus()&AliESDtrack::kITSin)>0) )
 720         {
 721             TPCtracksArr[nTPCtracks] = nGoodTracks;
 722             nTPCtracks++;
 723         }
 724
 725         nGoodTracks++;
 726     }//for itrack   -   sanity cuts
 727
 728     //printf("there are good tracks, %d in ITS and %d TPC.\n", nITStracks, nTPCtracks);
 729
 730     if( nITStracks < 2 || nTPCtracks < 2 )
 731     {
 732         delete [] goodtracksArr; goodtracksArr=0;
 733         delete [] ITStracksArr; ITStracksArr=0;
 734         delete [] TPCtracksArr; TPCtracksArr=0;
 735         delete [] nPointsArr; nPointsArr=0;
 736         delete [] phiArr; phiArr=0;
 737         delete [] thetaArr; thetaArr=0;
 738         delete [] distanceFromVertexArr; distanceFromVertexArr=0;
 739         return kFALSE;
 740     }
 741
 742     //find matching in TPC
 743     Float_t min = 10000000.;
 744     Int_t TPCgood1 = -1, TPCgood2 = -1;
 745     for(Int_t itr1=0; itr1<nTPCtracks; itr1++)
 746     {
 747         for(Int_t itr2=itr1+1; itr2<nTPCtracks; itr2++)
 748         {
 749             Float_t deltatheta = TMath::Abs(TMath::Pi()-thetaArr[TPCtracksArr[itr1]]-thetaArr[TPCtracksArr[itr2]]);
 750             if(deltatheta > fMaxMatchingAngle) continue;
 751             Float_t deltaphi = TMath::Abs(TMath::Abs(phiArr[TPCtracksArr[itr1]]-phiArr[TPCtracksArr[itr2]])-TMath::Pi());
 752             if(deltaphi > fMaxMatchingAngle) continue;
 753             //printf("TPC: %f  %f     %f  %f\n",deltaphi,deltatheta,thetaArr[TPCtracksArr[itr1]],thetaArr[TPCtracksArr[itr2]]);
 754             if(deltatheta+deltaphi<min) //only the best matching pair
 755             {
 756                 min=deltatheta+deltaphi;
 757                 TPCgood1 = TPCtracksArr[itr1];  //store the index of track in goodtracksArr[]
 758                 TPCgood2 = TPCtracksArr[itr2];
 759             }
 760         }
 761     }
 762     if (TPCgood1 < 0) //no dubble cosmic track
 763     {
 764         delete [] goodtracksArr; goodtracksArr=0;
 765         delete [] ITStracksArr; ITStracksArr=0;
 766         delete [] TPCtracksArr; TPCtracksArr=0;
 767         delete [] nPointsArr; nPointsArr=0;
 768         delete [] phiArr; phiArr=0;
 769         delete [] thetaArr; thetaArr=0;
 770         delete [] distanceFromVertexArr; distanceFromVertexArr=0;
 771         return kFALSE;
 772     }
 773
 774     //in ITS find 2 tracks that best match the found TPC cosmic
 775     Double_t min1 = 10000000.;
 776     Double_t min2 = 10000000.;
 777     Int_t ITSgood1 = -1, ITSgood2 = -1;
 778     for(Int_t itr1=0; itr1<nITStracks; itr1++)
 779     {
 780         if(distanceFromVertexArr[ITStracksArr[itr1]]>fMaxMatchingDistance) continue;
 781         Float_t deltatheta = TMath::Abs(TMath::Pi()-thetaArr[ITStracksArr[itr1]]-thetaArr[TPCgood1]);
 782         if(deltatheta > fMaxMatchingAngle) deltatheta = TMath::Abs( deltatheta - TMath::Pi() );
 783         if(deltatheta > fMaxMatchingAngle) continue;
 784         Float_t deltaphi = TMath::Abs(TMath::Abs(phiArr[ITStracksArr[itr1]]-phiArr[TPCgood1])-TMath::Pi());
 785         if(deltaphi > fMaxMatchingAngle) deltaphi = TMath::Abs( deltaphi - TMath::Pi() );
 786         if(deltaphi > fMaxMatchingAngle) continue;
 787         //printf("ITS %i dtheta, dphi, vrtx: %.4f, %.4f, %.4f\n",ITStracksArr[itr1], deltatheta, deltaphi,distanceFromVertexArr[ITStracksArr[itr1]]);
 788         if(deltatheta+deltaphi<min1) //only the best one
 789         {
 790             min1=deltatheta+deltaphi;
 791             //ITSgood2 = ITSgood1;
 792             ITSgood1 = ITStracksArr[itr1];
 793             continue;
 794         }
 795         if(deltatheta+deltaphi<min2) //the second best
 796         {
 797             min2=deltatheta+deltaphi;
 798             ITSgood2 = ITStracksArr[itr1];
 799             continue;
 800         }
 801     }
 802     if (ITSgood2 < 0) //no ITS cosmic track
 803     {
 804         delete [] goodtracksArr; goodtracksArr=0;
 805         delete [] ITStracksArr; ITStracksArr=0;
 806         delete [] TPCtracksArr; TPCtracksArr=0;
 807         delete [] nPointsArr; nPointsArr=0;
 808         delete [] phiArr; phiArr=0;
 809         delete [] thetaArr; thetaArr=0;
 810         delete [] distanceFromVertexArr; distanceFromVertexArr=0;
 811         return kFALSE;
 812     }
 813
 814     //we found a cosmic
 815     fNMatchedCosmics++;
 816
 817     //////////////////////////////////////////////////////////////////////////////////////
 818     // convert indexes from local goodtrackarrays to global track index
 819     TPCgood1 = goodtracksArr[TPCgood1];
 820     TPCgood2 = goodtracksArr[TPCgood2];
 821     ITSgood1 = goodtracksArr[ITSgood1];
 822     ITSgood2 = goodtracksArr[ITSgood2];
 823     /////////////////////////////////////////////////////////////////////////////////////
 824
 825     AliESDtrack * pTPCtrack1 = pEvent->GetTrack(TPCgood1);
 826     AliESDtrack * pTPCtrack2 = pEvent->GetTrack(TPCgood2);
 827     AliESDtrack * pITStrack1 = pEvent->GetTrack(ITSgood1);
 828     AliESDtrack * pITStrack2 = pEvent->GetTrack(ITSgood2);
 829     const AliTrackPointArray* pTPCArray1 = pTPCtrack1->GetTrackPointArray();
 830     const AliTrackPointArray* pTPCArray2 = pTPCtrack2->GetTrackPointArray();
 831     const AliTrackPointArray* pITSArray1 = pITStrack1->GetTrackPointArray();
 832     const AliTrackPointArray* pITSArray2 = pITStrack2->GetTrackPointArray();
 833
 834     AliTrackPointArray* pfullITStrackArray = new AliTrackPointArray(1);
 835     JoinTrackArrays( pfullITStrackArray, pITSArray1, pITSArray2 );
 836
 837     //std::cout<<"selected tracks: TPC: "<<TPCgood1<<" "<<TPCgood2<<" ITS: "<<ITSgood1<<" "<<ITSgood2<<std::endl;
 838
 839     //Fill the final trackpointarrays
 840     if (!ExtractSubTrackPointArray( fPTrackPointArray1,  fPVolIDArray1,  pfullITStrackArray, 1, 6 )) return kFALSE;
 841     if (!ExtractSubTrackPointArray( fPTrackPointArray2,  fPVolIDArray2,  pTPCArray1,         7, 8 )) return kFALSE;
 842     if (!ExtractSubTrackPointArray( fPTrackPointArray2b, fPVolIDArray2b, pTPCArray2,         7, 8 )) return kFALSE;
 843
 844     delete pfullITStrackArray; pfullITStrackArray=NULL;
 845     delete [] goodtracksArr; goodtracksArr=NULL;
 846     delete [] ITStracksArr; ITStracksArr=NULL;
 847     delete [] TPCtracksArr; TPCtracksArr=NULL;
 848     delete [] nPointsArr; nPointsArr=NULL;
 849     delete [] phiArr; phiArr=NULL;
 850     delete [] thetaArr; thetaArr=NULL;
 851     delete [] distanceFromVertexArr; distanceFromVertexArr=NULL;
 852     //printf("FindCosmicInEvent END\n");
 853     return kTRUE;
 854 }
 855
 856 Bool_t AliRelAlignerKalman::ExtractSubTrackPointArray( AliTrackPointArray* pTrackOut, TArrayI* pVolIDArrayOut, const AliTrackPointArray* pTrackIn, const Int_t firstlayer, const Int_t lastlayer )
 857 {
 858     //printf("ExtractSubTrackPointArray\n");
 859     //From pTrackIn select points between firstlayer and lastlayer and put the result in pTrackOut.
 860     //VolID's of selected points go in pVolIDArrayOut
 861     //only good points selected.
 862     //points can be ordered with z by setting fSortTrackPointsWithY
 863
 864     AliTrackPoint point;
 865     Int_t nPointsIn = pTrackIn->GetNPoints();
 866     if (nPointsIn<TMath::Min(fMinPointsVol1,fMinPointsVol2)) return kFALSE;
 867     Int_t iPointArr[1000];
 868     Int_t VolIDArr[1000];
 869     Float_t yArr[1000];
 870     Int_t iOuter=-1;
 871     Int_t OuterLayerID=0;
 872     Int_t nGood=0;
 873
 874     //loop over trackpoints and record the good ones
 875     for (Int_t i=0; i<nPointsIn; i++)
 876     {
 877         UShort_t VolIDshort = pTrackIn->GetVolumeID()[i];
 878         Int_t layerID = AliGeomManager::VolUIDToLayer(VolIDshort);
 879         //some points are very dirty: have nan's in coordinates or are otherwise sick
 880         {
 881         Float_t x = pTrackIn->GetX()[i];
 882         Float_t y = pTrackIn->GetY()[i];
 883         Float_t z = pTrackIn->GetZ()[i];
 884         if ( ((TMath::Abs(x)<1.)||(TMath::Abs(x)>1000.))&&
 885              ((TMath::Abs(y)<1.)||(TMath::Abs(y)>1000.))||
 886              ((TMath::Abs(z)>1000.))
 887            )
 888             continue;
 889         }
 890         if ( (layerID >= firstlayer) && (layerID <= lastlayer) )
 891         {
 892
 893             iPointArr[nGood] = i;
 894             VolIDArr[nGood] = VolIDshort;
 895             yArr[nGood] = pTrackIn->GetY()[i];
 896             if (layerID>OuterLayerID)
 897             {
 898                 OuterLayerID=layerID;
 899                 iOuter = nGood;
 900             }
 901             nGood++;
 902         }
 903         else continue;
 904     }//for i=0..nPointsIn
 905     if (nGood<TMath::Min(fMinPointsVol1,fMinPointsVol2)) return kFALSE;
 906     //Fill the output array of VolID's
 907     delete pVolIDArrayOut;
 908     pVolIDArrayOut = new TArrayI( nGood );
 909
 910     //fill the output trackarray
 911     Int_t* iSortedYArr = new Int_t[nGood];
 912     if (fSortTrackPointsWithY)
 913     {
 914         TMath::Sort( nGood, yArr, iSortedYArr, kFALSE );
 915     }
 916     delete pTrackOut;
 917     pTrackOut = new AliTrackPointArray( nGood );
 918
 919     for ( Int_t i=0; i<nGood; i++)
 920     {
 921         pTrackIn->GetPoint( point, iPointArr[(fSortTrackPointsWithY)?iSortedYArr[i]:i] );
 922         pTrackOut->AddPoint( i, &point );
 923         pVolIDArrayOut->AddAt( VolIDArr[(fSortTrackPointsWithY)?iSortedYArr[i]:i], i );
 924     }
 925     //printf("ExtractSubTrackPointArray END\n");
 926     return kTRUE;
 927 }
 928
 929 void AliRelAlignerKalman::SortTrackPointArrayWithY( AliTrackPointArray* pout, const AliTrackPointArray* pinn, const Bool_t descending )
 930 {
 931     //sorts a given trackpointarray by y coordinate
 932
 933     Int_t npoints = pinn->GetNPoints();
 934     const AliTrackPointArray* pin;
 935     if (pinn==pout)
 936         pin = new AliTrackPointArray(*pinn);
 937     else
 938     {
 939         pin = pinn;
 940         if (pout!=NULL) delete pout;
 941         pout = new AliTrackPointArray(npoints);
 942     }
 943
 944     Int_t* iarr = new Int_t[npoints];
 945
 946     TMath::Sort( npoints, pin->GetY(), iarr, descending );
 947
 948     AliTrackPoint p;
 949     for (Int_t i=0; i<npoints; i++)
 950     {
 951         pin->GetPoint( p, iarr[i] );
 952         pout->AddPoint( i, &p );
 953     }
 954     delete [] iarr;
 955 }
 956
 957 Bool_t AliRelAlignerKalman::FitTrackPointArrayRieman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint )
 958 {
 959     //printf("FitTrackPointArrayRieman\n");
 960     //Use AliTrackFitterRieman to fit a track through given point array
 961
 962     ///////////////////////////////////////
 963     //this is just to shut up the compiler!!
 964         TMatrixDSym* a = pDirCov;
 965         AliTrackPoint* b = pRefPoint;
 966         TArrayI* c = pVolIDs;
 967         a = a;
 968         b = b;
 969         c = c;
 970     ////////////////////////////////////
 971
 972     AliTrackFitterRieman fitter;
 973     AliTrackPoint trackPoint;
 974     const Float_t* pointcov;
 975     ////here's an ugly hack://///////
 976     //AliTrackPointArray* pTrackPointArray = const_cast < AliTrackPointArray* > (pTrackPointArrayIn);
 977     /////////////////////////////////
 978     fitter.SetTrackPointArray( pTrackPointArray, kFALSE );
 979     if ( !fitter.Fit( pVolIDs ) ) return kFALSE;
 980     if ( !fitter.GetPCA( *fPRefAliTrackPoint, trackPoint ) ) return kFALSE;
 981     Double_t xPoint = trackPoint.GetX();
 982     pPoint->SetXYZ( xPoint, trackPoint.GetY(), trackPoint.GetZ() );
 983     pDir->SetXYZ( 1., fitter.GetDYat(xPoint), fitter.GetDZat(xPoint) );
 984     *pDir = pDir->Unit();
 985     //TODO cov of dir!
 986
 987     pointcov = trackPoint.GetCov();
 988     (*pPointCov)(0,0) = pointcov[0];(*pPointCov)(0,1) = pointcov[1];(*pPointCov)(0,2) = pointcov[2];
 989     (*pPointCov)(1,0) = pointcov[1];(*pPointCov)(1,1) = pointcov[3];(*pPointCov)(1,2) = pointcov[4];
 990     (*pPointCov)(2,0) = pointcov[2];(*pPointCov)(2,1) = pointcov[4];(*pPointCov)(2,2) = pointcov[5];
 991
 992     //printf("FitTrackPointArrayRieman END\n");
 993     return kTRUE;
 994 }
 995
 996 Bool_t AliRelAlignerKalman::FitTrackPointArrayKalman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint )
 997 {
 998     //printf("FitTrackPointArrayKalman\n");
 999     //Use AliTrackFitterKalman to fit a track through given point array
1000     //still needs work
1001
1002     ///////////////////////////////////////
1003     //this is just to shut up the compiler!!
1004         AliTrackPoint* b = pRefPoint;
1005         TArrayI* c = pVolIDs;
1006         b = b;
1007         c = c;
1008     ////////////////////////////////////
1009
1010     AliTrackFitterKalman fitter;
1011     AliTrackPoint trackPoint, trackPoint2;
1012
1013     pTrackPointArray->GetPoint( trackPoint, 0 );
1014     pTrackPointArray->GetPoint( trackPoint2, 1 );
1015     //Make sure all points count
1016     fitter.SetMaxChi2(100000000.);
1017     if (!fitter.MakeSeed( &trackPoint, &trackPoint2 )) return kFALSE;
1018     Int_t npoints = pTrackPointArray->GetNPoints();
1019     for (Int_t i=2; i<npoints; i++)
1020     {
1021         pTrackPointArray->GetPoint( trackPoint, i );
1022         if (!fitter.AddPoint( &trackPoint )) continue;
1023     }//for i
1024     TMatrixDSym fullcov = fitter.GetCovariance();
1025     printf("FitTrackPointArrayKalman: the covariance matrix of the fit");
1026     fullcov.Print();
1027
1028     //now propagate to ref plane - its a hack that depends on the implementation of AddPoint()!
1029     // - first set the maxchi2 to 0 so addpoint returns false, but
1030     //actually the trackparameters have already been been propagated to the new planei, it's safe
1031     //because AliTrackFitterKalman::Propagate() is always kTRUE.
1032     fitter.SetMaxChi2(0.);
1033     fitter.AddPoint( fPRefAliTrackPoint );
1034
1035     const Double_t* pparams = fitter.GetParam();
1036     fullcov = fitter.GetCovariance();
1037
1038     pDir->SetXYZ(  pparams[3], 1., pparams[4] );
1039     pPoint->SetXYZ( pparams[0], pparams[1], pparams[2] );
1040
1041     (*pPointCov)(0,0) = fullcov(0,0);(*pPointCov)(0,1) = fullcov(0,1);(*pPointCov)(0,2) = fullcov(0,2);
1042     (*pPointCov)(1,0) = fullcov(1,0);(*pPointCov)(1,1) = fullcov(1,1);(*pPointCov)(1,2) = fullcov(1,2);
1043     (*pPointCov)(2,0) = fullcov(2,0);(*pPointCov)(2,1) = fullcov(2,1);(*pPointCov)(2,2) = fullcov(2,2);
1044
1045     (*pDirCov)(0,0)=fullcov(3,3); (*pDirCov)(0,1)=0.; (*pDirCov)(0,2)=fullcov(3,4);
1046     (*pDirCov)(1,0)=0.;           (*pDirCov)(1,1)=0.; (*pDirCov)(1,2)=0.;
1047     (*pDirCov)(2,0)=fullcov(4,3); (*pDirCov)(2,1)=0.; (*pDirCov)(2,2)=fullcov(4,4);
1048     return kTRUE;
1049 }
1050
1051 void AliRelAlignerKalman::SanitizeExtendedCovMatrix( TMatrixDSym& covout, const TMatrixDSym& pointcov, const TVector3& dir )
1052 {
1053     //reverse the effect of extending of the covariance matrix along the track
1054     //direction as done by the AliTrackFitters
1055
1056     //idea is to rotate to a system where track direction is y axis,
1057     //in that refsystem setting the y related components of covariance
1058     //then rotate back.
1059     TVector3 yaxis(0,1,0);
1060     Double_t angle = dir.Angle(yaxis);
1061     TVector3 rotaxisvec = dir.Cross(yaxis);
1062     TVector3 rotaxis = rotaxisvec.Unit();
1063     TMatrixD R(3,3);
1064
1065     //Fill the rotation matrix.
1066     Double_t ex=rotaxis(0);
1067     Double_t ey=rotaxis(1);
1068     Double_t ez=rotaxis(2);
1069     Double_t sin = TMath::Sin(angle);
1070     Double_t cos = TMath::Cos(angle);
1071     Double_t icos = 1 - cos;
1072     R(0,0) = cos + (ex*ex)*icos;
1073     R(0,1) = icos*ex*ey - ez*sin;
1074     R(0,2) = icos*ex*ez + ey*sin;
1075     R(1,0) = icos*ey*ex + ez*sin;
1076     R(1,1) = cos + (ey*ey)*icos;
1077     R(1,2) = icos*ey*ez - ex*sin;
1078     R(2,0) = icos*ez*ex - ey*sin;
1079     R(2,1) = icos*ez*ey + ex*sin;
1080     R(2,2) = cos + (ez*ez)*icos;
1081
1082     //Transform covariance:
1083     TMatrixD covR( pointcov, TMatrixD::kMultTranspose, R);
1084     TMatrixD RcovR( R, TMatrixD::kMult, covR );
1085     TMatrixD newcov(3,3);
1086     newcov(0,0)=RcovR(0,0);
1087     newcov(0,2)=RcovR(0,2);
1088     newcov(2,0)=RcovR(2,0);
1089     newcov(2,2)=RcovR(2,2);
1090     newcov(1,1)=(newcov(0,0)+newcov(2,2))/2.;  //this is a bit insane!!
1091     covR = newcov *R;
1092     RcovR = R.T() * covR;
1093     TMatrixDSym_from_TMatrixD( covout, RcovR );
1094 }
1095
1096 void AliRelAlignerKalman::JoinTrackArrays( AliTrackPointArray* pout, const AliTrackPointArray* pin1, const AliTrackPointArray* pin2 )
1097 {
1098     //Join two AliTrackPointArrays
1099
1100     Int_t np1 = pin1->GetNPoints();
1101     Int_t np2 = pin2->GetNPoints();
1102     if (pout!=NULL) delete pout;
1103     pout = new AliTrackPointArray(np1+np2);
1104     AliTrackPoint p;
1105     for (Int_t i=0;i<np1;i++)
1106     {
1107         pin1->GetPoint( p, i );
1108         pout->AddPoint( i, &p );
1109     }
1110     for (Int_t i=0;i<np2;i++)
1111     {
1112         pin2->GetPoint( p, i );
1113         pout->AddPoint( i+np1, &p );
1114     }
1115 }
1116
1117 Bool_t AliRelAlignerKalman::ProcessTrackPointArrays()
1118 {
1119     //Fit the track point arrays and update some household info
1120
1121     fFitted=kFALSE; //not fitted yet
1122     if ( !FitTrackPointArrayKalman( fPPoint2, fPPoint2Cov, fPDir2, fPDir2Cov,
1123                                      fPTrackPointArray2, fPVolIDArray2, fPRefAliTrackPoint ) )
1124     {
1125         fNUnfitted++;
1126         return kFALSE;
1127     }
1128
1129     if ( f3TracksMode )
1130     {
1131         if ( !FitTrackPointArrayKalman( fPPoint2b, fPPoint2bCov, fPDir2b, fPDir2bCov,
1132                                          fPTrackPointArray2b, fPVolIDArray2b, fPRefAliTrackPoint ) )
1133         {
1134             fNUnfitted++;
1135             return kFALSE;
1136         }
1137     }
1138
1139     if ( !FitTrackPointArrayKalman( fPPoint1, fPPoint1Cov, fPDir1, fPDir1Cov,
1140                                      fPTrackPointArray1, fPVolIDArray1, fPRefAliTrackPoint ) )
1141     {
1142         fNUnfitted++;
1143         return kFALSE;
1144     }
1145
1146     fFitted=kTRUE;
1147     //printf("ProcessTrackPointArrays: fitted!\n");
1148     return kTRUE;
1149 }
1150
1151 Bool_t AliRelAlignerKalman::UpdateCalibration()
1152 {
1153     //Update the calibration with new data, used in calibration pass
1154
1155     fPThetaMesHist->Fill( (*fPMeasurement)(0) );
1156     fPPhiMesHist->Fill( (*fPMeasurement)(1) );
1157     fPXMesHist->Fill( (*fPMeasurement)(2) );
1158     fPZMesHist->Fill( (*fPMeasurement)(3) );
1159     if (f3TracksMode)
1160     {
1161         fPThetaMesHist2->Fill( (*fPMeasurement)(4) );
1162         fPPhiMesHist2->Fill( (*fPMeasurement)(5) );
1163         fPXMesHist2->Fill( (*fPMeasurement)(6) );
1164         fPZMesHist2->Fill( (*fPMeasurement)(7) );
1165     }
1166     fCalibrationUpdated=kTRUE;
1167     return kTRUE;
1168 }
1169
1170 Bool_t AliRelAlignerKalman::SetCalibrationPass( Bool_t cp )
1171 {
1172     //sets the calibration mode
1173     if (cp)
1174     {
1175         fCalibrationPass=kTRUE;
1176         return kTRUE;
1177     }//if (cp)
1178     else
1179     {
1180         if (!fCalibrationUpdated) return kFALSE;
1181         if (fCalibrationPass) // do it only after the calibration pass
1182         {
1183             Double_t tmp;
1184             fPMeasurementCov->Zero(); //reset the covariance
1185
1186             fPThetaMesHist->Fit("gaus");
1187             TF1* fitformula = fPThetaMesHist->GetFunction("gaus");
1188             tmp = fitformula->GetParameter(2);
1189             (*fPMeasurementCov)(0,0) = tmp*tmp;
1190
1191             fPPhiMesHist->Fit("gaus");
1192             fitformula = fPPhiMesHist->GetFunction("gaus");
1193             tmp = fitformula->GetParameter(2);
1194             (*fPMeasurementCov)(1,1) = tmp*tmp;
1195
1196             fPXMesHist->Fit("gaus");
1197             fitformula = fPXMesHist->GetFunction("gaus");
1198             tmp = fitformula->GetParameter(2);
1199             (*fPMeasurementCov)(2,2) = tmp*tmp;
1200
1201             fPZMesHist->Fit("gaus");
1202             fitformula = fPZMesHist->GetFunction("gaus");
1203             tmp = fitformula->GetParameter(2);
1204             (*fPMeasurementCov)(3,3) = tmp*tmp;
1205
1206             if (f3TracksMode)
1207             {
1208                 fPThetaMesHist2->Fit("gaus");
1209                 fitformula = fPThetaMesHist2->GetFunction("gaus");
1210                 tmp = fitformula->GetParameter(2);
1211                 (*fPMeasurementCov)(4,4) = tmp*tmp;
1212
1213                 fPPhiMesHist2->Fit("gaus");
1214                 fitformula = fPPhiMesHist2->GetFunction("gaus");
1215                 tmp = fitformula->GetParameter(2);
1216                 (*fPMeasurementCov)(5,5) = tmp*tmp;
1217
1218                 fPXMesHist2->Fit("gaus");
1219                 fitformula = fPXMesHist2->GetFunction("gaus");
1220                 tmp = fitformula->GetParameter(2);
1221                 (*fPMeasurementCov)(6,6) = tmp*tmp;
1222
1223                 fPZMesHist2->Fit("gaus");
1224                 fitformula = fPZMesHist2->GetFunction("gaus");
1225                 tmp = fitformula->GetParameter(2);
1226                 (*fPMeasurementCov)(7,7) = tmp*tmp;
1227             }
1228
1229             fCalibrationPass=kFALSE;
1230             fFixedMeasurementCovariance=kTRUE;
1231             fPMeasurementCov->Print();
1232             return kTRUE;
1233         }//if (fCalibrationPass)
1234     return kFALSE;
1235     }//else (cp)
1236     return kFALSE;
1237 }
1238
1239 void AliRelAlignerKalman::PrintDebugInfo()
1240 {
1241     //prints debug info
1242     std::cout<<"AliRelAlignerKalman debug info"<<std::endl;
1243     printf("fPTrackPointArray1 y: ");
1244     for (Int_t i=0; i<fPTrackPointArray1->GetNPoints();i++)
1245     {
1246         printf("%.2f ",fPTrackPointArray1->GetY()[i]);
1247     }
1248     printf("\n");
1249     printf("fPTrackPointArray2 y: ");
1250     for (Int_t i=0; i<fPTrackPointArray2->GetNPoints();i++)
1251     {
1252         printf("%.2f ",fPTrackPointArray2->GetY()[i]);
1253     }
1254     printf("\n");
1255     printf("fPTrackPointArray2b y: ");
1256     for (Int_t i=0; i<fPTrackPointArray2b->GetNPoints();i++)
1257     {
1258         printf("%.2f ",fPTrackPointArray2b->GetY()[i]);
1259     }
1260     printf("\n");
1261
1262     printf("Measurement covariance");
1263     fPMeasurementCov->Print();
1264 }
1265