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